ICP 问题之 SVD

qq_26697045

已于 2022-10-10 22:17:38 修改

阅读量3k

点赞数 8

分类专栏： SLAM 文章标签：算法 slam markdown

于 2020-01-03 16:35:49 首次发布

本文链接：https://blog.csdn.net/qq_26697045/article/details/103819368

版权

SLAM 专栏收录该内容

30 篇文章 12 订阅

订阅专栏

欢迎访问我的博客首页。

ICP 问题之 SVD

1. SVD 求 ICP
2. 例子
3. 参考

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

1. SVD 求 ICP

1.1 构造最小二乘问题

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

ICP

图 1 ICP 问题

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

$e_i = p_2^i - (R \cdot p_1^i + \vec t \,) \tag{1}$

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

$\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}$

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

1.2 求解最小二乘问题

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

$\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.$

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

$\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}$

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

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

1.3 求位姿

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

$\left\{\begin{aligned} q_1^i &= p_1^i - p_1 \\ q_2^i &= p_2^i - p_2 \end{aligned}\right.$

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

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

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

$R^* = arg \min_{R} \frac{1}{2} \sum_{i=1}^n ||q_2^i - R \cdot q_1^i||^2$

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

$\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.$

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

$t^* = p_2 - R^* \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$ 的位姿变换是

$\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]$

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

$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$

空间点的质心坐标：

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

空间点的去质心坐标：

$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$

矩阵 $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$ 奇异值分解：

$\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$

旋转矩阵 $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$

平移向量 $\vec t$ ：

$\vec t = p - R \cdot p' = (0,-1,0)^T$

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

2.3 使用 Eigen 库

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

#include <iostream>
#include <iomanip>
#include <opencv2/opencv.hpp>
#include <Eigen/SVD>

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. 参考

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

qq_26697045

关注

8
点赞
踩
34

收藏

觉得还不错? 一键收藏
5
评论
ICP 问题之 SVD

欢迎访问我的博客首页。ICP 之 SVD1. SVD法1.1 构造最小二乘问题1.2 求解最小二乘问题1.1 求旋转矩阵1.2 求平移向量1.3 例子2. 使用程序计算2.1 源码2.2 配置文件 ICP 即 Iterative Closest Point，迭代最近点。已知三维点在两个坐标系中的坐标，求这两个坐标系的变换关系，称为 ICP 问题。1. SVD法1.1 构造最小二乘问题假设从空间坐标系 O1O_1O1 到空间坐标系 O2O_2O2 的变换是 (R,t⃗ )(R, \.
复制链接

扫一扫