ROS学习笔记（19）：建图与定位（3）

时间：2024-07-14 11:20:36

目录

0.前言

1.正文

1.第一篇论文

1.ICP迭代目标

2.变换矩阵

1.平移矩阵

2.旋转矩阵 

 3.线性近似 

4.回到ICP迭代目标:

 5.克莱姆法则求解

2.总结

0.前言

上文参考论文:2008-icra-plicp.pdf (censi.science)和lowk_point-to-plane_icp.doc (unc.edu)上期文章对扫描匹配进行了简单了解，本期我将分两期对上述两篇公式进行演算一遍。

1.正文

1.第一篇论文

 本期参考论文：Microsoft Word - lowk_point-to-plane_icp.doc (unc.edu)https://www.cs.unc.edu/techreports/04-004.pdf

1.ICP迭代目标

ICP迭代的目标就是找到M_{opt}:

M_{opt}=argmin_{M} \sum_{i}((M\cdot s_i-d_i)\cdot n_i)^2

M和M_{opt}都是4\times 4的三维变换矩阵。

M=Trans(\Delta x,\Delta y,\Delta z)\cdot Rot(\alpha ,\beta ,\gamma )=T(t_x,t_y,t_z)\cdot R(\alpha ,\beta ,\gamma )

2.变换矩阵

1.平移矩阵

T(t_x,t_y,t_z)=Trans(t_x,t_y,t_z)=\begin{bmatrix} 1& 0& 0& t_x\\ 0& 1& 0& t_y\\ 0& 0& 1& t_z\\ 0& 0& 0& 1 \end{bmatrix}

2.旋转矩阵 

旋转矩阵

R(\alpha ,\beta ,\gamma ) = R(z,\gamma ) \cdot R(y,\beta ) \cdot R(x,\alpha )  //注意点乘顺序是z到y到x，矩阵相乘不能调换。

R_x(\alpha )=Rot(x,\alpha ) =\begin{bmatrix} 1& 0& 0& 0\\ 0& cos\alpha& -sin\alpha& 0\\ 0& sin\alpha& cos\alpha& 0\\ 0& 0& 0& 1 \end{bmatrix}

R_y(\beta )=Rot(y,\beta ) =\begin{bmatrix} cos\beta & 0& sin\beta & 0\\ 0& 1& 0& 0\\ -sin\beta &0& cos\beta & 0\\ 0& 0& 0& 1 \end{bmatrix}

R_z(\gamma )=Rot(z,\gamma ) =\begin{bmatrix} cos\gamma & -sin\gamma & 0& 0\\ sin\gamma & cos\gamma & 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1 \end{bmatrix}

经过计算后得到：

R(\alpha ,\beta ,\gamma )=\begin{bmatrix} c\gamma c\beta& -s\gamma c\alpha+c\gamma s\beta s\alpha& s\gamma s\alpha+c\gamma s\beta c\alpha &0\\ s\gamma c\beta& c\gamma c\alpha+s\gamma s\beta s\alpha& -c\gamma s\alpha +s\gamma s\beta c\alpha & 0\\ -s\beta & c\beta s\alpha & c\beta c\alpha & 0\\ 0& 0& 0& 1 \end{bmatrix}

下图为我的手稿：

 3.线性近似 

M_{opt}=argmin_{M} \sum_{i}((M\cdot s_i-d_i)\cdot n_i)^2

M和M_{opt}都是4\times 4的三维变换矩阵。

M=Trans(\Delta x,\Delta y,\Delta z)\cdot Rot(\alpha ,\beta ,\gamma )=T(t_x,t_y,t_z)\cdot R(\alpha ,\beta ,\gamma )

 上述是一个最小二乘优化问题，求解时需要确认\alpha、\beta、\gamma、t_x、t_y、t_z的值，\alpha、\beta、\gamma在旋转矩阵中是非线性的（即不符合f(x)=ax+b）三角函数的参数，最小二乘法是应用在线性问题，将非线性转化成线性可引入高数极限的等价无穷小：lim_{x\rightarrow 0}sin\theta = \theta和lim_{x\rightarrow 0}cos\theta =1。

同时采用保留高阶无穷小的方式再次简化上面的公式得到：

4.回到ICP迭代目标M_{opt}:

M_{opt}=argmin_{M} \sum_{i}((M\cdot s_i-d_i)\cdot n_i)^2

列出s_i、d_i和n_i含xyz三轴的矩阵(线性表达)后会得到：

\widehat{M_{opt}}=argmin_{\widehat{M}}\sum_{i}((\begin{bmatrix} 1& -\gamma & \beta & t_x\\ \gamma & 1& -\alpha & t_y\\ -\beta & \alpha & 1& t_z\\ 0& 0& 0& 1 \end{bmatrix}\cdot \begin{bmatrix} s_{ix}\\ s_{iy}\\ s_{iz}\\ 1 \end{bmatrix}-\begin{bmatrix} d_{ix}\\ d_{iy}\\ d_{iz}\\ 1 \end{bmatrix})\cdot \begin{bmatrix} n_{ix}\\ n_{iy}\\ n_{iz}\\ 1 \end{bmatrix})^2

 计算后得到：

\widehat{M_{opt}}=argmin_{\widehat{M}}\sum _{i}[(s_{ix}n_{iy}-s_{iy}n_{ix})\gamma +(s_{iz}n_{ix}-s_{ix}n_{iz})\beta +(s_{iy}n_{iz}-s_{iz}n_{iy})\alpha+(n_{ix}t_x+n_{iy}t_y+n_{iz}t_z)-(n_{ix}d_{ix}+n_{iy}d_{iy}+n_{iz}d_{iz})+s_{ix}n_{ix}+s_{iy}n_{iy}+s_{iz}n_{iz} ]^2

下图为手稿： 

 5.克莱姆法则求解

按克莱姆法则得到下述公式：

\widehat{M_{opt}}=argmin_x(Ax-b)^2=argmin_{\widehat{M}}\sum _{i}[(s_{ix}n_{iy}-s_{iy}n_{ix})\gamma +(s_{iz}n_{ix}-s_{ix}n_{iz})\beta +(s_{iy}n_{iz}-s_{iz}n_{iy})\alpha+(n_{ix}t_x+n_{iy}t_y+n_{iz}t_z)-(n_{ix}d_{ix}+n_{iy}d_{iy}+n_{iz}d_{iz})+s_{ix}n_{ix}+s_{iy}n_{iy}+s_{iz}n_{iz} ]^2

f(x)=Ax-b=\sum_i[(s_{ix}n_{iy}-s_{iy}n_{ix})\gamma +(s_{iz}n_{ix}-s_{ix}n_{iz})\beta +(s_{iy}n_{iz}-s_{iz}n_{iy})\alpha+(n_{ix}t_x+n_{iy}t_y+n_{iz}t_z)-(n_{ix}d_{ix}+n_{iy}d_{iy}+n_{iz}d_{iz})+s_{ix}n_{ix}+s_{iy}n_{iy}+s_{iz}n_{iz}]b=\begin{bmatrix} n_{1x}d_{1x}+n_{1y}d_{1y}+n_{1z}d_{1z}-s_{1x}n_{1x}-s_{1y}n_{1y}-s_{1z}n_{1z}\\ n_{2x}d_{2x}+n_{2y}d_{2y}+n_{2z}d_{2z}-s_{2x}n_{2x}-s_{2y}n_{2y}-s_{2z}n_{2z}\\\ \vdots \\ n_{ix}d_{ix}+n_{iy}d_{iy}+n_{iz}d_{iz}-s_{ix}n_{ix}-s_{iy}n_{iy}-s_{iz}n_{iz}\ \end{bmatrix}

x=\begin{bmatrix} \alpha \\ \beta \\ \gamma \\ t_x \\ t_y\\ t_z \end{bmatrix}

A=\begin{bmatrix} a_{11} & a_{12}& a_{13}& a_{14}& a_{15}& a_{16}\\ a_{21}& a_{22}& a_{23}& a_{24}& a_{25}& a_{26}\\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ a_{i1}& a_{i2}& a_{i3}& a_{i4}& a_{i5}& a_{i6} \end{bmatrix}

 \left\{\begin{matrix} a_{i1}= n_{iz}s_{iy} -n_{iy}s_{iz}\\ a_{i2}= n_{ix}s_{iz} -n_{iz}s_{ix}\\ a_{i3}= n_{iy}s_{ix} -n_{ix}s_{iy}\\ a_{i4}=n_{ix}\\ a_{i5}=n_{iy}\\ a_{i6}=n_{iz} \end{matrix}\right.

根据克莱姆法则会得到x的一组解，其公式过于复杂，这里就不列出，有兴趣的各位可以自己去了解一下克莱姆法则：

克莱姆法则（Cramer's Rule）-****博客https://blog.****.net/jackghq/article/details/90712570?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522172087636016800211540993%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=172087636016800211540993&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~top_positive~default-1-90712570-null-null.142^v100^pc_search_result_base3&utm_term=%E5%85%8B%E8%8E%B1%E5%A7%86%E6%B3%95%E5%88%99&spm=1018.2226.3001.4187此时就得到了简化后的公式：

\widehat{M_{opt}}=argmin_x(Ax-b)^2

这就是线性最小二乘问题，使用奇异值分解(SVD)原理详解及推导_svd分解-****博客可以解决，这里我同样就不进行推导了，在写这篇文章的时候作者尝试回忆过往学习经历对svd求解确实也没有思路，也得补一下了，后续会对本文章进行更新补充。

论文得到的最后结果为：\widehat{M_{opt}}=A^+b

2.总结

本期的论文讲解到此，下一期我们来讲讲另一篇论文。

相关文章