vgicp_omp

CMakeLists.txt

@@ -74,6 +74,7 @@ add_executable(odometry_benchmark
   src/odometry_benchmark_fast_vgicp.cpp
   src/odometry_benchmark_small_gicp.cpp
   src/odometry_benchmark_small_gicp_omp.cpp
+  src/odometry_benchmark_small_vgicp_omp.cpp
   src/odometry_benchmark_small_gicp_tbb.cpp
   src/odometry_benchmark_small_vgicp_tbb.cpp
   src/odometry_benchmark_small_gicp_tbb_batch.cpp

src/odometry_benchmark_small_gicp_omp.cpp

@@ -17,7 +17,7 @@ public:
     Stopwatch sw;
     sw.start();
 
-    auto tree = std::make_shared<KdTree<PointCloud>>(points, params.num_threads);
+    auto tree = std::make_shared<KdTreeOMP<PointCloud>>(points, params.num_threads);
     estimate_covariances_omp(*points, *tree, 10, params.num_threads);
 
     if (target_points == nullptr) {
@@ -50,7 +50,7 @@ private:
   Summarizer reg_times;
 
   PointCloud::Ptr target_points;
-  KdTree<PointCloud>::Ptr target_tree;
+  KdTreeOMP<PointCloud>::Ptr target_tree;
 
   Eigen::Isometry3d T;
 };

src/odometry_benchmark_small_vgicp_omp.cpp

@@ -0,0 +1,65 @@
+#include <small_gicp/benchmark/benchmark_odom.hpp>
+
+#include <small_gicp/factors/gicp_factor.hpp>
+#include <small_gicp/points/point_cloud.hpp>
+#include <small_gicp/ann/kdtree_omp.hpp>
+#include <small_gicp/ann/gaussian_voxelmap.hpp>
+#include <small_gicp/util/normal_estimation_omp.hpp>
+#include <small_gicp/registration/reduction_omp.hpp>
+#include <small_gicp/registration/registration.hpp>
+
+namespace small_gicp {
+
+class SmallVGICPOnlineOdometryEstimationOMP : public OnlineOdometryEstimation {
+public:
+  SmallVGICPOnlineOdometryEstimationOMP(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) {}
+
+  Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override {
+    Stopwatch sw;
+    sw.start();
+
+    auto tree = std::make_shared<KdTreeOMP<PointCloud>>(points, params.num_threads);
+    estimate_covariances_omp(*points, *tree, 10, params.num_threads);
+
+    auto voxelmap = std::make_shared<GaussianVoxelMap>(params.voxel_resolution);
+    voxelmap->insert(*points);
+
+    if (target_points == nullptr) {
+      target_points = points;
+      target_voxelmap = voxelmap;
+      return T;
+    }
+
+    Registration<GICPFactor, ParallelReductionOMP, DistanceRejector, LevenbergMarquardtOptimizer> registration;
+    registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance;
+
+    auto result = registration.align(*target_voxelmap, *points, *target_voxelmap, Eigen::Isometry3d::Identity());
+
+    sw.stop();
+    reg_times.push(sw.msec());
+
+    T = T * result.T_target_source;
+
+    target_points = points;
+    target_voxelmap = voxelmap;
+
+    return T;
+  }
+
+  void report() override {  //
+    std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan]  total_throughput=" << total_times.str() << " [msec/scan]" << std::endl;
+  }
+
+private:
+  Summarizer reg_times;
+
+  PointCloud::Ptr target_points;
+  GaussianVoxelMap::Ptr target_voxelmap;
+
+  Eigen::Isometry3d T;
+};
+
+static auto small_vgicp_omp_registry =
+  register_odometry("small_vgicp_omp", [](const OdometryEstimationParams& params) { return std::make_shared<SmallVGICPOnlineOdometryEstimationOMP>(params); });
+
+}  // namespace small_gicp