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_);
}
void AHDE(int candidate,int k,int id)
{
int r1, r2, r3;
int i,j;
j = rand() % nreal;
selectSamples(candidate,&r1,&r2,&r3);
for(i = 0; i < nreal; i++)
popTrival[i] = population[candidate][i];
if( 1 == k )
{
cr = 0.8 + (0.2 * rand() / RAND_MAX);
factor = 0.8;
for (i = 0; i < nreal; ++i)
{
if ( (groupID[i] == id) && (((double)rand()/(double)RAND_MAX < cr) || (i == j)) )
{
popTrival[i] = population[r1][i]
+ factor * (population[r2][i] - population[r3][i]);
if ((popTrival[i] < low) || (popTrival[i] > high))
popTrival[i] = low+(high-low)*rand()/RAND_MAX;
}
}
}
else
{
cr = 1.0 * rand() / RAND_MAX;
factor = 0.6;
for (i = 0; i < nreal; ++i)
{
if ( (groupID[i] == id) && (((double)rand()/(double)RAND_MAX < cr) || (i == j)) )
{
popTrival[i] = population[candidate][i]
+ factor * (bestFitVec[i] - population[candidate][i])
+ (1.0 - factor) * (population[r1][i] - population[r2][i]);
if ((popTrival[i] < low) || (popTrival[i] > high))
popTrival[i] = low+(high-low)*rand()/RAND_MAX;
}
}
}
}