backface culling (#307)

Author: koide3

include/glim/viewer/standard_viewer.hpp

@@ -58,6 +58,9 @@ class StandardViewer : public ExtensionModule {
   std::atomic_bool kill_switch;
   std::thread thread;
 
+  bool enable_backface_culling;
+  Eigen::Vector2f backface_culling_range;
+
   bool enable_partial_rendering;
   int partial_rendering_budget;
 

src/glim/mapping/sub_mapping.cpp

@@ -368,7 +368,13 @@ void SubMapping::insert_keyframe(const int current, const EstimationFrame::Const
     for (int i = 0; i < frame->size(); i++) {
       frame->points[i] = odom_frame->T_lidar_imu.inverse() * frame->points[i];
     }
-    frame->add_covs(covariance_estimation->estimate(frame->points_storage, odom_frame->raw_frame->neighbors));
+
+    std::vector<Eigen::Vector4d> normals;
+    std::vector<Eigen::Matrix4d> covs;
+    covariance_estimation->estimate(frame->points_storage, odom_frame->raw_frame->neighbors, normals, covs);
+    frame->add_normals(normals);
+    frame->add_covs(covs);
+
     if (!odom_frame->raw_frame->intensities.empty()) {
       frame->add_intensities(odom_frame->raw_frame->intensities);
     }
@@ -487,16 +493,10 @@ SubMap::Ptr SubMapping::create_submap(bool force_create) const {
 #endif
 
   if (submap->frame == nullptr) {
-    submap->frame = gtsam_points::merge_frames_auto(poses_to_merge, keyframes_to_merge, params.submap_downsample_resolution);
+    submap->frame = gtsam_points::merge_frames(poses_to_merge, keyframes_to_merge, params.submap_downsample_resolution, params.submap_target_num_points);
   }
   logger->debug("|merged_submap|={}", submap->frame->size());
 
-  if (params.submap_target_num_points > 0 && submap->frame->size() > params.submap_target_num_points) {
-    std::mt19937 mt(submap_count * 643145 + submap->frame->size() * 4312);  // Just a random-like seed
-    submap->frame = gtsam_points::random_sampling(submap->frame, static_cast<double>(params.submap_target_num_points) / submap->frame->size(), mt);
-    logger->debug("|subsampled_submap|={}", submap->frame->size());
-  }
-
   return submap;
 }
 

src/glim/viewer/standard_viewer.cpp

@@ -93,6 +93,9 @@ StandardViewer::StandardViewer() : logger(create_module_logger("viewer")) {
 
   trajectory.reset(new TrajectoryManager);
 
+  enable_backface_culling = false;
+  backface_culling_range = Eigen::Vector2f(0.0f, 1.1f);
+
   enable_partial_rendering = config.param("standard_viewer", "enable_partial_rendering", false);
   partial_rendering_budget = config.param("standard_viewer", "partial_rendering_budget", 1024);
 

src/glim/viewer/standard_viewer_callbacks.cpp

@@ -81,6 +81,10 @@ void StandardViewer::set_callbacks() {
       auto viewer = guik::LightViewer::instance();
       auto cloud_buffer = std::make_shared<glk::PointCloudBuffer>(new_frame->frame->points, new_frame->frame->size());
 
+      if (new_frame->frame->has_normals()) {
+        cloud_buffer->add_normals(new_frame->frame->normals, new_frame->frame->size());
+      }
+
       std::vector<float> intensities;
       if (new_frame->raw_frame && new_frame->raw_frame->points.size() == new_frame->frame->size()) {
         intensities.resize(new_frame->raw_frame->intensities.size());
@@ -485,6 +489,9 @@ void StandardViewer::set_callbacks() {
 
       auto viewer = guik::LightViewer::instance();
       auto cloud_buffer = std::make_shared<glk::PointCloudBuffer>(submap->frame->points, submap->frame->size());
+      if (submap->frame->has_normals()) {
+        cloud_buffer->add_normals(submap->frame->normals, submap->frame->size());
+      }
       if (submap->frame->has_intensities()) {
         std::vector<float> intensities(submap->frame->intensities, submap->frame->intensities + submap->frame->size());
         cloud_buffer->add_colormap(intensities);

src/glim/viewer/standard_viewer_ui.cpp

@@ -240,6 +240,20 @@ void StandardViewer::drawable_selection() {
   ImGui::SetNextItemWidth(60);
   ImGui::DragInt("Budget", &partial_rendering_budget, 1, 1, 1000000);
 
+  if (ImGui::Checkbox("Backface cull", &enable_backface_culling)) {
+    if (enable_backface_culling) {
+      viewer->enable_backface_culling();
+    } else {
+      viewer->disable_backface_culling();
+    }
+  }
+
+  ImGui::SameLine();
+  ImGui::SetNextItemWidth(120);
+  if (ImGui::DragFloatRange2("Range", &backface_culling_range[0], &backface_culling_range[1], 0.01f, -1.1f, 1.1f)) {
+    viewer->set_backface_culling_range(backface_culling_range);
+  }
+
   ImGui::End();
 
   if (show_odometry_status) {