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);
}
double CalculateFitness(void)
{
double Return;
//unsigned char Pixel[2];
// Draw the Evolve Image
ClearSurface(Evolve);
for (unsigned int i = 0; i < POLYGON_COUNT; i++)
if (PolygonList[i].BitFlag & DEFINED_BIT)
filledPolygonRGBA(Evolve, PolygonList[i].X, PolygonList[i].Y, VERTEX_COUNT, PolygonList[i].Color[0], PolygonList[i].Color[1], PolygonList[i].Color[2], PolygonList[i].Color[3]);
// Look through the pixels of both surfaces and determine the fitness of the evolving surface
Return = 0;
for (int y = 0; y < Target->h; y++)
for (int x = 0; x < Target->w; x++)
{
ComparisonColor evolve_color(((unsigned char*)Evolve->pixels)[y * Target->pitch + x * TargetPixelSize], ((unsigned char*)Evolve->pixels)[y * Target->pitch + x * TargetPixelSize + 1], ((unsigned char*)Evolve->pixels)[y * Target->pitch + x * TargetPixelSize + 2]);
ComparisonColor target_color(((unsigned char*)Target->pixels)[y * Target->pitch + x * TargetPixelSize], ((unsigned char*)Target->pixels)[y * Target->pitch + x * TargetPixelSize + 1], ((unsigned char*)Target->pixels)[y * Target->pitch + x * TargetPixelSize + 2]);
Return += CompareColors(evolve_color, target_color, false);
//// Red
//Pixel[0] = ((unsigned char*)Target->pixels)[y * Target->pitch + x * TargetPixelSize];
//Pixel[1] = ((unsigned char*)Evolve->pixels)[y * Evolve->pitch + x * EvolvePixelSize];
//Return += (255 - abs((int)(Pixel[0] - Pixel[1]))) / 255.0;
//
//// Green
//Pixel[0] = ((unsigned char*)Target->pixels)[y * Target->pitch + x * TargetPixelSize + 1];
//Pixel[1] = ((unsigned char*)Evolve->pixels)[y * Evolve->pitch + x * EvolvePixelSize + 1];
//Return += (255 - abs((int)(Pixel[0] - Pixel[1]))) / 255.0;
//
//// Blue
//Pixel[0] = ((unsigned char*)Target->pixels)[y * Target->pitch + x * TargetPixelSize + 2];
//Pixel[1] = ((unsigned char*)Evolve->pixels)[y * Evolve->pitch + x * EvolvePixelSize + 2];
//Return += (255 - abs((int)(Pixel[0] - Pixel[1]))) / 255.0;
}
Return = (Return / (double)(Target->w * Target->h * 3) * 100.0);
return Return;
}
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);
}
int
main (int argc, char** argv)
{
// Loading first scan of room.
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan1.pcd", *target_cloud) == -1)
{
PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
return (-1);
}
std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;
// Loading second scan of room from new perspective.
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan2.pcd", *input_cloud) == -1)
{
PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
return (-1);
}
std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;
// Filtering input scan to roughly 10% of original size to increase speed of registration.
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
approximate_voxel_filter.setInputCloud (input_cloud);
approximate_voxel_filter.filter (*filtered_cloud);
std::cout << "Filtered cloud contains " << filtered_cloud->size ()
<< " data points from room_scan2.pcd" << std::endl;
// Initializing Normal Distributions Transform (NDT).
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
// Setting scale dependent NDT parameters
// Setting minimum transformation difference for termination condition.
ndt.setTransformationEpsilon (0.01);
// Setting maximum step size for More-Thuente line search.
ndt.setStepSize (0.1);
//Setting Resolution of NDT grid structure (VoxelGridCovariance).
ndt.setResolution (1.0);
// Setting max number of registration iterations.
ndt.setMaximumIterations (35);
// Setting point cloud to be aligned.
ndt.setInputCloud (filtered_cloud);
// Setting point cloud to be aligned to.
ndt.setInputTarget (target_cloud);
// Set initial alignment estimate found using robot odometry.
Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();
// Calculating required rigid transform to align the input cloud to the target cloud.
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
ndt.align (*output_cloud, init_guess);
std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
<< " score: " << ndt.getFitnessScore () << std::endl;
// Transforming unfiltered, input cloud using found transform.
pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());
// Saving transformed input cloud.
pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);
// Initializing point cloud visualizer
boost::shared_ptr<pcl::visualization::PCLVisualizer>
viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer_final->setBackgroundColor (0, 0, 0);
// Coloring and visualizing target cloud (red).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
target_color (target_cloud, 255, 0, 0);
viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "target cloud");
// Coloring and visualizing transformed input cloud (green).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
output_color (output_cloud, 0, 255, 0);
viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "output cloud");
// Starting visualizer
viewer_final->addCoordinateSystem (1.0);
viewer_final->initCameraParameters ();
// Wait until visualizer window is closed.
while (!viewer_final->wasStopped ())
{
viewer_final->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
