Skip to content

Commit

Permalink
Support for correspondence estimation with different point types
Browse files Browse the repository at this point in the history 
Fixes #329. Credits for @aichim, @jpapon and @taketwo .
Added unit test for XYZ point types.
  • Loading branch information
@SergioRAgostinho
SergioRAgostinho committed Dec 13, 2015
1 parent 8567baf commit 372a79e
Show file tree
Hide file tree
Showing 3 changed files with 102 additions and 86 deletions.
20 changes: 19 additions & 1 deletion registration/include/pcl/registration/correspondence_estimation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ namespace pcl
typedef typename KdTree::Ptr KdTreePtr;

typedef pcl::search::KdTree<PointSource> KdTreeReciprocal;
typedef typename KdTree::Ptr KdTreeReciprocalPtr;
typedef typename KdTreeReciprocal::Ptr KdTreeReciprocalPtr;

typedef pcl::PointCloud<PointSource> PointCloudSource;
typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
Expand All @@ -87,6 +87,7 @@ namespace pcl
typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;

typedef typename KdTree::PointRepresentationConstPtr PointRepresentationConstPtr;
typedef typename KdTreeReciprocal::PointRepresentationConstPtr ReciprocalPointRepresentationConstPtr;

/** \brief Empty constructor. */
CorrespondenceEstimationBase ()
Expand All @@ -96,6 +97,7 @@ namespace pcl
, target_ ()
, target_indices_ ()
, point_representation_ ()
, point_representation_source_ ()
, input_transformed_ ()
, input_fields_ ()
, target_cloud_updated_ (true)
Expand Down Expand Up @@ -293,6 +295,19 @@ namespace pcl
point_representation_ = point_representation;
}

/** \brief Provide a boost shared pointer to the PointRepresentation to be used
* when searching for nearest neighbors in the source cloud (only needed for reciprocal).
*
* \param[in] point_representation the PointRepresentation to be used by the
* source k-D tree for nearest neighbor search
*/
inline void
setSourcePointRepresentation (const ReciprocalPointRepresentationConstPtr &point_representation)
{
point_representation_source_ = point_representation;
}


/** \brief Clone and cast to CorrespondenceEstimationBase */
virtual boost::shared_ptr< CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> > clone () const = 0;

Expand All @@ -317,6 +332,9 @@ namespace pcl
/** \brief The point representation used (internal). */
PointRepresentationConstPtr point_representation_;

/** \brief The point representation used for reciprocal search (internal). */
ReciprocalPointRepresentationConstPtr point_representation_source_;

/** \brief The transformed input source point cloud dataset. */
PointCloudTargetPtr input_transformed_;

Expand Down
111 changes: 26 additions & 85 deletions registration/include/pcl/registration/impl/correspondence_estimation.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,8 +108,9 @@ pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar
// Only update source kd-tree if a new target cloud was set
if (source_cloud_updated_ && !force_no_recompute_reciprocal_)
{
if (point_representation_)
tree_reciprocal_->setPointRepresentation (point_representation_);
if (point_representation_source_)
tree_reciprocal_->setPointRepresentation (point_representation_source_);

// If the target indices have been given via setIndicesTarget
if (indices_)
tree_reciprocal_->setInputCloud (getInputSource(), getIndicesSource());
Expand Down Expand Up @@ -139,42 +140,17 @@ pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>::d
pcl::Correspondence corr;
unsigned int nr_valid_correspondences = 0;

// Check if the template types are the same. If true, avoid a copy.
// Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
if (isSamePointType<PointSource, PointTarget> ())
// Iterate over the input set of source indices
for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
{
// Iterate over the input set of source indices
for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
{
tree_->nearestKSearch (input_->points[*idx], 1, index, distance);
if (distance[0] > max_dist_sqr)
continue;

corr.index_query = *idx;
corr.index_match = index[0];
corr.distance = distance[0];
correspondences[nr_valid_correspondences++] = corr;
}
}
else
{
PointTarget pt;

// Iterate over the input set of source indices
for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
{
// Copy the source data to a target PointTarget format so we can search in the tree
copyPoint (input_->points[*idx], pt);

tree_->nearestKSearch (pt, 1, index, distance);
if (distance[0] > max_dist_sqr)
continue;

corr.index_query = *idx;
corr.index_match = index[0];
corr.distance = distance[0];
correspondences[nr_valid_correspondences++] = corr;
}
tree_->nearestKSearchT (input_->points[*idx], 1, index, distance);
if (distance[0] > max_dist_sqr)
continue;

corr.index_query = *idx;
corr.index_match = index[0];
corr.distance = distance[0];
correspondences[nr_valid_correspondences++] = corr;
}
correspondences.resize (nr_valid_correspondences);
deinitCompute ();
Expand Down Expand Up @@ -203,58 +179,23 @@ pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>::d
unsigned int nr_valid_correspondences = 0;
int target_idx = 0;

// Check if the template types are the same. If true, avoid a copy.
// Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
if (isSamePointType<PointSource, PointTarget> ())
// Iterate over the input set of source indices
for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
{
// Iterate over the input set of source indices
for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
{
tree_->nearestKSearch (input_->points[*idx], 1, index, distance);
if (distance[0] > max_dist_sqr)
continue;

target_idx = index[0];

tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
continue;

corr.index_query = *idx;
corr.index_match = index[0];
corr.distance = distance[0];
correspondences[nr_valid_correspondences++] = corr;
}
}
else
{
PointTarget pt_src;
PointSource pt_tgt;

// Iterate over the input set of source indices
for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
{
// Copy the source data to a target PointTarget format so we can search in the tree
copyPoint (input_->points[*idx], pt_src);

tree_->nearestKSearch (pt_src, 1, index, distance);
if (distance[0] > max_dist_sqr)
continue;

target_idx = index[0];
tree_->nearestKSearchT (input_->points[*idx], 1, index, distance);
if (distance[0] > max_dist_sqr)
continue;

// Copy the target data to a target PointSource format so we can search in the tree_reciprocal
copyPoint (target_->points[target_idx], pt_tgt);
target_idx = index[0];

tree_reciprocal_->nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal);
if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
continue;
tree_reciprocal_->nearestKSearchT (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
continue;

corr.index_query = *idx;
corr.index_match = index[0];
corr.distance = distance[0];
correspondences[nr_valid_correspondences++] = corr;
}
corr.index_query = *idx;
corr.index_match = index[0];
corr.distance = distance[0];
correspondences[nr_valid_correspondences++] = corr;
}
correspondences.resize (nr_valid_correspondences);
deinitCompute ();
Expand Down
57 changes: 57 additions & 0 deletions test/registration/test_correspondence_estimation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,63 @@ TEST (CorrespondenceEstimation, CorrespondenceEstimationSetSearchMethod)

}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename T> class CorrespondenceEstimationTest : public ::testing::Test { };
typedef ::testing::Types<BOOST_PP_SEQ_ENUM (PCL_XYZ_POINT_TYPES)> XYZPointTypes;
TYPED_TEST_CASE (CorrespondenceEstimationTest, XYZPointTypes);
TYPED_TEST (CorrespondenceEstimationTest, DifferentPointTypes)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ> ());
typename pcl::PointCloud<TypeParam>::Ptr cloud2 (new pcl::PointCloud<TypeParam> ());
pcl::Correspondences corr, rec_corr;

// Populate cloud1
cloud1->points.resize (4);
cloud1->points[0].getVector3fMap () = Eigen::Vector3f (1.f, 0.f, 0.f);
cloud1->points[1].getVector3fMap () = Eigen::Vector3f (2.f, 0.f, 0.f);
cloud1->points[2].getVector3fMap () = Eigen::Vector3f (0.f, 0.f, 12.f);
cloud1->points[3].getVector3fMap () = Eigen::Vector3f (9.f, 0.f, 0.f);
cloud1->is_dense = true;
cloud1->width = 4;
cloud1->height = 1;

// Populate cloud2
cloud2->points.resize (3);
cloud2->points[0].getVector3fMap () = Eigen::Vector3f (0.f, 0.f, 0.f);
cloud2->points[1].getVector3fMap () = Eigen::Vector3f (10.f, 0.f, 0.f);
cloud2->points[2].getVector3fMap () = Eigen::Vector3f (0.f, 0.f, 10.f);
cloud2->is_dense = true;
cloud2->width = 3;
cloud2->height = 1;

// Compute correspondences
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, TypeParam, double> ce;
ce.setInputSource (cloud1);
ce.setInputTarget (cloud2);
ce.determineCorrespondences (corr);
ce.determineReciprocalCorrespondences (rec_corr);

// Correspondences
ASSERT_EQ (corr.size (), 4);
ASSERT_EQ (corr[0].index_query, 0);
ASSERT_EQ (corr[0].index_match, 0);
ASSERT_EQ (corr[1].index_query, 1);
ASSERT_EQ (corr[1].index_match, 0);
ASSERT_EQ (corr[2].index_query, 2);
ASSERT_EQ (corr[2].index_match, 2);
ASSERT_EQ (corr[3].index_query, 3);
ASSERT_EQ (corr[3].index_match, 1);

// Reciprocal Correspondences
ASSERT_EQ (rec_corr.size (), 3);
ASSERT_EQ (rec_corr[0].index_query, 0);
ASSERT_EQ (rec_corr[0].index_match, 0);
ASSERT_EQ (rec_corr[1].index_query, 2);
ASSERT_EQ (rec_corr[1].index_match, 2);
ASSERT_EQ (rec_corr[2].index_query, 3);
ASSERT_EQ (rec_corr[2].index_match, 1);
}

/* ---[ */
int
main (int argc, char** argv)
Expand Down

0 comments on commit 372a79e

Please sign in to comment.