icp的英文全稱為iterative closest point，即為迭代最近點。它在雷射雷達應用頻率很高，主要是在點雲配準領域。icp演算法在是是視覺slam中應用也非常多，這個演算法還是很重要。我們下面的討論還是基於視覺slam，好了我們開始吧！
icp演算法顧名思義，就是找最近點。演算法流程如下：
假設我們有一組配對好的3d點如下：
\[\boldsymbol=\left\_, \cdots, \boldsymbol_\right\}, \quad \boldsymbol^=\left\_^, \cdots, \boldsymbol_^\right\}
\]現在我們想要找到乙個歐式變換，使得
\[\forall i, \boldsymbol_=\boldsymbol \boldsymbol_^+\boldsymbol.
\]上述的這個問題就可以使用icp演算法求解。這裡主要需要注意下，如果僅考慮3d點之間的變換，此時和相機並沒有關係。在rgb-d slam中用icp問題指代匹配好的兩組點間的運動估計問題（跟雷射slam有區別，雷射slam通常是未知的情況）。icp求解主要是兩種方式，一種是svd，另一種是非線性優化方式求解。
根據前面描述的icp問題，令第\(i\)對點的誤差為：
\[\boldsymbol_=\boldsymbol_-\left(\boldsymbol \boldsymbol_^+\boldsymbol\right).
\]這裡解釋一下，我們用乙個點 \(\boldsymbol_^\)（第二幅圖中資料）來估算另乙個點\(\boldsymbol_\)時（在第一幅中資料，即為觀測座標），必然會產生誤差。
我們構建最小二乘問題，求解使用平方和打到極小的\(r\)和\(t\)，則有
\[\min _, \boldsymbol} \frac \sum_^\left\|\left(\boldsymbol_-\left(\boldsymbol \boldsymbol_^+\boldsymbol\right)\right)\right\|_^.
\]定義影象1和影象2中兩組點的之心為：
\[\boldsymbol=\frac \sum_^\left(\boldsymbol_\right), \quad \boldsymbol^=\frac \sum_^\left(\boldsymbol_^\right).
\]在誤差函式做如下處理（去質心處理）：
\[\begin
\frac \sum_^\left\|\boldsymbol_-\left(\boldsymbol \boldsymbol_^+\boldsymbol\right)\right\|^=& \frac \sum_^\left\|\boldsymbol_-\boldsymbol \boldsymbol_^-\boldsymbol-\boldsymbol+\boldsymbol \boldsymbol^+\boldsymbol-\boldsymbol \boldsymbol^\right\|^ \\
=& \frac \sum_^\left\|\left(\boldsymbol_-\boldsymbol-\boldsymbol\left(\boldsymbol_^-\boldsymbol^\right)\right)+\left(\boldsymbol-\boldsymbol \boldsymbol^-\boldsymbol\right)\right\|^ \\
=& \frac \sum_^\left(\left\|\boldsymbol_-\boldsymbol-\boldsymbol\left(\boldsymbol_^-\boldsymbol^\right)\right\|^+\left\|\boldsymbol-\boldsymbol \boldsymbol^-\boldsymbol\right\|^+\right.\\
&\left.2\left(\boldsymbol_-\boldsymbol-\boldsymbol\left(\boldsymbol_^-\boldsymbol^\right)\right)^}\left(\boldsymbol-\boldsymbol \boldsymbol^-\boldsymbol\right)\right)
\end
\]最終優化目標函式簡化為
\[\min _, t} j=\frac \sum_^\left\|\boldsymbol_-\boldsymbol-\boldsymbol\left(\boldsymbol_^-\boldsymbol^\right)\right\|^+\left\|\boldsymbol-\boldsymbol \boldsymbol^-\boldsymbol\right\|^.
\]我們可以先求左邊的第一項，我們記去質心座標為：
\[\boldsymbol_=\boldsymbol_-\boldsymbol, \quad \boldsymbol_^=\boldsymbol_^-\boldsymbol^.
\]由於左邊第一項只和\(r\)有關，因此我們先計算旋轉矩陣\(r\)，則
\[\boldsymbol^=\arg \min _} \frac \sum_^\left\|\boldsymbol_-\boldsymbol \boldsymbol_^\right\|^
\]我們將\(\boldsymbol^\)進行展開，則為
\[\frac \sum_^\left\|\boldsymbol_-\boldsymbol \boldsymbol_^\right\|^=\frac \sum_^\left(\boldsymbol_^} \boldsymbol_+\boldsymbol_^} \boldsymbol^} \boldsymbol \boldsymbol_^-2 \boldsymbol_^} \boldsymbol \boldsymbol_^\right)
\]由於\(\boldsymbol^} \boldsymbol=\boldsymbol\)，繼續化簡優化目標函式變為
\[\sum_^-\boldsymbol_^} \boldsymbol \boldsymbol_^=\sum_^-\operatorname\left(\boldsymbol \boldsymbol_^ \boldsymbol_^}\right)=-\operatorname\left(\boldsymbol \sum_^ \boldsymbol_^ \boldsymbol_^}\right)
\]為了解\(r\)，定義矩陣：
\[\boldsymbol=\sum_^ \boldsymbol_ \boldsymbol_^}.
\]使用svd分解，使得\(\boldsymbol=\boldsymbol \boldsymbol \boldsymbol^}\)，當\(\boldsymbol\)滿秩時，則
\[\boldsymbol=\boldsymbol \boldsymbol^}
\]解得\(r\)以後，\(t\)即為
\[t^=p-r p^
\]李代數表達位姿，迭代方式找到最優值。目標函式為：
\[\min _}=\frac \sum_^\left\|\left(\boldsymbol_-\exp \left(\boldsymbol^\right) \boldsymbol_^\right)\right\|_^
\]svd方法
void pose_estimation_3d3d(const vector&pts1, const vector&pts2, mat &r, mat &t)
p1 = point3f(vec3f(p1) / n);
p2 = point3f(vec3f(p2) / n);
vectorq1(n), q2(n); // remove the center
for (int i = 0; i < n; i++)
// compute q1*q2^t
eigen::matrix3d w = eigen::matrix3d::zero();
for (int i = 0; i < n; i++)
cout << "w=" << w << endl;
// svd on w
eigen::jacobisvdsvd(w, eigen::computefullu | eigen::computefullv);
eigen::matrix3d u = svd.matrixu();
eigen::matrix3d v = svd.matrixv();
cout << "u=" << u << endl;
cout << "v=" << v << endl;
eigen::matrix3d r_ = u * (v.transpose());
if (r_.determinant() < 0)
eigen::vector3d t_ = eigen::vector3d(p1.x, p1.y, p1.z) - r_ * eigen::vector3d(p2.x, p2.y, p2.z);
// convert to cv::mat
r = (mat_(3, 3) <<
r_(0, 0), r_(0, 1), r_(0, 2),
r_(1, 0), r_(1, 1), r_(1, 2),
r_(2, 0), r_(2, 1), r_(2, 2)
);t = (mat_(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}
視覺里程計學習
大四要做畢業設計了，因為保送去了外校，決定畢業 跟研究生導師做。研究生導師是做gps導航的，想發展視覺導航方向，就想讓我做視覺里程計，嘗試把視覺定位研究一下。基本沒有計算機視覺的基礎的我，就這樣子誤打誤撞進了計算機這個深坑。後來發現了清華博士高翔寫的閒半居士部落格，在這裡有了乙個簡單的入門。他這裡還...
里程計 推算定位與視覺里程計
以下內容翻譯自wiki百科。里程計是一種利用從移動感測器獲得的資料來估計物體位置隨時間的變化而改變的方法。該方法被用在許多種機械人系統 輪式或者腿式 上面，來估計，而不是確定這些機械人相對於初始位置移動的距離。這種方法對由速度對時間積分來求得位置的估計時所產生的誤差十分敏感。快速 精確的資料採集，裝...
里程計 推算定位與視覺里程計
以下內容翻譯自wiki百科。里程計是一種利用從移動感測器獲得的資料來估計物體位置隨時間的變化而改變的方法。該方法被用在許多種機械人系統 輪式或者腿式 上面，來估計，而不是確定這些機械人相對於初始位置移動的距離。這種方法對由速度對時間積分來求得位置的估計時所產生的誤差十分敏感。快速 精確的資料採集，裝...