Пример #1
0
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_);
}
Пример #2
0
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;
			}
		}
	}
}