相似变换 SIM3

参考文献：Closed-form solution of absolute orientation using unit quaternions

1，求解步骤

会将当前关键帧地图点和闭环关键帧地图点进行Bow匹配，匹配上的两组3D点集之间可以用RANSAS求解Sim3变换，从而获得当前关键帧相对于闭环关键帧的Sim3，然后传播到相连关键者，并调整地图点，从而完成闭环调整。
        两张图上有n个点，令两图上的点为

                                                  

       从左图到右图的关系:

                                             

      下图为计算质心，并求每点到质心的距离，这个距离的累加是等于零的，求取误差的平方和。

                                               

                              如上式第一项和第三项是非零的，第一项与无关，要求此项的最小值，可以令等于零。

             误差可等于：

                      由于旋转不会改变长度的大小：    

令误差等于相当于除了一个根号s，我们只是求误差的最小值，除以一个系数是没关系的，这样做的好处是最后求得的s与旋转因子无关，并且便于后面求解。

                             

       上如第三个式子是让平方项等于零，这样才能使值更小。这样求出s。 

         这样就简化式子成求的最大值。

        上式用四元数表示的形式：

                                       

                                      

                                                  

           上面的式子主要就是要把N表达出来。 

            现在要把M引出来:

                                      

                    把M弄成上面的形式，每一项都是可以求出来的，上面只列举了两项。

                 

                   引出M来就是为了让其中的量来表示N，如上式。接下来就是求解最大的正特征值的过程。

                                                             

                    上式求出来的最大特征值，e对应其特征向量。并且就是四元数q的值。求证如下：

                                                        

                    四元数可以用特征向量表示  

                                                            

                                                    

                                                              令  

                                                      

                                               相当于

                   理解四元数是怎么表示旋转矩阵的然后就能求出旋转R，然后求出相应的平移t，尺度s也在前面的                                                   公式可以求出来。

                  想要更详细理解此原理可以去看上面的参考文献。这个算法的代码在网上也是可以查的到的，我就不传上来了。