template <typename PointSource, typename PointTarget> inline void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const std::vector<int> &indices_tgt,
Eigen::Matrix4f &transformation_matrix)
{
if (indices_src.size () != indices_tgt.size ())
{
PCL_ERROR ("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)indices_tgt.size ());
return;
}
// <cloud_src,cloud_src> is the source dataset
transformation_matrix.setIdentity ();
Eigen::Vector4f centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid (cloud_src, indices_src, centroid_src);
compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt);
// Subtract the centroids from source, target
Eigen::MatrixXf cloud_src_demean;
demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);
Eigen::MatrixXf cloud_tgt_demean;
demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean);
getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
}
template <typename PointSource, typename PointTarget> inline void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
Eigen::Matrix4f &transformation_matrix)
{
std::vector<int> indices_src, indices_tgt;
pcl::registration::getQueryIndices (correspondences, indices_src);
pcl::registration::getMatchIndices (correspondences, indices_tgt);
// <cloud_src,cloud_src> is the source dataset
Eigen::Vector4f centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid (cloud_src, indices_src, centroid_src);
compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt);
// Subtract the centroids from source, target
Eigen::MatrixXf cloud_src_demean;
demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);
Eigen::MatrixXf cloud_tgt_demean;
demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean);
getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
Matrix4 &transformation_matrix) const
{
// Convert to Eigen format
const int npts = static_cast <int> (source_it.size ());
Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_src (3, npts);
Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_tgt (3, npts);
for (int i = 0; i < npts; ++i)
{
cloud_src (0, i) = source_it->x;
cloud_src (1, i) = source_it->y;
cloud_src (2, i) = source_it->z;
++source_it;
cloud_tgt (0, i) = target_it->x;
cloud_tgt (1, i) = target_it->y;
cloud_tgt (2, i) = target_it->z;
++target_it;
}
if (use_umeyama_)
{
// Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false);
}
else
{
source_it.reset (); target_it.reset ();
// <cloud_src,cloud_src> is the source dataset
transformation_matrix.setIdentity ();
Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid (source_it, centroid_src);
compute3DCentroid (target_it, centroid_tgt);
source_it.reset (); target_it.reset ();
// Subtract the centroids from source, target
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
demeanPointCloud (source_it, centroid_src, cloud_src_demean);
demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean);
getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
}
}
template <typename PointInT, typename PointOutT> void
pcl::MomentInvariantsEstimation<PointInT, PointOutT>::computePointMomentInvariants (
const pcl::PointCloud<PointInT> &cloud, float &j1, float &j2, float &j3)
{
// Estimate the XYZ centroid
compute3DCentroid (cloud, xyz_centroid_);
// Initalize the centralized moments
float mu200 = 0, mu020 = 0, mu002 = 0, mu110 = 0, mu101 = 0, mu011 = 0;
// Iterate over the nearest neighbors set
for (size_t nn_idx = 0; nn_idx < cloud.points.size (); ++nn_idx )
{
// Demean the points
temp_pt_[0] = cloud.points[nn_idx].x - xyz_centroid_[0];
temp_pt_[1] = cloud.points[nn_idx].y - xyz_centroid_[1];
temp_pt_[2] = cloud.points[nn_idx].z - xyz_centroid_[2];
mu200 += temp_pt_[0] * temp_pt_[0];
mu020 += temp_pt_[1] * temp_pt_[1];
mu002 += temp_pt_[2] * temp_pt_[2];
mu110 += temp_pt_[0] * temp_pt_[1];
mu101 += temp_pt_[0] * temp_pt_[2];
mu011 += temp_pt_[1] * temp_pt_[2];
}
// Save the moment invariants
j1 = mu200 + mu020 + mu002;
j2 = mu200*mu020 + mu200*mu002 + mu020*mu002 - mu110*mu110 - mu101*mu101 - mu011*mu011;
j3 = mu200*mu020*mu002 + 2*mu110*mu101*mu011 - mu002*mu110*mu110 - mu020*mu101*mu101 - mu200*mu011*mu011;
}
template <typename PointInT, typename PointOutT> void
pcl::TBB_NormalEstimationTBB<PointInT, PointOutT>::operator () (const tbb::blocked_range <size_t> &r) const
{
float vpx, vpy, vpz;
feature_->getViewPoint (vpx, vpy, vpz);
// Iterating over the entire index vector
for (size_t idx = r.begin (); idx != r.end (); ++idx)
{
std::vector<int> nn_indices (feature_->getKSearch ());
std::vector<float> nn_dists (feature_->getKSearch ());
feature_->searchForNeighbors ((*feature_->getIndices ())[idx], feature_->getSearchParameter (), nn_indices, nn_dists);
// 16-bytes aligned placeholder for the XYZ centroid of a surface patch
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (*feature_->getSearchSurface (), nn_indices, xyz_centroid);
// Placeholder for the 3x3 covariance matrix at each surface patch
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (*feature_->getSearchSurface (), nn_indices, xyz_centroid, covariance_matrix);
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix,
output_.points[idx].normal[0], output_.points[idx].normal[1], output_.points[idx].normal[2], output_.points[idx].curvature);
flipNormalTowardsViewpoint<PointInT> (feature_->getSearchSurface ()->points[idx], vpx, vpy, vpz,
output_.points[idx].normal[0], output_.points[idx].normal[1], output_.points[idx].normal[2]);
}
}
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::estimateRigidTransformationSVD (
const pcl::PointCloud<PointT> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointT> &cloud_tgt,
const std::vector<int> &indices_tgt,
Eigen::VectorXf &transform)
{
transform.resize (16);
Eigen::Vector4f centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid (cloud_src, indices_src, centroid_src);
compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt);
// Subtract the centroids from source, target
Eigen::MatrixXf cloud_src_demean;
demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);
Eigen::MatrixXf cloud_tgt_demean;
demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean);
// Assemble the correlation matrix H = source * target'
Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
// Compute the Singular Value Decomposition
Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f u = svd.matrixU ();
Eigen::Matrix3f v = svd.matrixV ();
// Compute R = V * U'
if (u.determinant () * v.determinant () < 0)
{
for (int x = 0; x < 3; ++x)
v (x, 2) *= -1;
}
Eigen::Matrix3f R = v * u.transpose ();
// Return the correct transformation
transform.segment<3> (0) = R.row (0); transform[12] = 0;
transform.segment<3> (4) = R.row (1); transform[13] = 0;
transform.segment<3> (8) = R.row (2); transform[14] = 0;
Eigen::Vector3f t = centroid_tgt.head<3> () - R * centroid_src.head<3> ();
transform[3] = t[0]; transform[7] = t[1]; transform[11] = t[2]; transform[15] = 1.0;
}
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationTranslation<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
Matrix4 &transformation_matrix) const
{
source_it.reset (); target_it.reset ();
// <cloud_src,cloud_src> is the source dataset
transformation_matrix.setIdentity ();
Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid (source_it, centroid_src);
compute3DCentroid (target_it, centroid_tgt);
source_it.reset (); target_it.reset ();
getTransformationFromCorrelation (centroid_src, centroid_tgt, transformation_matrix);
}
template <typename PointSource, typename PointTarget> inline void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Eigen::Matrix4f &transformation_matrix)
{
// <cloud_src,cloud_src> is the source dataset
transformation_matrix.setIdentity ();
Eigen::Vector4f centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid (cloud_src, centroid_src);
compute3DCentroid (cloud_tgt, centroid_tgt);
// Subtract the centroids from source, target
Eigen::MatrixXf cloud_src_demean;
demeanPointCloud (cloud_src, centroid_src, cloud_src_demean);
Eigen::MatrixXf cloud_tgt_demean;
demeanPointCloud (cloud_tgt, centroid_tgt, cloud_tgt_demean);
getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
Matrix4 &transformation_matrix) const
{
source_it.reset (); target_it.reset ();
Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid (source_it, centroid_src);
compute3DCentroid (target_it, centroid_tgt);
source_it.reset (); target_it.reset ();
// ignore z component
centroid_src[2] = 0.0f;
centroid_tgt[2] = 0.0f;
// Subtract the centroids from source, target
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
demeanPointCloud (source_it, centroid_src, cloud_src_demean);
demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean);
getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
}
template <typename PointInT, typename PointNT> void
pcl::IntensityGradientEstimation<PointInT, PointNT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
{
// Resize the output dataset
output.points.resize (indices_->size (), 3);
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
output.is_dense = true;
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points.row (idx).setConstant (std::numeric_limits<float>::quiet_NaN ());
output.is_dense = false;
continue;
}
Eigen::Vector4f centroid;
compute3DCentroid (*surface_, nn_indices, centroid);
float mean_intensity = 0;
unsigned valid_neighbor_count = 0;
for (size_t nIdx = 0; nIdx < nn_indices.size (); ++nIdx)
{
const PointInT& p = (*surface_)[nn_indices[nIdx]];
if (!pcl_isfinite (p.intensity))
continue;
mean_intensity += p.intensity;
++valid_neighbor_count;
}
mean_intensity /= static_cast<float> (valid_neighbor_count);
Eigen::Vector3f normal = Eigen::Vector3f::Map (normals_->points[idx].normal);
Eigen::Vector3f gradient;
this->computePointIntensityGradient (*surface_, nn_indices, centroid.head<3> (), mean_intensity, normal, gradient);
output.points (idx, 0) = gradient[0];
output.points (idx, 1) = gradient[1];
output.points (idx, 2) = gradient[2];
}
}
template <typename PointInT> void
pcl16::NormalEstimationOMP<PointInT, Eigen::MatrixXf>::computeFeatureEigen (pcl16::PointCloud<Eigen::MatrixXf> &output)
{
float vpx, vpy, vpz;
getViewPoint (vpx, vpy, vpz);
output.is_dense = true;
// Resize the output dataset
output.points.resize (indices_->size (), 4);
// GCC 4.2.x seems to segfault with "internal compiler error" on MacOS X here
#if defined(_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2))
#pragma omp parallel for schedule (dynamic, threads_)
#endif
// Iterating over the entire index vector
for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
{
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points (idx, 0) = output.points (idx, 1) = output.points (idx, 2) = output.points (idx, 3) = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// 16-bytes aligned placeholder for the XYZ centroid of a surface patch
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (*surface_, nn_indices, xyz_centroid);
// Placeholder for the 3x3 covariance matrix at each surface patch
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (*surface_, nn_indices, xyz_centroid, covariance_matrix);
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix,
output.points (idx, 0), output.points (idx, 1), output.points (idx, 2), output.points (idx, 3));
flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx, vpy, vpz,
output.points (idx, 0), output.points (idx, 1), output.points (idx, 2));
}
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::IntensityGradientEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
output.is_dense = true;
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
PointOutT &p_out = output.points[idx];
if (!this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
{
p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
Eigen::Vector4f centroid;
compute3DCentroid (*surface_, nn_indices, centroid);
float mean_intensity = 0;
unsigned valid_neighbor_count = 0;
for (size_t nIdx = 0; nIdx < nn_indices.size (); ++nIdx)
{
const PointInT& p = (*surface_)[nn_indices[nIdx]];
if (!pcl_isfinite (p.intensity))
continue;
mean_intensity += p.intensity;
++valid_neighbor_count;
}
mean_intensity /= (float)valid_neighbor_count;
Eigen::Vector3f normal = Eigen::Vector3f::Map (normals_->points[idx].normal);
Eigen::Vector3f gradient;
computePointIntensityGradient (*surface_, nn_indices, centroid.head<3> (), mean_intensity, normal, gradient);
p_out.gradient[0] = gradient[0];
p_out.gradient[1] = gradient[1];
p_out.gradient[2] = gradient[2];
}
}
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::optimizeModelCoefficients (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 4)
{
PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
optimized_coefficients = model_coefficients;
return;
}
// Need at least 3 points to estimate a plane
if (inliers.size () < 4)
{
PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", (unsigned long)inliers.size ());
optimized_coefficients = model_coefficients;
return;
}
Eigen::Vector4f plane_parameters;
// Use Least-Squares to fit the plane through all the given sample points and find out its coefficients
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (*input_, inliers, xyz_centroid);
xyz_centroid[3] = 0;
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (*input_, inliers, xyz_centroid, covariance_matrix);
// Compute the model coefficients
EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
// Hessian form (D = nc . p_plane (centroid here) + p)
optimized_coefficients.resize (4);
optimized_coefficients[0] = eigen_vectors (0, 0);
optimized_coefficients[1] = eigen_vectors (1, 0);
optimized_coefficients[2] = eigen_vectors (2, 0);
optimized_coefficients[3] = 0;
optimized_coefficients[3] = -1 * optimized_coefficients.dot (xyz_centroid);
}
template <typename PointInT, typename PointOutT> void
pcl16::NormalEstimationOMP<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
float vpx, vpy, vpz;
getViewPoint (vpx, vpy, vpz);
output.is_dense = true;
// Iterating over the entire index vector
#pragma omp parallel for schedule (dynamic, threads_)
for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
{
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// 16-bytes aligned placeholder for the XYZ centroid of a surface patch
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (*surface_, nn_indices, xyz_centroid);
// Placeholder for the 3x3 covariance matrix at each surface patch
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (*surface_, nn_indices, xyz_centroid, covariance_matrix);
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);
flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx, vpy, vpz,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
}
}
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::optimizeModelCoefficients (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
{
optimized_coefficients = model_coefficients;
return;
}
// Need at least 2 points to estimate a line
if (inliers.size () <= 2)
{
PCL_ERROR ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ());
optimized_coefficients = model_coefficients;
return;
}
optimized_coefficients.resize (6);
// Compute the 3x3 covariance matrix
Eigen::Vector4f centroid;
compute3DCentroid (*input_, inliers, centroid);
Eigen::Matrix3f covariance_matrix;
computeCovarianceMatrix (*input_, inliers, centroid, covariance_matrix);
optimized_coefficients[0] = centroid[0];
optimized_coefficients[1] = centroid[1];
optimized_coefficients[2] = centroid[2];
// Extract the eigenvalues and eigenvectors
EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
pcl::eigen33 (covariance_matrix, eigen_values);
pcl::computeCorrespondingEigenVector (covariance_matrix, eigen_values [2], eigen_vector);
//pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
//optimized_coefficients.template tail<3> () = eigen_vector;
optimized_coefficients[0] = eigen_vector[0];
optimized_coefficients[1] = eigen_vector[1];
optimized_coefficients[2] = eigen_vector[2];
}
template<typename PointT> bool
pcl::PCA<PointT>::initCompute ()
{
if(!Base::initCompute ())
{
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] failed");
return (false);
}
if(indices_->size () < 3)
{
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] number of points < 3");
return (false);
}
// 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
Eigen::Matrix3f alpha = static_cast<Eigen::Matrix3f> (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);
}
// If not basis only then compute the coefficients
if (!basis_only_)
coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
compute_done_ = true;
return (true);
}
int main(int argc, char **argv){
if (argc < 2){
printf("Error: Debe especificar un archivo .pcd\n");
exit(1);
}
// Cargar nube de puntos
PointCloud::Ptr cloud (new PointCloud);
pcl::io::loadPCDFile(argv[1],*cloud);
// Crear visualizador
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = simpleVis(cloud);
// Segmentar
segmentator.setOptimizeCoefficients(true);
segmentator.setModelType(pcl::SACMODEL_PLANE);
segmentator.setMethodType(pcl::SAC_RANSAC);
segmentator.setDistanceThreshold(SEG_THRESHOLD);
segmentator.setInputCloud(cloud);
pcl::ModelCoefficients::Ptr coefs (new pcl::ModelCoefficients());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices());
segmentator.segment(*inliers,*coefs);
// Ver qué se obtuvo
printf("Se obtienen %d puntos\n",(int)inliers->indices.size());
if (inliers->indices.size() == 0){
printf("Segmentación no sirvió de nada.\n");
exit(1);
}
viewer->addPlane(*coefs, "planito");
// Obtener centroide y dibujarlo
Eigen::Vector4f centroid;
compute3DCentroid(*cloud,centroid);
viewer->addSphere(pcl::PointXYZ(centroid(0),centroid(1),centroid(2)), 0.2, "centroide");
// Dibujar un plano en el viewer para indicar más menos qué se obtuvo
// http://docs.pointclouds.org/trunk/classpcl_1_1visualization_1_1_p_c_l_visualizer.html
while (!viewer->wasStopped()){
viewer->spinOnce(100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// ---[ Step 1a : compute the centroid in XYZ space
Eigen::Vector4f xyz_centroid;
if (use_given_centroid_)
xyz_centroid = centroid_to_use_;
else
compute3DCentroid (*surface_, *indices_, xyz_centroid); // Estimate the XYZ centroid
// ---[ Step 1b : compute the centroid in normal space
Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero ();
int cp = 0;
// If the data is dense, we don't need to check for NaN
if (use_given_normal_)
normal_centroid = normal_to_use_;
else
{
if (normals_->is_dense)
{
for (size_t i = 0; i < indices_->size (); ++i)
{
normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
cp++;
}
}
// NaN or Inf values could exist => check for them
else
{
for (size_t i = 0; i < indices_->size (); ++i)
{
if (!pcl_isfinite (normals_->points[(*indices_)[i]].normal[0])
||
!pcl_isfinite (normals_->points[(*indices_)[i]].normal[1])
||
!pcl_isfinite (normals_->points[(*indices_)[i]].normal[2]))
continue;
normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
cp++;
}
}
normal_centroid /= cp;
}
// Compute the direction of view from the viewpoint to the centroid
Eigen::Vector4f viewpoint (vpx_, vpy_, vpz_, 0);
Eigen::Vector4f d_vp_p = viewpoint - xyz_centroid;
d_vp_p.normalize ();
// Estimate the SPFH at nn_indices[0] using the entire cloud
computePointSPFHSignature (xyz_centroid, normal_centroid, *surface_, *normals_, *indices_);
// We only output _1_ signature
output.points.resize (1);
output.width = 1;
output.height = 1;
// Estimate the FPFH at nn_indices[0] using the entire cloud and copy the resultant signature
for (int d = 0; d < hist_f1_.size (); ++d)
output.points[0].histogram[d + 0] = hist_f1_[d];
size_t data_size = hist_f1_.size ();
for (int d = 0; d < hist_f2_.size (); ++d)
output.points[0].histogram[d + data_size] = hist_f2_[d];
data_size += hist_f2_.size ();
for (int d = 0; d < hist_f3_.size (); ++d)
output.points[0].histogram[d + data_size] = hist_f3_[d];
data_size += hist_f3_.size ();
for (int d = 0; d < hist_f4_.size (); ++d)
output.points[0].histogram[d + data_size] = hist_f4_[d];
// ---[ Step 2 : obtain the viewpoint component
hist_vp_.setZero (nr_bins_vp_);
double hist_incr;
if (normalize_bins_)
hist_incr = 100.0 / (double)(indices_->size ());
else
hist_incr = 1.0;
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f normal (normals_->points[(*indices_)[i]].normal[0],
normals_->points[(*indices_)[i]].normal[1],
normals_->points[(*indices_)[i]].normal[2], 0);
// Normalize
double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5;
int fi = floor (alpha * hist_vp_.size ());
if (fi < 0)
fi = 0;
if (fi > ((int)hist_vp_.size () - 1))
fi = hist_vp_.size () - 1;
// Bin into the histogram
hist_vp_ [fi] += hist_incr;
}
data_size += hist_f4_.size ();
// Copy the resultant signature
for (int d = 0; d < hist_vp_.size (); ++d)
output.points[0].histogram[d + data_size] = hist_vp_[d];
}
void
NormalHoughProposer::houghVote(Entry &query, Entry &target, bin_t& bins)
{
// Compute the reference point for the R-table
Eigen::Vector4f centroid4;
compute3DCentroid(*(target.cloud), centroid4);
Eigen::Vector3f centroid(centroid4[0], centroid4[1], centroid4[2]);
assert(query.keypoints->size() == query.features->size());
assert(target.keypoints->size() == target.features->size());
// Figure out bin dimension
Eigen::Vector4f query_min4, query_max4;
getMinMax3D(*query.cloud, query_min4, query_max4);
Eigen::Vector3f query_min(query_min4[0], query_min4[1], query_min4[2]);
Eigen::Vector3f query_max(query_max4[0], query_max4[1], query_max4[2]);
Eigen::Affine3f t;
getTransformation(0, 0, 0, M_PI, 0.5, 1.5, t);
int correctly_matched = 0;
for (unsigned int i = 0; i < query.keypoints->size(); i++)
{
std::vector<int> feature_indices;
std::vector<float> sqr_distances;
int num_correspondences = 2;
if (!pcl_isfinite (query.features->points.row(i)(0)))
continue;
int num_found = target.tree->nearestKSearch(*query.features, i, num_correspondences, feature_indices, sqr_distances);
for (int j = 0; j < num_found; j++)
{
// For each one of the feature correspondences
int feature_index = feature_indices[j];
Eigen::Vector3f query_keypoint = query.keypoints->at(i).getVector3fMap();
Eigen::Vector3f target_keypoint = target.keypoints->at(feature_index).getVector3fMap();
target_keypoint = t * target_keypoint;
if ((query_keypoint - target_keypoint).norm() < 0.05)
{
correctly_matched++;
}
// Get corresponding target keypoint, and calculate its r to its centroid
PointNormal correspondence = target.keypoints->at(feature_index); // Since features correspond to the keypoints
Eigen::Vector3f r = correspondence.getVector3fMap() - centroid;
// Calculate the rotation transformation from the target normal to the query normal
Eigen::Vector3f target_normal = correspondence.getNormalVector3fMap();
target_normal.normalize();
Eigen::Vector3f query_normal = query.keypoints->at(i).getNormalVector3fMap();
query_normal.normalize();
double angle = acos( target_normal.dot(query_normal) / (target_normal.norm() * query_normal.norm()) );
Eigen::Vector3f axis = target_normal.normalized().cross(query_normal.normalized());
axis.normalize();
Eigen::Affine3f rot_transform;
rot_transform = Eigen::AngleAxisf (angle, axis);
// Check that the rotation matrix is correct
Eigen::Vector3f projected = rot_transform * target_normal;
projected.normalize();
// Transform r based on the difference between the normals
Eigen::Vector3f transformed_r = rot_transform * r;
for (int k = 0; k < num_angles_; k++)
{
float query_angle = (float(k) / num_angles_) * 2.0f * float (M_PI);
Eigen::Affine3f query_rot;
query_rot = Eigen::AngleAxisf(query_angle, query_normal.normalized());
Eigen::Vector3f guess_r = query_rot * transformed_r;
Eigen::Vector3f centroid_est = query.keypoints->at(i).getVector3fMap() - guess_r;
Eigen::Vector3f region = query_max - query_min;
Eigen::Vector3f bin_size = region / float (bins_);
Eigen::Vector3f diff = (centroid_est - query_min);
Eigen::Vector3f indices = diff.cwiseQuotient(bin_size);
castVotes(indices, bins);
}
}
}
}
void SetTgtPoints(std::vector<Point3d>& out_cloud)
{
compute3DCentroid(out_cloud, centroid_tgt);
demeanPointCloud(out_cloud, centroid_tgt, cloud_tgt_demean);
}
void SetRefPoints(std::vector<Point3d>& in_cloud)
{
compute3DCentroid(in_cloud, centroid_ref);
demeanPointCloud(in_cloud, centroid_ref, cloud_ref_demean);
}
template <typename PointInT> void
pcl::ConvexHull<PointInT>::performReconstruction (PointCloud &hull, std::vector<pcl::Vertices> &polygons,
bool fill_polygon_data)
{
// FInd the principal directions
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
compute3DCentroid (*input_, *indices_, xyz_centroid);
computeCovarianceMatrix (*input_, *indices_, xyz_centroid, covariance_matrix);
EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
Eigen::Affine3f transform1;
transform1.setIdentity ();
int dim = 3;
if (eigen_values[0] / eigen_values[2] < 1.0e-5)
{
// We have points laying on a plane, using 2d convex hull
// Compute transformation bring eigen_vectors.col(i) to z-axis
eigen_vectors.col (2) = eigen_vectors.col (0).cross (eigen_vectors.col (1));
eigen_vectors.col (1) = eigen_vectors.col (2).cross (eigen_vectors.col (0));
transform1 (0, 2) = eigen_vectors (0, 0);
transform1 (1, 2) = eigen_vectors (1, 0);
transform1 (2, 2) = eigen_vectors (2, 0);
transform1 (0, 1) = eigen_vectors (0, 1);
transform1 (1, 1) = eigen_vectors (1, 1);
transform1 (2, 1) = eigen_vectors (2, 1);
transform1 (0, 0) = eigen_vectors (0, 2);
transform1 (1, 0) = eigen_vectors (1, 2);
transform1 (2, 0) = eigen_vectors (2, 2);
transform1 = transform1.inverse ();
dim = 2;
}
else
transform1.setIdentity ();
PointCloud cloud_transformed;
pcl::demeanPointCloud (*input_, *indices_, xyz_centroid, cloud_transformed);
pcl::transformPointCloud (cloud_transformed, cloud_transformed, transform1);
// True if qhull should free points in qh_freeqhull() or reallocation
boolT ismalloc = True;
// option flags for qhull, see qh_opt.htm
char flags[] = "qhull Tc";
// output from qh_produce_output(), use NULL to skip qh_produce_output()
FILE *outfile = NULL;
// error messages from qhull code
FILE *errfile = stderr;
// 0 if no error from qhull
int exitcode;
// Array of coordinates for each point
coordT *points = (coordT *)calloc (cloud_transformed.points.size () * dim, sizeof(coordT));
for (size_t i = 0; i < cloud_transformed.points.size (); ++i)
{
points[i * dim + 0] = (coordT)cloud_transformed.points[i].x;
points[i * dim + 1] = (coordT)cloud_transformed.points[i].y;
if (dim > 2)
points[i * dim + 2] = (coordT)cloud_transformed.points[i].z;
}
// Compute convex hull
exitcode = qh_new_qhull (dim, cloud_transformed.points.size (), points, ismalloc, flags, outfile, errfile);
if (exitcode != 0)
{
PCL_ERROR ("[pcl::%s::performReconstrution] ERROR: qhull was unable to compute a convex hull for the given point cloud (%lu)!\n",
getClassName ().c_str (), (unsigned long) input_->points.size ());
//check if it fails because of NaN values...
if (!cloud_transformed.is_dense)
{
bool NaNvalues = false;
for (size_t i = 0; i < cloud_transformed.size (); ++i)
{
if (!pcl_isfinite (cloud_transformed.points[i].x) ||
!pcl_isfinite (cloud_transformed.points[i].y) ||
!pcl_isfinite (cloud_transformed.points[i].z))
{
NaNvalues = true;
break;
}
}
if (NaNvalues)
PCL_ERROR ("[pcl::%s::performReconstruction] ERROR: point cloud contains NaN values, consider running pcl::PassThrough filter first to remove NaNs!\n", getClassName ().c_str ());
}
hull.points.resize (0);
hull.width = hull.height = 0;
polygons.resize (0);
qh_freeqhull (!qh_ALL);
int curlong, totlong;
qh_memfreeshort (&curlong, &totlong);
return;
}
qh_triangulate ();
int num_facets = qh num_facets;
int num_vertices = qh num_vertices;
hull.points.resize (num_vertices);
vertexT * vertex;
int i = 0;
// Max vertex id
int max_vertex_id = -1;
FORALLvertices
{
if ((int)vertex->id > max_vertex_id)
max_vertex_id = vertex->id;
}
++max_vertex_id;
std::vector<int> qhid_to_pcidx (max_vertex_id);
FORALLvertices
{
// Add vertices to hull point_cloud
hull.points[i].x = vertex->point[0];
hull.points[i].y = vertex->point[1];
if (dim>2)
hull.points[i].z = vertex->point[2];
else
hull.points[i].z = 0;
qhid_to_pcidx[vertex->id] = i; // map the vertex id of qhull to the point cloud index
++i;
}
if (fill_polygon_data)
{
if (dim == 3)
{
polygons.resize (num_facets);
int dd = 0;
facetT * facet;
FORALLfacets
{
polygons[dd].vertices.resize (3);
// Needed by FOREACHvertex_i_
int vertex_n, vertex_i;
FOREACHvertex_i_((*facet).vertices)
//facet_vertices.vertices.push_back (qhid_to_pcidx[vertex->id]);
polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
++dd;
}
}
else
{
template <typename PointT> void
pcl::ExtractPolygonalPrismDataDebug<PointT>::segment (pcl::PointIndices &output)
{
output.header = input_->header;
if (!initCompute ())
{
output.indices.clear ();
return;
}
if ((int)planar_hull_->points.size () < min_pts_hull_)
{
PCL_ERROR ("[pcl::%s::segment] Not enough points (%lu) in the hull!\n", getClassName ().c_str (), (unsigned long)planar_hull_->points.size ());
output.indices.clear ();
return;
}
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (*planar_hull_, xyz_centroid);
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (*planar_hull_, xyz_centroid, covariance_matrix);
// Compute the model coefficients
EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
eigen33 (covariance_matrix, eigen_vectors, eigen_values);
model_coefficients[0] = eigen_vectors (0, 0);
model_coefficients[1] = eigen_vectors (1, 0);
model_coefficients[2] = eigen_vectors (2, 0);
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
// Need to flip the plane normal towards the viewpoint
Eigen::Vector4f vp (vpx_, vpy_, vpz_, 0);
// See if we need to flip any plane normals
vp -= planar_hull_->points[0].getVector4fMap ();
vp[3] = 0;
// Dot product between the (viewpoint - point) and the plane normal
float cos_theta = vp.dot (model_coefficients);
// Flip the plane normal
if (cos_theta < 0)
{
model_coefficients *= -1;
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
model_coefficients[3] = -1 * (model_coefficients.dot (planar_hull_->points[0].getVector4fMap ()));
}
// Project all points
PointCloud projected_points;
SampleConsensusModelPlane<PointT> sacmodel (input_);
sacmodel.projectPoints (*indices_, model_coefficients, projected_points, false);
// Create a X-Y projected representation for within bounds polygonal checking
int k0, k1, k2;
// Determine the best plane to project points onto
k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1;
k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
k1 = (k0 + 1) % 3;
k2 = (k0 + 2) % 3;
// Project the convex hull
pcl::PointCloud<PointT> polygon;
polygon.points.resize (planar_hull_->points.size ());
for (size_t i = 0; i < planar_hull_->points.size (); ++i)
{
Eigen::Vector4f pt (planar_hull_->points[i].x, planar_hull_->points[i].y, planar_hull_->points[i].z, 0);
polygon.points[i].x = pt[k1];
polygon.points[i].y = pt[k2];
polygon.points[i].z = 0;
}
PointT pt_xy;
pt_xy.z = 0;
output.indices.resize (indices_->size ());
int l = 0;
int count_inside = 0;
int count_dist = 0;
for (size_t i = 0; i < projected_points.points.size (); ++i)
{
// Check the distance to the user imposed limits from the table planar model
double distance = pointToPlaneDistanceSigned (input_->points[(*indices_)[i]], model_coefficients);
if (distance < height_limit_min_ || distance > height_limit_max_){
count_dist++;
continue;
}
// Check what points are inside the hull
Eigen::Vector4f pt (projected_points.points[i].x,
projected_points.points[i].y,
projected_points.points[i].z, 0);
pt_xy.x = pt[k1];
pt_xy.y = pt[k2];
if (!pcl::isXYPointIn2DXYPolygon(pt_xy, polygon, vertices_)){
count_inside++;
continue;
}
output.indices[l++] = (*indices_)[i];
}
output.indices.resize (l);
std::cout << count_dist << " Points not complying with height limits " << count_inside << "Points not inside 2D polygon" << std::endl;
deinitCompute ();
}
template <typename PointT> bool
pcl::isPointIn2DPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon)
{
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (polygon, xyz_centroid);
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (polygon, xyz_centroid, covariance_matrix);
// Compute the model coefficients
EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
eigen33 (covariance_matrix, eigen_vectors, eigen_values);
model_coefficients[0] = eigen_vectors (0, 0);
model_coefficients[1] = eigen_vectors (1, 0);
model_coefficients[2] = eigen_vectors (2, 0);
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
float distance_to_plane = model_coefficients[0] * point.x +
model_coefficients[1] * point.y +
model_coefficients[2] * point.z +
model_coefficients[3];
PointT ppoint;
// Calculate the projection of the point on the plane
ppoint.x = point.x - distance_to_plane * model_coefficients[0];
ppoint.y = point.y - distance_to_plane * model_coefficients[1];
ppoint.z = point.z - distance_to_plane * model_coefficients[2];
// Create a X-Y projected representation for within bounds polygonal checking
int k0, k1, k2;
// Determine the best plane to project points onto
k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1;
k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
k1 = (k0 + 1) % 3;
k2 = (k0 + 2) % 3;
// Project the convex hull
pcl::PointCloud<PointT> xy_polygon;
xy_polygon.points.resize (polygon.points.size ());
for (size_t i = 0; i < polygon.points.size (); ++i)
{
Eigen::Vector4f pt (polygon.points[i].x, polygon.points[i].y, polygon.points[i].z, 0);
xy_polygon.points[i].x = pt[k1];
xy_polygon.points[i].y = pt[k2];
xy_polygon.points[i].z = 0;
}
PointT xy_point;
xy_point.z = 0;
Eigen::Vector4f pt (ppoint.x, ppoint.y, ppoint.z, 0);
xy_point.x = pt[k1];
xy_point.y = pt[k2];
return (pcl::isXYPointIn2DXYPolygon (xy_point, xy_polygon));
}
