Skip to content

Commit

Permalink
Added template parameters for point and index types to connected comp…
Browse files Browse the repository at this point in the history 
…onent extraction functions and classes; API updates
  • Loading branch information
@kzampog
kzampog committed May 5, 2020
1 parent 06568a6 commit fe2ec7b
Show file tree
Hide file tree
Showing 7 changed files with 139 additions and 125 deletions.
4 changes: 2 additions & 2 deletions examples/robust_normal_estimation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,9 +44,9 @@ int main(int argc, char ** argv) {

cilantro::Timer ne_timer;
ne_timer.start();
cilantro::NormalEstimation<float, 3, size_t, cilantro::MinimumCovarianceDeterminant<float, 3>> ne(tree);
cilantro::NormalEstimation<float, 3, cilantro::MinimumCovarianceDeterminant<float, 3>> ne(tree);
ne.setViewPoint(Eigen::Vector3f::Zero());
ne.getCovarianceMethod().setChiSquareThreshold(6.25).setNumberOfTrials(2).setNumberOfRefinements(1); // 90% confidence ellipsoid
ne.covarianceMethod().setChiSquareThreshold(6.25).setNumberOfTrials(2).setNumberOfRefinements(1); // 90% confidence ellipsoid
cloud.normals = ne.getNormalsKNN(12);

cilantro::PointCloud3f invalid_cloud = invalidNormalsCloud(cloud);
Expand Down
6 changes: 3 additions & 3 deletions include/cilantro/clustering/clustering_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
#include <Eigen/Dense>

namespace cilantro {
template <typename ClusterIndexT = size_t, typename PointIndexT = size_t>
template <typename ClusterIndexT, typename PointIndexT>
std::vector<ClusterIndexT> getPointToClusterIndexMap(const std::vector<std::vector<PointIndexT>> &cluster_to_point,
size_t num_points)
{
Expand All @@ -20,7 +20,7 @@ namespace cilantro {

// Cluster indices in point_to_cluster are in [0, num_clusters - 1];
// a cluster index >= num_clusters signifies unlabeled point
template <typename PointIndexT = size_t, typename ClusterIndexT = size_t>
template <typename PointIndexT, typename ClusterIndexT>
std::vector<std::vector<PointIndexT>> getClusterToPointIndicesMap(const std::vector<ClusterIndexT> &point_to_cluster,
size_t num_clusters)
{
Expand Down Expand Up @@ -61,7 +61,7 @@ namespace cilantro {
}

// CRTP base class that holds clustering results and accessors
template <typename Derived, typename PointIndexT = size_t, typename ClusterIndexT = size_t>
template <typename Derived, typename PointIndexT, typename ClusterIndexT>
class ClusteringBase {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Expand Down
170 changes: 85 additions & 85 deletions include/cilantro/clustering/connected_component_extraction.hpp
View file

Large diffs are not rendered by default.

4 changes: 2 additions & 2 deletions include/cilantro/core/covariance.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,13 +31,13 @@ namespace cilantro {
size_t size = std::distance(begin, end);
if (size < points.rows()) return false;

mean.setZero();
mean.setZero(points.rows());
for (NeighborhoodResultIteratorT it = begin; it != end; ++it) {
mean += points.col(it->index);
}
mean *= (ScalarT)(1.0)/size;

cov.setZero();
cov.setZero(points.rows(), points.rows());
for (NeighborhoodResultIteratorT it = begin; it != end; ++it) {
auto tmp = points.col(it->index) - mean;
cov += tmp*tmp.transpose();
Expand Down
30 changes: 21 additions & 9 deletions include/cilantro/core/normal_estimation.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
#include <cilantro/core/kd_tree.hpp>

namespace cilantro {
template <typename ScalarT, ptrdiff_t EigenDim, typename IndexT = size_t, typename CovarianceT = Covariance<ScalarT, EigenDim>>
template <typename ScalarT, ptrdiff_t EigenDim, typename CovarianceT = Covariance<ScalarT, EigenDim>, typename IndexT = size_t>
class NormalEstimation {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Expand Down Expand Up @@ -36,8 +36,9 @@ namespace cilantro {
if (kd_tree_owned_) delete kd_tree_ptr_;
}

inline const CovarianceT& getCovarianceMethod() const { return compute_mean_and_covariance_; }
inline CovarianceT& getCovarianceMethod() { return compute_mean_and_covariance_; }
inline const CovarianceT& covarianceMethod() const { return compute_mean_and_covariance_; }

inline CovarianceT& covarianceMethod() { return compute_mean_and_covariance_; }

// Used for rudimentary normal consistency
// Enforces normals to point towards the given view point
Expand Down Expand Up @@ -471,10 +472,21 @@ namespace cilantro {
}
};

typedef NormalEstimation<float,2> NormalEstimation2f;
typedef NormalEstimation<double,2> NormalEstimation2d;
typedef NormalEstimation<float,3> NormalEstimation3f;
typedef NormalEstimation<double,3> NormalEstimation3d;
typedef NormalEstimation<float,Eigen::Dynamic> NormalEstimationXf;
typedef NormalEstimation<double,Eigen::Dynamic> NormalEstimationXd;
template <typename CovarianceT = Covariance<float,2>, typename IndexT = size_t>
using NormalEstimation2f = NormalEstimation<float,2,CovarianceT,IndexT>;

template <typename CovarianceT = Covariance<double,2>, typename IndexT = size_t>
using NormalEstimation2d = NormalEstimation<double,2,CovarianceT,IndexT>;

template <typename CovarianceT = Covariance<float,3>, typename IndexT = size_t>
using NormalEstimation3f = NormalEstimation<float,3,CovarianceT,IndexT>;

template <typename CovarianceT = Covariance<double,3>, typename IndexT = size_t>
using NormalEstimation3d = NormalEstimation<double,3,CovarianceT,IndexT>;

template <typename CovarianceT = Covariance<float,Eigen::Dynamic>, typename IndexT = size_t>
using NormalEstimationXf = NormalEstimation<float,Eigen::Dynamic,CovarianceT,IndexT>;

template <typename CovarianceT = Covariance<double,Eigen::Dynamic>, typename IndexT = size_t>
using NormalEstimationXd = NormalEstimation<double,Eigen::Dynamic,CovarianceT,IndexT>;
}
2 changes: 2 additions & 0 deletions include/cilantro/registration/icp_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,8 @@ namespace cilantro {
correspondence_search_engine_(corr_engine)
{}

inline const CorrespondenceSearchEngine& correspondenceSearchEngine() const { return correspondence_search_engine_; }

inline CorrespondenceSearchEngine& correspondenceSearchEngine() { return correspondence_search_engine_; }

inline size_t getMaxNumberOfIterations() const { return max_iterations_; }
Expand Down
48 changes: 24 additions & 24 deletions include/cilantro/utilities/point_cloud.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -269,106 +269,106 @@ namespace cilantro {
return res;
}

template <typename IndexT = size_t, typename CountT = size_t, typename CovarianceT = Covariance<ScalarT, EigenDim>>
template <typename CovarianceT = Covariance<ScalarT, EigenDim>, typename IndexT = size_t, typename CountT = size_t>
inline PointCloud& estimateNormalsKNN(CountT k, bool use_current_as_ref = false) {
use_current_as_ref = use_current_as_ref && hasNormals();
normals.resize(points.rows(), points.cols());
if (use_current_as_ref) {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(points).setReferenceNormals(normals).estimateNormalsKNN(normals, k);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(points).setReferenceNormals(normals).estimateNormalsKNN(normals, k);
} else {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(points).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormalsKNN(normals, k);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(points).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormalsKNN(normals, k);
}
return *this;
}

template <typename IndexT = size_t, typename CountT = size_t, typename CovarianceT = Covariance<ScalarT, EigenDim>>
template <typename CovarianceT = Covariance<ScalarT, EigenDim>, typename IndexT = size_t, typename CountT = size_t>
inline PointCloud& estimateNormalsKNN(const KDTree<ScalarT,EigenDim,KDTreeDistanceAdaptors::L2,IndexT> &kd_tree,
CountT k, bool use_current_as_ref = false)
{
use_current_as_ref = use_current_as_ref && hasNormals();
normals.resize(points.rows(), points.cols());
if (use_current_as_ref) {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(kd_tree).setReferenceNormals(normals).estimateNormalsKNN(normals, k);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(kd_tree).setReferenceNormals(normals).estimateNormalsKNN(normals, k);
} else {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(kd_tree).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormalsKNN(normals, k);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(kd_tree).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormalsKNN(normals, k);
}
return *this;
}

template <typename IndexT = size_t, typename CovarianceT = Covariance<ScalarT, EigenDim>>
template <typename CovarianceT = Covariance<ScalarT, EigenDim>, typename IndexT = size_t>
inline PointCloud& estimateNormalsRadius(ScalarT radius, bool use_current_as_ref = false) {
use_current_as_ref = use_current_as_ref && hasNormals();
normals.resize(points.rows(), points.cols());
if (use_current_as_ref) {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(points).setReferenceNormals(normals).estimateNormalsRadius(normals, radius);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(points).setReferenceNormals(normals).estimateNormalsRadius(normals, radius);
} else {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(points).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormalsRadius(normals, radius);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(points).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormalsRadius(normals, radius);
}
return *this;
}

template <typename IndexT = size_t, typename CovarianceT = Covariance<ScalarT, EigenDim>>
template <typename CovarianceT = Covariance<ScalarT, EigenDim>, typename IndexT = size_t>
inline PointCloud& estimateNormalsRadius(const KDTree<ScalarT,EigenDim,KDTreeDistanceAdaptors::L2,IndexT> &kd_tree,
ScalarT radius, bool use_current_as_ref = false)
{
use_current_as_ref = use_current_as_ref && hasNormals();
normals.resize(points.rows(), points.cols());
if (use_current_as_ref) {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(kd_tree).setReferenceNormals(normals).estimateNormalsRadius(normals, radius);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(kd_tree).setReferenceNormals(normals).estimateNormalsRadius(normals, radius);
} else {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(kd_tree).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormalsRadius(normals, radius);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(kd_tree).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormalsRadius(normals, radius);
}
return *this;
}

template <typename IndexT = size_t, typename CountT = size_t, typename CovarianceT = Covariance<ScalarT, EigenDim>>
template <typename CovarianceT = Covariance<ScalarT, EigenDim>, typename IndexT = size_t, typename CountT = size_t>
inline PointCloud& estimateNormalsKNNInRadius(CountT k, ScalarT radius, bool use_current_as_ref = false) {
use_current_as_ref = use_current_as_ref && hasNormals();
normals.resize(points.rows(), points.cols());
if (use_current_as_ref) {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(points).setReferenceNormals(normals).estimateNormalsKNNInRadius(normals, k, radius);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(points).setReferenceNormals(normals).estimateNormalsKNNInRadius(normals, k, radius);
} else {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(points).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormalsKNNInRadius(normals, k, radius);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(points).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormalsKNNInRadius(normals, k, radius);
}
return *this;
}

template <typename IndexT = size_t, typename CountT = size_t, typename CovarianceT = Covariance<ScalarT, EigenDim>>
template <typename CovarianceT = Covariance<ScalarT, EigenDim>, typename IndexT = size_t, typename CountT = size_t>
inline PointCloud& estimateNormalsKNNInRadius(const KDTree<ScalarT,EigenDim,KDTreeDistanceAdaptors::L2,IndexT> &kd_tree,
CountT k, ScalarT radius, bool use_current_as_ref = false)
{
use_current_as_ref = use_current_as_ref && hasNormals();
normals.resize(points.rows(), points.cols());
if (use_current_as_ref) {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(kd_tree).setReferenceNormals(normals).estimateNormalsKNNInRadius(normals, k, radius);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(kd_tree).setReferenceNormals(normals).estimateNormalsKNNInRadius(normals, k, radius);
} else {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(kd_tree).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormalsKNNInRadius(normals, k, radius);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(kd_tree).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormalsKNNInRadius(normals, k, radius);
}
return *this;
}

template <typename IndexT = size_t, typename NeighborhoodSpecT, typename CovarianceT = Covariance<ScalarT, EigenDim>>
template <typename NeighborhoodSpecT, typename CovarianceT = Covariance<ScalarT, EigenDim>, typename IndexT = size_t>
inline PointCloud& estimateNormals(const NeighborhoodSpecT &nh, bool use_current_as_ref = false) {
use_current_as_ref = use_current_as_ref && hasNormals();
normals.resize(points.rows(), points.cols());
if (use_current_as_ref) {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(points).setReferenceNormals(normals).estimateNormals(normals, nh);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(points).setReferenceNormals(normals).estimateNormals(normals, nh);
} else {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(points).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormals(normals, nh);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(points).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormals(normals, nh);
}
return *this;
}

template <typename IndexT = size_t, typename NeighborhoodSpecT, typename CovarianceT = Covariance<ScalarT, EigenDim>>
template <typename NeighborhoodSpecT, typename CovarianceT = Covariance<ScalarT, EigenDim>, typename IndexT = size_t>
inline PointCloud& estimateNormals(const KDTree<ScalarT,EigenDim,KDTreeDistanceAdaptors::L2,IndexT> &kd_tree,
const NeighborhoodSpecT &nh, bool use_current_as_ref = false)
{
use_current_as_ref = use_current_as_ref && hasNormals();
normals.resize(points.rows(), points.cols());
if (use_current_as_ref) {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(kd_tree).setReferenceNormals(normals).estimateNormals(normals, nh);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(kd_tree).setReferenceNormals(normals).estimateNormals(normals, nh);
} else {
NormalEstimation<ScalarT,EigenDim,IndexT,CovarianceT>(kd_tree).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormals(normals, nh);
NormalEstimation<ScalarT,EigenDim,CovarianceT,IndexT>(kd_tree).setViewPoint(Vector<ScalarT,EigenDim>::Zero(points.rows(),1)).estimateNormals(normals, nh);
}
return *this;
}
Expand Down

0 comments on commit fe2ec7b

Please sign in to comment.