New keyframe management and rendering settings

config/config_preprocess.json

@@ -10,6 +10,7 @@
   // outlier_removal_k           : Number of neighbors for outlier removal
   // outlier_std_mul_factor      : Standard deviation multiplier for outlier removal
   // k_correspondences           : Number of neighbors for covariance estimation
+  // plane_eps                   : Epsilon for covariance regularization (plane thickness)
   // num_threads                 : Number of threads for preprocessing
   */
   "preprocess": {
@@ -23,6 +24,7 @@
     "outlier_removal_k": 10,
     "outlier_std_mul_factor": 1.0,
     "k_correspondences": 10,
+    "plane_eps": 1e-3,
     "num_threads": 2
   }
 }

config/config_viewer.json

@@ -13,7 +13,12 @@
       4.0
     ],
     "enable_partial_rendering": false,
-    "partial_rendering_budget": 1024
+    "partial_rendering_budget": 1024,
+    "point_shape_circle": true,
+    "point_size_metric": false,
+    "point_size": 10.0,
+    "points_alpha": 0.5,
+    "factors_alpha": 0.5
   },
   "interactive_viewer": {
     "viewer_width": 2560,
@@ -23,6 +28,11 @@
       4.0
     ],
     "enable_partial_rendering": false,
-    "partial_rendering_budget": 1024
+    "partial_rendering_budget": 1024,
+    "point_shape_circle": true,
+    "point_size_metric": false,
+    "point_size": 10.0,
+    "points_alpha": 0.5,
+    "factors_alpha": 0.5
   }
 }

include/glim/common/cloud_covariance_estimation.hpp

@@ -15,7 +15,7 @@ enum class RegularizationMethod { NONE, PLANE, NORMALIZED_MIN_EIG, FROBENIUS };
  */
 class CloudCovarianceEstimation {
 public:
-  CloudCovarianceEstimation(const int num_threads = 1);
+  CloudCovarianceEstimation(const int num_threads = 1, const double plane_eps = 1e-3);
   ~CloudCovarianceEstimation();
 
   /**
@@ -60,6 +60,7 @@ class CloudCovarianceEstimation {
 private:
   const RegularizationMethod regularization_method;
   const int num_threads;
+  const double plane_eps;
 };
 
 }  // namespace glim

include/glim/odometry/odometry_estimation_gpu.hpp

@@ -20,7 +20,7 @@ struct OdometryEstimationGPUParams : public OdometryEstimationIMUParams {
   OdometryEstimationGPUParams();
   virtual ~OdometryEstimationGPUParams();
 
-  enum class KeyframeUpdateStrategy { OVERLAP, DISPLACEMENT, ENTROPY };
+  enum class KeyframeUpdateStrategy { OVERLAP, OVERLAP2, DISPLACEMENT, ENTROPY };
 
 public:
   // Registration params
@@ -57,6 +57,7 @@ class OdometryEstimationGPU : public OdometryEstimationIMU {
   virtual void update_frames(const int current, const gtsam::NonlinearFactorGraph& new_factors) override;
 
   void update_keyframes_overlap(int current);
+  void update_keyframes_overlap2(int current);
   void update_keyframes_displacement(int current);
   void update_keyframes_entropy(const gtsam::NonlinearFactorGraph& matching_cost_factors, int current);
 

include/glim/odometry/odometry_estimation_imu.hpp

@@ -37,6 +37,9 @@ struct OdometryEstimationIMUParams {
   virtual ~OdometryEstimationIMUParams();
 
 public:
+  // Preprocessing params
+  double plane_eps;
+
   // Sensor params;
   bool fix_imu_bias;
   double imu_bias_noise;

include/glim/viewer/interactive_viewer.hpp

@@ -94,8 +94,15 @@ class InteractiveViewer : public ExtensionModule {
   bool enable_partial_rendering;
   int partial_rendering_budget;
 
+  double point_size;
+  bool point_size_metric;
+  bool point_shape_circle;
+
   Eigen::Vector2f z_range;
 
+  double points_alpha;
+  double factors_alpha;
+
   // Click information
   Eigen::Vector4i right_clicked_info;
   Eigen::Vector3f right_clicked_pos;

include/glim/viewer/standard_viewer.hpp

@@ -79,8 +79,14 @@ class StandardViewer : public ExtensionModule {
   std::vector<std::pair<boost::weak_ptr<gtsam::NonlinearFactor>, FactorLineGetter>> odometry_factor_lines;
   std::unordered_map<std::uint64_t, Eigen::Isometry3f> odometry_poses;
 
+  double point_size;
+  bool point_size_metric;
+  bool point_shape_circle;
+
   Eigen::Vector2f z_range;
   Eigen::Vector2f auto_z_range;
+  double points_alpha;
+  double factors_alpha;
 
   std::unique_ptr<TrajectoryManager> trajectory;
   std::vector<Eigen::Isometry3f> submap_keyframes;

src/glim/common/cloud_covariance_estimation.cpp

@@ -10,7 +10,10 @@
 
 namespace glim {
 
-CloudCovarianceEstimation::CloudCovarianceEstimation(const int num_threads) : regularization_method(RegularizationMethod::PLANE), num_threads(num_threads) {}
+CloudCovarianceEstimation::CloudCovarianceEstimation(const int num_threads, const double plane_eps)
+: regularization_method(RegularizationMethod::PLANE),
+  num_threads(num_threads),
+  plane_eps(plane_eps) {}
 
 CloudCovarianceEstimation::~CloudCovarianceEstimation() {}
 
@@ -151,7 +154,7 @@ Eigen::Matrix4d CloudCovarianceEstimation::regularize(const Eigen::Matrix4d& cov
         *eigenvectors = eig.eigenvectors();
       }
 
-      Eigen::Vector3d values(1e-3, 1.0, 1.0);
+      Eigen::Vector3d values(plane_eps, 1.0, 1.0);
       Eigen::Matrix4d c = Eigen::Matrix4d::Zero();
       c.block<3, 3>(0, 0) = eig.eigenvectors() * values.asDiagonal() * eig.eigenvectors().transpose();
       return c;
@@ -169,15 +172,15 @@ Eigen::Matrix4d CloudCovarianceEstimation::regularize(const Eigen::Matrix4d& cov
       }
 
       Eigen::Vector3d values = eig.eigenvalues() / eig.eigenvalues()[2];
-      values = values.array().max(1e-3);
+      values = values.array().max(plane_eps);
 
       Eigen::Matrix4d c = Eigen::Matrix4d::Zero();
       c.block<3, 3>(0, 0) = eig.eigenvectors() * values.asDiagonal() * eig.eigenvectors().transpose();
       return c;
     }
 
     case RegularizationMethod::FROBENIUS: {
-      const double lambda = 1e-3;
+      const double lambda = plane_eps;
       Eigen::Matrix3d C = cov.block<3, 3>(0, 0) + lambda * Eigen::Matrix3d::Identity();
       Eigen::Matrix3d C_inv = C.inverse();
       Eigen::Matrix4d C_ = Eigen::Matrix4d::Zero();

src/glim/odometry/odometry_estimation_gpu.cpp

@@ -48,6 +48,8 @@ OdometryEstimationGPUParams::OdometryEstimationGPUParams() : OdometryEstimationI
   const std::string strategy = config.param<std::string>("odometry_estimation", "keyframe_update_strategy", "OVERLAP");
   if (strategy == "OVERLAP") {
     keyframe_strategy = KeyframeUpdateStrategy::OVERLAP;
+  } else if (strategy == "OVERLAP2") {
+    keyframe_strategy = KeyframeUpdateStrategy::OVERLAP2;
   } else if (strategy == "DISPLACEMENT") {
     keyframe_strategy = KeyframeUpdateStrategy::DISPLACEMENT;
   } else if (strategy == "ENTROPY") {
@@ -104,6 +106,9 @@ void OdometryEstimationGPU::update_frames(const int current, const gtsam::Nonlin
     case OdometryEstimationGPUParams::KeyframeUpdateStrategy::OVERLAP:
       update_keyframes_overlap(current);
       break;
+    case OdometryEstimationGPUParams::KeyframeUpdateStrategy::OVERLAP2:
+      update_keyframes_overlap2(current);
+      break;
     case OdometryEstimationGPUParams::KeyframeUpdateStrategy::DISPLACEMENT:
       update_keyframes_displacement(current);
       break;
@@ -170,12 +175,27 @@ gtsam::NonlinearFactorGraph OdometryEstimationGPU::create_factors(const int curr
     create_binary_factor(factors, X(target), X(current), frames[target], frames[current]);
   }
 
-  for (const auto& keyframe : keyframes) {
+  // Compute overlaps to keyframes
+  std::vector<gtsam_points::GaussianVoxelMap::ConstPtr> keyframe_voxelmaps(keyframes.size());
+  std::vector<Eigen::Isometry3d> Ts_keyframe_current(keyframes.size());
+  for (int i = 0; i < keyframes.size(); i++) {
+    keyframe_voxelmaps[i] = keyframes[i]->voxelmaps.back();
+    Ts_keyframe_current[i] = keyframes[i]->T_world_imu.inverse() * frames[current]->T_world_imu;
+  }
+  std::vector<gtsam_points::PointCloud::ConstPtr> current_points(keyframes.size(), frames[current]->frame);
+  std::vector<double> overlaps = gtsam_points::overlap_gpu(keyframe_voxelmaps, current_points, Ts_keyframe_current, *stream);
+
+  for (int i = 0; i < keyframes.size(); i++) {
+    const auto& keyframe = keyframes[i];
     if (keyframe->id >= current - params->full_connection_window_size) {
       // There already exists a factor
       continue;
     }
 
+    if (overlaps[i] < 0.01) {
+      continue;
+    }
+
     auto stream_buffer = stream_buffer_roundrobin->get_stream_buffer();
     const auto& stream = stream_buffer.first;
     const auto& buffer = stream_buffer.second;
@@ -284,6 +304,86 @@ void OdometryEstimationGPU::update_keyframes_overlap(int current) {
   Callbacks::on_marginalized_keyframes(marginalized_keyframes);
 }
 
+/**
+ * @brief Keyframe management based on an overlap metric
+ * @ref   Koide et al., "Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping", ICRA2022
+ */
+void OdometryEstimationGPU::update_keyframes_overlap2(int current) {
+  const auto params = static_cast<OdometryEstimationGPUParams*>(this->params.get());
+
+  if (!frames[current]->frame->size()) {
+    return;
+  }
+
+  if (keyframes.empty()) {
+    keyframes.push_back(frames[current]);
+    return;
+  }
+
+  std::vector<gtsam_points::GaussianVoxelMap::ConstPtr> keyframes_(keyframes.size());
+  std::vector<Eigen::Isometry3d> delta_from_keyframes(keyframes.size());
+  for (int i = 0; i < keyframes.size(); i++) {
+    keyframes_[i] = keyframes[i]->voxelmaps.back();
+    delta_from_keyframes[i] = keyframes[i]->T_world_imu.inverse() * frames[current]->T_world_imu;
+  }
+
+  const double overlap = gtsam_points::overlap_gpu(keyframes_, frames[current]->frame, delta_from_keyframes, *stream);
+  if (overlap > params->keyframe_max_overlap) {
+    return;
+  }
+
+  const auto& new_keyframe = frames[current];
+  keyframes.push_back(new_keyframe);
+
+  if (keyframes.size() <= params->max_num_keyframes) {
+    return;
+  }
+
+  std::vector<EstimationFrame::ConstPtr> marginalized_keyframes;
+
+  std::vector<gtsam_points::GaussianVoxelMap::ConstPtr> overlap_targets;
+  std::vector<gtsam_points::PointCloud::ConstPtr> overlap_sources;
+  std::vector<Eigen::Isometry3d> Ts_target_source;
+
+  for (int i = 0; i < keyframes.size(); i++) {
+    for (int j = i + 1; j < keyframes.size(); j++) {
+      overlap_targets.emplace_back(keyframes[i]->voxelmaps.back());
+      overlap_sources.emplace_back(keyframes[j]->frame);
+      Ts_target_source.emplace_back(keyframes[i]->T_world_imu.inverse() * keyframes[j]->T_world_imu);
+    }
+  }
+
+  std::vector<double> overlaps = gtsam_points::overlap_gpu(overlap_targets, overlap_sources, Ts_target_source, *stream);
+
+  Eigen::MatrixXd overlap_map = Eigen::MatrixXd::Ones(keyframes.size(), keyframes.size());
+  int count = 0;
+  for (int i = 0; i < keyframes.size(); i++) {
+    for (int j = i + 1; j < keyframes.size(); j++) {
+      overlap_map(i, j) = overlaps[count];
+      count++;
+    }
+  }
+
+  constexpr int power = 3;
+  std::vector<double> scores(keyframes.size() - 1, 0.0);
+  for (int i = 0; i < keyframes.size() - 1; i++) {
+    Eigen::MatrixXd overlap_map_i = overlap_map.topLeftCorner(keyframes.size() - 1, keyframes.size() - 1);
+    overlap_map_i.row(i).setZero();
+    overlap_map_i.col(i).setZero();
+
+    const double sum_overlap = overlap_map_i.array().pow(power).sum();
+    const double overlap_latest = overlap_map(i, keyframes.size() - 1);
+    scores[i] = std::pow(overlap_latest, 1.0 / power) * sum_overlap;
+  }
+
+  auto min_loc = std::min_element(scores.begin(), scores.end());
+  int frame_to_eliminate = std::distance(scores.begin(), min_loc);
+
+  marginalized_keyframes.push_back(keyframes[frame_to_eliminate]);
+  keyframes.erase(keyframes.begin() + frame_to_eliminate);
+  Callbacks::on_marginalized_keyframes(marginalized_keyframes);
+}
+
 /**
  * @brief Keyframe management based on displacement criteria
  * @ref   Engel et al., "Direct Sparse Odometry", IEEE Trans. PAMI, 2018

src/glim/odometry/odometry_estimation_imu.cpp

@@ -32,6 +32,9 @@ using gtsam::symbol_shorthand::V;  // IMU velocity   (v_world_imu)
 using gtsam::symbol_shorthand::X;  // IMU pose       (T_world_imu)
 
 OdometryEstimationIMUParams::OdometryEstimationIMUParams() {
+  Config preprocess_config(GlobalConfig::get_config_path("config_preprocess"));
+  plane_eps = preprocess_config.param<double>("preprocess", "plane_eps", 1e-3);
+
   // sensor config
   Config sensor_config(GlobalConfig::get_config_path("config_sensors"));
   T_lidar_imu = sensor_config.param<Eigen::Isometry3d>("sensors", "T_lidar_imu", Eigen::Isometry3d::Identity());
@@ -89,7 +92,7 @@ OdometryEstimationIMU::OdometryEstimationIMU(std::unique_ptr<OdometryEstimationI
 
   imu_integration.reset(new IMUIntegration);
   deskewing.reset(new CloudDeskewing);
-  covariance_estimation.reset(new CloudCovarianceEstimation(params->num_threads));
+  covariance_estimation.reset(new CloudCovarianceEstimation(params->num_threads, params->plane_eps));
 
   gtsam::ISAM2Params isam2_params;
   if (params->use_isam2_dogleg) {

src/glim/viewer/interactive_viewer.cpp

@@ -57,6 +57,13 @@ InteractiveViewer::InteractiveViewer() : logger(create_module_logger("viewer"))
   enable_partial_rendering = config.param("interactive_viewer", "enable_partial_rendering", false);
   partial_rendering_budget = config.param("interactive_viewer", "partial_rendering_budget", 1024);
 
+  points_alpha = config.param("interactive_viewer", "points_alpha", 1.0);
+  factors_alpha = config.param("interactive_viewer", "factors_alpha", 1.0);
+
+  point_size = config.param("interactive_viewer", "point_size", 1.0);
+  point_size_metric = config.param("interactive_viewer", "point_size_metric", false);
+  point_shape_circle = config.param("interactive_viewer", "point_shape_circle", true);
+
   z_range = config.param("interactive_viewer", "default_z_range", Eigen::Vector2d(-2.0, 4.0)).cast<float>();
 
   trajectory.reset(new TrajectoryManager);
@@ -90,6 +97,16 @@ void InteractiveViewer::viewer_loop() {
   viewer->enable_vsync();
   viewer->shader_setting().add("z_range", z_range);
 
+  viewer->shader_setting().set_point_size(point_size);
+
+  if (point_size_metric) {
+    viewer->shader_setting().set_point_scale_metric();
+  }
+
+  if (point_shape_circle) {
+    viewer->shader_setting().set_point_shape_circle();
+  }
+
   if (enable_partial_rendering) {
     viewer->enable_partial_rendering(0.1);
     viewer->shader_setting().add("dynamic_object", 1);
@@ -289,7 +306,7 @@ void InteractiveViewer::update_viewer() {
     } else {
       const Eigen::Vector4i info(static_cast<int>(PickType::POINTS), 0, 0, submap->id);
       auto cloud_buffer = std::make_shared<glk::PointCloudBuffer>(submap->frame->points, submap->frame->size());
-      auto shader_setting = guik::Rainbow(submap_pose).add("info_values", info);
+      auto shader_setting = guik::Rainbow(submap_pose).add("info_values", info).set_alpha(points_alpha);
 
       if (enable_partial_rendering) {
         cloud_buffer->enable_partial_rendering(partial_rendering_budget);
@@ -348,21 +365,21 @@ void InteractiveViewer::update_viewer() {
     Eigen::Vector4f color;
     switch (type) {
       case FactorType::MATCHING_COST:
-        color = Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f);
+        color = Eigen::Vector4f(0.0f, 1.0f, 0.0f, factors_alpha);
         break;
       case FactorType::BETWEEN:
-        color = Eigen::Vector4f(0.0f, 0.0f, 1.0f, 1.0f);
+        color = Eigen::Vector4f(0.0f, 0.0f, 1.0f, factors_alpha);
         break;
       case FactorType::IMU:
-        color = Eigen::Vector4f(1.0f, 0.0f, 0.0f, 1.0f);
+        color = Eigen::Vector4f(1.0f, 0.0f, 0.0f, factors_alpha);
         break;
     }
 
     factor_colors.push_back(color);
     factor_colors.push_back(color);
   }
 
-  viewer->update_drawable("factors", std::make_shared<glk::ThinLines>(factor_lines, factor_colors), guik::VertexColor());
+  viewer->update_drawable("factors", std::make_shared<glk::ThinLines>(factor_lines, factor_colors), guik::VertexColor().set_alpha(factors_alpha));
 
   std::vector<Eigen::Vector3f> traj;
   for (const auto& submap : submaps) {