template <typename PointSource, typename PointTarget, typename FeatureT> void
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output)
{
if (!input_features_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
PCL_ERROR ("No source features were given! Call setSourceFeatures before aligning.\n");
return;
}
if (!target_features_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
PCL_ERROR ("No target features were given! Call setTargetFeatures before aligning.\n");
return;
}
std::vector<int> sample_indices (nr_samples_);
std::vector<int> corresponding_indices (nr_samples_);
PointCloudSource input_transformed;
float error, lowest_error (0);
final_transformation_ = Eigen::Matrix4f::Identity ();
for (int i_iter = 0; i_iter < max_iterations_; ++i_iter)
{
// Draw nr_samples_ random samples
selectSamples (*input_, nr_samples_, min_sample_distance_, sample_indices);
// Find corresponding features in the target cloud
findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);
// Estimate the transform from the samples to their corresponding points
transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
// Tranform the data and compute the error
transformPointCloud (*input_, input_transformed, transformation_);
error = computeErrorMetric (input_transformed, (float) corr_dist_threshold_);
// If the new error is lower, update the final transformation
if (i_iter == 0 || error < lowest_error)
{
lowest_error = error;
final_transformation_ = transformation_;
}
}
// Apply the final transformation
transformPointCloud (*input_, output, final_transformation_);
}
float ATOM_TerrainPatch::computeSkirtLength (void) const
{
float skirtLength = 0.f;
ATOM_TerrainPatch *p = _parent;
while (p)
{
float f = computeErrorMetric (p);
if (f > skirtLength)
{
skirtLength = f;
}
p = p->_parent;
}
return skirtLength;
}
