refactoring ivox

include/gtsam_points/ann/incremental_voxelmap.hpp

@@ -28,6 +28,9 @@ struct VoxelInfo {
 /// @brief Incremental voxelmap.
 ///        This class supports incremental point cloud insertion and LRU-based voxel deletion.
 /// @note  This class can be used as a point cloud as well as a neighbor search structure.
+/// @note  For the compatibility with other nearest neighbor search methods, this implementation returns indices that encode the voxel and point IDs.
+///        The first ```point_id_bits``` (e.g., 32) bits of a point index represent the point ID, and the rest ```voxel_id_bits``` (e.g., 32) bits
+///        represent the voxel ID that contains the point. The specified point can be looked up by `voxelmap.point(index)`;
 template <typename VoxelContents>
 struct IncrementalVoxelMap : public NearestNeighborSearch {
 public:

include/gtsam_points/ann/ivox.hpp

@@ -13,6 +13,11 @@
 
 namespace gtsam_points {
 
+/**
+ * @brief Voxel-based incremental nearest neighbor search
+ *        Bai et al., "Faster-LIO: Lightweight Tightly Coupled Lidar-Inertial Odometry Using Parallel Sparse Incremental Voxels", IEEE RA-L, 2022
+ * @note  Only the linear iVox is implemented
+ */
 using iVox = IncrementalVoxelMap<FlatContainer>;
 
 }  // namespace gtsam_points

include/gtsam_points/factors/impl/integrated_ct_gicp_factor_impl.hpp

@@ -170,11 +170,10 @@ void IntegratedCT_GICPFactor_<TargetFrame, SourceFrame>::update_correspondences(
       const long target_index = this->correspondences[i];
       const auto& cov_A = frame::cov(*this->source, i);
       const auto& cov_B = frame::cov(*this->target, target_index);
-      Eigen::Matrix4d RCR = (cov_B + pose * cov_A * pose.transpose());
-      RCR(3, 3) = 1.0;
+      const Eigen::Matrix4d RCR = (cov_B + pose * cov_A * pose.transpose());
 
-      mahalanobis[i] = RCR.inverse();
-      mahalanobis[i](3, 3) = 0.0;
+      mahalanobis[i].setZero();
+      mahalanobis[i].block<3, 3>(0, 0) = RCR.block<3, 3>(0, 0).inverse();
     }
   }
 }

include/gtsam_points/factors/impl/integrated_ct_icp_factor_impl.hpp

@@ -6,8 +6,6 @@
 #include <gtsam/linear/HessianFactor.h>
 #include <gtsam_points/ann/kdtree2.hpp>
 
-#include <gtsam_points/util/expressions.hpp>
-
 namespace gtsam_points {
 
 template <typename TargetFrame, typename SourceFrame>

include/gtsam_points/util/stopwatch.hpp

@@ -0,0 +1,27 @@
+// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
+// SPDX-License-Identifier: MIT
+#pragma once
+
+#include <chrono>
+
+namespace gtsam_points {
+
+/// @brief Just a stopwatch
+struct Stopwatch {
+public:
+  void start() { t1 = t2 = std::chrono::high_resolution_clock::now(); }
+  void stop() { t2 = std::chrono::high_resolution_clock::now(); }
+  void lap() {
+    t1 = t2;
+    t2 = std::chrono::high_resolution_clock::now();
+  }
+
+  double sec() const { return std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e9; }
+  double msec() const { return std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e6; }
+
+public:
+  std::chrono::high_resolution_clock::time_point t1;
+  std::chrono::high_resolution_clock::time_point t2;
+};
+
+}  // namespace gtsam_points