Function compute

tools/compute_cloud_error.cpp:67–184

}

void
compute (const pcl::PCLPointCloud2::ConstPtr &cloud_source, const pcl::PCLPointCloud2::ConstPtr &cloud_target,
         pcl::PCLPointCloud2 &output, const std::string &correspondence_type)
{
  PointCloud<PointXYZ>::Ptr xyz_source (new PointCloud<PointXYZ> ());
  fromPCLPointCloud2 (*cloud_source, *xyz_source);
  PointCloud<PointXYZ>::Ptr xyz_target (new PointCloud<PointXYZ> ());
  fromPCLPointCloud2 (*cloud_target, *xyz_target);

  PointCloud<PointXYZI>::Ptr output_xyzi (new PointCloud<PointXYZI> ());
  output_xyzi->points.resize (xyz_source->size ());
  output_xyzi->height = cloud_source->height;
  output_xyzi->width = cloud_source->width;

  float rmse = 0.0f;

  if (correspondence_type == "index")
  {
//    print_highlight (stderr, "Computing using the equal indices correspondence heuristic.\n");

    if (xyz_source->size () != xyz_target->size ())
    {
      print_error ("Source and target clouds do not have the same number of points.\n");
      return;
    }

    for (std::size_t point_i = 0; point_i < xyz_source->size (); ++point_i)
    {
      if (!std::isfinite ((*xyz_source)[point_i].x) || !std::isfinite ((*xyz_source)[point_i].y) || !std::isfinite ((*xyz_source)[point_i].z))
        continue;
      if (!std::isfinite ((*xyz_target)[point_i].x) || !std::isfinite ((*xyz_target)[point_i].y) || !std::isfinite ((*xyz_target)[point_i].z))
        continue;


      float dist = squaredEuclideanDistance ((*xyz_source)[point_i], (*xyz_target)[point_i]);
      rmse += dist;

      (*output_xyzi)[point_i].x = (*xyz_source)[point_i].x;
      (*output_xyzi)[point_i].y = (*xyz_source)[point_i].y;
      (*output_xyzi)[point_i].z = (*xyz_source)[point_i].z;
      (*output_xyzi)[point_i].intensity = dist;
    }
    rmse = std::sqrt (rmse / static_cast<float> (xyz_source->size ()));
  }
  else if (correspondence_type == "nn")
  {
//    print_highlight (stderr, "Computing using the nearest neighbor correspondence heuristic.\n");

    KdTreeFLANN<PointXYZ>::Ptr tree (new KdTreeFLANN<PointXYZ> ());
    tree->setInputCloud (xyz_target);

    for (std::size_t point_i = 0; point_i < xyz_source->size (); ++ point_i)
    {
      if (!std::isfinite ((*xyz_source)[point_i].x) || !std::isfinite ((*xyz_source)[point_i].y) || !std::isfinite ((*xyz_source)[point_i].z))
        continue;

      pcl::Indices nn_indices (1);
      std::vector<float> nn_distances (1);