Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Support for correspondence estimation with different point types #1476

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open

Support for correspondence estimation with different point types #1476

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
8a6066a
Fix for issue #329
jpapon Oct 16, 2013
1bc5285
Added support for correspondence estimation with different point types
SergioRAgostinho Dec 14, 2017
610a9f2
Moved search helper to search and kdtree.
SergioRAgostinho Dec 19, 2017
b73d39e
Exploit return type SFINAE
SergioRAgostinho Dec 19, 2017
61d613c
Remove forward declarations of non existing classes.
SergioRAgostinho Dec 19, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
51 changes: 45 additions & 6 deletions kdtree/include/pcl/kdtree/kdtree.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -163,23 +163,42 @@ namespace pcl
}

/** \brief Search for k-nearest neighbors for the given query point.
* This method accepts a different template parameter for the point type.
* This method accepts a different template parameter for the point type
* and it is invoked if PointT is of a different type than PointTDiff.
* \param[in] point the given query point
* \param[in] k the number of neighbors to search for
* \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
* \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
* a priori!)
* \return number of neighbors found
* \return number of neighbors found (int)
*/
template <typename PointTDiff> inline int
nearestKSearchT (const PointTDiff &point, int k,
template <typename PointTDiff> inline typename boost::disable_if<boost::is_same<PointT, PointTDiff>, int>::type
nearestKSearchT (const PointTDiff &point, int k,
std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
{
PointT p;
copyPoint (point, p);
return (nearestKSearch (p, k, k_indices, k_sqr_distances));
}

/** \brief Search for k-nearest neighbors for the given query point.
* This method accepts a different template parameter for the point type,
* it is invoked if PointT is equal to PointTDiff and it is an alias
* to \a nearestKSearch.
* \param[in] point the given query point
* \param[in] k the number of neighbors to search for
* \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
* \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
* a priori!)
* \return number of neighbors found (int)
*/
template <typename PointTDiff> inline typename boost::enable_if<boost::is_same<PointT, PointTDiff>, int>::type
nearestKSearchT (const PointTDiff &point, int k,
std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
{
return (nearestKSearch (point, k, k_indices, k_sqr_distances));
}

/** \brief Search for k-nearest neighbors for the given query point (zero-copy).
*
* \attention This method does not do any bounds checking for the input index
Expand Down Expand Up @@ -254,16 +273,17 @@ namespace pcl
}

/** \brief Search for all the nearest neighbors of the query point in a given radius.
* This method is invoked if PointT is not equal to PointTDiff.
* \param[in] point the given query point
* \param[in] radius the radius of the sphere bounding all of p_q's neighbors
* \param[out] k_indices the resultant indices of the neighboring points
* \param[out] k_sqr_distances the resultant squared distances to the neighboring points
* \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
* 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
* returned.
* \return number of neighbors found in radius
* \return number of neighbors found in radius (int)
*/
template <typename PointTDiff> inline int
template <typename PointTDiff> inline typename boost::disable_if<boost::is_same<PointT, PointTDiff>, int>::type
radiusSearchT (const PointTDiff &point, double radius, std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
{
Expand All @@ -272,6 +292,25 @@ namespace pcl
return (radiusSearch (p, radius, k_indices, k_sqr_distances, max_nn));
}

/** \brief Search for all the nearest neighbors of the query point in a given radius.
* This method is invoked if PointT is equal to PointTDiff and it is an alias
* to \a radiusSearch
* \param[in] point the given query point
* \param[in] radius the radius of the sphere bounding all of p_q's neighbors
* \param[out] k_indices the resultant indices of the neighboring points
* \param[out] k_sqr_distances the resultant squared distances to the neighboring points
* \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
* 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
* returned.
* \return number of neighbors found in radius (int)
*/
template <typename PointTDiff> inline typename boost::enable_if<boost::is_same<PointT, PointTDiff>, int>::type
radiusSearchT (const PointTDiff &point, double radius, std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
{
return (radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn));
}

/** \brief Search for all the nearest neighbors of the query point in a given radius (zero-copy).
*
* \attention This method does not do any bounds checking for the input index
Expand Down
86 changes: 64 additions & 22 deletions registration/include/pcl/registration/correspondence_estimation.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,21 +62,23 @@ namespace pcl
template <typename PointSource, typename PointTarget, typename Scalar = float>
class CorrespondenceEstimationBase: public PCLBase<PointSource>
{
public:
typedef boost::shared_ptr<CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> > Ptr;
typedef boost::shared_ptr<const CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> > ConstPtr;
protected:

// using PCLBase<PointSource>::initCompute;
using PCLBase<PointSource>::deinitCompute;
using PCLBase<PointSource>::input_;
using PCLBase<PointSource>::indices_;

public:
typedef boost::shared_ptr<CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> > Ptr;
typedef boost::shared_ptr<const CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> > ConstPtr;

using PCLBase<PointSource>::setIndices;

typedef pcl::search::KdTree<PointTarget> KdTree;
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 +89,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 +99,7 @@ namespace pcl
, target_ ()
, target_indices_ ()
, point_representation_ ()
, point_representation_source_ ()
, input_transformed_ ()
, input_fields_ ()
, target_cloud_updated_ (true)
Expand All @@ -108,6 +112,19 @@ namespace pcl
/** \brief Empty destructor */
virtual ~CorrespondenceEstimationBase () {}

/** \brief An alias to setInputSource
* \param[in] cloud the input point cloud source
*/
void
setInputCloud (const PointCloudSourceConstPtr &cloud)
{
setInputSource (cloud);
}

/** \brief An alias to getInputSource. */
PointCloudSourceConstPtr const
getInputCloud () const { return getInputSource (); }

/** \brief Provide a pointer to the input source
* (e.g., the point cloud that we want to align to the target)
*
Expand Down Expand Up @@ -279,6 +296,18 @@ 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 @@ -303,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 All @@ -316,7 +348,7 @@ namespace pcl
/** \brief Internal computation initalization. */
bool
initCompute ();

/** \brief Internal computation initalization for reciprocal correspondences. */
bool
initComputeReciprocal ();
Expand All @@ -325,10 +357,12 @@ namespace pcl
* This way, we avoid rebuilding the kd-tree for the target cloud every time the determineCorrespondences () method
* is called. */
bool target_cloud_updated_;

/** \brief Variable that stores whether we have a new source cloud, meaning we need to pre-process it again.
* This way, we avoid rebuilding the reciprocal kd-tree for the source cloud every time the determineCorrespondences () method
* is called. */
bool source_cloud_updated_;

/** \brief A flag which, if set, means the tree operating on the target cloud
* will never be recomputed*/
bool force_no_recompute_;
Expand Down Expand Up @@ -363,9 +397,12 @@ namespace pcl
template <typename PointSource, typename PointTarget, typename Scalar = float>
class CorrespondenceEstimation : public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>
{
public:
typedef boost::shared_ptr<CorrespondenceEstimation<PointSource, PointTarget, Scalar> > Ptr;
typedef boost::shared_ptr<const CorrespondenceEstimation<PointSource, PointTarget, Scalar> > ConstPtr;
protected:

using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeReciprocal;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getClassName;
using PCLBase<PointSource>::deinitCompute;

using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::point_representation_;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_transformed_;
Expand All @@ -374,26 +411,31 @@ namespace pcl
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::corr_name_;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_indices_;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getClassName;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeReciprocal;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::indices_;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_fields_;
using PCLBase<PointSource>::deinitCompute;

typedef pcl::search::KdTree<PointTarget> KdTree;
typedef typename pcl::search::KdTree<PointTarget>::Ptr KdTreePtr;
public:

typedef pcl::PointCloud<PointSource> PointCloudSource;
typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
typedef boost::shared_ptr<CorrespondenceEstimation<PointSource, PointTarget, Scalar> > Ptr;
typedef boost::shared_ptr<const CorrespondenceEstimation<PointSource, PointTarget, Scalar> > ConstPtr;

typedef pcl::PointCloud<PointTarget> PointCloudTarget;
typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;
using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::KdTree;
using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::KdTreePtr;

typedef typename KdTree::PointRepresentationConstPtr PointRepresentationConstPtr;
using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::KdTreeReciprocal;
using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::KdTreeReciprocalPtr;

using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::PointCloudSource;
using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::PointCloudSourcePtr;
using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::PointCloudSourceConstPtr;

using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::PointCloudTarget;
using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::PointCloudTargetPtr;
using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::PointCloudTargetConstPtr;

using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::PointRepresentationConstPtr;
using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::ReciprocalPointRepresentationConstPtr;

/** \brief Empty constructor. */
CorrespondenceEstimation ()
Expand Down
Loading