ICP 问题之 SVD

欢迎访问我的博客首页。


ICP 问题之 SVD

  • 1. SVD 求 ICP
    • 1.1 构造最小二乘问题
    • 1.2 求解最小二乘问题
    • 1.3 求位姿
  • 2. 例子
    • 2.1 代码实现
    • 2.2 实例分析
    • 2.3 使用 Eigen 库
  • 3. 参考

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

1. SVD 求 ICP


1.1 构造最小二乘问题


  假设从空间坐标系 O 1 O_1 O1 到空间坐标系 O 2 O_2 O2 的变换是 ( R , t ⃗   ) (R, \vec t \,) (R,t ) p 1 i p_1^i p1i 是空间点在坐标系 O 1 O_1 O1 中的坐标, p 2 i p_2^i p2i 是空间点在坐标系 O 2 O_2 O2 中的坐标。

ICP 问题之 SVD_第1张图片

图 1 ICP 问题

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

e i = p 2 i − ( R ⋅ p 1 i + t ⃗   ) (1) e_i = p_2^i - (R \cdot p_1^i + \vec t \,) \tag{1} ei=p2i(Rp1i+t )(1)

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

min ⁡ R , t ⃗ J = 1 2 ∑ i = 1 n ∣ ∣ p 2 i − ( R ⋅ p 1 i + t ⃗   ) ∣ ∣ 2 2 (2) \min_{R, \vec t} J = \frac{1}{2} \sum_{i=1}^n || p_2^i - (R \cdot p_1^i + \vec t \,) ||_2^2 \tag{2} R,t minJ=21i=1n∣∣p2i(Rp1i+t )22(2)

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

1.2 求解最小二乘问题


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

{ p 1 = 1 n ∑ i = 1 n p 1 i p 2 = 1 n ∑ i = 1 n p 2 i \left\{\begin{aligned} 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{aligned}\right. p1p2=n1i=1np1i=n1i=1np2i

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

1 2 ∑ i = 1 n ∣ ∣ p 2 i − ( R ⋅ p 1 i + t ⃗ ) ∣ ∣ 2 = 1 2 ∑ i = 1 n ∣ ∣ p 2 i − R ⋅ p 1 i − t ⃗ ∣ ∣ 2 = 1 2 ∑ i = 1 n ∣ ∣ p 2 i − R ⋅ p 1 i − t ⃗ − p 2 + R ⋅ p 1 + p 2 − R ⋅ p 1 ) ∣ ∣ 2 = 1 2 ∑ i = 1 n ∣ ∣ [ p 2 i − p 2 − R ⋅ ( p 1 i − p 1 ) ] + ( p 2 − R ⋅ p 1 − t ⃗ ) ∣ ∣ 2 = 1 2 ∑ i = 1 n { ∣ ∣ p 2 i − p 2 − R ⋅ ( p 1 i − p 1 ) ∣ ∣ 2 + ∣ ∣ p 2 − R ⋅ p 1 − t ⃗ ∣ ∣ 2 + 2 [ p 2 i − p 2 − R ⋅ ( p 1 i − p 1 ) ] T ( p 2 − R ⋅ p 1 − t ⃗ ) } = 1 2 ∑ i = 1 n [ ∣ ∣ p 2 i − p 2 − R ⋅ ( p 1 i − p 1 ) ∣ ∣ 2 + ∣ ∣ p 2 − R ⋅ p 1 − t ⃗ ∣ ∣ 2 ] . (3) \begin{aligned} \frac{1}{2} \sum_{i=1}^n || p_2^i - (R \cdot p_1^i + \vec t) ||^2 & = \frac{1}{2} \sum_{i=1}^n || p_2^i - R \cdot p_1^i - \vec t||^2 \\ & = \frac{1}{2} \sum_{i=1}^n || p_2^i - R \cdot p_1^i - \vec 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 - \vec 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 - \vec t ||^2 \\ &+ 2[p_2^i - p_2 - R \cdot (p_1^i - p_1)]^T (p_2 - R \cdot p_1 - \vec 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 - \vec t ||^2]. \end{aligned} \tag{3} 21i=1n∣∣p2i(Rp1i+t )2=21i=1n∣∣p2iRp1it 2=21i=1n∣∣p2iRp1it p2+Rp1+p2Rp1)2=21i=1n∣∣[p2ip2R(p1ip1)]+(p2Rp1t )2=21i=1n{∣∣p2ip2R(p1ip1)2+∣∣p2Rp1t 2+2[p2ip2R(p1ip1)]T(p2Rp1t )}=21i=1n[∣∣p2ip2R(p1ip1)2+∣∣p2Rp1t 2].(3)

公式 (3) 的推导用到了二范数。交叉项 2 [ p 2 i − p 2 − R ⋅ ( p 1 i − p 1 ) ] T ( p 2 − R ⋅ p 1 − t ⃗ ) 2[p_2^i - p_2 - R \cdot (p_1^i - p_1)]^T (p_2 - R \cdot p_1 - \vec t) 2[p2ip2R(p1ip1)]T(p2Rp1t ) 累加后为 0,因此被舍弃。

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

1.3 求位姿


  假设 p 1 i p_1^i p1i p 2 i p_2^i p2i 的去质心坐标分别是

{ q 1 i = p 1 i − p 1 q 2 i = p 2 i − p 2 \left\{\begin{aligned} q_1^i &= p_1^i - p_1 \\ q_2^i &= p_2^i - p_2 \end{aligned}\right. {q1iq2i=p1ip1=p2ip2

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

1 2 ∑ i = 1 n ∣ ∣ q 2 i − R ⋅ q 1 i ∣ ∣ 2 = 0 \frac{1}{2} \sum_{i=1}^n || q_2^i - R \cdot q_1^i||^2 = 0 21i=1n∣∣q2iRq1i2=0

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

R ∗ = a r g min ⁡ R 1 2 ∑ i = 1 n ∣ ∣ q 2 i − R ⋅ q 1 i ∣ ∣ 2 R^* = arg \min_{R} \frac{1}{2} \sum_{i=1}^n ||q_2^i - R \cdot q_1^i||^2 R=argRmin21i=1n∣∣q2iRq1i2

  求解 R ∗ R^* R 的推理比较复杂,这里只给出计算方法:

{ W = ∑ i = 1 n ( q 2 i ⋅ q 1 i T ) W = U Σ V T R ∗ = U V T \left\{\begin{aligned} W &= \sum_{i=1}^{n} (q_2^i \cdot {q_1^i}^T) \\ W &= U \Sigma V^T \\ R^* &= U V^T \end{aligned}\right. WWR=i=1n(q2iq1iT)=UΣVT=UVT

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

t ∗ = p 2 − R ∗ ⋅ p 1 t^* = p_2 - R^* \cdot p_1 t=p2Rp1

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_1 O1 到坐标系 O 2 O_2 O2 的位姿变换是

R = [ 0 1 0 − 1 0 0 0 0 1 ] t ⃗ = [ 0 − 1 0 ] R = \left[ \begin{matrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 1 \end{matrix} \right] \quad \quad \vec t = \left[ \begin{matrix} 0 \\ -1 \\ 0 \end{matrix} \right] R= 010100001 t = 010

  1. 空间点在变换前后的坐标系中的坐标:
O 1 O_1 O1 O 1 O_1 O1
p 1 1 = ( − 4 , 2 , 1 ) T p_1^1=(-4,2,1)^T p11=(4,2,1)T p 2 1 = ( 2 , 3 , 1 ) T p_2^1=(2,3,1)^T p21=(2,3,1)T
p 1 2 = ( 1 , 2 , 3 ) T p_1^2=(1,2,3)^T p12=(1,2,3)T p 2 2 = ( 2 , − 2 , 3 ) T p_2^2=(2,-2,3)^T p22=(2,2,3)T
p 1 3 = ( 1 , 3 , 2 ) T p_1^3=(1,3,2)^T p13=(1,3,2)T p 2 3 = ( 3 , − 2 , 2 ) T p_2^3=(3,-2,2)^T p23=(3,2,2)T
p 1 4 = ( 2 , 1 , 1 ) T p_1^4=(2,1,1)^T p14=(2,1,1)T p 2 4 = ( 1 , − 3 , 1 ) T p_2^4=(1, -3, 1)^T p24=(1,3,1)T
p 1 5 = ( − 1 , 4 , 2 ) T p_1^5=(-1,4,2)^T p15=(1,4,2)T p 2 5 = ( 4 , 0 , 2 ) T p_2^5=(4, 0, 2)^T p25=(4,0,2)T
p 1 6 = ( 7 , 0 , 3 ) T p_1^6=(7,0,3)^T p16=(7,0,3)T p 2 6 = ( 0 , − 8 , 3 ) T p_2^6=(0, -8, 3)^T p26=(0,8,3)T
  1. 空间点的质心坐标:
O 1 O_1 O1 O 2 O_2 O2
p 1 = ( 1 , 2 , 2 ) T p_1=(1,2,2)^T p1=(1,2,2)T p 2 = ( 2 , − 2 , 2 ) T p_2=(2,-2,2)^T p2=(2,2,2)T
  1. 空间点的去质心坐标:
O 1 O_1 O1 O 2 O_2 O2
q 1 1 = ( − 5 , 0 , − 1 ) T q_1^1=(-5,0,-1)^T q11=(5,0,1)T q 2 1 = ( 0 , 5 , − 1 ) T q_2^1=(0,5,-1)^T q21=(0,5,1)T
q 1 2 = ( 0 , 0 , 1 ) T q_1^2=(0,0,1)^T q12=(0,0,1)T q 2 2 = ( 0 , 0 , 1 ) T q_2^2=(0,0,1)^T q22=(0,0,1)T
q 1 3 = ( 0 , 1 , 0 ) T q_1^3=(0,1,0)^T q13=(0,1,0)T q 2 3 = ( 1 , 0 , 0 ) T q_2^3=(1,0,0)^T q23=(1,0,0)T
q 1 4 = ( 1 , − 1 , − 1 ) T q_1^4=(1,-1,-1)^T q14=(1,1,1)T q 2 4 = ( − 1 , − 1 , − 1 ) T q_2^4=(-1,-1,-1)^T q24=(1,1,1)T
q 1 5 = ( − 2 , 2 , 0 ) T q_1^5=(-2,2,0)^T q15=(2,2,0)T q 2 5 = ( 2 , 2 , 0 ) T q_2^5=(2,2,0)^T q25=(2,2,0)T
q 1 6 = ( 6 , − 2 , 1 ) T q_1^6=(6,-2,1)^T q16=(6,2,1)T q 2 6 = ( − 2 , − 6 , 1 ) T q_2^6=(-2,-6,1)^T q26=(2,6,1)T
  1. 矩阵 W W W

W = ∑ i = 1 6 q 2 i ⋅ q 1 i T = [ − 17 10 − 1 − 66 17 − 10 10 − 1 4 ] W = \sum_{i=1}^6 q_2^i \cdot {q_1^i}^T = \left[ \begin{matrix} -17 & 10 & -1 \\ -66 & 17 & -10 \\ 10 & -1 & 4 \end{matrix} \right] W=i=16q2iq1iT= 176610101711104

  1. 矩阵 W W W 奇异值分解:

W = [ − 0.2631 0.8835 − 0.3876 − 0.9540 − 0.1784 0.2410 0.1438 0.4332 0.8898 ] ⋅ [ 72.1947 0 0 0 6.0778 0 0 0 1.7275 ] ⋅ [ 0.9540 − 0.2631 0.1438 0.1784 0.8835 0.4332 − 0.2410 − 0.3876 0.8898 ] T W = \left[ \begin{matrix} -0.2631 & 0.8835 & -0.3876 \\ -0.9540 & -0.1784 & 0.2410 \\ 0.1438 & 0.4332 & 0.8898 \end{matrix} \right] \cdot \left[ \begin{matrix} 72.1947 & 0 & 0 \\ 0 & 6.0778 & 0 \\ 0 & 0& 1.7275 \end{matrix} \right] \cdot \left[ \begin{matrix} 0.9540 & -0.2631 & 0.1438 \\ 0.1784 & 0.8835 & 0.4332 \\ -0.2410 & -0.3876 & 0.8898 \end{matrix} \right] ^T W= 0.26310.95400.14380.88350.17840.43320.38760.24100.8898 72.19470006.07780001.7275 0.95400.17840.24100.26310.88350.38760.14380.43320.8898 T

  1. 旋转矩阵 R R R

R = [ − 0.2631 0.8835 0.3876 − 0.9540 − 0.1784 − 0.2410 0.1438 0.4332 − 0.8898 ] ⋅ [ 0.9540 − 0.2631 0.1438 0.1784 0.8835 0.4332 0.2410 0.3876 − 0.8898 ] T = [ 0 1 0 − 1 0 0 0 0 1 ] ⋅ R= \left[ \begin{matrix} -0.2631 & 0.8835 & 0.3876 \\ -0.9540 & -0.1784& -0.2410 \\ 0.1438 & 0.4332& -0.8898 \end{matrix} \right] \cdot \left[ \begin{matrix} 0.9540 & -0.2631 & 0.1438 \\ 0.1784 & 0.8835 & 0.4332 \\ 0.2410 & 0.3876& -0.8898 \end{matrix} \right] ^T = \left[ \begin{matrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0& 1 \end{matrix} \right] \cdot R= 0.26310.95400.14380.88350.17840.43320.38760.24100.8898 0.95400.17840.24100.26310.88350.38760.14380.43320.8898 T= 010100001

  1. 平移向量 t ⃗ \vec t t

t ⃗ = p − R ⋅ p ′ = ( 0 , − 1 , 0 ) T \vec t = p - R \cdot p' = (0,-1,0)^T t =pRp=(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. 奇异值分解求旋转矩阵,知乎专栏。

你可能感兴趣的:(SLAM,算法,slam,markdown)