default to 20 neighbors

include/small_gicp/ann/kdtree.hpp

@@ -11,7 +11,7 @@ namespace small_gicp {
 template <typename Distance, class DatasetAdaptor, int DIM = -1, typename IndexType = size_t>
 class KDTreeSingleIndexAdaptor;
 
-/// @brief Unsafe KdTree with arbitrary nanoflann Adaptor
+/// @brief Unsafe KdTree with arbitrary nanoflann Adaptor.
 /// @note  This class does not hold the ownership of the input points.
 ///        You must keep the input points along with this class.
 template <class PointCloud, template <typename, typename, int, typename> class Adaptor>

include/small_gicp/benchmark/benchmark_odom.hpp

@@ -16,6 +16,7 @@ struct OdometryEstimationParams {
 public:
   bool visualize = false;
   int num_threads = 4;
+  int num_neighbors = 20;
   double downsample_resolution = 0.25;
   double voxel_resolution = 1.0;
   double max_correspondence_distance = 1.0;

scripts/plot_downsampling.py

@@ -83,8 +83,8 @@ def main():
     
     baseline = results['pcl_voxelgrid'].time_mean[leaf_size_index]
     axes[i].plot([num_threads[0], num_threads[-1]], [baseline, baseline], label='pcl_voxelgrid', linestyle='--')
-    axes[i].scatter(num_threads, time_tbb, label='small_voxelgrid_tbb')
-    axes[i].scatter(num_threads, time_omp, label='small_voxelgrid_omp')
+    axes[i].scatter(num_threads, time_tbb, label='small_voxelgrid_tbb', marker='^')
+    axes[i].scatter(num_threads, time_omp, label='small_voxelgrid_omp', marker='^')
     
     axes[i].grid()
     axes[i].set_xscale('log')
@@ -98,11 +98,14 @@ def main():
   fig, axes = pyplot.subplots(1, 2)
   fig.set_size_inches(18, 3)
 
-  methods = ['pcl_voxelgrid', 'pcl_approx_voxelgrid', 'small_voxelgrid', 'small_voxelgrid_tbb (2 threads)', 'small_voxelgrid_tbb (3 threads)', 'small_voxelgrid_tbb (4 threads)', 'small_voxelgrid_tbb (6 threads)']
-  labels = ['pcl_voxelgrid', 'pcl_approx_voxelgrid', 'small_voxelgrid', 'small_voxelgrid_tbb (2 threads)', 'small_voxelgrid_tbb (3 threads)', 'small_voxelgrid_tbb (4 threads)', 'small_voxelgrid_tbb (6 threads)']
-  for method, label in zip(methods, labels):
-    axes[0].plot(leaf_sizes, results[method].time_mean, label=label, linestyle='--' if 'pcl' in method else '-')
-    axes[1].plot(leaf_sizes, results[method].points_mean / results['pcl_voxelgrid'].points_mean, label=label, linestyle='--' if 'pcl' in method else '-')
+  methods = ['small_voxelgrid', 'small_voxelgrid_tbb (2 threads)', 'small_voxelgrid_tbb (3 threads)', 'small_voxelgrid_tbb (4 threads)', 'small_voxelgrid_tbb (6 threads)', 'pcl_voxelgrid', 'pcl_approx_voxelgrid']
+  labels = ['small_voxelgrid', 'small_voxelgrid_tbb (2 threads)', 'small_voxelgrid_tbb (3 threads)', 'small_voxelgrid_tbb (4 threads)', 'small_voxelgrid_tbb (6 threads)', 'pcl_voxelgrid', 'pcl_approx_voxelgrid']
+  markers = ['^', '^', '^', '^', '^', 'o', 'o']
+  for method, label, marker in zip(methods, labels, markers):
+    axes[0].plot(leaf_sizes, results[method].time_mean, label=label, linestyle='--' if 'pcl' in method else '-', marker=marker)
+    
+    if 'threads' not in method or '2 threads' in method:
+      axes[1].plot(leaf_sizes, results[method].points_mean / results['pcl_voxelgrid'].points_mean, label=label, linestyle='--' if 'pcl' in method else '-', marker=marker)
   
   axes[0].set_xlabel('Leaf size [m]')
   axes[0].set_ylabel('Processing time [msec/scan]')

scripts/plot_odometry.py

@@ -0,0 +1,88 @@
+#!/usr/bin/python3
+import os
+import re
+import numpy
+from collections import namedtuple
+from matplotlib import pyplot
+
+Result = namedtuple('Result', ['reg_mean', 'reg_std', 'tp_mean', 'tp_std'])
+
+def parse_result(filename):
+  reg_mean = None
+  reg_std = None
+  throughput_mean = None
+  throughput_std = None
+  with open(filename, 'r') as f:
+    for line in f.readlines():
+      found = re.findall(r'([^=]+)\s*\+\-\s*(\S+)', line)
+      if not found or len(found) != 2:
+        found = re.findall(r'total_throughput=(\S+)', line)
+        if found:
+          throughput_mean = float(found[0])
+        continue
+      
+      reg_mean = float(found[0][0].strip())
+      reg_std = float(found[0][1].strip())
+      throughput_mean = float(found[1][0].strip())
+      throughput_std = float(found[1][1].strip())
+    
+  return Result(reg_mean, reg_std, throughput_mean, throughput_std)
+
+def main():
+  results_path = os.path.dirname(__file__) + '/results'
+
+  results = {}
+  for filename in os.listdir(results_path):
+    found = re.findall(r'odometry_benchmark_(\S+)_(\d+).txt', filename)
+    if not found:
+      continue    
+   
+    rets = parse_result(results_path + '/' + filename)
+    results['{}_{}'.format(found[0][0], found[0][1])] = rets
+  
+  fig, axes = pyplot.subplots(2, 2, figsize=(24, 12))
+  
+  num_threads = [1, 2, 4, 8, 16, 32, 64, 128]
+
+  pcl_reg = results['pcl_1'].reg_mean
+  pcl_tp = results['pcl_1'].tp_mean
+  axes[0, 0].plot([num_threads[0], num_threads[-1]], [pcl_reg, pcl_reg], label='pcl_gicp', linestyle='--')
+  axes[0, 1].plot([num_threads[0], num_threads[-1]], [pcl_tp, pcl_tp], label='pcl_gicp', linestyle='--')
+  axes[1, 0].plot([num_threads[0], num_threads[-1]], [1.0, 1.0], label='pcl_gicp', linestyle='--')
+  axes[1, 1].plot([num_threads[0], num_threads[-1]], [1.0, 1.0], label='pcl_gicp', linestyle='--')
+
+  methods = ['fast_gicp', 'fast_vgicp', 'small_gicp_omp', 'small_gicp_tbb', 'small_vgicp_tbb', 'small_vgicp_omp']
+  markers = ['o', 'o', '^', '^', 's', 's']
+
+  for method, marker in zip(methods, markers):
+    reg_means = [results['{}_{}'.format(method, N)].reg_mean for N in num_threads]
+    axes[0, 0].plot(num_threads, reg_means, label=method, marker=marker)
+    axes[1, 0].plot(num_threads, pcl_reg / numpy.array(reg_means), label=method, marker=marker)
+
+  for method, marker in zip(methods, markers):
+    tp_means = [results['{}_{}'.format(method, N)].tp_mean for N in num_threads]
+    axes[0, 1].plot(num_threads, tp_means, label=method, marker=marker)
+    axes[1, 1].plot(num_threads, pcl_tp / numpy.array(tp_means), label=method, marker=marker)
+  flow_tp_means = [results['small_gicp_tbb_flow_{}'.format(N)].tp_mean for N in num_threads]
+  axes[0, 1].plot(num_threads, flow_tp_means, label='small_gicp_tbb_flow', marker='*')
+  axes[1, 1].plot(num_threads, pcl_tp / numpy.array(flow_tp_means), label='small_gicp_tbb_flow', marker='*')
+
+  axes[0, 0].set_title('Net registration time (KdTree construction + cov estimation + pose estimation)')
+  axes[1, 0].set_title('Net registration time (KdTree construction + cov estimation + pose estimation)')
+  axes[0, 1].set_title('Total throughput (Downsampling + registration)')
+  axes[1, 1].set_title('Total throughput (Downsampling + registration)')
+  axes[0, 0].set_ylabel('Time [msec/scan]')
+  axes[0, 1].set_ylabel('Time [msec/scan]')
+  axes[1, 0].set_ylabel('Processing speed ratio (pcl_gicp=1.0)')
+  axes[1, 1].set_ylabel('Processing speed ratio (pcl_gicp=1.0)')
+  for i in range(2):
+    for j in range(2):
+      axes[i, j].set_xlabel('Number of threads = [1, 2, 4, ..., 128]')
+      axes[i, j].set_xscale('log')
+      axes[i, j].legend()
+      axes[i, j].grid()
+  
+  pyplot.show()
+
+if __name__ == "__main__":
+  main()

scripts/plot_odometry_accuracy.py

@@ -0,0 +1,78 @@
+#!/usr/bin/python3
+import re
+import os
+import pathos
+import subprocess
+from collections import namedtuple
+
+def run_evo(commands):
+  p = subprocess.Popen(commands, shell=False, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
+  p.wait()
+  stdout, stderr = p.communicate()
+
+  if len(stderr):
+    print(stderr.decode('utf-8'))
+
+  result = stdout.decode('utf-8')
+  results = {}
+  for item in re.findall(r'([a-z]+)\s+([0-9]+\.[0-9]+)', result):
+    results[item[0]] = float(item[1])
+
+  return results
+
+
+def eval_ape(gt_filename, traj_filename, t_offset=0.0):
+  ret = run_evo(['evo_ape', 'kitti', gt_filename, traj_filename, '-a'])
+  return ret
+
+
+def eval_rpe(gt_filename, traj_filename, delta_unit='m', delta=100, all_pairs=True, t_offset=0.0):
+  commands = ['evo_rpe', 'kitti', gt_filename, traj_filename, '-a', '--delta_unit', str(delta_unit), '--delta', str(delta)]
+  if all_pairs:
+    commands += ['--all_pairs']
+
+  ret = run_evo(commands)
+  return ret
+
+
+def main():
+  gt_path = '/home/koide/datasets/ssd/kitti/poses/00_lidar.txt'
+
+  results_path = os.path.dirname(__file__) + '/results'
+  
+  filenames = []
+  for filename in os.listdir(results_path):
+    found = re.findall(r'traj_lidar_(\S+)_(\d+).txt', filename)
+    if not found:
+      continue
+    
+    method = found[0][0] + '_' + found[0][1]
+    filenames.append((method, results_path + '/' + filename))
+  
+  Result = namedtuple('Result', ['ape', 'rpe'])
+  def evaluate(filename):
+    ape = eval_ape(gt_path, filename)
+    rpe = eval_rpe(gt_path, filename, delta=100)
+    return Result(ape, rpe)
+
+  print('evaluating')
+  with pathos.multiprocessing.ProcessingPool() as p:
+    errors = p.map(evaluate, [filename for method, filename in filenames])
+  
+  results = {}
+  for (method, filename), error in zip(filenames, 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]
+    print('{:20s} : APE {:.3f} +- {:.3f}  RPE {:.3f} +- {:.3f}'.format(label, ape['rmse'], ape['std'], rpe['rmse'], rpe['std']))
+  
+    
+  
+  
+
+if __name__ == '__main__':
+  main()

src/downsampling_benchmark.cpp

@@ -59,11 +59,15 @@ int main(int argc, char** argv) {
   int num_threads = 4;
   size_t max_num_frames = 1000;
 
-  for (auto arg = argv + 2; arg != argv + argc; arg++) {
-    if (std::string(*arg) == "--num_threads") {
-      num_threads = std::stoi(*(arg + 1));
-    } else if (std::string(*arg) == "--max_num_frames") {
-      max_num_frames = std::stoul(*(arg + 1));
+  for (int i = 1; i < argc; i++) {
+    const std::string arg(argv[i]);
+    if (arg == "--num_threads") {
+      num_threads = std::stoi(argv[i + 1]);
+    } else if (arg == "--max_num_frames") {
+      max_num_frames = std::stoul(argv[i + 1]);
+    } else if (arg.size() >= 2 && arg.substr(0, 2) == "--") {
+      std::cerr << "unknown option: " << arg << std::endl;
+      return 1;
     }
   }
 

src/odometry_benchmark.cpp

@@ -15,7 +15,8 @@ int main(int argc, char** argv) {
     std::cout << "OPTIONS:" << std::endl;
     std::cout << "  --visualize" << std::endl;
     std::cout << "  --num_threads <value> (default: 4)" << std::endl;
-    std::cout << "  --downsample_resolution <value> (default: 0.25)" << std::endl;
+    std::cout << "  --num_neighbors <value> (default: 20)" << std::endl;
+    std::cout << "  --downsampling_resolution <value> (default: 0.25)" << std::endl;
     std::cout << "  --voxel_resolution <value> (default: 2.0)" << std::endl;
 
     const auto odom_names = odometry_names();
@@ -37,17 +38,23 @@ int main(int argc, char** argv) {
   OdometryEstimationParams params;
   std::string engine = "small_gicp";
 
-  for (auto arg = argv + 1; arg != argv + argc; arg++) {
-    if (std::string(*arg) == "--visualize") {
+  for (int i = 0; i < argc; i++) {
+    const std::string arg = argv[i];
+    if (arg == "--visualize") {
       params.visualize = true;
-    } else if (std::string(*arg) == "--num_threads") {
-      params.num_threads = std::stoi(*(arg + 1));
-    } else if (std::string(*arg) == "--downsampling_resolution") {
-      params.downsample_resolution = std::stod(*(arg + 1));
-    } else if (std::string(*arg) == "--voxel_resolution") {
-      params.voxel_resolution = std::stod(*(arg + 1));
-    } else if (std::string(*arg) == "--engine") {
-      engine = *(arg + 1);
+    } else if (arg == "--num_threads") {
+      params.num_threads = std::stoi(argv[i + 1]);
+    } else if (arg == "--num_neighbors") {
+      params.num_neighbors = std::stoi(argv[i + 1]);
+    } else if (arg == "--downsampling_resolution") {
+      params.downsample_resolution = std::stod(argv[i + 1]);
+    } else if (arg == "--voxel_resolution") {
+      params.voxel_resolution = std::stod(argv[i + 1]);
+    } else if (arg == "--engine") {
+      engine = argv[i + 1];
+    } else if (arg.size() >= 2 && arg.substr(0, 2) == "--") {
+      std::cerr << "unknown option: " << arg << std::endl;
+      return 1;
     }
   }
 
@@ -55,6 +62,7 @@ int main(int argc, char** argv) {
   std::cout << "output_path=" << output_path << std::endl;
   std::cout << "registration_engine=" << engine << std::endl;
   std::cout << "num_threads=" << params.num_threads << std::endl;
+  std::cout << "num_neighbors=" << params.num_neighbors << std::endl;
   std::cout << "downsampling_resolution=" << params.downsample_resolution << std::endl;
   std::cout << "voxel_resolution=" << params.voxel_resolution << std::endl;
   std::cout << "visualize=" << params.visualize << std::endl;

src/odometry_benchmark_fast_gicp.cpp

@@ -13,7 +13,7 @@ namespace small_gicp {
 class FastGICPOdometryEstimation : public OnlineOdometryEstimation {
 public:
   FastGICPOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) {
-    gicp.setCorrespondenceRandomness(10);
+    gicp.setCorrespondenceRandomness(params.num_neighbors);
     gicp.setMaxCorrespondenceDistance(params.max_correspondence_distance);
     gicp.setNumThreads(params.num_threads);
   }

src/odometry_benchmark_fast_vgicp.cpp

@@ -15,8 +15,9 @@ namespace small_gicp {
 class FastVGICPOdometryEstimation : public OnlineOdometryEstimation {
 public:
   FastVGICPOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) {
-    vgicp.setCorrespondenceRandomness(10);
+    vgicp.setCorrespondenceRandomness(params.num_neighbors);
     vgicp.setResolution(params.voxel_resolution);
+    vgicp.setMaxCorrespondenceDistance(params.max_correspondence_distance);
     vgicp.setNumThreads(params.num_threads);
   }
 

src/odometry_benchmark_pcl.cpp

@@ -9,7 +9,7 @@ namespace small_gicp {
 class PCLOnlineOdometryEstimation : public OnlineOdometryEstimation {
 public:
   PCLOnlineOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) {
-    gicp.setCorrespondenceRandomness(10);
+    gicp.setCorrespondenceRandomness(params.num_neighbors);
     gicp.setMaxCorrespondenceDistance(params.max_correspondence_distance);
   }
 

src/odometry_benchmark_small_gicp.cpp

@@ -18,7 +18,7 @@ public:
     sw.start();
 
     auto tree = std::make_shared<KdTree<PointCloud>>(points);
-    estimate_covariances(*points, *tree, 10);
+    estimate_covariances(*points, *tree, params.num_neighbors);
 
     if (target_points == nullptr) {
       target_points = points;

src/odometry_benchmark_small_gicp_omp.cpp

@@ -18,7 +18,7 @@ public:
     sw.start();
 
     auto tree = std::make_shared<KdTreeOMP<PointCloud>>(points, params.num_threads);
-    estimate_covariances_omp(*points, *tree, 10, params.num_threads);
+    estimate_covariances_omp(*points, *tree, params.num_neighbors, params.num_threads);
 
     if (target_points == nullptr) {
       target_points = points;

src/odometry_benchmark_small_gicp_tbb.cpp

@@ -22,7 +22,7 @@ public:
     sw.start();
 
     auto tree = std::make_shared<KdTreeTBB<PointCloud>>(points);
-    estimate_covariances_tbb(*points, *tree, 10);
+    estimate_covariances_tbb(*points, *tree, params.num_neighbors);
 
     if (target_points == nullptr) {
       target_points = points;

src/odometry_benchmark_small_gicp_tbb_flow.cpp

@@ -42,7 +42,7 @@ public:
       input->sw.start();
       input->points = voxelgrid_sampling(*input->points, params.downsample_resolution);
       input->kdtree = std::make_shared<KdTree<PointCloud>>(input->points);
-      estimate_covariances(*input->points, *input->kdtree, 10);
+      estimate_covariances(*input->points, *input->kdtree, params.num_neighbors);
       return input;
     });
     tbb::flow::sequencer_node<InputFrame::Ptr> postpre_sequencer_node(graph, [](const InputFrame::Ptr& input) { return input->id; });

src/odometry_benchmark_small_vgicp_model_tbb.cpp

@@ -21,7 +21,7 @@ public:
     Stopwatch sw;
     sw.start();
 
-    estimate_covariances_tbb(*points, 10);
+    estimate_covariances_tbb(*points, params.num_neighbors);
 
     if (voxelmap == nullptr) {
       voxelmap = std::make_shared<GaussianVoxelMap>(params.voxel_resolution);

src/odometry_benchmark_small_vgicp_omp.cpp

@@ -19,7 +19,7 @@ public:
     sw.start();
 
     auto tree = std::make_shared<KdTreeOMP<PointCloud>>(points, params.num_threads);
-    estimate_covariances_omp(*points, *tree, 10, params.num_threads);
+    estimate_covariances_omp(*points, *tree, params.num_neighbors, params.num_threads);
 
     auto voxelmap = std::make_shared<GaussianVoxelMap>(params.voxel_resolution);
     voxelmap->insert(*points);

src/odometry_benchmark_small_vgicp_tbb.cpp

@@ -21,7 +21,7 @@ public:
     Stopwatch sw;
     sw.start();
 
-    estimate_covariances_tbb(*points, 10);
+    estimate_covariances_tbb(*points, params.num_neighbors);
 
     auto voxelmap = std::make_shared<GaussianVoxelMap>(params.voxel_resolution);
     voxelmap->insert(*points);