Skip to content

Commit

Permalink
Refactored PCA to use Covariance class; added optional parallelizatio…
Browse files Browse the repository at this point in the history 
…n to covariance classes; optimized HyperplaneRANSACEstimator
  • Loading branch information
@kzampog
kzampog committed May 9, 2020
1 parent 81de2c0 commit aab6719
Show file tree
Hide file tree
Showing 3 changed files with 191 additions and 104 deletions.
171 changes: 140 additions & 31 deletions include/cilantro/core/covariance.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,13 @@
#pragma once

#include <algorithm>
#include <iterator>
#include <limits>
#include <cilantro/config.hpp>
#include <cilantro/core/data_containers.hpp>
#include <cilantro/core/nearest_neighbors.hpp>
#include <cilantro/core/random.hpp>
#include <cilantro/core/openmp_reductions.hpp>

namespace cilantro {
template <typename ScalarT, ptrdiff_t EigenDim>
Expand All @@ -18,39 +21,146 @@ namespace cilantro {

Covariance() = default;

inline bool operator()(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points, Vector<ScalarT,EigenDim>& mean, Eigen::Matrix<ScalarT,EigenDim,EigenDim>& cov) const {
if (points.cols() < points.rows()) return false;
mean = points.rowwise().mean();
auto centered = points.colwise() - mean; // Lazy evaluation
cov.noalias() = (ScalarT(1.0)/static_cast<ScalarT>(points.cols() - 1)) * (centered * centered.transpose());
return true;
inline bool operator()(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points, Vector<ScalarT,EigenDim>& mean, Eigen::Matrix<ScalarT,EigenDim,EigenDim>& cov, bool parallel = false) const {
if (points.cols() < 2) {
if (points.cols() == 0) {
mean.setConstant(points.rows(), 1, std::numeric_limits<ScalarT>::quiet_NaN());
} else {
mean = points.rowwise().mean();
}
cov.setConstant(points.rows(), points.rows(), std::numeric_limits<ScalarT>::quiet_NaN());
return false;
}

if (parallel) {
Vector<ScalarT,EigenDim> mean_sum(Vector<ScalarT,EigenDim>::Zero(points.rows(), 1));
#ifdef ENABLE_NON_DETERMINISTIC_PARALLELISM
DECLARE_MATRIX_SUM_REDUCTION(ScalarT,EigenDim,1)
#pragma omp parallel for MATRIX_SUM_REDUCTION(ScalarT,EigenDim,1,mean_sum)
//#pragma omp parallel for reduction (internal::MatrixReductions<ScalarT,EigenDim,1>::operator+: mean_sum)
#endif
for (size_t i = 0; i < points.cols(); i++) {
mean_sum.noalias() += points.col(i);
}
mean.noalias() = (ScalarT(1.0)/static_cast<ScalarT>(points.cols()))*mean_sum;

Eigen::Matrix<ScalarT,EigenDim,EigenDim> cov_sum(Eigen::Matrix<ScalarT,EigenDim,EigenDim>::Zero(points.rows(), points.rows()));
#ifdef ENABLE_NON_DETERMINISTIC_PARALLELISM
DECLARE_MATRIX_SUM_REDUCTION(ScalarT,EigenDim,EigenDim)
#pragma omp parallel for MATRIX_SUM_REDUCTION(ScalarT,EigenDim,EigenDim,cov_sum)
//#pragma omp parallel for reduction (internal::MatrixReductions<ScalarT,EigenDim,EigenDim>::operator+: cov_sum)
#endif
for (size_t i = 0; i < points.cols(); i++) {
Vector<ScalarT,EigenDim> tmp = points.col(i) - mean;
cov_sum.noalias() += tmp*tmp.transpose();
}
cov.noalias() = (ScalarT(1.0)/static_cast<ScalarT>(points.cols() - 1))*cov_sum;
} else {
Vector<ScalarT,EigenDim> mean_sum(Vector<ScalarT,EigenDim>::Zero(points.rows(), 1));
for (size_t i = 0; i < points.cols(); i++) {
mean_sum.noalias() += points.col(i);
}
mean.noalias() = (ScalarT(1.0)/static_cast<ScalarT>(points.cols()))*mean_sum;

Eigen::Matrix<ScalarT,EigenDim,EigenDim> cov_sum(Eigen::Matrix<ScalarT,EigenDim,EigenDim>::Zero(points.rows(), points.rows()));
for (size_t i = 0; i < points.cols(); i++) {
Vector<ScalarT,EigenDim> tmp = points.col(i) - mean;
cov_sum.noalias() += tmp*tmp.transpose();
}
cov.noalias() = (ScalarT(1.0)/static_cast<ScalarT>(points.cols() - 1))*cov_sum;
}

return points.cols() >= points.rows();
}

template <typename IteratorT>
inline bool operator()(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points, IteratorT begin, IteratorT end, Vector<ScalarT,EigenDim>& mean, Eigen::Matrix<ScalarT,EigenDim,EigenDim>& cov) const {
size_t size = std::distance(begin, end);
if (size < points.rows()) return false;
// Conditionally enable parallelization if iterators are random access
template <typename IteratorT, typename std::enable_if<std::is_same<typename std::iterator_traits<IteratorT>::iterator_category, std::random_access_iterator_tag>::value, int>::type = 0>
inline bool operator()(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points, IteratorT begin, IteratorT end, Vector<ScalarT,EigenDim>& mean, Eigen::Matrix<ScalarT,EigenDim,EigenDim>& cov, bool parallel = false) const {
const size_t size = std::distance(begin, end);
if (size < 2) {
if (size == 0) {
mean.setConstant(points.rows(), 1, std::numeric_limits<ScalarT>::quiet_NaN());
} else {
mean = points.rowwise().mean();
}
cov.setConstant(points.rows(), points.rows(), std::numeric_limits<ScalarT>::quiet_NaN());
return false;
}

mean.setZero(points.rows(), 1);
if (parallel) {
Vector<ScalarT,EigenDim> mean_sum(Vector<ScalarT,EigenDim>::Zero(points.rows(), 1));
#ifdef ENABLE_NON_DETERMINISTIC_PARALLELISM
DECLARE_MATRIX_SUM_REDUCTION(ScalarT,EigenDim,1)
#pragma omp parallel for MATRIX_SUM_REDUCTION(ScalarT,EigenDim,1,mean_sum)
//#pragma omp parallel for reduction (internal::MatrixReductions<ScalarT,EigenDim,1>::operator+: mean_sum)
#endif
for (IteratorT it = begin; it < end; ++it) {
mean_sum.noalias() += points.col(*it);
}
mean.noalias() = (ScalarT(1.0)/static_cast<ScalarT>(size))*mean_sum;

Eigen::Matrix<ScalarT,EigenDim,EigenDim> cov_sum(Eigen::Matrix<ScalarT,EigenDim,EigenDim>::Zero(points.rows(), points.rows()));
#ifdef ENABLE_NON_DETERMINISTIC_PARALLELISM
DECLARE_MATRIX_SUM_REDUCTION(ScalarT,EigenDim,EigenDim)
#pragma omp parallel for MATRIX_SUM_REDUCTION(ScalarT,EigenDim,EigenDim,cov_sum)
//#pragma omp parallel for reduction (internal::MatrixReductions<ScalarT,EigenDim,EigenDim>::operator+: cov_sum)
#endif
for (IteratorT it = begin; it < end; ++it) {
Vector<ScalarT,EigenDim> tmp = points.col(*it) - mean;
cov_sum.noalias() += tmp*tmp.transpose();
}
cov.noalias() = (ScalarT(1.0)/static_cast<ScalarT>(size - 1))*cov_sum;
} else {
Vector<ScalarT,EigenDim> mean_sum(Vector<ScalarT,EigenDim>::Zero(points.rows(), 1));
for (IteratorT it = begin; it != end; ++it) {
mean_sum.noalias() += points.col(*it);
}
mean.noalias() = (ScalarT(1.0)/static_cast<ScalarT>(size))*mean_sum;

Eigen::Matrix<ScalarT,EigenDim,EigenDim> cov_sum(Eigen::Matrix<ScalarT,EigenDim,EigenDim>::Zero(points.rows(), points.rows()));
for (IteratorT it = begin; it != end; ++it) {
Vector<ScalarT,EigenDim> tmp = points.col(*it) - mean;
cov_sum.noalias() += tmp*tmp.transpose();
}
cov.noalias() = (ScalarT(1.0)/static_cast<ScalarT>(size - 1))*cov_sum;
}

return size >= points.rows();
}

// Non-random access iterators: this overload ignores the parallelization flag
template <typename IteratorT, typename std::enable_if<std::is_same<typename std::iterator_traits<IteratorT>::iterator_category, std::random_access_iterator_tag>::value == false, int>::type = 0>
inline bool operator()(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points, IteratorT begin, IteratorT end, Vector<ScalarT,EigenDim>& mean, Eigen::Matrix<ScalarT,EigenDim,EigenDim>& cov, bool) const {
const size_t size = std::distance(begin, end);
if (size < 2) {
if (size == 0) {
mean.setConstant(points.rows(), 1, std::numeric_limits<ScalarT>::quiet_NaN());
} else {
mean = points.rowwise().mean();
}
cov.setConstant(points.rows(), points.rows(), std::numeric_limits<ScalarT>::quiet_NaN());
return false;
}

Vector<ScalarT,EigenDim> mean_sum(Vector<ScalarT,EigenDim>::Zero(points.rows(), 1));
for (IteratorT it = begin; it != end; ++it) {
// mean.noalias() += points.col(it->index);
mean.noalias() += points.col(*it);
mean_sum.noalias() += points.col(*it);
}
mean *= ScalarT(1.0)/static_cast<ScalarT>(size);
mean.noalias() = (ScalarT(1.0)/static_cast<ScalarT>(size))*mean_sum;

cov.setZero(points.rows(), points.rows());
Eigen::Matrix<ScalarT,EigenDim,EigenDim> cov_sum(Eigen::Matrix<ScalarT,EigenDim,EigenDim>::Zero(points.rows(), points.rows()));
for (IteratorT it = begin; it != end; ++it) {
// auto tmp = points.col(it->index) - mean;
auto tmp = points.col(*it) - mean;
cov.noalias() += tmp*tmp.transpose();
Vector<ScalarT,EigenDim> tmp = points.col(*it) - mean;
cov_sum.noalias() += tmp*tmp.transpose();
}
cov *= ScalarT(1.0)/static_cast<ScalarT>(size - 1);
return true;
cov.noalias() = (ScalarT(1.0)/static_cast<ScalarT>(size - 1))*cov_sum;

return size >= points.rows();
}

template <typename ContainerT>
inline bool operator()(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points, const ContainerT &subset, Vector<ScalarT,EigenDim>& mean, Eigen::Matrix<ScalarT,EigenDim,EigenDim>& cov) const {
return (*this)(points, subset.begin(), subset.end(), mean, cov);
inline bool operator()(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points, const ContainerT &subset, Vector<ScalarT,EigenDim>& mean, Eigen::Matrix<ScalarT,EigenDim,EigenDim>& cov, bool parallel = false) const {
return (*this)(points, subset.begin(), subset.end(), mean, cov, parallel);
}
};

Expand All @@ -69,20 +179,19 @@ namespace cilantro {

MinimumCovarianceDeterminant() = default;

inline bool operator()(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points, Vector<ScalarT,EigenDim>& mean, Eigen::Matrix<ScalarT,EigenDim,EigenDim>& cov) const {
inline bool operator()(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points, Vector<ScalarT,EigenDim>& mean, Eigen::Matrix<ScalarT,EigenDim,EigenDim>& cov, bool parallel = false) const {
// Neighborhood<ScalarT> subset(points.cols());
// size_t count = 0;
// std::generate(subset.begin(), subset.end(), [&count]() mutable { return Neighbor<ScalarT>(count++, ScalarT(0.0)); });
std::vector<size_t> subset(points.cols());
for (size_t i = 0; i < subset.size(); i++) subset[i] = i;
return (*this)(points, subset, mean, cov);
return (*this)(points, subset, mean, cov, parallel);
}

template <typename IteratorT>
inline bool operator()(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points, IteratorT begin, IteratorT end, Vector<ScalarT,EigenDim>& mean, Eigen::Matrix<ScalarT,EigenDim,EigenDim>& cov) const {
inline bool operator()(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points, IteratorT begin, IteratorT end, Vector<ScalarT,EigenDim>& mean, Eigen::Matrix<ScalarT,EigenDim,EigenDim>& cov, bool parallel = false) const {
const size_t size = std::distance(begin, end);
if (size < points.rows()) return false;
if (size == points.rows()) return compute_mean_and_covariance_(points, begin, end, mean, cov);
if (size <= points.rows()) return compute_mean_and_covariance_(points, begin, end, mean, cov, false);

Neighborhood<ScalarT> range_copy(size);
size_t k = 0;
Expand All @@ -107,11 +216,11 @@ namespace cilantro {
ScalarT best_determinant = std::numeric_limits<ScalarT>::max();
for (int j = 0; j < num_trials_; ++j) {
std::generate(subset.begin(), subset.end(), [&copy_begin, &copy_end, &random]() { return *random(copy_begin, copy_end); });
compute_mean_and_covariance_(points, subset.begin(), subset.end(), mean, cov);
compute_mean_and_covariance_(points, subset.begin(), subset.end(), mean, cov, false);
for (int l = 0; l < num_refinements_; ++l) {
mahalanobisDistance(points, copy_begin, copy_end, mean, cov.inverse());
std::partial_sort(copy_begin, copy_begin + h, copy_end, typename Neighbor<ScalarT>::ValueLessComparator());
compute_mean_and_covariance_(points, copy_begin, copy_begin + h, mean, cov);
compute_mean_and_covariance_(points, copy_begin, copy_begin + h, mean, cov, parallel);
}
ScalarT determinant = cov.determinant();
if (determinant < best_determinant) {
Expand All @@ -129,8 +238,8 @@ namespace cilantro {
}

template <typename ContainerT>
inline bool operator()(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points, const ContainerT &subset, Vector<ScalarT,EigenDim>& mean, Eigen::Matrix<ScalarT,EigenDim,EigenDim>& cov) const {
return (*this)(points, subset.begin(), subset.end(), mean, cov);
inline bool operator()(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points, const ContainerT &subset, Vector<ScalarT,EigenDim>& mean, Eigen::Matrix<ScalarT,EigenDim,EigenDim>& cov, bool parallel = false) const {
return (*this)(points, subset.begin(), subset.end(), mean, cov, parallel);
}

inline const Covariance& evaluator() const { return compute_mean_and_covariance_; }
Expand Down
91 changes: 33 additions & 58 deletions include/cilantro/core/principal_component_analysis.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,7 @@
#pragma once

#include <cilantro/config.hpp>
#include <cilantro/core/data_containers.hpp>
#include <cilantro/core/openmp_reductions.hpp>
#include <cilantro/core/covariance.hpp>

namespace cilantro {
template <typename ScalarT, ptrdiff_t EigenDim>
Expand All @@ -14,68 +13,33 @@ namespace cilantro {

enum { Dimension = EigenDim };

PrincipalComponentAnalysis(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &data, bool parallel = false) {
if (parallel) {
Vector<ScalarT,EigenDim> sum(Vector<ScalarT,EigenDim>::Zero(data.rows(), 1));

#ifdef ENABLE_NON_DETERMINISTIC_PARALLELISM
DECLARE_MATRIX_SUM_REDUCTION(ScalarT,EigenDim,1)
#pragma omp parallel for MATRIX_SUM_REDUCTION(ScalarT,EigenDim,1,sum)
//#pragma omp parallel for reduction (internal::MatrixReductions<ScalarT,EigenDim,1>::operator+: sum)
#endif
for (size_t i = 0; i < data.cols(); i++) {
sum.noalias() += data.col(i);
}
mean_.noalias() = (ScalarT(1.0)/static_cast<ScalarT>(data.cols()))*sum;

Eigen::Matrix<ScalarT,EigenDim,EigenDim> cov(Eigen::Matrix<ScalarT,EigenDim,EigenDim>::Zero(data.rows(), data.rows()));

#ifdef ENABLE_NON_DETERMINISTIC_PARALLELISM
DECLARE_MATRIX_SUM_REDUCTION(ScalarT,EigenDim,EigenDim)
#pragma omp parallel for MATRIX_SUM_REDUCTION(ScalarT,EigenDim,EigenDim,cov)
//#pragma omp parallel for reduction (internal::MatrixReductions<ScalarT,EigenDim,EigenDim>::operator+: cov)
#endif
for (size_t i = 0; i < data.cols(); i++) {
Eigen::Matrix<ScalarT,EigenDim,1> ptc = data.col(i) - mean_;
cov.noalias() += ptc*ptc.transpose();
}
cov *= ScalarT(1.0)/static_cast<ScalarT>(data.cols() - 1);

Eigen::SelfAdjointEigenSolver<Eigen::Matrix<ScalarT,EigenDim,EigenDim>> eig(cov);
eigenvectors_ = eig.eigenvectors().rowwise().reverse();
if (eigenvectors_.determinant() < ScalarT(0.0)) {
auto last_col = eigenvectors_.col(data.rows() - 1);
last_col = -last_col;
}
eigenvalues_ = eig.eigenvalues().reverse();
} else {
Vector<ScalarT,EigenDim> sum(Vector<ScalarT,EigenDim>::Zero(data.rows(), 1));
for (size_t i = 0; i < data.cols(); i++) {
sum.noalias() += data.col(i);
}
mean_.noalias() = (ScalarT(1.0)/static_cast<ScalarT>(data.cols()))*sum;

Eigen::Matrix<ScalarT,EigenDim,EigenDim> cov(Eigen::Matrix<ScalarT,EigenDim,EigenDim>::Zero(data.rows(), data.rows()));
for (size_t i = 0; i < data.cols(); i++) {
Eigen::Matrix<ScalarT,EigenDim,1> ptc = data.col(i) - mean_;
cov.noalias() += ptc*ptc.transpose();
}
cov *= ScalarT(1.0)/static_cast<ScalarT>(data.cols() - 1);

Eigen::SelfAdjointEigenSolver<Eigen::Matrix<ScalarT,EigenDim,EigenDim>> eig(cov);
eigenvectors_ = eig.eigenvectors().rowwise().reverse();
if (eigenvectors_.determinant() < ScalarT(0.0)) {
auto last_col = eigenvectors_.col(data.rows() - 1);
last_col = -last_col;
}
eigenvalues_ = eig.eigenvalues().reverse();
}
typedef Eigen::Matrix<ScalarT,EigenDim,EigenDim> CovarianceMatrix;

template <typename CovarianceT = Covariance<ScalarT,EigenDim>>
PrincipalComponentAnalysis(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &data,
bool parallel = false,
const CovarianceT& cov_eval = CovarianceT())
{
cov_eval(data, mean_, covariance_, parallel);
compute_();
}

template <typename ContainerT, typename CovarianceT = Covariance<ScalarT,EigenDim>>
PrincipalComponentAnalysis(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &data,
const ContainerT& subset,
bool parallel = false,
const CovarianceT& cov_eval = CovarianceT())
{
cov_eval(data, subset.begin(), subset.end(), mean_, covariance_, parallel);
compute_();
}

~PrincipalComponentAnalysis() {}

inline const Vector<ScalarT,EigenDim>& getDataMean() const { return mean_; }

inline const Eigen::Matrix<ScalarT,EigenDim,EigenDim>& getDataCovariance() const { return covariance_; }

inline const Vector<ScalarT,EigenDim>& getEigenValues() const { return eigenvalues_; }

inline const Eigen::Matrix<ScalarT,EigenDim,EigenDim>& getEigenVectors() const { return eigenvectors_; }
Expand All @@ -102,8 +66,19 @@ DECLARE_MATRIX_SUM_REDUCTION(ScalarT,EigenDim,EigenDim)

protected:
Vector<ScalarT,EigenDim> mean_;
Eigen::Matrix<ScalarT,EigenDim,EigenDim> covariance_;
Vector<ScalarT,EigenDim> eigenvalues_;
Eigen::Matrix<ScalarT,EigenDim,EigenDim> eigenvectors_;

inline void compute_() {
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<ScalarT,EigenDim,EigenDim>> eig(covariance_);
eigenvectors_ = eig.eigenvectors().rowwise().reverse();
if (eigenvectors_.determinant() < ScalarT(0.0)) {
auto last_col = eigenvectors_.col(mean_.rows() - 1);
last_col.noalias() = -last_col;
}
eigenvalues_ = eig.eigenvalues().reverse();
}
};

typedef PrincipalComponentAnalysis<float,2> PrincipalComponentAnalysis2f;
Expand Down
Loading

0 comments on commit aab6719

Please sign in to comment.