LiDAR intensity data support in GLIM (#247)

* intensity support for glimGPU

* removed dependency on glk, using gtsam_point::PointCloud instead

* refactor submap point transform to Eigen::Vector4d

* check raw_frame intensities before adding

Author: sandeepzachariah

include/glim/mapping/async_global_mapping.hpp

@@ -72,7 +72,7 @@ class AsyncGlobalMapping {
    */
   void save(const std::string& path);
 
-  std::vector<Eigen::Vector4d> export_points();
+  gtsam_points::PointCloud::Ptr export_points();
 
   std::shared_ptr<glim::GlobalMappingBase> get_global_mapping() {
     std::lock_guard<std::mutex> lock(global_mapping_mutex);

include/glim/mapping/global_mapping.hpp

@@ -69,7 +69,7 @@ class GlobalMapping : public GlobalMappingBase {
   virtual void optimize() override;
 
   virtual void save(const std::string& path) override;
-  virtual std::vector<Eigen::Vector4d> export_points() override;
+  virtual gtsam_points::PointCloud::Ptr export_points() override;
 
   /**
    * @brief Load a mapping result from a dumped directory
@@ -109,4 +109,4 @@ class GlobalMapping : public GlobalMappingBase {
 
   std::shared_ptr<void> tbb_task_arena;
 };
-}  // namespace glim
+}  // namespace glim

include/glim/mapping/global_mapping_base.hpp

@@ -7,6 +7,7 @@
 #endif
 
 #include <glim/mapping/sub_map.hpp>
+#include <gtsam_points/types/point_cloud.hpp>
 
 namespace spdlog {
 class logger;
@@ -70,7 +71,7 @@ class GlobalMappingBase {
   /**
    * @brief Export all the submap points
    */
-  virtual std::vector<Eigen::Vector4d> export_points() { return std::vector<Eigen::Vector4d>(); }
+  virtual gtsam_points::PointCloud::Ptr export_points() { return nullptr; }
 
   /**
    * @brief Load a global mapping module from a shared library

include/glim/mapping/global_mapping_pose_graph.hpp

@@ -97,7 +97,7 @@ class GlobalMappingPoseGraph : public GlobalMappingBase {
   virtual void optimize() override;
 
   virtual void save(const std::string& path) override;
-  virtual std::vector<Eigen::Vector4d> export_points() override;
+  virtual gtsam_points::PointCloud::Ptr export_points() override;
 
 private:
   void insert_submap(int current, const SubMap::Ptr& submap);
@@ -131,4 +131,4 @@ class GlobalMappingPoseGraph : public GlobalMappingBase {
 
   std::shared_ptr<void> tbb_task_arena;
 };
-}  // namespace glim
+}  // namespace glim

src/glim/mapping/async_global_mapping.cpp

@@ -67,7 +67,7 @@ void AsyncGlobalMapping::save(const std::string& path) {
   logger->info("saved");
 }
 
-std::vector<Eigen::Vector4d> AsyncGlobalMapping::export_points() {
+gtsam_points::PointCloud::Ptr AsyncGlobalMapping::export_points() {
   std::lock_guard<std::mutex> lock(global_mapping_mutex);
   logger->info("exporting points");
   auto points = global_mapping->export_points();
@@ -139,4 +139,4 @@ void AsyncGlobalMapping::run() {
   }
 }
 
-}  // namespace glim
+}  // namespace glim

src/glim/mapping/global_mapping.cpp

@@ -634,22 +634,57 @@ void GlobalMapping::save(const std::string& path) {
   GlobalConfig::instance()->dump(path + "/config");
 }
 
-std::vector<Eigen::Vector4d> GlobalMapping::export_points() {
-  int num_all_points = 0;
+
+gtsam_points::PointCloud::Ptr GlobalMapping::export_points() {
+  auto merged = std::make_shared<gtsam_points::PointCloudCPU>();
+
+  size_t total_points = 0;
   for (const auto& submap : submaps) {
-    num_all_points += submap->frame->size();
+    if (!submap || !submap->frame) {
+      continue;
+    }
+    total_points += submap->frame->size();
   }
 
-  std::vector<Eigen::Vector4d> all_points;
-  all_points.reserve(num_all_points);
+  std::vector<Eigen::Vector4d> points;
+  points.reserve(total_points);
 
+  bool export_intensities = true;
   for (const auto& submap : submaps) {
-    std::transform(submap->frame->points, submap->frame->points + submap->frame->size(), std::back_inserter(all_points), [&](const Eigen::Vector4d& p) {
-      return submap->T_world_origin * p;
-    });
+    if (!submap || !submap->frame || !submap->frame->has_intensities()) {
+      export_intensities = false;
+      break;
+    }
+  }
+
+  std::vector<double> intensities;
+  if (export_intensities) {
+    intensities.reserve(total_points);
+  }
+
+  for (const auto& submap : submaps) {
+    if (!submap || !submap->frame) {
+      continue;
+    }
+
+    for (int i = 0; i < submap->frame->size(); i++) {
+      const Eigen::Vector4d point = submap->T_world_origin * submap->frame->points[i];
+      points.push_back(point);
+
+      if (export_intensities) {
+        intensities.push_back(submap->frame->intensities[i]);
+      }
+    }
+  }
+
+  if (!points.empty()) {
+    merged->add_points(points);
+    if (export_intensities) {
+      merged->add_intensities(intensities);
+    }
   }
 
-  return all_points;
+  return merged;
 }
 
 bool GlobalMapping::load(const std::string& path) {

src/glim/mapping/global_mapping_pose_graph.cpp

@@ -8,6 +8,7 @@
 #include <gtsam/slam/BetweenFactor.h>
 
 #include <gtsam_points/ann/kdtree.hpp>
+#include <gtsam_points/types/point_cloud_cpu.hpp>
 #include <gtsam_points/types/gaussian_voxelmap_cpu.hpp>
 #include <gtsam_points/factors/integrated_gicp_factor.hpp>
 #include <gtsam_points/factors/integrated_vgicp_factor.hpp>
@@ -243,8 +244,8 @@ void GlobalMappingPoseGraph::save(const std::string& path) {
   }
 }
 
-std::vector<Eigen::Vector4d> GlobalMappingPoseGraph::export_points() {
-  return {};
+gtsam_points::PointCloud::Ptr GlobalMappingPoseGraph::export_points() {
+  return std::make_shared<gtsam_points::PointCloudCPU>();
 }
 
 void GlobalMappingPoseGraph::insert_submap(int current, const SubMap::Ptr& submap) {
@@ -477,4 +478,4 @@ void GlobalMappingPoseGraph::update_submaps() {
   }
 }
 
-}  // namespace glim
+}  // namespace glim

src/glim/mapping/sub_mapping.cpp

@@ -338,7 +338,6 @@ void SubMapping::insert_frame(const EstimationFrame::ConstPtr& odom_frame_) {
 
 void SubMapping::insert_keyframe(const int current, const EstimationFrame::ConstPtr& odom_frame) {
   gtsam_points::PointCloud::ConstPtr deskewed_frame = odom_frame->frame;
-
   // Re-perform deskewing with smoothed IMU poses
   if (params.enable_imu && odom_frame->raw_frame && odom_frame->imu_rate_trajectory.cols() >= 2) {
     if (std::abs(odom_frame->stamp - odom_frame->imu_rate_trajectory(0, 0)) > 1e-3) {
@@ -370,7 +369,9 @@ void SubMapping::insert_keyframe(const int current, const EstimationFrame::Const
       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));
-
+    if (!odom_frame->raw_frame->intensities.empty()) {
+      frame->add_intensities(odom_frame->raw_frame->intensities);
+    }
     deskewed_frame = frame;
   }
 

src/glim/viewer/offline_viewer.cpp

@@ -215,12 +215,31 @@ bool OfflineViewer::export_map(guik::ProgressInterface& progress, const std::str
   progress.set_text("Concatenating submaps");
   progress.set_maximum(3);
   progress.increment();
-  const auto points = async_global_mapping->export_points();
+  auto points = async_global_mapping->export_points();
+
+  if (!points || !points->has_points()) {
+    logger->warn("No points available for export");
+    return false;
+  }
 
   progress.set_text("Writing to file");
   progress.increment();
-  glk::save_ply_binary(path, points.data(), points.size());
 
+  glk::PLYData ply;
+  ply.vertices.reserve(points->size());
+  const bool has_intensities = points->has_intensities();
+  if (has_intensities) {
+    ply.intensities.reserve(points->size());
+  }
+
+  for (size_t i = 0; i < points->size(); ++i) {
+    ply.vertices.push_back(points->points[i].head<3>().cast<float>());
+    if (has_intensities) {
+      ply.intensities.push_back(points->intensities[i]);
+    }
+  }
+
+  glk::save_ply_binary(path, ply);
   return true;
 }