template <typename PointT> void
pcl::MaximumLikelihoodSampleConsensus<PointT>::computeMedian (
const PointCloudConstPtr &cloud, const IndicesPtr &indices, Eigen::Vector4f &median)
{
// Copy the values to vectors for faster sorting
std::vector<float> x (indices->size ());
std::vector<float> y (indices->size ());
std::vector<float> z (indices->size ());
for (size_t i = 0; i < indices->size (); ++i)
{
x[i] = cloud->points[(*indices)[i]].x;
y[i] = cloud->points[(*indices)[i]].y;
z[i] = cloud->points[(*indices)[i]].z;
}
std::sort (x.begin (), x.end ());
std::sort (y.begin (), y.end ());
std::sort (z.begin (), z.end ());
int mid = indices->size () / 2;
if (indices->size () % 2 == 0)
{
median[0] = (x[mid-1] + x[mid]) / 2;
median[1] = (y[mid-1] + y[mid]) / 2;
median[2] = (z[mid-1] + z[mid]) / 2;
}
else
{
median[0] = x[mid];
median[1] = y[mid];
median[2] = z[mid];
}
median[3] = 0;
}
template <typename PointT> double
pcl::MaximumLikelihoodSampleConsensus<PointT>::computeMedianAbsoluteDeviation (
const PointCloudConstPtr &cloud, const IndicesPtr &indices, double sigma)
{
std::vector<double> distances (indices->size ());
Eigen::Vector4f median;
// median (dist (x - median (x)))
computeMedian (cloud, indices, median);
for (size_t i = 0; i < indices->size (); ++i)
{
pcl::Vector4fMapConst pt = cloud->points[(*indices)[i]].getVector4fMap ();
Eigen::Vector4f ptdiff = pt - median;
ptdiff[3] = 0;
distances[i] = ptdiff.dot (ptdiff);
}
std::sort (distances.begin (), distances.end ());
double result;
int mid = indices->size () / 2;
// Do we have a "middle" point or should we "estimate" one ?
if (indices->size () % 2 == 0)
result = (sqrt (distances[mid-1]) + sqrt (distances[mid])) / 2;
else
result = sqrt (distances[mid]);
return (sigma * result);
}
template <typename PointT> void
pcl::MaximumLikelihoodSampleConsensus<PointT>::getMinMax (
const PointCloudConstPtr &cloud, const IndicesPtr &indices, Eigen::Vector4f &min_p, Eigen::Vector4f &max_p)
{
min_p.setConstant (FLT_MAX);
max_p.setConstant (-FLT_MAX);
min_p[3] = max_p[3] = 0;
for (size_t i = 0; i < indices->size (); ++i)
{
if (cloud->points[(*indices)[i]].x < min_p[0]) min_p[0] = cloud->points[(*indices)[i]].x;
if (cloud->points[(*indices)[i]].y < min_p[1]) min_p[1] = cloud->points[(*indices)[i]].y;
if (cloud->points[(*indices)[i]].z < min_p[2]) min_p[2] = cloud->points[(*indices)[i]].z;
if (cloud->points[(*indices)[i]].x > max_p[0]) max_p[0] = cloud->points[(*indices)[i]].x;
if (cloud->points[(*indices)[i]].y > max_p[1]) max_p[1] = cloud->points[(*indices)[i]].y;
if (cloud->points[(*indices)[i]].z > max_p[2]) max_p[2] = cloud->points[(*indices)[i]].z;
}
}
pcl::MovingLeastSquares<PointInT, PointOutT>::MLSVoxelGrid::MLSVoxelGrid (PointCloudInConstPtr& cloud,
IndicesPtr &indices,
float voxel_size) :
voxel_grid_ (), bounding_min_ (), bounding_max_ (), data_size_ (), voxel_size_ (voxel_size)
{
pcl::getMinMax3D (*cloud, *indices, bounding_min_, bounding_max_);
Eigen::Vector4f bounding_box_size = bounding_max_ - bounding_min_;
double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
// Put initial cloud in voxel grid
data_size_ = static_cast<uint64_t> (1.5 * max_size / voxel_size_);
for (unsigned int i = 0; i < indices->size (); ++i)
if (pcl_isfinite (cloud->points[(*indices)[i]].x))
{
Eigen::Vector3i pos;
getCellIndex (cloud->points[(*indices)[i]].getVector3fMap (), pos);
uint64_t index_1d;
getIndexIn1D (pos, index_1d);
Leaf leaf;
voxel_grid_[index_1d] = leaf;
}
}