Uses MapBuilder and cleans up some configurations. (#51)

cartographer_ros/configuration_files/backpack_2d.lua

@@ -12,25 +12,27 @@
 -- See the License for the specific language governing permissions and
 -- limitations under the License.
 
-include "trajectory_builder.lua"
+include "map_builder.lua"
-include "sparse_pose_graph.lua"
 
 options = {
-  sparse_pose_graph = SPARSE_POSE_GRAPH,
+  map_builder = MAP_BUILDER,
-  trajectory_builder = TRAJECTORY_BUILDER,
   map_frame = "map",
-  odom_frame = "odom",
   tracking_frame = "base_link",
+  odom_frame = "odom",
   provide_odom_frame = true,
-  expect_odometry_data = false,
+  use_odometry_data = false,
   publish_occupancy_grid = false,
-  laser_min_range = 0.,
+  use_horizontal_laser = false,
-  laser_max_range = 30.,
+  use_horizontal_multi_echo_laser = true,
-  laser_missing_echo_ray_length = 5.,
+  horizontal_laser_min_range = 0.,
-  lookup_transform_timeout = 0.01,
+  horizontal_laser_max_range = 30.,
+  horizontal_laser_missing_echo_ray_length = 5.,
+  num_lasers_3d = 0,
+  lookup_transform_timeout_sec = 0.01,
   submap_publish_period_sec = 0.3,
   pose_publish_period_sec = 5e-3,
-  use_multi_echo_laser_scan_2d = true
 }
 
+options.map_builder.use_trajectory_builder_2d = true
+
 return options

cartographer_ros/configuration_files/backpack_3d.lua

@@ -12,27 +12,28 @@
 -- See the License for the specific language governing permissions and
 -- limitations under the License.
 
-include "trajectory_builder_3d.lua"
+include "map_builder.lua"
-include "sparse_pose_graph.lua"
 
 options = {
-  sparse_pose_graph = SPARSE_POSE_GRAPH,
+  map_builder = MAP_BUILDER,
-  trajectory_builder = TRAJECTORY_BUILDER_3D,
   map_frame = "map",
-  odom_frame = "odom",
   tracking_frame = "base_link",
+  odom_frame = "odom",
   provide_odom_frame = true,
-  expect_odometry_data = false,
+  use_odometry_data = false,
   publish_occupancy_grid = false,
-  laser_min_range = 0.,
+  use_horizontal_laser = false,
-  laser_max_range = 30.,
+  use_horizontal_multi_echo_laser = false,
-  laser_missing_echo_ray_length = 5.,
+  horizontal_laser_min_range = 0.,
-  lookup_transform_timeout = 0.01,
+  horizontal_laser_max_range = 30.,
+  horizontal_laser_missing_echo_ray_length = 5.,
+  num_lasers_3d = 2,
+  lookup_transform_timeout_sec = 0.01,
   submap_publish_period_sec = 0.3,
   pose_publish_period_sec = 5e-3,
-  num_lasers_3d = 2
 }
 
+options.map_builder.use_trajectory_builder_3d = true
 options.sparse_pose_graph.optimize_every_n_scans = 320
 options.sparse_pose_graph.constraint_builder.sampling_ratio = 0.03
 options.sparse_pose_graph.optimization_problem.ceres_solver_options.max_num_iterations = 10

cartographer_ros/configuration_files/turtlebot.lua

@@ -12,27 +12,28 @@
 -- See the License for the specific language governing permissions and
 -- limitations under the License.
 
-include "trajectory_builder.lua"
+include "map_builder.lua"
-include "sparse_pose_graph.lua"
 
 options = {
-  sparse_pose_graph = SPARSE_POSE_GRAPH,
+  map_builder = MAP_BUILDER,
-  trajectory_builder = TRAJECTORY_BUILDER,
   map_frame = "map",
-  odom_frame = "odom",
   tracking_frame = "base_link",
+  odom_frame = "odom",
   provide_odom_frame = false,
-  expect_odometry_data = false,
+  use_odometry_data = false,
   publish_occupancy_grid = false,
-  laser_min_range = 0.,
+  use_horizontal_laser = true,
-  laser_max_range = 30.,
+  use_horizontal_multi_echo_laser = false,
-  laser_missing_echo_ray_length = 5.,
+  horizontal_laser_min_range = 0.,
-  lookup_transform_timeout = 0.01,
+  horizontal_laser_max_range = 30.,
+  horizontal_laser_missing_echo_ray_length = 5.,
+  num_lasers_3d = 0,
+  lookup_transform_timeout_sec = 0.01,
   submap_publish_period_sec = 0.3,
   pose_publish_period_sec = 5e-3,
-  use_laser_scan_2d = true
 }
 
+options.map_builder.use_trajectory_builder_2d = true
 options.trajectory_builder.expect_imu_data = false
 options.trajectory_builder.use_online_correlative_scan_matching = true
 

cartographer_ros/src/cartographer_node_main.cc

@@ -32,6 +32,7 @@
 #include "cartographer/common/time.h"
 #include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping/global_trajectory_builder_interface.h"
+#include "cartographer/mapping/map_builder.h"
 #include "cartographer/mapping/proto/submaps.pb.h"
 #include "cartographer/mapping/sensor_collator.h"
 #include "cartographer/mapping_2d/global_trajectory_builder.h"
@@ -85,7 +86,7 @@ using carto::transform::Rigid3d;
 using carto::kalman_filter::PoseCovariance;
 
 // TODO(hrapp): Support multi trajectory mapping.
-constexpr int64 kTrajectoryId = 0;
+constexpr int64 kTrajectoryBuilderId = 0;
 constexpr int kSubscriberQueueSize = 150;
 constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
 
@@ -97,12 +98,86 @@ constexpr char kImuTopic[] = "/imu";
 constexpr char kOdometryTopic[] = "/odom";
 constexpr char kOccupancyGridTopic[] = "/map";
 
+struct NodeOptions {
+  carto::mapping::proto::MapBuilderOptions map_builder_options;
+  string map_frame;
+  string tracking_frame;
+  string odom_frame;
+  bool publish_occupancy_grid;
+  bool provide_odom_frame;
+  bool use_odometry_data;
+  bool use_horizontal_laser;
+  bool use_horizontal_multi_echo_laser;
+  double horizontal_laser_min_range;
+  double horizontal_laser_max_range;
+  double horizontal_laser_missing_echo_ray_length;
+  int num_lasers_3d;
+  double lookup_transform_timeout_sec;
+  double submap_publish_period_sec;
+  double pose_publish_period_sec;
+};
+
+NodeOptions CreateNodeOptions(
+    carto::common::LuaParameterDictionary* const lua_parameter_dictionary) {
+  NodeOptions options;
+  options.map_builder_options = carto::mapping::CreateMapBuilderOptions(
+      lua_parameter_dictionary->GetDictionary("map_builder").get());
+  options.map_frame = lua_parameter_dictionary->GetString("map_frame");
+  options.tracking_frame =
+      lua_parameter_dictionary->GetString("tracking_frame");
+  options.odom_frame = lua_parameter_dictionary->GetString("odom_frame");
+  options.publish_occupancy_grid =
+      lua_parameter_dictionary->GetBool("publish_occupancy_grid");
+  options.provide_odom_frame =
+      lua_parameter_dictionary->GetBool("provide_odom_frame");
+  options.use_odometry_data =
+      lua_parameter_dictionary->GetBool("use_odometry_data");
+  options.use_horizontal_laser =
+      lua_parameter_dictionary->GetBool("use_horizontal_laser");
+  options.use_horizontal_multi_echo_laser =
+      lua_parameter_dictionary->GetBool("use_horizontal_multi_echo_laser");
+  options.horizontal_laser_min_range =
+      lua_parameter_dictionary->GetDouble("horizontal_laser_min_range");
+  options.horizontal_laser_max_range =
+      lua_parameter_dictionary->GetDouble("horizontal_laser_max_range");
+  options.horizontal_laser_missing_echo_ray_length =
+      lua_parameter_dictionary->GetDouble(
+          "horizontal_laser_missing_echo_ray_length");
+  options.num_lasers_3d =
+      lua_parameter_dictionary->GetNonNegativeInt("num_lasers_3d");
+  options.lookup_transform_timeout_sec =
+      lua_parameter_dictionary->GetDouble("lookup_transform_timeout_sec");
+  options.submap_publish_period_sec =
+      lua_parameter_dictionary->GetDouble("submap_publish_period_sec");
+  options.pose_publish_period_sec =
+      lua_parameter_dictionary->GetDouble("pose_publish_period_sec");
+
+  CHECK_EQ(options.use_horizontal_laser +
+               options.use_horizontal_multi_echo_laser +
+               (options.num_lasers_3d > 0),
+           1)
+      << "Configuration error: 'use_horizontal_laser', "
+         "'use_horizontal_multi_echo_laser' and 'num_lasers_3d' are "
+         "mutually exclusive, but one is required.";
+  CHECK_EQ(
+      options.map_builder_options.use_trajectory_builder_2d(),
+      options.use_horizontal_laser || options.use_horizontal_multi_echo_laser);
+  CHECK_EQ(options.map_builder_options.use_trajectory_builder_3d(),
+           options.num_lasers_3d > 0);
+  if (options.publish_occupancy_grid) {
+    CHECK(options.map_builder_options.use_trajectory_builder_2d())
+        << "Publishing OccupancyGrids for 3D data is not yet supported";
+  }
+  return options;
+}
+
 // Node that listens to all the sensor data that we are interested in and wires
 // it up to the SLAM.
 class Node {
  public:
-  Node();
+  Node(const NodeOptions& options);
   ~Node();
+
   Node(const Node&) = delete;
   Node& operator=(const Node&) = delete;
 
@@ -121,8 +196,8 @@ class Node {
   void AddLaserFan3D(int64 timestamp, const string& frame_id,
                      const proto::LaserFan3D& laser_fan_3d);
 
-  // Returns a transform for 'frame_id' to 'tracking_frame_' if it exists at
+  // Returns a transform for 'frame_id' to 'options_.tracking_frame' if it
-  // 'time' or throws tf2::TransformException if it does not exist.
+  // exists at 'time' or throws tf2::TransformException if it does not exist.
   Rigid3d LookupToTrackingTransformOrThrow(carto::common::Time time,
                                            const string& frame_id);
 
@@ -130,42 +205,30 @@ class Node {
       ::cartographer_ros_msgs::SubmapQuery::Request& request,
       ::cartographer_ros_msgs::SubmapQuery::Response& response);
 
-  void PublishSubmapList(const ros::WallTimerEvent& timer_event);
+  void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
-  void PublishPose(const ros::WallTimerEvent& timer_event);
+  void PublishPose(const ::ros::WallTimerEvent& timer_event);
   void SpinOccupancyGridThreadForever();
 
-  // TODO(hrapp): Pull out the common functionality between this and MapWriter
+  const NodeOptions options_;
-  // into an open sourcable MapWriter.
-  carto::mapping::SensorCollator<SensorData> sensor_collator_;
-  SensorDataProducer sensor_data_producer_;
-  ros::NodeHandle node_handle_;
-  ros::Subscriber imu_subscriber_;
-  ros::Subscriber laser_subscriber_2d_;
-  std::vector<ros::Subscriber> laser_subscribers_3d_;
-  ros::Subscriber odometry_subscriber_;
-  string tracking_frame_;
-  string odom_frame_;
-  string map_frame_;
-  bool provide_odom_frame_;
-  bool expect_odometry_data_;
-  double laser_min_range_;
-  double laser_max_range_;
-  double laser_missing_echo_ray_length_;
-  double lookup_transform_timeout_;
 
-  tf2_ros::Buffer tf_buffer_;
-  tf2_ros::TransformListener tf_;
-  carto::common::ThreadPool thread_pool_;
   carto::common::Mutex mutex_;
-  std::unique_ptr<carto::mapping::GlobalTrajectoryBuilderInterface>
+  std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_data_
-      trajectory_builder_ GUARDED_BY(mutex_);
-  std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_node_data_
       GUARDED_BY(mutex_);
-  std::unique_ptr<carto::mapping::SparsePoseGraph> sparse_pose_graph_;
+  carto::mapping::MapBuilder map_builder_ GUARDED_BY(mutex_);
+  carto::mapping::SensorCollator<SensorData> sensor_collator_
+      GUARDED_BY(mutex_);
+  SensorDataProducer sensor_data_producer_ GUARDED_BY(mutex_);
 
+  ::ros::NodeHandle node_handle_;
+  ::ros::Subscriber imu_subscriber_;
+  ::ros::Subscriber horizontal_laser_scan_subscriber_;
+  std::vector<::ros::Subscriber> laser_subscribers_3d_;
+  ::ros::Subscriber odometry_subscriber_;
   ::ros::Publisher submap_list_publisher_;
   ::ros::ServiceServer submap_query_server_;
 
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_;
   tf2_ros::TransformBroadcaster tf_broadcaster_;
 
   ::ros::Publisher occupancy_grid_publisher_;
@@ -177,17 +240,18 @@ class Node {
   std::chrono::steady_clock::time_point last_sensor_data_rates_logging_time_;
   std::map<string, carto::common::RateTimer<>> rate_timers_;
 
-  // We have to keep the timer handles of ros::WallTimers around, otherwise they
+  // We have to keep the timer handles of ::ros::WallTimers around, otherwise
-  // do not fire.
+  // they do not fire.
-  std::vector<ros::WallTimer> wall_timers_;
+  std::vector<::ros::WallTimer> wall_timers_;
 };
 
-Node::Node()
+Node::Node(const NodeOptions& options)
-    : sensor_data_producer_(kTrajectoryId, &sensor_collator_),
+    : options_(options),
+      map_builder_(options.map_builder_options, &constant_data_),
+      sensor_data_producer_(kTrajectoryBuilderId, &sensor_collator_),
       node_handle_("~"),
-      tf_buffer_(ros::Duration(1000)),
+      tf_buffer_(::ros::Duration(1000)),
-      tf_(tf_buffer_),
+      tf_(tf_buffer_) {}
-      thread_pool_(10) {}
 
 Node::~Node() {
   {
@@ -201,15 +265,16 @@ Node::~Node() {
 
 Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time,
                                                const string& frame_id) {
-  return ToRigid3d(
+  return ToRigid3d(tf_buffer_.lookupTransform(
-      tf_buffer_.lookupTransform(tracking_frame_, frame_id, ToRos(time),
+      options_.tracking_frame, frame_id, ToRos(time),
-                                 ros::Duration(lookup_transform_timeout_)));
+      ::ros::Duration(options_.lookup_transform_timeout_sec)));
 }
 
 void Node::AddOdometry(int64 timestamp, const string& frame_id,
                        const Rigid3d& pose, const PoseCovariance& covariance) {
   const carto::common::Time time = carto::common::FromUniversal(timestamp);
-  trajectory_builder_->AddOdometerPose(time, pose, covariance);
+  map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
+      ->AddOdometerPose(time, pose, covariance);
 }
 
 void Node::AddImu(const int64 timestamp, const string& frame_id,
@@ -222,14 +287,15 @@ void Node::AddImu(const int64 timestamp, const string& frame_id,
         << "The IMU frame must be colocated with the tracking frame. "
            "Transforming linear accelaration into the tracking frame will "
            "otherwise be imprecise.";
-    trajectory_builder_->AddImuData(
+    map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
-        time, sensor_to_tracking.rotation() *
+        ->AddImuData(time,
+                     sensor_to_tracking.rotation() *
                          carto::transform::ToEigen(imu.linear_acceleration()),
                      sensor_to_tracking.rotation() *
                          carto::transform::ToEigen(imu.angular_velocity()));
   } catch (const tf2::TransformException& ex) {
-    LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
+    LOG(WARNING) << "Cannot transform " << frame_id << " -> "
-                 << ": " << ex.what();
+                 << options_.tracking_frame << ": " << ex.what();
   }
 }
 void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id,
@@ -239,16 +305,18 @@ void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id,
     const Rigid3d sensor_to_tracking =
         LookupToTrackingTransformOrThrow(time, frame_id);
     const carto::sensor::LaserFan laser_fan = carto::sensor::ToLaserFan(
-        laser_scan, laser_min_range_, laser_max_range_,
+        laser_scan, options_.horizontal_laser_min_range,
-        laser_missing_echo_ray_length_);
+        options_.horizontal_laser_max_range,
+        options_.horizontal_laser_missing_echo_ray_length);
 
     const auto laser_fan_3d = carto::sensor::TransformLaserFan3D(
         carto::sensor::ToLaserFan3D(laser_fan),
         sensor_to_tracking.cast<float>());
-    trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d);
+    map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
+        ->AddHorizontalLaserFan(time, laser_fan_3d);
   } catch (const tf2::TransformException& ex) {
-    LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
+    LOG(WARNING) << "Cannot transform " << frame_id << " -> "
-                 << ": " << ex.what();
+                 << options_.tracking_frame << ": " << ex.what();
   }
 }
 
@@ -258,62 +326,24 @@ void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id,
   try {
     const Rigid3d sensor_to_tracking =
         LookupToTrackingTransformOrThrow(time, frame_id);
-    trajectory_builder_->AddLaserFan3D(
+    map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
-        time, carto::sensor::TransformLaserFan3D(
+        ->AddLaserFan3D(time, carto::sensor::TransformLaserFan3D(
                                   carto::sensor::FromProto(laser_fan_3d),
                                   sensor_to_tracking.cast<float>()));
   } catch (const tf2::TransformException& ex) {
-    LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
+    LOG(WARNING) << "Cannot transform " << frame_id << " -> "
-                 << ": " << ex.what();
+                 << options_.tracking_frame << ": " << ex.what();
   }
 }
 
 void Node::Initialize() {
-  auto file_resolver =
-      carto::common::make_unique<carto::common::ConfigurationFileResolver>(
-          std::vector<string>{FLAGS_configuration_directory});
-  const string code =
-      file_resolver->GetFileContentOrDie(FLAGS_configuration_basename);
-  carto::common::LuaParameterDictionary lua_parameter_dictionary(
-      code, std::move(file_resolver), nullptr);
-
-  tracking_frame_ = lua_parameter_dictionary.GetString("tracking_frame");
-  odom_frame_ = lua_parameter_dictionary.GetString("odom_frame");
-  map_frame_ = lua_parameter_dictionary.GetString("map_frame");
-  provide_odom_frame_ = lua_parameter_dictionary.GetBool("provide_odom_frame");
-  expect_odometry_data_ =
-      lua_parameter_dictionary.GetBool("expect_odometry_data");
-  laser_min_range_ = lua_parameter_dictionary.GetDouble("laser_min_range");
-  laser_max_range_ = lua_parameter_dictionary.GetDouble("laser_max_range");
-  laser_missing_echo_ray_length_ =
-      lua_parameter_dictionary.GetDouble("laser_missing_echo_ray_length");
-  lookup_transform_timeout_ =
-      lua_parameter_dictionary.GetDouble("lookup_transform_timeout");
-
   // Set of all topics we subscribe to. We use the non-remapped default names
   // which are unique.
   std::unordered_set<string> expected_sensor_identifiers;
 
-  // Subscribe to exactly one laser.
+  // For 2D SLAM, subscribe to exactly one horizontal laser.
-  const bool has_laser_scan_2d =
+  if (options_.use_horizontal_laser) {
-      lua_parameter_dictionary.HasKey("use_laser_scan_2d") &&
+    horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
-      lua_parameter_dictionary.GetBool("use_laser_scan_2d");
-  const bool has_multi_echo_laser_scan_2d =
-      lua_parameter_dictionary.HasKey("use_multi_echo_laser_scan_2d") &&
-      lua_parameter_dictionary.GetBool("use_multi_echo_laser_scan_2d");
-  const int num_lasers_3d =
-      lua_parameter_dictionary.HasKey("num_lasers_3d")
-          ? lua_parameter_dictionary.GetNonNegativeInt("num_lasers_3d")
-          : 0;
-
-  CHECK(has_laser_scan_2d + has_multi_echo_laser_scan_2d +
-            (num_lasers_3d > 0) ==
-        1)
-      << "Parameters 'use_laser_scan_2d', 'use_multi_echo_laser_scan_2d' and "
-         "'num_lasers_3d' are mutually exclusive, but one is required.";
-
-  if (has_laser_scan_2d) {
-    laser_subscriber_2d_ = node_handle_.subscribe(
         kLaserScanTopic, kSubscriberQueueSize,
         boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
             [this](const sensor_msgs::LaserScan::ConstPtr& msg) {
@@ -321,8 +351,8 @@ void Node::Initialize() {
             }));
     expected_sensor_identifiers.insert(kLaserScanTopic);
   }
-  if (has_multi_echo_laser_scan_2d) {
+  if (options_.use_horizontal_multi_echo_laser) {
-    laser_subscriber_2d_ = node_handle_.subscribe(
+    horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
         kMultiEchoLaserScanTopic, kSubscriberQueueSize,
         boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
             [this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
@@ -332,28 +362,11 @@ void Node::Initialize() {
     expected_sensor_identifiers.insert(kMultiEchoLaserScanTopic);
   }
 
-  bool expect_imu_data = true;
+  // For 3D SLAM, subscribe to all 3D lasers.
-  if (has_laser_scan_2d || has_multi_echo_laser_scan_2d) {
+  if (options_.num_lasers_3d > 0) {
-    auto sparse_pose_graph_2d =
+    for (int i = 0; i < options_.num_lasers_3d; ++i) {
-        carto::common::make_unique<carto::mapping_2d::SparsePoseGraph>(
-            carto::mapping::CreateSparsePoseGraphOptions(
-                lua_parameter_dictionary.GetDictionary("sparse_pose_graph")
-                    .get()),
-            &thread_pool_, &constant_node_data_);
-    auto options = carto::mapping_2d::CreateLocalTrajectoryBuilderOptions(
-        lua_parameter_dictionary.GetDictionary("trajectory_builder").get());
-    submaps_options_ = options.submaps_options();
-    expect_imu_data = options.expect_imu_data();
-    trajectory_builder_ =
-        carto::common::make_unique<carto::mapping_2d::GlobalTrajectoryBuilder>(
-            options, sparse_pose_graph_2d.get());
-    sparse_pose_graph_ = std::move(sparse_pose_graph_2d);
-  }
-
-  if (num_lasers_3d > 0) {
-    for (int i = 0; i < num_lasers_3d; ++i) {
       string topic = kPointCloud2Topic;
-      if (num_lasers_3d > 1) {
+      if (options_.num_lasers_3d > 1) {
         topic += "_" + std::to_string(i + 1);
       }
       laser_subscribers_3d_.push_back(node_handle_.subscribe(
@@ -364,24 +377,14 @@ void Node::Initialize() {
               })));
       expected_sensor_identifiers.insert(topic);
     }
-    auto sparse_pose_graph_3d =
-        carto::common::make_unique<carto::mapping_3d::SparsePoseGraph>(
-            carto::mapping::CreateSparsePoseGraphOptions(
-                lua_parameter_dictionary.GetDictionary("sparse_pose_graph")
-                    .get()),
-            &thread_pool_, &constant_node_data_);
-    trajectory_builder_ =
-        carto::common::make_unique<carto::mapping_3d::GlobalTrajectoryBuilder>(
-            carto::mapping_3d::CreateLocalTrajectoryBuilderOptions(
-                lua_parameter_dictionary.GetDictionary("trajectory_builder")
-                    .get()),
-            sparse_pose_graph_3d.get());
-    sparse_pose_graph_ = std::move(sparse_pose_graph_3d);
   }
-  CHECK(sparse_pose_graph_ != nullptr);
 
-  // Maybe subscribe to the IMU.
+  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
-  if (expect_imu_data) {
+  // required.
+  if (options_.map_builder_options.use_trajectory_builder_3d() ||
+      (options_.map_builder_options.use_trajectory_builder_2d() &&
+       options_.map_builder_options.trajectory_builder_2d_options()
+           .use_imu_data())) {
     imu_subscriber_ = node_handle_.subscribe(
         kImuTopic, kSubscriberQueueSize,
         boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
@@ -391,7 +394,7 @@ void Node::Initialize() {
     expected_sensor_identifiers.insert(kImuTopic);
   }
 
-  if (expect_odometry_data_) {
+  if (options_.use_odometry_data) {
     odometry_subscriber_ = node_handle_.subscribe(
         kOdometryTopic, kSubscriberQueueSize,
         boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
@@ -401,8 +404,10 @@ void Node::Initialize() {
     expected_sensor_identifiers.insert(kOdometryTopic);
   }
 
+  // TODO(damonkohler): Add multi-trajectory support.
+  CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder());
   sensor_collator_.AddTrajectory(
-      kTrajectoryId, expected_sensor_identifiers,
+      kTrajectoryBuilderId, expected_sensor_identifiers,
       [this](const int64 timestamp, std::unique_ptr<SensorData> sensor_data) {
         HandleSensorData(timestamp, std::move(sensor_data));
       });
@@ -413,9 +418,7 @@ void Node::Initialize() {
   submap_query_server_ = node_handle_.advertiseService(
       kSubmapQueryServiceName, &Node::HandleSubmapQuery, this);
 
-  if (lua_parameter_dictionary.GetBool("publish_occupancy_grid")) {
+  if (options_.publish_occupancy_grid) {
-    CHECK_EQ(num_lasers_3d, 0)
-        << "Publishing OccupancyGrids for 3D data is not yet supported";
     occupancy_grid_publisher_ =
         node_handle_.advertise<::nav_msgs::OccupancyGrid>(kOccupancyGridTopic,
                                                           1, true);
@@ -424,13 +427,11 @@ void Node::Initialize() {
   }
 
   wall_timers_.push_back(node_handle_.createWallTimer(
-      ros::WallDuration(
+      ::ros::WallDuration(options_.submap_publish_period_sec),
-          lua_parameter_dictionary.GetDouble("submap_publish_period_sec")),
       &Node::PublishSubmapList, this));
   wall_timers_.push_back(node_handle_.createWallTimer(
-      ros::WallDuration(
+      ::ros::WallDuration(options_.pose_publish_period_sec), &Node::PublishPose,
-          lua_parameter_dictionary.GetDouble("pose_publish_period_sec")),
+      this));
-      &Node::PublishPose, this));
 }
 
 bool Node::HandleSubmapQuery(
@@ -442,7 +443,8 @@ bool Node::HandleSubmapQuery(
 
   carto::common::MutexLocker lock(&mutex_);
   // TODO(hrapp): return error messages and extract common code from MapBuilder.
-  carto::mapping::Submaps* submaps = trajectory_builder_->submaps();
+  carto::mapping::Submaps* submaps =
+      map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps();
   if (request.submap_id < 0 || request.submap_id >= submaps->size()) {
     return false;
   }
@@ -452,10 +454,10 @@ bool Node::HandleSubmapQuery(
   response_proto.set_submap_version(
       submaps->Get(request.submap_id)->end_laser_fan_index);
   const std::vector<carto::transform::Rigid3d> submap_transforms =
-      sparse_pose_graph_->GetSubmapTransforms(*submaps);
+      map_builder_.sparse_pose_graph()->GetSubmapTransforms(*submaps);
 
   submaps->SubmapToProto(request.submap_id,
-                         sparse_pose_graph_->GetTrajectoryNodes(),
+                         map_builder_.sparse_pose_graph()->GetTrajectoryNodes(),
                          submap_transforms[request.submap_id], &response_proto);
 
   response.submap_version = response_proto.submap_version();
@@ -483,11 +485,12 @@ bool Node::HandleSubmapQuery(
   return true;
 }
 
-void Node::PublishSubmapList(const ros::WallTimerEvent& timer_event) {
+void Node::PublishSubmapList(const ::ros::WallTimerEvent& timer_event) {
   carto::common::MutexLocker lock(&mutex_);
-  const carto::mapping::Submaps* submaps = trajectory_builder_->submaps();
+  const carto::mapping::Submaps* submaps =
+      map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps();
   const std::vector<carto::transform::Rigid3d> submap_transforms =
-      sparse_pose_graph_->GetSubmapTransforms(*submaps);
+      map_builder_.sparse_pose_graph()->GetSubmapTransforms(*submaps);
   CHECK_EQ(submap_transforms.size(), submaps->size());
 
   ::cartographer_ros_msgs::TrajectorySubmapList ros_trajectory;
@@ -499,44 +502,49 @@ void Node::PublishSubmapList(const ros::WallTimerEvent& timer_event) {
   }
 
   ::cartographer_ros_msgs::SubmapList ros_submap_list;
-  ros_submap_list.header.stamp = ros::Time::now();
+  ros_submap_list.header.stamp = ::ros::Time::now();
-  ros_submap_list.header.frame_id = map_frame_;
+  ros_submap_list.header.frame_id = options_.map_frame;
   ros_submap_list.trajectory.push_back(ros_trajectory);
   submap_list_publisher_.publish(ros_submap_list);
 }
 
-void Node::PublishPose(const ros::WallTimerEvent& timer_event) {
+void Node::PublishPose(const ::ros::WallTimerEvent& timer_event) {
   carto::common::MutexLocker lock(&mutex_);
-  const Rigid3d tracking_to_local = trajectory_builder_->pose_estimate().pose;
+  const Rigid3d tracking_to_local =
-  const carto::mapping::Submaps* submaps = trajectory_builder_->submaps();
+      map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
+          ->pose_estimate()
+          .pose;
+  const carto::mapping::Submaps* submaps =
+      map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps();
   const Rigid3d local_to_map =
-      sparse_pose_graph_->GetLocalToGlobalTransform(*submaps);
+      map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform(*submaps);
   const Rigid3d tracking_to_map = local_to_map * tracking_to_local;
 
-  const ros::Time now = ros::Time::now();
+  const ::ros::Time now = ::ros::Time::now();
   geometry_msgs::TransformStamped stamped_transform;
   stamped_transform.header.stamp = now;
-  stamped_transform.header.frame_id = map_frame_;
+  stamped_transform.header.frame_id = options_.map_frame;
-  stamped_transform.child_frame_id = odom_frame_;
+  stamped_transform.child_frame_id = options_.odom_frame;
 
-  if (provide_odom_frame_) {
+  if (options_.provide_odom_frame) {
     stamped_transform.transform = ToGeometryMsgTransform(local_to_map);
     tf_broadcaster_.sendTransform(stamped_transform);
 
-    stamped_transform.header.frame_id = odom_frame_;
+    stamped_transform.header.frame_id = options_.odom_frame;
-    stamped_transform.child_frame_id = tracking_frame_;
+    stamped_transform.child_frame_id = options_.tracking_frame;
     stamped_transform.transform = ToGeometryMsgTransform(tracking_to_local);
     tf_broadcaster_.sendTransform(stamped_transform);
   } else {
     try {
       const Rigid3d tracking_to_odom =
-          LookupToTrackingTransformOrThrow(FromRos(now), odom_frame_).inverse();
+          LookupToTrackingTransformOrThrow(FromRos(now), options_.odom_frame)
+              .inverse();
       const Rigid3d odom_to_map = tracking_to_map * tracking_to_odom.inverse();
       stamped_transform.transform = ToGeometryMsgTransform(odom_to_map);
       tf_broadcaster_.sendTransform(stamped_transform);
     } catch (const tf2::TransformException& ex) {
-      LOG(WARNING) << "Cannot transform " << tracking_frame_ << " -> "
+      LOG(WARNING) << "Cannot transform " << options_.tracking_frame << " -> "
-                   << odom_frame_ << ": " << ex.what();
+                   << options_.odom_frame << ": " << ex.what();
     }
   }
 }
@@ -549,7 +557,8 @@ void Node::SpinOccupancyGridThreadForever() {
         return;
       }
     }
-    const auto trajectory_nodes = sparse_pose_graph_->GetTrajectoryNodes();
+    const auto trajectory_nodes =
+        map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
     if (trajectory_nodes.empty()) {
       std::this_thread::sleep_for(carto::common::FromMilliseconds(1000));
       continue;
@@ -571,7 +580,7 @@ void Node::SpinOccupancyGridThreadForever() {
 
     ::nav_msgs::OccupancyGrid occupancy_grid;
     occupancy_grid.header.stamp = ToRos(trajectory_nodes.back().time());
-    occupancy_grid.header.frame_id = map_frame_;
+    occupancy_grid.header.frame_id = options_.map_frame;
     occupancy_grid.info.map_load_time = occupancy_grid.header.stamp;
 
     Eigen::Array2i offset;
@@ -661,10 +670,18 @@ void Node::HandleSensorData(const int64 timestamp,
   LOG(FATAL);
 }
 
-void Node::SpinForever() { ros::spin(); }
+void Node::SpinForever() { ::ros::spin(); }
 
 void Run() {
-  Node node;
+  auto file_resolver =
+      carto::common::make_unique<carto::common::ConfigurationFileResolver>(
+          std::vector<string>{FLAGS_configuration_directory});
+  const string code =
+      file_resolver->GetFileContentOrDie(FLAGS_configuration_basename);
+  carto::common::LuaParameterDictionary lua_parameter_dictionary(
+      code, std::move(file_resolver), nullptr);
+
+  Node node(CreateNodeOptions(&lua_parameter_dictionary));
   node.Initialize();
   node.SpinForever();
 }
@@ -729,11 +746,11 @@ int main(int argc, char** argv) {
   CHECK(!FLAGS_configuration_basename.empty())
       << "-configuration_basename is missing.";
 
-  ros::init(argc, argv, "cartographer_node");
+  ::ros::init(argc, argv, "cartographer_node");
-  ros::start();
+  ::ros::start();
 
   ::cartographer_ros::ScopedRosLogSink ros_log_sink;
   ::cartographer_ros::Run();
-  ros::shutdown();
+  ::ros::shutdown();
   return 0;
 }