Skip to content

Commit

Permalink
Templatized control and regularization neighborhoods in non-rigid ICP…
Browse files Browse the repository at this point in the history 
… classes; KDTree fix
  • Loading branch information
@kzampog
kzampog committed May 3, 2020
1 parent 6deff7c commit de77854
Show file tree
Hide file tree
Showing 4 changed files with 22 additions and 22 deletions.
10 changes: 5 additions & 5 deletions include/cilantro/core/kd_tree.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,9 +32,9 @@ namespace cilantro {
template <class BBOX>
bool kdtree_get_bbox(BBOX& /*bb*/) const { return false; }
};
}
} // namespace KDTreeDataAdaptors

struct KDTreeDistanceAdaptors {
namespace KDTreeDistanceAdaptors {
template <class DataAdaptor>
using L1 = nanoflann::L1_Adaptor<typename DataAdaptor::coord_t, DataAdaptor, typename DataAdaptor::coord_t>;

Expand All @@ -49,7 +49,7 @@ namespace cilantro {

template <class DataAdaptor>
using SO3 = nanoflann::SO3_Adaptor<typename DataAdaptor::coord_t, DataAdaptor, typename DataAdaptor::coord_t>;
};
} // namespace KDTreeDistanceAdaptors

template <typename ScalarT, typename IndexT = size_t, typename CountT = size_t>
class KNNSearchResultAdaptor {
Expand Down Expand Up @@ -185,8 +185,8 @@ namespace cilantro {

// Do not call if tree is empty!
// Unlike the other batch searches, this one returns a flat vector of Neighbor
inline NeighborSet<ScalarT> nearestNeighborSearch(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &query_pts) const {
NeighborSet<ScalarT> results;
inline NeighborhoodResult nearestNeighborSearch(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &query_pts) const {
NeighborhoodResult results;
nearestNeighborSearch(query_pts, results);
return results;
}
Expand Down
8 changes: 4 additions & 4 deletions include/cilantro/registration/icp_common_instances.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@ namespace cilantro {
};

template <class TransformT, class CorrSearchT>
using DefaultCombinedMetricDenseWarpFieldICP = CombinedMetricDenseWarpFieldICP<TransformT,CorrSearchT,UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>,UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>,RBFKernelWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar,true>>;
using DefaultCombinedMetricDenseWarpFieldICP = CombinedMetricDenseWarpFieldICP<TransformT,CorrSearchT,NeighborhoodSet<typename TransformT::Scalar>,UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>,UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>,RBFKernelWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar,true>>;

template <class TransformT, class CorrSearchT>
class DefaultCombinedMetricDenseWarpFieldICPEntities {
Expand Down Expand Up @@ -126,12 +126,12 @@ namespace cilantro {
const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &src_points,
const std::vector<NeighborSet<typename TransformT::Scalar>> &regularization_neighborhoods)
: DefaultCombinedMetricDenseWarpFieldICPEntities<TransformT,CorrSearchT>(dst_points, src_points),
DefaultCombinedMetricDenseWarpFieldICP<TransformT,CorrSearchT>(dst_points, dst_normals, src_points, this->corr_search_, this->point_corr_weight_eval_, this->plane_corr_weight_eval_, regularization_neighborhoods, this->reg_weight_eval_)
DefaultCombinedMetricDenseWarpFieldICP<TransformT,CorrSearchT>(dst_points, dst_normals, src_points, this->corr_search_, regularization_neighborhoods, this->point_corr_weight_eval_, this->plane_corr_weight_eval_, this->reg_weight_eval_)
{}
};

template <class TransformT, class CorrSearchT>
using DefaultCombinedMetricSparseWarpFieldICP = CombinedMetricSparseWarpFieldICP<TransformT,CorrSearchT,UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>,UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>,RBFKernelWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar,true>,RBFKernelWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar,true>>;
using DefaultCombinedMetricSparseWarpFieldICP = CombinedMetricSparseWarpFieldICP<TransformT,CorrSearchT,NeighborhoodSet<typename TransformT::Scalar>,NeighborhoodSet<typename TransformT::Scalar>,UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>,UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>,RBFKernelWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar,true>,RBFKernelWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar,true>>;

template <class TransformT, class CorrSearchT>
class DefaultCombinedMetricSparseWarpFieldICPEntities {
Expand Down Expand Up @@ -169,7 +169,7 @@ namespace cilantro {
size_t num_control_nodes,
const std::vector<NeighborSet<typename TransformT::Scalar>> &control_regularization_neighborhoods)
: DefaultCombinedMetricSparseWarpFieldICPEntities<TransformT,CorrSearchT>(dst_points, src_points),
DefaultCombinedMetricSparseWarpFieldICP<TransformT,CorrSearchT>(dst_points, dst_normals, src_points, this->corr_search_, this->point_corr_weight_eval_, this->plane_corr_weight_eval_, src_to_control_neighborhoods, num_control_nodes, this->control_weight_eval_, control_regularization_neighborhoods, this->reg_weight_eval_)
DefaultCombinedMetricSparseWarpFieldICP<TransformT,CorrSearchT>(dst_points, dst_normals, src_points, this->corr_search_, src_to_control_neighborhoods, num_control_nodes, control_regularization_neighborhoods, this->point_corr_weight_eval_, this->plane_corr_weight_eval_, this->control_weight_eval_, this->reg_weight_eval_)
{}
};

Expand Down
10 changes: 5 additions & 5 deletions include/cilantro/registration/icp_warp_field_combined_metric_dense.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,9 @@

namespace cilantro {
// TransformT is the local motion model
template <class TransformT, class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>, class RegularizationWeightEvaluatorT = RBFKernelWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar,true>>
class CombinedMetricDenseWarpFieldICP : public IterativeClosestPointBase<CombinedMetricDenseWarpFieldICP<TransformT,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT,RegularizationWeightEvaluatorT>,TransformSet<TransformT>,CorrespondenceSearchEngineT,VectorSet<typename TransformT::Scalar,1>> {
typedef IterativeClosestPointBase<CombinedMetricDenseWarpFieldICP<TransformT,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT,RegularizationWeightEvaluatorT>,TransformSet<TransformT>,CorrespondenceSearchEngineT,VectorSet<typename TransformT::Scalar,1>> Base;
template <class TransformT, class CorrespondenceSearchEngineT, class RegNeighborhoodSetT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>, class RegularizationWeightEvaluatorT = RBFKernelWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar,true>>
class CombinedMetricDenseWarpFieldICP : public IterativeClosestPointBase<CombinedMetricDenseWarpFieldICP<TransformT,CorrespondenceSearchEngineT,RegNeighborhoodSetT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT,RegularizationWeightEvaluatorT>,TransformSet<TransformT>,CorrespondenceSearchEngineT,VectorSet<typename TransformT::Scalar,1>> {
typedef IterativeClosestPointBase<CombinedMetricDenseWarpFieldICP<TransformT,CorrespondenceSearchEngineT,RegNeighborhoodSetT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT,RegularizationWeightEvaluatorT>,TransformSet<TransformT>,CorrespondenceSearchEngineT,VectorSet<typename TransformT::Scalar,1>> Base;
friend Base;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Expand All @@ -24,9 +24,9 @@ namespace cilantro {
const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &dst_n,
const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &src_p,
CorrespondenceSearchEngineT &corr_engine,
const RegNeighborhoodSetT &regularization_neighborhoods,
PointToPointCorrWeightEvaluatorT &point_corr_eval,
PointToPlaneCorrWeightEvaluatorT &plane_corr_eval,
const NeighborhoodSet<typename TransformT::Scalar> &regularization_neighborhoods,
RegularizationWeightEvaluatorT &reg_eval)
: Base(corr_engine),
dst_points_(dst_p), dst_normals_(dst_n), src_points_(src_p),
Expand Down Expand Up @@ -113,7 +113,7 @@ namespace cilantro {
ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> dst_points_;
ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> dst_normals_;
ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> src_points_;
const NeighborhoodSet<typename TransformT::Scalar>& regularization_neighborhoods_;
const RegNeighborhoodSetT& regularization_neighborhoods_;

typename TransformT::Scalar point_to_point_weight_;
typename TransformT::Scalar point_to_plane_weight_;
Expand Down
16 changes: 8 additions & 8 deletions include/cilantro/registration/icp_warp_field_combined_metric_sparse.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,9 @@

namespace cilantro {
// TransformT is the local motion model
template <class TransformT, class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>, class ControlWeightEvaluatorT = RBFKernelWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar,true>, class RegularizationWeightEvaluatorT = RBFKernelWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar,true>>
class CombinedMetricSparseWarpFieldICP : public IterativeClosestPointBase<CombinedMetricSparseWarpFieldICP<TransformT,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT,ControlWeightEvaluatorT,RegularizationWeightEvaluatorT>,TransformSet<TransformT>,CorrespondenceSearchEngineT,VectorSet<typename TransformT::Scalar,1>> {
typedef IterativeClosestPointBase<CombinedMetricSparseWarpFieldICP<TransformT,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT,ControlWeightEvaluatorT,RegularizationWeightEvaluatorT>,TransformSet<TransformT>,CorrespondenceSearchEngineT,VectorSet<typename TransformT::Scalar,1>> Base;
template <class TransformT, class CorrespondenceSearchEngineT, class SrcToCtrlNeighborhoodSetT, class CtrlRegNeighborhoodSetT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>, class ControlWeightEvaluatorT = RBFKernelWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar,true>, class RegularizationWeightEvaluatorT = RBFKernelWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar,true>>
class CombinedMetricSparseWarpFieldICP : public IterativeClosestPointBase<CombinedMetricSparseWarpFieldICP<TransformT,CorrespondenceSearchEngineT,SrcToCtrlNeighborhoodSetT,CtrlRegNeighborhoodSetT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT,ControlWeightEvaluatorT,RegularizationWeightEvaluatorT>,TransformSet<TransformT>,CorrespondenceSearchEngineT,VectorSet<typename TransformT::Scalar,1>> {
typedef IterativeClosestPointBase<CombinedMetricSparseWarpFieldICP<TransformT,CorrespondenceSearchEngineT,SrcToCtrlNeighborhoodSetT,CtrlRegNeighborhoodSetT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT,ControlWeightEvaluatorT,RegularizationWeightEvaluatorT>,TransformSet<TransformT>,CorrespondenceSearchEngineT,VectorSet<typename TransformT::Scalar,1>> Base;
friend Base;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Expand All @@ -26,12 +26,12 @@ namespace cilantro {
const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &dst_n,
const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &src_p,
CorrespondenceSearchEngineT &corr_engine,
const SrcToCtrlNeighborhoodSetT &src_to_ctrl_neighborhoods,
size_t num_ctrl_nodes,
const CtrlRegNeighborhoodSetT &ctrl_regularization_neighborhoods,
PointToPointCorrWeightEvaluatorT &point_corr_eval,
PointToPlaneCorrWeightEvaluatorT &plane_corr_eval,
const NeighborhoodSet<typename TransformT::Scalar> &src_to_ctrl_neighborhoods,
size_t num_ctrl_nodes,
ControlWeightEvaluatorT &control_eval,
const NeighborhoodSet<typename TransformT::Scalar> &ctrl_regularization_neighborhoods,
RegularizationWeightEvaluatorT &reg_eval)
: Base(corr_engine),
dst_points_(dst_p), dst_normals_(dst_n), src_points_(src_p),
Expand Down Expand Up @@ -126,9 +126,9 @@ namespace cilantro {
ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> dst_points_;
ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> dst_normals_;
ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> src_points_;
const NeighborhoodSet<typename TransformT::Scalar>& src_to_ctrl_neighborhoods_;
const SrcToCtrlNeighborhoodSetT& src_to_ctrl_neighborhoods_;
size_t num_ctrl_nodes_;
const NeighborhoodSet<typename TransformT::Scalar>& ctrl_regularization_neighborhoods_;
const CtrlRegNeighborhoodSetT& ctrl_regularization_neighborhoods_;

typename TransformT::Scalar point_to_point_weight_;
typename TransformT::Scalar point_to_plane_weight_;
Expand Down

0 comments on commit de77854

Please sign in to comment.