WIP binning lidar point clouds around vertex timestamps

algorithms/dense-reconstruction/depth_integration/include/depth-integration/depth-integration-inl.h

@@ -532,7 +532,7 @@ void integrateAllSensorDepthResourcesOfType(
             // If we have timing information for the points undistort the point
             // cloud, based on the current poses.
             if (point_cloud.hasTimes()) {
-              int32_t min_time_ns, max_time_ns;
+              int64_t min_time_ns, max_time_ns;
               point_cloud.getMinMaxTimeNanoseconds(&min_time_ns, &max_time_ns);
 
               // The undistortion might take us outside the posegraph range

algorithms/online-map-builders/include/online-map-builders/stream-map-builder.h

@@ -3,8 +3,12 @@
 
 #include <Eigen/Dense>
 #include <algorithm>
+#include <cstddef>
+#include <cstdint>
 #include <feature-tracking/vo-outlier-rejection-pipeline.h>
+#include <iostream>
 #include <landmark-triangulation/pose-interpolator.h>
+#include <limits>
 #include <map-resources/resource-conversion.h>
 #include <memory>
 #include <posegraph/unique-id.h>
@@ -47,6 +51,8 @@ class StreamMapBuilder {
   void apply(const vio::MapUpdate& update);
   void apply(const vio::MapUpdate& update, bool deep_copy_nframe);
 
+  void finishMapping();
+
   vi_map::MissionId getMissionId() const {
     return mission_id_;
   }
@@ -114,6 +120,10 @@ class StreamMapBuilder {
   void notifyLoopClosureConstraintBuffer();
   void notifyWheelOdometryConstraintBuffer();
   void notifyExternalFeaturesMeasurementBuffer();
+  // if dump_remaining_points is true, the buffer will be emptied into the best
+  // vertex available. Otherwise, the buffer will only be used to associate
+  // points to N-1 vertices.
+  void notifyLidarMeasurementBuffer(bool dump_remaining_points);
 
   void addRootViwlsVertex(
       const std::shared_ptr<aslam::VisualNFrame>& nframe,
@@ -198,6 +208,9 @@ class StreamMapBuilder {
       external_features_outlier_rejection_pipelines_;
 
   static constexpr size_t kKeepNMostRecentImages = 10u;
+
+  std::unordered_map<aslam::SensorId, resources::PointCloud>
+      lidar_point_cloud_buffer_;
 };
 
 template <typename PointCloudType>
@@ -226,24 +239,29 @@ void StreamMapBuilder::attachLidarMeasurement(
   if (lidar_sensor.hasPointTimestamps()) {
     const uint32_t convert_to_ns =
         lidar_sensor.getTimestampConversionToNanoseconds();
+
     const int64_t time_offset_ns =
         lidar_sensor.hasRelativePointTimestamps()
-            ? lidar_measurement.getTimestampNanoseconds()
-            : 0;
+            ? 0
+            : -lidar_measurement.getTimestampNanoseconds();
+    // The point cloud timestamps are absolute.
     backend::convertPointCloudType<PointCloudType, resources::PointCloud>(
         lidar_measurement.getPointCloud(), &point_cloud, true, convert_to_ns,
         time_offset_ns);
+    lidar_point_cloud_buffer_[lidar_sensor_id].append(point_cloud);
+    return;
   } else {
     backend::convertPointCloudType<PointCloudType, resources::PointCloud>(
         lidar_measurement.getPointCloud(), &point_cloud, false);
-  }
 
-  backend::ResourceType point_cloud_type =
-      backend::getResourceTypeForPointCloud(point_cloud);
-  vi_map::VIMission& mission = map_->getMission(mission_id_);
-  map_->addSensorResource(
-      point_cloud_type, lidar_sensor_id,
-      lidar_measurement.getTimestampNanoseconds(), point_cloud, &mission);
+    backend::ResourceType point_cloud_type =
+        backend::getResourceTypeForPointCloud(point_cloud);
+    vi_map::VIMission& mission = map_->getMission(mission_id_);
+    map_->addSensorResource(
+        point_cloud_type, lidar_sensor_id,
+        lidar_measurement.getTimestampNanoseconds(), point_cloud, &mission);
+    return;
+  }
 }
 
 template <typename PointCloudType>

algorithms/online-map-builders/src/stream-map-builder.cc

@@ -164,6 +164,12 @@ void StreamMapBuilder::apply(
   notifyBuffers();
 }
 
+void StreamMapBuilder::finishMapping() {
+  LOG(INFO) << "[StreamMapBuilder] Mapping finished callbacks...";
+  // Dump lidar point cloud buffer
+  notifyLidarMeasurementBuffer(/*dump_remaining_points=*/true);
+}
+
 void StreamMapBuilder::addRootViwlsVertex(
     const aslam::VisualNFrame::Ptr& nframe,
     const vio::ViNodeState& vinode_state) {
@@ -465,6 +471,7 @@ void StreamMapBuilder::notifyBuffers() {
   notifyLoopClosureConstraintBuffer();
   notifyWheelOdometryConstraintBuffer();
   notifyExternalFeaturesMeasurementBuffer();
+  notifyLidarMeasurementBuffer(/*dump_remaining_points=*/false);
 }
 
 void StreamMapBuilder::notifyAbsolute6DoFConstraintBuffer() {
@@ -1144,4 +1151,110 @@ void StreamMapBuilder::notifyExternalFeaturesMeasurementBuffer() {
   }
 }
 
+void StreamMapBuilder::notifyLidarMeasurementBuffer(
+    const bool dump_remaining_points) {
+  if (map_->numVertices() < 1u || lidar_point_cloud_buffer_.empty()) {
+    return;
+  }
+  vi_map::VIMission& mission = map_->getMission(mission_id_);
+
+  for (auto& buffer_with_sensor_id : lidar_point_cloud_buffer_) {
+    const auto& lidar_sensor_id = buffer_with_sensor_id.first;
+    auto& point_cloud_buffer = buffer_with_sensor_id.second;
+
+    CHECK(std::is_sorted(
+        point_cloud_buffer.times_ns.begin(), point_cloud_buffer.times_ns.end()))
+        << "[StreamMapBuilder] The lidar point cloud buffer is not sorted!";
+
+    const backend::ResourceType point_cloud_type =
+        backend::getResourceTypeForPointCloud(point_cloud_buffer);
+
+    pose_graph::VertexId current_vertex_id;
+    pose_graph::VertexId next_vertex_id;
+
+    const auto initial_min_lidar_time = point_cloud_buffer.times_ns.front();
+
+    // Get the vertex with the timestamp closest to the min lidar time.
+    uint64_t delta_ns = 0;
+    // NOTE(gtonetti): We always want to include the lidar measurements, so we
+    // set the max interpolation time to the maximum possible value
+    // (tolerance_ns is cast to int64_t)
+    constexpr uint64_t kMaxInterpolationTimeNs =
+        static_cast<uint64_t>(std::numeric_limits<int64_t>::max());
+
+    if (!queries_.getClosestVertexIdByTimestamp(
+            initial_min_lidar_time, kMaxInterpolationTimeNs, &current_vertex_id,
+            &delta_ns)) {
+      LOG(FATAL)
+          << "[StreamMapBuilder] Could not find a vertex for the initial "
+             "lidar measurement!";
+    }
+
+    if (!map_->getNextVertex(current_vertex_id, &next_vertex_id)) {
+      CHECK(current_vertex_id == last_vertex_);
+    }
+    while (current_vertex_id != last_vertex_ && point_cloud_buffer.size() > 0) {
+      const auto current_vertex_ts =
+          map_->getVertex(current_vertex_id).getMinTimestampNanoseconds();
+      const auto next_vertex_ts =
+          map_->getVertex(next_vertex_id).getMinTimestampNanoseconds();
+      const auto midpoint_time =
+          current_vertex_ts + (next_vertex_ts - current_vertex_ts) / 2;
+
+      const auto min_lidar_time = point_cloud_buffer.times_ns.front();
+      const auto max_lidar_time = point_cloud_buffer.times_ns.back();
+
+      CHECK(midpoint_time >= min_lidar_time)
+          << "[StreamMapBuilder] Something went wrong! the the min_lidar_time "
+             "should always be closer to the current vertex than than the next "
+             "vertex!";
+
+      if (midpoint_time > max_lidar_time) {
+        // we should wait for the next lidar measurement to arrive before
+        // processing this vertex
+        break;
+      }
+
+      resources::PointCloud points_for_current_vertex =
+          point_cloud_buffer.splitAtTime(midpoint_time, /*is_sorted=*/true);
+      CHECK(points_for_current_vertex.times_ns.back() <= midpoint_time);
+      CHECK(point_cloud_buffer.times_ns.front() > midpoint_time);
+      CHECK(point_cloud_buffer.size() > 0)
+          << "[StreamMapBuilder] Split did not leave any points in the "
+             "buffer! "
+             "This is impossible!";
+
+      const auto num_points_split = points_for_current_vertex.size();
+
+      // Associate the split points with the current vertex.
+      CHECK(!map_->hasSensorResource(
+          mission, point_cloud_type, lidar_sensor_id, current_vertex_ts))
+          << "[StreamMapBuilder] There is already a point cloud resource at "
+             "the current vertex timestamp!";
+
+      // shift timestamps to the current vertex timestamp
+      points_for_current_vertex.shiftTimestamps(current_vertex_ts);
+      map_->addSensorResource(
+          point_cloud_type, lidar_sensor_id, current_vertex_ts,
+          points_for_current_vertex, &mission);
+      VLOG(3) << "[StreamMapBuilder] Associated " << num_points_split
+              << " points with vertex " << current_vertex_id << " (ts "
+              << current_vertex_ts << ")";
+      current_vertex_id = next_vertex_id;
+      map_->getNextVertex(current_vertex_id, &next_vertex_id);
+    }
+    if (dump_remaining_points) {
+      VLOG(3) << "[StreamMapBuilder] Dumping remaining points "
+              << point_cloud_buffer.size() << " into current "
+              << "vertex " << current_vertex_id;
+      // Dump the remaining points into the current vertex
+      map_->addSensorResource(
+          point_cloud_type, lidar_sensor_id,
+          map_->getVertex(current_vertex_id).getMinTimestampNanoseconds(),
+          point_cloud_buffer, &mission);
+      point_cloud_buffer.clear();
+    }
+  }
+}
+
 }  // namespace online_map_builders

applications/maplab-node/src/map-builder-flow.cc

@@ -277,6 +277,8 @@ bool MapBuilderFlow::saveMapAndOptionallyOptimize(
 
   if (stop_mapping) {
     mapping_terminated_ = true;
+    // Finish mapping
+    stream_map_builder_.finishMapping();
   }
 
   // Early exit if the map is empty.

backend/map-resources/include/map-resources/resource-conversion-inl.h

@@ -1,6 +1,7 @@
 #ifndef MAP_RESOURCES_RESOURCE_CONVERSION_INL_H_
 #define MAP_RESOURCES_RESOURCE_CONVERSION_INL_H_
 
+#include <cstdint>
 #include <gflags/gflags.h>
 #include <glog/logging.h>
 #include <maplab-common/pose_types.h>
@@ -133,17 +134,17 @@ void addPointToPointCloud(
 
 template <typename PointCloudType>
 void addTimeToPointCloud(
-    const int32_t time, const size_t index, PointCloudType* point_cloud) {
+    const int64_t time, const size_t index, PointCloudType* point_cloud) {
   LOG(FATAL) << "This point cloud either does not support times"
              << "or it is not implemented!";
 }
 template <>
 void addTimeToPointCloud(
-    const int32_t time, const size_t index,
+    const int64_t time, const size_t index,
     resources::PointCloud* point_cloud);
 template <>
 void addTimeToPointCloud(
-    const int32_t time, const size_t index,
+    const int64_t time, const size_t index,
     sensor_msgs::PointCloud2* point_cloud);
 
 template <typename PointCloudType>
@@ -364,20 +365,20 @@ void getLabelFromPointCloud(
 template <typename PointCloudType>
 void getTimeFromPointCloud(
     const PointCloudType& /*point_cloud*/, const size_t /*index*/,
-    int32_t* /*time*/, const int32_t /*convert_to_ns*/,
+    int64_t* /*time*/, const int32_t /*convert_to_ns*/,
     const int64_t /*time_offset_ns*/) {
   LOG(FATAL) << "This point cloud either does not support times of the "
              << "requested type or it is not implemented!";
 }
 template <>
 void getTimeFromPointCloud(
     const resources::PointCloud& point_cloud, const size_t index,
-    int32_t* time, const int32_t /*convert_to_ns*/,
+    int64_t* time, const int32_t /*convert_to_ns*/,
     const int64_t /*time_offset_ns*/);
 template <>
 void getTimeFromPointCloud(
     const sensor_msgs::PointCloud2& point_cloud, const size_t index,
-    int32_t* time, const int32_t convert_to_ns, const int64_t time_offset_ns);
+    int64_t* time, const int32_t convert_to_ns, const int64_t time_offset_ns);
 
 template <typename PointCloudType>
 bool convertDepthMapToPointCloud(
@@ -550,7 +551,7 @@ bool convertPointCloudType(
     }
 
     if (with_timestamps && output_has_times) {
-      int32_t time;
+      int64_t time;
       getTimeFromPointCloud(
           input_cloud, point_idx, &time, convert_to_ns, time_offset_ns);
       addTimeToPointCloud(time, point_idx, output_cloud);

backend/map-resources/include/map-resources/resource-conversion.h

@@ -2,6 +2,7 @@
 #define MAP_RESOURCES_RESOURCE_CONVERSION_H_
 
 #include <aslam/cameras/camera.h>
+#include <cstdint>
 #include <maplab-common/pose_types.h>
 #include <opencv2/core.hpp>
 #include <resources-common/point-cloud.h>
@@ -64,7 +65,7 @@ void addColorToPointCloud(
     PointCloudType* point_cloud);
 template <typename PointCloudType>
 void addTimeToPointCloud(
-    const int32_t time, const size_t index, PointCloudType* point_cloud);
+    const int64_t time, const size_t index, PointCloudType* point_cloud);
 
 template <typename PointCloudType>
 void getPointFromPointCloud(
@@ -82,7 +83,7 @@ void getColorFromPointCloud(
     resources::RgbaColor* color);
 template <typename PointCloudType>
 void getTimeFromPointCloud(
-    const PointCloudType& point_cloud, const size_t index, int32_t* time,
+    const PointCloudType& point_cloud, const size_t index, int64_t* time,
     const int32_t convert_to_ns, const int64_t time_offset_ns);
 
 template <typename PointCloudType>

backend/map-resources/src/resource-conversion.cc

@@ -3,6 +3,7 @@
 #include <aslam/cameras/camera-unified-projection.h>
 #include <aslam/cameras/camera.h>
 #include <aslam/cameras/distortion.h>
+#include <cstdint>
 #include <glog/logging.h>
 #include <maplab-common/pose_types.h>
 #include <opencv2/core.hpp>
@@ -652,7 +653,7 @@ void getLabelFromPointCloud(
 
 template <>
 void addTimeToPointCloud(
-    const int32_t time, const size_t index,
+    const int64_t time, const size_t index,
     resources::PointCloud* point_cloud) {
   DCHECK_NOTNULL(point_cloud);
   DCHECK_LT(index, point_cloud->times_ns.size());
@@ -661,9 +662,9 @@ void addTimeToPointCloud(
 
 template <>
 void addTimeToPointCloud(
-    const int32_t time, const size_t index,
+    const int64_t time, const size_t index,
     sensor_msgs::PointCloud2* point_cloud) {
-  sensor_msgs::PointCloud2Iterator<int32_t> it_time(
+  sensor_msgs::PointCloud2Iterator<int64_t> it_time(
       *point_cloud, kPointCloud2TimeV1);
 
   it_time += index;
@@ -673,7 +674,7 @@ void addTimeToPointCloud(
 template <>
 void getTimeFromPointCloud(
     const resources::PointCloud& point_cloud, const size_t index,
-    int32_t* time, const int32_t /*convert_to_ns*/,
+    int64_t* time, const int32_t /*convert_to_ns*/,
     const int64_t /*time_offset_ns*/) {
   DCHECK_NOTNULL(time);
 
@@ -684,7 +685,7 @@ void getTimeFromPointCloud(
 template <>
 void getTimeFromPointCloud(
     const sensor_msgs::PointCloud2& point_cloud, const size_t index,
-    int32_t* time, const int32_t convert_to_ns,
+    int64_t* time, const int32_t convert_to_ns,
     const int64_t time_offset_ns) {
   DCHECK_NOTNULL(time);
   sensor_msgs::PointField field = getTimeField(point_cloud);
@@ -698,14 +699,14 @@ void getTimeFromPointCloud(
     case sensor_msgs::PointField::FLOAT64: {
       double time_f =
           boost::apply_visitor(time_visitor_double.setIndex(index), var);
-      *time = static_cast<int32_t>(time_f * convert_to_ns - time_offset_ns);
+      *time = static_cast<int64_t>(time_f * convert_to_ns - time_offset_ns);
       break;
     }
     case sensor_msgs::PointField::INT32:
     case sensor_msgs::PointField::UINT32: {
       int64_t time_i =
           boost::apply_visitor(time_visitor_int64.setIndex(index), var);
-      *time = static_cast<int32_t>(time_i * convert_to_ns - time_offset_ns);
+      *time = static_cast<int64_t>(time_i * convert_to_ns - time_offset_ns);
       break;
     }
     default: {