-
Notifications
You must be signed in to change notification settings - Fork 5
/
ICP_Registration.cpp
172 lines (139 loc) · 9.05 KB
/
ICP_Registration.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
//
// Created by dinhnambkhn on 21. 11. 30..
// http://www.open3d.org/docs/latest/tutorial/Basic/icp_registration.html
// This tutorial demonstrates the ICP (Iterative Closest Point) registration algorithm.
// It has been a mainstay of geometric registration in both research and industry for many years.
// The input are two point clouds and an initial transformation that roughly aligns the source point cloud to the target point cloud.
// The output is a refined transformation that tightly aligns the two point clouds.
// A helper function draw_registration_result visualizes the alignment during the registration process.
// In this tutorial, we show two ICP variants, the point-to-point ICP and the point-to-plane ICP [Rusinkiewicz2001]_.
// https://ieeexplore.ieee.org/document/924423
#include <iostream>
#include <open3d/Open3D.h>
//Helper visualization function
void draw_registration_result(
const open3d::geometry::PointCloud &source, //shared_ptr<PointCloud>
const open3d::geometry::PointCloud &target,
const Eigen::Matrix4d &transformation) {
open3d::geometry::PointCloud source_down, target_down;
//std::shared_ptr<open3d::geometry::PointCloud> cloud_ptr(new open3d::geometry::PointCloud(cloud));
//to make copies and protect the original point clouds.
source_down = source;
target_down = target;
std::shared_ptr<open3d::geometry::PointCloud> source_temp(new open3d::geometry::PointCloud(source_down));
std::shared_ptr<open3d::geometry::PointCloud> target_temp(new open3d::geometry::PointCloud(target_down));
source_temp->PaintUniformColor(Eigen::Vector3d(1, 0.706, 0));
target_temp->PaintUniformColor(Eigen::Vector3d(0, 0.651, 0.929));
source_temp->Transform(transformation);
double zoom = 0.4459;
auto look_at = Eigen::Vector3d(1.6784, 2.0612, 1.4451);
auto front = Eigen::Vector3d(0.9288, -0.2951, -0.2242);
auto up = Eigen::Vector3d(-0.3402, -0.9189, -0.1996);
open3d::visualization::DrawGeometries({source_temp, target_temp}, "ICP-PCL",
640, 480, 50, 50,
false, false, false,
&look_at,
&up,
&front,
&zoom);
}
int main() {
//The code below reads a source point cloud and a target point cloud from two files. A rough transformation is given.
//The initial alignment is usually obtained by a global registration algorithm. See Global registration for examples.
//read the source point cloud
open3d::geometry::PointCloud source;
open3d::io::ReadPointCloud("/home/dinhnambkhn/Open3D/examples/test_data/ICP/cloud_bin_0.pcd", source);
//read the target point cloud
open3d::geometry::PointCloud target;
open3d::io::ReadPointCloud("/home/dinhnambkhn/Open3D/examples/test_data/ICP/cloud_bin_1.pcd", target);
//check the point cloud is loaded correctly
std::cout << "Loaded source: " << source.points_.size() << "points and target data "
<< target.points_.size() << " points with " << std::endl;
auto threshold = 0.02;
//transformation init to [0.862, 0.011, -0.507, 0.5],
// [-0.139, 0.967, -0.215, 0.7],
// [0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]
auto trans_init = Eigen::Matrix4d({{0.862, 0.011, -0.507, 0.5},
{-0.139, 0.967, -0.215, 0.7},
{0.487, 0.255, 0.835, -1.4},
{0.0, 0.0, 0.0, 1.0}});
//show trans_init matrix
std::cout << "init matrix " << trans_init.matrix() << std::endl;
//draw
draw_registration_result(source, target, trans_init);
//The function evaluate_registration calculates two main metrics:
//fitness, which measures the overlapping area (# of inlier correspondences / # of points in target). The higher the better.
//inlier_rmse, which measures the RMSE of all inlier correspondences. The lower the better.
//Initial alignment
std::cout << "Initial alignment: " << std::endl;
//evaluate_registration
auto evaluation = open3d::pipelines::registration::EvaluateRegistration(source, target, threshold, trans_init);
//print evaluation
std::cout << "RegistrationResult with fitness " << evaluation.fitness_ << std::endl;
std::cout << "inline_rmse_ " << evaluation.inlier_rmse_ << std::endl;
std::cout << "correspondence_set_ " << evaluation.correspondence_set_.size() << std::endl;
//Point-to-point ICP
//In general, the ICP algorithm iterates over two steps:
//step 1: Find correspondence set K={(p,q)} from target point cloud P,
// and source point cloud Q transformed with current transformation matrix T.
//Step 2: Update the transformation T by minimizing an objective function E(T) defined over the correspondence set K.
std::cout << "Apply point-to-point ICP" << std::endl;
//point_to_point_registration
//begin counting time
auto start = std::chrono::system_clock::now();
auto reg_p2p = open3d::pipelines::registration::RegistrationICP(source, target, threshold, trans_init,
open3d::pipelines::registration::TransformationEstimationPointToPoint());
//stop counting time
auto stop = std::chrono::system_clock::now();
//print time
std::cout << "time for Transformation from color ms: " <<
std::chrono::duration_cast<std::chrono::milliseconds>(stop - start).count() << std::endl;
std::cout << "RegistrationResult with fitness " << reg_p2p.fitness_ << std::endl;
std::cout << "inline_rmse_ " << reg_p2p.inlier_rmse_ << std::endl;
std::cout << "correspondence_set_ " << reg_p2p.correspondence_set_.size() << std::endl;
std::cout << "Transformation is: \n" << reg_p2p.transformation_ << std::endl;
draw_registration_result(source, target, reg_p2p.transformation_);
//The fitness score increases from 0.174723 to 0.372450.
// The inlier_rmse reduces from 0.011771 to 0.007760.
// By default, registration_icp runs until convergence or reaches a maximum number of iterations (30 by default).
// It can be changed to allow more computation time and to improve the results further.
//let change trans_init
//trans_init = reg_p2p.transformation_;
start = std::chrono::system_clock::now();
reg_p2p = open3d::pipelines::registration::RegistrationICP(source, target, threshold, trans_init,
open3d::pipelines::registration::TransformationEstimationPointToPoint(),
open3d::pipelines::registration::ICPConvergenceCriteria(
1e-6, 1e-6, 1000));
stop = std::chrono::system_clock::now();
//print time
std::cout << "with 1000 integrations, time for Transformation from color ms: " <<
std::chrono::duration_cast<std::chrono::milliseconds>(stop - start).count() << std::endl;
std::cout << "RegistrationResult with fitness " << reg_p2p.fitness_ << std::endl;
std::cout << "inline_rmse_ " << reg_p2p.inlier_rmse_ << std::endl;
std::cout << "correspondence_set_ " << reg_p2p.correspondence_set_.size() << std::endl;
//show the interation number
//The final alignment is tight. The fitness score improves to 0.621123. The inlier_rmse reduces to 0.006583.
std::cout << "Transformation is: \n" << reg_p2p.transformation_ << std::endl;
draw_registration_result(source, target, reg_p2p.transformation_);
//Point-to-plane ICP
//The point-to-plane ICP algorithm iterates using normal of point p
// Apply point-to-plane ICP
std::cout << "Apply point-to-plane ICP" << std::endl;
//begin counting time
start = std::chrono::system_clock::now();
auto reg_p2l = open3d::pipelines::registration::RegistrationICP(source, target, threshold, trans_init,
open3d::pipelines::registration::TransformationEstimationPointToPlane());
stop = std::chrono::system_clock::now();
//print time
std::cout << "PointToPlane time for Transformation from color ms: " <<
std::chrono::duration_cast<std::chrono::milliseconds>(stop - start).count() << std::endl;
std::cout << "RegistrationResult with fitness " << reg_p2l.fitness_ << std::endl;
std::cout << "inline_rmse_ " << reg_p2l.inlier_rmse_ << std::endl;
std::cout << "correspondence_set_ " << reg_p2l.correspondence_set_.size() << std::endl;
//The final alignment is tight. The fitness score improves to 0.621123. The inlier_rmse reduces to 0.006583.
//The point-to-plane ICP reaches tight alignment within 30 iterations
// (a fitness score of 0.620972 and an inlier_rmse score of 0.006581).
std::cout << "Transformation is: \n" << reg_p2l.transformation_ << std::endl;
draw_registration_result(source, target, reg_p2l.transformation_);
return 0;
}