Loading...
Searching...
No Matches
centroid.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2009-present, 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$
*
*/
#pragma once
#include <pcl/common/centroid.h>
#include <pcl/conversions.h>
#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <Eigen/Eigenvalues> // for EigenSolver
#include <boost/fusion/algorithm/transformation/filter_if.hpp> // for boost::fusion::filter_if
#include <boost/fusion/algorithm/iteration/for_each.hpp> // for boost::fusion::for_each
#include <boost/mpl/size.hpp> // for boost::mpl::size
namespace pcl
{
template <typename PointT, typename Scalar> inline unsigned int
compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator,
Eigen::Matrix<Scalar, 4, 1> &centroid)
{
Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
unsigned int cp = 0;
// For each point in the cloud
// If the data is dense, we don't need to check for NaN
while (cloud_iterator.isValid ())
{
// Check if the point is invalid
if (pcl::isFinite (*cloud_iterator))
{
accumulator[0] += cloud_iterator->x;
accumulator[1] += cloud_iterator->y;
accumulator[2] += cloud_iterator->z;
++cp;
}
++cloud_iterator;
}
if (cp > 0) {
centroid = accumulator;
centroid /= static_cast<Scalar> (cp);
centroid[3] = 1;
}
return (cp);
}
template <typename PointT, typename Scalar> inline unsigned int
compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
Eigen::Matrix<Scalar, 4, 1> &centroid)
{
if (cloud.empty ())
return (0);
// For each point in the cloud
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
// Initialize to 0
centroid.setZero ();
for (const auto& point: cloud)
{
centroid[0] += point.x;
centroid[1] += point.y;
centroid[2] += point.z;
}
centroid /= static_cast<Scalar> (cloud.size ());
centroid[3] = 1;
return (static_cast<unsigned int> (cloud.size ()));
}
// NaN or Inf values could exist => check for them
unsigned int cp = 0;
Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
for (const auto& point: cloud)
{
// Check if the point is invalid
if (!isFinite (point))
continue;
accumulator[0] += point.x;
accumulator[1] += point.y;
accumulator[2] += point.z;
++cp;
}
if (cp > 0) {
centroid = accumulator;
centroid /= static_cast<Scalar> (cp);
centroid[3] = 1;
}
return (cp);
}
template <typename PointT, typename Scalar> inline unsigned int
compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
const Indices &indices,
Eigen::Matrix<Scalar, 4, 1> &centroid)
{
if (indices.empty ())
return (0);
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
// Initialize to 0
centroid.setZero ();
for (const auto& index : indices)
{
centroid[0] += cloud[index].x;
centroid[1] += cloud[index].y;
centroid[2] += cloud[index].z;
}
centroid /= static_cast<Scalar> (indices.size ());
centroid[3] = 1;
return (static_cast<unsigned int> (indices.size ()));
}
// NaN or Inf values could exist => check for them
Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
unsigned int cp = 0;
for (const auto& index : indices)
{
// Check if the point is invalid
if (!isFinite (cloud [index]))
continue;
accumulator[0] += cloud[index].x;
accumulator[1] += cloud[index].y;
accumulator[2] += cloud[index].z;
++cp;
}
if (cp > 0) {
centroid = accumulator;
centroid /= static_cast<Scalar> (cp);
centroid[3] = 1;
}
return (cp);
}
template <typename PointT, typename Scalar> inline unsigned int
compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
const pcl::PointIndices &indices,
Eigen::Matrix<Scalar, 4, 1> &centroid)
{
return (pcl::compute3DCentroid (cloud, indices.indices, centroid));
}
template <typename PointT, typename Scalar> inline unsigned
computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
const Eigen::Matrix<Scalar, 4, 1> &centroid,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
if (cloud.empty ())
return (0);
unsigned point_count;
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
covariance_matrix.setZero ();
point_count = static_cast<unsigned> (cloud.size ());
// For each point in the cloud
for (const auto& point: cloud)
{
Eigen::Matrix<Scalar, 4, 1> pt;
pt[0] = point.x - centroid[0];
pt[1] = point.y - centroid[1];
pt[2] = point.z - centroid[2];
covariance_matrix (1, 1) += pt.y () * pt.y ();
covariance_matrix (1, 2) += pt.y () * pt.z ();
covariance_matrix (2, 2) += pt.z () * pt.z ();
pt *= pt.x ();
covariance_matrix (0, 0) += pt.x ();
covariance_matrix (0, 1) += pt.y ();
covariance_matrix (0, 2) += pt.z ();
}
}
// NaN or Inf values could exist => check for them
else
{
Eigen::Matrix<Scalar, 3, 3> temp_covariance_matrix;
temp_covariance_matrix.setZero();
point_count = 0;
// For each point in the cloud
for (const auto& point: cloud)
{
// Check if the point is invalid
if (!isFinite (point))
continue;
Eigen::Matrix<Scalar, 4, 1> pt;
pt[0] = point.x - centroid[0];
pt[1] = point.y - centroid[1];
pt[2] = point.z - centroid[2];
temp_covariance_matrix (1, 1) += pt.y () * pt.y ();
temp_covariance_matrix (1, 2) += pt.y () * pt.z ();
temp_covariance_matrix (2, 2) += pt.z () * pt.z ();
pt *= pt.x ();
temp_covariance_matrix (0, 0) += pt.x ();
temp_covariance_matrix (0, 1) += pt.y ();
temp_covariance_matrix (0, 2) += pt.z ();
++point_count;
}
if (point_count > 0) {
covariance_matrix = temp_covariance_matrix;
}
}
if (point_count == 0) {
return 0;
}
covariance_matrix (1, 0) = covariance_matrix (0, 1);
covariance_matrix (2, 0) = covariance_matrix (0, 2);
covariance_matrix (2, 1) = covariance_matrix (1, 2);
return (point_count);
}
template <typename PointT, typename Scalar> inline unsigned int
computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
const Eigen::Matrix<Scalar, 4, 1> &centroid,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
unsigned point_count = pcl::computeCovarianceMatrix (cloud, centroid, covariance_matrix);
if (point_count != 0)
covariance_matrix /= static_cast<Scalar> (point_count);
return (point_count);
}
template <typename PointT, typename Scalar> inline unsigned int
computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
const Indices &indices,
const Eigen::Matrix<Scalar, 4, 1> &centroid,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
if (indices.empty ())
return (0);
std::size_t point_count;
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
covariance_matrix.setZero ();
point_count = indices.size ();
// For each point in the cloud
for (const auto& idx: indices)
{
Eigen::Matrix<Scalar, 4, 1> pt;
pt[0] = cloud[idx].x - centroid[0];
pt[1] = cloud[idx].y - centroid[1];
pt[2] = cloud[idx].z - centroid[2];
covariance_matrix (1, 1) += pt.y () * pt.y ();
covariance_matrix (1, 2) += pt.y () * pt.z ();
covariance_matrix (2, 2) += pt.z () * pt.z ();
pt *= pt.x ();
covariance_matrix (0, 0) += pt.x ();
covariance_matrix (0, 1) += pt.y ();
covariance_matrix (0, 2) += pt.z ();
}
}
// NaN or Inf values could exist => check for them
else
{
Eigen::Matrix<Scalar, 3, 3> temp_covariance_matrix;
temp_covariance_matrix.setZero ();
point_count = 0;
// For each point in the cloud
for (const auto &index : indices)
{
// Check if the point is invalid
if (!isFinite (cloud[index]))
continue;
Eigen::Matrix<Scalar, 4, 1> pt;
pt[0] = cloud[index].x - centroid[0];
pt[1] = cloud[index].y - centroid[1];
pt[2] = cloud[index].z - centroid[2];
temp_covariance_matrix (1, 1) += pt.y () * pt.y ();
temp_covariance_matrix (1, 2) += pt.y () * pt.z ();
temp_covariance_matrix (2, 2) += pt.z () * pt.z ();
pt *= pt.x ();
temp_covariance_matrix (0, 0) += pt.x ();
temp_covariance_matrix (0, 1) += pt.y ();
temp_covariance_matrix (0, 2) += pt.z ();
++point_count;
}
if (point_count > 0) {
covariance_matrix = temp_covariance_matrix;
}
}
if (point_count == 0) {
return 0;
}
covariance_matrix (1, 0) = covariance_matrix (0, 1);
covariance_matrix (2, 0) = covariance_matrix (0, 2);
covariance_matrix (2, 1) = covariance_matrix (1, 2);
return (static_cast<unsigned int> (point_count));
}
template <typename PointT, typename Scalar> inline unsigned int
computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
const pcl::PointIndices &indices,
const Eigen::Matrix<Scalar, 4, 1> &centroid,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
return (pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix));
}
template <typename PointT, typename Scalar> inline unsigned int
computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
const Indices &indices,
const Eigen::Matrix<Scalar, 4, 1> &centroid,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
unsigned point_count = pcl::computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix);
if (point_count != 0)
covariance_matrix /= static_cast<Scalar> (point_count);
return (point_count);
}
template <typename PointT, typename Scalar> inline unsigned int
computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
const pcl::PointIndices &indices,
const Eigen::Matrix<Scalar, 4, 1> &centroid,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
return computeCovarianceMatrixNormalized(cloud, indices.indices, centroid, covariance_matrix);
}
template <typename PointT, typename Scalar> inline unsigned int
computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
// create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
unsigned int point_count;
if (cloud.is_dense)
{
point_count = static_cast<unsigned int> (cloud.size ());
// For each point in the cloud
for (const auto& point: cloud)
{
accu [0] += point.x * point.x;
accu [1] += point.x * point.y;
accu [2] += point.x * point.z;
accu [3] += point.y * point.y;
accu [4] += point.y * point.z;
accu [5] += point.z * point.z;
}
}
else
{
point_count = 0;
for (const auto& point: cloud)
{
if (!isFinite (point))
continue;
accu [0] += point.x * point.x;
accu [1] += point.x * point.y;
accu [2] += point.x * point.z;
accu [3] += point.y * point.y;
accu [4] += point.y * point.z;
accu [5] += point.z * point.z;
++point_count;
}
}
if (point_count != 0)
{
accu /= static_cast<Scalar> (point_count);
covariance_matrix.coeffRef (0) = accu [0];
covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
covariance_matrix.coeffRef (4) = accu [3];
covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
covariance_matrix.coeffRef (8) = accu [5];
}
return (point_count);
}
template <typename PointT, typename Scalar> inline unsigned int
computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
const Indices &indices,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
// create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
unsigned int point_count;
if (cloud.is_dense)
{
point_count = static_cast<unsigned int> (indices.size ());
for (const auto &index : indices)
{
//const PointT& point = cloud[*iIt];
accu [0] += cloud[index].x * cloud[index].x;
accu [1] += cloud[index].x * cloud[index].y;
accu [2] += cloud[index].x * cloud[index].z;
accu [3] += cloud[index].y * cloud[index].y;
accu [4] += cloud[index].y * cloud[index].z;
accu [5] += cloud[index].z * cloud[index].z;
}
}
else
{
point_count = 0;
for (const auto &index : indices)
{
if (!isFinite (cloud[index]))
continue;
++point_count;
accu [0] += cloud[index].x * cloud[index].x;
accu [1] += cloud[index].x * cloud[index].y;
accu [2] += cloud[index].x * cloud[index].z;
accu [3] += cloud[index].y * cloud[index].y;
accu [4] += cloud[index].y * cloud[index].z;
accu [5] += cloud[index].z * cloud[index].z;
}
}
if (point_count != 0)
{
accu /= static_cast<Scalar> (point_count);
covariance_matrix.coeffRef (0) = accu [0];
covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
covariance_matrix.coeffRef (4) = accu [3];
covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
covariance_matrix.coeffRef (8) = accu [5];
}
return (point_count);
}
template <typename PointT, typename Scalar> inline unsigned int
computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
const pcl::PointIndices &indices,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
return (computeCovarianceMatrix (cloud, indices.indices, covariance_matrix));
}
template <typename PointT, typename Scalar> inline unsigned int
computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
Eigen::Matrix<Scalar, 4, 1> &centroid)
{
// Shifted data/with estimate of mean. This gives very good accuracy and good performance.
// create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
Eigen::Matrix<Scalar, 3, 1> K(0.0, 0.0, 0.0);
for(const auto& point: cloud)
if(isFinite(point)) {
K.x() = point.x; K.y() = point.y; K.z() = point.z; break;
}
std::size_t point_count;
if (cloud.is_dense)
{
point_count = cloud.size ();
// For each point in the cloud
for (const auto& point: cloud)
{
Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
accu [0] += x * x;
accu [1] += x * y;
accu [2] += x * z;
accu [3] += y * y;
accu [4] += y * z;
accu [5] += z * z;
accu [6] += x;
accu [7] += y;
accu [8] += z;
}
}
else
{
point_count = 0;
for (const auto& point: cloud)
{
if (!isFinite (point))
continue;
Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
accu [0] += x * x;
accu [1] += x * y;
accu [2] += x * z;
accu [3] += y * y;
accu [4] += y * z;
accu [5] += z * z;
accu [6] += x;
accu [7] += y;
accu [8] += z;
++point_count;
}
}
if (point_count != 0)
{
accu /= static_cast<Scalar> (point_count);
centroid[0] = accu[6] + K.x(); centroid[1] = accu[7] + K.y(); centroid[2] = accu[8] + K.z();//effective mean E[P=(x,y,z)]
centroid[3] = 1;
covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];//(0,0)xx : E[(x-E[x])^2]=E[x^2]-E[x]^2=E[(x-Kx)^2]-E[x-Kx]^2
covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];//(0,1)xy : E[(x-E[x])(y-E[y])]=E[xy]-E[x]E[y]=E[(x-Kx)(y-Ky)]-E[x-Kx]E[y-Ky]
covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];//(0,2)xz
covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];//(1,1)yy
covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];//(1,2)yz
covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];//(2,2)zz
covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); //(1,0)yx
covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); //(2,0)zx
covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); //(2,1)zy
}
return (static_cast<unsigned int> (point_count));
}
template <typename PointT, typename Scalar> inline unsigned int
computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
const Indices &indices,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
Eigen::Matrix<Scalar, 4, 1> &centroid)
{
// Shifted data/with estimate of mean. This gives very good accuracy and good performance.
// create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
Eigen::Matrix<Scalar, 3, 1> K(0.0, 0.0, 0.0);
for(const auto& index : indices)
if(isFinite(cloud[index])) {
K.x() = cloud[index].x; K.y() = cloud[index].y; K.z() = cloud[index].z; break;
}
std::size_t point_count;
if (cloud.is_dense)
{
point_count = indices.size ();
for (const auto &index : indices)
{
Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
accu [0] += x * x;
accu [1] += x * y;
accu [2] += x * z;
accu [3] += y * y;
accu [4] += y * z;
accu [5] += z * z;
accu [6] += x;
accu [7] += y;
accu [8] += z;
}
}
else
{
point_count = 0;
for (const auto &index : indices)
{
if (!isFinite (cloud[index]))
continue;
++point_count;
Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
accu [0] += x * x;
accu [1] += x * y;
accu [2] += x * z;
accu [3] += y * y;
accu [4] += y * z;
accu [5] += z * z;
accu [6] += x;
accu [7] += y;
accu [8] += z;
}
}
if (point_count != 0)
{
accu /= static_cast<Scalar> (point_count);
centroid[0] = accu[6] + K.x(); centroid[1] = accu[7] + K.y(); centroid[2] = accu[8] + K.z();//effective mean E[P=(x,y,z)]
centroid[3] = 1;
covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];//(0,0)xx : E[(x-E[x])^2]=E[x^2]-E[x]^2=E[(x-Kx)^2]-E[x-Kx]^2
covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];//(0,1)xy : E[(x-E[x])(y-E[y])]=E[xy]-E[x]E[y]=E[(x-Kx)(y-Ky)]-E[x-Kx]E[y-Ky]
covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];//(0,2)xz
covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];//(1,1)yy
covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];//(1,2)yz
covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];//(2,2)zz
covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); //(1,0)yx
covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); //(2,0)zx
covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); //(2,1)zy
}
return (static_cast<unsigned int> (point_count));
}
template <typename PointT, typename Scalar> inline unsigned int
computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
const pcl::PointIndices &indices,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
Eigen::Matrix<Scalar, 4, 1> &centroid)
{
return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid));
}
template <typename PointT, typename Scalar> inline unsigned int
computeCentroidAndOBB (const pcl::PointCloud<PointT> &cloud,
Eigen::Matrix<Scalar, 3, 1> &centroid,
Eigen::Matrix<Scalar, 3, 1> &obb_center,
Eigen::Matrix<Scalar, 3, 1> &obb_dimensions,
Eigen::Matrix<Scalar, 3, 3> &obb_rotational_matrix)
{
Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
Eigen::Matrix<Scalar, 4, 1> centroid4;
const auto point_count = computeMeanAndCovarianceMatrix(cloud, covariance_matrix, centroid4);
if (!point_count)
return (0);
centroid(0) = centroid4(0);
centroid(1) = centroid4(1);
centroid(2) = centroid4(2);
const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 3, 3>> evd(covariance_matrix);
const Eigen::Matrix<Scalar, 3, 3> eigenvectors_ = evd.eigenvectors();
const Eigen::Matrix<Scalar, 3, 1> minor_axis = eigenvectors_.col(0);//the eigenvectors do not need to be normalized (they are already)
const Eigen::Matrix<Scalar, 3, 1> middle_axis = eigenvectors_.col(1);
// Enforce right hand rule:
const Eigen::Matrix<Scalar, 3, 1> major_axis = middle_axis.cross(minor_axis);
obb_rotational_matrix <<
major_axis(0), middle_axis(0), minor_axis(0),
major_axis(1), middle_axis(1), minor_axis(1),
major_axis(2), middle_axis(2), minor_axis(2);
//obb_rotational_matrix.col(0)==major_axis
//obb_rotational_matrix.col(1)==middle_axis
//obb_rotational_matrix.col(2)==minor_axis
//Transforming the point cloud in the (Centroid, ma-mi-mi_axis) reference
//with homogeneous matrix
//[R^t , -R^t*Centroid ]
//[0 , 1 ]
Eigen::Matrix<Scalar, 4, 4> transform = Eigen::Matrix<Scalar, 4, 4>::Identity();
transform.template topLeftCorner<3, 3>() = obb_rotational_matrix.transpose();
transform.template topRightCorner<3, 1>().noalias() =-transform.template topLeftCorner<3, 3>()*centroid;
//when Scalar==double on a Windows 10 machine and MSVS:
//if you substitute the following Scalars with floats you get a 20% worse processing time, if with 2 PointT 55% worse
Scalar obb_min_pointx, obb_min_pointy, obb_min_pointz;
Scalar obb_max_pointx, obb_max_pointy, obb_max_pointz;
obb_min_pointx = obb_min_pointy = obb_min_pointz = std::numeric_limits<Scalar>::max();
obb_max_pointx = obb_max_pointy = obb_max_pointz = std::numeric_limits<Scalar>::min();
if (cloud.is_dense)
{
const auto& point = cloud[0];
Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
obb_min_pointx = obb_max_pointx = P(0);
obb_min_pointy = obb_max_pointy = P(1);
obb_min_pointz = obb_max_pointz = P(2);
for (size_t i=1; i<cloud.size();++i)
{
const auto& point = cloud[i];
Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
if (P(0) < obb_min_pointx)
obb_min_pointx = P(0);
else if (P(0) > obb_max_pointx)
obb_max_pointx = P(0);
if (P(1) < obb_min_pointy)
obb_min_pointy = P(1);
else if (P(1) > obb_max_pointy)
obb_max_pointy = P(1);
if (P(2) < obb_min_pointz)
obb_min_pointz = P(2);
else if (P(2) > obb_max_pointz)
obb_max_pointz = P(2);
}
}
else
{
size_t i = 0;
for (; i < cloud.size(); ++i)
{
const auto& point = cloud[i];
if (!isFinite(point))
continue;
Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
obb_min_pointx = obb_max_pointx = P(0);
obb_min_pointy = obb_max_pointy = P(1);
obb_min_pointz = obb_max_pointz = P(2);
++i;
break;
}
for (; i<cloud.size();++i)
{
const auto& point = cloud[i];
if (!isFinite(point))
continue;
Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
if (P(0) < obb_min_pointx)
obb_min_pointx = P(0);
else if (P(0) > obb_max_pointx)
obb_max_pointx = P(0);
if (P(1) < obb_min_pointy)
obb_min_pointy = P(1);
else if (P(1) > obb_max_pointy)
obb_max_pointy = P(1);
if (P(2) < obb_min_pointz)
obb_min_pointz = P(2);
else if (P(2) > obb_max_pointz)
obb_max_pointz = P(2);
}
}
const Eigen::Matrix<Scalar, 3, 1> //shift between point cloud centroid and OBB center (position of the OBB center relative to (p.c.centroid, major_axis, middle_axis, minor_axis))
shift((obb_max_pointx + obb_min_pointx) / 2.0f,
(obb_max_pointy + obb_min_pointy) / 2.0f,
(obb_max_pointz + obb_min_pointz) / 2.0f);
obb_dimensions(0) = obb_max_pointx - obb_min_pointx;
obb_dimensions(1) = obb_max_pointy - obb_min_pointy;
obb_dimensions(2) = obb_max_pointz - obb_min_pointz;
obb_center.noalias() = centroid + obb_rotational_matrix * shift;//position of the OBB center in the same reference Oxyz of the point cloud
return (point_count);
}
template <typename PointT, typename Scalar> inline unsigned int
computeCentroidAndOBB (const pcl::PointCloud<PointT> &cloud,
const Indices &indices,
Eigen::Matrix<Scalar, 3, 1> &centroid,
Eigen::Matrix<Scalar, 3, 1> &obb_center,
Eigen::Matrix<Scalar, 3, 1> &obb_dimensions,
Eigen::Matrix<Scalar, 3, 3> &obb_rotational_matrix)
{
Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
Eigen::Matrix<Scalar, 4, 1> centroid4;
const auto point_count = computeMeanAndCovarianceMatrix(cloud, indices, covariance_matrix, centroid4);
if (!point_count)
return (0);
centroid(0) = centroid4(0);
centroid(1) = centroid4(1);
centroid(2) = centroid4(2);
const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 3, 3>> evd(covariance_matrix);
const Eigen::Matrix<Scalar, 3, 3> eigenvectors_ = evd.eigenvectors();
const Eigen::Matrix<Scalar, 3, 1> minor_axis = eigenvectors_.col(0);//the eigenvectors do not need to be normalized (they are already)
const Eigen::Matrix<Scalar, 3, 1> middle_axis = eigenvectors_.col(1);
// Enforce right hand rule:
const Eigen::Matrix<Scalar, 3, 1> major_axis = middle_axis.cross(minor_axis);
obb_rotational_matrix <<
major_axis(0), middle_axis(0), minor_axis(0),
major_axis(1), middle_axis(1), minor_axis(1),
major_axis(2), middle_axis(2), minor_axis(2);
//obb_rotational_matrix.col(0)==major_axis
//obb_rotational_matrix.col(1)==middle_axis
//obb_rotational_matrix.col(2)==minor_axis
//Transforming the point cloud in the (Centroid, ma-mi-mi_axis) reference
//with homogeneous matrix
//[R^t , -R^t*Centroid ]
//[0 , 1 ]
Eigen::Matrix<Scalar, 4, 4> transform = Eigen::Matrix<Scalar, 4, 4>::Identity();
transform.template topLeftCorner<3, 3>() = obb_rotational_matrix.transpose();
transform.template topRightCorner<3, 1>().noalias() =-transform.template topLeftCorner<3, 3>()*centroid;
//when Scalar==double on a Windows 10 machine and MSVS:
//if you substitute the following Scalars with floats you get a 20% worse processing time, if with 2 PointT 55% worse
Scalar obb_min_pointx, obb_min_pointy, obb_min_pointz;
Scalar obb_max_pointx, obb_max_pointy, obb_max_pointz;
obb_min_pointx = obb_min_pointy = obb_min_pointz = std::numeric_limits<Scalar>::max();
obb_max_pointx = obb_max_pointy = obb_max_pointz = std::numeric_limits<Scalar>::min();
if (cloud.is_dense)
{
const auto& point = cloud[indices[0]];
Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
obb_min_pointx = obb_max_pointx = P(0);
obb_min_pointy = obb_max_pointy = P(1);
obb_min_pointz = obb_max_pointz = P(2);
for (size_t i=1; i<indices.size();++i)
{
const auto & point = cloud[indices[i]];
Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
if (P(0) < obb_min_pointx)
obb_min_pointx = P(0);
else if (P(0) > obb_max_pointx)
obb_max_pointx = P(0);
if (P(1) < obb_min_pointy)
obb_min_pointy = P(1);
else if (P(1) > obb_max_pointy)
obb_max_pointy = P(1);
if (P(2) < obb_min_pointz)
obb_min_pointz = P(2);
else if (P(2) > obb_max_pointz)
obb_max_pointz = P(2);
}
}
else
{
size_t i = 0;
for (; i<indices.size();++i)
{
const auto& point = cloud[indices[i]];
if (!isFinite(point))
continue;
Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
obb_min_pointx = obb_max_pointx = P(0);
obb_min_pointy = obb_max_pointy = P(1);
obb_min_pointz = obb_max_pointz = P(2);
++i;
break;
}
for (; i<indices.size();++i)
{
const auto& point = cloud[indices[i]];
if (!isFinite(point))
continue;
Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
if (P(0) < obb_min_pointx)
obb_min_pointx = P(0);
else if (P(0) > obb_max_pointx)
obb_max_pointx = P(0);
if (P(1) < obb_min_pointy)
obb_min_pointy = P(1);
else if (P(1) > obb_max_pointy)
obb_max_pointy = P(1);
if (P(2) < obb_min_pointz)
obb_min_pointz = P(2);
else if (P(2) > obb_max_pointz)
obb_max_pointz = P(2);
}
}
const Eigen::Matrix<Scalar, 3, 1> //shift between point cloud centroid and OBB center (position of the OBB center relative to (p.c.centroid, major_axis, middle_axis, minor_axis))
shift((obb_max_pointx + obb_min_pointx) / 2.0f,
(obb_max_pointy + obb_min_pointy) / 2.0f,
(obb_max_pointz + obb_min_pointz) / 2.0f);
obb_dimensions(0) = obb_max_pointx - obb_min_pointx;
obb_dimensions(1) = obb_max_pointy - obb_min_pointy;
obb_dimensions(2) = obb_max_pointz - obb_min_pointz;
obb_center.noalias() = centroid + obb_rotational_matrix * shift;//position of the OBB center in the same reference Oxyz of the point cloud
return (point_count);
}
template <typename PointT, typename Scalar> void
demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
const Eigen::Matrix<Scalar, 4, 1> &centroid,
pcl::PointCloud<PointT> &cloud_out,
int npts)
{
// Calculate the number of points if not given
if (npts == 0)
{
while (cloud_iterator.isValid ())
{
++npts;
++cloud_iterator;
}
cloud_iterator.reset ();
}
int i = 0;
cloud_out.resize (npts);
// Subtract the centroid from cloud_in
while (cloud_iterator.isValid ())
{
cloud_out[i].x = cloud_iterator->x - centroid[0];
cloud_out[i].y = cloud_iterator->y - centroid[1];
cloud_out[i].z = cloud_iterator->z - centroid[2];
++i;
++cloud_iterator;
}
cloud_out.width = cloud_out.size ();
cloud_out.height = 1;
}
template <typename PointT, typename Scalar> void
demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const Eigen::Matrix<Scalar, 4, 1> &centroid,
pcl::PointCloud<PointT> &cloud_out)
{
cloud_out = cloud_in;
// Subtract the centroid from cloud_in
for (auto& point: cloud_out)
{
point.x -= static_cast<float> (centroid[0]);
point.y -= static_cast<float> (centroid[1]);
point.z -= static_cast<float> (centroid[2]);
}
}
template <typename PointT, typename Scalar> void
demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const Indices &indices,
const Eigen::Matrix<Scalar, 4, 1> &centroid,
pcl::PointCloud<PointT> &cloud_out)
{
cloud_out.header = cloud_in.header;
cloud_out.is_dense = cloud_in.is_dense;
if (indices.size () == cloud_in.size ())
{
cloud_out.width = cloud_in.width;
cloud_out.height = cloud_in.height;
}
else
{
cloud_out.width = indices.size ();
cloud_out.height = 1;
}
cloud_out.resize (indices.size ());
// Subtract the centroid from cloud_in
for (std::size_t i = 0; i < indices.size (); ++i)
{
cloud_out[i].x = static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
cloud_out[i].y = static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
cloud_out[i].z = static_cast<float> (cloud_in[indices[i]].z - centroid[2]);
}
}
template <typename PointT, typename Scalar> void
demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const pcl::PointIndices& indices,
const Eigen::Matrix<Scalar, 4, 1> &centroid,
pcl::PointCloud<PointT> &cloud_out)
{
return (demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
}
template <typename PointT, typename Scalar> void
demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
const Eigen::Matrix<Scalar, 4, 1> &centroid,
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
int npts)
{
// Calculate the number of points if not given
if (npts == 0)
{
while (cloud_iterator.isValid ())
{
++npts;
++cloud_iterator;
}
cloud_iterator.reset ();
}
cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
int i = 0;
while (cloud_iterator.isValid ())
{
cloud_out (0, i) = cloud_iterator->x - centroid[0];
cloud_out (1, i) = cloud_iterator->y - centroid[1];
cloud_out (2, i) = cloud_iterator->z - centroid[2];
++i;
++cloud_iterator;
}
}
template <typename PointT, typename Scalar> void
demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const Eigen::Matrix<Scalar, 4, 1> &centroid,
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
{
std::size_t npts = cloud_in.size ();
cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
for (std::size_t i = 0; i < npts; ++i)
{
cloud_out (0, i) = cloud_in[i].x - centroid[0];
cloud_out (1, i) = cloud_in[i].y - centroid[1];
cloud_out (2, i) = cloud_in[i].z - centroid[2];
// One column at a time
//cloud_out.block<4, 1> (0, i) = cloud_in[i].getVector4fMap () - centroid;
}
// Make sure we zero the 4th dimension out (1 row, N columns)
//cloud_out.block (3, 0, 1, npts).setZero ();
}
template <typename PointT, typename Scalar> void
demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const Indices &indices,
const Eigen::Matrix<Scalar, 4, 1> &centroid,
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
{
std::size_t npts = indices.size ();
cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
for (std::size_t i = 0; i < npts; ++i)
{
cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0];
cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1];
cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2];
// One column at a time
//cloud_out.block<4, 1> (0, i) = cloud_in[indices[i]].getVector4fMap () - centroid;
}
// Make sure we zero the 4th dimension out (1 row, N columns)
//cloud_out.block (3, 0, 1, npts).setZero ();
}
template <typename PointT, typename Scalar> void
demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const pcl::PointIndices &indices,
const Eigen::Matrix<Scalar, 4, 1> &centroid,
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
{
return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
}
template <typename PointT, typename Scalar> inline void
computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
{
using FieldList = typename pcl::traits::fieldList<PointT>::type;
// Get the size of the fields
centroid.setZero (boost::mpl::size<FieldList>::value);
if (cloud.empty ())
return;
// Iterate over each point
for (const auto& pt: cloud)
{
// Iterate over each dimension
pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (pt, centroid));
}
centroid /= static_cast<Scalar> (cloud.size ());
}
template <typename PointT, typename Scalar> inline void
computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
const Indices &indices,
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
{
using FieldList = typename pcl::traits::fieldList<PointT>::type;
// Get the size of the fields
centroid.setZero (boost::mpl::size<FieldList>::value);
if (indices.empty ())
return;
// Iterate over each point
for (const auto& index: indices)
{
// Iterate over each dimension
pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[index], centroid));
}
centroid /= static_cast<Scalar> (indices.size ());
}
template <typename PointT, typename Scalar> inline void
computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
const pcl::PointIndices &indices,
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
{
return (pcl::computeNDCentroid (cloud, indices.indices, centroid));
}
template <typename PointT> void
CentroidPoint<PointT>::add (const PointT& point)
{
// Invoke add point on each accumulator
boost::fusion::for_each (accumulators_, detail::AddPoint<PointT> (point));
++num_points_;
}
template <typename PointT>
template <typename PointOutT> void
CentroidPoint<PointT>::get (PointOutT& point) const
{
if (num_points_ != 0)
{
// Filter accumulators so that only those that are compatible with
// both PointT and requested point type remain
auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
// Invoke get point on each accumulator in filtered list
boost::fusion::for_each (ca, detail::GetPoint<PointOutT> (point, num_points_));
}
}
template <typename PointInT, typename PointOutT> std::size_t
computeCentroid (const pcl::PointCloud<PointInT>& cloud,
PointOutT& centroid)
{
pcl::CentroidPoint<PointInT> cp;
if (cloud.is_dense)
for (const auto& point: cloud)
cp.add (point);
else
for (const auto& point: cloud)
if (pcl::isFinite (point))
cp.add (point);
cp.get (centroid);
return (cp.getSize ());
}
template <typename PointInT, typename PointOutT> std::size_t
computeCentroid (const pcl::PointCloud<PointInT>& cloud,
const Indices& indices,
PointOutT& centroid)
{
pcl::CentroidPoint<PointInT> cp;
if (cloud.is_dense)
for (const auto &index : indices)
cp.add (cloud[index]);
else
for (const auto &index : indices)
if (pcl::isFinite (cloud[index]))
cp.add (cloud[index]);
cp.get (centroid);
return (cp.getSize ());
}
} // namespace pcl
centroid.h
Define methods for centroid estimation and covariance matrix calculus.
pcl::CentroidPoint
A generic class that computes the centroid of points fed to it.
Definition centroid.h:1077
pcl::CentroidPoint::get
void get(PointOutT &point) const
Retrieve the current centroid.
Definition centroid.hpp:1173
pcl::CentroidPoint::add
void add(const PointT &point)
Add a new point to the centroid computation.
Definition centroid.hpp:1164
pcl::ConstCloudIterator
Iterator class for point clouds with or without given indices.
Definition cloud_iterator.h:121
pcl::ConstCloudIterator::isValid
bool isValid() const
Definition cloud_iterator.hpp:547
pcl::ConstCloudIterator::reset
void reset()
Definition cloud_iterator.hpp:540
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:174
pcl::PointCloud::empty
bool empty() const
Definition point_cloud.h:447
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition point_cloud.h:404
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition point_cloud.h:463
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition point_cloud.h:399
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition point_cloud.h:393
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition point_cloud.h:401
pcl::PointCloud::size
std::size_t size() const
Definition point_cloud.h:444
pcl::computeCentroidAndOBB
unsigned int computeCentroidAndOBB(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 1 > &centroid, Eigen::Matrix< Scalar, 3, 1 > &obb_center, Eigen::Matrix< Scalar, 3, 1 > &obb_dimensions, Eigen::Matrix< Scalar, 3, 3 > &obb_rotational_matrix)
Compute centroid, OBB (Oriented Bounding Box), PCA axes of a given set of points.
Definition centroid.hpp:663
pcl::computeCentroid
std::size_t computeCentroid(const pcl::PointCloud< PointInT > &cloud, PointOutT &centroid)
Compute the centroid of a set of points and return it as a point.
Definition centroid.hpp:1187
pcl::computeNDCentroid
void computeNDCentroid(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &centroid)
General, all purpose nD centroid estimation for a set of points using their indices.
Definition centroid.hpp:1111
pcl::computeMeanAndCovarianceMatrix
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
Definition centroid.hpp:509
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::computeCovarianceMatrixNormalized
unsigned int computeCovarianceMatrixNormalized(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute normalized the 3x3 covariance matrix of a given set of points.
Definition centroid.hpp:269
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.
Definition centroid.hpp:192
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::K
@ K
Definition norms.h:54
pcl
Definition convolution.h:46
pcl::isFinite
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition point_tests.h:55
pcl::for_each_type
void for_each_type(F f)
Definition for_each_type.h:91
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
pcl::NdCentroidFunctor
Helper functor structure for n-D centroid estimation.
Definition centroid.h:898
pcl::PointIndices
Definition PointIndices.h:12
pcl::PointIndices::indices
Indices indices
Definition PointIndices.h:20
pcl::detail::AddPoint
Definition accumulators.hpp:252
pcl::detail::GetPoint
Definition accumulators.hpp:270