Unwarp each (MultiEcho)LaserScan point in the assets_writer_main. (#526)

Since these contain timing information, we can use the SLAM trajectory
to compensate for ego motion (unwarp) and the TF messages and URDF to
compensate for sensor-to-tracking motion.

This improves quality greatly.

cartographer_ros/cartographer_ros/assets_writer_main.cc

@@ -76,30 +76,37 @@ std::unique_ptr<carto::io::PointsBatch> HandleMessage(
     const tf2_ros::Buffer& tf_buffer,
     const carto::transform::TransformInterpolationBuffer&
         transform_interpolation_buffer) {
-  const carto::common::Time time = FromRos(message.header.stamp);
+  const carto::common::Time start_time = FromRos(message.header.stamp);
-  if (!transform_interpolation_buffer.Has(time)) {
-    return nullptr;
-  }
-  const carto::transform::Rigid3d tracking_to_map =
-      transform_interpolation_buffer.Lookup(time);
-  const carto::transform::Rigid3d sensor_to_tracking =
-      ToRigid3d(tf_buffer.lookupTransform(
-          tracking_frame, message.header.frame_id, message.header.stamp));
-  const carto::transform::Rigid3f sensor_to_map =
-      (tracking_to_map * sensor_to_tracking).cast<float>();
 
   auto points_batch = carto::common::make_unique<carto::io::PointsBatch>();
-  points_batch->time = time;
+  points_batch->start_time = start_time;
-  points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero();
   points_batch->frame_id = message.header.frame_id;
 
   carto::sensor::PointCloudWithIntensities point_cloud =
       ToPointCloudWithIntensities(message);
-  CHECK(point_cloud.intensities.size() == point_cloud.points.size());
+  CHECK_EQ(point_cloud.intensities.size(), point_cloud.points.size());
+  CHECK_EQ(point_cloud.offset_seconds.size(), point_cloud.points.size());
 
   for (size_t i = 0; i < point_cloud.points.size(); ++i) {
+    const carto::common::Time time =
+        start_time + carto::common::FromSeconds(point_cloud.offset_seconds[i]);
+    if (!transform_interpolation_buffer.Has(time)) {
+      continue;
+    }
+    const carto::transform::Rigid3d tracking_to_map =
+        transform_interpolation_buffer.Lookup(time);
+    const carto::transform::Rigid3d sensor_to_tracking =
+        ToRigid3d(tf_buffer.lookupTransform(
+            tracking_frame, message.header.frame_id, ToRos(time)));
+    const carto::transform::Rigid3f sensor_to_map =
+        (tracking_to_map * sensor_to_tracking).cast<float>();
     points_batch->points.push_back(sensor_to_map * point_cloud.points[i]);
     points_batch->intensities.push_back(point_cloud.intensities[i]);
+    // We use the last transform for the origin, which is approximately correct.
+    points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero();
+  }
+  if (points_batch->points.empty()) {
+    return nullptr;
   }
   return points_batch;
 }
@@ -189,9 +196,8 @@ void Run(const string& pose_graph_filename,
           }
         }
 
-        while (!delayed_messages.empty() &&
+        while (!delayed_messages.empty() && delayed_messages.front().getTime() <
-               delayed_messages.front().getTime() <
+                                                message.getTime() - kDelay) {
-                   message.getTime() - kDelay) {
           const rosbag::MessageInstance& delayed_message =
               delayed_messages.front();
 

cartographer_ros/cartographer_ros/msg_conversion.cc

@@ -110,6 +110,7 @@ PointCloudWithIntensities LaserScanToPointCloudWithIntensities(
         const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
         point_cloud.points.push_back(rotation *
                                      (first_echo * Eigen::Vector3f::UnitX()));
+        point_cloud.offset_seconds.push_back(i * msg.time_increment);
         if (msg.intensities.size() > 0) {
           CHECK_EQ(msg.intensities.size(), msg.ranges.size());
           const auto& echo_intensities = msg.intensities[i];
@@ -172,6 +173,7 @@ PointCloudWithIntensities ToPointCloudWithIntensities(
     for (const auto& point : pcl_point_cloud) {
       point_cloud.points.emplace_back(point.x, point.y, point.z);
       point_cloud.intensities.push_back(point.intensity);
+      point_cloud.offset_seconds.push_back(0.f);
     }
   } else {
     pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
@@ -182,6 +184,7 @@ PointCloudWithIntensities ToPointCloudWithIntensities(
     for (const auto& point : pcl_point_cloud) {
       point_cloud.points.emplace_back(point.x, point.y, point.z);
       point_cloud.intensities.push_back(1.0);
+      point_cloud.offset_seconds.push_back(0.f);
     }
   }
   return point_cloud;

cartographer_ros/cartographer_ros/sensor_bridge.cc

@@ -114,16 +114,14 @@ void SensorBridge::HandleImuMessage(const string& sensor_id,
 void SensorBridge::HandleLaserScanMessage(
     const string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
   HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
-                  ToPointCloudWithIntensities(*msg).points,
+                  ToPointCloudWithIntensities(*msg));
-                  msg->time_increment);
 }
 
 void SensorBridge::HandleMultiEchoLaserScanMessage(
     const string& sensor_id,
     const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
   HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
-                  ToPointCloudWithIntensities(*msg).points,
+                  ToPointCloudWithIntensities(*msg));
-                  msg->time_increment);
 }
 
 void SensorBridge::HandlePointCloud2Message(
@@ -140,21 +138,21 @@ void SensorBridge::HandlePointCloud2Message(
 
 const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
 
-void SensorBridge::HandleLaserScan(const string& sensor_id,
+void SensorBridge::HandleLaserScan(
-                                   const carto::common::Time start_time,
+    const string& sensor_id, const carto::common::Time start_time,
-                                   const string& frame_id,
+    const string& frame_id,
-                                   const carto::sensor::PointCloud& points,
+    const carto::sensor::PointCloudWithIntensities& points) {
-                                   const double seconds_between_points) {
   for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
     const size_t start_index =
-        points.size() * i / num_subdivisions_per_laser_scan_;
+        points.points.size() * i / num_subdivisions_per_laser_scan_;
     const size_t end_index =
-        points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
+        points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
-    const carto::sensor::PointCloud subdivision(points.begin() + start_index,
+    const carto::sensor::PointCloud subdivision(
-                                                points.begin() + end_index);
+        points.points.begin() + start_index, points.points.begin() + end_index);
+    const size_t middle_index = (start_index + end_index) / 2;
     const carto::common::Time subdivision_time =
-        start_time + carto::common::FromSeconds((start_index + end_index) / 2. *
+        start_time +
-                                                seconds_between_points);
+        carto::common::FromSeconds(points.offset_seconds.at(middle_index));
     HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
   }
 }

cartographer_ros/cartographer_ros/sensor_bridge.h

@@ -66,11 +66,10 @@ class SensorBridge {
   const TfBridge& tf_bridge() const;
 
  private:
-  void HandleLaserScan(const string& sensor_id,
+  void HandleLaserScan(
-                       ::cartographer::common::Time start_time,
+      const string& sensor_id, ::cartographer::common::Time start_time,
-                       const string& frame_id,
+      const string& frame_id,
-                       const ::cartographer::sensor::PointCloud& points,
+      const ::cartographer::sensor::PointCloudWithIntensities& points);
-                       double seconds_between_points);
   void HandleRangefinder(const string& sensor_id,
                          ::cartographer::common::Time time,
                          const string& frame_id,