Skip to content

Commit

Permalink
Merge pull request #4901 from al-tu/no-more-typeid-its-constexpr
Browse files Browse the repository at this point in the history 
no more typeid, it's constexpr
  • Loading branch information
@mvieth
mvieth authored Jun 11, 2023
2 parents f0c7c46 + 7bc1e23 commit 956f89c
Show file tree
Hide file tree
Showing 7 changed files with 371 additions and 491 deletions.
50 changes: 44 additions & 6 deletions registration/include/pcl/registration/correspondence_estimation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,9 +72,11 @@ class CorrespondenceEstimationBase : public PCLBase<PointSource> {

using KdTree = pcl::search::KdTree<PointTarget>;
using KdTreePtr = typename KdTree::Ptr;
using KdTreeConstPtr = typename KdTree::ConstPtr;

using KdTreeReciprocal = pcl::search::KdTree<PointSource>;
using KdTreeReciprocalPtr = typename KdTree::Ptr;
using KdTreeReciprocalPtr = typename KdTreeReciprocal::Ptr;
using KdTreeReciprocalConstPtr = typename KdTreeReciprocal::ConstPtr;

using PointCloudSource = pcl::PointCloud<PointSource>;
using PointCloudSourcePtr = typename PointCloudSource::Ptr;
Expand All @@ -85,6 +87,8 @@ class CorrespondenceEstimationBase : public PCLBase<PointSource> {
using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;

using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr;
using PointRepresentationReciprocalConstPtr =
typename KdTreeReciprocal::PointRepresentationConstPtr;

/** \brief Empty constructor. */
CorrespondenceEstimationBase()
Expand Down Expand Up @@ -275,8 +279,8 @@ class CorrespondenceEstimationBase : public PCLBase<PointSource> {
pcl::Correspondences& correspondences,
double max_distance = std::numeric_limits<double>::max()) = 0;

/** \brief Provide a boost shared pointer to the PointRepresentation to be used
* when searching for nearest neighbors.
/** \brief Provide a boost shared pointer to the PointRepresentation for target cloud
* to be used when searching for nearest neighbors.
*
* \param[in] point_representation the PointRepresentation to be used by the
* k-D tree for nearest neighbor search
Expand All @@ -287,6 +291,19 @@ class CorrespondenceEstimationBase : public PCLBase<PointSource> {
point_representation_ = point_representation;
}

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

/** \brief Clone and cast to CorrespondenceEstimationBase */
virtual typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::Ptr
clone() const = 0;
Expand All @@ -307,9 +324,12 @@ class CorrespondenceEstimationBase : public PCLBase<PointSource> {
/** \brief The target point cloud dataset indices. */
IndicesPtr target_indices_;

/** \brief The point representation used (internal). */
/** \brief The target point representation used (internal). */
PointRepresentationConstPtr point_representation_;

/** \brief The source point representation used (internal). */
PointRepresentationReciprocalConstPtr point_representation_reciprocal_;

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

Expand Down Expand Up @@ -396,8 +416,24 @@ class CorrespondenceEstimation
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_fields_;
using PCLBase<PointSource>::deinitCompute;

using KdTree = pcl::search::KdTree<PointTarget>;
using KdTreePtr = typename KdTree::Ptr;
using KdTree =
typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::KdTree;
using KdTreePtr = typename CorrespondenceEstimationBase<PointSource,
PointTarget,
Scalar>::KdTreePtr;
using KdTreeConstPtr = typename CorrespondenceEstimationBase<PointSource,
PointTarget,
Scalar>::KdTreeConstPtr;
using KdTreeReciprocal =
typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
KdTreeReciprocal;
using KdTreeReciprocalPtr =
typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
KdTreeReciprocalPtr;

using KdTreeReciprocalConstPtr =
typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
KdTreeReciprocalConstPtr;

using PointCloudSource = pcl::PointCloud<PointSource>;
using PointCloudSourcePtr = typename PointCloudSource::Ptr;
Expand All @@ -408,6 +444,8 @@ class CorrespondenceEstimation
using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;

using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr;
using PointRepresentationReciprocalConstPtr =
typename KdTreeReciprocal::PointRepresentationConstPtr;

/** \brief Empty constructor. */
CorrespondenceEstimation() { corr_name_ = "CorrespondenceEstimation"; }
Expand Down
14 changes: 12 additions & 2 deletions registration/include/pcl/registration/correspondence_estimation_normal_shooting.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,8 +101,18 @@ class CorrespondenceEstimationNormalShooting
point_representation_;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_indices_;

using KdTree = pcl::search::KdTree<PointTarget>;
using KdTreePtr = typename KdTree::Ptr;
using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::KdTree;
using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
KdTreePtr;
using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
KdTreeConstPtr;

using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
KdTreeReciprocal;
using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
KdTreeReciprocalPtr;
using typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
KdTreeReciprocalConstPtr;

using PointCloudSource = pcl::PointCloud<PointSource>;
using PointCloudSourcePtr = typename PointCloudSource::Ptr;
Expand Down
162 changes: 71 additions & 91 deletions registration/include/pcl/registration/impl/correspondence_estimation.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,8 +98,8 @@ CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeRecip
{
// 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_reciprocal_)
tree_reciprocal_->setPointRepresentation(point_representation_reciprocal_);
// If the target indices have been given via setIndicesTarget
if (indices_)
tree_reciprocal_->setInputCloud(getInputSource(), getIndicesSource());
Expand All @@ -112,6 +112,35 @@ CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeRecip
return (true);
}

namespace detail {

template <
typename PointTarget,
typename PointSource,
typename Index,
typename std::enable_if_t<isSamePointType<PointSource, PointTarget>()>* = nullptr>
const PointSource&
pointCopyOrRef(typename pcl::PointCloud<PointSource>::ConstPtr& input, const Index& idx)
{
return (*input)[idx];
}

template <
typename PointTarget,
typename PointSource,
typename Index,
typename std::enable_if_t<!isSamePointType<PointSource, PointTarget>()>* = nullptr>
PointTarget
pointCopyOrRef(typename pcl::PointCloud<PointSource>::ConstPtr& input, const Index& idx)
{
// Copy the source data to a target PointTarget format so we can search in the tree
PointTarget pt;
copyPoint((*input)[idx], pt);
return pt;
}

} // namespace detail

template <typename PointSource, typename PointTarget, typename Scalar>
void
CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences(
Expand All @@ -120,50 +149,30 @@ CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorresponde
if (!initCompute())
return;

double max_dist_sqr = max_distance * max_distance;

correspondences.resize(indices_->size());

pcl::Indices index(1);
std::vector<float> distance(1);
pcl::Correspondence corr;
unsigned int nr_valid_correspondences = 0;
double max_dist_sqr = max_distance * max_distance;

// 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 (const auto& idx : (*indices_)) {
tree_->nearestKSearch((*input_)[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 (const auto& idx : (*indices_)) {
// Copy the source data to a target PointTarget format so we can search in the
// tree
copyPoint((*input_)[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;
}
// Iterate over the input set of source indices
for (const auto& idx : (*indices_)) {
// 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!
const auto& pt = detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx);
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;
}

correspondences.resize(nr_valid_correspondences);
deinitCompute();
}
Expand Down Expand Up @@ -192,59 +201,30 @@ CorrespondenceEstimation<PointSource, PointTarget, Scalar>::
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 (const auto& idx : (*indices_)) {
tree_->nearestKSearch((*input_)[idx], 1, index, distance);
if (distance[0] > max_dist_sqr)
continue;

target_idx = index[0];

tree_reciprocal_->nearestKSearch(
(*target_)[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 (const auto& idx : (*indices_)) {
// Copy the source data to a target PointTarget format so we can search in the
// tree
copyPoint((*input_)[idx], pt_src);

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

target_idx = index[0];

// Copy the target data to a target PointSource format so we can search in the
// tree_reciprocal
copyPoint((*target_)[target_idx], pt_tgt);

tree_reciprocal_->nearestKSearch(
pt_tgt, 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;
}
// Iterate over the input set of source indices
for (const auto& idx : (*indices_)) {
// 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!

const auto& pt_src = detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx);

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

target_idx = index[0];
const auto& pt_tgt =
detail::pointCopyOrRef<PointSource, PointTarget>(target_, target_idx);

tree_reciprocal_->nearestKSearch(pt_tgt, 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;
}
correspondences.resize(nr_valid_correspondences);
deinitCompute();
Expand Down
Loading

0 comments on commit 956f89c

Please sign in to comment.