C++ PCL_ERROR函数代码示例

int
main(int, char** argv)
{
  std::string filename = argv[1];
  std::cout << "Reading " << filename << std::endl;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
  if(pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud_xyz) == -1) // load the file
  {
    PCL_ERROR ("Couldn't read file");
    return -1;
  }
  std::cout << "points: " << cloud_xyz->points.size () <<std::endl;
  
  // Parameters for sift computation
  const float min_scale = 0.01f;
  const int n_octaves = 3;
  const int n_scales_per_octave = 4;
  const float min_contrast = 0.001f;
  
  // Estimate the normals of the cloud_xyz
  pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
  pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointNormal>);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n(new pcl::search::KdTree<pcl::PointXYZ>());

  ne.setInputCloud(cloud_xyz);
  ne.setSearchMethod(tree_n);
  ne.setRadiusSearch(0.2);
  ne.compute(*cloud_normals);

  // Copy the xyz info from cloud_xyz and add it to cloud_normals as the xyz field in PointNormals estimation is zero
  for(size_t i = 0; i<cloud_normals->points.size(); ++i)
  {
    cloud_normals->points[i].x = cloud_xyz->points[i].x;
    cloud_normals->points[i].y = cloud_xyz->points[i].y;
    cloud_normals->points[i].z = cloud_xyz->points[i].z;
  }

  // Estimate the sift interest points using normals values from xyz as the Intensity variants
  pcl::SIFTKeypoint<pcl::PointNormal, pcl::PointWithScale> sift;
  pcl::PointCloud<pcl::PointWithScale> result;
  pcl::search::KdTree<pcl::PointNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointNormal> ());
  sift.setSearchMethod(tree);
  sift.setScales(min_scale, n_octaves, n_scales_per_octave);
  sift.setMinimumContrast(min_contrast);
  sift.setInputCloud(cloud_normals);
  sift.compute(result);

  std::cout << "No of SIFT points in the result are " << result.points.size () << std::endl;

/*
  // Copying the pointwithscale to pointxyz so as visualize the cloud
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZ>);
  copyPointCloud(result, *cloud_temp);
  std::cout << "SIFT points in the cloud_temp are " << cloud_temp->points.size () << std::endl;
  
  
  // Visualization of keypoints along with the original cloud
  pcl::visualization::PCLVisualizer viewer("PCL Viewer");
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (cloud_temp, 0, 255, 0);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color_handler (cloud_xyz, 255, 0, 0);
  viewer.setBackgroundColor( 0.0, 0.0, 0.0 );
  viewer.addPointCloud(cloud_xyz, cloud_color_handler, "cloud");
  viewer.addPointCloud(cloud_temp, keypoints_color_handler, "keypoints");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
  
  while(!viewer.wasStopped ())
  {
  viewer.spinOnce ();
  }
  
*/

  return 0;
  
}

template <typename PointInT, typename PointOutT, typename PointRFT> void
pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computePointDescriptor (size_t index, /*float rf[9],*/ std::vector<float> &desc)
{
  pcl::Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();

  const Eigen::Vector3f x_axis (frames_->points[index].x_axis[0],
                                frames_->points[index].x_axis[1],
                                frames_->points[index].x_axis[2]);
  //const Eigen::Vector3f& y_axis = frames_->points[index].y_axis.getNormalVector3fMap ();
  const Eigen::Vector3f normal (frames_->points[index].z_axis[0],
                                frames_->points[index].z_axis[1],
                                frames_->points[index].z_axis[2]);

  // Find every point within specified search_radius_
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;
  const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
  // For each point within radius
  for (size_t ne = 0; ne < neighb_cnt; ne++)
  {
    if (pcl::utils::equal(nn_dists[ne], 0.0f))
      continue;
    // Get neighbours coordinates
    Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();

    // ----- Compute current neighbour polar coordinates -----

    // Get distance between the neighbour and the origin
    float r = std::sqrt (nn_dists[ne]);

    // Project point into the tangent plane
    Eigen::Vector3f proj;
    pcl::geometry::project (neighbour, origin, normal, proj);
    proj -= origin;

    // Normalize to compute the dot product
    proj.normalize ();

    // Compute the angle between the projection and the x axis in the interval [0,360]
    Eigen::Vector3f cross = x_axis.cross (proj);
    float phi = rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
    phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
    /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
    Eigen::Vector3f no = neighbour - origin;
    no.normalize ();
    float theta = normal.dot (no);
    theta = pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta))));

    /// Bin (j, k, l)
    size_t j = 0;
    size_t k = 0;
    size_t l = 0;

    /// Compute the Bin(j, k, l) coordinates of current neighbour
    for (size_t rad = 1; rad < radius_bins_ + 1; rad++)
    {
      if (r <= radii_interval_[rad])
      {
        j = rad - 1;
        break;
      }
    }

    for (size_t ang = 1; ang < elevation_bins_ + 1; ang++)
    {
      if (theta <= theta_divisions_[ang])
      {
        k = ang - 1;
        break;
      }
    }

    for (size_t ang = 1; ang < azimuth_bins_ + 1; ang++)
    {
      if (phi <= phi_divisions_[ang])
      {
        l = ang - 1;
        break;
      }
    }

    /// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
    std::vector<int> neighbour_indices;
    std::vector<float> neighbour_didtances;
    float point_density = static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances));
    /// point_density is always bigger than 0 because FindPointsWithinRadius returns at least the point itself
    float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
                                                   (k*radius_bins_) +
                                                   j];

    assert (w >= 0.0);
    if (w == std::numeric_limits<float>::infinity ())
      PCL_ERROR ("Shape Context Error INF!\n");
    if (std::isnan(w))
      PCL_ERROR ("Shape Context Error IND!\n");
    /// Accumulate w into correspondent Bin(j,k,l)
    desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;

    assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
  } // end for each neighbour
//.........这里部分代码省略.........

template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPoints (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 7)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCylinder::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
    return;
  }

  projected_points.header = input_->header;
  projected_points.is_dense = input_->is_dense;

  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  float ptdotdir = line_pt.dot (line_dir);
  float dirdotdir = 1.0f / line_dir.dot (line_dir);

  // Copy all the data fields from the input cloud to the projected one?
  if (copy_data_fields)
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (input_->points.size ());
    projected_points.width    = input_->width;
    projected_points.height   = input_->height;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < projected_points.points.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the cylinder
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      Eigen::Vector4f p (input_->points[inliers[i]].x,
                         input_->points[inliers[i]].y,
                         input_->points[inliers[i]].z,
                         1);

      float k = (p.dot (line_dir) - ptdotdir) * dirdotdir;

      pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
      pp.matrix () = line_pt + k * line_dir;

      Eigen::Vector4f dir = p - pp;
      dir.normalize ();

      // Calculate the projection of the point onto the cylinder
      pp += dir * model_coefficients[6];
    }
  }
  else
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (inliers.size ());
    projected_points.width    = static_cast<uint32_t> (inliers.size ());
    projected_points.height   = 1;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < inliers.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
      pcl::Vector4fMapConst p = input_->points[inliers[i]].getVector4fMap ();

      float k = (p.dot (line_dir) - ptdotdir) * dirdotdir;
      // Calculate the projection of the point on the line
      pp.matrix () = line_pt + k * line_dir;

      Eigen::Vector4f dir = p - pp;
      dir.normalize ();

      // Calculate the projection of the point onto the cylinder
      pp += dir * model_coefficients[6];
    }
  }
}

int main(int argc, char** argv)
{

  if (argc < 5)
  {
    PCL_INFO ("Usage %s -input_dir /dir/with/models -output_dir /output/dir [options]\n", argv[0]);
    PCL_INFO (" * where options are:\n"
        "         -height <X>            : simulate scans with sensor mounted on height X\n"
        "         -noise_std <X>         : std of gaussian noise added to pointcloud. Default value 0.0001.\n"
        "         -distance <X>          : simulate scans with object located on a distance X. Can be used multiple times. Default value 4.\n"
        "         -tilt <X>              : tilt sensor for X degrees. X == 0 - sensor looks strait. X < 0 Sensor looks down. X > 0 Sensor looks up . Can be used multiple times. Default value -15.\n"
        "         -shift <X>             : shift object from the straight line. Can be used multiple times. Default value 0.\n"
        "         -num_views <X>         : how many times rotate the object in for every distance, tilt and shift. Default value 6.\n"

        "");
    return -1;
  }

  std::string input_dir, output_dir;
  double height = 1.5;
  double num_views = 6;
  double noise_std = 0.0001;
  std::vector<double> distances;
  std::vector<double> tilt;
  std::vector<double> shift;
  int full_model_n_points = 30000;

  pcl::console::parse_argument(argc, argv, "-input_dir", input_dir);
  pcl::console::parse_argument(argc, argv, "-output_dir", output_dir);
  pcl::console::parse_argument(argc, argv, "-num_views", num_views);
  pcl::console::parse_argument(argc, argv, "-height", height);
  pcl::console::parse_argument(argc, argv, "-noise_std", noise_std);
  pcl::console::parse_multiple_arguments(argc, argv, "-distance", distances);
  pcl::console::parse_multiple_arguments(argc, argv, "-tilt", tilt);
  pcl::console::parse_multiple_arguments(argc, argv, "-shift", shift);

  PCL_INFO("distances size: %d\n", distances.size());
  for (size_t i = 0; i < distances.size(); i++)
  {
    PCL_INFO("distance: %f\n", distances[i]);
  }

  // Set default values if user didn't provide any
  if (distances.size() == 0)
    distances.push_back(4);
  if (tilt.size() == 0)
    tilt.push_back(-15);
  if (shift.size() == 0)
    shift.push_back(0);

  // Check if input path exists
  boost::filesystem::path input_path(input_dir);
  if (!boost::filesystem::exists(input_path))
  {
    PCL_ERROR("Input directory doesnt exists.");
    return -1;
  }

  // Check if input path is a directory
  if (!boost::filesystem::is_directory(input_path))
  {
    PCL_ERROR("%s is not directory.", input_path.c_str());
    return -1;
  }

  // Check if output directory exists
  boost::filesystem::path output_path(output_dir);
  if (!boost::filesystem::exists(output_path) || !boost::filesystem::is_directory(output_path))
  {
    if (!boost::filesystem::create_directories(output_path))
    {
      PCL_ERROR ("Error creating directory %s.\n", output_path.c_str ());
      return -1;
    }
  }

  // Find all .vtk files in the input directory
  std::vector<std::string> files_to_process;
  PCL_INFO("Processing following files:\n");
  boost::filesystem::directory_iterator end_iter;
  for (boost::filesystem::directory_iterator iter(input_path); iter != end_iter; iter++)
  {
    boost::filesystem::path class_dir_path(*iter);
    for (boost::filesystem::directory_iterator iter2(class_dir_path); iter2 != end_iter; iter2++)
    {
      boost::filesystem::path file(*iter2);
      if (file.extension() == ".vtk")
      {
        files_to_process.push_back(file.c_str());
        PCL_INFO("\t%s\n", file.c_str());
      }
    }

  }

  // Check if there are any .vtk files to process
  if (files_to_process.size() == 0)
  {
    PCL_ERROR("Directory %s has no .vtk files.", input_path.c_str());
    return -1;
//.........这里部分代码省略.........

int
main (int argc, char *argv[])
{

	PointCloudT::Ptr	cloud (new PointCloudT),
				cloud_inliers_table (new PointCloudT),
				cloud_outliers_table (new PointCloudT),
				cloud_edge (new PointCloudT),
				cloud_edge_projected (new PointCloudT),
				cloud_poligonal_prism (new PointCloudT);


	/*// Load point cloud
	if (pcl::io::loadPCDFile ("caixacomobjectos.pcd", *cloud) < 0) {
		PCL_ERROR ("Could not load PCD file !\n");
		return (-1);
	}*/

	// Load point cloud
	if (pcl::io::loadPCDFile (argv[1], *cloud) < 0) {
		PCL_ERROR ("Could not load PCD file !\n");
		return (-1);
	}

///////////////////////////////////////////
//          Table Plane Extract          //
///////////////////////////////////////////

	// Segment the ground
	pcl::ModelCoefficients::Ptr plane_table (new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers_plane_table (new pcl::PointIndices);
	PointCloudT::Ptr cloud_plane_table (new PointCloudT);

	// Make room for a plane equation (ax+by+cz+d=0)
	plane_table->values.resize (4);

	pcl::SACSegmentation<PointT> seg_table;	// Create the segmentation object
	seg_table.setOptimizeCoefficients (true);			// Optional
	seg_table.setMethodType (pcl::SAC_RANSAC);
	seg_table.setModelType (pcl::SACMODEL_PLANE);
	seg_table.setDistanceThreshold (0.025f);
	seg_table.setInputCloud (cloud);
	seg_table.segment (*inliers_plane_table, *plane_table);

	if (inliers_plane_table->indices.size () == 0) {
		PCL_ERROR ("Could not estimate a planar model for the given dataset.\n");
		return (-1);
	}

	// Extract inliers
	pcl::ExtractIndices<PointT> extract;
	extract.setInputCloud (cloud);
	extract.setIndices (inliers_plane_table);
	extract.setNegative (false);			// Extract the inliers
	extract.filter (*cloud_inliers_table);		// cloud_inliers contains the plane

	// Extract outliers
	//extract.setInputCloud (cloud);		// Already done line 50
	//extract.setIndices (inliers);			// Already done line 51
	extract.setNegative (true);				// Extract the outliers
	extract.filter (*cloud_outliers_table);		// cloud_outliers contains everything but the plane

	printf ("Plane segmentation equation [ax+by+cz+d]=0: [%3.4f | %3.4f | %3.4f | %3.4f]     \t\n", 
			plane_table->values[0], plane_table->values[1], plane_table->values[2] , plane_table->values[3]);




///////////////////////////////////////////
//           Box Edge Extract            //
///////////////////////////////////////////
	
	pcl::PassThrough<PointT> pass;
    	pass.setInputCloud (cloud_outliers_table);
    	pass.setFilterFieldName ("z");
    	pass.setFilterLimits (0.63, 0.68);
    	pass.filter (*cloud_edge);

	pcl::ModelCoefficients::Ptr coefficients_edge (new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers_edge (new pcl::PointIndices);
	// Create the segmentation object
	pcl::SACSegmentation<pcl::PointXYZ> seg_edge;
	// Optional
	seg_edge.setOptimizeCoefficients (true);
	// Mandatory
	seg_edge.setModelType (pcl::SACMODEL_PLANE);
	seg_edge.setMethodType (pcl::SAC_RANSAC);
	seg_edge.setDistanceThreshold (0.01);

	seg_edge.setInputCloud (cloud_edge);
	seg_edge.segment (*inliers_edge, *coefficients_edge);


	///////////////////////////////
	// Project the model inliers //
	///////////////////////////////
	pcl::ProjectInliers<pcl::PointXYZ> proj_edge;
	proj_edge.setModelType (pcl::SACMODEL_PLANE);
	proj_edge.setIndices (inliers_edge);
	proj_edge.setInputCloud (cloud_edge);
//.........这里部分代码省略.........

void
pcl::kinfuLS::WorldModel<PointT>::getWorldAsCubes (const double size, std::vector<typename WorldModel<PointT>::PointCloudPtr> &cubes, std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &transforms, double overlap)
{
  
  if(world_->points.size () == 0)
  {
	PCL_INFO("The world is empty, returning nothing\n");
	return;
  }

  PCL_INFO ("Getting world as cubes. World contains %d points.\n", world_->points.size ());

  // remove nans from world cloud
  world_->is_dense = false;
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud ( *world_, *world_, indices);
	
  PCL_INFO ("World contains %d points after nan removal.\n", world_->points.size ());
  

  // check cube size value
  double cubeSide = size;
  if (cubeSide <= 0.0f)
  {
	PCL_ERROR ("Size of the cube must be positive and non null (%f given). Setting it to 3.0 meters.\n", cubeSide);
	cubeSide = 512.0f;
  }

  std::cout << "cube size is set to " << cubeSide << std::endl;

  // check overlap value
  double step_increment = 1.0f - overlap;
  if (overlap < 0.0)
  {
	PCL_ERROR ("Overlap ratio must be positive or null (%f given). Setting it to 0.0 procent.\n", overlap);
	step_increment = 1.0f;
  }
  if (overlap > 1.0)
  {
	PCL_ERROR ("Overlap ratio must be less or equal to 1.0 (%f given). Setting it to 10 procent.\n", overlap);
	step_increment = 0.1f;
  }

  
  // get world's bounding values on XYZ
  PointT min, max;
  pcl::getMinMax3D(*world_, min, max);

  PCL_INFO ("Bounding box for the world: \n\t [%f - %f] \n\t [%f - %f] \n\t [%f - %f] \n", min.x, max.x, min.y, max.y, min.z, max.z);

  PointT origin = min;
  
  // clear returned vectors
  cubes.clear();
  transforms.clear();

  // iterate with box filter
  while (origin.x < max.x)
  {
	origin.y = min.y;
	while (origin.y < max.y)
	{
	  origin.z = min.z;
	  while (origin.z < max.z)
	  {
		// extract cube here
		PCL_INFO ("Extracting cube at: [%f, %f, %f].\n",  origin.x,  origin.y,  origin.z);

		// pointcloud for current cube.
		PointCloudPtr box (new pcl::PointCloud<PointT>);


		// set conditional filter
		ConditionAndPtr range_cond (new pcl::ConditionAnd<PointT> ());
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, origin.x)));
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, origin.x + cubeSide)));
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, origin.y)));
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, origin.y + cubeSide)));
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, origin.z)));
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, origin.z + cubeSide)));

		// build the filter
		pcl::ConditionalRemoval<PointT> condrem;
		condrem.setCondition (range_cond);
		condrem.setInputCloud (world_);
		condrem.setKeepOrganized(false);
		// apply filter
		condrem.filter (*box);

		// also push transform along with points.
		if(box->points.size() > 0)
		{
		  Eigen::Vector3f transform;
		  transform[0] = origin.x, transform[1] = origin.y, transform[2] = origin.z;
		  transforms.push_back(transform);
		  cubes.push_back(box);        
		}
		else
		{
		  PCL_INFO ("Extracted cube was empty, skiping this one.\n");
//.........这里部分代码省略.........

template <typename PointSource, typename PointTarget> void
pcl::IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
{
  // Allocate enough space to hold the results
  std::vector<int> nn_indices (1);
  std::vector<float> nn_dists (1);

  // Point cloud containing the correspondences of each point in <input, indices>
  PointCloudTarget input_corresp;
  input_corresp.points.resize (indices_->size ());

  nr_iterations_ = 0;
  converged_ = false;
  double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;

  // If the guessed transformation is non identity
  if (guess != Eigen::Matrix4f::Identity ())
  {
    // Initialise final transformation to the guessed one
    final_transformation_ = guess;
    // Apply guessed transformation prior to search for neighbours
    transformPointCloud (output, output, guess);
  }

  // Resize the vector of distances between correspondences 
  std::vector<float> previous_correspondence_distances (indices_->size ());
  correspondence_distances_.resize (indices_->size ());

  while (!converged_)           // repeat until convergence
  {
    // Save the previously estimated transformation
    previous_transformation_ = transformation_;
    // And the previous set of distances
    previous_correspondence_distances = correspondence_distances_;

    int cnt = 0;
    std::vector<int> source_indices (indices_->size ());
    std::vector<int> target_indices (indices_->size ());

    // Iterating over the entire index vector and  find all correspondences
    for (size_t idx = 0; idx < indices_->size (); ++idx)
    {
      if (!this->searchForNeighbors (output, (*indices_)[idx], nn_indices, nn_dists))
      {
        PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[idx]);
        return;
      }

      // Check if the distance to the nearest neighbor is smaller than the user imposed threshold
      if (nn_dists[0] < dist_threshold)
      {
        source_indices[cnt] = (*indices_)[idx];
        target_indices[cnt] = nn_indices[0];
        cnt++;
      }

      // Save the nn_dists[0] to a global vector of distances
      correspondence_distances_[(*indices_)[idx]] = std::min (nn_dists[0], (float)dist_threshold);
    }
    if (cnt < min_number_correspondences_)
    {
      PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
      converged_ = false;
      return;
    }

    // Resize to the actual number of valid correspondences
    source_indices.resize (cnt); target_indices.resize (cnt);

    std::vector<int> source_indices_good;
    std::vector<int> target_indices_good;
    {
      // From the set of correspondences found, attempt to remove outliers
      // Create the registration model
      typedef typename SampleConsensusModelRegistration<PointSource>::Ptr SampleConsensusModelRegistrationPtr;
      SampleConsensusModelRegistrationPtr model;
      model.reset (new SampleConsensusModelRegistration<PointSource> (output.makeShared (), source_indices));
      // Pass the target_indices
      model->setInputTarget (target_, target_indices);
      // Create a RANSAC model
      RandomSampleConsensus<PointSource> sac (model, inlier_threshold_);
      sac.setMaxIterations (ransac_iterations_);

      // Compute the set of inliers
      if (!sac.computeModel ())
      {
        source_indices_good = source_indices;
        target_indices_good = target_indices;
      }
      else
      {
        std::vector<int> inliers;
        // Get the inliers
        sac.getInliers (inliers);
        source_indices_good.resize (inliers.size ());
        target_indices_good.resize (inliers.size ());

        boost::unordered_map<int, int> source_to_target;
        for (unsigned int i = 0; i < source_indices.size(); ++i)
          source_to_target[source_indices[i]] = target_indices[i];
//.........这里部分代码省略.........

Eigen::Matrix4f Calibration::stitch(pcl::PointCloud<PointT>& points, double epsilon, double maxCorrespondanceDistance)
{
	if (this->stitching.size() == 0)
	{
		pcl::copyPointCloud(points, this->stitching);
		return Eigen::Matrix4f::Identity(); // Hardcore hack !!
	}

	//	Compute surface normals and curvature
	pcl::PointCloud<PointT> tempTarget;
	pcl::copyPointCloud(this->stitching, tempTarget);

	pcl::PointCloud<pcl::PointNormal>::Ptr pointsWithNormalSource (new pcl::PointCloud<pcl::PointNormal>);
	pcl::PointCloud<pcl::PointNormal>::Ptr pointsWithNormalTarget (new pcl::PointCloud<pcl::PointNormal>);

	pcl::NormalEstimation<PointT, pcl::PointNormal> normalEstimate;
	pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
	normalEstimate.setSearchMethod(tree);
	normalEstimate.setKSearch(30);

	normalEstimate.setInputCloud(points.makeShared());
	normalEstimate.compute(*pointsWithNormalSource);
	pcl::copyPointCloud(points, *pointsWithNormalSource);

	normalEstimate.setInputCloud (tempTarget.makeShared());
	normalEstimate.compute(*pointsWithNormalTarget);
	pcl::copyPointCloud (tempTarget, *pointsWithNormalTarget);

	//	Instantiate custom point representation
	MyPointNormal pointNormal;
	//	... and weight the 'curvature' dimension so that it is balanced against x, y, and z
	float alpha[4] = {1.0, 1.0, 1.0, 1.0};
	pointNormal.setRescaleValues(alpha);

	//	Align
	pcl::IterativeClosestPointNonLinear<pcl::PointNormal, pcl::PointNormal> registration;
	registration.setTransformationEpsilon(epsilon);
	//	Set the maximum distance between two correspondences
	registration.setMaxCorrespondenceDistance(maxCorrespondanceDistance);
	//	Set the point representation
	registration.setPointRepresentation (boost::make_shared<const MyPointNormal> (pointNormal));

	registration.setInputSource(pointsWithNormalSource);
	registration.setInputTarget(pointsWithNormalTarget);
	registration.setMaximumIterations(30);

	PCL_ERROR("Source size: %d  --  Target size: %d\n", (int)pointsWithNormalSource.get()->size(), (int)pointsWithNormalTarget.get()->size());

	Eigen::Matrix4f tf = Eigen::Matrix4f::Identity();
	pcl::PointCloud<pcl::PointNormal>::Ptr regResult = pointsWithNormalSource;

	PCL_ERROR("Stitching ... ");
	registration.align(*regResult);
	PCL_ERROR("Done!\n");

	tf = registration.getFinalTransformation().inverse();

//	PCL_ERROR("\nTransform:\n");
//	PCL_ERROR("| %f %f %f %f |\n", tf(0,0), tf(0,1), tf (0,2), tf(0,3));
//	PCL_ERROR("| %f %f %f %f |\n", tf(1,0), tf(1,1), tf (1,2), tf(1,3));
//	PCL_ERROR("| %f %f %f %f |\n", tf(2,0), tf(2,1), tf (2,2), tf(2,3));
//	PCL_ERROR("| %f %f %f %f |\n\n", tf(3,0), tf(3,1), tf (3,2), tf(3,3));

	pcl::transformPointCloud(*pointsWithNormalSource, *regResult, tf);
	*regResult += *pointsWithNormalTarget;

	pcl::copyPointCloud(*regResult, this->stitching);

	return tf;
}

int
main (int argc, char** argv)
{
//  /////////////////////////////////////////////////////////////////////////////////////////////////////

  if (argc != 3)
  {
    std::cerr << "please specify the paths to the two point clouds to be registered" << std::endl;
    exit (0);
  }
//  /////////////////////////////////////////////////////////////////////////////////////////////////////

//  /////////////////////////////////////////////////////////////////////////////////////////////////////
  pcl::PointCloud<pcl::PointXYZ> inputCloud;
  pcl::PointCloud<pcl::PointXYZ> targetCloud;

  if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], inputCloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read the first .pcd file \n");
    return (-1);
  }

  if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[2], targetCloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read the second .pcd file \n");
    return (-1);
  }
//  /////////////////////////////////////////////////////////////////////////////////////////////////////

//  /////////////////////////////////////////////////////////////////////////////////////////////////////
  pcl::PointCloud<pcl::PointXYZ> inputCloudFiltered;
  pcl::PointCloud<pcl::PointXYZ> targetCloudFiltered;

  pcl::VoxelGrid<pcl::PointXYZ> sor;
//  sor.setLeafSize (0.01, 0.01, 0.01);
  sor.setLeafSize (0.02f, 0.02f, 0.02f);
//  sor.setLeafSize (0.05, 0.05, 0.05);
//  sor.setLeafSize (0.1, 0.1, 0.1);
//  sor.setLeafSize (0.4, 0.4, 0.4);
//  sor.setLeafSize (0.5, 0.5, 0.5);

  sor.setInputCloud (inputCloud.makeShared());
  std::cout<<"\n inputCloud.size()="<<inputCloud.size()<<std::endl;
  sor.filter (inputCloudFiltered);
  std::cout<<"\n inputCloudFiltered.size()="<<inputCloudFiltered.size()<<std::endl;

  sor.setInputCloud (targetCloud.makeShared());
  std::cout<<"\n targetCloud.size()="<<targetCloud.size()<<std::endl;
  sor.filter (targetCloudFiltered);
  std::cout<<"\n targetCloudFiltered.size()="<<targetCloudFiltered.size()<<std::endl;
//  /////////////////////////////////////////////////////////////////////////////////////////////////////

//  /////////////////////////////////////////////////////////////////////////////////////////////////////
//  pcl::PointCloud<pcl::PointXYZ>::ConstPtr source = inputCloud.makeShared();
//  pcl::PointCloud<pcl::PointXYZ>::ConstPtr target = targetCloud.makeShared();

  pcl::PointCloud<pcl::PointXYZ>::ConstPtr source = inputCloudFiltered.makeShared();
  pcl::PointCloud<pcl::PointXYZ>::ConstPtr target = targetCloudFiltered.makeShared();

  pcl::PointCloud<pcl::PointXYZ> source_aligned;
//  /////////////////////////////////////////////////////////////////////////////////////////////////////

//  /////////////////////////////////////////////////////////////////////////////////////////////////////
  pcl::RegistrationVisualizer<pcl::PointXYZ, pcl::PointXYZ> registrationVisualizer;

  registrationVisualizer.startDisplay();

  registrationVisualizer.setMaximumDisplayedCorrespondences (100);
//  /////////////////////////////////////////////////////////////////////////////////////////////////////

//  ///////////////////////////////////////////////////////////////////////////////////////////////////
  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

  icp.setMaximumIterations(10000);

//  icp.setMaxCorrespondenceDistance (0.6);
  icp.setMaxCorrespondenceDistance (0.8);
//  icp.setMaxCorrespondenceDistance (1.5);

//  icp.setRANSACOutlierRejectionThreshold (0.1);
  icp.setRANSACOutlierRejectionThreshold (0.6);
//  icp.setRANSACOutlierRejectionThreshold (1.5);
//  icp.setRANSACOutlierRejectionThreshold (5.0);

  icp.setInputCloud  (source);
  icp.setInputTarget (target);

  // Register the registration algorithm to the RegistrationVisualizer
  registrationVisualizer.setRegistration (icp);

  // Start registration process
  icp.align (source_aligned);

  std::cout << "has converged:" << icp.hasConverged () << " score: " << icp.getFitnessScore () << std::endl;
  std::cout << icp.getFinalTransformation () << std::endl;
//  ///////////////////////////////////////////////////////////////////////////////////////////////////

//  ///////////////////////////////////////////////////////////////////////////////////////////////////
//
//  pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> icpnl;
//.........这里部分代码省略.........

//////////////////////////////////////////////////////////////////////////////
//const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pcl::StereoMatching::getPointCloud(float uC, float vC, float focal, float baseline)
bool 
pcl::StereoMatching::getPointCloud (
    float u_c, float v_c, float focal, float baseline, 
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
  
  //disp map has not been computed yet..
  if ( disp_map_ == NULL)
  {

    PCL_ERROR(
      "[pcl::StereoMatching::getPointCloud] Error: a disparity map has not been computed yet. The resulting cloud can not be computed..\n"
    );

    return false;
  }

  //cloud needs to be re-allocated
  if (cloud->width != width_ || cloud->height != height_)
  {
    cloud->resize(width_*height_);
    cloud->width = width_;
    cloud->height = height_;
    cloud->is_dense = false;
  }

  if ( cloud->is_dense)
    cloud->is_dense = false;

  //Loop
  pcl::PointXYZ temp_point;
  pcl::PointXYZ nan_point;
  nan_point.x = std::numeric_limits<float>::quiet_NaN();
  nan_point.y = std::numeric_limits<float>::quiet_NaN();
  nan_point.z = std::numeric_limits<float>::quiet_NaN();
  //nan_point.intensity = std::numeric_limits<float>::quiet_NaN();

  //all disparities are multiplied by a constant equal to 16; 
  //this must be taken into account when computing z values
  float depth_scale = baseline * focal * 16.0f;

  for ( int j=0; j<height_; j++)
  {
    for ( int i=0; i<width_; i++)
    {
      if ( disp_map_[ j*width_ + i] > 0 )
      {

        temp_point.z = depth_scale / disp_map_[j * width_ + i];
        temp_point.x = ((static_cast<float> (i) - u_c) * temp_point.z) / focal;
        temp_point.y = ((static_cast<float> (j) - v_c) * temp_point.z) / focal;
        //temp_point.intensity = 255;

        (*cloud)[j * width_ + i] = temp_point;
      }
      //adding NaN value
      else
      {
        (*cloud)[j * width_ + i] = nan_point;
      }
    }
  }

  return (true);
}

bool
pcl::concatenateFields (const sensor_msgs::PointCloud2 &cloud1,
                        const sensor_msgs::PointCloud2 &cloud2,
                        sensor_msgs::PointCloud2 &cloud_out)
{
    // If the cloud's sizes differ (points wise), then exit with error
    if (cloud1.width != cloud2.width || cloud1.height != cloud2.height)
    {
        PCL_ERROR ("[pcl::concatenateFields] Dimensions of input clouds do not match: cloud1 (w, %d, h, %d), cloud2 (w, %d, h, %d)\n", cloud1.width, cloud1.height, cloud2.width, cloud2.height );
        return (false);
    }


    if (cloud1.is_bigendian != cloud2.is_bigendian)
    {
        PCL_ERROR ("[pcl::concatenateFields] Endianness of clouds does not match\n");
        return (false);
    }

    // Else, copy the second cloud (width, height, header stay the same)
    // we do this since fields from the second cloud are supposed to overwrite
    // those of the first
    cloud_out.header = cloud2.header;
    cloud_out.fields = cloud2.fields;
    cloud_out.width = cloud2.width;
    cloud_out.height = cloud2.height;
    cloud_out.is_bigendian = cloud2.is_bigendian;

    //We need to find how many fields overlap between the two clouds
    size_t total_fields = cloud2.fields.size ();

    //for the non-matching fields in cloud1, we need to store the offset
    //from the beginning of the point
    std::vector<const sensor_msgs::PointField*> cloud1_unique_fields;
    std::vector<int> field_sizes;

    //We need to make sure that the fields for cloud 1 are sorted
    //by offset so that we can compute sizes correctly. There is no
    //guarantee that the fields are in the correct order when they come in
    std::vector<const sensor_msgs::PointField*> cloud1_fields_sorted;
    for (size_t i = 0; i < cloud1.fields.size (); ++i)
        cloud1_fields_sorted.push_back (&(cloud1.fields[i]));

    std::sort (cloud1_fields_sorted.begin (), cloud1_fields_sorted.end (), fieldComp);

    for (size_t i = 0; i < cloud1_fields_sorted.size (); ++i)
    {
        bool match = false;
        for (size_t j = 0; j < cloud2.fields.size (); ++j)
        {
            if (cloud1_fields_sorted[i]->name == cloud2.fields[j].name)
                match = true;
        }

        //if the field is new, we'll increment out total fields
        if (!match && cloud1_fields_sorted[i]->name != "_")
        {
            cloud1_unique_fields.push_back (cloud1_fields_sorted[i]);

            int size = 0;
            size_t next_valid_field = i + 1;

            while (next_valid_field < cloud1_fields_sorted.size())
            {
                if (cloud1_fields_sorted[next_valid_field]->name != "_")
                    break;
                next_valid_field++;
            }

            if (next_valid_field < cloud1_fields_sorted.size ())
                //compute the true size of the field, including padding
                size = cloud1_fields_sorted[next_valid_field]->offset - cloud1_fields_sorted[i]->offset;
            else
                //for the last point, we'll just use the point step to compute the size
                size = cloud1.point_step - cloud1_fields_sorted[i]->offset;

            field_sizes.push_back (size);

            total_fields++;
        }
    }

    //we need to compute the size of the additional data added from cloud 1
    uint32_t cloud1_unique_point_step = 0;
    for (size_t i = 0; i < cloud1_unique_fields.size (); ++i)
        cloud1_unique_point_step += field_sizes[i];

    //the total size of extra data should be the size of data per point
    //multiplied by the total number of points in the cloud
    uint32_t cloud1_unique_data_size = cloud1_unique_point_step * cloud1.width * cloud1.height;

    // Point step must increase with the length of each matching field
    cloud_out.point_step = cloud2.point_step + cloud1_unique_point_step;
    // Recalculate row_step
    cloud_out.row_step = cloud_out.point_step * cloud_out.width;

    // Resize data to hold all clouds
    cloud_out.data.resize (cloud2.data.size () + cloud1_unique_data_size);

    // Concatenate fields
//.........这里部分代码省略.........

template <typename PointXYZT, typename PointRGBT> bool
pcl::LineRGBD<PointXYZT, PointRGBT>::loadTemplates (const std::string &file_name, const size_t object_id)
{
  // Open the file
  int ltm_fd = pcl_open (file_name.c_str (), O_RDONLY);
  if (ltm_fd == -1)
    return (false);
  
  int ltm_offset = 0;

  pcl::io::TARHeader ltm_header;
  PCDReader pcd_reader;

  std::string pcd_ext (".pcd");
  std::string sqmmt_ext (".sqmmt");

  // While there still is an LTM header to be read
  while (readLTMHeader (ltm_fd, ltm_header))
  {
    ltm_offset += 512;

    // Search for extension
    std::string chunk_name (ltm_header.file_name);
    std::transform (chunk_name.begin (), chunk_name.end (), chunk_name.begin (), ::tolower);
    std::string::size_type it;

    if ((it = chunk_name.find (pcd_ext)) != std::string::npos &&
        (pcd_ext.size () - (chunk_name.size () - it)) == 0)
    {
      PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a PCD file.\n", chunk_name.c_str ());
      // Read the next PCD file
      template_point_clouds_.resize (template_point_clouds_.size () + 1);
      pcd_reader.read (file_name, template_point_clouds_[template_point_clouds_.size () - 1], ltm_offset);

      // Increment the offset for the next file
      ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512);
    }
    else if ((it = chunk_name.find (sqmmt_ext)) != std::string::npos &&
             (sqmmt_ext.size () - (chunk_name.size () - it)) == 0)
    {
      PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a SQMMT file.\n", chunk_name.c_str ());

      unsigned int fsize = ltm_header.getFileSize ();
      char *buffer = new char[fsize];
      int result = static_cast<int> (::read (ltm_fd, reinterpret_cast<char*> (&buffer[0]), fsize));
      if (result == -1)
      {
        delete [] buffer;
        PCL_ERROR ("[pcl::LineRGBD::loadTemplates] Error reading SQMMT template from file!\n");
        break;
      }

      // Read a SQMMT file
      std::stringstream stream (std::stringstream::in | std::stringstream::out | std::stringstream::binary);
      stream.write (buffer, fsize);
      SparseQuantizedMultiModTemplate sqmmt;
      sqmmt.deserialize (stream);

      linemod_.addTemplate (sqmmt);
      object_ids_.push_back (object_id);

      // Increment the offset for the next file
      ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512);

      delete [] buffer;
    }

    if (static_cast<int> (pcl_lseek (ltm_fd, ltm_offset, SEEK_SET)) < 0)
      break;
  }

  // Close the file
  pcl_close (ltm_fd);

  // Compute 3D bounding boxes from the template point clouds
  bounding_boxes_.resize (template_point_clouds_.size ());
  for (size_t i = 0; i < template_point_clouds_.size (); ++i)
  {
    PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i];
    BoundingBoxXYZ & bb = bounding_boxes_[i];
    bb.x = bb.y = bb.z = std::numeric_limits<float>::max ();
    bb.width = bb.height = bb.depth = 0.0f;

    float center_x = 0.0f;
    float center_y = 0.0f;
    float center_z = 0.0f;
    float min_x = std::numeric_limits<float>::max ();
    float min_y = std::numeric_limits<float>::max ();
    float min_z = std::numeric_limits<float>::max ();
    float max_x = -std::numeric_limits<float>::max ();
    float max_y = -std::numeric_limits<float>::max ();
    float max_z = -std::numeric_limits<float>::max ();
    size_t counter = 0;
    for (size_t j = 0; j < template_point_cloud.size (); ++j)
    {
      const PointXYZRGBA & p = template_point_cloud.points[j];

      if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
        continue;

//.........这里部分代码省略.........

void
pcl::RadiusOutlierRemoval<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &output)
{
  output.is_dense = true;
  // If fields x/y/z are not present, we cannot filter
  if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1)
  {
    PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.data.clear ();
    return;
  }

  if (search_radius_ == 0.0)
  {
    PCL_ERROR ("[pcl::%s::applyFilter] No radius defined!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.data.clear ();
    return;
  }
  // Send the input dataset to the spatial locator
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg (*input_, *cloud);

  // Initialize the spatial locator
  if (!tree_)
  {
    if (cloud->isOrganized ())
      tree_.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ());
    else
      tree_.reset (new pcl::search::KdTree<pcl::PointXYZ> (false));
  }
  tree_->setInputCloud (cloud);

  // Allocate enough space to hold the results
  std::vector<int> nn_indices (indices_->size ());
  std::vector<float> nn_dists (indices_->size ());

  // Copy the common fields
  output.is_bigendian = input_->is_bigendian;
  output.point_step = input_->point_step;
  output.height = 1;

  output.data.resize (input_->width * input_->point_step); // reserve enough space
  removed_indices_->resize (input_->data.size ());

  int nr_p = 0;
  int nr_removed_p = 0;
  // Go over all the points and check which doesn't have enough neighbors
  for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
  {
    int k = tree_->radiusSearch ((*indices_)[cp], search_radius_, nn_indices, nn_dists);
    // Check if the number of neighbors is larger than the user imposed limit
    if (k < min_pts_radius_)
    {
      if (extract_removed_indices_)
      {
        (*removed_indices_)[nr_removed_p] = cp;
        nr_removed_p++;
      }
      continue;
    }

    memcpy (&output.data[nr_p * output.point_step], &input_->data[(*indices_)[cp] * output.point_step],
            output.point_step);
    nr_p++;
  }

  output.width = nr_p;
  output.height = 1;
  output.data.resize (output.width * output.point_step);
  output.row_step = output.point_step * output.width;

  removed_indices_->resize (nr_removed_p);
}

template <typename PointT> bool
pcl::registration::ELCH<PointT>::initCompute ()
{
  /*if (!PCLBase<PointT>::initCompute ())
  {
    PCL_ERROR ("[pcl::registration:ELCH::initCompute] Init failed.\n");
    return (false);
  }*/ //TODO

  if (!loop_start_)
  {
    PCL_ERROR ("[pcl::registration::ELCH::initCompute] no start of loop defined is empty!\n");
    deinitCompute ();
    return (false);
  }

  if (!loop_end_)
  {
    PCL_ERROR ("[pcl::registration::ELCH::initCompute] no end of loop defined is empty!\n");
    deinitCompute ();
    return (false);
  }

  //compute transformation if it's not given
  if (compute_loop_)
  {
    PointCloudPtr meta_start (new PointCloud);
    PointCloudPtr meta_end (new PointCloud);
    *meta_start = *(*loop_graph_)[loop_start_].cloud;
    *meta_end = *(*loop_graph_)[loop_end_].cloud;

    typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end;
    for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++)
      *meta_start += *(*loop_graph_)[*si].cloud;

    for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_end_, *loop_graph_); si != si_end; si++)
      *meta_end += *(*loop_graph_)[*si].cloud;

    //TODO use real pose instead of centroid
    //Eigen::Vector4f pose_start;
    //pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start);

    //Eigen::Vector4f pose_end;
    //pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end);

    PointCloudPtr tmp (new PointCloud);
    //Eigen::Vector4f diff = pose_start - pose_end;
    //Eigen::Translation3f translation (diff.head (3));
    //Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity ();
    //pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans);

    reg_->setInputTarget (meta_start);

    reg_->setInputCloud (meta_end);

    reg_->align (*tmp);

    loop_transform_ = reg_->getFinalTransformation ();
    //TODO hack
    //loop_transform_ *= trans.matrix ();

  }

  return (true);
}

template <typename PointT> bool
pcl::MaximumLikelihoodSampleConsensus<PointT>::computeModel (int debug_verbosity_level)
{
  // Warn and exit if no threshold was set
  if (threshold_ == std::numeric_limits<double>::max())
  {
    PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] No threshold set!\n");
    return (false);
  }

  iterations_ = 0;
  double d_best_penalty = std::numeric_limits<double>::max();
  double k = 1.0;

  std::vector<int> best_model;
  std::vector<int> selection;
  Eigen::VectorXf model_coefficients;
  std::vector<double> distances;

  // Compute sigma - remember to set threshold_ correctly !
  sigma_ = computeMedianAbsoluteDeviation (sac_model_->getInputCloud (), sac_model_->getIndices (), threshold_);
  if (debug_verbosity_level > 1)
    PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated sigma value: %f.\n", sigma_);

  // Compute the bounding box diagonal: V = sqrt (sum (max(pointCloud) - min(pointCloud)^2))
  Eigen::Vector4f min_pt, max_pt;
  getMinMax (sac_model_->getInputCloud (), sac_model_->getIndices (), min_pt, max_pt);
  max_pt -= min_pt;
  double v = sqrt (max_pt.dot (max_pt));

  int n_inliers_count = 0;
  size_t indices_size;
  unsigned skipped_count = 0;
  // supress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
  const unsigned max_skip = max_iterations_ * 10;
  
  // Iterate
  while (iterations_ < k && skipped_count < max_skip)
  {
    // Get X samples which satisfy the model criteria
    sac_model_->getSamples (iterations_, selection);

    if (selection.empty ()) break;

    // Search for inliers in the point cloud for the current plane model M
    if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
    {
      //iterations_++;
      ++ skipped_count;
      continue;
    }

    // Iterate through the 3d points and calculate the distances from them to the model
    sac_model_->getDistancesToModel (model_coefficients, distances);

    if (distances.empty ())
    {
      //iterations_++;
      ++skipped_count;
      continue;
    }
    
    // Use Expectiation-Maximization to find out the right value for d_cur_penalty
    // ---[ Initial estimate for the gamma mixing parameter = 1/2
    double gamma = 0.5;
    double p_outlier_prob = 0;

    indices_size = sac_model_->getIndices ()->size ();
    std::vector<double> p_inlier_prob (indices_size);
    for (int j = 0; j < iterations_EM_; ++j)
    {
      // Likelihood of a datum given that it is an inlier
      for (size_t i = 0; i < indices_size; ++i)
        p_inlier_prob[i] = gamma * exp (- (distances[i] * distances[i] ) / 2 * (sigma_ * sigma_) ) /
                           (sqrt (2 * M_PI) * sigma_);

      // Likelihood of a datum given that it is an outlier
      p_outlier_prob = (1 - gamma) / v;

      gamma = 0;
      for (size_t i = 0; i < indices_size; ++i)
        gamma += p_inlier_prob [i] / (p_inlier_prob[i] + p_outlier_prob);
      gamma /= static_cast<double>(sac_model_->getIndices ()->size ());
    }

    // Find the log likelihood of the model -L = -sum [log (pInlierProb + pOutlierProb)]
    double d_cur_penalty = 0;
    for (size_t i = 0; i < indices_size; ++i)
      d_cur_penalty += log (p_inlier_prob[i] + p_outlier_prob);
    d_cur_penalty = - d_cur_penalty;

    // Better match ?
    if (d_cur_penalty < d_best_penalty)
    {
      d_best_penalty = d_cur_penalty;

      // Save the current model/coefficients selection as being the best so far
      model_              = selection;
      model_coefficients_ = model_coefficients;

//.........这里部分代码省略.........