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
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
|
# -*- coding: utf-8 -*-
# Interactive Iterative Closest Point
# http://pointclouds.org/documentation/tutorials/interactive_icp.php#interactive-icp
# #include <iostream>
# #include <string>
#
# #include <pcl/io/ply_io.h>
# #include <pcl/point_types.h>
# #include <pcl/registration/icp.h>
# #include <pcl/visualization/pcl_visualizer.h>
# #include <pcl/console/time.h> // TicToc
#
# typedef pcl::PointXYZ PointT;
# typedef pcl::PointCloud<PointT> PointCloudT;
impiort pcl
# import pcl.visualization
# bool next_iteration = false;
next_iteration = false
# def print4x4Matrix (const Eigen::Matrix4d & matrix)
# {
# printf ("Rotation matrix :\n");
# printf (" | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2));
# printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2));
# printf (" | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2));
# printf ("Translation vector :\n");
# printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3));
# }
# void keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event, void* nothing)
# {
# if (event.getKeySym () == "space" && event.keyDown ())
# next_iteration = true;
# }
### main
# // 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
# if (argc < 2)
# {
# printf ("Usage :\n");
# printf ("\t\t%s file.ply number_of_ICP_iterations\n", argv[0]);
# PCL_ERROR ("Provide one ply file.\n");
# return (-1);
# }
#
# int iterations = 1; // Default number of ICP iterations
# if (argc > 2)
# {
# // If the user passed the number of iteration as an argument
# iterations = atoi (argv[2]);
# if (iterations < 1)
# {
# PCL_ERROR ("Number of initial iterations must be >= 1\n");
# return (-1);
# }
# }
#
# pcl::console::TicToc time;
# time.tic ();
# if (pcl::io::loadPLYFile (argv[1], *cloud_in) < 0)
# {
# PCL_ERROR ("Error loading cloud %s.\n", argv[1]);
# return (-1);
# }
# std::cout << "\nLoaded file " << argv[1] << " (" << cloud_in->size () << " points) in " << time.toc () << " ms\n" << 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 / 8; // The angle of rotation in radians
# transformation_matrix (0, 0) = cos (theta);
# transformation_matrix (0, 1) = -sin (theta);
# transformation_matrix (1, 0) = sin (theta);
# transformation_matrix (1, 1) = cos (theta);
#
# // A translation on Z axis (0.4 meters)
# transformation_matrix (2, 3) = 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 (1); // 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;
#
# if (icp.hasConverged ())
# {
# std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;
# std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
# transformation_matrix = icp.getFinalTransformation ().cast<double>();
# print4x4Matrix (transformation_matrix);
# }
# else
# {
# PCL_ERROR ("\nICP has not converged.\n");
# return (-1);
# }
#
# // Visualization
# pcl::visualization::PCLVisualizer viewer ("ICP demo");
# // Create two verticaly separated viewports
# int v1 (0);
# int v2 (1);
# viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1);
# viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2);
#
# // 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", v1);
# viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v2", v2);
#
# // Transformed point cloud is green
# pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h (cloud_tr, 20, 180, 20);
# viewer.addPointCloud (cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);
#
# // ICP aligned point cloud is red
# pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h (cloud_icp, 180, 20, 20);
# viewer.addPointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);
#
# // Adding text descriptions in each viewport
# viewer.addText ("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
# viewer.addText ("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);
#
# std::stringstream ss;
# ss << iterations;
# std::string iterations_cnt = "ICP iterations = " + ss.str ();
# viewer.addText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);
#
# // Set background color
# viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
# viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);
#
# // Set camera position and orientation
# viewer.setCameraPosition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
# 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" :
# if (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;
#
# if (icp.hasConverged ())
# {
# printf ("\033[11A"); // Go up 11 lines in terminal output.
# printf ("\nICP has converged, score is %+.0e\n", icp.getFitnessScore ());
# std::cout << "\nICP 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
#
# ss.str ("");
# ss << iterations;
# std::string iterations_cnt = "ICP iterations = " + ss.str ();
# viewer.updateText (iterations_cnt, 10, 60, 16, 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 ("\nICP has not converged.\n");
# return (-1);
# }
# }
# next_iteration = false;
# }
|
