From b15ead92dfa82a96d54b881e10eaa935e2d1c859 Mon Sep 17 00:00:00 2001 From: Yangyan Li Date: Mon, 2 Dec 2013 14:33:11 -0800 Subject: [PATCH 1/2] Fix degenerated rotation axis bug when normal is parallel to x axis. --- features/include/pcl/features/impl/ppf.hpp | 10 ++++--- .../registration/impl/ppf_registration.hpp | 28 ++++++++----------- 2 files changed, 18 insertions(+), 20 deletions(-) diff --git a/features/include/pcl/features/impl/ppf.hpp b/features/include/pcl/features/impl/ppf.hpp index 31eb7455bb5..e1a58720c20 100644 --- a/features/include/pcl/features/impl/ppf.hpp +++ b/features/include/pcl/features/impl/ppf.hpp @@ -85,9 +85,11 @@ pcl::PPFEstimation::computeFeature (PointCloudOut Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (), model_reference_normal = normals_->points[i].getNormalVector3fMap (), model_point = input_->points[j].getVector3fMap (); - Eigen::AngleAxisf rotation_mg (acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())), - model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ()); - Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg; + float rotation_angle = acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())); + bool parallel_to_x = (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f); + Eigen::Vector3f rotation_axis = (parallel_to_x)?(Eigen::Vector3f::UnitY ()):(model_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized()); + Eigen::AngleAxisf rotation_mg (rotation_angle, rotation_axis); + Eigen::Affine3f transform_mg (Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg); Eigen::Vector3f model_point_transformed = transform_mg * model_point; float angle = atan2f ( -model_point_transformed(2), model_point_transformed(1)); @@ -97,7 +99,7 @@ pcl::PPFEstimation::computeFeature (PointCloudOut } else { - PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %zu and %zu went wrong.\n", getClassName ().c_str (), i, j); + PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %u and %u went wrong.\n", getClassName ().c_str (), i, j); p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = std::numeric_limits::quiet_NaN (); output.is_dense = false; } diff --git a/registration/include/pcl/registration/impl/ppf_registration.hpp b/registration/include/pcl/registration/impl/ppf_registration.hpp index 05fd86af058..fc752a48e1f 100644 --- a/registration/include/pcl/registration/impl/ppf_registration.hpp +++ b/registration/include/pcl/registration/impl/ppf_registration.hpp @@ -43,6 +43,7 @@ #ifndef PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_ #define PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_ +#include #include #include @@ -114,7 +115,6 @@ pcl::PPFRegistration::setInputTarget (const PointCloud scene_search_tree_->setInputCloud (target_); } - ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::PPFRegistration::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) @@ -149,9 +149,11 @@ pcl::PPFRegistration::computeTransformation (PointClou Eigen::Vector3f scene_reference_point = target_->points[scene_reference_index].getVector3fMap (), scene_reference_normal = target_->points[scene_reference_index].getNormalVector3fMap (); - Eigen::AngleAxisf rotation_sg (acosf (scene_reference_normal.dot (Eigen::Vector3f::UnitX ())), - scene_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized()); - Eigen::Affine3f transform_sg (Eigen::Translation3f (rotation_sg * ((-1) * scene_reference_point)) * rotation_sg); + float rotation_angle_sg = acosf (scene_reference_normal.dot (Eigen::Vector3f::UnitX ())); + bool parallel_to_x_sg = (scene_reference_normal.y() == 0.0f && scene_reference_normal.z() == 0.0f); + Eigen::Vector3f rotation_axis_sg = (parallel_to_x_sg)?(Eigen::Vector3f::UnitY ()):(scene_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized()); + Eigen::AngleAxisf rotation_sg (rotation_angle_sg, rotation_axis_sg); + Eigen::Affine3f transform_sg (Eigen::Translation3f ( rotation_sg * ((-1) * scene_reference_point)) * rotation_sg); // For every other point in the scene => now have pair (s_r, s_i) fixed std::vector indices; @@ -178,18 +180,9 @@ pcl::PPFRegistration::computeTransformation (PointClou // Compute alpha_s angle Eigen::Vector3f scene_point = target_->points[scene_point_index].getVector3fMap (); - Eigen::AngleAxisf rotation_sg (acosf (scene_reference_normal.dot (Eigen::Vector3f::UnitX ())), - scene_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ()); - Eigen::Affine3f transform_sg = Eigen::Translation3f ( rotation_sg * ((-1) * scene_reference_point)) * rotation_sg; -// float alpha_s = acos (Eigen::Vector3f::UnitY ().dot ((transform_sg * scene_point).normalized ())); Eigen::Vector3f scene_point_transformed = transform_sg * scene_point; float alpha_s = atan2f ( -scene_point_transformed(2), scene_point_transformed(1)); - if ( alpha_s != alpha_s) - { - PCL_ERROR ("alpha_s is nan\n"); - continue; - } if (sin (alpha_s) * scene_point_transformed(2) < 0.0f) alpha_s *= (-1); alpha_s *= (-1); @@ -205,7 +198,7 @@ pcl::PPFRegistration::computeTransformation (PointClou accumulator_array[model_reference_index][alpha_discretized] ++; } } - else PCL_ERROR ("[pcl::PPFRegistration::computeTransformation] Computing pair feature vector between points %zu and %zu went wrong.\n", scene_reference_index, scene_point_index); + else PCL_ERROR ("[pcl::PPFRegistration::computeTransformation] Computing pair feature vector between points %u and %u went wrong.\n", scene_reference_index, scene_point_index); } } @@ -227,8 +220,11 @@ pcl::PPFRegistration::computeTransformation (PointClou Eigen::Vector3f model_reference_point = input_->points[max_votes_i].getVector3fMap (), model_reference_normal = input_->points[max_votes_i].getNormalVector3fMap (); - Eigen::AngleAxisf rotation_mg (acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())), model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ()); - Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg; + float rotation_angle_mg = acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())); + bool parallel_to_x_mg = (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f); + Eigen::Vector3f rotation_axis_mg = (parallel_to_x_mg)?(Eigen::Vector3f::UnitY ()):(model_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized()); + Eigen::AngleAxisf rotation_mg (rotation_angle_mg, rotation_axis_mg); + Eigen::Affine3f transform_mg (Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg); Eigen::Affine3f max_transform = transform_sg.inverse () * Eigen::AngleAxisf ((static_cast (max_votes_j) - floorf (static_cast (M_PI) / search_method_->getAngleDiscretizationStep ())) * search_method_->getAngleDiscretizationStep (), Eigen::Vector3f::UnitX ()) * From 8ed661a50af92116f289d1ec36bb4d543ac45d80 Mon Sep 17 00:00:00 2001 From: Yangyan Li Date: Tue, 3 Dec 2013 10:47:00 -0800 Subject: [PATCH 2/2] Remove the redundant scene_search_tree_. --- .../pcl/registration/impl/ppf_registration.hpp | 12 +----------- .../include/pcl/registration/ppf_registration.h | 9 --------- 2 files changed, 1 insertion(+), 20 deletions(-) diff --git a/registration/include/pcl/registration/impl/ppf_registration.hpp b/registration/include/pcl/registration/impl/ppf_registration.hpp index fc752a48e1f..f2225cf2d7d 100644 --- a/registration/include/pcl/registration/impl/ppf_registration.hpp +++ b/registration/include/pcl/registration/impl/ppf_registration.hpp @@ -105,16 +105,6 @@ pcl::PPFHashMapSearch::nearestNeighborSearch (float &f1, float &f2, float &f3, f } -////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::PPFRegistration::setInputTarget (const PointCloudTargetConstPtr &cloud) -{ - Registration::setInputTarget (cloud); - - scene_search_tree_ = typename pcl::KdTreeFLANN::Ptr (new pcl::KdTreeFLANN); - scene_search_tree_->setInputCloud (target_); -} - ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::PPFRegistration::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) @@ -158,7 +148,7 @@ pcl::PPFRegistration::computeTransformation (PointClou // For every other point in the scene => now have pair (s_r, s_i) fixed std::vector indices; std::vector distances; - scene_search_tree_->radiusSearch (target_->points[scene_reference_index], + tree_->radiusSearch (target_->points[scene_reference_index], search_method_->getModelDiameter () /2, indices, distances); diff --git a/registration/include/pcl/registration/ppf_registration.h b/registration/include/pcl/registration/ppf_registration.h index 2560ec6fd63..789f0016b00 100644 --- a/registration/include/pcl/registration/ppf_registration.h +++ b/registration/include/pcl/registration/ppf_registration.h @@ -234,12 +234,6 @@ namespace pcl inline PPFHashMapSearch::Ptr getSearchMethod () { return search_method_; } - /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to) - * \param cloud the input point cloud target - */ - void - setInputTarget (const PointCloudTargetConstPtr &cloud); - private: /** \brief Method that calculates the transformation between the input_ and target_ point clouds, based on the PPF features */ @@ -257,9 +251,6 @@ namespace pcl * poses are considered to be in the same cluster (for the clustering phase of the algorithm) */ float clustering_position_diff_threshold_, clustering_rotation_diff_threshold_; - /** \brief use a kd-tree with range searches of range max_dist to skip an O(N) pass through the point cloud */ - typename pcl::KdTreeFLANN::Ptr scene_search_tree_; - /** \brief static method used for the std::sort function to order two PoseWithVotes * instances by their number of votes*/ static bool