环境是VS2010 Win32,PCL16.0。按照书上的例程下来,在IterativeClosestPoint 处error,如下,
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h> // TicToc
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));
}
int main ()
{
// The point clouds we will be using
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>); // Original point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tr (new pcl::PointCloud<pcl::PointXYZ>); // Transformed point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp (new pcl::PointCloud<pcl::PointXYZ>); // ICP output point cloud
int iterations = 1;
pcl::console::TicToc time;
time.tic();
if (pcl::io::loadPCDFile ("cloud.pcd", *cloud_in) < 0)
{
PCL_ERROR ("Error loading cloud %s.\n");
return (-1);
}
std::cout << "\nLoaded file " << "cloud.pcd" << " (" << 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
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<pcl::PointXYZ,pcl::PointXYZ> icp;
icp.setInputCloud (cloud_icp);
icp.setInputTarget (cloud_in);
icp.align (*cloud_icp);
std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl;
return (0);
}
更正,PCL的版本为1.6.0。代码如上。