Three-dimensional reconstruction based on PCL--point cloud Registration (I.) The realization of ICP algorithm

In the fields of reverse engineering, computer vision, and digitization of cultural relics, due to the incomplete point cloud, rotation dislocation, translation dislocation, etc., so that the complete point cloud to be obtained needs to register the local point cloud, in order to get the complete data model of the object being measured, we need to determine a suitable coordinate system, Merging point sets from various perspectives into a unified coordinate system to form a complete point cloud, which can then be easily visualized, is the registration of point cloud data. Point Cloud Registration has the registration of the manual registration depends on the instrument, and automatic registration, point cloud automatic registration technology is through a certain algorithm or statistical law to calculate the dislocation between two point clouds, so as to achieve two point cloud automatic registration effect, the essence is the different coordinate system measured in the data point cloud coordinate system transformation, In order to get the whole data model, the key of the problem is how to get the parameters R (rotation matrix) and T (translation vector) of the coordinate transformation, so that the three-dimensional data measured at two angles of view is the smallest distance after the coordinate transformation, and the current registration algorithm can be divided into the whole registration and the local registration according to the process. There is a separate registration module in the PCL, which realizes the basic data structure of registration and the classical registration algorithm such as ICP.

ICP algorithm treats stitching 2 point cloud, first establishes corresponding point set P and Q according to certain criteria, which corresponds to the number of point pairs, then calculates the optimal coordinate transformation by the least multiplication iteration, namely the rotation matrix R and the vector T, makes the error function is the smallest, the ICP processing flow divides into four main steps:

1. Sampling Raw Point cloud data
2. Determine the initial set of corresponding points
3. Remove the wrong corresponding point pair

4. Coordinate Transformation Solution

The following code reads two PCD point cloud registration, with a space to control the number of iterations, and visualization visualization, where white is used to display the original point cloud data, the point cloud after a certain rotation translation is green, red is used to display the ICP matching point cloud data. The selection matrix and the shift vector at this time are played for each iteration.

#include <iostream> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/
registration/icp.h> #include <pcl/visualization/pcl_visualizer.h>//visual header file typedef PCL::P OINTXYZ Pointt;

typedef PCL::P ointcloud<pointt> Pointcloudt;

BOOL next_iteration = false;
	void Print4x4matrix (const EIGEN::MATRIX4D & Matrix)//print rotation matrix and pan 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) {//Use the SPACEBAR to increase the number of iterations and update the display
if (event.getkeysym () = = "Space" && Event.keydown ()) next_iteration = true; } int Main (int argc, char** argv) {//Create point cloud pointer Pointcloudt::P TR cloud_in (new Pointcloudt);  Original Point cloud Pointcloudt::P tr cloud_tr (new Pointcloudt);  Converted Point Cloud Pointcloudt::P tr CLOUD_ICP (new Pointcloudt); ICP output Point cloud//Read the PCD file if (PCL::IO::LOADPCDFILE&LT;PCL::P ointxyz> ("YOU.PCD", *cloud_in) = =-1) {Pcl_error ("couldn
		' t read file1 \ n ');
	Return (-1);

	} std::cout << "Loaded" << cloud_in->size () << "data points from file1" << Std::endl;
		if (PCL::IO::LOADPCDFILE&LT;PCL::P ointxyz> ("ZUO.PCD", *cloud_icp) = =-1) {Pcl_error ("couldn ' t read file2 \ n");
	Return (-1);


	} std::cout << "Loaded" << cloud_icp->size () << "data points from file2" << Std::endl;  int iterations = 1;  


	Default ICP iterations//define rotation matrix and shift vector matrix4d is for 4*4 matrix eigen::matrix4d Transformation_matrix = eigen::matrix4d::identity ();  Initializes a double theta = M_PI/8;
	The angle of rotation is represented by 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);

	Z-axis translation vector (0.4 meters) Transformation_matrix (2, 3) = 0.4;
	Print conversion matrix Std::cout << "Applying this rigid transformation to:cloud_in-CLOUD_ICP" << Std::endl;

	Print4x4matrix (Transformation_matrix);
	Perform point cloud conversion pcl::transformpointcloud (*cloud_in, *CLOUD_ICP, Transformation_matrix);  *CLOUD_TR = *CLOUD_ICP; Backup CLOUD_ICP assignment to CLOUD_TR for later use//ICP registration PCL::ITERATIVECLOSESTPOINT&LT;PCL::P ointxyz, PCL::P ointxyz> ICP;    Create an ICP object for the ICP Registration Icp.setmaximumiterations (iterations); Sets the maximum number of iterations Iterations=true Icp.setinputcloud (CLOUD_ICP); Set the input point cloud Icp.setinputtarget (cloud_in);          Set the target point cloud (input point cloud for affine transformation, get target point Cloud) icp.align (*CLOUD_ICP);  Match after source point cloud icp.setmaximumiterations (1);
	Set to 1 for next call to Std::cout << "applied" << iterations << "ICP iteration (s)" << Std::endl; if (icp.hasconverged ())//icp.hasconverged () =1 (true) evaluation of the suitability of the output transformation matrix {std::cout << \niCP has converged, score are "<< 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 have not converged.\n");
	Return (-1);
	}//Visual pcl::visualization::P Clvisualizer Viewer ("ICP demo");
	Creation of two observation viewpoints 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);  Defines the color information to display float bckgr_gray_level = 0.0;
	Black float txt_gray_lvl = 1.0-bckgr_gray_level; The original point cloud is set to white pcl::visualization::P ointcloudcolorhandlercustom<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);//Set the original point cloud to be displayed as white Viewer.addpointcloud (cloud_in , CLOUd_in_color_h, "CLOUD_IN_V2", V2); The converted point cloud is shown in green pcl::visualization::P ointcloudcolorhandlercustom<pointt> cloud_tr_color_h (CLOUD_TR, 20, 180, 20
	);

	Viewer.addpointcloud (Cloud_tr, Cloud_tr_color_h, "Cloud_tr_v1", v1); The point cloud after ICP registration is Red pcl::visualization::P ointcloudcolorhandlercustom<pointt> cloud_icp_color_h (CLOUD_ICP, 180, 20
	, 20);

	Viewer.addpointcloud (CLOUD_ICP, Cloud_icp_color_h, "CLOUD_ICP_V2", V2); Add a description of the text in the respective viewport interface//at the specified viewport viewport=v1 the string "white ... "Where" Icp_info_1 "is the ID flag of the added string, (10,15) is the character size of coordinates 16 followed by the RGB value Viewer.addtext (" White:original Point Cloud\ngreen:matrix
	Transformed Point Cloud ", Txt_gray_lvl, Txt_gray_lvl, TXT_GRAY_LVL," Icp_info_1 ", v1); Viewer.addtext ("white:original point CLOUD\NRED:ICP aligned Point Cloud", ten, A, TXT_GRAY_LVL, TXT_GRAY_LVL, Txt_gr

	AY_LVL, "Icp_info_2", V2);
	Std::stringstream SS;            SS << iterations;
	Number of iterations entered std::string iterations_cnt = "ICP iterations =" + Ss.str (); Viewer.addtext (IteratiOns_cnt, 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 the camera's coordinates and direction viewer.setcameraposition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947,-0.256907, 0);  Viewer.setsize (1280, 1024);

	Visualize window size//Register key callback function Viewer.registerkeyboardcallback (&keyboardeventoccurred, (void*) NULL);

		Show while (!viewer.wasstopped ()) {viewer.spinonce ();
			Press the SPACEBAR function if (next_iteration) {///Nearest point Iteration algorithm icp.align (*CLOUD_ICP);  if (icp.hasconverged ()) {printf ("\033[11a");
				Go up one lines in terminal output.
				printf ("\NICP have 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 isn't accurate!  Print4x4matrix (Transformation_matrix);
				Print matrix transform Ss.str ("");
				SS << iterations;
				std::string iterations_cnt = "ICP iterations =" + Ss.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 ("\NICP have not converged.\n");
			Return (-1);
	}} next_iteration = false;
} return 0; }