void processCloud(const sensor_msgs::PointCloud2 msg)
{
//********* Retirive and process raw pointcloud************
std::cout<<"Recieved cloud"<<std::endl;
//std::cout<<"Create Octomap"<<std::endl;
//octomap::OcTree tree(res);
//std::cout<<"Load points "<<std::endl;
pcl::PCLPointCloud2 cloud;
pcl_conversions::toPCL(msg,cloud);
pcl::PointCloud<pcl::PointXYZ> pcl_pc;
pcl::fromPCLPointCloud2(cloud,pcl_pc);
//std::cout<<"Filter point clouds for NAN"<<std::endl;
std::vector<int> nan_indices;
pcl::removeNaNFromPointCloud(pcl_pc,pcl_pc,nan_indices);
//octomap::Pointcloud oct_pc;
//octomap::point3d origin(0.0f,0.0f,0.0f);
//std::cout<<"Adding point cloud to octomap"<<std::endl;
//octomap::point3d origin(0.0f,0.0f,0.0f);
//for(int i = 0;i<pcl_pc.points.size();i++){
//oct_pc.push_back((float) pcl_pc.points[i].x,(float) pcl_pc.points[i].y,(float) pcl_pc.points[i].z);
//}
//tree.insertPointCloud(oct_pc,origin,-1,false,false);
//*********** Remove the oldest data, update the data***************
cloud_seq_loaded.push_back(pcl_pc);
std::cout<<cloud_seq_loaded.size()<<std::endl;
if(cloud_seq_loaded.size()>2){
cloud_seq_loaded.pop_front();
}
//*********** Process currently observerd and buffered data*********
if(cloud_seq_loaded.size()==2){
//std::cout<< "Generating octomap"<<std::endl;
std::cout<<"Ransac"<<std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr prev_pc (new pcl::PointCloud<pcl::PointXYZ>);
*prev_pc = cloud_seq_loaded.front();
pcl::PointCloud<pcl::PointXYZ>::Ptr curr_pc (new pcl::PointCloud<pcl::PointXYZ>);
*curr_pc =pcl_pc;
pcl::RandomSample<pcl::PointXYZ> rs(true);
rs.setSample(20000);
rs.setInputCloud(curr_pc);
std::vector<int> indices;
rs.filter (indices);
pcl::PointCloud<pcl::PointXYZ> curr_cld_out;
rs.filter(curr_cld_out);
*curr_pc = curr_cld_out;
std::cout<<curr_pc->size()<<std::endl;
rs.setInputCloud(prev_pc);
rs.filter(indices);
pcl::PointCloud<pcl::PointXYZ> prev_cld_out;
rs.filter(prev_cld_out);
*prev_pc = prev_cld_out;
/*
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ> esTrSVD;
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ>::Matrix4 trMat;
esTrSVD.estimateRigidTransformation(*prev_pc,*curr_pc,trMat);
std::cout<<trMat<<std::endl;
*/
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
//pcl::registration::TransformationEstimationPointToPlaneLLS < pcl::PointXYZ,pcl::PointXYZ,float>::Ptr trans_lls (new pcl::registration::TransformationEstimationPointToPlaneLLS < pcl::PointXYZ,pcl::PointXYZ,float>);
pcl::registration::TransformationEstimationLM< pcl::PointXYZ,pcl::PointXYZ,float >::Ptr trans_LM (new pcl::registration::TransformationEstimationLM< pcl::PointXYZ,pcl::PointXYZ,float >);
icp.setInputSource(prev_pc);
icp.setInputTarget(curr_pc);
icp.setTransformationEstimation (trans_LM);
//icp.setMaximumIterations (2);
//icp.setRANSACIterations(5);
icp.setTransformationEpsilon (1e-8);
icp.setMaxCorrespondenceDistance (0.5);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ>::Matrix4 trMat;
trMat = icp.getFinalTransformation();
//*curr_pc = pcl_pc;
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::transformPointCloud (*prev_pc, *transformed_cloud, trMat);
pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
// Define R,G,B colors for the point cloud
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (prev_pc, 255, 255, 255);
// We add the point cloud to the viewer and pass the color handler
viewer.addPointCloud (prev_pc, source_cloud_color_handler, "original_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
viewer.addCoordinateSystem (1.0, 0);
viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
//viewer.setPosition(800, 400); // Setting visualiser window position
while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
viewer.spinOnce ();
}
//std::cout << "Node center: " << it.getCoordinate() << std::endl;
//std::cout << "Node size: " << it.getSize() << std::endl;
//std::cout << "Node value: " << it->getValue() << std::endl;
}
//********** print out the statistics ******************
//**************Process Point cloud in octree data structure *****************
/*
//******************Traverse the tree ********************
for(octomap::OcTree::tree_iterator it =tree.begin_tree(), end = tree.end_tree();it!= end;it++){
//manipulate node, e.g.:
std::cout << "_____________________________________"<<std::endl;
std::cout << "Node center: " << it.getCoordinate() << std::endl;
std::cout << "Node size: " << it.getSize() << std::endl;
std::cout << "Node depth: "<<it.getDepth() << std::endl;
std::cout << "Is Leaf : "<< it.isLeaf()<< std::endl;
std::cout << "_____________________________________"<<std::endl;
}
//**********************************************************
*/
std::cout<<"finished"<<std::endl;
std::cout<<std::endl;
}
// I need to change the parameters to account for the
// transformation matrices. I need to save the best TM
// so I can combine it with the icp TM.
//
// Does kdtree search on each rotation of 45 degrees around
// the central point of the moved_cloud
Eigen::Matrix4f kdtree_search(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr moved_cloud, float radius)
{
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
// Total number of nearest neighbors
int num_matches = 0;
// Return the cloud with the most number of nearest neighbors
// within a given radius of the searchPoint
int max_neighbors = 0;
pcl::PointCloud<pcl::PointXYZ>::Ptr best_fit_cloud ;
Eigen::Matrix4f best_fit_transform = Eigen::Matrix4f::Identity();
Eigen::Vector4f centroid1 = compute_centroid(*moved_cloud);
cout << "centroid of source cloud - " << centroid1(0)
<< ", " << centroid1(1) << ", " << centroid1(2) << endl;
// Executing the transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr rotated_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
Eigen::Matrix4f transform_2 = Eigen::Matrix4f::Identity();
for (int i = 0; i < 8; i++)
{
float theta = 45 * i + 45;
transform_1 (0,0) = cos (theta);
transform_1 (0,2) = -sin (theta);
transform_1 (2,0) = sin (theta);
transform_1 (2,2) = cos (theta);
cout << "cloud with " << theta << " degrees of rotation" << endl;
pcl::transformPointCloud (*moved_cloud, *rotated_cloud, transform_1);
// Probably need to compute centroid of the new transformed cloud
// because the transformation seems to translate it
Eigen::Vector4f centroid2 = compute_centroid(*rotated_cloud);
cout << "centroid of rotated cloud - " << centroid2(0)
<< ", " << centroid2(1) << ", " << centroid2(2) << endl;
float distance_x = centroid1(0) - centroid2(0);
float distance_y = centroid1(1) - centroid2(1);
float distance_z = centroid1(2) - centroid2(2);
cout << "distance between centroids: (" << distance_x << ", " << distance_y << ", " << distance_z << ")" << endl;
transform_2 (0,3) = (distance_x);
transform_2 (1,3) = (distance_y);
transform_2 (2,3) = (distance_z);
pcl::transformPointCloud (*rotated_cloud, *transformed_cloud, transform_2);
// Rotate the cloud by 45 degrees each time
// May want to add some random translation as well.
// This would correspond to doing kdtree search on a number of
// clouds that are presumably close to the target point cloud.
kdtree.setInputCloud(transformed_cloud);
// Run the kdtree search on every 10th point of the moved_cloud
// Increase this number to speed up the search
// Test with different increments of i to see effect on speed
for (int j = 0; j < (*moved_cloud).points.size(); j += 10)
{
pcl::PointXYZ searchPoint = moved_cloud->points[j];
int num_neighbors = kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
num_matches += num_neighbors;
cout << "performed kdtree nearest neighbor search, found " << num_neighbors << " within " << radius << " radius" << endl;
}
cout << "num_matches = " << num_matches << endl;
cout << "max_neighbors = " << max_neighbors << endl;
if (num_matches > max_neighbors) {
max_neighbors = num_matches;
best_fit_cloud = transformed_cloud;
// are these transforms relative? or absolute?
// this currently calculates relative
// should be transform_2 * transform_1 if absolute
best_fit_transform = transform_2 * transform_1;
}
num_matches = 0;
printf( "\nPoint cloud colors : white = original point cloud\n"
" red = transformed point cloud\n"
" blue = moved point cloud\n"
" green = rotated point cloud\n");
pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
// Define R,G,B colors for the point cloud
// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255); // white
// We add the point cloud to the viewer and pass the color handler
// viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> rotated_cloud_color_handler (rotated_cloud, 20, 245, 20); // green
viewer.addPointCloud (rotated_cloud, rotated_cloud_color_handler, "rotated_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> moved_cloud_color_handler (moved_cloud, 20, 230, 230); // blue
viewer.addPointCloud (moved_cloud, moved_cloud_color_handler, "moved_cloud");
viewer.addCoordinateSystem (1.0, 0);
viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
// viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "rotated_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "moved_cloud");
//viewer.setPosition(800, 400); // Setting visualiser window position
while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
viewer.spinOnce ();
}
}
// check whether the translations are relative or absolute
pcl::PointCloud<pcl::PointXYZ>::Ptr best_transform_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::transformPointCloud (*moved_cloud, *best_transform_cloud, best_fit_transform);
pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> best_transform_cloud_color_handler (best_transform_cloud, 245, 20, 20); // red
viewer.addPointCloud (best_transform_cloud, best_transform_cloud_color_handler, "best_transform_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> best_fit_cloud_color_handler (best_fit_cloud, 20, 245, 20); // green
viewer.addPointCloud (best_transform_cloud, best_fit_cloud_color_handler, "best_fit_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> input_cloud_color_handler (input_cloud, 255, 255, 255); // white
viewer.addPointCloud (input_cloud, input_cloud_color_handler, "input_cloud");
viewer.addCoordinateSystem (1.0, 0);
viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "best_transform_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "best_fit_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "input_cloud");
while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
viewer.spinOnce ();
}
return best_fit_transform;
}
// This is the main function
int
main (int argc, char** argv)
{
// Show help
if (pcl::console::find_switch (argc, argv, "-h") || pcl::console::find_switch (argc, argv, "--help")) {
showHelp (argv[0]);
return 0;
}
// Fetch point cloud filename in arguments | Works with PCD and PLY files
std::vector<int> filenames;
bool file_is_pcd = false;
filenames = pcl::console::parse_file_extension_argument (argc, argv, ".ply");
if (filenames.size () != 1) {
filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
if (filenames.size () != 1) {
showHelp (argv[0]);
return -1;
} else {
file_is_pcd = true;
}
}
// Load file | Works with PCD and PLY files
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
if (file_is_pcd) {
if (pcl::io::loadPCDFile (argv[filenames[0]], *source_cloud) < 0) {
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
showHelp (argv[0]);
return -1;
}
} else {
if (pcl::io::loadPLYFile (argv[filenames[0]], *source_cloud) < 0) {
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
showHelp (argv[0]);
return -1;
}
}
/* Reminder: how transformation matrices work :
|-------> This column is the translation
| 1 0 0 x | \
| 0 1 0 y | }-> The identity 3x3 matrix (no rotation) on the left
| 0 0 1 z | /
| 0 0 0 1 | -> We do not use this line (and it has to stay 0,0,0,1)
METHOD #1: Using a Matrix4f
This is the "manual" method, perfect to understand but error prone !
*/
Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
// Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
float theta = M_PI/4; // The angle of rotation in radians
transform_1 (0,0) = cos (theta);
transform_1 (0,1) = -sin(theta);
transform_1 (1,0) = sin (theta);
transform_1 (1,1) = cos (theta);
// (row, column)
// Define a translation of 2.5 meters on the x axis.
transform_1 (0,3) = 2.5;
// Print the transformation
printf ("Method #1: using a Matrix4f\n");
std::cout << transform_1 << std::endl;
/* METHOD #2: Using a Affine3f
This method is easier and less error prone
*/
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
// Define a translation of 2.5 meters on the x axis.
transform_2.translation() << 2.5, 0.0, 0.0;
// The same rotation matrix as before; theta radians arround Z axis
transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));
// Print the transformation
printf ("\nMethod #2: using an Affine3f\n");
std::cout << transform_2.matrix() << std::endl;
// Executing the transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
// You can either apply transform_1 or transform_2; they are the same
pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2);
// Visualization
printf( "\nPoint cloud colors : white = original point cloud\n"
" red = transformed point cloud\n");
pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
// Define R,G,B colors for the point cloud
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255);
// We add the point cloud to the viewer and pass the color handler
viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
viewer.addCoordinateSystem (1.0, "cloud", 0);
viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
//viewer.setPosition(800, 400); // Setting visualiser window position
while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
viewer.spinOnce ();
}
return 0;
}
// Main function
int main (int argc, char** argv)
{
// Show help
if (pcl::console::find_switch (argc, argv, "-h") || pcl::console::find_switch (argc, argv, "--help")) {
showHelp (argv[0]);
return 0;
}
// Fetch point cloud filename in arguments | Works with PCD files
std::vector<int> filenames;
bool file_is_pcd = false;
if (filenames.size () != 2) {
filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
if (filenames.size () != 2) {
showHelp (argv[0]);
return -1;
} else {
file_is_pcd = true;
}
}
// Load file | Works with PCD and PLY files
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
// The source cloud is the original cloud
if (pcl::io::loadPCDFile (argv[filenames[0]], *source_cloud) < 0) {
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
showHelp (argv[0]);
return -1;
}
// The moved cloud is the original cloud after a transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr moved_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::io::loadPCDFile (argv[filenames[1]], *moved_cloud);
// compute the distance between the source_cloud's centroid and
// the moved_cloud's centroid
/*
Eigen::Vector4f zero(4);
zero << 0,0,0,0;
unsigned int centroid1 = pcl::compute3DCentroid(*source_cloud, zero);
unsigned int centroid2 = pcl::compute3DCentroid(*moved_cloud, zero);
float distance = 0;
float centroid1d = (float)centroid1;
float centroid2d = (float)centroid2;
// ints or floats won't do. I need a 3D vector or something
// so i can translate the x,y,z points over to the new location.
// unfortunately, the only other compute centroid function I can
// find returns void.
distance = (centroid1d - centroid2d) / 1000.0;
cout << centroid1 << std::endl;
cout << centroid2 << std::endl;
cout << distance << std::endl;
*/
/*
If I'm going to translate the new cloud to the correct
centroid position later, I don't need this initial translation.
Eigen::Vector4f centroid2 = compute_centroid(*moved_cloud);
float distance_x = centroid2(0) - centroid1(0);
float distance_y = centroid2(1) - centroid1(1);
float distance_z = centroid2(2) - centroid1(2);
Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
// Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
// float theta = 45; // The angle of rotation in radians
// transform_1 (2,0) = sin (theta);
// transform_1 (2,1) = cos (theta);
transform_1 (0,3) = abs(distance_x);
transform_1 (1,3) = abs(distance_y);
transform_1 (2,3) = abs(distance_z);
*/
// Executing the transformation
// pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
// pcl::transformPointCloud (*source_cloud, *transform_cloud, transform_1);
// transformed_cloud = kdtree_search(moved_cloud, transform_cloud);
/*
for (int i = 0; i < 8; i++)
{
// this transformation isn't giving me an overlay of the moved
// cloud at different angle rotations
// it's accompanied by a translation that puts the resulting
// cloud too far from the moved_cloud, so the kdtree estimate
// won't be too good
// However, if icp can correct for this, it might be ok
// ideally, I would want this to work as well as possible
// to speed up icp.
// I also am not passing the original centroid-translated
// point cloud to this kdtree search. I need to find a way to do that
//
// The issue here is that I'm not translating the cloud correctly
// based on the distance between the centroids. It's more complicated
// than just translating the cloud by the distance.
// Look at the picture Robbie drew. There's lin alg involved.
// passing in 0 for theta doesn't work because cos(0) = 1
// Add 45 degrees so that you don't do cos 0
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
Eigen::Matrix4f best_fit_transform = kdtree_search(source_cloud, moved_cloud, 0.04);
pcl::transformPointCloud (*moved_cloud, *transformed_cloud, best_fit_transform);
// Visualization
printf( "\nPoint cloud colors : white = original point cloud\n"
" red = transformed point cloud\n"
" blue = moved point cloud\n");
// " green = rotated point cloud\n");
pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
// Define R,G,B colors for the point cloud
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255); // white
// We add the point cloud to the viewer and pass the color handler
viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");
/*
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> rotated_cloud_color_handler (rotated_cloud, 20, 245, 20); // green
viewer.addPointCloud (rotated_cloud, rotated_cloud_color_handler, "rotated_cloud");
*/
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> moved_cloud_color_handler (moved_cloud, 20, 230, 230); // blue
viewer.addPointCloud (moved_cloud, moved_cloud_color_handler, "moved_cloud");
viewer.addCoordinateSystem (1.0, 0);
viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
// viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "rotated_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "moved_cloud");
//viewer.setPosition(800, 400); // Setting visualiser window position
while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
viewer.spinOnce ();
}
return 0;
}
