【點雲精配準】Iterative Closest Point（ICP）

1。 ICP 演算法簡介

通常，點雲配準分為兩步，先做粗配準，再做精配準：

粗配準（Coarse Global Registeration）：基於區域性幾何特徵

精配準（Fine Local Registeration）：需要初始位姿（initial alignment）

本文所講的 ICP 就是最常用的點雲精配准算法。

ICP 的經典論文：P。J。 Besl， A method for registration of 3-D shapes， 1992。

發表在 IEEE Transactions on Pattern Analysis and Machine Intelligence，此刊在工程技術大類屬於1區，算是神刊了。

【大類】工程技術（1區）；

【小類】COMPUTER SCIENCE， ARTIFICIAL INTELLIGENCE（1區）；ENGINEERING， ELECTRICAL & ELECTRONIC（1區）；

【2016-2018年平均IF】11。838；

【2017-2018年總被引】102333

進入正題……

ICP 演算法的第一步就是找到 Source 點雲與 Target 點雲中的對應點（corresponding point sets），然後針對對應點，透過最小二乘法構建目標函式，進行迭代最佳化。

1。1 估計對應點（

Correspondences estimation

）

ICP 稱為 Iterative Closest Point，顧名思義，是透過最近鄰法來估計對應點的。

對 Source 點雲中的一點，求解其與 Target 點雲中距離最近的那個點，作為其對應點。

當然，這樣操作的時間複雜度很高，為了加速計算，我們不需要計算 Target 點雲中每個點到 Source 點雲中一點的距離。可以設定一個閾值，當距離小於閾值時，就將其作為對應點。

還有一些其他加速求解對應點的方法，如 ANN（Approximate Nearest Neighbor）。

1。2 迭代最佳化

找到對應點後，我們就構建了兩組對應的點集：

$P = \left\{p_1, p_2, ..., p_n \right\} \\ Q = \left\{ q_1, q_2, ..., q_n \right\} \\$

求歐式變換

使得：

$\forall i, \quad q_i = Rp_{i} + t \\$

ICP 演算法基於最小二乘法進行迭代計算，使得誤差平方和達到極小值：

$\underset{R, t}{\operatorname{argmin}} \sum_{i=1}^{n}\left\|(Rp_i + t)-q_i\right\|^{2} \\$

（1）求解 translation

$F(\mathbf{t})=\sum_{i=1}^{n} \left\|\left(R \mathbf{p}_{i}+\mathbf{t}\right)-\mathbf{q}_{i}\right\|^{2} \\$

$F(\mathbf{t})$

對

求偏導，可得：

$\frac{\partial F}{\partial \mathbf{t}} =\sum_{i=1}^{n} 2 \left(R \mathbf{p}_{i}+\mathbf{t}-\mathbf{q}_{i}\right)=2 R\sum_{i=1}^{n} \mathbf{p}_{i} + 2 \mathbf{t} \cdot n-2 \sum_{i=1}^{n} \mathbf{q}_{i} \\$

令

$\frac{\partial F}{\partial \mathbf{t}}=0$

，求得：

$\mathbf{t} = \frac{1}{n}\sum_{i=1}^{n} \mathbf{q}_{i}-R\frac{1}{n} \sum_{i=1}^{n} \mathbf{p}_{i} \\$

定義質心：

$\bar{p} = \frac{1}{n}\sum_{i=1}^{n}{p_i} \\ \bar{q} = \frac{1}{n}\sum_{i=1}^{n}{q_i}$

因而，

$\mathbf{t} =\bar{q}- R\bar{p} \\$

將

代入

$F(\mathbf{t})$

可得：

$\begin{aligned} \sum_{i=1}^{n} \left\|\left(R \mathbf{p}_{i}+\mathbf{t}\right)-\mathbf{q}_{i}\right\|^{2} &=\sum_{i=1}^{n} \left\|R \mathbf{p}_{i}+\overline{\mathbf{q}}-R \overline{\mathbf{p}}-\mathbf{q}_{i}\right\|^{2}\\ &=\sum_{i=1}^{n} \left\|R\left(\mathbf{p}_{i}-\overline{\mathbf{p}}\right)-\left(\mathbf{q}_{i}-\overline{\mathbf{q}}\right)\right\|^{2} \end{aligned} \\$

定義去質心座標：

$x_i = \mathbf{p}_{i}-\overline{\mathbf{p}} \\ y_i = \mathbf{q}_{i}-\overline{\mathbf{q}} \\$

因此，目標函式變為：

$\underset{R}{\operatorname{argmin}} \sum_{i=1}^{n}\left\|R \mathbf{x}_{i}-\mathbf{y}_{i}\right\|^{2} \\$

（2）求解 rotation

$\begin{aligned} \left\|R \mathbf{x}_{i}-\mathbf{y}_{i}\right\|^{2} &=\left(R \mathbf{x}_{i}-\mathbf{y}_{i}\right)^{\top}\left(R \mathbf{x}_{i}-\mathbf{y}_{i}\right)\\ &=\left(\mathbf{x}_{i}^{\top} R^{\top}-\mathbf{y}_{i}^{\top}\right)\left(R \mathbf{x}_{i}-\mathbf{y}_{i}\right)\\ &=\mathbf{x}_{i}^{\top} R^{\top} R \mathbf{x}_{i}-\mathbf{y}_{i}^{\top} R \mathbf{x}_{i}-\mathbf{x}_{i}^{\top} R^{\top} \mathbf{y}_{i}+\mathbf{y}_{i}^{\top} \mathbf{y}_{i}\\ &=\mathbf{x}_{i}^{\top} \mathbf{x}_{i}-\mathbf{y}_{i}^{\top} R \mathbf{x}_{i}-\mathbf{x}_{i}^{\top} R^{\top} \mathbf{y}_{i}+\mathbf{y}_{i}^{\top} \mathbf{y}_{i} \end{aligned}$

$\mathbf{x}_{i}^{\top} R^{\top} \mathbf{y}_{i}$

是標量，對於任意標量

，都滿足

，因此，

$\mathbf{x}_{i}^{\top} R^{\top} \mathbf{y}_{i}=\left(\mathbf{x}_{i}^{\top} R^{\top} \mathbf{y}_{i}\right)^{\top}=\mathbf{y}_{i}^{\top} R \mathbf{x}_{i} \\$

$\left\|R \mathbf{x}_{i}-\mathbf{y}_{i}\right\|^{2}=\mathbf{x}_{i}^{\top} \mathbf{x}_{i}-2 \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}+\mathbf{y}_{i}^{\top} \mathbf{y}_{i} \\$

將其代入目標函式，可得：

$\begin{aligned} \underset{R}{\operatorname{argmin}} \sum_{i=1}^{n}\left\|R \mathbf{x}_{i}-\mathbf{y}_{i}\right\|^{2} &=\underset{R}{\operatorname{argmin}} \sum_{i=1}^{n} \left(\mathbf{x}_{i}^{\top} \mathbf{x}_{i}-2 \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}+\mathbf{y}_{i}^{\top} \mathbf{y}_{i}\right)\\ &=\underset{R}{\operatorname{argmin}}\left(\sum_{i=1}^{n} \mathbf{x}_{i}^{\top} \mathbf{x}_{i}-2 \sum_{i=1}^{n} \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}+\sum_{i=1}^{n} \mathbf{y}_{i}^{\top} \mathbf{y}_{i}\right)\\ &= \underset{R }{\operatorname{argmin}}\left(-2 \sum_{i=1}^{n} \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}\right) \end{aligned}$

因而，

$\underset{R }{\operatorname{argmin}}\left(-2 \sum_{i=1}^{n} \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}\right)=\underset{R}{\operatorname{argmax}} \sum_{i=1}^{n} \mathbf{y}_{i}^{\top} R \mathbf{x}_{i} \\$

由上圖可知，

$\sum_{i=1}^{n} \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}=\operatorname{tr}\left(Y^{\top} R X\right) =\operatorname{tr}\left(R XY^{\top} \right) \\$

其中，

和

為

$3\times n$

維矩陣。

定義協方差矩陣

$S = XY^{\top}$

，對

做 SVD 分解：

$S = U \Sigma V^{\top}\\$

因而，求解問題變為使下式最大化：

$tr\left(R X Y^{\top}\right)=tr(R S)=tr\left(R U \Sigma V^{\top}\right)=tr\left(\Sigma V^{\top} R U\right)$

令

$M = V^{\top}RU$

，其是正交陣，其列向量

是 orthonormal vectors，即

$m_j^{\top}m_j=1$

。因此，對

的所有元素都有

$m_{ij} \leq 1$

。

$\operatorname{tr}(\Sigma M)=\left(\begin{array}{cc} \sigma_{1} & \\ & \sigma_{2} \\ && \ddots & \\ && & \sigma_{d} \end{array}\right) \left(\begin{array}{cccc} m_{11} & m_{12} & \ldots & m_{1 d} \\ m_{21} & m_{22} & \cdots & m_{2 d} \\ \vdots & \vdots & \vdots & \vdots \\ m_{d 1} & m_{d 2} & \cdots & m_{d d} \end{array}\right)=\sum_{i=1}^{d} \sigma_{i} m_{i i} \leq \sum_{i=1}^{d} \sigma_{i}$

當

$m_{ii}=1$

時，

$\operatorname{tr}(\Sigma M)$

最大；

又是正交陣，因此

必為單位陣。

$I=M=V^{\top} R U \Rightarrow R=V U^{\top} \\$

以下求解 ICP 的程式碼來自：高翔，視覺SLAM十四講。

void

pose_estimation_3d3d

（

const

vector

Point3f

pts1

，

const

vector

Point3f

pts2

，

Mat

，

Mat

）

{

// center of mass

Point3f

，

；

int

pts1

。

size

（）；

for

（

int

；

）

{

pts1

［

］；

pts2

［

］；

}

；

// subtract COM

vector

Point3f

（

），

（

）；

for

（

int

；

）

{

［

］

pts1

［

］

；

［

］

pts2

［

］

；

}

// compute q1*q2^T

Eigen

：：

Matrix3d

Eigen

：：

Matrix3d

：：

Zero

（）；

for

（

int

；

）

{

Eigen

：：

Vector3d

（

［

］。

，

［

］。

，

［

］。

）

Eigen

：：

Vector3d

（

［

］。

，

［

］。

，

［

］。

）。

transpose

（）；

}

cout

“W=”

endl

；

// SVD on W

Eigen

：：

JacobiSVD

Eigen

：：

Matrix3d

svd

（

，

Eigen

：：

ComputeFullU

Eigen

：：

ComputeFullV

）；

Eigen

：：

Matrix3d

svd

。

matrixU

（）；

Eigen

：：

Matrix3d

svd

。

matrixV

（）；

cout

“U=”

endl

；

cout

“V=”

endl

；

Eigen

：：

Matrix3d

（

。

transpose

（））；

Eigen

：：

Vector3d

Eigen

：：

Vector3d

（

。

，

。

，

。

）

Eigen

：：

Vector3d

（

。

，

。

，

。

）；

// convert to cv：：Mat

（

Mat_

double

（

，

）

（

，

），

（

，

），

（

，

），

（

，

），

（

，

），

（

，

），

（

，

），

（

，

），

（

，

））；

（

Mat_

double

（

，

）

（

，

），

（

，

），

（

，

））；

}

2。 PCL 庫中的 ICP 演算法

PCL 庫中提供了以下 ICP 的介面及其變種：

點到點：pcl：：IterativeClosestPoint< PointSource， PointTarget， Scalar >

點到面：pcl：：IterativeClosestPointWithNormals< PointSource， PointTarget， Scalar >

面到面：pcl：：GeneralizedIterativeClosestPoint< PointSource， PointTarget >

……

其中，IterativeClosestPoint 模板類是 ICP 演算法的一個基本實現，其最佳化求解方法基於 Singular Value Decomposition （SVD），演算法迭代結束條件包括：

最大迭代次數：Number of iterations has reached the maximum user imposed number of iterations （via

setMaximumIterations

）

兩次變換矩陣之間的差值：The epsilon （difference） between the previous transformation and the current estimated transformation is smaller than an user imposed value （via

setTransformationEpsilon

）

均方誤差：The sum of Euclidean squared errors is smaller than a user defined threshold （via

setEuclideanFitnessEpsilon

）

基本用法：

IterativeClosestPoint

PointXYZ

，

PointXYZ

icp

；

// Set the input source and target

icp

。

setInputCloud

（

cloud_source

）；

icp

。

setInputTarget

（

cloud_target

）；

// Set the max correspondence distance to 5cm （e。g。， correspondences with higher distances will be ignored）

icp

。

setMaxCorrespondenceDistance

（

0。05

）；

// Set the maximum number of iterations （criterion 1）

icp

。

setMaximumIterations

（

）；

// Set the transformation epsilon （criterion 2）

icp

。

setTransformationEpsilon

（

1e-8

）；

// Set the euclidean distance difference epsilon （criterion 3）

icp

。

setEuclideanFitnessEpsilon

（

）；

// Perform the alignment

icp

。

align

（

cloud_source_registered

）；

// Obtain the transformation that aligned cloud_source to cloud_source_registered

Eigen

：：

Matrix4f

transformation

icp

。

getFinalTransformation

（）；

Demo

以下兩個demo 來自 PCL Tutorial

（1）Interactive ICP（互動式 ICP）

效果：按一下空格鍵，迭代一次

#include

// TicToc

typedef

pcl

：：

PointXYZ

PointT

；

typedef

pcl

：：

PointCloud

PointT

PointCloudT

；

bool

next_iteration

false

；

void

print4x4Matrix

（

const

Eigen

：：

Matrix4d

matrix

）

{

printf

（

“Rotation matrix ：

”

）；

printf

（

“ | %6。3f %6。3f %6。3f |

”

，

matrix

（

，

），

matrix

（

，

），

matrix

（

，

））；

printf

（

“R = | %6。3f %6。3f %6。3f |

”

，

matrix

（

，

），

matrix

（

，

），

matrix

（

，

））；

printf

（

“ | %6。3f %6。3f %6。3f |

”

，

matrix

（

，

），

matrix

（

，

），

matrix

（

，

））；

printf

（

“Translation vector ：

”

）；

printf

（

“t = < %6。3f， %6。3f， %6。3f >

\n\n

”

，

matrix

（

，

），

matrix

（

，

），

matrix

（

，

））；

}

void

keyboardEventOccurred

（

const

pcl

：：

visualization

：：

KeyboardEvent

event

，

void

nothing

）

{

（

event

。

getKeySym

（）

“space”

event

。

keyDown

（））

next_iteration

true

；

}

int

main

（

int

argc

，

char

argv

［］）

{

// The point clouds we will be using

PointCloudT

：：

Ptr

cloud_in

（

new

PointCloudT

）；

// Original point cloud

PointCloudT

：：

Ptr

cloud_tr

（

new

PointCloudT

）；

// Transformed point cloud

PointCloudT

：：

Ptr

cloud_icp

（

new

PointCloudT

）；

// ICP output point cloud

// Checking program arguments

（

argc

）

{

printf

（

“Usage ：

”

）；

printf

（

“

\t\t

%s file。ply number_of_ICP_iterations

”

，

argv

［

］）；

PCL_ERROR

（

“Provide one ply file。

”

）；

return

（

）；

}

int

iterations

；

// Default number of ICP iterations

（

argc

）

{

// If the user passed the number of iteration as an argument

iterations

atoi

（

argv

［

］）；

（

iterations

）

{

PCL_ERROR

（

“Number of initial iterations must be >= 1

”

）；

return

（

）；

}

pcl

：：

console

：：

TicToc

time

；

time

。

tic

（）；

（

pcl

：：

loadPLYFile

（

argv

［

］，

cloud_in

）

{

PCL_ERROR

（

“Error loading cloud %s。

”

，

argv

［

］）；

return

（

）；

}

std

：：

cout

“

Loaded file ”

argv

［

］

“ （”

cloud_in

size

（）

“ points） in ”

time

。

toc

（）

“ ms

”

std

：：

endl

；

// Defining a rotation matrix and translation vector

Eigen

：：

Matrix4d

transformation_matrix

Eigen

：：

Matrix4d

：：

Identity

（）；

// A rotation matrix （see https：//en。wikipedia。org/wiki/Rotation_matrix）

double

theta

M_PI

；

// The angle of rotation in radians

transformation_matrix

（

，

）

std

：：

cos

（

theta

）；

transformation_matrix

（

，

）

sin

（

theta

）；

transformation_matrix

（

，

）

sin

（

theta

）；

transformation_matrix

（

，

）

std

：：

cos

（

theta

）；

// A translation on Z axis （0。4 meters）

transformation_matrix

（

，

）

0。4

；

// Display in terminal the transformation matrix

std

：：

cout

“Applying this rigid transformation to： cloud_in -> cloud_icp”

std

：：

endl

；

print4x4Matrix

（

transformation_matrix

）；

// Executing the transformation

pcl

：：

transformPointCloud

（

cloud_in

，

cloud_icp

，

transformation_matrix

）；

cloud_tr

cloud_icp

；

// We backup cloud_icp into cloud_tr for later use

// The Iterative Closest Point algorithm

time

。

tic

（）；

pcl

：：

IterativeClosestPoint

PointT

，

PointT

icp

；

icp

。

setMaximumIterations

（

iterations

）；

icp

。

setInputSource

（

cloud_icp

）；

icp

。

setInputTarget

（

cloud_in

）；

icp

。

align

（

cloud_icp

）；

icp

。

setMaximumIterations

（

）；

// We set this variable to 1 for the next time we will call 。align （） function

std

：：

cout

“Applied ”

iterations

“ ICP iteration（s） in ”

time

。

toc

（）

“ ms”

std

：：

endl

；

（

icp

。

hasConverged

（））

{

std

：：

cout

“

ICP has converged， score is ”

icp

。

getFitnessScore

（）

std

：：

endl

；

std

：：

cout

“

ICP transformation ”

iterations

“ ： cloud_icp -> cloud_in”

std

：：

endl

；

transformation_matrix

icp

。

getFinalTransformation

（）。

cast

double

（）；

print4x4Matrix

（

transformation_matrix

）；

}

else

{

PCL_ERROR

（

“

ICP has not converged。

”

）；

return

（

）；

}

// Visualization

pcl

：：

visualization

：：

PCLVisualizer

viewer

（

“ICP demo”

）；

// Create two vertically separated viewports

int

（

）；

int

（

）；

viewer

。

createViewPort

（

0。0

，

0。0

，

0。5

，

1。0

，

）；

viewer

。

createViewPort

（

0。5

，

0。0

，

1。0

，

1。0

，

）；

// The color we will be using

float

bckgr_gray_level

0。0

；

// Black

float

txt_gray_lvl

1。0

bckgr_gray_level

；

// Original point cloud is white

pcl

：：

visualization

：：

PointCloudColorHandlerCustom

PointT

cloud_in_color_h

（

cloud_in

，

（

int

）

255

txt_gray_lvl

，

（

int

）

255

txt_gray_lvl

，

（

int

）

255

txt_gray_lvl

）；

viewer

。

addPointCloud

（

cloud_in

，

cloud_in_color_h

，

“cloud_in_v1”

，

）；

viewer

。

addPointCloud

（

cloud_in

，

cloud_in_color_h

，

“cloud_in_v2”

，

）；

// Transformed point cloud is green

pcl

：：

visualization

：：

PointCloudColorHandlerCustom

PointT

cloud_tr_color_h

（

cloud_tr

，

180

，

）；

viewer

。

addPointCloud

（

cloud_tr

，

cloud_tr_color_h

，

“cloud_tr_v1”

，

）；

// ICP aligned point cloud is red

pcl

：：

visualization

：：

PointCloudColorHandlerCustom

PointT

cloud_icp_color_h

（

cloud_icp

，

180

，

）；

viewer

。

addPointCloud

（

cloud_icp

，

cloud_icp_color_h

，

“cloud_icp_v2”

，

）；

// Adding text descriptions in each viewport

viewer

。

addText

（

“White： Original point cloud

Green： Matrix transformed point cloud”

，

txt_gray_lvl

，

txt_gray_lvl

，

txt_gray_lvl

，

“icp_info_1”

，

）；

viewer

。

addText

（

“White： Original point cloud

Red： ICP aligned point cloud”

，

txt_gray_lvl

，

txt_gray_lvl

，

txt_gray_lvl

，

“icp_info_2”

，

）；

std

：：

stringstream

；

iterations

；

std

：：

string

iterations_cnt

“ICP iterations = ”

。

str

（）；

viewer

。

addText

（

iterations_cnt

，

txt_gray_lvl

，

txt_gray_lvl

，

txt_gray_lvl

，

“iterations_cnt”

，

）；

// Set background color

viewer

。

setBackgroundColor

（

bckgr_gray_level

，

bckgr_gray_level

，

bckgr_gray_level

，

）；

viewer

。

setBackgroundColor

（

bckgr_gray_level

，

bckgr_gray_level

，

bckgr_gray_level

，

）；

// Set camera position and orientation

viewer

。

setCameraPosition

（

3。68332

，

2。94092

，

5。71266

，

0。289847

，

0。921947

，

0。256907

，

）；

viewer

。

setSize

（

1280

，

1024

）；

// Visualiser window size

// Register keyboard callback ：

viewer

。

registerKeyboardCallback

（

keyboardEventOccurred

，

（

void

）

NULL

）；

// Display the visualiser

while

（

！

viewer

。

wasStopped

（））

{

viewer

。

spinOnce

（）；

// The user pressed “space” ：

（

next_iteration

）

{

// The Iterative Closest Point algorithm

time

。

tic

（）；

icp

。

align

（

cloud_icp

）；

std

：：

cout

“Applied 1 ICP iteration in ”

time

。

toc

（）

“ ms”

std

：：

endl

；

（

icp

。

hasConverged

（））

{

printf

（

“

\033

［11A”

）；

// Go up 11 lines in terminal output。

printf

（

“

ICP has converged， score is %+。0e

”

，

icp

。

getFitnessScore

（））；

std

：：

cout

“

ICP transformation ”

iterations

“ ： cloud_icp -> cloud_in”

std

：：

endl

；

transformation_matrix

icp

。

getFinalTransformation

（）。

cast

double

（）；

// WARNING /！\ This is not accurate！ For “educational” purpose only！

print4x4Matrix

（

transformation_matrix

）；

// Print the transformation between original pose and current pose

。

str

（

“”

）；

iterations

；

std

：：

string

iterations_cnt

“ICP iterations = ”

。

str

（）；

viewer

。

updateText

（

iterations_cnt

，

txt_gray_lvl

，

txt_gray_lvl

，

txt_gray_lvl

，

“iterations_cnt”

）；

viewer

。

updatePointCloud

（

cloud_icp

，

cloud_icp_color_h

，

“cloud_icp_v2”

）；

}

else

{

PCL_ERROR

（

“

ICP has not converged。

”

）；

return

（

）；

}

next_iteration

false

；

}

return

（

）；

}

（2）Incrementally register pairs of clouds（多幅點雲配準）

點雲模型：

https：//

github。com/PointCloudLi

brary/data/tree/master/tutorials/pairwise

效果：按 q 鍵進行迭代配準，同時生成配準後的點雲

#include

using

pcl

：：

visualization

：：

PointCloudColorHandlerGenericField

；

using

pcl

：：

visualization

：：

PointCloudColorHandlerCustom

；

//convenient typedefs

typedef

pcl

：：

PointXYZ

PointT

；

typedef

pcl

：：

PointCloud

PointT

PointCloud

；

typedef

pcl

：：

PointNormal

PointNormalT

；

typedef

pcl

：：

PointCloud

PointNormalT

PointCloudWithNormals

；

// visualizer

pcl

：：

visualization

：：

PCLVisualizer

；

// left and right viewports

int

vp_1

，

vp_2

；

//convenient structure to handle our pointclouds

struct

PCD

{

PointCloud

：：

Ptr

cloud

；

std

：：

string

f_name

；

PCD

（）

：

cloud

（

new

PointCloud

）

{}；

}；

struct

PCDComparator

{

bool

operator

（）

（

const

PCD

，

const

PCD

）

{

return

（

。

f_name

。

f_name

）；

}

}；

// Define a new point representation for < x， y， z， curvature >

class

MyPointRepresentation

：

public

pcl

：：

PointRepresentation

PointNormalT

{

using

pcl

：：

PointRepresentation

PointNormalT

>：：

nr_dimensions_

；

public

：

MyPointRepresentation

（）

{

// Define the number of dimensions

nr_dimensions_

；

}

// Override the copyToFloatArray method to define our feature vector

virtual

void

copyToFloatArray

（

const

PointNormalT

，

float

out

）

const

{

// < x， y， z， curvature >

out

［

］

。

；

out

［

］

。

；

out

［

］

。

；

out

［

］

。

curvature

；

}

}；

////////////////////////////////////////////////////////////////////////////////

/** \brief Display source and target on the first viewport of the visualizer

void

showCloudsLeft

（

const

PointCloud

：：

Ptr

cloud_target

，

const

PointCloud

：：

Ptr

cloud_source

）

{

removePointCloud

（

“vp1_target”

）；

removePointCloud

（

“vp1_source”

）；

PointCloudColorHandlerCustom

PointT

tgt_h

（

cloud_target

，

255

，

）；

PointCloudColorHandlerCustom

PointT

src_h

（

cloud_source

，

255

，

）；

addPointCloud

（

cloud_target

，

tgt_h

，

“vp1_target”

，

vp_1

）；

addPointCloud

（

cloud_source

，

src_h

，

“vp1_source”

，

vp_1

）；

PCL_INFO

（

“Press q to begin the registration。

”

）；

spin

（）；

}

////////////////////////////////////////////////////////////////////////////////

/** \brief Display source and target on the second viewport of the visualizer

void

showCloudsRight

（

const

PointCloudWithNormals

：：

Ptr

cloud_target

，

const

PointCloudWithNormals

：：

Ptr

cloud_source

）

{

removePointCloud

（

“source”

）；

removePointCloud

（

“target”

）；

PointCloudColorHandlerGenericField

PointNormalT

tgt_color_handler

（

cloud_target

，

“curvature”

）；

（

！

tgt_color_handler

。

isCapable

（））

PCL_WARN

（

“Cannot create curvature color handler！”

）；

PointCloudColorHandlerGenericField

PointNormalT

src_color_handler

（

cloud_source

，

“curvature”

）；

（

！

src_color_handler

。

isCapable

（））

PCL_WARN

（

“Cannot create curvature color handler！”

）；

addPointCloud

（

cloud_target

，

tgt_color_handler

，

“target”

，

vp_2

）；

addPointCloud

（

cloud_source

，

src_color_handler

，

“source”

，

vp_2

）；

spinOnce

（）；

}

////////////////////////////////////////////////////////////////////////////////

/** \brief Load a set of PCD files that we want to register together

* \param argc the number of arguments （pass from main （））

* \param argv the actual command line arguments （pass from main （））

* \param models the resultant vector of point cloud datasets

void

loadData

（

int

argc

，

char

argv

，

std

：：

vector

PCD

，

Eigen

：：

aligned_allocator

PCD

models

）

{

std

：：

string

extension

（

“。pcd”

）；

// Suppose the first argument is the actual test model

for

（

int

；

argc

；

）

{

std

：：

string

fname

std

：：

string

（

argv

［

］）；

// Needs to be at least 5：。plot

（

fname

。

size

（）

extension

。

size

（））

continue

；

std

：：

transform

（

fname

。

begin

（），

fname

。

end

（），

fname

。

begin

（），

（

int

（

）（

int

））

tolower

）；

//check that the argument is a pcd file

（

fname

。

compare

（

fname

。

size

（）

extension

。

size

（），

extension

。

size

（），

extension

）

{

// Load the cloud and saves it into the global list of models

PCD

；

。

f_name

argv

［

］；

pcl

：：

loadPCDFile

（

argv

［

］，

。

cloud

）；

//remove NAN points from the cloud

std

：：

vector

int

indices

；

pcl

：：

removeNaNFromPointCloud

（

。

cloud

，

。

cloud

，

indices

）；

models

。

push_back

（

）；

}

////////////////////////////////////////////////////////////////////////////////

/** \brief Align a pair of PointCloud datasets and return the result

* \param cloud_src the source PointCloud

* \param cloud_tgt the target PointCloud

* \param output the resultant aligned source PointCloud

* \param final_transform the resultant transform between source and target

void

pairAlign

（

const

PointCloud

：：

Ptr

cloud_src

，

const

PointCloud

：：

Ptr

cloud_tgt

，

PointCloud

：：

Ptr

output

，

Eigen

：：

Matrix4f

final_transform

，

bool

downsample

false

）

{

// Downsample for consistency and speed

// \note enable this for large datasets

PointCloud

：：

Ptr

src

（

new

PointCloud

）；

PointCloud

：：

Ptr

tgt

（

new

PointCloud

）；

pcl

：：

VoxelGrid

PointT

grid

；

（

downsample

）

{

grid

。

setLeafSize

（

0。05

，

0。05

，

0。05

）；

grid

。

setInputCloud

（

cloud_src

）；

grid

。

filter

（

src

）；

grid

。

setInputCloud

（

cloud_tgt

）；

grid

。

filter

（

tgt

）；

}

else

{

src

cloud_src

；

tgt

cloud_tgt

；

}

// Compute surface normals and curvature

PointCloudWithNormals

：：

Ptr

points_with_normals_src

（

new

PointCloudWithNormals

）；

PointCloudWithNormals

：：

Ptr

points_with_normals_tgt

（

new

PointCloudWithNormals

）；

pcl

：：

NormalEstimation

PointT

，

PointNormalT

norm_est

；

pcl

：：

KdTree

pcl

：：

PointXYZ

>：：

Ptr

tree

（

new

pcl

：：

KdTree

pcl

：：

PointXYZ

（））；

norm_est

。

setSearchMethod

（

tree

）；

norm_est

。

setKSearch

（

）；

norm_est

。

setInputCloud

（

src

）；

norm_est

。

compute

（

points_with_normals_src

）；

pcl

：：

copyPointCloud

（

src

，

points_with_normals_src

）；

norm_est

。

setInputCloud

（

tgt

）；

norm_est

。

compute

（

points_with_normals_tgt

）；

pcl

：：

copyPointCloud

（

tgt

，

points_with_normals_tgt

）；

// Instantiate our custom point representation （defined above）。。。

MyPointRepresentation

point_representation

；

// 。。。 and weight the ‘curvature’ dimension so that it is balanced against x， y， and z

float

alpha

［

］

{

1。0

，

1。0

，

1。0

，

1。0

}；

point_representation

。

setRescaleValues

（

alpha

）；

// Align

pcl

：：

IterativeClosestPointNonLinear

PointNormalT

，

PointNormalT

reg

；

reg

。

setTransformationEpsilon

（

1e-6

）；

// Set the maximum distance between two correspondences （src<->tgt） to 10cm

// Note： adjust this based on the size of your datasets

reg

。

setMaxCorrespondenceDistance

（

0。1

）；

// Set the point representation

reg

。

setPointRepresentation

（

boost

：：

make_shared

const

MyPointRepresentation

（

point_representation

））；

reg

。

setInputSource

（

points_with_normals_src

）；

reg

。

setInputTarget

（

points_with_normals_tgt

）；

// Run the same optimization in a loop and visualize the results

Eigen

：：

Matrix4f

Eigen

：：

Matrix4f

：：

Identity

（），

，

targetToSource

；

PointCloudWithNormals

：：

Ptr

reg_result

points_with_normals_src

；

reg

。

setMaximumIterations

（

）；

for

（

int

；

）

{

PCL_INFO

（

“Iteration Nr。 %d。

”

，

）；

// save cloud for visualization purpose

points_with_normals_src

reg_result

；

// Estimate

reg

。

setInputSource

（

points_with_normals_src

）；

reg

。

align

（

reg_result

）；

//accumulate transformation between each Iteration

reg

。

getFinalTransformation

（）

；

//if the difference between this transformation and the previous one

//is smaller than the threshold， refine the process by reducing

//the maximal correspondence distance

（

std

：：

abs

（（

reg

。

getLastIncrementalTransformation

（）

）。

sum

（））

reg

。

getTransformationEpsilon

（））

reg

。

setMaxCorrespondenceDistance

（

reg

。

getMaxCorrespondenceDistance

（）

0。001

）；

reg

。

getLastIncrementalTransformation

（）；

// visualize current state

showCloudsRight

（

points_with_normals_tgt

，

points_with_normals_src

）；

}

// Get the transformation from target to source

targetToSource

。

inverse

（）；

// Transform target back in source frame

pcl

：：

transformPointCloud

（

cloud_tgt

，

output

，

targetToSource

）；

removePointCloud

（

“source”

）；

removePointCloud

（

“target”

）；

PointCloudColorHandlerCustom

PointT

cloud_tgt_h

（

output

，

255

，

）；

PointCloudColorHandlerCustom

PointT

cloud_src_h

（

cloud_src

，

255

，

）；

addPointCloud

（

output

，

cloud_tgt_h

，

“target”

，

vp_2

）；

addPointCloud

（

cloud_src

，

cloud_src_h

，

“source”

，

vp_2

）；

PCL_INFO

（

“Press q to continue the registration。

”

）；

spin

（）；

removePointCloud

（

“source”

）；

removePointCloud

（

“target”

）；

//add the source to the transformed target

output

cloud_src

；

final_transform

targetToSource

；

}

/* ——-［ */

int

main

（

int

argc

，

char

argv

）

{

// Load data

std

：：

vector

PCD

，

Eigen

：：

aligned_allocator

PCD

data

；

loadData

（

argc

，

argv

，

data

）；

// Check user input

（

data

。

empty

（））

{

PCL_ERROR

（

“Syntax is： %s ［*］”

，

argv

［

］）；

PCL_ERROR

（

“［*］ - multiple files can be added。 The registration results of （i， i+1） will be registered against （i+2）， etc”

）；

return

（

）；

}

PCL_INFO

（

“Loaded %d datasets。”

，

（

int

）

data

。

size

（））；

// Create a PCLVisualizer object

new

pcl

：：

visualization

：：

PCLVisualizer

（

argc

，

argv

，

“Pairwise Incremental Registration example”

）；

createViewPort

（

0。0

，

0。5

，

1。0

，

vp_1

）；

createViewPort

（

0。5

，

1。0

，

1。0

，

vp_2

）；

PointCloud

：：

Ptr

result

（

new

PointCloud

），

source

，

target

；

Eigen

：：

Matrix4f

GlobalTransform

Eigen

：：

Matrix4f

：：

Identity

（），

pairTransform

；

for

（

std

：：

size_t

；

data

。

size

（）；

）

{

source

data

［

］。

cloud

；

target

data

［

］。

cloud

；

// Add visualization data

showCloudsLeft

（

source

，

target

）；

PointCloud

：：

Ptr

temp

（

new

PointCloud

）；

PCL_INFO

（

“Aligning %s （%d） with %s （%d）。

”

，

data

［

］。

f_name

。

c_str

（），

source

points

。

size

（），

data

［

］。

f_name

。

c_str

（），

target

points

。

size

（））；

pairAlign

（

source

，

target

，

temp

，

pairTransform

，

true

）；

//transform current pair into the global transform

pcl

：：

transformPointCloud

（

temp

，

result

，

GlobalTransform

）；

//update the global transform

GlobalTransform

pairTransform

；

//save aligned pair， transformed into the first cloud‘s frame

std

：：

stringstream

；

“。pcd”

；

pcl

：：

savePCDFile

（

。

str

（），

result

，

true

）；

}

參考

高翔，視覺 SLAM 十四講

Least-Squares Rigid Motion Using SVD

ICP（迭代最近點）演算法

PCL點雲庫：ICP演算法

Interactive Iterative Closest Point

How to incrementally register pairs of clouds

【點雲精配準】Iterative Closest Point（ICP）

林家棟和劉德華的關係有多好？

神探夏洛克的第一季的第一集怎麼又兩個版本啊？、、

隨便看看

不給生命留白是什麼意思？

書包掛鉤原理？

貨車電瓶開關接法？

電動門感應門停電了怎麼關？

【點雲精配準】Iterative Closest Point（ICP）

林家棟和劉德華的關係有多好？

神探夏洛克的第一季的第一集怎麼又兩個版本啊？、、

猜你喜歡

有什麼好用的任務管理器？我用的是蘋果電腦，求推薦？

【強化學習理論 66】StatisticalRL 9

什麼是ICP 分析？？

隨便看看

不給生命留白是什麼意思？

書包掛鉤原理？

貨車電瓶開關接法？

電動門感應門停電了怎麼關？