mirror
/
small_gicp
mirror of https://github.com/koide3/small_gicp.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

scripts

This commit is contained in:
k.koide 2024-03-27 18:47:22 +09:00
parent ef72d7ce9b
commit e4b88db3a2
8 changed files with 190 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -1,3 +1,5 @@
.vscode/*
build/*
imgui.ini
scripts/results/*

19
include/small_gicp/benchmark/benchmark_odom.hpp
View File

@ -14,6 +14,7 @@ namespace small_gicp {
struct OdometryEstimationParams {
public:
bool visualize = false;
int num_threads = 4;
double downsample_resolution = 0.25;
double voxel_resolution = 1.0;
@ -38,7 +39,7 @@ protected:
class OnlineOdometryEstimation : public OdometryEstimation {
public:
OnlineOdometryEstimation(const OdometryEstimationParams& params) : OdometryEstimation(params), z_range(-5.0f, 5.0f) {}
~OnlineOdometryEstimation() { guik::async_destroy(); }
~OnlineOdometryEstimation() {}
std::vector<Eigen::Isometry3d> estimate(std::vector<PointCloud::Ptr>& points) override {
std::vector<Eigen::Isometry3d> traj;
@ -54,13 +55,15 @@ public:
const Eigen::Isometry3d T = estimate(downsampled);
traj.emplace_back(T);
auto async_viewer = guik::async_viewer();
z_range[0] = std::min<double>(z_range[0], T.translation().z() - 5.0f);
z_range[1] = std::max<double>(z_range[1], T.translation().z() + 5.0f);
async_viewer->invoke([=] { guik::viewer()->shader_setting().add("z_range", z_range); });
async_viewer->update_points("current", downsampled->points, guik::FlatOrange(T).set_point_scale(2.0f));
async_viewer->update_points(guik::anon(), voxelgrid_sampling(*downsampled, 1.0)->points, guik::Rainbow(T));
async_viewer->lookat(T.translation().cast<float>());
if (params.visualize) {
auto async_viewer = guik::async_viewer();
z_range[0] = std::min<double>(z_range[0], T.translation().z() - 5.0f);
z_range[1] = std::max<double>(z_range[1], T.translation().z() + 5.0f);
async_viewer->invoke([=] { guik::viewer()->shader_setting().add("z_range", z_range); });
async_viewer->update_points("current", downsampled->points, guik::FlatOrange(T).set_point_scale(2.0f));
async_viewer->update_points(guik::anon(), voxelgrid_sampling(*downsampled, 1.0)->points, guik::Rainbow(T));
async_viewer->lookat(T.translation().cast<float>());
}
points[i].reset();

120
scripts/plot_downsampling.py Normal file
View File

@ -0,0 +1,120 @@
#!/usr/bin/python3
import re
import os
import numpy
from collections import namedtuple
from matplotlib import pyplot
Result = namedtuple('Result', ['time_mean', 'time_std', 'points_mean', 'points_std'])
def parse_result(filename):
leaf_sizes = []
results = {}
with open(filename, 'r') as f:
for line in f.readlines():
if '(warmup)' in line:
continue
found = re.findall(r'leaf_size=(\S+)', line)
if found:
leaf_sizes.append(float(found[0]))
if len(leaf_sizes) == 0:
continue
stat = r'\s*(\S+)\s*\+\-\s*(\S+)\s*'
found = re.findall(stat, line)
if len(found) != 2:
continue
name = line.split(':')[0].strip()
if name not in results:
results[name] = []
results[name].append(Result(float(found[0][0]), float(found[0][1]), float(found[1][0]), float(found[1][1])))
return leaf_sizes, results
def main():
results_path = os.path.dirname(__file__) + '/results'
leaf_sizes = None
raw_results = []
for filename in os.listdir(results_path):
found = re.findall(r'downsampling_benchmark_(\d+).txt', filename)
if not found:
continue
leaf_sizes, rets = parse_result(results_path + '/' + filename)
raw_results.append((int(found[0]), rets))
raw_results = sorted(raw_results, key=lambda x: x[0])
def summarize(rets):
time_mean = numpy.array([x.time_mean for x in rets])
time_std = numpy.array([x.time_std for x in rets])
points_mean = numpy.array([x.points_mean for x in rets])
points_std = numpy.array([x.points_std for x in rets])
return Result(time_mean, time_std, points_mean, points_std)
results = {}
results['pcl_voxelgrid'] = summarize(raw_results[0][1]['pcl_voxelgrid'])
results['pcl_approx_voxelgrid'] = summarize(raw_results[0][1]['pcl_approx_voxelgrid'])
results['small_voxelgrid'] = summarize(raw_results[0][1]['small_voxelgrid'])
for num_threads, rets in raw_results:
results['small_voxelgrid_tbb ({} threads)'.format(num_threads)] = summarize(rets['small_voxelgrid_tbb'])
results['small_voxelgrid_omp ({} threads)'.format(num_threads)] = summarize(rets['small_voxelgrid_omp'])
fig, axes = pyplot.subplots(1, 5)
fig.set_size_inches(18, 3)
leaf_size_indices = [0, 1, 4, 9, -1]
for i, leaf_size_index in enumerate(leaf_size_indices):
leaf_size = leaf_sizes[leaf_size_index]
num_threads = [x[0] for x in raw_results]
time_tbb = []
time_omp = []
for N in [x[0] for x in raw_results]:
time_tbb.append(results['small_voxelgrid_tbb ({} threads)'.format(N)].time_mean[leaf_size_index])
time_omp.append(results['small_voxelgrid_omp ({} threads)'.format(N)].time_mean[leaf_size_index])
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].grid()
axes[i].set_xscale('log')
axes[i].set_xlabel('Num threads')
axes[i].set_title('Leaf size = {} m'.format(leaf_size))
axes[0].set_ylabel('Processing time [msec/scan]')
axes[2].legend(loc='upper center', bbox_to_anchor=(0.5, -0.11), ncol=3)
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 '-')
axes[0].set_xlabel('Leaf size [m]')
axes[0].set_ylabel('Processing time [msec/scan]')
axes[1].set_xlabel('Leaf size [m]')
axes[1].set_ylabel('Num points ratio to pcl_voxelgrid')
axes[0].legend()
axes[1].legend()
axes[0].grid()
axes[1].grid()
pyplot.show()
if __name__ == '__main__':
main()

11
scripts/run_downsampling_benchmark.sh Executable file
View File

@ -0,0 +1,11 @@
#!/bin/bash
dataset_path=$1
exe_path=../build/downsampling_benchmark
mkdir results
num_threads=(2 3 4 5 6 7 8 16 32 64 128)
for N in ${num_threads[@]}; do
echo $exe_path $dataset_path --num_threads $N | tee results/downsampling_benchmark_$N.txt
$exe_path $dataset_path --num_threads $N | tee results/downsampling_benchmark_$N.txt
done

20
scripts/run_odometry_benchmark.sh Executable file
View File

@ -0,0 +1,20 @@
#!/bin/bash
dataset_path=$1
exe_path=../build/odometry_benchmark
mkdir results
engines=(pcl fast_gicp fast_vgicp small_gicp small_gicp_tbb small_gicp_omp)
for engine in ${engines[@]}; do
N=1
$exe_path $dataset_path $(printf "results/traj_lidar_%s_%d.txt" $engine $N) --num_threads $N --engine $engine | tee $(printf "results/odometry_benchmark_%s_%d.txt" $engine $N)
done
engines=(fast_gicp fast_vgicp small_gicp_omp small_gicp_tbb small_vgicp_omp small_vgicp_tbb small_gicp_tbb_flow)
num_threads=(128 96 64 32 16 8 4 2)
for N in ${num_threads[@]}; do
for engine in ${engines[@]}; do
$exe_path $dataset_path $(printf "results/traj_lidar_%s_%d.txt" $engine $N) --num_threads $N --engine $engine | tee $(printf "results/odometry_benchmark_%s_%d.txt" $engine $N)
done
done

23
src/odometry_benchmark.cpp
View File

@ -13,6 +13,7 @@ int main(int argc, char** argv) {
if (argc < 3) {
std::cout << "USAGE: odometry_benchmark <dataset_path> <output_path> [options]" << std::endl;
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 << " --voxel_resolution <value> (default: 2.0)" << std::endl;
@ -33,18 +34,18 @@ int main(int argc, char** argv) {
const std::string dataset_path = argv[1];
const std::string output_path = argv[2];
int num_threads = 4;
double downsampling_resolution = 0.25;
double voxel_resolution = 2.0;
OdometryEstimationParams params;
std::string engine = "small_gicp";
for (auto arg = argv + 3; arg != argv + argc; arg++) {
if (std::string(*arg) == "--num_threads") {
num_threads = std::stoi(*(arg + 1));
if (std::string(*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") {
downsampling_resolution = std::stod(*(arg + 1));
params.downsample_resolution = std::stod(*(arg + 1));
} else if (std::string(*arg) == "--voxel_resolution") {
voxel_resolution = std::stod(*(arg + 1));
params.voxel_resolution = std::stod(*(arg + 1));
} else if (std::string(*arg) == "--engine") {
engine = *(arg + 1);
}
@ -53,11 +54,11 @@ int main(int argc, char** argv) {
std::cout << "dataset_path=" << dataset_path << std::endl;
std::cout << "output_path=" << output_path << std::endl;
std::cout << "registration_engine=" << engine << std::endl;
std::cout << "num_threads=" << num_threads << std::endl;
std::cout << "downsampling_resolution=" << downsampling_resolution << std::endl;
std::cout << "voxel_resolution=" << voxel_resolution << std::endl;
std::cout << "num_threads=" << params.num_threads << 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;
OdometryEstimationParams params{num_threads, downsampling_resolution, voxel_resolution};
std::shared_ptr<OdometryEstimation> odom = create_odometry(engine, params);
if (odom == nullptr) {
return 1;

20
src/odometry_benchmark_small_gicp_tbb_flow.cpp
View File

@ -33,8 +33,6 @@ public:
~SmallGICPFlowEstimationTBB() { guik::async_destroy(); }
std::vector<Eigen::Isometry3d> estimate(std::vector<PointCloud::Ptr>& points) override {
auto async_viewer = guik::async_viewer();
std::vector<Eigen::Isometry3d> traj;
traj.reserve(points.size());
@ -67,7 +65,7 @@ public:
return pair.source;
});
tbb::flow::sequencer_node<InputFrame::Ptr> postreg_sequencer_node(graph, [](const InputFrame::Ptr& input) { return input->id; });
tbb::flow::function_node<InputFrame::Ptr> viewer_node(graph, 1, [&](const InputFrame::Ptr& input) {
tbb::flow::function_node<InputFrame::Ptr> output_node(graph, 1, [&](const InputFrame::Ptr& input) {
if (traj.empty()) {
traj.emplace_back(input->T_last_current);
} else {
@ -82,12 +80,20 @@ public:
const double elapsed_msec = std::chrono::duration_cast<std::chrono::nanoseconds>(input->sw.t2 - t0).count() / 1e6;
throughput = elapsed_msec / (input->id + 1);
if (input->id && input->id % 1024 == 0) {
if (input->id && input->id % 256 == 0) {
report();
}
async_viewer->update_points(guik::anon(), input->points->points, guik::Rainbow(T));
async_viewer->update_points("points", input->points->points, guik::FlatOrange(T).set_point_scale(2.0f));
if (params.visualize) {
static Eigen::Vector2f z_range(0.0f, 0.0f);
z_range[0] = std::min<double>(z_range[0], T.translation().z() - 5.0f);
z_range[1] = std::max<double>(z_range[1], T.translation().z() + 5.0f);
auto async_viewer = guik::async_viewer();
async_viewer->invoke([=] { guik::viewer()->shader_setting().add("z_range", z_range); });
async_viewer->update_points(guik::anon(), input->points->points, guik::Rainbow(T));
async_viewer->update_points("points", input->points->points, guik::FlatOrange(T).set_point_scale(2.0f));
}
});
tbb::flow::make_edge(input_node, preprocess_node);
@ -95,7 +101,7 @@ public:
tbb::flow::make_edge(postpre_sequencer_node, pairing_node);
tbb::flow::make_edge(pairing_node, registration_node);
tbb::flow::make_edge(registration_node, postreg_sequencer_node);
tbb::flow::make_edge(postreg_sequencer_node, viewer_node);
tbb::flow::make_edge(postreg_sequencer_node, output_node);
Stopwatch sw;
sw.start();

12
src/odometry_benchmark_small_vgicp_model_tbb.cpp
View File

@ -8,8 +8,6 @@
#include <small_gicp/registration/reduction_tbb.hpp>
#include <small_gicp/registration/registration.hpp>
#include <guik/viewer/async_light_viewer.hpp>
namespace small_gicp {
class SmallVGICPModelOnlineOdometryEstimationTBB : public OnlineOdometryEstimation {
@ -17,9 +15,7 @@ public:
SmallVGICPModelOnlineOdometryEstimationTBB(const OdometryEstimationParams& params)
: OnlineOdometryEstimation(params),
control(tbb::global_control::max_allowed_parallelism, params.num_threads),
T(Eigen::Isometry3d::Identity()) {
sub_viewer = guik::async_viewer()->async_sub_viewer("model points");
}
T(Eigen::Isometry3d::Identity()) {}
Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override {
Stopwatch sw;
@ -39,10 +35,6 @@ public:
T = result.T_target_source;
voxelmap->insert(*points, T);
sub_viewer.update_points("current", points->points, guik::FlatOrange(T).set_point_scale(2.0f));
sub_viewer.update_normal_dists("voxelmap", voxelmap->voxel_means(), voxelmap->voxel_covs(), 1.0, guik::Rainbow());
sub_viewer.lookat(T.translation().cast<float>());
sw.stop();
reg_times.push(sw.msec());
@ -54,8 +46,6 @@ public:
}
private:
guik::AsyncLightViewerContext sub_viewer;
tbb::global_control control;
Summarizer reg_times;