ICP (Iterative Closest Point) 最近点迭代法，用于计算两个点集之间的运动。本文主要分为一下几部分：算法原理、算法实现、算法优化（用 kd-tree 实现匹配对应点）。
本文代码：https://gitee.com/simon12138/ICP
1. 算法原理
本部分参考了《SLAM 14讲》
我们有两个点集 A , B A, B A,B ，其中 B B B 点集经过一定的变化能得到 A A A ，设变化可表示为 p a , i = R ⋅ p b , i + t , p a , i ∈ A , p b , i ∈ B p_{a,i}=\textbf{R}\cdot p_{b,i} + \textbf{t}, \ p_{a,i}\in A, \ p_{b,i}\in B pa,i=R⋅pb,i+t, pa,i∈A, pb,i∈B
我们的目的就是求出
R
,
t
\textbf{R}, \textbf{t}
R,t ，现在式子里所有的量都为未知量，我们不知道两个点集间点的对应关系，所以 ICP 算法的第一步即为匹配两个点集间的点，我们认为
A
,
B
A,B
A,B 点集间空间上距离最近的点即为对应点。基于这个认定，我们遍历
A
A
A, 找到
B
B
B 中与之对应的点，于是我们便有了
p
a
,
1
,
p
a
,
2
,
.
.
.
,
p
a
,
n
p_{a,1} , p_{a,2} , ... , p_{a,n}
pa,1,pa,2,...,pa,n 与对应的
p
b
,
1
,
p
b
,
2
,
.
.
.
,
p
b
,
n
p_{b,1} , p_{b,2} , ... , p_{b,n}
pb,1,pb,2,...,pb,n 。则有：
R
,
t
=
arg
min
1
2
∑
i
=
1
n
∣
∣
p
a
,
i
−
(
R
⋅
p
b
,
i
+
t
)
∣
∣
2
\textbf{R},\textbf{t}=\arg\min\frac{1}{2}\sum_{i=1}^n ||\ \ p_{a,i}-(\textbf{R}\cdot p_{b,i} + \textbf{t})\ \ ||^2
R,t=argmin21i=1∑n∣∣ pa,i−(R⋅pb,i+t) ∣∣2
当然我们可以用解一般的非线性最小二乘问题的方法解决这个问题，比如用 ceres 开源库，这里我们介绍一下用 SVD (奇异值分解) 来解决这个问题。
首先定义两个点集的质心：
p
a
=
∑
i
=
1
n
p
a
,
i
,
p
b
=
∑
i
=
1
n
p
b
,
i
p_a=\sum_{i=1}^np_{a,i}\ \ ,\ \ p_b=\sum_{i=1}^np_{b,i}
pa=i=1∑npa,i , pb=i=1∑npb,i
则有：
1
2
∑
i
=
1
n
∣
∣
p
a
,
i
−
(
R
⋅
p
b
,
i
+
t
)
∣
∣
2
=
1
2
∑
i
=
1
n
∣
∣
(
p
a
,
i
′
−
R
⋅
p
b
,
i
′
)
+
(
p
a
−
(
R
⋅
p
b
+
t
)
)
∣
∣
2
\frac{1}{2}\sum_{i=1}^n ||\ \ p_{a,i}-(\textbf{R}\cdot p_{b,i} + \textbf{t})\ \ ||^2=\frac{1}{2}\sum_{i=1}^n ||\ \ (p_{a,i}'-\textbf{R}\cdot p_{b,i}')+(p_a-(\textbf{R}\cdot p_b + \textbf{t}))\ \ ||^2\\
21i=1∑n∣∣ pa,i−(R⋅pb,i+t) ∣∣2=21i=1∑n∣∣ (pa,i′−R⋅pb,i′)+(pa−(R⋅pb+t)) ∣∣2
其中 p a ( b ) , i ′ p_{a(b),i}' pa(b),i′ 为各自质心参考系下点的坐标。
化简得：
1
2
∑
i
=
1
n
∣
∣
p
a
,
i
′
−
R
⋅
p
b
,
i
′
∣
∣
2
+
∣
∣
p
a
−
(
R
⋅
p
b
+
t
)
∣
∣
2
\frac{1}{2}\sum_{i=1}^n ||\ \ p_{a,i}'-\textbf{R}\cdot p_{b,i}'\ \ ||^2+||\ \ p_a-(\textbf{R}\cdot p_b+\textbf{t})\ \ ||^2
21i=1∑n∣∣ pa,i′−R⋅pb,i′ ∣∣2+∣∣ pa−(R⋅pb+t) ∣∣2
易得：
R
=
arg
min
1
2
∑
i
=
1
n
∣
∣
p
a
,
i
′
−
R
⋅
p
b
,
i
′
∣
∣
2
\textbf{R} = \arg\min \frac{1}{2}\sum_{i=1}^n ||\ \ p_{a,i}'-\textbf{R}\cdot p_{b,i}'\ \ ||^2
R=argmin21i=1∑n∣∣ pa,i′−R⋅pb,i′ ∣∣2
t = p a − R ⋅ p b \textbf{t}=p_a-\textbf{R}\cdot p_b t=pa−R⋅pb
则我们要求解的最小二乘问题为：
R
=
arg
min
∑
i
=
1
n
−
p
a
,
i
′
T
⋅
R
⋅
p
b
,
i
′
=
arg
min
−
t
r
(
R
⋅
∑
i
=
1
n
p
a
,
i
′
T
⋅
p
b
,
i
′
)
\textbf{R} = \arg\min\sum_{i=1}^n -\ p_{a,i}'^T\cdot\textbf{R}\cdot p_{b,i}'=\arg\min-tr(\textbf{R}\cdot\sum_{i=1}^n p_{a,i}'^T\cdot p_{b,i}')
R=argmini=1∑n− pa,i′T⋅R⋅pb,i′=argmin−tr(R⋅i=1∑npa,i′T⋅pb,i′)
令：
W
=
∑
i
=
1
n
p
a
,
i
′
T
⋅
p
b
,
i
′
\textbf{W}=\sum_{i=1}^n p_{a,i}'^T\cdot p_{b,i}'
W=i=1∑npa,i′T⋅pb,i′
对
W
\textbf{W}
W 进行 SVD 分解得：
W
=
U
Σ
V
T
\textbf{W}=\textbf{U}\Sigma\textbf{V}^T
W=UΣVT ，当
W
\textbf{W}
W 满秩时：
R
=
U
V
T
\textbf{R} = \textbf{U}\textbf{V}^T
R=UVT
代入之前的式子便能求出 t \textbf{t} t 。
综上，ICP 算法的步骤为：
初始：
平
均
误
差
=
1
,
A
,
B
,
R
=
o
n
e
s
(
3
,
3
)
,
t
=
z
e
r
o
s
(
3
,
1
)
平均误差=1, \ A,\ B,\ \textbf{R}=ones(3,3),\ \textbf{t}=zeros(3,1)
平均误差=1, A, B, R=ones(3,3), t=zeros(3,1)
当平均误差小于最大误差时循环：
搜索匹配点；计算平均误差 (因为搜索匹配点的时候能顺便计算每点的误差) ；
计算
d
R
,
d
t
d\textbf{R},d\textbf{t}
dR,dt ；
更新
B
,
R
=
d
R
⋅
R
,
t
=
d
R
⋅
t
+
t
B, \ \textbf{R}=d\textbf{R}\cdot\textbf{R},\textbf{t}=d\textbf{R}\cdot\textbf{t}+\textbf{t}
B, R=dR⋅R,t=dR⋅t+t
结束： 输出结果。
2. 算法实现
这部分就直接给代码了，在这里找了一个点云集：《The Stanford 3D Scanning Repository》。
ICP.cpp
ICP.h 就是 ICP.cpp 的头文件，里面 include 了一些要用到的库，比如 opencv 、Eigen 、STL 。
#include "ICP.h"
#include "kdTree.h" // 这部分用不到
double distance(const Point3d& a, const Point3d& b) {
double x2 = (a.x-b.x) * (a.x-b.x);
double y2 = (a.y-b.y) * (a.y-b.y);
double z2 = (a.z-b.z) * (a.z-b.z);
return std::sqrt(x2+y2+z2);
}
// 暴力搜索 B 点集里距离 a 最近的点，返回点在 B 点集的位置
int FindClosedPoint(const vector<Point3d>& B, const Point3d& a) {
int index = 0;
double dis = distance(a, B[0]);
for (int i=1; i<B.size(); i++) {
double d = distance(a, B[i]);
if (d < dis) {
dis = d;
index = i;
}
}
return index;
}
// 旋转整个点集，用于更新 B 点集
void rotate_ensemble(const vector<Point3d>& B, vector<Point3d>& C,
const Eigen::Matrix3d& R, const Eigen::Vector3d& t) {
C.clear();
for (int i=0; i<B.size(); i++) {
Vector3d b_original(B[i].x, B[i].y, B[i].z);
Vector3d c_transformed = R * b_original + t;
Point3d c_trans(c_transformed(0,0), c_transformed(1,0), c_transformed(2,0));
C.push_back(c_trans);
}
}
void pose_estimate_ICP(const vector<cv::Point3d>& A,
vector<cv::Point3d>& B,
Eigen::Matrix3d& R, Eigen::Vector3d& t) {
// 初始化
R = Eigen::MatrixXd::Identity(3,3);
t = Eigen::MatrixXd::Zero(3,1);
double max_error = 1e-7;
double average_error = 1;
int loop = 1;
int itermax = 200;
int N = A.size();
vector<int> matched_index(N); // 用于存放 B 点集匹配点的位置
vector<Point3d> C(B.size()); // 用于临时存放 旋转后的 B 点集
double sum_error; // 用于存放误差的和
while ( average_error > max_error && loop < itermax ) {
cout << "loop: " << loop++ << endl;
matched_index.clear();
// 搜索最近点匹配，顺带计算平均误差
sum_error = 0;
for ( int i=0; i<N; i++ ) {
int index = FindClosedPoint(B, A[i]);
matched_index.push_back(index);
double d = distance(B[index], A[i]);
sum_error += d;
}
average_error = sum_error / N;
// 计算质心
Point3d p1, p2;
for ( int i=0; i<N; i++ )
{
p1 += A[i];
p2 += B[matched_index[i]];
}
p1 = Point3d( Vec3d(p1) / N);
p2 = Point3d( Vec3d(p2) / N);
// 去质心
vector<Point3d> q1 (N), q2(N);
for ( int i=0; i<N; i++ )
{
q1[i] = A[i] - p1;
q2[i] = B[matched_index[i]] - p2;
}
// 计算 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();
}
// SVD 分解 W
Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
Eigen::Matrix3d d_R;
Eigen::Vector3d d_t;
d_R = U* ( V.transpose() );
d_t = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - d_R * Eigen::Vector3d ( p2.x, p2.y, p2.z );
// 更新 R，t
R = d_R * R;
t = d_R * t + d_t;
// 更新 B 点集
rotate_ensemble(B, C, d_R, d_t);
B.clear();
B.swap(C);
cout << "Average error = " << average_error << endl;
}
}
main.cpp
#include "ICP.h"
#include <chrono>
#include <stdlib.h>
#define PI 3.14159
int main(int argc, char** argv) {
// 文件格式就是一个点的信息占一行
if (argc != 2) {
cout << "usage: main filename\n";
return 1;
}
// 读取数据
string filename = argv[1];
ifstream fin(filename);
if (!fin) {
cout << "the file does not exit!\n";
return 1;
}
vector<Point3d> B;
Point3d temp;
int line = 1;
double tempp; // 临时储存点的除空间位置以外的信息
for (int i=0; i<num; i++) {
fin >> temp.x;
fin >> temp.y;
fin >> temp.z;
fin >> tempp;
fin >> tempp;
if (line++%2 == 1) B.push_back(temp); // 因为点太多了所以稀疏了一下
}
fin.close();
Eigen::AngleAxisd rotation_vector(PI/4, Eigen::Vector3d(0, 0, 1));
Eigen::Matrix3d R_real = rotation_vector.toRotationMatrix();
Eigen::Vector3d t_real(0.05, 0.05, 0.05);
// 创造 A 点集
vector<Point3d> A(B.size());
rotate_ensemble(B, A, R_real, t_real);
// 把初始数据写入文件
ofstream fout("initialA.dat");
for (int i=0; i<A.size(); i++) {
fout << A[i].x << " " << A[i].y << " " << A[i].z << endl;
}
fout.close();
fout.open("initialB.dat");
for (int i=0; i<B.size(); i++) {
fout << B[i].x << " " << B[i].y << " " << B[i].z << endl;
}
fout.close();
// 初始化，开始计时
Eigen::Matrix3d R;
Eigen::Vector3d t;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
pose_estimate_ICP(A, B, R, t);
// 输出时间和结果
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
cout << "R = \n";
cout << R; cout << endl;
cout << "t = "; cout << t.transpose(); cout << endl;
// 把最终点集写入文件
fout.open("finalA.dat");
for (int i=0; i<A.size(); i++) {
fout << A[i].x << " " << A[i].y << " " << A[i].z << endl;
}
fout.close();
fout.open("finalB.dat");
for (int i=0; i<B.size(); i++) {
fout << B[i].x << " " << B[i].y << " " << B[i].z << endl;
}
fout.close();
return 0;
}
3. 算法优化
这部分主要介绍如何用 kd-tree 优化搜索最邻近点。其实还有其他方面可以优化，比如如何处理误匹配点，点加入更多信息（颜色等）提高匹配精度。
3.1. 原理介绍
kd-tree 的构建：
kd-tree (k dimension tree) 类似于 search binary tree ，只是每一个节点的 value 的维度是k。
构建的思路如下：
- 选择一个维度，即 value 的其中一维，比如三维即为 (x, y, z) 其中之一。
- 若数据大小为一，则结束。若不为一，则按选择维度选取一个点分割数据，即所有此维度数据比这个点小的数据放在其左子树，大的放在右子树，选中的点作为 parent 。
- 对该点 left child 和 right child 做 1-2 的操作。
用图来表示如下：
 |  |
|---|
最邻近搜索：
最邻近搜索的基本思路是：从根节点开始，通过二叉树搜索，如果节点的分割维度值小于查找点的维度值表示查找点位于左子树空间中，则进入左子树，如果大于则进入右子树，直到达到叶子节点为止，将搜索路径上的每一个节点都加入到路径中；然后再回溯搜索路径，并判断未加入路径的其他子节点空间中是否可能有距离搜索点更近的节点，如果有可能，则遍历子节点空间，并将遍历到的节点加入到搜索路径中，重复这个过程直到搜索路径为空。
示意图如下：
流程如下：
- 开始依次把 ( 11 , 13 ) , ( 7 , 6 ) , ( 3 , 10 ) , ( 8 , 11 ) (11,13),(7,6),(3,10),(8,11) (11,13),(7,6),(3,10),(8,11) 加入路径。
- 更新路径为 ( 11 , 13 ) , ( 7 , 6 ) , ( 3 , 10 ) (11,13),(7,6),(3,10) (11,13),(7,6),(3,10)，计算与 ( 8 , 11 ) (8,11) (8,11) 的距离，此时最小距离为 + ∞ +\infin +∞ ，故更新最小距离。
- 更新路径为 ( 11 , 13 ) , ( 7 , 6 ) (11,13),(7,6) (11,13),(7,6)，计算与 ( 3 , 10 ) (3,10) (3,10) 的距离，该距离大于 d 0 d_0 d0，计算到 ( 3 , 10 ) (3,10) (3,10) 切分平面（这里是线）的距离，该距离大于 d 0 d_0 d0（这一步其实就是判断以当前最小距离为半径的圆（球面、超球面）与 ( 3 , 10 ) (3,10) (3,10) 的切分的另一部分有无交集，若有交集则把另一部分点按之前的方法加入路径） 。
- 更新路径为
(
11
,
13
)
(11,13)
(11,13)，计算与
(
7
,
6
)
(7,6)
(7,6) 的距离，结论同
2。 - 更新路径为
∅
\empty
∅，计算与
(
11
,
3
)
(11, 3)
(11,3) 的距离，结论同
2。 - 路径为空，结束并返回最小距离的点。
3.2. 代码部分
kdTree.h
#ifndef KDTREE_H
#define KDTREE_H
#include "ICP.h"
#include <algorithm>
#include <queue>
// 最临近点搜索时储存 node 和距离的结构体
struct NearestNode {
int node;
double distance;
NearestNode() {node=distance=0;}
NearestNode(int n, double d):node(n), distance(d) {}
};
struct cmp {
bool operator()(NearestNode a, NearestNode b) {return a.distance < b.distance;}
};
class kdTree {
public:
enum {X, Y, Z};
private:
int *treePtr;
int **tree;
int treeRoot;
int treeSize;
int kDimension;
vector<Point3d> data;
double getKval(const Point3d& p, int axis) {
switch (axis) {
case(X): return p.x;
case(Y): return p.y;
case(Z): return p.z;
default: cout << "Error!!\n"; return 0;
}
}
double computeDistance(const Point3d& p, int node) {
return distance(p, data[node]);
}
public:
kdTree(vector<Point3d>& B) {
data = B;
bool suc = setSize(3, B.size());
buildTree();
}
~kdTree() {}
bool setSize(int dim, unsigned int size) {
kDimension = dim;
treeSize = size;
if (kDimension > 0 && treeSize > 0) {
tree = new int *[4];
treePtr = new int[4 * treeSize];
for (int i=0; i<4; i++) tree[i] = treePtr + i * treeSize;
}
return true;
}
int buildTree() {
vector<int> vtr(treeSize);
for (int i=0; i<treeSize; i++) vtr[i] = i;
std::random_shuffle(vtr.begin(), vtr.end());
treeRoot = buildTree(&vtr[0], treeSize, -1);
return treeRoot;
}
// 选择切分维度，从方差最大的维度切分，顺便返回该维度的均值
int chooseSplitDimension(int *ids, int sz, double &key) {
int split = 0;
double *var = new double[kDimension];
double *mean = new double[kDimension];
double rt = 1.0 / sz;
for (int i=0; i< kDimension; i++) {
double sum1 = 0, sum2 = 0;
for (int j=0; j<sz; j++) {
sum1 += rt * getKval(data[ids[j]], i) * getKval(data[ids[j]], i);
sum2 += rt * getKval(data[ids[j]], i);
}
var[i] = sum1 - sum2 * sum2;
mean[i] = sum2;
}
double max = 0;
for (int i=0; i<kDimension; i++) {
if (var[i] > max) {
key = mean[i];
max = var[i];
split = i;
}
}
return split;
}
// 选择切分的数据点，按选择切分维度时求出来的均值把数据分成两部分
int chooseMiddleNode(int *ids, int sz, int dim, double key) {
int left = 0;
int right = sz - 1;
while (true) {
while (left <= right && getKval(data[ids[left]], dim) <= key) ++left;
while (left <= right && getKval(data[ids[right]], dim) >= key) --right;
if (left > right) break;
std::swap(ids[left], ids[right]);
++left;
--right;
}
double max = -9999999;
int maxIndex = 0;
for (int i=0; i<left; i++) {
if (getKval(data[ids[i]], dim) > max) {
max = getKval(data[ids[i]], dim);
maxIndex = i;
}
}
if (maxIndex != left - 1) std::swap(ids[maxIndex], ids[left-1]);
return left - 1;
}
int buildTree(int *indices, int count, int parent) {
if (count == 1) {
int rd = indices[0];
tree[0][rd] = 0;
tree[1][rd] = parent;
tree[2][rd] = -1;
tree[3][rd] = -1;
return rd;
}
else {
double key = 0;
int split = chooseSplitDimension(indices, count, key);
int idx = chooseMiddleNode(indices, count, split, key);
int rd = indices[idx];
tree[0][rd] = split;
tree[1][rd] = parent;
if (idx > 0) tree[2][rd] = buildTree(indices, idx, rd);
else tree[2][rd] = -1;
if (idx+1 < count) tree[3][rd] = buildTree(indices+idx+1, count-idx-1, rd);
else tree[3][rd] = -1;
return rd;
}
}
// 找最临近的k个点
int findKNearests(const Point3d& p, int k, int *res) {
std::priority_queue<NearestNode, std::vector<NearestNode>, cmp> kNeighbors;
std::stack<int> paths;
// 依次把点加入路径
int node = treeRoot;
while (node > -1) {
paths.emplace(node);
node = getKval(p, tree[0][node]) <= getKval(data[node], tree[0][node]) ? tree[2][node] : tree[3][node];
}
kNeighbors.emplace(-1, 9999999);
double distance = 0;
while (!paths.empty()) {
node = paths.top();
paths.pop();
distance = computeDistance(p, node);
// 比较到该点的距离与最临近点集中最大的距离
if (kNeighbors.size() < k) kNeighbors.emplace(node, distance);
else {
if (distance < kNeighbors.top().distance) {
kNeighbors.pop();
kNeighbors.emplace(node, distance);
}
}
// 判断未加入路径的其他点是否可能有更小的距离
if (tree[2][node] + tree[3][node] > -2) {
int dim = tree[0][node];
if (getKval(p, dim) > getKval(data[node], dim)) {
if (getKval(p, dim)-getKval(data[node], dim) < kNeighbors.top().distance && tree[2][node] > -1) {
int reNode = tree[2][node];
while (reNode > -1) {
paths.emplace(reNode);
reNode = getKval(p, tree[0][reNode]) <= getKval(data[reNode], tree[0][reNode]) ? tree[2][reNode] : tree[3][reNode];
}
}
}
else {
if (getKval(data[node], dim)-getKval(p, dim) < kNeighbors.top().distance && tree[3][node] > -1) {
int reNode = tree[3][node];
while (reNode > -1) {
paths.emplace(reNode);
reNode = getKval(p, tree[0][reNode]) <= getKval(data[reNode], tree[0][reNode]) ? tree[2][reNode] : tree[3][reNode];
}
}
}
}
}
int i = kNeighbors.size();
while (!kNeighbors.empty()) {
res[--i] = kNeighbors.top().node;
kNeighbors.pop();
}
return 0;
}
};
ICP.cpp 中 pose_estimate_ICP 函数的更改：
void pose_estimate_ICP(const vector<cv::Point3d>& A,
vector<cv::Point3d>& B,
Eigen::Matrix3d& R, Eigen::Vector3d& t) {
...
while ( average_error > max_error && loop < itermax ) {
cout << "loop: " << loop++ << endl;
matched_index.clear();
//建立kd-tree
kdTree KD(B);
sum_error = 0;
for ( int i=0; i<N; i++ ) {
int *res = new int[1];
// kd search
KD.findKNearests(A[i], 1, res);
int index = res[0];
matched_index.push_back(index);
double d = distance(B[index], A[i]);
error.push_back(d);
sum_error += d;
}
average_error = sum_error / N;
...
}
}
4. 结果
下图为优化前后的结果：
 |  |
|---|
下图为点集初始和最终的位置关系：
 |  |
|---|
用Matlab画的图，最终的图因为两个点集的点都几乎重叠在一起了，所以只显示了一种颜色，转个角度也可能所有点都变成蓝色的。
参考资料：
3054
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
