add accuracy (#7)

BENCHMARK.md

@@ -101,4 +101,19 @@ Results:
 - `BUILD_WITH_MARCH_NATIVE=OFF` : `Eigen::SimdInstructionSetsInUse()=SSE, SSE2`
 - `BUILD_WITH_MARCH_NATIVE=ON` : `Eigen::SimdInstructionSetsInUse()=AVX, SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2`
 
-![odometry_native](docs/assets/odometry_native.png)
+![odometry_native](docs/assets/odometry_native.png)
+
+**Accuracy**
+
+- `small_gicp::GICP` outputs mostly identical results to those of `fast_gicp::GICP`.
+- The results of `small_gicp::VGICP` slightly differ from `fast_gicp::VGICP`. Although the difference is marginal, it needs to be investigated.
+
+```
+pcl_gicp             : APE=6.451 +- 3.421  RPE(100)=2.424 +- 1.707  RPE(400)=8.416 +- 4.284  RPE(800)=12.652 +- 6.799
+fast_gicp            : APE=6.118 +- 3.078  RPE(100)=1.212 +- 0.717  RPE(400)=6.058 +- 3.128  RPE(800)=10.356 +- 6.335
+fast_vgicp           : APE=6.791 +- 3.215  RPE(100)=1.253 +- 0.734  RPE(400)=6.315 +- 3.011  RPE(800)=10.367 +- 6.147
+small_gicp           : APE=6.096 +- 3.056  RPE(100)=1.211 +- 0.717  RPE(400)=6.057 +- 3.123  RPE(800)=10.364 +- 6.336
+small_gicp (tbb)     : APE=6.096 +- 3.056  RPE(100)=1.211 +- 0.717  RPE(400)=6.057 +- 3.123  RPE(800)=10.364 +- 6.336
+small_gicp (omp)     : APE=6.096 +- 3.056  RPE(100)=1.211 +- 0.717  RPE(400)=6.057 +- 3.123  RPE(800)=10.364 +- 6.336
+small_vgicp          : APE=5.956 +- 2.725  RPE(100)=1.315 +- 0.762  RPE(400)=6.849 +- 3.401  RPE(800)=10.396 +- 6.972
+```

scripts/plot_odometry_accuracy.py

@@ -4,6 +4,7 @@ import os
 import pathos
 import subprocess
 from collections import namedtuple
+from matplotlib import pyplot
 
 def run_evo(commands):
   p = subprocess.Popen(commands, shell=False, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
@@ -49,26 +50,38 @@ def main():
     method = found[0][0] + '_' + found[0][1]
     filenames.append((method, results_path + '/' + filename))
   
-  Result = namedtuple('Result', ['ape', 'rpe'])
+  methods = ['pcl_1', 'fast_gicp_128', 'fast_vgicp_128', 'small_gicp_1', 'small_gicp_tbb_128', 'small_gicp_omp_128', 'small_vgicp_tbb_128']
-  def evaluate(filename):
+  labels = ['pcl_gicp', 'fast_gicp', 'fast_vgicp', 'small_gicp', 'small_gicp (tbb)', 'small_gicp (omp)', 'small_vgicp']
+  
+  Result = namedtuple('Result', ['ape', 'rpe100', 'rpe400', 'rpe800'])
+  def evaluate(inputs):
+    method, filename = inputs
+    print('.', end='', flush=True)
     ape = eval_ape(gt_path, filename)
-    rpe = eval_rpe(gt_path, filename, delta=100)
+    rpe100 = eval_rpe(gt_path, filename, delta=100)
-    return Result(ape, rpe)
+    rpe400 = eval_rpe(gt_path, filename, delta=400)
+    rpe800 = eval_rpe(gt_path, filename, delta=800)
+    return method, Result(ape, rpe100, rpe400, rpe800)
 
   print('evaluating')
   with pathos.multiprocessing.ProcessingPool() as p:
-    errors = p.map(evaluate, [filename for method, filename in filenames])
+    errors = p.map(evaluate, [(method, filename) for method, filename in filenames if method in methods])
+  print()
   
   results = {}
-  for (method, filename), error in zip(filenames, errors):
+  for method, error in errors:
     results[method] = error
-  
+    
-  methods = ['pcl_1', 'fast_gicp_1', 'fast_vgicp_1', 'small_gicp_1', 'small_gicp_tbb_1', 'small_gicp_omp_1', 'small_vgicp_tbb_1']
-  labels = ['pcl_gicp', 'fast_gicp', 'fast_vgicp', 'small_gicp', 'small_gicp (tbb)', 'small_gicp (omp)', 'small_vgicp']
-  
   for method, label in zip(methods, labels):
-    ape, rpe = results[method]
+    ape, rpe100, rpe400, rpe800 = results[method]
-    print('{:20s} : APE {:.3f} +- {:.3f}  RPE {:.3f} +- {:.3f}'.format(label, ape['rmse'], ape['std'], rpe['rmse'], rpe['std']))
+    print('{:20s} : APE={:.3f} +- {:.3f}  RPE(100)={:.3f} +- {:.3f}  RPE(400)={:.3f} +- {:.3f}  RPE(800)={:.3f} +- {:.3f}'
+          .format(label, ape['rmse'], ape['std'], rpe100['rmse'], rpe100['std'], rpe400['rmse'], rpe400['std'], rpe800['rmse'], rpe800['std']))
+  
+  fig, axes = pyplot.subplots(1, 3, figsize=(15, 5))
+  apes = [results[method].ape['rmse'] for method in methods]
+  axes[0].bar(labels, apes)
+  
+  pyplot.show()
   
     
   

src/benchmark/odometry_benchmark_small_gicp_tbb_flow.cpp

@@ -30,11 +30,20 @@ public:
   SmallGICPFlowEstimationTBB(const OdometryEstimationParams& params)
   : OdometryEstimation(params),
     control(tbb::global_control::max_allowed_parallelism, params.num_threads),
-    throughput(0.0) {}
+    throughput(0.0) {
+#ifdef BUILD_WITH_IRIDESCENCE
+    if (params.visualize) {
+      auto async_viewer = guik::async_viewer();
+      async_viewer->use_orbit_camera_control(250.0);
+    }
+#endif
+  }
 
   ~SmallGICPFlowEstimationTBB() {
 #ifdef BUILD_WITH_IRIDESCENCE
-    guik::async_destroy();
+    if (params.visualize) {
+      guik::async_destroy();
+    }
 #endif
   }