经典的小猴子ICP配准:
每次当用户按下space按键时,ICP配准将迭代用户设置的次数,显示器也随之更新最新的配准结果。
#pragma warning(disable:4996)
#include <vtkAutoInit.h>
VTK_MODULE_INIT(vtkRenderingOpenGL);
VTK_MODULE_INIT(vtkInteractionStyle);
#define BOOST_TYPEOF_EMULATION
#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(int argc,
char* argv[])
{
// 我们要使用的点云
PointCloudT::Ptr cloud_in(new PointCloudT); // 初始点云
PointCloudT::Ptr cloud_tr(new PointCloudT); // 转换点云
PointCloudT::Ptr cloud_icp(new PointCloudT); // 输出点云
// 检查输入参数
if (argc < 2)
{
printf("Usage :\n");
printf("\t\t%s file.ply number_of_ICP_i