void VoxelGrid::inputCloud(typename pcl::PointCloud<PointT>::ConstPtr cloud)
{
double res = readParameter<double>("resolution");
Eigen::Vector4f leaf(res, res, res, 0);
typename pcl::PointCloud<PointT>::Ptr out(new pcl::PointCloud<PointT>);
typename pcl::PointCloud<PointT>::ConstPtr in;
if (remove_nan_) {
typename pcl::PointCloud<PointT>::Ptr tmp(new pcl::PointCloud<PointT>);
tmp->points.reserve(cloud->points.size());
for (auto it = cloud->begin(); it != cloud->end(); ++it) {
if (!std::isnan(it->x)) {
tmp->points.push_back(*it);
}
}
in = tmp;
} else {
in = cloud;
}
pcl::VoxelGrid<PointT> voxel_f;
voxel_f.setInputCloud(in);
voxel_f.setLeafSize(leaf);
voxel_f.filter(*out);
PointCloudMessage::Ptr msg(new PointCloudMessage(cloud->header.frame_id, cloud->header.stamp));
out->header = cloud->header;
msg->value = out;
msg::publish(output_, msg);
}
template<typename PointT> void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
const typename pcl::search::KdTree<PointT>::Ptr kdtree,
std::vector<Eigen::Matrix3d>& cloud_covariances)
{
if (k_correspondences_ > int (cloud->size ()))
{
PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->size (), k_correspondences_);
return;
}
Eigen::Vector3d mean;
std::vector<int> nn_indecies; nn_indecies.reserve (k_correspondences_);
std::vector<float> nn_dist_sq; nn_dist_sq.reserve (k_correspondences_);
// We should never get there but who knows
if(cloud_covariances.size () < cloud->size ())
cloud_covariances.resize (cloud->size ());
typename pcl::PointCloud<PointT>::const_iterator points_iterator = cloud->begin ();
std::vector<Eigen::Matrix3d>::iterator matrices_iterator = cloud_covariances.begin ();
for(;
points_iterator != cloud->end ();
++points_iterator, ++matrices_iterator)
{
const PointT &query_point = *points_iterator;
Eigen::Matrix3d &cov = *matrices_iterator;
// Zero out the cov and mean
cov.setZero ();
mean.setZero ();
// Search for the K nearest neighbours
kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
// Find the covariance matrix
for(int j = 0; j < k_correspondences_; j++) {
const PointT &pt = (*cloud)[nn_indecies[j]];
mean[0] += pt.x;
mean[1] += pt.y;
mean[2] += pt.z;
cov(0,0) += pt.x*pt.x;
cov(1,0) += pt.y*pt.x;
cov(1,1) += pt.y*pt.y;
cov(2,0) += pt.z*pt.x;
cov(2,1) += pt.z*pt.y;
cov(2,2) += pt.z*pt.z;
}
mean /= static_cast<double> (k_correspondences_);
// Get the actual covariance
for (int k = 0; k < 3; k++)
for (int l = 0; l <= k; l++)
{
cov(k,l) /= static_cast<double> (k_correspondences_);
cov(k,l) -= mean[k]*mean[l];
cov(l,k) = cov(k,l);
}
// Compute the SVD (covariance matrix is symmetric so U = V')
Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
cov.setZero ();
Eigen::Matrix3d U = svd.matrixU ();
// Reconstitute the covariance matrix with modified singular values using the column // vectors in V.
for(int k = 0; k < 3; k++) {
Eigen::Vector3d col = U.col(k);
double v = 1.; // biggest 2 singular values replaced by 1
if(k == 2) // smallest singular value replaced by gicp_epsilon
v = gicp_epsilon_;
cov+= v * col * col.transpose();
}
}
}