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;
}
// searches for points from a given point cloud in another pointcloud to convert a pointcloud to indices
pcl17::PointIndicesPtr getIndicesFromPointCloud(const PointCloudConstPtr& input_cloud_ptr,
const pcl17::PointIndicesConstPtr& input_indices_ptr,
const PointCloudConstPtr& search_cloud_ptr)
{
PointCloudPtr source_cloud (new PointCloud);
pcl17::ExtractIndices<PointType> extractor;
extractor.setIndices(input_indices_ptr);
extractor.setInputCloud(input_cloud_ptr);
extractor.filter(*source_cloud);
return getIndicesFromPointCloud(source_cloud, search_cloud_ptr);
}
//get intersection points
void get_intr_points(MyPointCloud& source_mpc, MyPointCloud& sample_mpc, float search_r, int* intr_points_num){
*intr_points_num=0;
PointCloudPtr source_cloud(new PointCloud);
MyPointCloud2PointCloud(source_mpc, source_cloud);
PointCloudPtr sample_cloud(new PointCloud);
MyPointCloud2PointCloud(sample_mpc, sample_cloud);
PointCloudPtr intr_cloud(new PointCloud);
float resolution = 0.005f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);
octree.setInputCloud (source_cloud);
octree.addPointsFromInputCloud ();
for(int i=0;i<sample_mpc.mypoints.size();i++){
// Neighbors within radius search
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
float radius = search_r;
if (octree.radiusSearch(sample_cloud->at(i), radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
{
if(pointIdxRadiusSearch.size()>0){
PointCloudPtr cloud_tem(new PointCloud);
for (size_t j = 0; j < pointIdxRadiusSearch.size (); ++j){
cloud_tem->push_back(source_cloud->points[ pointIdxRadiusSearch[j]]);
}
Point finded_pt;
if(findNearestNeighbor(cloud_tem, intr_cloud, sample_cloud->at(i), finded_pt)){
intr_cloud->push_back(finded_pt);
(*intr_points_num)+=1;
}
}
}
}
}
// 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;
}
int main (int argc, char** argv)
{
//---------------------------------------------------------------------------------------------------
//-- Initialization stuff
//---------------------------------------------------------------------------------------------------
//-- Command-line arguments
float ransac_threshold = 0.02;
float hsv_s_threshold = 0.30;
float hsv_v_threshold = 0.35;
//-- Show usage
if (pcl::console::find_switch(argc, argv, "-h") || pcl::console::find_switch(argc, argv, "--help"))
{
show_usage(argv[0]);
return 0;
}
if (pcl::console::find_switch(argc, argv, "--ransac-threshold"))
pcl::console::parse_argument(argc, argv, "--ransac-threshold", ransac_threshold);
else
{
std::cerr << "RANSAC theshold not specified, using default value..." << std::endl;
}
if (pcl::console::find_switch(argc, argv, "--hsv-s-threshold"))
pcl::console::parse_argument(argc, argv, "--hsv-s-threshold", hsv_s_threshold);
else
{
std::cerr << "Saturation theshold not specified, using default value..." << std::endl;
}
if (pcl::console::find_switch(argc, argv, "--hsv-v-threshold"))
pcl::console::parse_argument(argc, argv, "--hsv-v-threshold", hsv_v_threshold);
else
{
std::cerr << "Value theshold not specified, using default value..." << std::endl;
}
//-- Get point cloud file from arguments
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)
{
show_usage(argv[0]);
return -1;
}
file_is_pcd = true;
}
//-- Load point cloud data
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;
show_usage(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;
show_usage(argv[0]);
return -1;
}
}
//-- Load point cloud data (with color)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_cloud_color(new pcl::PointCloud<pcl::PointXYZRGB>);
if (file_is_pcd)
{
if (pcl::io::loadPCDFile(argv[filenames[0]], *source_cloud_color) < 0)
{
std::cout << "Error loading colored point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
else
{
if (pcl::io::loadPLYFile(argv[filenames[0]], *source_cloud_color) < 0)
{
std::cout << "Error loading colored point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
//-- Print arguments to user
std::cout << "Selected arguments: " << std::endl
<< "\tRANSAC threshold: " << ransac_threshold << std::endl
<< "\tColor point threshold: " << hsv_s_threshold << std::endl
<< "\tColor region threshold: " << hsv_v_threshold << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
//--------------------------------------------------------------------------------------------------------
//-- Program does actual work from here
//--------------------------------------------------------------------------------------------------------
Debug debug;
debug.setAutoShow(false);
debug.setEnabled(false);
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(source_cloud_color, Debug::COLOR_ORIGINAL);
debug.show("Original with color");
//-- Downsample the dataset prior to plane detection (using a leaf size of 1cm)
//-----------------------------------------------------------------------------------
pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
voxel_grid.setInputCloud(source_cloud);
voxel_grid.setLeafSize(0.01f, 0.01f, 0.01f);
voxel_grid.filter(*cloud_filtered);
std::cout << "Initially PointCloud has: " << source_cloud->points.size () << " data points." << std::endl;
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
//-- Detect all possible planes
//-----------------------------------------------------------------------------------
std::vector<pcl::ModelCoefficientsPtr> all_planes;
pcl::SACSegmentation<pcl::PointXYZ> ransac_segmentation;
ransac_segmentation.setOptimizeCoefficients(true);
ransac_segmentation.setModelType(pcl::SACMODEL_PLANE);
ransac_segmentation.setMethodType(pcl::SAC_RANSAC);
ransac_segmentation.setDistanceThreshold(ransac_threshold);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr current_plane(new pcl::ModelCoefficients);
int i=0, nr_points = (int) cloud_filtered->points.size();
while (cloud_filtered->points.size() > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
ransac_segmentation.setInputCloud(cloud_filtered);
ransac_segmentation.segment(*inliers, *current_plane);
if (inliers->indices.size() == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
// Remove the planar inliers, extract the rest
extract.setNegative(true);
extract.filter(*cloud_f);
*cloud_filtered = *cloud_f;
//-- Save plane
pcl::ModelCoefficients::Ptr copy_current_plane(new pcl::ModelCoefficients);
*copy_current_plane = *current_plane;
all_planes.push_back(copy_current_plane);
//-- Debug stuff
debug.setEnabled(false);
debug.plotPlane(*current_plane, Debug::COLOR_BLUE);
debug.plotPointCloud<pcl::PointXYZ>(cloud_filtered, Debug::COLOR_RED);
debug.show("Plane segmentation");
}
//-- Filter planes to obtain garment plane
//-----------------------------------------------------------------------------------
pcl::ModelCoefficients::Ptr garment_plane(new pcl::ModelCoefficients);
float min_height = FLT_MAX;
pcl::PointXYZ garment_projected_center;
for(int i = 0; i < all_planes.size(); i++)
{
//-- Check orientation
Eigen::Vector3f normal_vector(all_planes[i]->values[0],
all_planes[i]->values[1],
all_planes[i]->values[2]);
normal_vector.normalize();
Eigen::Vector3f good_orientation(0, -1, -1);
good_orientation.normalize();
std::cout << "Checking vector with dot product: " << std::abs(normal_vector.dot(good_orientation)) << std::endl;
if (std::abs(normal_vector.dot(good_orientation)) >= 0.9)
{
//-- Check "height" (height is defined in the local frame of reference in the yz direction)
//-- With this frame, it is approximately equal to the norm of the vector OO' (being O the
//-- center of the old frame and O' the projection of that center onto the plane).
//-- Project center point onto given plane:
pcl::PointCloud<pcl::PointXYZ>::Ptr center_to_be_projected_cloud(new pcl::PointCloud<pcl::PointXYZ>);
center_to_be_projected_cloud->points.push_back(pcl::PointXYZ(0,0,0));
pcl::PointCloud<pcl::PointXYZ>::Ptr center_projected_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ProjectInliers<pcl::PointXYZ> project_inliners;
project_inliners.setModelType(pcl::SACMODEL_PLANE);
project_inliners.setInputCloud(center_to_be_projected_cloud);
project_inliners.setModelCoefficients(all_planes[i]);
project_inliners.filter(*center_projected_cloud);
pcl::PointXYZ projected_center = center_projected_cloud->points[0];
Eigen::Vector3f projected_center_vector(projected_center.x, projected_center.y, projected_center.z);
float height = projected_center_vector.norm();
if (height < min_height)
{
min_height = height;
*garment_plane = *all_planes[i];
garment_projected_center = projected_center;
}
}
}
if (!(min_height < FLT_MAX))
{
std::cerr << "Garment plane not found!" << std::endl;
return -3;
}
else
{
std::cout << "Found closest plane with h=" << min_height << std::endl;
//-- Debug stuff
debug.setEnabled(true);
debug.plotPlane(*garment_plane, Debug::COLOR_BLUE);
debug.plotPointCloud<pcl::PointXYZ>(source_cloud, Debug::COLOR_RED);
debug.show("Garment plane");
}
//-- Reorient cloud to origin (with color point cloud)
//-----------------------------------------------------------------------------------
//-- Translating to center
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr centered_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Affine3f translation_transform = Eigen::Affine3f::Identity();
translation_transform.translation() << -garment_projected_center.x, -garment_projected_center.y, -garment_projected_center.z;
//pcl::transformPointCloud(*source_cloud_color, *centered_cloud, translation_transform);
//-- Orient using the plane normal
pcl::PointCloud<pcl::PointXYZRGB>::Ptr oriented_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Vector3f normal_vector(garment_plane->values[0], garment_plane->values[1], garment_plane->values[2]);
//-- Check normal vector orientation
if (normal_vector.dot(Eigen::Vector3f::UnitZ()) >= 0 && normal_vector.dot(Eigen::Vector3f::UnitY()) >= 0)
normal_vector = -normal_vector;
Eigen::Quaternionf rotation_quaternion = Eigen::Quaternionf().setFromTwoVectors(normal_vector, Eigen::Vector3f::UnitZ());
//pcl::transformPointCloud(*centered_cloud, *oriented_cloud, Eigen::Vector3f(0,0,0), rotation_quaternion);
Eigen::Transform<float, 3, Eigen::Affine> t(rotation_quaternion * translation_transform);
pcl::transformPointCloud(*source_cloud_color, *oriented_cloud, t);
//-- Save to file
record_transformation(argv[filenames[0]]+std::string("-transform1.txt"), translation_transform, rotation_quaternion);
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(oriented_cloud, Debug::COLOR_GREEN);
debug.show("Oriented");
//-- Filter points under the garment table
//-----------------------------------------------------------------------------------
pcl::PointCloud<pcl::PointXYZRGB>::Ptr garment_table_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PassThrough<pcl::PointXYZRGB> passthrough_filter;
passthrough_filter.setInputCloud(oriented_cloud);
passthrough_filter.setFilterFieldName("z");
passthrough_filter.setFilterLimits(-ransac_threshold/2.0f, FLT_MAX);
passthrough_filter.setFilterLimitsNegative(false);
passthrough_filter.filter(*garment_table_cloud);
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(garment_table_cloud, Debug::COLOR_GREEN);
debug.show("Table cloud (filtered)");
//-- Color segmentation of the garment
//-----------------------------------------------------------------------------------
//-- HSV thresholding
pcl::PointCloud<pcl::PointXYZHSV>::Ptr hsv_cloud(new pcl::PointCloud<pcl::PointXYZHSV>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered_garment_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloudXYZRGBtoXYZHSV(*garment_table_cloud, *hsv_cloud);
for (int i = 0; i < hsv_cloud->points.size(); i++)
{
if (isnan(hsv_cloud->points[i].x) || isnan(hsv_cloud->points[i].y || isnan(hsv_cloud->points[i].z)))
continue;
if (hsv_cloud->points[i].s > hsv_s_threshold && hsv_cloud->points[i].v > hsv_v_threshold)
filtered_garment_cloud->push_back(garment_table_cloud->points[i]);
}
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(filtered_garment_cloud, Debug::COLOR_GREEN);
debug.show("Garment cloud");
//-- Euclidean Clustering of the resultant cloud
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud(filtered_garment_cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> euclidean_custering;
euclidean_custering.setClusterTolerance(0.005);
euclidean_custering.setMinClusterSize(100);
euclidean_custering.setSearchMethod(tree);
euclidean_custering.setInputCloud(filtered_garment_cloud);
euclidean_custering.extract(cluster_indices);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr largest_color_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
int largest_cluster_size = 0;
for (auto it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
for (auto pit = it->indices.begin (); pit != it->indices.end (); ++pit)
cloud_cluster->points.push_back(filtered_garment_cloud->points[*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "Found cluster of " << cloud_cluster->points.size() << " points." << std::endl;
if (cloud_cluster->points.size() > largest_cluster_size)
{
largest_cluster_size = cloud_cluster->points.size();
*largest_color_cluster = *cloud_cluster;
}
}
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(largest_color_cluster, Debug::COLOR_GREEN);
debug.show("Filtered garment cloud");
//-- Centering the point cloud before saving it
//-----------------------------------------------------------------------------------
//-- Find bounding box
pcl::MomentOfInertiaEstimation<pcl::PointXYZRGB> feature_extractor;
pcl::PointXYZRGB min_point_AABB, max_point_AABB;
pcl::PointXYZRGB min_point_OBB, max_point_OBB;
pcl::PointXYZRGB position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
feature_extractor.setInputCloud(largest_color_cluster);
feature_extractor.compute();
feature_extractor.getAABB(min_point_AABB, max_point_AABB);
feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
//-- Translating to center
pcl::PointCloud<pcl::PointXYZRGB>::Ptr centered_garment_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Affine3f garment_translation_transform = Eigen::Affine3f::Identity();
garment_translation_transform.translation() << -position_OBB.x, -position_OBB.y, -position_OBB.z;
pcl::transformPointCloud(*largest_color_cluster, *centered_garment_cloud, garment_translation_transform);
//-- Orient using the principal axes of the bounding box
pcl::PointCloud<pcl::PointXYZRGB>::Ptr oriented_garment_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Vector3f principal_axis_x(max_point_OBB.x - min_point_OBB.x, 0, 0);
Eigen::Quaternionf garment_rotation_quaternion = Eigen::Quaternionf().setFromTwoVectors(principal_axis_x, Eigen::Vector3f::UnitX()); //-- This transformation is wrong (I guess)
Eigen::Transform<float, 3, Eigen::Affine> t2 = Eigen::Transform<float, 3, Eigen::Affine>::Identity();
t2.rotate(rotational_matrix_OBB.inverse());
//pcl::transformPointCloud(*centered_garment_cloud, *oriented_garment_cloud, Eigen::Vector3f(0,0,0), garment_rotation_quaternion);
pcl::transformPointCloud(*centered_garment_cloud, *oriented_garment_cloud, t2);
//-- Save to file
record_transformation(argv[filenames[0]]+std::string("-transform2.txt"), garment_translation_transform, Eigen::Quaternionf(t2.rotation()));
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(oriented_garment_cloud, Debug::COLOR_GREEN);
debug.plotBoundingBox(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB, Debug::COLOR_YELLOW);
debug.show("Oriented garment patch");
//-- Save point cloud in file to process it in Python
pcl::io::savePCDFileBinary(argv[filenames[0]]+std::string("-output.pcd"), *oriented_garment_cloud);
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;
}
int main(int argc, char* argv[])
{
//-- Main program parameters (default values)
double threshold = 0.03;
std::string output_histogram_image = "histogram_image.m";
std::string output_rsd_data = "rsd_data.m";
double rsd_normal_radius = 0.05;
double rsd_curvature_radius = 0.07;
double rsd_plane_threshold = 0.2;
//-- Show usage
if (pcl::console::find_switch(argc, argv, "-h") || pcl::console::find_switch(argc, argv, "--help"))
{
show_usage(argv[0]);
return 0;
}
//-- Check for parameters in arguments
if (pcl::console::find_switch(argc, argv, "-t"))
pcl::console::parse_argument(argc, argv, "-t", threshold);
else if (pcl::console::find_switch(argc, argv, "--threshold"))
pcl::console::parse_argument(argc, argv, "--threshold", threshold);
if (pcl::console::find_switch(argc, argv, "--histogram"))
pcl::console::parse_argument(argc, argv, "--histogram", output_histogram_image);
if (pcl::console::find_switch(argc, argv, "-r"))
pcl::console::parse_argument(argc, argv, "-r", output_rsd_data);
else if (pcl::console::find_switch(argc, argv, "--rsd"))
pcl::console::parse_argument(argc, argv, "--rsd", output_rsd_data);
if (pcl::console::find_switch(argc, argv, "--rsd-params"))
pcl::console::parse_3x_arguments(argc, argv, "--rsd-params", rsd_normal_radius, rsd_curvature_radius,
rsd_plane_threshold);
//-- Get point cloud file from arguments
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)
{
std::cerr << "No input file specified!" << std::endl;
show_usage(argv[0]);
return -1;
}
file_is_pcd = true;
}
//-- Print arguments to user
std::cout << "Selected arguments: " << std::endl
<< "\tRANSAC threshold: " << threshold << std::endl
#ifdef HISTOGRAM
<< "\tHistogram image: " << output_histogram_image << std::endl
#endif
#ifdef CURVATURE
<< "\tRSD:" << std::endl
<< "\t\tFile: " << output_rsd_data << std::endl
<< "\t\tNormal search radius: " << rsd_normal_radius << std::endl
<< "\t\tCurvature search radius: " << rsd_curvature_radius << std::endl
<< "\t\tPlane threshold: " << rsd_plane_threshold << std::endl
#endif
<< "\tInput file: " << argv[filenames[0]] << std::endl;
//-- Load point cloud data
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;
show_usage(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;
show_usage(argv[0]);
return -1;
}
}
/********************************************************************************************
* Stuff goes on here
*********************************************************************************************/
//-- Initial pre-processing of the mesh
//------------------------------------------------------------------------------------
std::cout << "[+] Pre-processing the mesh..." << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr garment_points(new pcl::PointCloud<pcl::PointXYZ>);
MeshPreprocessor<pcl::PointXYZ> preprocessor;
preprocessor.setRANSACThresholdDistance(threshold);
preprocessor.setInputCloud(source_cloud);
preprocessor.process(*garment_points);
//-- Find bounding box (not really required)
pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extractor;
pcl::PointXYZ min_point_AABB, max_point_AABB;
pcl::PointXYZ min_point_OBB, max_point_OBB;
pcl::PointXYZ position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
feature_extractor.setInputCloud(garment_points);
feature_extractor.compute();
feature_extractor.getAABB(min_point_AABB, max_point_AABB);
feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
#ifdef CURVATURE
//-- Curvature stuff
//-----------------------------------------------------------------------------------
std::cout << "[+] Calculating curvature descriptors..." << std::endl;
// Object for storing the normals.
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
// Object for storing the RSD descriptors for each point.
pcl::PointCloud<pcl::PrincipalRadiiRSD>::Ptr descriptors(new pcl::PointCloud<pcl::PrincipalRadiiRSD>());
// Note: you would usually perform downsampling now. It has been omitted here
// for simplicity, but be aware that computation can take a long time.
// Estimate the normals.
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> normalEstimation;
normalEstimation.setInputCloud(garment_points);
normalEstimation.setRadiusSearch(rsd_normal_radius);
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
normalEstimation.setSearchMethod(kdtree);
normalEstimation.setViewPoint(0, 0 , 2);
normalEstimation.compute(*normals);
// RSD estimation object.
pcl::RSDEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalRadiiRSD> rsd;
rsd.setInputCloud(garment_points);
rsd.setInputNormals(normals);
rsd.setSearchMethod(kdtree);
// Search radius, to look for neighbors. Note: the value given here has to be
// larger than the radius used to estimate the normals.
rsd.setRadiusSearch(rsd_curvature_radius);
// Plane radius. Any radius larger than this is considered infinite (a plane).
rsd.setPlaneRadius(rsd_plane_threshold);
// Do we want to save the full distance-angle histograms?
rsd.setSaveHistograms(false);
rsd.compute(*descriptors);
//-- Save to mat file
std::ofstream rsd_file(output_rsd_data.c_str());
for (int i = 0; i < garment_points->points.size(); i++)
{
rsd_file << garment_points->points[i].x << " "
<< garment_points->points[i].y << " "
<< garment_points->points[i].z << " "
<< descriptors->points[i].r_min << " "
<< descriptors->points[i].r_max << "\n";
}
rsd_file.close();
#endif
#ifdef HISTOGRAM
//-- Obtain histogram image
//-----------------------------------------------------------------------------------
std::cout << "[+] Calculating histogram image..." << std::endl;
HistogramImageCreator<pcl::PointXYZ> histogram_image_creator;
histogram_image_creator.setInputPointCloud(garment_points);
histogram_image_creator.setResolution(1024);
histogram_image_creator.setUpsampling(true);
histogram_image_creator.compute();
Eigen::MatrixXi image = histogram_image_creator.getDepthImageAsMatrix();
//-- Temporal fix to get image (through file)
std::ofstream file(output_histogram_image.c_str());
file << image;
file.close();
#endif
return 0;
}
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;
}
