tools/compute_cloud_error bug fix

git-svn-id: svn+ssh://svn.pointclouds.org/pcl/trunk@3262 a9d63959-f2ad-4865-b262-bf0e56cfafb6

tools/compute_cloud_error.cpp

@@ -68,13 +68,13 @@ bool
 loadCloud (const std::string &filename, sensor_msgs::PointCloud2 &cloud)
 {
   TicToc tt;
-  print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
+//  print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
 
   tt.tic ();
   if (loadPCDFile (filename, cloud) < 0)
     return (false);
-  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
-  print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
+//  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" seconds : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
+//  print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
 
   return (true);
 }
@@ -101,7 +101,7 @@ compute (const sensor_msgs::PointCloud2::ConstPtr &cloud_source, const sensor_ms
 
   if (correspondence_type == "index")
   {
-    print_highlight (stderr, "Computing using the equal indices correspondence heuristic.\n");
+//    print_highlight (stderr, "Computing using the equal indices correspondence heuristic.\n");
 
     if (xyz_source->points.size () != xyz_target->points.size ())
     {
@@ -129,7 +129,7 @@ compute (const sensor_msgs::PointCloud2::ConstPtr &cloud_source, const sensor_ms
   }
   else if (correspondence_type == "nn")
   {
-    print_highlight (stderr, "Computing using the nearest neighbor correspondence heuristic.\n");
+//    print_highlight (stderr, "Computing using the nearest neighbor correspondence heuristic.\n");
 
     KdTreeFLANN<PointXYZ>::Ptr tree (new KdTreeFLANN<PointXYZ> ());
     tree->setInputCloud (xyz_target);
@@ -141,7 +141,7 @@ compute (const sensor_msgs::PointCloud2::ConstPtr &cloud_source, const sensor_ms
 
       std::vector<int> nn_indices (1);
       std::vector<float> nn_distances (1);
-      if (!tree->nearestKSearch (point_i, 1, nn_indices, nn_distances))
+      if (!tree->nearestKSearch (xyz_source->points[point_i], 1, nn_indices, nn_distances))
         continue;
       size_t point_nn_i = nn_indices.front();
 
@@ -158,7 +158,7 @@ compute (const sensor_msgs::PointCloud2::ConstPtr &cloud_source, const sensor_ms
   }
   else if (correspondence_type == "nnplane")
   {
-    print_highlight (stderr, "Computing using the nearest neighbor plane projection correspondence heuristic.\n");
+//    print_highlight (stderr, "Computing using the nearest neighbor plane projection correspondence heuristic.\n");
 
     PointCloud<Normal>::Ptr normals_target (new PointCloud<Normal> ());
     fromROSMsg (*cloud_target, *normals_target);
@@ -173,7 +173,7 @@ compute (const sensor_msgs::PointCloud2::ConstPtr &cloud_source, const sensor_ms
 
       std::vector<int> nn_indices (1);
       std::vector<float> nn_distances (1);
-      if (!tree->nearestKSearch (point_i, 1, nn_indices, nn_distances))
+      if (!tree->nearestKSearch (xyz_source->points[point_i], 1, nn_indices, nn_distances))
         continue;
       size_t point_nn_i = nn_indices.front();
 
@@ -193,13 +193,13 @@ compute (const sensor_msgs::PointCloud2::ConstPtr &cloud_source, const sensor_ms
   }
   else
   {
-    print_error ("Unrecognized correspondence type. Check legal arguments by using the -h option\n");
+//    print_error ("Unrecognized correspondence type. Check legal arguments by using the -h option\n");
     return;
   }
 
   toROSMsg (*output_xyzi, output);
 
-  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
+//  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" seconds]\n");
   print_highlight ("RMSE Error: %f\n", rmse);
 }
 
@@ -209,18 +209,18 @@ saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &output)
   TicToc tt;
   tt.tic ();
 
-  print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
+//  print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
 
   pcl::io::savePCDFile (filename, output);
 
-  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
+//  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" seconds : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
 }
 
 /* ---[ */
 int
 main (int argc, char** argv)
 {
-  print_info ("Compute the differences between two point clouds and visualizing them as an output intensity cloud. For more information, use: %s -h\n", argv[0]);
+//  print_info ("Compute the differences between two point clouds and visualizing them as an output intensity cloud. For more information, use: %s -h\n", argv[0]);
 
   if (argc < 4)
   {