int main (int argc, char **argv) {
PointCloudxyz::Ptr object (new PointCloudxyz);
PointCloudxyz::Ptr scene (new PointCloudxyz);
PointCloudxyz::Ptr object_aligned (new PointCloudxyz);
// Get input object and scene
if (argc != 2)
{
pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
return (1);
}
// Load object and scene
pcl::console::print_highlight ("Loading point clouds...\n");
pcl_tools::cloud_from_ply(argv[1], *object);
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*object, *object, indices);
int index = closest_point_line(*object, Eigen::Vector3f(0.57735054, 0.57735054, 0.57735054), Eigen::Vector3f(0.0, 0.0, 0.0));
// int index = closest_point_line(*object, Eigen::Vector3f::UnitY(), Eigen::Vector3f(0.0, 0.0, 0.0));
pcl::PointXYZ centerpt = object->points[index];
pcl::visualization::PCLVisualizer visu("LineFind");
visu.addPointCloud(object, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(object, 0.0, 0.0, 255.0), "object");
visu.addSphere(centerpt, 0.005, "location");
// visu.addLine<Eigen::Vector4f, Eigen::Vector4f>(Eigen::Vector4f(0.0, 0.0, 0.0, 1.0), Eigen::Vector4f(0.0, 0.0, 0.0, 1.0) + Eigen::Vector4f::UnitX(), "line");
visu.addCoordinateSystem (0.04, "View_1");
// visu.addPointCloudNormals<PointNT>(object);
visu.spin ();
}
// Align a rigid object to a scene with clutter and occlusions
int
main (int argc, char **argv)
{
// Point clouds
PointCloudT::Ptr object (new PointCloudT);
PointCloudT::Ptr object_aligned (new PointCloudT);
PointCloudT::Ptr scene (new PointCloudT);
FeatureCloudT::Ptr object_features (new FeatureCloudT);
FeatureCloudT::Ptr scene_features (new FeatureCloudT);
// Get input object and scene
if (argc != 3)
{
pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
return (1);
}
// Load object and scene
pcl::console::print_highlight ("Loading point clouds...\n");
if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 ||
pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0)
{
pcl::console::print_error ("Error loading object/scene file!\n");
return (1);
}
// Downsample
pcl::console::print_highlight ("Downsampling...\n");
pcl::VoxelGrid<PointNT> grid;
const float leaf = 0.005f;
grid.setLeafSize (leaf, leaf, leaf);
grid.setInputCloud (object);
grid.filter (*object);
grid.setInputCloud (scene);
grid.filter (*scene);
// Estimate normals for scene
pcl::console::print_highlight ("Estimating scene normals...\n");
pcl::NormalEstimationOMP<PointNT,PointNT> nest;
nest.setRadiusSearch (0.01);
nest.setInputCloud (scene);
nest.compute (*scene);
// Estimate features
pcl::console::print_highlight ("Estimating features...\n");
FeatureEstimationT fest;
fest.setRadiusSearch (0.025);
fest.setInputCloud (object);
fest.setInputNormals (object);
fest.compute (*object_features);
fest.setInputCloud (scene);
fest.setInputNormals (scene);
fest.compute (*scene_features);
// Perform alignment
pcl::console::print_highlight ("Starting alignment...\n");
pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align;
align.setInputSource (object);
align.setSourceFeatures (object_features);
align.setInputTarget (scene);
align.setTargetFeatures (scene_features);
align.setMaximumIterations (100000); // Number of RANSAC iterations
align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose
align.setCorrespondenceRandomness (5); // Number of nearest features to use
align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold
align.setMaxCorrespondenceDistance (2.5f * leaf); // Inlier threshold
align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis
{
pcl::ScopeTime t("Alignment");
align.align (*object_aligned);
}
if (align.hasConverged ())
{
// Print results
printf ("\n");
Eigen::Matrix4f transformation = align.getFinalTransformation ();
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
pcl::console::print_info ("\n");
pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
pcl::console::print_info ("\n");
pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
// Show alignment
pcl::visualization::PCLVisualizer visu("Alignment");
visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
visu.spin ();
}
else
{
pcl::console::print_error ("Alignment failed!\n");
return (1);
}
return (0);
}
// Align a rigid object to a scene with clutter and occlusions
int main (int argc, char **argv)
{
// Point clouds
PointCloudT::Ptr object (new PointCloudT);
PointCloudT::Ptr scene (new PointCloudT);
PointCloudT::Ptr object_aligned (new PointCloudT);
// Get input object and scene
if (argc < 3)
{
pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
return (1);
}
pcl::console::parse_argument (argc, argv, "--max_iterations", in_max_iterations);
pcl::console::parse_argument (argc, argv, "--num_samples", in_num_samples);
pcl::console::parse_argument (argc, argv, "--s_thresh", in_similarity_thresh);
pcl::console::parse_argument (argc, argv, "--max_cdist", in_max_corresp_dist);
pcl::console::parse_argument (argc, argv, "--inlier_frac", in_inlier_frac);
pcl::console::parse_argument (argc, argv, "--leaf", in_leaf);
pcl::console::parse_argument (argc, argv, "--normal_radius", normal_radius);
pcl::console::parse_argument (argc, argv, "--feature_radius", feature_radius);
pcl::console::parse_argument (argc, argv, "--icp", in_icp);
pcl::console::parse_argument (argc, argv, "--max_corr_icp", max_corr_icp);
pcl::console::parse_argument (argc, argv, "--icp_eps", max_eps_icp);
// Load object and scene
pcl::console::print_highlight ("Loading point clouds...\n");
pcl_tools::cloud_from_stl(argv[2], *object);
if (pcl::io::loadPCDFile<PointNT> (argv[1], *scene) < 0)
{
pcl::console::print_error ("Error loading object/scene file!\n");
return (1);
}
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*scene, *scene, indices);
pcl::removeNaNFromPointCloud(*object, *object, indices);
// /*pcl_tools::icp_result align = */alp_align(object, scene, object_aligned, 50000, 3, 0.9f, 5.5f * leaf, 0.7f);
// /*pcl_tools::icp_result align = */alp_align(object_aligned, scene, object_aligned, 50000, 3, 0.9f, 7.5f * leaf, 0.4f);
std::cout << "Inlier frac " << in_inlier_frac << std::endl;
pcl_tools::icp_result align = alp_align(object, scene, object_aligned, in_max_iterations, in_num_samples, in_similarity_thresh, in_max_corresp_dist, in_inlier_frac, in_leaf);
pcl::visualization::PCLVisualizer visu("Alignment");
if (align.converged)
{
pcl::console::print_info ("Inliers: %i/%i, scene: %i\n", align.inliers, object->size (), scene->size ());
// Show alignment
visu.addPointCloud (object, ColorHandlerT (object, 255.0, 0.0, 0.0), "object");
visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
// visu.addPointCloudNormals<PointNT>(object);
visu.spin ();
}
else
{
pcl::console::print_error ("Alignment failed!\n");
return (1);
}
if (in_icp) {
pcl::console::print_highlight ("Applying ICP now\n");
pcl::IterativeClosestPointNonLinear<PointNT, PointNT> icp;
// pcl::IterativeClosestPoint<PointNT, PointNT> icp;
pcl_tools::affine_cloud(Eigen::Vector3f::UnitZ(), 0.0, Eigen::Vector3f(0.0, 0.0, 0.02), *object_aligned, *object_aligned);
icp.setMaximumIterations (100);
icp.setMaxCorrespondenceDistance(max_corr_icp);
icp.setTransformationEpsilon (max_eps_icp);
icp.setInputSource (object_aligned);
icp.setInputTarget (scene);
icp.align (*object_aligned);
if (icp.hasConverged()) {
pcl::console::print_highlight ("ICP: Converged with fitness %f\n", icp.getFitnessScore());
}
// pcl::visualization::PCLVisualizer visu("Alignment");
// visu.addPointCloud (object, ColorHandlerT (object, 255.0, 0.0, 0.0), "object");
// visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
visu.updatePointCloud (object_aligned, ColorHandlerT (object_aligned, 100.0, 50.0, 200.0), "object_aligned");
// visu.addPointCloudNormals<PointNT>(object);
visu.spin ();
}
return (0);
}
void FeatureRegistration::estimate(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
const sensor_msgs::PointCloud2::ConstPtr& feature_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!reference_cloud_ || !reference_feature_) {
JSK_NODELET_ERROR("Not yet reference data is available");
return;
}
pcl::PointCloud<pcl::PointNormal>::Ptr
cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::PointNormal>::Ptr
object_aligned (new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr
feature (new pcl::PointCloud<pcl::FPFHSignature33>);
pcl::fromROSMsg(*cloud_msg, *cloud);
pcl::fromROSMsg(*feature_msg, *feature);
pcl::SampleConsensusPrerejective<pcl::PointNormal,
pcl::PointNormal,
pcl::FPFHSignature33> align;
align.setInputSource(reference_cloud_);
align.setSourceFeatures(reference_feature_);
align.setInputTarget(cloud);
align.setTargetFeatures(feature);
align.setMaximumIterations(max_iterations_); // Number of RANSAC iterations
align.setNumberOfSamples(3); // Number of points to sample for generating/prerejecting a pose
align.setCorrespondenceRandomness(correspondence_randomness_); // Number of nearest features to use
align.setSimilarityThreshold(similarity_threshold_); // Polygonal edge length similarity threshold
align.setMaxCorrespondenceDistance(max_correspondence_distance_); // Inlier threshold
align.setInlierFraction(inlier_fraction_); // Required inlier fraction for accepting a pose hypothesis
align.align (*object_aligned);
if (align.hasConverged ())
{
// Print results
printf ("\n");
Eigen::Affine3f transformation(align.getFinalTransformation());
geometry_msgs::PoseStamped ros_pose;
tf::poseEigenToMsg(transformation, ros_pose.pose);
ros_pose.header = cloud_msg->header;
pub_pose_.publish(ros_pose);
pcl::PointCloud<pcl::PointNormal>::Ptr
result_cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::transformPointCloud(
*reference_cloud_, *result_cloud, transformation);
sensor_msgs::PointCloud2 ros_cloud;
pcl::toROSMsg(*object_aligned, ros_cloud);
ros_cloud.header = cloud_msg->header;
pub_cloud_.publish(ros_cloud);
//pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
}
else {
JSK_NODELET_WARN("failed to align pointcloud");
}
}
// Align a rigid object to a scene with clutter and occlusions
int
main (int argc, char **argv)
{
// Point clouds
PointCloudT::Ptr object (new PointCloudT);
PointCloudT::Ptr object_aligned (new PointCloudT);
PointCloudT::Ptr scene (new PointCloudT);
FeatureCloudT::Ptr object_features (new FeatureCloudT);
FeatureCloudT::Ptr scene_features (new FeatureCloudT);
// parameter reader
readParameters param_reader;
param_reader.setConfigureFile("param.cfg");
// Get input object and scene
if (argc != 3)
{
pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
return (1);
}
// Load object and scene
pcl::console::print_highlight ("Loading point clouds...\n");
if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 ||
pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0)
{
pcl::console::print_error ("Error loading object/scene file!\n");
return (1);
}
//------------------------------------------------------------
// pre translation the object model
bool pre_transform_p = false;
param_reader.get<bool>("pre_transform_model_p", pre_transform_p);
if(pre_transform_p)
{
Eigen::Vector4f scene_center(0.0f, 0.0f, 0.0f, 0.0f);
pcl::compute3DCentroid(*scene, scene_center);
// generate the rotation matrix:
// define the rotation angle and rotation axis
float rotation_angle = (float)20.0/180.0*M_PI;
Eigen::Vector3f rotation_axis(0.2f, 1.0f, 1.0f);
Eigen::AngleAxisf rotation_mg (rotation_angle, rotation_axis);
// generate the whole tranform:
// translate the model to the origin first, and then do the rotation.
Eigen::Vector3f tmpVec3f(scene_center(0),
scene_center(1),
scene_center(2));
// tmpVec3f = tmpVec3f*0.09;
// translate first, then rotation
Eigen::Affine3f transform_mg (
rotation_mg*Eigen::Translation3f((-1) * tmpVec3f));
// transform the model cloud
pcl::transformPointCloud(*object, *object, transform_mg);
}
//------------------------------------------------------------
// Downsample
pcl::console::print_highlight ("Downsampling...\n");
pcl::VoxelGrid<PointNT> grid;
float leaf = 0.003f;
param_reader.get<float>("leaf_size", leaf);
grid.setLeafSize (leaf, leaf, leaf);
grid.setInputCloud (object);
grid.filter (*object);
grid.setInputCloud (scene);
grid.filter (*scene);
// Estimate normals for scene
pcl::console::print_highlight ("Estimating scene normals...\n");
pcl::NormalEstimationOMP<PointNT,PointNT> nest;
float normal_search_radius = 0.015;
param_reader.get<float>("normal_search_radius", normal_search_radius);
nest.setRadiusSearch (normal_search_radius);
nest.setInputCloud (scene);
nest.compute (*scene);
// Estimate features
pcl::console::print_highlight ("Estimating features...\n");
FeatureEstimationT fest;
float feature_search_radius=0.01;
param_reader.get<float>("feature_search_radius", feature_search_radius);
fest.setRadiusSearch (feature_search_radius);
fest.setInputCloud (object);
fest.setInputNormals (object);
fest.compute (*object_features);
fest.setInputCloud (scene);
fest.setInputNormals (scene);
fest.compute (*scene_features);
// Perform alignment
pcl::console::print_highlight ("Starting alignment...\n");
// Initialize Sample Consensus Initial Alignment (SAC-IA)
pcl::SampleConsensusInitialAlignment<PointNT, PointNT, FeatureT> reg;
reg.setMinSampleDistance (0.01f);
reg.setMaxCorrespondenceDistance (0.01);
reg.setMaximumIterations (1000);
reg.setInputSource (object);
reg.setInputTarget (scene);
reg.setSourceFeatures (object_features);
reg.setTargetFeatures (scene_features);
{
pcl::ScopeTime t("Alignment");
reg.align (*object_aligned);
}
if (reg.hasConverged ())
{
// Print results
printf ("\n");
Eigen::Matrix4f transformation = reg.getFinalTransformation ();
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
pcl::console::print_info ("\n");
pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
pcl::console::print_info ("\n");
// pcl::console::print_info ("Inliers: %i/%i\n", reg.getInliers ().size (), object->size ());
// Show alignment
pcl::visualization::PCLVisualizer visu("Alignment");
visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
visu.spin ();
}
else
{
pcl::console::print_error ("Alignment failed!\n");
return (1);
}
return (0);
}
