Write XRays for 3D runs when finalizing a trajectory. (#116)

* Write XRays for 3D runs when finalizing a trajectory.

cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -87,6 +87,14 @@ google_library(time_conversion
     time_conversion.h
 )
 
+google_library(xray
+  USES_CARTOGRAPHER
+  SRCS
+    xray.cc
+  HDRS
+    xray.h
+)
+
 google_catkin_test(time_conversion_test
   USES_CARTOGRAPHER
   USES_ROS
@@ -113,6 +121,7 @@ google_binary(cartographer_node
     sensor_bridge
     tf_bridge
     time_conversion
+    xray
 )
 
 install(TARGETS cartographer_node

cartographer_ros/cartographer_ros/node_main.cc

@@ -55,6 +55,7 @@
 #include "cartographer_ros/sensor_bridge.h"
 #include "cartographer_ros/tf_bridge.h"
 #include "cartographer_ros/time_conversion.h"
+#include "cartographer_ros/xray.h"
 #include "cartographer_ros_msgs/FinishTrajectory.h"
 #include "cartographer_ros_msgs/SubmapEntry.h"
 #include "cartographer_ros_msgs/SubmapList.h"
@@ -363,17 +364,26 @@ bool Node::HandleFinishTrajectory(
   // TODO(whess): Add multi-trajectory support.
   sensor_collator_.FinishTrajectory(kTrajectoryBuilderId);
   map_builder_.sparse_pose_graph()->RunFinalOptimization();
-  // TODO(whess): Write X-rays in 3D.
-  if (options_.map_builder_options.use_trajectory_builder_2d()) {
+
   const auto trajectory_nodes =
       map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
-    if (!trajectory_nodes.empty()) {
+  if (trajectory_nodes.empty()) {
+    LOG(WARNING) << "Map is empty and will not be saved.";
+    return true;
+  }
+
+  if (options_.map_builder_options.use_trajectory_builder_2d()) {
     ::nav_msgs::OccupancyGrid occupancy_grid;
     BuildOccupancyGrid(trajectory_nodes, options_, &occupancy_grid);
     WriteOccupancyGridToPgmAndYaml(occupancy_grid, request.stem);
-    } else {
-      LOG(WARNING) << "Map is empty and will not be saved.";
   }
+
+  if (options_.map_builder_options.use_trajectory_builder_3d()) {
+    WriteXRayImages(trajectory_nodes,
+                    options_.map_builder_options.trajectory_builder_3d_options()
+                        .submaps_options()
+                        .high_resolution(),
+                    request.stem);
   }
   return true;
 }

cartographer_ros/cartographer_ros/xray.cc

@@ -0,0 +1,57 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer_ros/xray.h"
+
+#include "cartographer/io/null_points_processor.h"
+#include "cartographer/io/points_processor.h"
+#include "cartographer/io/xray_points_processor.h"
+
+namespace cartographer_ros {
+
+namespace carto = ::cartographer;
+
+void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>&
+                         trajectory_nodes,
+                     const double voxel_size, const std::string& stem) {
+  carto::io::NullPointsProcessor null_points_processor;
+  carto::io::XRayPointsProcessor xy_xray_points_processor(
+      voxel_size, carto::transform::Rigid3f::Rotation(
+                      Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
+      stem + "_xray_xy.png", &null_points_processor);
+  carto::io::XRayPointsProcessor yz_xray_points_processor(
+      voxel_size, carto::transform::Rigid3f::Rotation(
+                      Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
+      stem + "_xray_yz.png", &xy_xray_points_processor);
+  carto::io::XRayPointsProcessor xz_xray_points_processor(
+      voxel_size, carto::transform::Rigid3f::Rotation(
+                      Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
+      stem + "_xray_xz.png", &yz_xray_points_processor);
+
+  for (const auto& node : trajectory_nodes) {
+    const carto::sensor::LaserFan laser_fan = carto::sensor::TransformLaserFan(
+        carto::sensor::Decompress(node.constant_data->laser_fan_3d),
+        node.pose.cast<float>());
+
+    carto::io::PointsBatch points_batch;
+    points_batch.origin = laser_fan.origin;
+    points_batch.points = laser_fan.returns;
+    xz_xray_points_processor.Process(points_batch);
+  }
+  xz_xray_points_processor.Flush();
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/xray.h

@@ -0,0 +1,35 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_ROS_XRAY_H_
+#define CARTOGRAPHER_ROS_XRAY_H_
+
+#include <string>
+#include <vector>
+
+#include "cartographer/mapping/trajectory_node.h"
+
+namespace cartographer_ros {
+
+// Writes X-ray images from the 'trajectory_nodes'. The filenames will all start
+// with 'stem'.
+void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>&
+                         trajectory_nodes,
+                     double voxel_size, const std::string& stem);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_XRAY_H_