Loading...
Searching...
No Matches
pca.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, 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$
*/
#pragma once
#include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
#include <pcl/common/centroid.h>
#include <pcl/common/eigen.h>
#include <pcl/common/pca.h>
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
#include <pcl/exceptions.h>
namespace pcl
{
template<typename PointT> bool
PCA<PointT>::initCompute ()
{
if(!Base::initCompute ())
{
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] failed");
}
if(indices_->size () < 3)
{
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] number of points < 3");
}
// Compute mean
mean_ = Eigen::Vector4f::Zero ();
compute3DCentroid (*input_, *indices_, mean_);
// Compute demeanished cloud
Eigen::MatrixXf cloud_demean;
demeanPointCloud (*input_, *indices_, mean_, cloud_demean);
assert (cloud_demean.cols () == int (indices_->size ()));
// Compute the product cloud_demean * cloud_demean^T
const Eigen::Matrix3f alpha = (1.f / (static_cast<float>(indices_->size ()) - 1.f))
* cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ();
// Compute eigen vectors and values
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
// Organize eigenvectors and eigenvalues in ascendent order
for (int i = 0; i < 3; ++i)
{
eigenvalues_[i] = evd.eigenvalues () [2-i];
eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
}
// Enforce right hand rule
eigenvectors_.col(2) = eigenvectors_.col(0).cross(eigenvectors_.col(1));
// If not basis only then compute the coefficients
if (!basis_only_)
coefficients_.noalias() = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
compute_done_ = true;
return (true);
}
template<typename PointT> inline void
PCA<PointT>::update (const PointT& input_point, FLAG flag)
{
if (!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");
Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
const std::size_t n = eigenvectors_.cols ();// number of eigen vectors
Eigen::VectorXf meanp = (static_cast<float>(n) * (mean_.head<3>() + input)) / static_cast<float>(n + 1);
Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
Eigen::VectorXf h = y - input;
if (h.norm() > 0)
h.normalize ();
else
h.setZero ();
float gamma = h.dot(input - mean_.head<3>());
Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
D.block(0,0,n,n) = a * a.transpose();
D /= static_cast<float>(n)/static_cast<float>((n+1) * (n+1));
for(std::size_t i=0; i < a.size(); i++) {
D(i,i)+= static_cast<float>(n)/static_cast<float>(n+1)*eigenvalues_(i);
D(D.rows()-1,i) = static_cast<float>(n) / static_cast<float>((n+1) * (n+1)) * gamma * a(i);
D(i,D.cols()-1) = D(D.rows()-1,i);
D(D.rows()-1,D.cols()-1) = static_cast<float>(n)/static_cast<float>((n+1) * (n+1)) * gamma * gamma;
}
Eigen::MatrixXf R(D.rows(), D.cols());
Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
Eigen::VectorXf alphap = D_evd.eigenvalues().real();
eigenvalues_.resize(eigenvalues_.size() +1);
for(std::size_t i=0;i<eigenvalues_.size();i++) {
eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
R.col(i) = D.col(D.cols()-i-1);
}
Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
Up.rightCols<1>() = h;
eigenvectors_ = Up*R;
if (!basis_only_) {
Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
coefficients_(coefficients_.rows()-1,i) = 0;
coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
}
a.resize(a.size()+1);
a(a.size()-1) = 0;
coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
}
mean_.head<3>() = meanp;
switch (flag)
{
case increase:
if (eigenvectors_.rows() >= eigenvectors_.cols())
break;
case preserve:
if (!basis_only_)
coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
eigenvalues_.resize(eigenvalues_.size()-1);
break;
default:
PCL_ERROR("[pcl::PCA] unknown flag\n");
}
}
template<typename PointT> inline void
PCA<PointT>::project (const PointT& input, PointT& projection)
{
if(!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
projection.getVector3fMap ().noalias () = eigenvectors_.transpose() * demean_input;
}
template<typename PointT> inline void
PCA<PointT>::project (const PointCloud& input, PointCloud& projection)
{
if(!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
if (input.is_dense)
{
projection.resize (input.size ());
for (std::size_t i = 0; i < input.size (); ++i)
project (input[i], projection[i]);
}
else
{
PointT p;
for (const auto& pt: input)
{
if (!std::isfinite (pt.x) ||
!std::isfinite (pt.y) ||
!std::isfinite (pt.z))
continue;
project (pt, p);
projection.push_back (p);
}
}
}
template<typename PointT> inline void
PCA<PointT>::reconstruct (const PointT& projection, PointT& input)
{
if(!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed");
input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap ();
input.getVector3fMap ()+= mean_.head<3> ();
}
template<typename PointT> inline void
PCA<PointT>::reconstruct (const PointCloud& projection, PointCloud& input)
{
if(!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed");
if (input.is_dense)
{
input.resize (projection.size ());
for (std::size_t i = 0; i < projection.size (); ++i)
reconstruct (projection[i], input[i]);
}
else
{
PointT p;
for (std::size_t i = 0; i < input.size (); ++i)
{
if (!std::isfinite (input[i].x) ||
!std::isfinite (input[i].y) ||
!std::isfinite (input[i].z))
continue;
reconstruct (projection[i], p);
input.push_back (p);
}
}
}
} // namespace pcl
centroid.h
Define methods for centroid estimation and covariance matrix calculus.
pcl::InitFailedException
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition exceptions.h:197
pcl::PCA::FLAG
FLAG
Updating method flag.
Definition pca.h:78
pcl::PCA::preserve
@ preserve
preserve subspace dimension
Definition pca.h:82
pcl::PCA::increase
@ increase
keep the new basis vectors if possible
Definition pca.h:80
pcl::PCA::project
void project(const PointT &input, PointT &projection)
Project point on the eigenspace.
Definition pca.hpp:166
pcl::PCA::update
void update(const PointT &input, FLAG flag=preserve)
update PCA with a new point
Definition pca.hpp:96
pcl::PCA::PointCloud
typename Base::PointCloud PointCloud
Definition pca.h:65
pcl::PCA::reconstruct
void reconstruct(const PointT &projection, PointT &input)
Reconstruct point from its projection.
Definition pca.hpp:208
pcl::PCLBase::initCompute
bool initCompute()
This method should get called before starting the actual computation.
Definition pcl_base.hpp:138
point_types.h
Defines all the PCL implemented PointT point type structures.
pcl::demeanPointCloud
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > &centroid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
Definition centroid.hpp:933
pcl::compute3DCentroid
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition centroid.hpp:57
pcl
Definition convolution.h:46