ICP 问题之 SVD

欢迎访问我的博客首页。

---

  ICP 即 Iterative Closest Point，迭代最近点。已知三维点在两个坐标系中的坐标，求这两个坐标系的变换关系，称为 ICP 问题。

1. SVD 求 ICP

---

1.1 构造最小二乘问题

---

  假设从空间坐标系 $O_1$ 到空间坐标系 $O_2$ 的变换是 $(R,t)$，$p_1^i$ 是空间点在坐标系 $O_1$ 中的坐标，$p_2^i$ 是空间点在坐标系 $O_2$ 中的坐标。

图 1 ICP 问题

  一对匹配的空间点经过位姿变换的误差是

$$e_i=p_2^i-(R\cdot p_1^i+t)\text{\hspace{1em}(1)}$$

使用 n 对空间点的变换误差构造最小二乘问题

$$\underset{R,t}{\min }J=\frac{1}{2}\sum _{i=1}^n||p_2^i-(R\cdot p_1^i+t)|{|}_2^2\text{\hspace{1em}(2)}$$

  基于 SVD 的 ICP 算法就是通过求解上面的最小二乘问题得到位姿变换 $(R,t)$。求和的每一项是一个二范数的平方，表示二范数的下标可以省略。

1.2 求解最小二乘问题

---

  下面求解公式 (2)。首先定义质心坐标

$$\{\begin{array}{rl}{p}_1& =\frac{1}{n}\sum _{i=1}^n{p}_1^i\\ p_2& =\frac{1}{n}\sum _{i=1}^n{p}_2^i\end{array}$$

注意，空间点坐标与质心坐标的区别是有无上标。那么

$$\begin{array}{rl}\frac{1}{2}\sum _{i=1}^n||p_2^i-(R\cdot p_1^i+t)|{|}^2& =\frac{1}{2}\sum _{i=1}^n||p_2^i-R\cdot p_1^i-t|{|}^2\\ & =\frac{1}{2}\sum _{i=1}^n||p_2^i-R\cdot p_1^i-t-p_2+R\cdot p_1+p_2-R\cdot p_1)|{|}^2\\ & =\frac{1}{2}\sum _{i=1}^n||[p_2^i-p_2-R\cdot (p_1^i-p_1)]+(p_2-R\cdot p_1-t)|{|}^2\\ & =\frac{1}{2}\sum _{i=1}^n\{||p_2^i-p_2-R\cdot (p_1^i-p_1)|{|}^2+||p_2-R\cdot p_1-t|{|}^2\\ & +2[p_2^i-p_2-R\cdot (p_1^i-p_1){]}^T(p_2-R\cdot p_1-t)\}\\ & =\frac{1}{2}\sum _{i=1}^n[||p_2^i-p_2-R\cdot (p_1^i-p_1)|{|}^2+||p_2-R\cdot p_1-t|{|}^2].\end{array}\text{\hspace{1em}(3)}$$

公式 (3) 的推导用到了二范数。交叉项 $2[p_2^i-p_2-R\cdot (p_1^i-p_1){]}^T(p_2-R\cdot p_1-t)$ 累加后为 0，因此被舍弃。

  公式 (3) 可以看成两项相加的形式：第 1 项是第一个二范数累加，这一项只与旋转矩阵 $R$ 有关；第 2 项是第二个二范数累加，这一项与 $R$ 和平移向量 $t$ 都有关。因此可以令第 1 项为 0 求得 $R$，再令第 2 项为 0 求得 $t$。

1.3 求位姿

---

  假设 $p_1^i$ 和 $p_2^i$ 的去质心坐标分别是

$$\{\begin{array}{rl}{q}_1^i& =p_1^i-p_1\\ q_2^i& =p_2^i-p_2\end{array}$$

那么，令公式 (3) 的第 1 项为 0，就是

$$\frac{1}{2}\sum _{i=1}^n||q_2^i-R\cdot q_1^i|{|}^2=0$$

于是，求旋转矩阵就是求最小二乘问题

$$R^{\ast }=arg\underset{R}{\min }\frac{1}{2}\sum _{i=1}^n||q_2^i-R\cdot q_1^i|{|}^2$$

  求解 $R^{\ast }$ 的推理比较复杂，这里只给出计算方法：

$$\{\begin{array}{rl}W& =\sum _{i=1}^n(q_2^i\cdot {q_1^i}^T)\\ W& =U\Sigma V^T\\ R^{\ast }& =U{V}^T\end{array}$$

即，对求得的 W 进行 SVD 分解，得到 U 和 V，进而得到 $R^{\ast }$。然后令公式 (3) 的第二项为 0 即可求得平移向量

$$t^{\ast }=p_2-R^{\ast }\cdot p_1$$

2. 例子

---

2.1 代码实现

---

  下面是 ORB_SLAM2 的 EPnP 算法中，使用 SVD 求解 ICP 的代码：

// SVD 求解 ICP 问题：世界坐标系到相机坐标系的变换。
void PnPsolver::estimate_R_and_t(double R[3][3], double t[3]) {
    // 1. 求质心坐标 pc0、pw0。
    double pc0[3], pw0[3];
    pc0[0] = pc0[1] = pc0[2] = 0.0;
    pw0[0] = pw0[1] = pw0[2] = 0.0;

    for (int i = 0; i < number_of_correspondences; i++) {
        const double *pc = pcs + 3 * i;
        const double *pw = pws + 3 * i;
        for (int j = 0; j < 3; j++) {
            pc0[j] += pc[j];
            pw0[j] += pw[j];
        }
    }

    for (int j = 0; j < 3; j++) {
        pc0[j] /= number_of_correspondences;
        pw0[j] /= number_of_correspondences;
    }

    // 2. W = pc x pw^T.
    double abt[3 * 3];
    CvMat ABt = cvMat(3, 3, CV_64F, abt);
    cvSetZero(&ABt);
    for (int i = 0; i < number_of_correspondences; i++) {
        double *pc = pcs + 3 * i;
        double *pw = pws + 3 * i;

        for (int j = 0; j < 3; j++) {
            abt[3 * j + 0] += (pc[j] - pc0[j]) * (pw[0] - pw0[0]);
            abt[3 * j + 1] += (pc[j] - pc0[j]) * (pw[1] - pw0[1]);
            abt[3 * j + 2] += (pc[j] - pc0[j]) * (pw[2] - pw0[2]);
        }
    }

    // 3. ABt = ABt_U x ABt_D x ABt_V^T.
    double abt_d[3], abt_u[3 * 3], abt_v[3 * 3];
    CvMat ABt_D = cvMat(3, 1, CV_64F, abt_d);
    CvMat ABt_U = cvMat(3, 3, CV_64F, abt_u);
    CvMat ABt_V = cvMat(3, 3, CV_64F, abt_v);
    cvSVD(&ABt, &ABt_D, &ABt_U, &ABt_V, CV_SVD_MODIFY_A);
    // R = ABt_U x ABt_V^T.
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j);
    // 旋转矩阵的行列式要大于 0。
    const double det =
        R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] + R[0][2] * R[1][0] * R[2][1] -
        R[0][2] * R[1][1] * R[2][0] - R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1];
    if (det < 0) {
        R[2][0] = -R[2][0];
        R[2][1] = -R[2][1];
        R[2][2] = -R[2][2];
    }
    // t = pc0 - R x pw0.
    t[0] = pc0[0] - dot(R[0], pw0);
    t[1] = pc0[1] - dot(R[1], pw0);
    t[2] = pc0[2] - dot(R[2], pw0);
}

2.2 实例分析

---

  下面我们具体分析一个例子。假设从坐标系 $O_1$ 到坐标系 $O_2$ 的位姿变换是

$$R=\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 1\end{array}\right]t=\left[\begin{array}{c}0\\ -1\\ 0\end{array}\right]$$

1. 空间点在变换前后的坐标系中的坐标：

$O_1$	$O_1$
$p_1^1=(-4,2,1{)}^T$	$p_2^1=(2,3,1{)}^T$
$p_1^2=(1,2,3{)}^T$	$p_2^2=(2,-2,3{)}^T$
$p_1^3=(1,3,2{)}^T$	$p_2^3=(3,-2,2{)}^T$
$p_1^4=(2,1,1{)}^T$	$p_2^4=(1,-3,1{)}^T$
$p_1^5=(-1,4,2{)}^T$	$p_2^5=(4,0,2{)}^T$
$p_1^6=(7,0,3{)}^T$	$p_2^6=(0,-8,3{)}^T$

2. 空间点的质心坐标：

$O_1$	$O_2$
$p_1=(1,2,2{)}^T$	$p_2=(2,-2,2{)}^T$

3. 空间点的去质心坐标：

$O_1$	$O_2$
$q_1^1=(-5,0,-1{)}^T$	$q_2^1=(0,5,-1{)}^T$
$q_1^2=(0,0,1{)}^T$	$q_2^2=(0,0,1{)}^T$
$q_1^3=(0,1,0{)}^T$	$q_2^3=(1,0,0{)}^T$
$q_1^4=(1,-1,-1{)}^T$	$q_2^4=(-1,-1,-1{)}^T$
$q_1^5=(-2,2,0{)}^T$	$q_2^5=(2,2,0{)}^T$
$q_1^6=(6,-2,1{)}^T$	$q_2^6=(-2,-6,1{)}^T$

4. 矩阵 $W$：

$$W=\sum _{i=1}^6{q}_2^i\cdot {q_1^i}^T=\left[\begin{array}{ccc}-17& 10& -1\\ -66& 17& -10\\ 10& -1& 4\end{array}\right]$$

5. 矩阵 $W$ 奇异值分解：

$$W=\left[\begin{array}{ccc}-0.2631& 0.8835& -0.3876\\ -0.9540& -0.1784& 0.2410\\ 0.1438& 0.4332& 0.8898\end{array}\right]\cdot \left[\begin{array}{ccc}72.1947& 0& 0\\ 0& 6.0778& 0\\ 0& 0& 1.7275\end{array}\right]\cdot {\left[\begin{array}{ccc}0.9540& -0.2631& 0.1438\\ 0.1784& 0.8835& 0.4332\\ -0.2410& -0.3876& 0.8898\end{array}\right]}^T$$

6. 旋转矩阵 $R$：

$$R=\left[\begin{array}{ccc}-0.2631& 0.8835& 0.3876\\ -0.9540& -0.1784& -0.2410\\ 0.1438& 0.4332& -0.8898\end{array}\right]\cdot {\left[\begin{array}{ccc}0.9540& -0.2631& 0.1438\\ 0.1784& 0.8835& 0.4332\\ 0.2410& 0.3876& -0.8898\end{array}\right]}^T=\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 1\end{array}\right]\cdot$$

7. 平移向量 $t$：

$$t=p-R\cdot p^{\prime }=(0,-1,0{)}^T$$

  上面就是用三维点在两个坐标系中的坐标，通过奇异值分解，求两个坐标系变换关系的过程。

2.3 使用 Eigen 库

---

  Eigen 是一个开源的C++线性代数库，这里使用它帮助我们进行矩阵运算。

#include 
#include 
#include 
#include 

using namespace std;
using namespace cv;

void pose_estimation_3d3d(const vector<Point3f>& pts1,
			  const vector<Point3f>& pts2) {
	// 求质心坐标
	Point3f p1, p2;
	int N = pts1.size();
	for(int i=0; i<N; i++) {
		p1 += pts1[i];
		p2 += pts2[i];
	}
	p1 = Point3f(Vec3f(p1) / N);
	p2 = Point3f(Vec3f(p2) / N);

	// 求去质心坐标
	vector<Point3f> q1(N), q2(N);
	for(int i=0; i<N; i++) {
		q1[i] = pts1[i] - p1;
		q2[i] = pts2[i] - p2;
	}

	// 求和: W = E(q1 * q2^T)
	Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
	for(int i=0; i<N; i++) {
		W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) *
		     Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
	}
	cout << "W=" << endl << W << endl << endl;

	// 对W进行SVD分解
	Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU|Eigen::ComputeFullV);
	Eigen::Matrix3d U = svd.matrixU();
	Eigen::Matrix3d V = svd.matrixV();
	cout << "U=" << endl << U << endl << endl;
	cout << "V=" << endl << V << endl << endl;

	// 求R
	Eigen::Matrix3d R = U * (V.transpose());
	cout << "R=" << endl << fixed << setprecision(2) << R << endl << endl;

	// 求t
	Eigen::Vector3d t = Eigen::Vector3d(p1.x, p1.y, p1.z) - R * Eigen::Vector3d(p2.x, p2.y, p2.z);
	cout << "t=" << endl << t << endl;
}

int main (int argc, char** argv) {
	vector<Point3f> pts1, pts2;
	pts1.push_back(Point3f(2, 3, 1));
	pts1.push_back(Point3f(2, -2, 3));
	pts1.push_back(Point3f(3, -2, 2));
	pts1.push_back(Point3f(1, -3, 1));
	pts1.push_back(Point3f(4, 0, 2));
	pts1.push_back(Point3f(0, -8, 3));
	
	pts2.push_back(Point3f(-4, 2, 1));
	pts2.push_back(Point3f(1, 2, 3));
	pts2.push_back(Point3f(1, 3, 2));
	pts2.push_back(Point3f(2, 1, 1));
	pts2.push_back(Point3f(-1, 4, 2 ));
	pts2.push_back(Point3f(7, 0, 3));

	pose_estimation_3d3d(pts1, pts2);
}

  如果使用 CMake 编译，可以参考这个 CMakeLists.txt。

cmake_minimum_required(VERSION 2.8)
project(SVD)

find_package(OpenCV 3.1 REQUIRED)

include_directories( 
    ${OpenCV_INCLUDE_DIRS} 
    "/usr/include/eigen3/"
)

add_executable(svd svd.cpp)
target_link_libraries(svd 
	${OpenCV_LIBS}
)

3. 参考

---

1. 奇异值分解求旋转矩阵，知乎专栏。