template <typename PointSource, typename PointTarget, typename FeatureT> float
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeErrorMetric (
const PointCloudSource &cloud, float threshold)
{
std::vector<int> nn_index (1);
std::vector<float> nn_distance (1);
float error = 0;
for (size_t i = 0; i < cloud.points.size (); ++i)
{
// Find the distance between cloud.points[i] and its nearest neighbor in the target point cloud
tree_->nearestKSearch (cloud, (int) i, 1, nn_index, nn_distance);
/*
// Huber penalty measure
float e = nn_distance[0];
if (e <= threshold)
error += 0.5 * e*e;
else
error += 0.5 * threshold * (2.0 * fabs (e) - threshold);
*/
// Truncated error
float e = nn_distance[0];
if (e <= threshold)
error += e / threshold;
else
error += 1.0;
}
return (error);
}
template <typename PointSource, typename PointTarget, typename FeatureT> float
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeErrorMetric (
const PointCloudSource &cloud, float)
{
std::vector<int> nn_index (1);
std::vector<float> nn_distance (1);
const ErrorFunctor & compute_error = *error_functor_;
float error = 0;
for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i)
{
// Find the distance between cloud.points[i] and its nearest neighbor in the target point cloud
tree_->nearestKSearch (cloud, i, 1, nn_index, nn_distance);
// Compute the error
error += compute_error (nn_distance[0]);
}
return (error);
}
