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

Sfm那篇博客已经介绍,3D-3D的变换,不同学科称呼不同。
在测绘领域,称作为坐标转换,即七参数转换—(3个旋转,3个平移,1个尺度),通常尺度因子可以不计。最常见的情景诸如,54坐标到80坐标,80到CGS200坐标等。
在摄影测量学科里,称为绝对定向,即是把模型纳入到地面摄影测量坐标系中(像空间辅助到像—地面摄影测量)
公式如下:
你不知道的SVD 算法------点云配准+绝对定向+坐标转换_第1张图片
求解该方程的绝对定向元素的时候,至少知道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

你可能感兴趣的:(python,资料)