本发明涉及图像处理领域，具体是一种使用照片或视频重建三维牙模的方法及使用RGBD图像重建三维牙模的方法。
背景技术：
目前，随着计算机人工智能以及机器视觉辅助医疗诊断设计的发展，在口腔正畸，修复等领域。越来越多的需要对患者口腔内的牙齿进行数字化三维取模。以方便医生对患者的牙齿进行数字化正畸，数字化修复治疗，或对病例的治疗效果进行跟踪，对特殊病例样例进行保存。因此，扫描数字化的三维牙模的需求也随之提升。传统的镭射扫描或其他的扫描方式，价格昂贵，处理周期长；设备体积巨大，无法携带。而随着计算机视觉中虚拟现实领域的高速发展。基于使用照片合成三维技术，使用带深度信息的相机获取三维牙模，使得无需特殊扫描设备便可取得牙齿三维牙模成为可能。医生可直接使用手机或者使用RGBD相机拍摄照片或者视频，上传到图像处理服务器进行处理，处理完成就可以得到病患的三维牙齿模型，颠覆了原有使用镭射等昂贵方式繁琐的三维模型制作流程。
技术实现要素：
本发明针对背景技术中存在的问题，公开了使用SFM(structureformotion)处理RGB图像数据，SLAM处理(simultaneouslocalizationandmapping)RGBD图像数据来获取三维牙齿模型的方法。
本发明公开了第一种使用照片或视频重建三维牙模的方法，它分为增量的方法和全局的方法，具体包括以下步骤：
第一步，拍摄一组多角度多位置RGB牙齿图像，并去除产生畸变的图像；
第二步，
对所有的图像提取特征并匹配(可以使用SIFT，SURF或FAST算法)；
第三步，
针对增量的方法，选择一对图像做初始的配对，计算相对位姿，估算初始的三维牙模；
针对全局的方法，估算所有图像的相机位姿；
第四步，
针对增量的方法：通过增量的方式增加图像，以补全初始的三维牙模，并全局优化；
针对全局的方法，估算三维牙模，并做全局优化；
第五步，输出牙齿的三维牙模，并按照标定还原模型尺寸。
优选的，针对增量的方法，第二步具体为：
2.1读取牙齿图像，并对所有牙齿图像进行特征提取，每张牙齿图像都得到：一组特征点Keypt，一组描述子Descp以及特征点的颜色信息Keycolor，若特征点少于设定阈值(优选的为50)则排除该牙齿图像；
2.2对所有牙齿图像中的Descp进行顺次匹配，第n张PicN与第n+1张PicN+1进行匹配；并过滤错误匹配的点(优选的，错误匹配的点是指匹配距离大于最小距离四倍的点，此为经验值，随拍摄的照片而定)；最后保存匹配信息M，M由PicN中与PicN+1匹配上的Keypt的索引配对组成；
2.3根据匹配信息M选取最多有效匹配点的一对牙齿图像作为种子图像对(因为有可能增加图像，增量的方式选取的种子对一般是相对优选的),对于种子图像对中图像的特征点Keypt，根据M中的索引，获取其匹配特征点坐标组pt1(的Keypt)；对于图像的特征点Keypt，根据M中的索引，获取其匹配特征点坐标组pt2(的Keypt)；并根据M中的索引，保存匹配特征点的颜色信息Keycolor；
优选的，针对全局的方法，第二步具体为：
2.1读取牙齿图像，并对所有牙齿图像进行特征提取；每牙齿图像都得到：一组特征点Keypt以及一组描述子Descp和特征点的颜色信息Keycolor，若特征点少于设定阈值(优选的为50)则排除该牙齿图像；
2.2因为全局的方法不考虑新增图像，因此对所有图像中的Descp进行匹配，每一图像与其他所有的图像进行匹配，并过滤错误匹配的点(优选的，错误匹配的点是指匹配距离大于最小距离四倍的点，此为经验值，随拍摄的照片而定)，每两图像成功匹配构成一个图像对；最后保存匹配点信息M。M由PicN中与PicN+1匹配上的Keypt的索引配对组成；
2.3对于每个图像对中的图像PicN的Keypt，根据M中的索引，获取其匹配特征点坐标组pt1(pt1∈PicN的Keypt)；对于图像的PicN+1的的Keypt，根据M中的索引，获取其匹配特征点坐标组pt2(pt2∈PicN+1的Keypt)；并根据M中的索引，保存匹配特征点的颜色信息Keycolor。
优选的，针对增量的方法，第三步具体为：
3.1估算图像对的相机位姿，先求取图像对中与的相对变化矩阵，使用opencv库函数findEssentialMat函数计算两图像的本征矩阵E，findEssentialMat需要提供关键参数如下：
1.图像对匹配点组信息pt1和pt2
2.从相机内参矩阵K提取像素焦距fpix和光心坐标Pp(cx,cy)
3.选择随机抽样一致性算法RANSAC作为参数
然后通过分解本征矩阵E进行SVD奇异值分解，可得到相对变换；使用opencv库函数recoverPose反算图像对的相对变换，也就是计算R和T；recoverPose输入参数如下：
2.本征矩阵E
3.从相机内参矩阵K提取像素焦距fpix和光心坐标Pp(cx,cy)
3.2使用三角测量计算图像对对应点的空间坐标，即图像对的三维牙模的初始估算；P为空间中一点，x1为空间点P在PicN的投影，x1’为空间点P在PicN+1的投影；通过两个相机投影矩阵Camproj1和Camproj2，以及匹配点组pt1(x1∈pt1)和pt2(x1’∈pt2)，使用opencv库函数triangulatePoints计算三维结构，得到三维结构Struc；在这个计算过程中，匹配点的深度信息得以获取。其中Camproj1与Camproj2均为相机投影矩阵，由相机内参矩阵K乘以相机外参矩阵得到，相机外参矩阵即图像对中各自的旋转矩阵R与平移向量T组合得到；triangulatePoints使用的关键参数如下：
1.投影矩阵Camproj1和Camproj2
2.匹配点组pt1和pt2；
优选的，针对全局的方法，第三步具体为：
3.1估算所有图像对的相机位姿：首先求取所有图像对中PicN与PicN+1的相对变换矩阵，使用opencv库函数findEssentialMat计算两张图像的本征矩阵E，findEssentialMat需要提供参数如下
1.图像对匹配点组信息pt1和pt2，
然后通过分解本征矩阵E进行SVD奇异值分解，可得到相对变换；将本征矩阵带入opencv库函数recoverPose反算图像对的相对变换，也就是计算Rij和Tij；recoverPose输入参数如下：
3.2根据所有图像，估算几何结构：全局的方式和增量的方式的不同在于，对所有帧计算相机位姿。对于相机i，其位置信息(Ri，Ti)，对于相机j，其位置信息(Rj，Tj)；二者之间的绝对旋转可以通过对以下公式求解求得：
Rj＝RijRi
其中：
Rij为图PicN与PicN+1的相对旋转，已由上一步获取；
Ri与Rj为图PicN与PicN+1的绝对旋转；
计算方法：首先构造位姿图，将两图像间有相对位姿的图像加入图，如图像PicN为节点1，节点1的子节点PicN+1为节点2，节点2的子节点PicN+2为节点3；将PicN的绝对旋转设为单位矩阵，使用以上公式求得节点2的绝对旋转；当PicN+1的绝对旋转被确定下来后，随之继续计算所有剩余的图中的绝对旋转；通过3.1步，相对旋转Rij和平移向量Tij已知，可根据下式求解绝对平移Ti：
||Tj-RijTi-λijTij||＝0
其中λij为比例因子，λij设定大于等于1，令T1为{0，0，0}，将T1使代入上式，可使用CeresSolver进行非线性最小二乘法进行估算。
优选的，针对增量的方法，第四步具体为：
4.1为了提高三维牙模及估算出的相机位姿的精度，减少重新投影的误差，使用BundleAdjustment做全局优化；当牙模数据的三维结构不够完整时，增加新的牙齿图像；上一步通过对图像对中PicN与PicN+1进行三维重建后，获取了三维结构Struc；新增图像为图像PicN+2，使用opencv库中的solvePnPRansac的函数，求得PicN+2的相对旋转矩阵R与平移向量T；继续使用三角测量计算并将新的未添加进三维牙模结构的点增加到三维牙模Struc中，并保存新增点的颜色信息Keycolor；
solvePnPRansac使用的关键参数如下：
1.三维结构Struc
2.图PicN+2二维匹配信息pt2
3.3*3的相机内参矩阵K
若仍有新增图像，则重复本步骤；
优选的，针对全局的方法，第四步具体为：
使用三角测量计算图像对对应点的空间坐标，即图像对的三维牙模的初始估算：P为空间中一点，x1为空间点P在PicN的投影，x1’为空间点P在PicN+1的投影；通过两个相机投影矩阵Camproj1和Camproj2，以及匹配点组pt1(x1∈pt1)和pt2(x1’∈pt2)，使用opencv库函数triangulatePoints计算三维结构，得到三维结构Struc；在这个计算过程中，匹配点的深度信息得以获取。其中Camproj1与Camproj2均为相机投影矩阵，由相机内参矩阵K乘以相机外参矩阵得到，外相机参矩阵即图像对中各自的旋转矩阵R与平移向量T组合得到；triangulatePoints使用的关键参数如下：
为了提高三维牙模及估算出的相机位姿的精度，减少重新投影的误差，使用BundleAdjustment做全局优化。
本发明还公开了第二种使用照片或视频重建三维牙模的方法，它分为增量优化(所述优化是指在第一种增量式的方法的基础上更为优化)的方法和全局优化(所述优化是指在第一种全局式的方法的基础上更为优化)的方法，具体包括以下步骤：
第一步，拍摄一组多位置且固定角度差的RGB牙齿图像，拍摄时，将牙模位置固定在托盘圆心上，每次按照固定角度旋转托盘，此时每两张图的旋转矩阵相同，每两张图的平移向量T也相同，将问题简化为以牙模为轴心的固定轨迹的周期运算，并去除产生畸变的图像；
第二步，对所有的图像提取特征并匹配(可以使用SIFT，SURF或FAST算法)；
针对增量的方法，第二步具体为：
针对全局的方法，第二步具体为：
2.3对于每个图像对中的图像PicN的Keypt，根据M中的索引，获取其匹配特征点坐标组pt1(pt1∈PicN的Keypt)；对于图像的PicN+1的的Keypt，根据M中的索引，获取其匹配特征点坐标组pt2(pt2∈PicN+1的Keypt)；并根据M中的索引，保存匹配特征点的颜色信息Keycolor
针对增量的方法/针对全局的方法，估算所有图像的相机位姿；
3.1对于相对旋转，将欧拉角转换为旋转矩阵带入相机外参矩阵，直接获取相对旋转矩阵。再通过计算可以得到平移向量。
首先基于欧拉角生成旋转矩阵：
然后，根据轴向(XYZ)旋转的不同，通过矩阵相乘得到3*3的旋转矩阵R；再根据X′1＝RX1+T求解平移向量T，其中匹配点组x1∈pt1，x1’∈pt2：
最后将计算出的相对旋转矩阵R和平移向量T代入下式：
以上求得的旋转矩阵R与平移向量T可直接代入其他图像对的相机外参矩阵；
针对增量的方法：通过增量的方式增加图像，以补全初始的三维牙模，并全局优化：
4.1为了提高三维牙模及估算出的相机位姿的精度，减少重新投影的误差，使用BundleAdjustment做全局优化；当牙模数据的三维结构不够完整时，增加新的牙齿图像；上一步通过对图像对中PicN与PicN+1进行三维重建后，获取了三维结构Struc；新增图像为图像PicN+2，使用已经计算出的相对旋转矩阵R与平移向量T；继续使用三角测量计算并将新的未添加进三维牙模结构的点增加到三维牙模Struc中，并保存新增点的颜色信息Keycolor；
4.2若仍有新增图像，则重复本步骤；
针对全局的方法，
本发明还公开了第一种使用RGBD图像重建三维牙模的方法，包括以下步骤：
第一步，拍摄一组多角度RGBD牙齿图像；
第二步，特征提取匹配及配准(可以使用SIFT，SURF或FAST算法)；
第三步，将图像转化为点云数据；
第四步，拼接点云数据；
优选的，第二步具体为：读取RGB图像，并对所有图像进行特征提取，每个图像都得到：一组特征点Keypt，一组描述子Descp以及特征点的颜色信息Keycolor，若特征点少于设定阈值(优选的为50)则排除该牙齿图像；对所有图像中的Descp进行顺次匹配，第n张PicN与第n+1张PicN+1进行匹配；可使用基于KD-Tree的ANN算法匹配，并过滤错误匹配的点(优选的，错误匹配的点是指匹配距离大于最小距离四倍的点，此为经验值，随拍摄的照片而定),匹配距离大于最小距离四倍的点(此为经验值，随拍摄的照片而定)。最后保存匹配信息M，M由PicN中与PicN+1匹配上的Keypt的索引配对组成；根据匹配信息M选取最多有效匹配点的一对图像作为种子图像对,对于种子图像对中的图从M中获取其所有特征点坐标组pt1,对于图像从M中获取其所有特征点坐标组pt2。
优选的，第三步具体为：
遍历深度图，从图像的深度图获取特征点组pt1的深度信息d；利用深度信息，相机焦距和光心坐标，反算三维空间点坐标组
对中的一个空间点(x,y,z)和它在图像中的像素坐标(u,v,d)有如下关系：
z＝d/s
x＝(u-cx)*z/fx
y＝(v-cy)*z/fy
cx,cy：光心坐标，在图像的中心，由最高分辨率除以2得到；
fpix：像素焦距；
u，v，d,为空间点投影到图像的像素坐标，其中d为深度信息；
x，y，z,为世界坐标系中空间点的齐次坐标；
s：深度图的缩放因子，可根据不同设备获取；
从RGB图像颜色图获取颜色信息Color，使用opencv库中的solvePnPRansac的函数，计算相对矩阵[R|T]；solvePnPRansac使用的关键参数如下：
1.三维空间点坐标组
2.图二维匹配信息pt2
3.3*3的相机内参矩阵K。
优选的，第四步具体为：
拼接点云数据，将图像PicN与图像PicN+1转换为点云数据Ptcloud1与Ptcloud2，以及变换矩阵[R|T]来拼接点云，利用点云库函数pcl::transformPointCloud来拼接点云数据；Ptcloud1与Ptcloud2合并得到Ptall；对于新增的图像，使用opencv库中的solvePnPRansac的函数，将新图像中匹配点的二维坐标带入，计算新的[R|T]；solvePnPRansac使用的关键参数如下：
若有新图像则继续计算，将未添加进三维牙模结构的点合并到Ptall中，最后继续使用BundleAdjustment做全局优化。
本发明还公开了第二种使用RGBD图像优化重建三维牙模的方法(所述优化是指在第一种使用RGBD图像的方法的基础上更为优化)，包括以下步骤：
第一步，拍摄一组多位置且固定角度差的RGBD牙齿图像；
第二步，特征提取匹配及配准(可以使用SIFT，SURF或FAST算法)：
读取RGB图像，并对所有图像进行特征提取，每个图像都得到：一组特征点Keypt，一组描述子Descp以及特征点的颜色信息Keycolor，若特征点少于设定阈值(优选的为50)则排除该牙齿图像；对所有图像中的Descp进行顺次匹配，第n张PicN与第n+1张PicN+1进行匹配；可使用基于KD-Tree的ANN算法匹配，并过滤错误匹配的点(优选的，错误匹配的点是指匹配距离大于最小距离四倍的点，此为经验值，随拍摄的照片而定),匹配距离大于最小距离四倍的点(此为经验值，随拍摄的照片而定)。最后保存匹配信息M，M由PicN中与PicN+1匹配上的Keypt的索引配对组成；根据匹配信息M选取最多有效匹配点的一对图像作为种子图像对,对于种子图像对中的图从M中获取其所有特征点坐标组pt1,对于图像从M中获取其所有特征点坐标组pt2；
第三步，将图像转化为点云数据：
3.1遍历深度图，从图像的深度图获取特征点组pt1的深度信息d；利用深度信息，相机焦距和光心坐标，反算三维空间点坐标组
3.2对于相对旋转，将欧拉角转换为旋转矩阵带入相机外参矩阵，直接获取相对旋转矩阵。再通过计算可以得到平移向量。
首先基于欧拉角生成旋转矩阵将欧拉角转换为旋转矩阵：
第四步，拼接点云数据：
拼接点云数据，将图像PicN与图像PicN+1转换为点云数据Ptcloud1与Ptcloud2，以及变换矩阵[R|T]来拼接点云，利用点云库函数pcl::transformPointCloud来拼接点云数据；Ptcloud1与Ptcloud2合并得到Ptall；对于新增的图像，使用已经计算出的相机外参矩阵[R|T]；并将未添加进三维牙模结构的点合并到Ptall中，最后继续使用BundleAdjustment做全局优化；
本发明的有益效果
本发明公开了一种使用照片或视频重建三维牙模的方法，
第一种增量式的方法具有：增量式的方案可以满足当生成牙模不够完整，或者在某些关键区域不够清晰的情况下。增加新角度的照片，进行处理，以达到更好的生成效果。
第一种全局式的方法具有：全局式的方案可以满足当拍摄角度固定而且完整，确保关键区域可以清晰的表达的情况下。一次性生成牙模，不再需要人工判断或干预。
第二种(优化)增量式的方法在第一种增量式的基础上，还具有：增量优化的方式可以简单的根据物理世界的平移旋转，减少处理流程中最不精确和耗时的旋转矩阵和平移向量的计算。减少了处理流程，增加了处理精度。
第二种(优化)全局式的方法在第一种全局式的基础上，还具有：全局优化的方式同样可以简单的根据物理世界的平移旋转，减少处理流程中最不精确和耗时的旋转矩阵和平移向量的计算。减少了处理流程，增加了处理精度。
本发明还公开了一种使用RGBD图像重建三维牙模的方法，
第一种RGBD的方法具有：RGBD的图像不需要计算深度信息是对于非RGBD图像计算上的优化。对于深度信息的计算，使用RGBD图像可以获取深度信息的精度高于需要估算的非RGBD图像。
第二种(优化)RGBD的方法在第一种RGBD的基础上，还具有：优化的RGBD的图像处理方式可以简单的根据物理世界的平移旋转，减少处理流程中最不精确和耗时的旋转矩阵和平移向量的计算。减少了处理流程，增加了处理精度。也将为未来机器人大规模实时计算建模提供了思路，既当运行轨迹固定，拍摄角度可以设定的情况下，自动的完成三维牙模的生成和处理。
附图说明
图1为第一种增量式/全局式重建三维牙模的流程图。
图2为第一种增量式重建三维牙模的详细流程图。
图3为第一种全局式重建三维牙模的详细流程图。
图4为第二种(优化)增量式/全局式重建三维牙模的流程图。
图5为第二种(优化)增量式重建三维牙模的详细流程图。
图6为第二种(优化)全局式重建三维牙模的详细流程图。
图7为第一种RGBD重建三维牙模的流程图。
图8为第二种(优化)RGBD重建三维牙模的详细流程图。
图9为牙齿图像采集方式示意图。
图10为估算图相对的相机位姿的原理分析图。
图11为优化的原理分析图。
具体实施方式
下面结合实施例对本发明作进一步说明，但本发明的保护范围不限于此：
技术方案如发明内容所述：图1给出了第一种增量式/全局式重建三维牙模的流程图，图2给出了第一种增量式重建三维牙模的详细流程图，图3给出了第一种全局式重建三维牙模的详细流程图，图4给出了第二种(优化)增量式/全局式重建三维牙模的流程图，图5给出了增量式重建三维牙模的详细流程图，图6给出了第二种(优化)全局式重建三维牙模的详细流程图，图7给出了第一种RGBD重建三维牙模的流程图，图8给出了第二种(优化)RGBD重建三维牙模的详细流程图，此处不再赘述。现对技术方案的可替代方案以及原理进行说明。
结合图1、图4、图7，其第一步中所述拍摄一组多角度牙齿图像是指拍摄至少一圈，其所围平面与牙模所处平面重合。如附图9所示，更优的实施例中，它还包括附加的两圈，第二圈所围平面边缘与牙模呈40-60度夹角；第三圈所围平面边缘与牙模呈80-90度夹角。如附图9所示，更优的实施例中，它还包括第四圈，第四圈纵向围绕牙模拍摄。在图1、图4中所述第一步中拍摄一组多角度多位置RGB牙齿图像是通过拍摄照片获得；或者是拍摄视频通过提取关键帧获得。
输出牙齿的三维牙模，并按照标定还原模型尺寸，具体为：输出三维牙模，使用PMVS或者CMVS等工具将牙齿模型的稀疏矩阵转化为三维稠密矩阵；最后使用Meshlab等工具将点云数据转化为带颜色的ply等格式三维牙模文件或者不带颜色的stl等格式三维牙模文件；通过测量三维牙模上的标定，计算三维牙模与真实牙模的比例，并放大模型至真实大小。
原理说明(以增量式重建三维牙模为例，第三步中)：
使用针孔摄像机的投影矩阵模型,可以将空间三维点投影到像平面；相机内参完成从图像坐标系到相机坐标系的变换，因为相机坐标是毫米为单位，而图像坐标是以像素为单位，因此需要线性变换；内参矩阵K表示如下：
K：3*3的矩阵，代表相机内部参数
cx,cy：光心坐标，通常在图像的中心，由最高分辨率除以2得到
fpix：像素焦距(可由下面的算法计算)
为了优化相机内参，可以根据不同设备计算不同的像素焦距，当所有图像均由统一设备拍摄，计算使用设备的像素焦距可由下面等式计算：
fpix＝max(wpix,hpix)*fmm/ccdwmm
wpix是图像的像素宽，hpix像素高
ccdwmm感应器的宽，毫米为单位
fmm为毫米焦距
以上参数均可由设备参数提供：
而当使用多种拍摄设备，也可以使用固定像素焦距，固定像素焦距的计算公式：
fpix＝max(wpix,hpix)*1.2
相机外参完成从相机坐标系到世界坐标系的变换，是通过旋转矩阵R和平移矩阵T完成的。
R：代表相机坐标到世界坐标的旋转变换，R就是r11到r33构成3*3的矩阵
T：代表相机坐标到世界坐标的平移变换，tx,ty,tz：相机在xyz方向上的平移
则图像中的两维点与三维世界坐标点的关系如下:
其中u，v，为空间点投影到图像的像素坐标，X，Y，Z,为世界坐标系中空间点的齐次坐标。
估算图像对的相机位姿，实际上就是求取图像对中图n与图n+1的相对变换矩阵；使用opencv库函数findEssentialMat可计算两张图的本征矩阵E，参数使用第二步获取的匹配点组pt1和pt2，从相机内参矩阵K提取像素焦距fpix光心坐标pp(cx,cy)，以及使用RANSAC算法；通过分解本征矩阵E进行SVD奇异值分解，可得到相对变换；将本征矩阵带入opencv库函数recoverPose可以反算图像对的相对变换，也就是计算R和T；
结合图10所示，对于两个相机坐标系来说，两张图片上匹配上的两个点，具有以下关联：
X′1＝RX1+T
点X1'是点X1经过旋转和平移得到的；
因此其本征矩阵则为：
若以图像对中包含点X1'的图像n的相机位姿作为参考相机初始位姿，R就是图像对中图像n+1的点X1'相对于图像n中点X1的旋转矩阵。T就是图像对中图像n+1的点X1'相对于图像n的X1的位移矩阵；因此通过使用OpenCV的函数findEssentialMat可以计算出本征矩阵E，进而由OpenCV的函数recoverPose计算相对变换矩阵R与T。
原理说明(优化)：
对于牙模的拍摄，与普通的SFM或者SLAM处理场景可以做如下优化。对于工业领域，并不需要在随机的位置围绕牙模牙齿模型进行拍摄，可通过固定相机，固定拍摄角度，同时，使用圆盘周期旋转，可以固定相对位移。如图11所示，圆心处放置牙模。相机中心在O1,O2,O3处拍摄，g1与g2角度相等。O1到O2的距离为l1，O2到O3距离为l2，l1＝l2。因此旋转矩阵相同，平移向量相同。
无论SFM的方式还是SLAM的方式拍摄牙模，也不管使用增量还是全局的方式求解，当将牙模位置固定在托盘圆心上，每次旋转固定角度。可以将问题简化为绕轴心(圆心)固定轨迹的周期运算。这样使得相对旋转以及相对平移的运算大大简化。
1.首先对于本征矩阵的运算以及分解只要一次即可。因为物理世界中旋转角度固定，平移位置固定。当本征矩阵分解出相对旋转和平移向量也随之固定。每两帧的本征矩阵相等，之后的运算可直接使用。
2.对于相对旋转，可以将欧拉角转换为旋转矩阵带入矩阵，直接获取相对旋转矩阵。再通过计算可以得到平移向量。X1'＝RX1+T
将欧拉角转换为旋转矩阵
根据轴向旋转的不同，矩阵相乘得到旋转矩阵。再根据以下公式求解平移：
本文中所描述的具体实施例仅仅是对本发明精神做举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。