PCLでICPアルゴリズムを使ってみる。

目的

PCLのICPアルゴリズムで、2つの点群の移動量を求める。

環境

コンパイルを簡略化するため、PCL_ROSを使って、MakeFileでコンパイルする。
PCL_ROSをMakeFileでコンパイルする
-Ubuntu 1404 x64
-ROS indigo

実装

pcl::IterativeClosestPointに、移動前の点群と移動後の点群を入力する。
移動量は4x4の変換Matrixで出力される。

main.cpp

# include <iostream>
# include <pcl/io/pcd_io.h>
# include <pcl/point_types.h>
# include <pcl/registration/icp.h>

using namespace std;

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 (int argc, char** argv){

	//点群を宣言する。
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);

	//点群の移動量を設定
	double x_offset=0.7f;
	double y_offset=0.5f;
	
	//点群cloud_inにデータを入力
	cloud_in->width    = 5;
	cloud_in->height   = 1;
	cloud_in->is_dense = false;
	cloud_in->points.resize (cloud_in->width * cloud_in->height);

	for (size_t i = 0; i < cloud_in->points.size (); ++i)	{
		cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
		cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
		cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
	}

	std::cout << "Saved " << cloud_in->points.size () << " data points to input:"<< std::endl;
	for (size_t i = 0; i < cloud_in->points.size (); ++i) std::cout << "    " <<
	cloud_in->points[i].x << " " <<
	cloud_in->points[i].y << " " <<
	cloud_in->points[i].z << std::endl;
	*cloud_out = *cloud_in;
	std::cout << "size:" << cloud_out->points.size() << std::endl;

	//点群を移動させcloud_outを作る。
	for (size_t i = 0; i < cloud_in->points.size (); ++i){
		cloud_out->points[i].x = cloud_in->points[i].x + x_offset;
		cloud_out->points[i].y = cloud_in->points[i].y + y_offset;

	}
	std::cout << "Transformed " << cloud_in->points.size () << " data points:"	<< std::endl;

	for (size_t i = 0; i < cloud_out->points.size (); ++i)
	std::cout << "    " <<
	cloud_out->points[i].x << " " <<
	cloud_out->points[i].y << " " <<
	cloud_out->points[i].z << std::endl;

	//cloud_inとcloud_outをICPアルゴズムにより変換matrixを求める。
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputCloud(cloud_in);
	icp.setInputTarget(cloud_out);

	pcl::PointCloud<pcl::PointXYZ> Final;
	icp.align(Final);

	//変換matrixを表示する
 	Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();
   	transformation_matrix = icp.getFinalTransformation ().cast<double>();
	print4x4Matrix (transformation_matrix);

	return (0);
}

$ ./pcl_sample 
Saved 5 data points to input:
    0.352222 -0.151883 -0.106395
    -0.397406 -0.473106 0.292602
    -0.731898 0.667105 0.441304
    -0.734766 0.854581 -0.0361733
    -0.4607 -0.277468 -0.916762
size:5
Transformed 5 data points:
    1.05222 0.348117 -0.106395
    0.302594 0.0268941 0.292602
    -0.0318983 1.1671 0.441304
    -0.0347655 1.35458 -0.0361733
    0.2393 0.222532 -0.916762
Rotation matrix :
    |  1.000 -0.000  0.000 | 
R = | -0.000  1.000  0.000 | 
    | -0.000  0.000  1.000 | 
Translation vector :
t = <  0.700,  0.500, -0.000 >

x成分移動量=0.7,y成分移動量=0.5,z成分移動量=0.0を与えていたが、
pcl::IterativeClosestPointで、誤差ない結果が得られた。

参考

tutorials:iterative_closest_point