点云精配准

点云精细配准最传统的方法是ICP(Iterative Closest Point)

Point-to-Point ICP

现在假设有
n个目标点云\boldsymbol p_i^t,i = 1,\cdots,n
n个源点云\boldsymbol p_i^s,i = 1,\cdots,n
那么点云配准就是在求解最优的\boldsymbol R^*,\boldsymbol t^*使得满足如下的式子
\boldsymbol R^*,\boldsymbol t^* = \arg\min_{\boldsymbol R,\boldsymbol t} \frac{1}{n} \sum_{i =1}^{n} \| \boldsymbol R \boldsymbol p_i^s + \boldsymbol t - \boldsymbol p_i^t \|^2

如果认为第i个点拥有权重w_i,那么上面的式子将会变成
\boldsymbol R^*,\boldsymbol t^* = \arg\min_{\boldsymbol R,\boldsymbol t} \frac{1}{n} \sum_{i =1}^{n}w_i \| (\boldsymbol R \boldsymbol p_i^s + \boldsymbol t - \boldsymbol p_i^t) \|^2


\begin{aligned} F(\boldsymbol R, \boldsymbol t) =& \sum_{i =1}^{n}w_i \| \boldsymbol R \boldsymbol p_i^s + \boldsymbol t - \boldsymbol p_i^t \|^2 \\ =& \sum_{i =1}^{n}w_i (\boldsymbol R \boldsymbol p_i^s + \boldsymbol t - \boldsymbol p_i^t)^T (\boldsymbol R \boldsymbol p_i^s + \boldsymbol t - \boldsymbol p_i^t) \\ =& \sum_{i =1}^{n}w_i (\boldsymbol p_i^{sT} \boldsymbol R^T \boldsymbol R \boldsymbol p_i^s + 2\boldsymbol t^T \boldsymbol R \boldsymbol p_i^s-2\boldsymbol p_i^{tT} \boldsymbol R \boldsymbol p_i^s - 2\boldsymbol t^T \boldsymbol p_i^t + \boldsymbol p_i^{tT} \boldsymbol p_i^t + \boldsymbol t^T \boldsymbol t)\\ =& \sum_{i =1}^{n}w_i \boldsymbol p_i^{tT} \boldsymbol p_i^t + \sum_{i =1}^{n}w_i (\boldsymbol p_i^{sT} \boldsymbol R^T \boldsymbol R \boldsymbol p_i^s + 2\boldsymbol t^T \boldsymbol R \boldsymbol p_i^s-2\boldsymbol p_i^{tT} \boldsymbol R \boldsymbol p_i^s - 2\boldsymbol t^T \boldsymbol p_i^t + \boldsymbol t^T \boldsymbol t) \end{aligned}

假设\boldsymbol R已知的情况下,对\boldsymbol t求导可以得到
\begin{aligned} \frac{\partial F(\boldsymbol R, \boldsymbol t)}{\partial \boldsymbol t} \bigg|_{\boldsymbol t = \boldsymbol t^*} =& 0 \\ 2\sum_{i =1}^{n}w_i (\boldsymbol R \boldsymbol p_i^s - \boldsymbol p_i^t + \boldsymbol t^*) =& 0 \\ \boldsymbol t^* =& \frac{\sum_{i = 1}^n w_i \boldsymbol p_i^t}{\sum_{i = 1}^n w_i} - \boldsymbol R \frac{\sum_{i = 1}^n w_i \boldsymbol p_i^s}{\sum_{i = 1}^n w_i} \end{aligned}

定义\overline{\boldsymbol p^t} = \frac{\sum_{i = 1}^n w_i \boldsymbol p_i^t}{\sum_{i = 1}^n w_i}, \overline{\boldsymbol p^s} = \frac{\sum_{i = 1}^n w_i \boldsymbol p_i^s}{\sum_{i = 1}^n w_i},那么上式可以表达成
\boldsymbol t^* = \overline{\boldsymbol p^t} - \boldsymbol R \overline{\boldsymbol p^s}

这是固定\boldsymbol R下求解\boldsymbol t的方法,如果令\hat{\boldsymbol {p}_i^s} = \boldsymbol p_i^s - \overline{\boldsymbol p^s},\hat{\boldsymbol {p}_i^t} = \boldsymbol p_i^t - \overline{\boldsymbol p^t},那么有
\begin{aligned} F(\boldsymbol R, \boldsymbol t^*) =& \sum_{i =1}^{n}w_i \| \boldsymbol R \boldsymbol p_i^s + \boldsymbol t^* - \boldsymbol p_i^t \|^2 \\ =& \sum_{i =1}^{n}w_i \| \boldsymbol R \boldsymbol p_i^s + \overline{\boldsymbol p^t} - \boldsymbol R \overline{\boldsymbol p^s} - \boldsymbol p_i^t \|^2 \\ =& \sum_{i =1}^{n}w_i \| \boldsymbol R \hat{\boldsymbol p_i^s} - \hat{\boldsymbol p_i^t} \|^2 \\ =& \sum_{i =1}^{n}w_i (\boldsymbol R \hat{\boldsymbol p_i^s} - \hat{\boldsymbol p_i^t})^T (\boldsymbol R \hat{\boldsymbol p_i^s} - \hat{\boldsymbol p_i^t}) \\ =& \sum_{i =1}^{n}w_i \hat {\boldsymbol p_i^{tT}} \hat{\boldsymbol p_i^t} + \sum_{i =1}^{n}w_i (\hat {\boldsymbol p_i^{sT}} \boldsymbol R^T \boldsymbol R \hat{\boldsymbol p_i^s} - 2\hat{\boldsymbol p_i^{tT}} \boldsymbol R \hat{\boldsymbol p_i^s}) \end{aligned}

考虑到\boldsymbol R^T \boldsymbol R = \boldsymbol I,因此有
F(\boldsymbol R, \boldsymbol t) = \sum_{i =1}^{n}w_i \hat {\boldsymbol p_i^t}^T \hat{\boldsymbol p_i^t} + \sum_{i =1}^{n}w_i \hat {\boldsymbol p_i^s}^T \hat{\boldsymbol p_i^s} - 2 \sum_{i =1}^{n}w_i \hat{\boldsymbol p_i^t}^T \boldsymbol R \hat{\boldsymbol p_i^s}

因此
\begin{aligned} \boldsymbol R^* =& \arg \min_{\boldsymbol R} F(\boldsymbol R, \boldsymbol t^*) = \arg \max_{\boldsymbol R} \sum_{i =1}^{n}w_i \hat{\boldsymbol p_i^t}^T \boldsymbol R \hat{\boldsymbol p_i^s} \\ =& \arg\max_{\boldsymbol R} trace \bigg( \begin{bmatrix} w_1 & 0 & \cdots & 0 \\ 0 & w_2 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & w_n \end{bmatrix} \begin{bmatrix} \hat{\boldsymbol p_1^t}^T \\ \vdots \\ \hat{\boldsymbol p_n^t}^T \end{bmatrix} \boldsymbol R \begin{bmatrix} \hat{\boldsymbol p_1^s} & \hat{\boldsymbol p_2^s} & \cdots & \hat{\boldsymbol p_n^s} \end{bmatrix} \bigg)\\ =& \arg\max_{\boldsymbol R} trace(\boldsymbol W {\boldsymbol P^t}^T \boldsymbol R \boldsymbol P^s)\\ =& \arg\max_{\boldsymbol R} trace(\boldsymbol R \boldsymbol P^s \boldsymbol W {\boldsymbol P^t}^T) \end{aligned}

\boldsymbol P^s \boldsymbol W {\boldsymbol P^t}^T做 SVD 分解后得到的结果 \boldsymbol P^s \boldsymbol W {\boldsymbol P^t}^T = \boldsymbol U \boldsymbol \Lambda \boldsymbol V^T,则上式变为
\begin{aligned} \boldsymbol R^* =& \arg\max_{\boldsymbol R} trace(\boldsymbol R \boldsymbol P^s \boldsymbol W {\boldsymbol P^t}^T) \\ =& \arg\max_{\boldsymbol R} trace(\boldsymbol R \boldsymbol U \boldsymbol \Lambda \boldsymbol V^T)\\ =& \arg\max_{\boldsymbol R} trace(\boldsymbol \Lambda \boldsymbol V^T \boldsymbol R \boldsymbol U) \end{aligned}

考虑到\boldsymbol V^T \boldsymbol R \boldsymbol U为正交矩阵,该矩阵行(列)向量满足\boldsymbol x_i^T \boldsymbol x_i = 1,因此该矩阵内每个元素(包括对角线元素)均有x_{ij} \le 1,当 \boldsymbol V^T \boldsymbol R \boldsymbol U = \boldsymbol I 时,上式取到最大值,即
\boldsymbol R^* = \boldsymbol V \boldsymbol U^T

最后对Point-to-Point ICP配准进行总结:

  1. 从目标点云和源点云中找到距离小于\varepsilon的点对\boldsymbol p_i^t,\boldsymbol p_i^s
  2. 根据权重,计算等效中心\overline{\boldsymbol p^t} = \frac{\sum_{i = 1}^n w_i \boldsymbol p_i^t}{\sum_{i = 1}^n w_i}, \overline{\boldsymbol p^s} = \frac{\sum_{i = 1}^n w_i \boldsymbol p_i^s}{\sum_{i = 1}^n w_i}
  3. 将每个点去中心化\hat{\boldsymbol p_i^s} = \boldsymbol p_i^s - \overline{\boldsymbol p^s},\hat{\boldsymbol p_i^t} = \boldsymbol p_i^t - \overline{\boldsymbol p^t}
  4. 对协方差矩阵进行SVD分解\Sigma = \boldsymbol U \boldsymbol \Lambda \boldsymbol V^T,其中协方差矩阵\Sigma
    \Sigma = \begin{bmatrix} \hat{\boldsymbol p_1^s} & \hat{\boldsymbol p_2^s} & \cdots & \hat{\boldsymbol p_n^s} \end{bmatrix} \begin{bmatrix} w_1 & 0 & \cdots & 0 \\ 0 & w_2 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & w_n \end{bmatrix} \begin{bmatrix} \hat{\boldsymbol p_1^t}^T \\ \vdots \\ \hat{\boldsymbol p_n^t}^T \end{bmatrix}
  5. 求解得到旋转矩阵\boldsymbol R^* = \boldsymbol V \begin{bmatrix} 1 & 0 & \cdots & 0 \\ 0 & 1 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & |\boldsymbol V \boldsymbol U^T| \end{bmatrix}\boldsymbol U^T
  6. 求解得到平移向量\boldsymbol t^* = \overline{\boldsymbol p^t} - \boldsymbol R^* \overline{\boldsymbol p^s}

注:在三维情况下,需要求解旋转矩阵\boldsymbol R,如前面所示
\boldsymbol R^*= \arg \max_{\boldsymbol R} \sum_{i =1}^{n}w_i \hat{\boldsymbol {p}_i^t}^T \boldsymbol R \hat{\boldsymbol {p}_i^s}

在某些情况下该问题可以退化为BEV二维平面上的问题,忽略高度z,则上式变成
\begin{aligned} \boldsymbol R^* &= \arg \max_{\theta} \sum_{i =1}^{n}w_i \begin{bmatrix} \hat {x_i^t} & \hat {y_i^t} \end{bmatrix} \begin{bmatrix} \cos\theta & \sin\theta \\ -\sin\theta & \cos\theta \\ \end{bmatrix} \begin{bmatrix} \hat {x_i^s} \\ \hat {y_i^s} \end{bmatrix} \\ &= \arg \max_{\theta} \sum_{i =1}^{n}w_i [(\hat {x_i^t} \hat {x_i^s} + \hat {y_i^t} \hat {y_i^s}) \cos\theta + (\hat {x_i^t} \hat {y_i^s} - \hat {y_i^t} \hat {x_i^s}) \sin\theta] \\ &= \arg \max_{\theta} \alpha \cos\theta + \beta\sin\theta \end{aligned}

其中
\begin{aligned} \alpha &= \sum_{i =1}^{n}w_i (\hat {x_i^t} \hat {x_i^s} + \hat {y_i^t} \hat {y_i^s}) \\ \beta &= \sum_{i =1}^{n}w_i (\hat {x_i^t} \hat {y_i^s} - \hat {y_i^t} \hat {x_i^s}) \end{aligned}

上式的最优解在\theta = \arctan\frac{\beta}{\alpha}的时候得到,此时有
\begin{aligned} \cos\theta &= \frac{\alpha}{\sqrt{\alpha^2 + \beta^2}}\\ \sin\theta &= \frac{\beta}{\sqrt{\alpha^2 + \beta^2}} \end{aligned}

Point-to-Plane ICP

点面配准在点点配准的基础上,需要额外知道n个目标点云\boldsymbol p_i^t 所在表面的单位法向量 \boldsymbol n_i^t, i = 1,\cdots,n,而优化方程也变成了
\boldsymbol R^*,\boldsymbol t^* = \arg\min_{\boldsymbol R,\boldsymbol t} \frac{1}{n} \sum_{i =1}^{n}w_i [{\boldsymbol n_i^t}^T (\boldsymbol R \boldsymbol p_i^s + \boldsymbol t - \boldsymbol p_i^t)]^2

如果已经完成粗配准阶段,可以近似认为 \boldsymbol R \approx \boldsymbol I,如果用欧拉角来表示旋转矩阵则
\begin{aligned} \boldsymbol R =& \boldsymbol R_z(yall) \boldsymbol R_y(pitch) \boldsymbol R_x(roll) \\ =& \begin{bmatrix} \cos y & -\sin y & 0\\ \sin y & \cos y & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \cos p & 0 & \sin p\\ 0 & 1 & 0\\ -\sin p & 0 & \cos p \end{bmatrix} \begin{bmatrix} 1 & 0 & 0\\ 0 & \cos r & -\sin r\\ 0 & \sin r & \cos r\\ \end{bmatrix} \\ =& \begin{bmatrix} \cos y \cos p & \cos y \sin p \sin r - \sin y \cos r & \cos y \sin p \cos r + \sin y \sin r\\ \sin y \cos p & \sin y \sin p \sin r + \cos y \cos r & \sin y \sin p \cos r - \cos y \sin r\\ -\sin p & \cos p \sin r & \cos p \cos r \\ \end{bmatrix} \\ \approx& \begin{bmatrix} 1 & pr-y & p + yr\\ y & ypr + 1 & yp - r\\ -p & r & 1\\ \end{bmatrix} \\ \approx& \begin{bmatrix} 1 & -y & p\\ y & 1 & -r\\ -p & r & 1\\ \end{bmatrix} =\boldsymbol I + \begin{bmatrix} r\\ p\\ y \end{bmatrix}^\land \end{aligned}

原优化方程可以写成
\begin{aligned} &\frac{1}{n} \sum_{i =1}^{n}w_i [{\boldsymbol n_i^t}^T \cdot (\boldsymbol R \boldsymbol p_i^s + \boldsymbol t - \boldsymbol p_i^t)]^2 \\ \Rightarrow& \frac{1}{n} \sum_{i =1}^{n}w_i [{\boldsymbol n_i^t}^T \cdot (\boldsymbol p_i^s + \begin{bmatrix} r\\ p\\ y \end{bmatrix}^\land \boldsymbol p_i^s+ \boldsymbol t - \boldsymbol p_i^t)]^2 \\ \Rightarrow& \frac{1}{n} \sum_{i =1}^{n}w_i [{\boldsymbol n_i^t}^T \cdot (-{\boldsymbol p_i^s}^\land \begin{bmatrix} r\\ p\\ y \end{bmatrix} + \boldsymbol t + \boldsymbol p_i^s - \boldsymbol p_i^t)]^2 \\ \Rightarrow& \frac{1}{n} \sum_{i =1}^{n} \big [ \begin{bmatrix} -\sqrt{w_i}{\boldsymbol n_i^t}^T {\boldsymbol p_i^s}^\land & \sqrt{w_i}{\boldsymbol n_i^t}^T \end{bmatrix} \begin{bmatrix} r\\ p\\ y\\ \boldsymbol t \end{bmatrix} + \sqrt{w_i}{\boldsymbol n_i^t}^T(\boldsymbol p_i^s - \boldsymbol p_i^t) \big]^2\\ \Rightarrow& \frac{1}{n} \bigg\| \begin{bmatrix} -\sqrt{w_1}{\boldsymbol n_1^t}^T {\boldsymbol p_1^s}^\land & \sqrt{w_1}{\boldsymbol n_1^t}^T \\ \vdots & \vdots \\ -\sqrt{w_n}{\boldsymbol n_n^t}^T {\boldsymbol p_n^s}^\land & \sqrt{w_n}{\boldsymbol n_n^t}^T \end{bmatrix} \begin{bmatrix} r\\ p\\ y\\ \boldsymbol t \end{bmatrix} - \begin{bmatrix} \sqrt{w_1}{\boldsymbol n_1^t}^T(\boldsymbol p_1^t - \boldsymbol p_1^s) \\ \vdots \\ \sqrt{w_n}{\boldsymbol n_n^t}^T(\boldsymbol p_n^t - \boldsymbol p_n^s) \end{bmatrix} \bigg\|^2 \end{aligned}

该问题变成典型的 \| \boldsymbol A \boldsymbol x - \boldsymbol b \|^2 最小值问题,在超定情况下该问题的解为
\boldsymbol x = (\boldsymbol A^T \boldsymbol A)^{-1} \boldsymbol A^T \boldsymbol b = \boldsymbol V \boldsymbol \Lambda^+ \boldsymbol U^T \boldsymbol b

其中 \boldsymbol A = \boldsymbol U \boldsymbol{\Lambda} \boldsymbol V^T\boldsymbol{\Lambda}^+\boldsymbol{\Lambda}伪逆
\boldsymbol{\Lambda}^+ = \begin{bmatrix} \frac{1}{\lambda_1}\\ & \ddots\\ && \frac{1}{\lambda_m}\\ &&& 0\\ &&&& \ddots\\ &&&&& 0 \end{bmatrix}

最后对Point-to-Plane ICP配准进行总结:

  1. 从目标点云和源点云中找到距离小于\varepsilon的点对\boldsymbol p_i^t,\boldsymbol p_i^s
  2. 根据权重w_i,和目标点云的单位法向量\boldsymbol n_i^t,计算
    \begin{aligned} \boldsymbol A &= \begin{bmatrix} -\sqrt{w_1}{\boldsymbol n_1^t}^T {\boldsymbol p_1^s}^\land & \sqrt{w_1}{\boldsymbol n_1^t}^T \\ \vdots & \vdots \\ -\sqrt{w_n}{\boldsymbol n_n^t}^T {\boldsymbol p_n^s}^\land & \sqrt{w_n}{\boldsymbol n_n^t}^T \end{bmatrix} \\ \boldsymbol b &= \begin{bmatrix} \sqrt{w_1}{\boldsymbol n_1^t}^T(\boldsymbol p_1^t - \boldsymbol p_1^s) \\ \vdots \\ \sqrt{w_n}{\boldsymbol n_n^t}^T(\boldsymbol p_n^t - \boldsymbol p_n^s) \end{bmatrix} \end{aligned}
  3. 对矩阵 \boldsymbol A 进行 SVD 分解 \boldsymbol A = \boldsymbol U \boldsymbol{\Lambda} \boldsymbol V^T
  4. 得到结果 \begin{bmatrix} r\\ p\\ y\\ \boldsymbol t \end{bmatrix} = \boldsymbol V \boldsymbol \Lambda^+ \boldsymbol U^T \boldsymbol b,从中分离出\boldsymbol t,并且进一步求解出 \boldsymbol R = \begin{bmatrix} 1 & -y & p\\ y & 1 & -r\\ -p & r & 1 \end{bmatrix}
最后编辑于
©著作权归作者所有,转载或内容合作请联系作者
  • 序言:七十年代末,一起剥皮案震惊了整个滨河市,随后出现的几起案子,更是在滨河造成了极大的恐慌,老刑警刘岩,带你破解...
    沈念sama阅读 205,132评论 6 478
  • 序言:滨河连续发生了三起死亡事件,死亡现场离奇诡异,居然都是意外死亡,警方通过查阅死者的电脑和手机,发现死者居然都...
    沈念sama阅读 87,802评论 2 381
  • 文/潘晓璐 我一进店门,熙熙楼的掌柜王于贵愁眉苦脸地迎上来,“玉大人,你说我怎么就摊上这事。” “怎么了?”我有些...
    开封第一讲书人阅读 151,566评论 0 338
  • 文/不坏的土叔 我叫张陵,是天一观的道长。 经常有香客问我,道长,这世上最难降的妖魔是什么? 我笑而不...
    开封第一讲书人阅读 54,858评论 1 277
  • 正文 为了忘掉前任,我火速办了婚礼,结果婚礼上,老公的妹妹穿的比我还像新娘。我一直安慰自己,他们只是感情好,可当我...
    茶点故事阅读 63,867评论 5 368
  • 文/花漫 我一把揭开白布。 她就那样静静地躺着,像睡着了一般。 火红的嫁衣衬着肌肤如雪。 梳的纹丝不乱的头发上,一...
    开封第一讲书人阅读 48,695评论 1 282
  • 那天,我揣着相机与录音,去河边找鬼。 笑死,一个胖子当着我的面吹牛,可吹牛的内容都是我干的。 我是一名探鬼主播,决...
    沈念sama阅读 38,064评论 3 399
  • 文/苍兰香墨 我猛地睁开眼,长吁一口气:“原来是场噩梦啊……” “哼!你这毒妇竟也来了?” 一声冷哼从身侧响起,我...
    开封第一讲书人阅读 36,705评论 0 258
  • 序言:老挝万荣一对情侣失踪,失踪者是张志新(化名)和其女友刘颖,没想到半个月后,有当地人在树林里发现了一具尸体,经...
    沈念sama阅读 42,915评论 1 300
  • 正文 独居荒郊野岭守林人离奇死亡,尸身上长有42处带血的脓包…… 初始之章·张勋 以下内容为张勋视角 年9月15日...
    茶点故事阅读 35,677评论 2 323
  • 正文 我和宋清朗相恋三年,在试婚纱的时候发现自己被绿了。 大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
    茶点故事阅读 37,796评论 1 333
  • 序言:一个原本活蹦乱跳的男人离奇死亡,死状恐怖,灵堂内的尸体忽然破棺而出,到底是诈尸还是另有隐情,我是刑警宁泽,带...
    沈念sama阅读 33,432评论 4 322
  • 正文 年R本政府宣布,位于F岛的核电站,受9级特大地震影响,放射性物质发生泄漏。R本人自食恶果不足惜,却给世界环境...
    茶点故事阅读 39,041评论 3 307
  • 文/蒙蒙 一、第九天 我趴在偏房一处隐蔽的房顶上张望。 院中可真热闹,春花似锦、人声如沸。这庄子的主人今日做“春日...
    开封第一讲书人阅读 29,992评论 0 19
  • 文/苍兰香墨 我抬头看了看天上的太阳。三九已至,却和暖如春,着一层夹袄步出监牢的瞬间,已是汗流浃背。 一阵脚步声响...
    开封第一讲书人阅读 31,223评论 1 260
  • 我被黑心中介骗来泰国打工, 没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留,地道东北人。 一个月前我还...
    沈念sama阅读 45,185评论 2 352
  • 正文 我出身青楼,却偏偏与公主长得像,于是被迫代替她去往敌国和亲。 传闻我的和亲对象是个残疾皇子,可洞房花烛夜当晚...
    茶点故事阅读 42,535评论 2 343

推荐阅读更多精彩内容