int
main (int argc, char** argv)
{
pcl::PCDReader reader;
pcl::PCDWriter writer;
//レジストレーションを行うソースとターゲットのポイントクラウド
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_source (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_target (new pcl::PointCloud<pcl::PointXYZRGBA>);
//ソースとターゲットの読み込み
reader.read (argv[1], *cloud_source);
std::cout << "Source PointCloud "<< argv[1] << " has: " << cloud_source->points.size () << " data points." << std::endl; //*
reader.read (argv[2], *cloud_target);
std::cout << "Target PointCloud "<< argv[2] << " has: " << cloud_target->points.size () << " data points." << std::endl; //*
//ICP用を行うクラスIterativeClosestPoint<SourcePointT,TargetPoint>を作成
pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA> icp;
icp.setInputCloud(cloud_source); //ソースのポイントクラウドを設定
icp.setInputTarget(cloud_target); //ターゲットのポイントクラウドを設定
//※ここでは一切設定していないが、ICPは各種パラメータを設定できます
//ICPの結果を用いて位置合わされたソースのポイントクラウド
pcl::PointCloud<pcl::PointXYZRGBA> cloud_source_transformed;
//ICPアルゴリズムを実行
icp.align(cloud_source_transformed);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud_final(new pcl::PointCloud<pcl::PointXYZRGBA>);
cloud_final = (cloud_source_transformed + *cloud_target).makeShared() ;
//レジストレーション結果（cloud_final.pcd）を.pcdファイルに保存
writer.write("cloud_final.pcd",*cloud_final);
//PCLVaizualizerを準備して、移動後のソースとターゲットを色つきで表示。
//（カメラの初期位置が中央に偏っていてポイントクラウドが見えない場合はマウスでカメラ視点を調整。）
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("Registered Result"));
viewer->setBackgroundColor (255, 255, 255);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGBA> source_color(cloud_source_transformed.makeShared(), 0, 0, 255);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGBA> target_color(cloud_target, 255, 0, 0);
viewer->addPointCloud<pcl::PointXYZRGBA> (cloud_source_transformed.makeShared(), source_color, "source cloud transformed");
viewer->addPointCloud<pcl::PointXYZRGBA> (cloud_target, target_color, "target cloud");
viewer->addCoordinateSystem (1.0);
viewer->initCameraParameters ();
while (!viewer->wasStopped ())
{
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
int main ( int argc, char** argv )
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source ( new pcl::PointCloud<pcl::PointXYZ> () );
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target ( new pcl::PointCloud<pcl::PointXYZ> () );
{
// load source
loadFile ( argv[1], *cloud_source );
// load target
loadFile ( argv[2], *cloud_target );
}
// transformed source ---> target
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_trans ( new pcl::PointCloud<pcl::PointXYZ> () );
cloud_source_trans->resize ( cloud_source->size() );
{ // ICP registration
std::cout << "registration" << std::endl;
Eigen::Matrix3Xd X ( 3, cloud_source->size() ); // source, transformed
Eigen::Matrix3Xd Y ( 3, cloud_target->size() ); // target
for(int i = 0; i < cloud_source->size(); i++)
{
X(0,i) = cloud_source->points[i].x;
X(1,i) = cloud_source->points[i].y;
X(2,i) = cloud_source->points[i].z;
}
for(int i = 0; i < cloud_target->size(); i++)
{
Y(0,i) = cloud_target->points[i].x;
Y(1,i) = cloud_target->points[i].y;
Y(2,i) = cloud_target->points[i].z;
}
// ICP::point_to_point ( X, Y ); // standard ICP
SICP::point_to_point ( X, Y ); // sparse ICP
for(int i = 0; i < cloud_source_trans->size(); i++)
{
cloud_source_trans->points[i].x = X(0,i);
cloud_source_trans->points[i].y = X(1,i);
cloud_source_trans->points[i].z = X(2,i);
}
}
{ // visualization
boost::shared_ptr< pcl::visualization::PCLVisualizer > viewer ( new pcl::visualization::PCLVisualizer ("3D Viewer") );
viewer->setBackgroundColor (0, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color ( cloud_source, 0, 255, 0 );
viewer->addPointCloud<pcl::PointXYZ> (cloud_source, source_color, "source");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color ( cloud_target, 255, 255, 255 );
viewer->addPointCloud<pcl::PointXYZ> ( cloud_target, target_color, "target");
viewer->setPointCloudRenderingProperties ( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target" );
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_trans_color ( cloud_source_trans, 255, 0, 255 );
viewer->addPointCloud<pcl::PointXYZ> ( cloud_source_trans, source_trans_color, "source trans" );
viewer->setPointCloudRenderingProperties ( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source trans" );
// orthographic (parallel) projection; same with pressing key 'o'
viewer->getRenderWindow()->GetRenderers()->GetFirstRenderer()->GetActiveCamera()->SetParallelProjection( 1 );
viewer->resetCamera();
viewer->spin ();
}
return(0);
}
