inline void coarse_search_new(std::vector< std::pair<FloatBox, Identifier> >& local_domain, std::vector< std::pair<FloatBox, Identifier> >& local_range, NewSearchMethod algorithm, MPI_Comm comm, std::vector<std::pair<Identifier,Identifier> >& searchResults)
{
if ( algorithm == GTK )
{
kdtree_search(local_domain, local_range, comm, searchResults);
}
else if ( algorithm == OCTREE )
{
stk::search::coarse_search(local_domain, local_range, stk::search::OCTREE, comm, searchResults);
}
else if ( algorithm == BOOST_RTREE )
{
stk::search::coarse_search(local_domain, local_range, stk::search::BOOST_RTREE, comm, searchResults);
}
else
{
throw("Invalid search algorithm: not supported.\n");
}
}
// 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;
}
