3D-3D坐标 SVD奇异值分解 ICP迭代最近点 G2O优化 求解 求旋转平移矩阵 R t

* 迭代最近点 Iterative Closest Point, ICP求解 3D坐标 到 3D坐标的转换矩阵(不用求解距离 激光SLAM 以及 RGB-D SLAM)

* 使用 线性代数SVD奇异值分解 或者 非线性优化方法 求解

* 使用深度图将 平面图 转化成 3维点

* 特征点匹配 得到

* 三维点对 P={p1， p2, ... ,pn} P' = {p1'， p2', ... ,pn'}

* 存在 旋转矩阵R 和 平移矩阵t

* 使得 pi = R* pi' + t

* 当在激光SLAM直接使用 三维点进行 匹配(距离最近)得到3D点对(激光数据 特征不够丰富，匹配得到的点误匹配的多)

* 求解方法为 ICP Iterative Closet Point 迭代最近点

* 【1】 线性代数求解 SVD奇异值分解方法

* 误差 ei = pi - (R*pi' + t)

* 最小化误差和 min (sum(ei^2)) ; e^2 = (pi-p - R*(pi' - p'))^2 + (p - R*p' - t)^2得到 R t

* (1) 计算两组 点集的质心位置 p 、 p‘ ，然后计算每个点的去质心坐标 qi = pi - p qi’ = pi' - p‘

* (2) min(sum(qi - R * qi')^2) >>> 得到 旋转矩阵R

* (3) 有R计算 t t = p - R * p'

* (qi - R * qi')^2 = qi转置 * qi - 2*qi转置 * R * qi + qi转置 * R转置* R * qi 第一项与R无关 第三项 应为 R转置* R = I

* qi转置 * R * qi W= sum(qi * qi转置) = U * 对角矩阵 * V转置 奇异值分解 R= U * V转置

* 【2】 非线性优化方法

* ei = pi -exp(f) * pi' = P - P‘ 李代数形式 的 变换矩阵 对误差求导 得到 迭代优化 梯度

* 3×6的雅克比矩阵 误差 对应的 导数 优化变量更新 增量

* e 对 ∇f的导数 = P'对∇f的偏导数

* P'对∇f的偏导数 = [ I -P'叉乘矩阵] 3*6大小 平移在前 旋转在后

* = [ 1 0 0 0 Z' -Y'

* 0 1 0 -Z' 0 X'

* 0 0 1 Y' -X 0]

* 旋转在前 平移在后

* = [ 0 Z' -Y' 1 0 0

* -Z' 0 X' 0 1 0

* Y' -X 0 0 0 1]

* J = - P'对∇f的偏导数

* = [ 0 -Z' Y' -1 0 0

*     Z' 0 -X' 0 -1 0

                -Y' X’ 0 0 0 -1]

github连接  点击打开链接

#include <iostream