diff --git a/include/small_gicp/ann/kdtree.hpp b/include/small_gicp/ann/kdtree.hpp index c7e5d75..4ed25a8 100644 --- a/include/small_gicp/ann/kdtree.hpp +++ b/include/small_gicp/ann/kdtree.hpp @@ -11,7 +11,7 @@ namespace small_gicp { template 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 Adaptor> diff --git a/include/small_gicp/benchmark/benchmark_odom.hpp b/include/small_gicp/benchmark/benchmark_odom.hpp index 818f8d5..27431f0 100644 --- a/include/small_gicp/benchmark/benchmark_odom.hpp +++ b/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; diff --git a/scripts/plot_downsampling.py b/scripts/plot_downsampling.py index a3f05eb..8fa4a17 100644 --- a/scripts/plot_downsampling.py +++ b/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]') diff --git a/scripts/plot_odometry.py b/scripts/plot_odometry.py new file mode 100644 index 0000000..492064f --- /dev/null +++ b/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() \ No newline at end of file diff --git a/scripts/plot_odometry_accuracy.py b/scripts/plot_odometry_accuracy.py new file mode 100644 index 0000000..32515a9 --- /dev/null +++ b/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() diff --git a/src/downsampling_benchmark.cpp b/src/downsampling_benchmark.cpp index 391a71d..e0e8a62 100644 --- a/src/downsampling_benchmark.cpp +++ b/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; } } diff --git a/src/odometry_benchmark.cpp b/src/odometry_benchmark.cpp index b12d681..d79f9ef 100644 --- a/src/odometry_benchmark.cpp +++ b/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 (default: 4)" << std::endl; - std::cout << " --downsample_resolution (default: 0.25)" << std::endl; + std::cout << " --num_neighbors (default: 20)" << std::endl; + std::cout << " --downsampling_resolution (default: 0.25)" << std::endl; std::cout << " --voxel_resolution (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; diff --git a/src/odometry_benchmark_fast_gicp.cpp b/src/odometry_benchmark_fast_gicp.cpp index 0e5602e..c21f2f3 100644 --- a/src/odometry_benchmark_fast_gicp.cpp +++ b/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); } diff --git a/src/odometry_benchmark_fast_vgicp.cpp b/src/odometry_benchmark_fast_vgicp.cpp index f28f377..3e4d5f5 100644 --- a/src/odometry_benchmark_fast_vgicp.cpp +++ b/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); } diff --git a/src/odometry_benchmark_pcl.cpp b/src/odometry_benchmark_pcl.cpp index 15c7bca..8251cc4 100644 --- a/src/odometry_benchmark_pcl.cpp +++ b/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); } diff --git a/src/odometry_benchmark_small_gicp.cpp b/src/odometry_benchmark_small_gicp.cpp index c960b5a..a3b6022 100644 --- a/src/odometry_benchmark_small_gicp.cpp +++ b/src/odometry_benchmark_small_gicp.cpp @@ -18,7 +18,7 @@ public: sw.start(); auto tree = std::make_shared>(points); - estimate_covariances(*points, *tree, 10); + estimate_covariances(*points, *tree, params.num_neighbors); if (target_points == nullptr) { target_points = points; diff --git a/src/odometry_benchmark_small_gicp_omp.cpp b/src/odometry_benchmark_small_gicp_omp.cpp index 92431bd..ce05771 100644 --- a/src/odometry_benchmark_small_gicp_omp.cpp +++ b/src/odometry_benchmark_small_gicp_omp.cpp @@ -18,7 +18,7 @@ public: sw.start(); auto tree = std::make_shared>(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; diff --git a/src/odometry_benchmark_small_gicp_tbb.cpp b/src/odometry_benchmark_small_gicp_tbb.cpp index 247c37a..50d7cac 100644 --- a/src/odometry_benchmark_small_gicp_tbb.cpp +++ b/src/odometry_benchmark_small_gicp_tbb.cpp @@ -22,7 +22,7 @@ public: sw.start(); auto tree = std::make_shared>(points); - estimate_covariances_tbb(*points, *tree, 10); + estimate_covariances_tbb(*points, *tree, params.num_neighbors); if (target_points == nullptr) { target_points = points; diff --git a/src/odometry_benchmark_small_gicp_tbb_flow.cpp b/src/odometry_benchmark_small_gicp_tbb_flow.cpp index 3bde309..24af6b5 100644 --- a/src/odometry_benchmark_small_gicp_tbb_flow.cpp +++ b/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>(input->points); - estimate_covariances(*input->points, *input->kdtree, 10); + estimate_covariances(*input->points, *input->kdtree, params.num_neighbors); return input; }); tbb::flow::sequencer_node postpre_sequencer_node(graph, [](const InputFrame::Ptr& input) { return input->id; }); diff --git a/src/odometry_benchmark_small_vgicp_model_tbb.cpp b/src/odometry_benchmark_small_vgicp_model_tbb.cpp index a039f49..c8d4e4d 100644 --- a/src/odometry_benchmark_small_vgicp_model_tbb.cpp +++ b/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(params.voxel_resolution); diff --git a/src/odometry_benchmark_small_vgicp_omp.cpp b/src/odometry_benchmark_small_vgicp_omp.cpp index 691a546..2906d98 100644 --- a/src/odometry_benchmark_small_vgicp_omp.cpp +++ b/src/odometry_benchmark_small_vgicp_omp.cpp @@ -19,7 +19,7 @@ public: sw.start(); auto tree = std::make_shared>(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(params.voxel_resolution); voxelmap->insert(*points); diff --git a/src/odometry_benchmark_small_vgicp_tbb.cpp b/src/odometry_benchmark_small_vgicp_tbb.cpp index 6f10b72..5320e33 100644 --- a/src/odometry_benchmark_small_vgicp_tbb.cpp +++ b/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(params.voxel_resolution); voxelmap->insert(*points);