add diplay for median distance and voxel resolutions (#103)

config/config_global_mapping_gpu.json

@@ -4,11 +4,11 @@
   // enable_imu                     : If true, create IMU preintegration factors
   // enable_optimization            : If false, do not submap optimization (use only odom data)
   // init_pose_damping_scale        : Damping scale for initial pose (to fix the gauge freedom)
-  // 
+  //
   // --- Relative pose factors ---
   // create_between_factors         : If true, create relative pose factors between consecutive frames (i.e., odom_factor)
   // between_registration_type      : Registration factor type for information matrix evaluation of relative pose factors ("NONE" / "GICP")
-  // 
+  //
   // --- Registration error factors ---
   // registration_error_factor_type : Registration factor type for matching cost minimization ("VGICP" or "VGICP_GPU")
   // randomsampling_rate            : Random sampling rate for registration error factors (1.0 = use all points)
@@ -36,7 +36,7 @@
     "registration_error_factor_type": "VGICP_GPU",
     "randomsampling_rate": 1.0,
     "submap_voxel_resolution": 0.5,
-    "submap_voxel_resolution_max": 2.0,
+    "submap_voxel_resolution_max": 1.0,
     "submap_voxel_resolution_dmin": 5.0,
     "submap_voxel_resolution_dmax": 20.0,
     "submap_voxelmap_levels": 2,

config/config_odometry_gpu.json

@@ -5,14 +5,14 @@
   //                             : "NAIVE" simply uses the linear acceleration direction as the gravity direction
   // initialization_window_size  : Initialization window size for the "LOOSE" initialization mode
   // init_pose_damping_scale     : Damping scale for the initial pose (to fix the gauge freedom)
-  // 
+  //
   // --- Optimization params ---
   // smoother_lag                : Fixed-lag smoothing window size [sec]
   // use_isam2_dogleg            : Use the dogleg optimizer as the backend of ISAM2 that is slow but more robust
   // isam2_relinearize_skip      :
   // isam2_relinearize_thresh    :
   // fix_imu_bias                : If true, disable IMU bias estimation and use the initial IMU bias
-  // 
+  //
   // --- Voxel params ---
   // voxel_resolution            : Base resolution for VGICP voxels
   // voxel_resolution_max        : Maximum base resolution for VGICP voxels.
@@ -22,15 +22,15 @@
   // voxelmap_scaling_factor     : Multi-resolution voxelmap scaling factor
   // full_connection_window_size : Latest sensor pose variable is connected with all of the past $full_connection_window_size poses
   //                             : Must be >1. Increase this param for aggressive motion data (3~5)
-  // 
+  //
   // --- Keyframe params ---
   // keyframe_update_strategy    : "OVERLAP" / "DISPLACEMENT" / "ENTROPY"
   // max_num_keyframes           : Maximum number of keyframes
   // keyframe_min_overlap        : Keyframes that overlap to the latest keyframe with a rate smaller than this will be eliminated
   // keyframe_max_overlap        : If the overlap between the latest frame and keyframes is smaller than this,
   //                             : it is inserted into the keyframe list (lager = more frequent keyframe insertion)
   // keyframe_delta_(trans|rot)  : Keyframe displacement threshold [m / rad]
-  // 
+  //
   // --- MISC ---
   // save_imu_rate_trajectory     : If true, save the IMU rate trajectory
   // num_threads                  : Number of threads for odometry estimation
@@ -49,7 +49,7 @@
     "fix_imu_bias": false,
     // Voxel params
     "voxel_resolution": 0.25,
-    "voxel_resolution_max": 1.0,
+    "voxel_resolution_max": 0.5,
     "voxel_resolution_dmin": 5.0,
     "voxel_resolution_dmax": 20.0,
     "voxelmap_levels": 2,

config/config_sensors.json

@@ -45,7 +45,7 @@
     "imu_acc_noise": 0.05,
     "imu_gyro_noise": 0.02,
     "imu_int_noise": 0.001,
-    "imu_bias_noise": 0.001,
+    "imu_bias_noise": 1e-5,
     // LiDAR config
     "global_shutter_lidar": false,
     "T_lidar_imu": [

include/glim/viewer/standard_viewer.hpp

@@ -73,6 +73,8 @@ class StandardViewer : public ExtensionModule {
   std::pair<double, double> last_point_stamps;
   Eigen::Vector3d last_imu_vel;
   Eigen::Matrix<double, 6, 1> last_imu_bias;
+  double last_median_distance;
+  std::vector<double> last_voxel_resolutions;
 
   using FactorLine = std::tuple<Eigen::Vector3f, Eigen::Vector3f, Eigen::Vector4f, Eigen::Vector4f>;
   using FactorLineGetter = std::function<std::optional<FactorLine>(const gtsam::NonlinearFactor*)>;

src/glim/viewer/standard_viewer.cpp

@@ -5,6 +5,7 @@
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam_points/config.hpp>
+#include <gtsam_points/types/point_cloud_cpu.hpp>
 #include <gtsam_points/factors/integrated_matching_cost_factor.hpp>
 #include <gtsam_points/optimizers/isam2_result_ext.hpp>
 #include <gtsam_points/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp>
@@ -55,6 +56,7 @@ StandardViewer::StandardViewer() : logger(create_module_logger("viewer")) {
   last_point_stamps = std::make_pair(0.0, 0.0);
   last_imu_vel.setZero();
   last_imu_bias.setZero();
+  last_median_distance = 0.0;
 
   z_range = config.param("standard_viewer", "default_z_range", Eigen::Vector2d(-2.0, 4.0)).cast<float>();
   auto_z_range << 0.0f, 0.0f;
@@ -156,6 +158,13 @@ void StandardViewer::set_callbacks() {
       }
       last_imu_vel = new_frame->v_world_imu;
       last_imu_bias = new_frame->imu_bias;
+      last_median_distance = gtsam_points::median_distance(new_frame->frame, 256);
+      last_voxel_resolutions.clear();
+      for (const auto& voxelmap : new_frame->voxelmaps) {
+        if (voxelmap) {
+          last_voxel_resolutions.emplace_back(voxelmap->voxel_resolution());
+        }
+      }
 
       trajectory->add_odom(new_frame->stamp, new_frame->T_world_sensor(), 1);
       const Eigen::Isometry3f pose = resolve_pose(new_frame);
@@ -753,6 +762,20 @@ void StandardViewer::drawable_selection() {
     ImGui::Begin("odometry status", &show_odometry_status, ImGuiWindowFlags_AlwaysAutoResize);
     ImGui::Text("frame ID:%d", last_id);
     ImGui::Text("points:%d", last_num_points);
+    ImGui::Text("median dist:%.3f", last_median_distance);
+
+    std::stringstream sst;
+    if (last_voxel_resolutions.empty()) {
+      sst << "voxel_resolution: N/A";
+    } else {
+      sst << "voxel_resolution: ";
+      for (double r : last_voxel_resolutions) {
+        sst << fmt::format("{:.3f}", r) << " ";
+      }
+    }
+    const std::string text = sst.str();
+    ImGui::Text("%s", text.c_str());
+
     ImGui::Text("stamp:%.3f ~ %.3f", last_point_stamps.first, last_point_stamps.second);
     ImGui::Text("vel:%.3f %.3f %.3f", last_imu_vel[0], last_imu_vel[1], last_imu_vel[2]);
     ImGui::Text("bias:%.3f %.3f %.3f %.3f %.3f %.3f", last_imu_bias[0], last_imu_bias[1], last_imu_bias[2], last_imu_bias[3], last_imu_bias[4], last_imu_bias[5]);