int
main(int argc, char **argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud_registered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr map_cloud (new pcl::PointCloud<pcl::PointXYZ>);
int start_index = 1, end_index = 20;
std::string prefix("/media/work/datasets/BRL/unorganized/scan");
std::string source_pcd_file, target_pcd_file, source_pose_file, target_pose_file;
map_cloud->points.clear();
// for (int index = start_index; index < end_index; index++)
// {
// char buf[4];
// doubletmp;
// sprintf(buf, "%03d", index);
// target_pcd_file = prefix + std::string(buf) + ".pcd";
// target_pose_file = prefix + std::string(buf) + ".pose";
// doubletarget_odometry[3];
// if (pcl::io::loadPCDFile<pcl::PointXYZ> (target_pcd_file, *target_cloud) == -1) //* load the file
// {
// PCL_ERROR ("Couldn't read file %s!\n", target_pcd_file.c_str());
// return (-1);
// }
// PCL_INFO ("Loaded %d points: (width %d, height: %d) from %s.\n",
// target_cloud->points.size(), target_cloud->width, target_cloud->height, target_pcd_file.c_str());
// std::ifstream target_pose_in (target_pose_file.c_str());
// target_pose_in >> target_odometry[0] >> target_odometry[1] >> tmp >> tmp >> tmp >> target_odometry[2];
// PCL_INFO ("target odometry (x, y, theta): (%f, %f, %f)!\n", target_odometry[0], target_odometry[1], target_odometry[2]);
// if (index == start_index)
// map_cloud->points.insert(map_cloud->points.begin(),
// target_cloud->points.begin(), target_cloud->points.end());
//
// sprintf(buf, "%03d", index + 1);
// source_pcd_file = prefix + std::string(buf) + ".pcd";
// source_pose_file = prefix + std::string(buf) + ".pose";
// doublesource_odometry[3];
// if (pcl::io::loadPCDFile<pcl::PointXYZ> (source_pcd_file, *source_cloud) == -1) //* load the file
// {
// PCL_ERROR ("Couldn't read file %s!\n", source_pcd_file.c_str());
// return (-1);
// }
// PCL_INFO ("Loaded %d points: (width %d, height: %d) from %s.\n",
// source_cloud->points.size(), source_cloud->width, source_cloud->height, source_pcd_file.c_str());
// std::ifstream source_pose_in(source_pose_file.c_str());
// source_pose_in >> source_odometry[0] >> source_odometry[1] >> tmp >> tmp >> tmp >> source_odometry[2];
// PCL_INFO ("source odometry (x, y, theta): (%f, %f, %f)!\n", source_odometry[0], source_odometry[1], source_odometry[2]);
//
// Eigen::Matrix4d guess(Eigen::Matrix4d::Zero());
// doubletheta = (source_odometry[2] - target_odometry[2]) * 3.1415926 / 180;
// guess(0,0) = cos(theta);
// guess(0,1) = -sin(theta);
// guess(1,0) = sin(theta);
// guess(1,1) = cos(theta);
// guess(2,2) = 1;
// guess(3,3) = 1;
// guess(0,3) = source_odometry[0] - target_odometry[0];
// guess(1,3) = source_odometry[1] - target_odometry[1];
//
// pcl::transformPointCloud(*source_cloud, *source_cloud_registered, guess);
// pcl::visualization::PCLVisualizer viewer ("visualization for translation");
// viewer.addPointCloud (target_cloud, "target_cloud", 0);
// viewer.addPointCloud (source_cloud_registered, "source_cloud_registered", 0);
// viewer.addCoordinateSystem (1.0);
// viewer.setBackgroundColor (1,1,1);
// viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target_cloud");
// viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source_cloud_registered");
// viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "target_cloud");
// viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "source_cloud_registered");
// viewer.spin ();
// }
for (int index = start_index; index <= end_index; index++)
{
char buf[4];
double tmp;
sprintf(buf, "%03d", index);
target_pcd_file = prefix + std::string(buf) + ".pcd";
target_pose_file = prefix + std::string(buf) + ".pose";
double target_odometry[3];
if (pcl::io::loadPCDFile<pcl::PointXYZ> (target_pcd_file, *target_cloud) == -1) //* load the file
{
PCL_ERROR ("Couldn't read file %s!\n", target_pcd_file.c_str());
return (-1);
}
PCL_INFO ("Loaded %d points: (width %d, height: %d) from %s.\n",
target_cloud->points.size(), target_cloud->width, target_cloud->height, target_pcd_file.c_str());
std::ifstream target_pose_in (target_pose_file.c_str());
target_pose_in >> target_odometry[0] >> target_odometry[1] >> tmp >> tmp >> tmp >> target_odometry[2];
PCL_INFO ("target odometry (x, y, theta): (%f, %f, %f)!\n", target_odometry[0], target_odometry[1], target_odometry[2]);
Eigen::Matrix4f transformation(Eigen::Matrix4f::Zero());
double theta = target_odometry[2] * 3.14 / 180;
transformation(0,0) = cos(theta);
transformation(0,1) = -sin(theta);
transformation(1,0) = sin(theta);
transformation(1,1) = cos(theta);
transformation(2,2) = 1;
transformation(3,3) = 1;
transformation(0,3) = target_odometry[0];
transformation(1,3) = target_odometry[1];
pcl::transformPointCloud(*target_cloud, *target_cloud, transformation);
map_cloud->points.insert(map_cloud->points.end(),
target_cloud->points.begin(),
target_cloud->points.end());
}
map_cloud->width = map_cloud->points.size();
map_cloud->height = 1;
pcl::io::savePCDFileBinary("map.pcd", *map_cloud);
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);
}
