Skip to content

Commit

Permalink
Improved cost function for selecting the best correspondence in the C…
Browse files Browse the repository at this point in the history 
…orrespondenceEstimationBackProjection.
  • Loading branch information
@carlosmccosta
carlosmccosta committed Aug 24, 2018
1 parent 58655dd commit 8b32567
Show file tree
Hide file tree
Showing 2 changed files with 83 additions and 21 deletions.
44 changes: 43 additions & 1 deletion registration/include/pcl/registration/correspondence_estimation_backprojection.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,8 @@
#ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_H_
#define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_H_

#include <cmath>
#include <pcl/common/angles.h>
#include <pcl/registration/correspondence_types.h>
#include <pcl/registration/correspondence_estimation.h>

Expand Down Expand Up @@ -94,6 +96,8 @@ namespace pcl
, source_normals_transformed_ ()
, target_normals_ ()
, k_ (10)
, normals_angle_filtering_threshold_ (80.0f)
, normals_angle_penalty_factor_ (4.0f)
{
corr_name_ = "CorrespondenceEstimationBackProjection";
}
Expand Down Expand Up @@ -186,7 +190,39 @@ namespace pcl
*/
inline unsigned int
getKSearch () const { return (k_); }


/** \brief Set the threshold for filtering the correspondences based on their relative angle.
* \param[in] threshold the maximum relative angle (in degrees) between the correspondent points normals
*/
inline void
setNormalsAngleFilteringThreshold (float threshold)
{
normals_angle_filtering_threshold_ = threshold;
}

/** \brief Get the threshold for filtering the correspondences based on their relative angle. */
inline float
getNormalsAngleFilteringThreshold () const
{
return (normals_angle_filtering_threshold_);
}

/** \brief Set the penalty for the relative angle between correspondence normals.
* \param[in] penalty the polynomial degree for penalizing the relative angle between normals
*/
inline void
setNormalsAnglePenaltyFactor (float penalty)
{
normals_angle_penalty_factor_ = penalty;
}

/** \brief Get the penalty for the relative angle between correspondence normals. */
inline float
getNormalsAnglePenaltyFactor () const
{
return (normals_angle_penalty_factor_);
}

/** \brief Clone and cast to CorrespondenceEstimationBase */
virtual boost::shared_ptr< CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> >
clone () const
Expand Down Expand Up @@ -219,6 +255,12 @@ namespace pcl

/** \brief The number of neighbours to be considered in the target point cloud */
unsigned int k_;

/** \brief The correspondence filtering threshold based on their normals relative angle */
float normals_angle_filtering_threshold_;

/** \brief The factor used to penalize the relative angle between correspondence normals */
float normals_angle_penalty_factor_;
};
}
}
Expand Down
60 changes: 40 additions & 20 deletions registration/include/pcl/registration/impl/correspondence_estimation_backprojection.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,12 +95,17 @@ pcl::registration::CorrespondenceEstimationBackProjection<PointSource, PointTarg
float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);

if (dist < min_dist)

if (cos_angle > std::cos (pcl::deg2rad (normals_angle_filtering_threshold_)))
{
min_dist = dist;
min_index = static_cast<int> (j);
float normals_angle_penalization = std::pow (-cos_angle + 2.0f, normals_angle_penalty_factor_);
float dist = nn_dists[j] * normals_angle_penalization;

if (dist < min_dist)
{
min_dist = dist;
min_index = static_cast<int> (j);
}
}
}
if (min_dist > max_distance)
Expand Down Expand Up @@ -137,12 +142,17 @@ pcl::registration::CorrespondenceEstimationBackProjection<PointSource, PointTarg
float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);

if (dist < min_dist)

if (cos_angle > std::cos (pcl::deg2rad (normals_angle_filtering_threshold_)))
{
min_dist = dist;
min_index = static_cast<int> (j);
float normals_angle_penalization = std::pow (-cos_angle + 2.0f, normals_angle_penalty_factor_);
float dist = nn_dists[j] * normals_angle_penalization;

if (dist < min_dist)
{
min_dist = dist;
min_index = static_cast<int> (j);
}
}
}
if (min_dist > max_distance)
Expand Down Expand Up @@ -206,12 +216,17 @@ pcl::registration::CorrespondenceEstimationBackProjection<PointSource, PointTarg
float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);

if (dist < min_dist)

if (cos_angle > std::cos (pcl::deg2rad (normals_angle_filtering_threshold_)))
{
min_dist = dist;
min_index = static_cast<int> (j);
float normals_angle_penalization = std::pow (-cos_angle + 2.0f, normals_angle_penalty_factor_);
float dist = nn_dists[j] * normals_angle_penalization;

if (dist < min_dist)
{
min_dist = dist;
min_index = static_cast<int> (j);
}
}
}
if (min_dist > max_distance)
Expand Down Expand Up @@ -255,12 +270,17 @@ pcl::registration::CorrespondenceEstimationBackProjection<PointSource, PointTarg
float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);

if (dist < min_dist)

if (cos_angle > std::cos (pcl::deg2rad (normals_angle_filtering_threshold_)))
{
min_dist = dist;
min_index = static_cast<int> (j);
float normals_angle_penalization = std::pow (-cos_angle + 2.0f, normals_angle_penalty_factor_);
float dist = nn_dists[j] * normals_angle_penalization;

if (dist < min_dist)
{
min_dist = dist;
min_index = static_cast<int> (j);
}
}
}
if (min_dist > max_distance)
Expand Down

0 comments on commit 8b32567

Please sign in to comment.