Show the unprojected point cloud in 2D SLAM. (#59)

We now project to 2D later in 2D SLAM, so that the roll and pitch
applied to the laser fan in visible in the visualization.

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -47,7 +47,7 @@ void GlobalTrajectoryBuilder::AddHorizontalLaserFan(
   if (insertion_result != nullptr) {
     sparse_pose_graph_->AddScan(
         insertion_result->time, insertion_result->tracking_to_tracking_2d,
-        insertion_result->laser_fan_in_tracking_2d,
+        sensor::ProjectLaserFan(insertion_result->laser_fan_in_tracking_2d),
         insertion_result->pose_estimate_2d,
         kalman_filter::Project2D(insertion_result->covariance_estimate),
         insertion_result->submaps, insertion_result->matching_submap,

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -78,29 +78,24 @@ kalman_filter::PoseTracker* LocalTrajectoryBuilder::pose_tracker() const {
   return pose_tracker_.get();
 }
 
-sensor::LaserFan LocalTrajectoryBuilder::BuildProjectedLaserFan(
+sensor::LaserFan3D LocalTrajectoryBuilder::BuildCroppedLaserFan(
     const transform::Rigid3f& tracking_to_tracking_2d,
     const sensor::LaserFan3D& laser_fan) const {
-  const sensor::LaserFan projected_fan = sensor::ProjectCroppedLaserFan(
+  const sensor::LaserFan3D cropped_fan = sensor::CropLaserFan(
       sensor::TransformLaserFan3D(laser_fan, tracking_to_tracking_2d),
-      Eigen::Vector3f(-std::numeric_limits<float>::infinity(),
+      options_.horizontal_laser_min_z(), options_.horizontal_laser_max_z());
-                      -std::numeric_limits<float>::infinity(),
+  return sensor::LaserFan3D{
-                      options_.horizontal_laser_min_z()),
+      cropped_fan.origin,
-      Eigen::Vector3f(std::numeric_limits<float>::infinity(),
+      sensor::VoxelFiltered(cropped_fan.returns,
-                      std::numeric_limits<float>::infinity(),
-                      options_.horizontal_laser_max_z()));
-  return sensor::LaserFan{
-      projected_fan.origin,
-      sensor::VoxelFiltered(projected_fan.point_cloud,
                             options_.horizontal_laser_voxel_filter_size()),
-      sensor::VoxelFiltered(projected_fan.missing_echo_point_cloud,
+      sensor::VoxelFiltered(cropped_fan.misses,
                             options_.horizontal_laser_voxel_filter_size())};
 }
 
 void LocalTrajectoryBuilder::ScanMatch(
     common::Time time, const transform::Rigid3d& pose_prediction,
     const transform::Rigid3d& tracking_to_tracking_2d,
-    const sensor::LaserFan& laser_fan_in_tracking_2d,
+    const sensor::LaserFan3D& laser_fan_in_tracking_2d,
     transform::Rigid3d* pose_observation,
     kalman_filter::PoseCovariance* covariance_observation) {
   const ProbabilityGrid& probability_grid =
@@ -113,7 +108,8 @@ void LocalTrajectoryBuilder::ScanMatch(
   sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
       options_.adaptive_voxel_filter_options());
   const sensor::PointCloud2D filtered_point_cloud_in_tracking_2d =
-      adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.point_cloud);
+      sensor::ProjectToPointCloud2D(
+          adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.returns));
   if (options_.use_online_correlative_scan_matching()) {
     real_time_correlative_scan_matcher_.Match(
         pose_prediction_2d, filtered_point_cloud_in_tracking_2d,
@@ -170,10 +166,10 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
               -transform::GetYaw(pose_prediction), Eigen::Vector3d::UnitZ())) *
           pose_prediction.rotation());
 
-  const sensor::LaserFan laser_fan_in_tracking_2d =
+  const sensor::LaserFan3D laser_fan_in_tracking_2d =
-      BuildProjectedLaserFan(tracking_to_tracking_2d.cast<float>(), laser_fan);
+      BuildCroppedLaserFan(tracking_to_tracking_2d.cast<float>(), laser_fan);
 
-  if (laser_fan_in_tracking_2d.point_cloud.empty()) {
+  if (laser_fan_in_tracking_2d.returns.empty()) {
     LOG(WARNING) << "Dropped empty horizontal laser point cloud.";
     return nullptr;
   }
@@ -202,9 +198,8 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
       {pose_observation, covariance_observation},
       {scan_matcher_pose_estimate_, covariance_estimate},
       scan_matcher_pose_estimate_,
-      sensor::TransformPointCloud(
+      sensor::TransformPointCloud(laser_fan_in_tracking_2d.returns,
-          sensor::ToPointCloud(laser_fan_in_tracking_2d.point_cloud),
+                                  tracking_2d_to_map.cast<float>())};
-          tracking_2d_to_map.cast<float>())};
 
   const transform::Rigid2d pose_estimate_2d =
       transform::Project2D(tracking_2d_to_map);
@@ -218,8 +213,8 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
   for (int insertion_index : submaps_.insertion_indices()) {
     insertion_submaps.push_back(submaps_.Get(insertion_index));
   }
-  submaps_.InsertLaserFan(TransformLaserFan(laser_fan_in_tracking_2d,
+  submaps_.InsertLaserFan(sensor::ProjectLaserFan(TransformLaserFan3D(
-                                            pose_estimate_2d.cast<float>()));
+      laser_fan_in_tracking_2d, tracking_2d_to_map.cast<float>())));
 
   return common::make_unique<InsertionResult>(InsertionResult{
       time, &submaps_, matching_submap, insertion_submaps,

cartographer/mapping_2d/local_trajectory_builder.h

@@ -49,7 +49,7 @@ class LocalTrajectoryBuilder {
     std::vector<const mapping::Submap*> insertion_submaps;
     transform::Rigid3d tracking_to_tracking_2d;
     transform::Rigid3d tracking_2d_to_map;
-    sensor::LaserFan laser_fan_in_tracking_2d;
+    sensor::LaserFan3D laser_fan_in_tracking_2d;
     transform::Rigid2d pose_estimate_2d;
     kalman_filter::PoseCovariance covariance_estimate;
   };
@@ -75,9 +75,8 @@ class LocalTrajectoryBuilder {
   kalman_filter::PoseTracker* pose_tracker() const;
 
  private:
-  // Transforms 'laser_scan', projects it onto the ground plane,
+  // Transforms 'laser_scan', crops and voxel filters.
-  // crops and voxel filters.
+  sensor::LaserFan3D BuildCroppedLaserFan(
-  sensor::LaserFan BuildProjectedLaserFan(
       const transform::Rigid3f& tracking_to_tracking_2d,
       const sensor::LaserFan3D& laser_fan) const;
 
@@ -85,7 +84,7 @@ class LocalTrajectoryBuilder {
   // 'pose_observation' and 'covariance_observation' with the result.
   void ScanMatch(common::Time time, const transform::Rigid3d& pose_prediction,
                  const transform::Rigid3d& tracking_to_tracking_2d,
-                 const sensor::LaserFan& laser_fan_in_tracking_2d,
+                 const sensor::LaserFan3D& laser_fan_in_tracking_2d,
                  transform::Rigid3d* pose_observation,
                  kalman_filter::PoseCovariance* covariance_observation);
 

cartographer/sensor/laser.cc

@@ -113,12 +113,16 @@ LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan,
   return result;
 }
 
-LaserFan ProjectCroppedLaserFan(const LaserFan3D& laser_fan,
+LaserFan3D CropLaserFan(const LaserFan3D& laser_fan, const float min_z,
-                                const Eigen::Vector3f& min,
+                        const float max_z) {
-                                const Eigen::Vector3f& max) {
+  return LaserFan3D{laser_fan.origin, Crop(laser_fan.returns, min_z, max_z),
+                    Crop(laser_fan.misses, min_z, max_z)};
+}
+
+LaserFan ProjectLaserFan(const LaserFan3D& laser_fan) {
   return LaserFan{laser_fan.origin.head<2>(),
-                  ProjectToPointCloud2D(Crop(laser_fan.returns, min, max)),
+                  ProjectToPointCloud2D(laser_fan.returns),
-                  ProjectToPointCloud2D(Crop(laser_fan.misses, min, max))};
+                  ProjectToPointCloud2D(laser_fan.misses)};
 }
 
 CompressedLaserFan3D Compress(const LaserFan3D& laser_fan) {

cartographer/sensor/laser.h

@@ -25,10 +25,6 @@
 namespace cartographer {
 namespace sensor {
 
-// Builds a LaserFan from 'proto' and separates any beams with ranges outside
-// the range ['min_range', 'max_range']. Beams beyond 'max_range' inserted into
-// the 'missing_echo_point_cloud' with length 'missing_echo_ray_length'. The
-// points in both clouds are stored in scan order.
 struct LaserFan {
   Eigen::Vector2f origin;
   PointCloud2D point_cloud;
@@ -52,6 +48,10 @@ struct LaserFan3D {
   std::vector<uint8> reflectivities;
 };
 
+// Builds a LaserFan from 'proto' and separates any beams with ranges outside
+// the range ['min_range', 'max_range']. Beams beyond 'max_range' are inserted
+// into the 'misses' point cloud with length 'missing_echo_ray_length'. The
+// points in both clouds are stored in scan order.
 LaserFan3D ToLaserFan3D(const proto::LaserScan& proto, float min_range,
                         float max_range, float missing_echo_ray_length);
 
@@ -70,11 +70,11 @@ LaserFan3D TransformLaserFan3D(const LaserFan3D& laser_fan,
 LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan,
                                     float max_range);
 
-// Projects 'laser_fan' into 2D and crops it according to the cuboid defined by
+// Crops 'laser_fan' according to the region defined by 'min_z' and 'max_z'.
-// 'min' and 'max'.
+LaserFan3D CropLaserFan(const LaserFan3D& laser_fan, float min_z, float max_z);
-LaserFan ProjectCroppedLaserFan(const LaserFan3D& laser_fan,
+
-                                const Eigen::Vector3f& min,
+// Projects 'laser_fan' into 2D.
-                                const Eigen::Vector3f& max);
+LaserFan ProjectLaserFan(const LaserFan3D& laser_fan);
 
 // Like LaserFan3D but with compressed point clouds. The point order changes
 // when converting from LaserFan3D.

cartographer/sensor/point_cloud.cc

@@ -65,12 +65,11 @@ PointCloud2D ProjectToPointCloud2D(const PointCloud& point_cloud) {
   return point_cloud_2d;
 }
 
-PointCloud Crop(const PointCloud& point_cloud, const Eigen::Vector3f& min,
+PointCloud Crop(const PointCloud& point_cloud, const float min_z,
-                const Eigen::Vector3f& max) {
+                const float max_z) {
   PointCloud cropped_point_cloud;
   for (const auto& point : point_cloud) {
-    if (min.x() <= point.x() && point.x() <= max.x() && min.y() <= point.y() &&
+    if (min_z <= point.z() && point.z() <= max_z) {
-        point.y() <= max.y() && min.z() <= point.z() && point.z() <= max.z()) {
       cropped_point_cloud.push_back(point);
     }
   }

cartographer/sensor/point_cloud.h

@@ -45,10 +45,9 @@ PointCloud ToPointCloud(const PointCloud2D& point_cloud_2d);
 // Converts 'point_cloud' to a 2D point cloud by removing the z component.
 PointCloud2D ProjectToPointCloud2D(const PointCloud& point_cloud);
 
-// Returns a new point cloud without points that fall outside the axis-aligned
+// Returns a new point cloud without points that fall outside the region defined
-// cuboid defined by 'min' and 'max'.
+// by 'min_z' and 'max_z'.
-PointCloud Crop(const PointCloud& point_cloud, const Eigen::Vector3f& min,
+PointCloud Crop(const PointCloud& point_cloud, float min_z, float max_z);
-                const Eigen::Vector3f& max);
 
 // Converts 'point_cloud' to a proto::PointCloud.
 proto::PointCloud ToProto(const PointCloud& point_cloud);