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 构造最小二乘问题
假设从空间坐标系 O1O_1O1 到空间坐标系 O2O_2O2 的变换是 (R,t⃗)(R, \vec t \,)(R,t)，p1ip_1^ip1i 是空间点在坐标系 O1O_1O1 中的坐标，p2ip_2^ip2i 是空间点在坐标系 O2O_2O2 中的坐标。
图 1 ICP 问题
一对匹配的空间点经过位姿变换的误差是
ei=p2i−(R⋅p1i+t⃗)(1)e_i = p_2^i - (R \cdot p_1^i + \vec t \,) \tag{1} ei=p2i−(R⋅p1i+t)(1)
使用 n 对空间点的变换误差构造最小二乘问题
minR,t⃗J=12∑i=1n∣∣p2i−(R⋅p1i+t⃗)∣∣22(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,tminJ=21i=1∑n∣∣p2i−(R⋅p1i+t)∣∣22(2)
基于 SVD 的 ICP 算法就是通过求解上面的最小二乘问题得到位姿变换 (R,t⃗)(R, \vec t \,)(R,t)。求和的每一项是一个二范数的平方，表示二范数的下标可以省略。
1.2 求解最小二乘问题
下面求解公式 (2)。首先定义质心坐标
{p1=1n∑i=1np1ip2=1n∑i=1np2i\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=1∑np1i=n1i=1∑np2i
注意，空间点坐标与质心坐标的区别是有无上标。那么
12∑i=1n∣∣p2i−(R⋅p1i+t⃗)∣∣2=12∑i=1n∣∣p2i−R⋅p1i−t⃗∣∣2=12∑i=1n∣∣p2i−R⋅p1i−t⃗−p2+R⋅p1+p2−R⋅p1)∣∣2=12∑i=1n∣∣[p2i−p2−R⋅(p1i−p1)]+(p2−R⋅p1−t⃗)∣∣2=12∑i=1n{∣∣p2i−p2−R⋅(p1i−p1)∣∣2+∣∣p2−R⋅p1−t⃗∣∣2+2[p2i−p2−R⋅(p1i−p1)]T(p2−R⋅p1−t⃗)}=12∑i=1n[∣∣p2i−p2−R⋅(p1i−p1)∣∣2+∣∣p2−R⋅p1−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=1∑n∣∣p2i−(R⋅p1i+t)∣∣2=21i=1∑n∣∣p2i−R⋅p1i−t∣∣2=21i=1∑n∣∣p2i−R⋅p1i−t−p2+R⋅p1+p2−R⋅p1)∣∣2=21i=1∑n∣∣[p2i−p2−R⋅(p1i−p1)]+(p2−R⋅p1−t)∣∣2=21i=1∑n{∣∣p2i−p2−R⋅(p1i−p1)∣∣2+∣∣p2−R⋅p1−t∣∣2+2[p2i−p2−R⋅(p1i−p1)]T(p2−R⋅p1−t)}=21i=1∑n[∣∣p2i−p2−R⋅(p1i−p1)∣∣2+∣∣p2−R⋅p1−t∣∣2].(3)
公式 (3) 的推导用到了二范数。交叉项 2[p2i−p2−R⋅(p1i−p1)]T(p2−R⋅p1−t⃗)2[p_2^i - p_2 - R \cdot (p_1^i - p_1)]^T (p_2 - R \cdot p_1 - \vec t)2[p2i−p2−R⋅(p1i−p1)]T(p2−R⋅p1−t) 累加后为 0，因此被舍弃。
公式 (3) 可以看成两项相加的形式：第 1 项是第一个二范数累加，这一项只与旋转矩阵 RRR 有关；第 2 项是第二个二范数累加，这一项与 RRR 和平移向量 t⃗\vec tt 都有关。因此可以令第 1 项为 0 求得 RRR，再令第 2 项为 0 求得 t⃗\vec tt。
1.3 求位姿
假设 p1ip_1^ip1i 和 p2ip_2^ip2i 的去质心坐标分别是
{q1i=p1i−p1q2i=p2i−p2\left\{\begin{aligned} q_1^i &= p_1^i - p_1 \\ q_2^i &= p_2^i - p_2 \end{aligned}\right. {q1iq2i=p1i−p1=p2i−p2
那么，令公式 (3) 的第 1 项为 0，就是
12∑i=1n∣∣q2i−R⋅q1i∣∣2=0\frac{1}{2} \sum_{i=1}^n || q_2^i - R \cdot q_1^i||^2 = 0 21i=1∑n∣∣q2i−R⋅q1i∣∣2=0
于是，求旋转矩阵就是求最小二乘问题
R∗=argminR12∑i=1n∣∣q2i−R⋅q1i∣∣2R^* = arg \min_{R} \frac{1}{2} \sum_{i=1}^n ||q_2^i - R \cdot q_1^i||^2 R∗=argRmin21i=1∑n∣∣q2i−R⋅q1i∣∣2
求解 R∗R^*R∗ 的推理比较复杂，这里只给出计算方法：
{W=∑i=1n(q2i⋅q1iT)W=UΣVTR∗=UVT\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=1∑n(q2i⋅q1iT)=UΣVT=UVT
即，对求得的 W 进行 SVD 分解，得到 U 和 V，进而得到 R∗R^*R∗。然后令公式 (3) 的第二项为 0 即可求得平移向量
t∗=p2−R∗⋅p1t^* = p_2 - R^* \cdot p_1 t∗=p2−R∗⋅p1
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 实例分析
下面我们具体分析一个例子。假设从坐标系 O1O_1O1 到坐标系 O2O_2O2 的位姿变换是
R=[010−100001]t⃗=[0−10]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=⎣⎡0−10100001⎦⎤t=⎣⎡0−10⎦⎤
- 空间点在变换前后的坐标系中的坐标：
| O1O_1O1 | O1O_1O1 |
|---|---|
| p11=(−4,2,1)Tp_1^1=(-4,2,1)^Tp11=(−4,2,1)T | p21=(2,3,1)Tp_2^1=(2,3,1)^Tp21=(2,3,1)T |
| p12=(1,2,3)Tp_1^2=(1,2,3)^Tp12=(1,2,3)T | p22=(2,−2,3)Tp_2^2=(2,-2,3)^Tp22=(2,−2,3)T |
| p13=(1,3,2)Tp_1^3=(1,3,2)^Tp13=(1,3,2)T | p23=(3,−2,2)Tp_2^3=(3,-2,2)^Tp23=(3,−2,2)T |
| p14=(2,1,1)Tp_1^4=(2,1,1)^Tp14=(2,1,1)T | p24=(1,−3,1)Tp_2^4=(1, -3, 1)^Tp24=(1,−3,1)T |
| p15=(−1,4,2)Tp_1^5=(-1,4,2)^Tp15=(−1,4,2)T | p25=(4,0,2)Tp_2^5=(4, 0, 2)^Tp25=(4,0,2)T |
| p16=(7,0,3)Tp_1^6=(7,0,3)^Tp16=(7,0,3)T | p26=(0,−8,3)Tp_2^6=(0, -8, 3)^Tp26=(0,−8,3)T |
- 空间点的质心坐标：
| O1O_1O1 | O2O_2O2 |
|---|---|
| p1=(1,2,2)Tp_1=(1,2,2)^Tp1=(1,2,2)T | p2=(2,−2,2)Tp_2=(2,-2,2)^Tp2=(2,−2,2)T |
- 空间点的去质心坐标：
| O1O_1O1 | O2O_2O2 |
|---|---|
| q11=(−5,0,−1)Tq_1^1=(-5,0,-1)^Tq11=(−5,0,−1)T | q21=(0,5,−1)Tq_2^1=(0,5,-1)^Tq21=(0,5,−1)T |
| q12=(0,0,1)Tq_1^2=(0,0,1)^Tq12=(0,0,1)T | q22=(0,0,1)Tq_2^2=(0,0,1)^Tq22=(0,0,1)T |
| q13=(0,1,0)Tq_1^3=(0,1,0)^Tq13=(0,1,0)T | q23=(1,0,0)Tq_2^3=(1,0,0)^Tq23=(1,0,0)T |
| q14=(1,−1,−1)Tq_1^4=(1,-1,-1)^Tq14=(1,−1,−1)T | q24=(−1,−1,−1)Tq_2^4=(-1,-1,-1)^Tq24=(−1,−1,−1)T |
| q15=(−2,2,0)Tq_1^5=(-2,2,0)^Tq15=(−2,2,0)T | q25=(2,2,0)Tq_2^5=(2,2,0)^Tq25=(2,2,0)T |
| q16=(6,−2,1)Tq_1^6=(6,-2,1)^Tq16=(6,−2,1)T | q26=(−2,−6,1)Tq_2^6=(-2,-6,1)^Tq26=(−2,−6,1)T |
- 矩阵 WWW：
W=∑i=16q2i⋅q1iT=[−1710−1−6617−1010−14]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=1∑6q2i⋅q1iT=⎣⎡−17−66101017−1−1−104⎦⎤
- 矩阵 WWW 奇异值分解：
W=[−0.26310.8835−0.3876−0.9540−0.17840.24100.14380.43320.8898]⋅[72.19470006.07780001.7275]⋅[0.9540−0.26310.14380.17840.88350.4332−0.2410−0.38760.8898]TW = \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.2631−0.95400.14380.8835−0.17840.4332−0.38760.24100.8898⎦⎤⋅⎣⎡72.19470006.07780001.7275⎦⎤⋅⎣⎡0.95400.1784−0.2410−0.26310.8835−0.38760.14380.43320.8898⎦⎤T
- 旋转矩阵 RRR：
R=[−0.26310.88350.3876−0.9540−0.1784−0.24100.14380.4332−0.8898]⋅[0.9540−0.26310.14380.17840.88350.43320.24100.3876−0.8898]T=[010−100001]⋅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.2631−0.95400.14380.8835−0.17840.43320.3876−0.2410−0.8898⎦⎤⋅⎣⎡0.95400.17840.2410−0.26310.88350.38760.14380.4332−0.8898⎦⎤T=⎣⎡0−10100001⎦⎤⋅
- 平移向量 t⃗\vec tt：
t⃗=p−R⋅p′=(0,−1,0)T\vec t = p - R \cdot p' = (0,-1,0)^Tt=p−R⋅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;// 求REigen::Matrix3d R = U * (V.transpose());cout << "R=" << endl << fixed << setprecision(2) << R << endl << endl;// 求tEigen::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. 参考
- 奇异值分解求旋转矩阵，知乎专栏。
ICP 问题之 SVD相关推荐
- 六、3D-3D ICP问题线性SVD解法与非线性BA解法
ICP(Iterative Closest Point)求解点云数据对的变换矩阵 误差模型 线性解SVD,非线性解BA #include <iostream> #include <o ...
- 【动手学MVG】ICP算法原理和代码实现
介绍 本文介绍了相同尺度的点云ICP算法,若点云之间尺度不同,则需要估计尺度,可以看一下另一篇博文<通过ICP配准两组尺度不同点云, 统一坐标系(附代码)> 本文介绍的ICP问题,是在3D ...
- 点云配准NDT+ICP
点云配准NDT+ICP #include <iostream> #include <pcl/io/pcd_io.h> #include <pcl/point_types. ...
- SAC-IA粗配准+ICP精配准
最近一直在看点云配准相关的算法,在这里记录一下我试验过的配准算法. 第一弹:SAC-IA粗配准+ICP精配准 采样一致性初始配准算法(Sample Consensus Initial Aligment ...
- python 点云配准_点云配准(Registration)算法——以PCL为例
本文为PCL官方教程的Registration模块的中文简介版. An Overview of Pairwise Registration 点云配准包括以下步骤: from a set of poin ...
- SLAM系列——视觉里程计（特征法）
目录 1 特征点法 1.1 特征点 1.2 ORB特征 1.3 特征匹配 2 特征提取与匹配 3 2D-2D:对极几何 3.1 对极约束 3.2 本质矩阵 3.3 单应矩阵 讨论 4 实践:对极约束求 ...
- 视觉SLAM十四讲——ch7
视觉SLAM十四讲--ch7 ch7视觉里程计 本章目标: 1.理解图像特征点的意义,并掌握在单副图像中提取出特征点及多副图像中匹配特征点的方法 2.理解对极几何的原理,利用对极几何的约束,恢复出图像 ...
- ICP算法概述以及使用SVD进行算法推导
本文转载于B站UP主 摆烂世家的视频ICP算法概述及使用SVD推导(组会录像) 作者:苏浩田 (注:如有侵权,私信我下立马删咯) 1 ICP 算法 三维点云拼接技术在不同场合亦被称为重定位.配准或 ...
- 【手写ICP】ICP -SVD 手动实现与例程（上）
文章目录 1. 写在前面 1. 配和代码的 SVD-ICP 流程梳理 1.1 SVD-ICP 算法 5 步走 1.2 预处理 1.3 点云变换 1.4 通过 KD-tree 找最近匹配点 1.5 SV ...
最新文章
- sql实现两张表的拷贝
- linux POSIX 信号集,读书笔记：第10章 Posix信号量 (6)
- 围观云栖大会有感：从不“栖”而遇到后会有“栖”
- 2017-2018-1 20155338 《信息安全系统设计基础》 第三周学习总结
- CentOS下安装7-zip
- 黑侠百度URL批量推送程序
- 测试开发系类之接口自动化测试
- linux安装opencv让输入密码,linux下安装opencv的全过程（对初学者或者linux不熟悉的童鞋，非常适合）...
- matlab mxarray赋值,C++中数组与MATLAB mxArray相互赋值
- 百度地图和solr展示资源和附近等功能的实现 二
- orange's系统可以装mysql 吗?_bochs 2.4.2 ubuntu 安装运行问题《orange#39;s 一个操作系统的...
- CSV Converter Pro for Mac(CSV数据转换工具)
- FPGA——sdram控制器1
- HTML基础知识笔记
- Beginning Lua with World of Warcraft Add-ons第三章翻译总结及一些工具
- Googler为什么很幸福？
- EDA笔记(4)--语言要素
- 二维数组作为函数参数的传递
- 微型计算机2017年2月,2017年1-2月份规模以上工业增加值增长6.3%
- Visual Studio滚动条设置