Skip to content

Commit

Permalink
Merge pull request #391 from yangyanli/fix_ppf_rotation_axis
Browse files Browse the repository at this point in the history 
Fix degenerated rotation axis bug when normal is parallel to x axis.
  • Loading branch information
@rbrusu
rbrusu committed Jan 2, 2014
2 parents 0b604cc + 8ed661a commit 0010c34
Show file tree
Hide file tree
Showing 3 changed files with 19 additions and 40 deletions.
10 changes: 6 additions & 4 deletions features/include/pcl/features/impl/ppf.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,9 +85,11 @@ pcl::PPFEstimation<PointInT, PointNT, PointOutT>::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));
Expand All @@ -97,7 +99,7 @@ pcl::PPFEstimation<PointInT, PointNT, PointOutT>::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<float>::quiet_NaN ();
output.is_dense = false;
}
Expand Down
40 changes: 13 additions & 27 deletions registration/include/pcl/registration/impl/ppf_registration.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@
#ifndef PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
#define PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_

#include <pcl/registration/ppf_registration.h>
#include <pcl/features/ppf.h>
#include <pcl/common/transforms.h>

Expand Down Expand Up @@ -104,17 +105,6 @@ pcl::PPFHashMapSearch::nearestNeighborSearch (float &f1, float &f2, float &f3, f
}


//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget> void
pcl::PPFRegistration<PointSource, PointTarget>::setInputTarget (const PointCloudTargetConstPtr &cloud)
{
Registration<PointSource, PointTarget>::setInputTarget (cloud);

scene_search_tree_ = typename pcl::KdTreeFLANN<PointTarget>::Ptr (new pcl::KdTreeFLANN<PointTarget>);
scene_search_tree_->setInputCloud (target_);
}


//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget> void
pcl::PPFRegistration<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
Expand Down Expand Up @@ -149,14 +139,16 @@ pcl::PPFRegistration<PointSource, PointTarget>::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<int> indices;
std::vector<float> distances;
scene_search_tree_->radiusSearch (target_->points[scene_reference_index],
tree_->radiusSearch (target_->points[scene_reference_index],
search_method_->getModelDiameter () /2,
indices,
distances);
Expand All @@ -178,18 +170,9 @@ pcl::PPFRegistration<PointSource, PointTarget>::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);
Expand All @@ -205,7 +188,7 @@ pcl::PPFRegistration<PointSource, PointTarget>::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);
}
}

Expand All @@ -227,8 +210,11 @@ pcl::PPFRegistration<PointSource, PointTarget>::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<float> (max_votes_j) - floorf (static_cast<float> (M_PI) / search_method_->getAngleDiscretizationStep ())) * search_method_->getAngleDiscretizationStep (), Eigen::Vector3f::UnitX ()) *
Expand Down
9 changes: 0 additions & 9 deletions registration/include/pcl/registration/ppf_registration.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand All @@ -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<PointTarget>::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
Expand Down

0 comments on commit 0010c34

Please sign in to comment.