你不知道的SVD 算法------点云配准+绝对定向+坐标转换

Sfm那篇博客已经介绍，3D-3D的变换，不同学科称呼不同。
在测绘领域，称作为坐标转换，即七参数转换—（3个旋转，3个平移，1个尺度），通常尺度因子可以不计。最常见的情景诸如，54坐标到80坐标，80到CGS200坐标等。
在摄影测量学科里，称为绝对定向，即是把模型纳入到地面摄影测量坐标系中(像空间辅助到像—地面摄影测量)
公式如下：

求解该方程的绝对定向元素的时候，至少知道3对点才能求解。因为上式是非线性方程，故需要线性化。列出误差方程，然后迭代求解直到改正数小于某一个阈值。
在计算机视觉中，我们可以称为为SLAM/点云配准。与二维坐标配准类似，只不过增加一个维度。
以上求解定向参数可以借用SVD算法轻易解得，SVD算法作为线性代数的知识，其有很多用处，分解本质矩阵E得到相机位姿态、求解线性方程解等：
1、 points1->m*3，points2->n*3 m=n
2、 center_points1=mean(points,0) ->1*3
center_points2=mean(points,0) ->1*3
3、 坐标中心化，其目的是减少有效位数，保证计算精度
New_points1=Points1- np.tile(center_points1,(m,1)) ->m*3
New_points2=Points2- np.tile(center_points2,(m,1)) ->n*3
4、 得到转换矩阵M
M= New_points2.T* New_points1或者M= New_points1.T* New_points2 结果是一样的，笔者试验过了 (注意是points1 到points2的变换)
5、 分解M，得到R and T
注意：笔者最开始犯了一个严重的错误。也是很多人容易忽略的。
在matlab 中：
U,S,V=SVD(M)
->U*S*V.T=M
在python 中：
U,S,Vt=SVD(M)
->U*S*Vt=M
也就是python 分解出来的v是转置后的，不是真真的V。
所以对于matlab 版本：
R=U*V’
对于python :
R=U@Vt T都是相同的
至少3个点求解完定向参数后，对于points1坐标系下剩下的点可以利用R*points+T来完成将其统一到points2坐标系下的过程。
代码如下：

# -*- coding: utf-8 -*-
#3d points transform
import numpy as np
def points3D_transform(points1,points2):
    '''points1 m*3 points2 n*3  m=n'''
    center_points1=np.mean(points1,0)
    center_points2=np.mean(points2,0)
    new_points1=points1-np.tile(center_points1,(points1.shape[0],1))
    new_points2=points2-np.tile(center_points2,(points2.shape[0],1))
    M=new_points2.T @ new_points1 #3*3 向量化编程，拒绝循环
    u,s,vt=np.linalg.svd(M)
    R=u@vt
    if np.linalg.det(R)<0:
        R=-R
    T=center_points2.T-R@center_points1
    return R,T