赞
踩
本博文参考:https://blog.csdn.net/wokaowokaowokao12345/article/details/73741957
给定两个来自不同坐标系的三维数据点集,找到两个点集空间的变换关系,使得两个点集能统一到同一坐标系统中,即配准过程。配准的目标是在全局坐标框架中找到单独获取的视图的相对位置和方向,使得它们之间的相交区域完全重叠。对于从不同视图(views)获取的每一组点云数据,需要一个能够将它们对齐在一起的单一点云模型,从而可以应用后续处理步骤,如分割和对象重构。这篇博文就基于PCL中的ICP算法,实现不同视图点云数据的配准,并延伸相关知识。
两个(pairwise)点云数据集的配准,即在其中一个数据上进行旋转和平移,使得他们之间的相交区域完全重合。这里输出结果是一个(4x4)刚性(rigid)变换矩阵。
两个数据集的计算步骤:
点云中的特征点是一些兴趣点,如目标的转点、角点等具有明显特征(feature)的点或点集。
特征描述基于发现的关键点,提取特征组合信息并生成向量来进行比较。一致性估计 correspondence estimate给定两组数据获取相应的特征描述向量,找到相应的特征以在数据中找到重叠的部分。
舍弃糟糕的一致性估计
不是所有的一致性估计都是正确的,错误的一致性将对变换产生负影响,因此需要舍弃。
变换 transformation
步骤:
ICP算法本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对,计算最优刚体变换这一过程,直到满足正确配准的收敛精度要求。ICP是一个广泛使用的配准算法,主要目的就是找到旋转和平移参数,将两个不同坐标系下的点云,以其中一个点云坐标系为全局坐标系,另一个点云经过旋转和平移后两组点云重合部分完全重叠。ICP算法最早由Chen and Medioni,and Besl and McKay提出。
算法的输入:参考点云和目标点云,停止迭代的标准。
算法的输出:旋转和平移矩阵,即转换矩阵。
对于目标点云中的每个点,匹配参考点云(或选定集合)中的最近点。求得使上述对应点对计算均方根(root mean square,RMS)最小的刚体变换,求得平移参数和旋转参数。使用获得的转换矩阵来转换目标点云。迭代(重新关联点),直到满足终止迭代的条件(迭代次数或误差小于阈值)。这里的误差最小,可以是相邻两次均方根差的绝对值小于某一限差。
#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; bool next_iteration = false; void 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; } int 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 int iterations = 1; // Default number of ICP iterations pcl::console::TicToc time; time.tic (); if (pcl::io::loadPLYFile ("fish-2.ply", *cloud_in) < 0) { PCL_ERROR ("Error loading cloud %s.\n"); return (-1); } std::cout << "\nLoaded file " << "fish-2.ply" << " (" << 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 vertically 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; } return (0); }
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。