covariance_sampling.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2009-2012, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, Inc.
* All rights reserved.
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*/
#ifndef PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_
#define PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_
#include <pcl/common/eigen.h>
#include <pcl/filters/covariance_sampling.h>
#include <list>
///////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename PointNT> bool
pcl::CovarianceSampling<PointT, PointNT>::initCompute ()
{
if (!FilterIndices<PointT>::initCompute ())
return false;
if (num_samples_ > indices_->size ())
{
PCL_ERROR ("[pcl::CovarianceSampling::initCompute] The number of samples you asked for (%d) is larger than the number of input indices (%lu)\n",
num_samples_, indices_->size ());
return false;
}
// Prepare the point cloud by centering at the origin and then scaling the points such that the average distance from
// the origin is 1.0 => rotations and translations will have the same magnitude
Eigen::Vector3f centroid (0.f, 0.f, 0.f);
for (size_t p_i = 0; p_i < indices_->size (); ++p_i)
centroid += (*input_)[(*indices_)[p_i]].getVector3fMap ();
centroid /= float (indices_->size ());
scaled_points_.resize (indices_->size ());
double average_norm = 0.0;
for (size_t p_i = 0; p_i < indices_->size (); ++p_i)
{
scaled_points_[p_i] = (*input_)[(*indices_)[p_i]].getVector3fMap () - centroid;
average_norm += scaled_points_[p_i].norm ();
}
average_norm /= double (scaled_points_.size ());
for (size_t p_i = 0; p_i < scaled_points_.size (); ++p_i)
scaled_points_[p_i] /= float (average_norm);
return (true);
}
///////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename PointNT> double
pcl::CovarianceSampling<PointT, PointNT>::computeConditionNumber ()
{
Eigen::Matrix<double, 6, 6> covariance_matrix;
if (!computeCovarianceMatrix (covariance_matrix))
return (-1.);
Eigen::EigenSolver<Eigen::Matrix<double, 6, 6> > eigen_solver;
eigen_solver.compute (covariance_matrix, true);
Eigen::MatrixXcd complex_eigenvalues = eigen_solver.eigenvalues ();
double max_ev = -std::numeric_limits<double>::max (),
min_ev = std::numeric_limits<double>::max ();
for (size_t i = 0; i < 6; ++i)
{
if (real (complex_eigenvalues (i, 0)) > max_ev)
max_ev = real (complex_eigenvalues (i, 0));
if (real (complex_eigenvalues (i, 0)) < min_ev)
min_ev = real (complex_eigenvalues (i, 0));
}
return (max_ev / min_ev);
}
///////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename PointNT> double
pcl::CovarianceSampling<PointT, PointNT>::computeConditionNumber (const Eigen::Matrix<double, 6, 6> &covariance_matrix)
{
Eigen::EigenSolver<Eigen::Matrix<double, 6, 6> > eigen_solver;
eigen_solver.compute (covariance_matrix, true);
Eigen::MatrixXcd complex_eigenvalues = eigen_solver.eigenvalues ();
double max_ev = -std::numeric_limits<double>::max (),
min_ev = std::numeric_limits<double>::max ();
for (size_t i = 0; i < 6; ++i)
{
if (real (complex_eigenvalues (i, 0)) > max_ev)
max_ev = real (complex_eigenvalues (i, 0));
if (real (complex_eigenvalues (i, 0)) < min_ev)
min_ev = real (complex_eigenvalues (i, 0));
}
return (max_ev / min_ev);
}
///////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename PointNT> bool
pcl::CovarianceSampling<PointT, PointNT>::computeCovarianceMatrix (Eigen::Matrix<double, 6, 6> &covariance_matrix)
{
if (!initCompute ())
return false;
//--- Part A from the paper
// Set up matrix F
Eigen::Matrix<double, 6, Eigen::Dynamic> f_mat = Eigen::Matrix<double, 6, Eigen::Dynamic> (6, indices_->size ());
for (size_t p_i = 0; p_i < scaled_points_.size (); ++p_i)
{
f_mat.block<3, 1> (0, p_i) = scaled_points_[p_i].cross (
(*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ()).template cast<double> ();
f_mat.block<3, 1> (3, p_i) = (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ().template cast<double> ();
}
// Compute the covariance matrix C and its 6 eigenvectors (initially complex, move them to a double matrix)
covariance_matrix = f_mat * f_mat.transpose ();
return true;
}
///////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename PointNT> void
pcl::CovarianceSampling<PointT, PointNT>::applyFilter (std::vector<int> &sampled_indices)
{
if (!initCompute ())
return;
//--- Part A from the paper
// Set up matrix F
Eigen::Matrix<double, 6, Eigen::Dynamic> f_mat = Eigen::Matrix<double, 6, Eigen::Dynamic> (6, indices_->size ());
for (size_t p_i = 0; p_i < scaled_points_.size (); ++p_i)
{
f_mat.block<3, 1> (0, p_i) = scaled_points_[p_i].cross (
(*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ()).template cast<double> ();
f_mat.block<3, 1> (3, p_i) = (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ().template cast<double> ();
}
// Compute the covariance matrix C and its 6 eigenvectors (initially complex, move them to a double matrix)
Eigen::Matrix<double, 6, 6> c_mat (f_mat * f_mat.transpose ());
Eigen::EigenSolver<Eigen::Matrix<double, 6, 6> > eigen_solver;
eigen_solver.compute (c_mat, true);
Eigen::MatrixXcd complex_eigenvectors = eigen_solver.eigenvectors ();
Eigen::Matrix<double, 6, 6> x;
for (size_t i = 0; i < 6; ++i)
for (size_t j = 0; j < 6; ++j)
x (i, j) = real (complex_eigenvectors (i, j));
//--- Part B from the paper
/// TODO figure out how to fill the candidate_indices - see subsequent paper paragraphs
std::vector<size_t> candidate_indices;
candidate_indices.resize (indices_->size ());
for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
candidate_indices[p_i] = p_i;
// Compute the v 6-vectors
typedef Eigen::Matrix<double, 6, 1> Vector6d;
std::vector<Vector6d, Eigen::aligned_allocator<Vector6d> > v;
v.resize (candidate_indices.size ());
for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
{
v[p_i].block<3, 1> (0, 0) = scaled_points_[p_i].cross (
(*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ()).template cast<double> ();
v[p_i].block<3, 1> (3, 0) = (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ().template cast<double> ();
}
// Set up the lists to be sorted
std::vector<std::list<std::pair<int, double> > > L;
L.resize (6);
for (size_t i = 0; i < 6; ++i)
{
for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
L[i].push_back (std::make_pair (p_i, fabs (v[p_i].dot (x.block<6, 1> (0, i)))));
// Sort in decreasing order
L[i].sort (sort_dot_list_function);
}
// Initialize the 6 t's
std::vector<double> t (6, 0.0);
sampled_indices.resize (num_samples_);
std::vector<bool> point_sampled (candidate_indices.size (), false);
// Now select the actual points
for (size_t sample_i = 0; sample_i < num_samples_; ++sample_i)
{
// Find the most unconstrained dimension, i.e., the minimum t
size_t min_t_i = 0;
for (size_t i = 0; i < 6; ++i)
{
if (t[min_t_i] > t[i])
min_t_i = i;
}
// Add the point from the top of the list corresponding to the dimension to the set of samples
while (point_sampled [L[min_t_i].front ().first])
L[min_t_i].pop_front ();
sampled_indices[sample_i] = L[min_t_i].front ().first;
point_sampled[L[min_t_i].front ().first] = true;
L[min_t_i].pop_front ();
// Update the running totals
for (size_t i = 0; i < 6; ++i)
{
double val = v[sampled_indices[sample_i]].dot (x.block<6, 1> (0, i));
t[i] += val * val;
}
}
// Remap the sampled_indices to the input_ cloud
for (size_t i = 0; i < sampled_indices.size (); ++i)
sampled_indices[i] = (*indices_)[candidate_indices[sampled_indices[i]]];
}
///////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename PointNT> void
pcl::CovarianceSampling<PointT, PointNT>::applyFilter (Cloud &output)
{
std::vector<int> sampled_indices;
applyFilter (sampled_indices);
output.resize (sampled_indices.size ());
output.header = input_->header;
output.height = 1;
output.width = uint32_t (output.size ());
output.is_dense = true;
for (size_t i = 0; i < sampled_indices.size (); ++i)
output[i] = (*input_)[sampled_indices[i]];
}
#define PCL_INSTANTIATE_CovarianceSampling(T,NT) template class PCL_EXPORTS pcl::CovarianceSampling<T,NT>;
#endif /* PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_ */
pcl::PointCloud::size
size_t size() const
Definition: point_cloud.h:440
pcl::CovarianceSampling::initCompute
bool initCompute()
Definition: covariance_sampling.hpp:50
pcl::CovarianceSampling::applyFilter
void applyFilter(Cloud &output)
Sample of point indices into a separate PointCloud.
Definition: covariance_sampling.hpp:259
pcl::PointCloud::height
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
pcl::FilterIndices
FilterIndices represents the base class for filters that are about binary point removal.
Definition: filter_indices.h:75
pcl::PointCloud::width
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
pcl::computeCovarianceMatrix
unsigned int computeCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Definition: projection_matrix.h:53
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
pcl::PointCloud::resize
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:447
pcl::CovarianceSampling::computeCovarianceMatrix
bool computeCovarianceMatrix(Eigen::Matrix< double, 6, 6 > &covariance_matrix)
Computes the covariance matrix of the input cloud.
Definition: covariance_sampling.hpp:137
pcl::CovarianceSampling::computeConditionNumber
double computeConditionNumber()
Compute the condition number of the input point cloud.
Definition: covariance_sampling.hpp:85