int main (int argc, char* argv[])
{
boost::program_options::options_description desc("Options");
desc.add_options()
("help", "Print help message")
("pcd_filename", boost::program_options::value<std::string>()->required(), "pcl pcd filename");
boost::program_options::variables_map vm;
try {
boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
if( vm.count("help") ){
std::cout << "Resize Point cloud using voxel grid filter" << std::endl;
std::cerr << desc << std::endl;
return 0;
}
boost::program_options::notify(vm);
} catch (boost::program_options::error& e) {
std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
std::cerr << desc << std::endl;
return -1;
}
std::string pcd_fname = vm["pcd_filename"].as<std::string>();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile (pcd_fname, *source_cloud);
// Rotate pointcloud 0.6[rad] around z axis
Eigen::Matrix4f rotate_around_zaxis = Eigen::Matrix4f::Identity();
float theta = 0.6;
rotate_around_zaxis(0,0) = cos(theta);
rotate_around_zaxis(0,1) = -sin(theta);
rotate_around_zaxis(1,0) = sin(theta);
rotate_around_zaxis(1,1) = cos(theta);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rotated_cloud(new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::transformPointCloud(*source_cloud, *rotated_cloud, rotate_around_zaxis);
// Transform point cloud using homogeneous transformation matrix
// which is got by icp mathcing between 3D map reference point and manual map reference point
Eigen::Matrix4f icp_transform_matrix = Eigen::Matrix4f::Identity();
icp_transform_matrix(0, 0) = 0.950235;
icp_transform_matrix(0, 1) = -0.307961;
icp_transform_matrix(0, 2) = 0.0470266;
icp_transform_matrix(0, 3) = 0.147511;
icp_transform_matrix(1, 0) = 0.30832;
icp_transform_matrix(1, 1) = 0.951281;
icp_transform_matrix(1, 2) = -0.000411891;
icp_transform_matrix(1, 3) = -0.0305933;
icp_transform_matrix(2, 0) = -0.0446088;
icp_transform_matrix(2, 1) = 0.0148907;
icp_transform_matrix(2, 2) = 0.998893;
icp_transform_matrix(2, 3) = -0.0312964;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::transformPointCloud(*rotated_cloud, *transformed_cloud, icp_transform_matrix);
pcl::io::savePCDFileASCII("transformed_cloud.pcd", *transformed_cloud);
std::cout << "Write to transformed_cloud.pcd" << std::endl;
return 0;
}
template <typename PointT> typename open_ptrack::detection::GroundBasedPeopleDetectionApp<PointT>::PointCloudPtr
open_ptrack::detection::GroundBasedPeopleDetectionApp<PointT>::rotateCloud(PointCloudPtr cloud, Eigen::Affine3f transform )
{
PointCloudPtr rotated_cloud (new PointCloud);
pcl::transformPointCloud(*cloud, *rotated_cloud, transform);
rotated_cloud->header.frame_id = cloud->header.frame_id;
return rotated_cloud;
}
// 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;
}
