Add interface to process fixed frame pose, e.g. GPS. (#471)

cartographer/mapping/collated_trajectory_builder.cc

@@ -92,6 +92,11 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData(
       wrapped_trajectory_builder_->AddOdometerData(data->time,
                                                    data->odometer_pose);
       return;
+
+    case sensor::Data::Type::kFixedFramePose:
+      wrapped_trajectory_builder_->AddFixedFramePoseData(
+          sensor::FixedFramePoseData{data->time, data->fixed_frame_pose.pose});
+      return;
   }
   LOG(FATAL);
 }

cartographer/mapping/global_trajectory_builder_interface.h

@@ -25,6 +25,7 @@
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/pose_estimate.h"
 #include "cartographer/mapping/submaps.h"
+#include "cartographer/sensor/fixed_frame_pose_data.h"
 #include "cartographer/sensor/imu_data.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/sensor/range_data.h"
@@ -55,6 +56,8 @@ class GlobalTrajectoryBuilderInterface {
   virtual void AddImuData(const sensor::ImuData& imu_data) = 0;
   virtual void AddOdometerData(common::Time time,
                                const transform::Rigid3d& pose) = 0;
+  virtual void AddFixedFramePoseData(
+      const sensor::FixedFramePoseData& fixed_frame_pose) = 0;
 };
 
 }  // namespace mapping

cartographer/mapping/trajectory_builder.h

@@ -75,6 +75,12 @@ class TrajectoryBuilder {
     AddSensorData(sensor_id,
                   common::make_unique<sensor::Data>(time, odometer_pose));
   }
+
+  void AddFixedFramePose(const string& sensor_id, common::Time time,
+                         const transform::Rigid3d& fixed_frame_pose) {
+    AddSensorData(sensor_id,
+                  common::make_unique<sensor::Data>(time, fixed_frame_pose));
+  }
 };
 
 }  // namespace mapping

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -56,6 +56,11 @@ void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
                                       sensor::OdometryData{time, pose});
 }
 
+void GlobalTrajectoryBuilder::AddFixedFramePoseData(
+    const sensor::FixedFramePoseData& fixed_frame_pose) {
+  sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
+}
+  
 const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const {
   return local_trajectory_builder_.pose_estimate();
 }

cartographer/mapping_2d/global_trajectory_builder.h

@@ -44,6 +44,8 @@ class GlobalTrajectoryBuilder
   void AddImuData(const sensor::ImuData& imu_data) override;
   void AddOdometerData(common::Time time,
                        const transform::Rigid3d& pose) override;
+  void AddFixedFramePoseData(
+      const sensor::FixedFramePoseData& fixed_frame_pose) override;
 
  private:
   const int trajectory_id_;

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -166,6 +166,12 @@ void SparsePoseGraph::AddOdometerData(
   });
 }
 
+void SparsePoseGraph::AddFixedFramePoseData(
+    const int trajectory_id,
+    const sensor::FixedFramePoseData& fixed_frame_pose_data) {
+  LOG(FATAL) << "Not yet implemented for 2D.";
+}
+
 void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
                                         const mapping::SubmapId& submap_id) {
   CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);

cartographer/mapping_2d/sparse_pose_graph.h

@@ -38,6 +38,7 @@
 #include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h"
 #include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h"
 #include "cartographer/mapping_2d/submaps.h"
+#include "cartographer/sensor/fixed_frame_pose_data.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
@@ -76,10 +77,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       EXCLUDES(mutex_);
 
-  // Adds new IMU data to be used in the optimization.
   void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
   void AddOdometerData(int trajectory_id,
                        const sensor::OdometryData& odometry_data);
+  void AddFixedFramePoseData(
+      int trajectory_id,
+      const sensor::FixedFramePoseData& fixed_frame_pose_data);
 
   void FreezeTrajectory(int trajectory_id) override;
   void AddSubmapFromProto(int trajectory_id,

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -52,6 +52,11 @@ void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
   local_trajectory_builder_.AddOdometerData(time, pose);
 }
 
+void GlobalTrajectoryBuilder::AddFixedFramePoseData(
+    const sensor::FixedFramePoseData& fixed_frame_pose) {
+  sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
+}
+
 const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const {
   return local_trajectory_builder_.pose_estimate();
 }

cartographer/mapping_3d/global_trajectory_builder.h

@@ -41,6 +41,9 @@ class GlobalTrajectoryBuilder
                           const sensor::PointCloud& ranges) override;
   void AddOdometerData(common::Time time,
                        const transform::Rigid3d& pose) override;
+  void AddFixedFramePoseData(
+      const sensor::FixedFramePoseData& fixed_frame_pose) override;
+
   const mapping::PoseEstimate& pose_estimate() const override;
 
  private:

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -155,6 +155,12 @@ void SparsePoseGraph::AddImuData(const int trajectory_id,
   });
 }
 
+void SparsePoseGraph::AddFixedFramePoseData(
+    const int trajectory_id,
+    const sensor::FixedFramePoseData& fixed_frame_pose_data) {
+  LOG(FATAL) << "Not yet implemented for 3D.";
+}
+
 void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
                                         const mapping::SubmapId& submap_id) {
   CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);

cartographer/mapping_3d/sparse_pose_graph.h

@@ -38,6 +38,7 @@
 #include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h"
 #include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h"
 #include "cartographer/mapping_3d/submaps.h"
+#include "cartographer/sensor/fixed_frame_pose_data.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
@@ -74,8 +75,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       EXCLUDES(mutex_);
 
-  // Adds new IMU data to be used in the optimization.
   void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
+  void AddFixedFramePoseData(
+      int trajectory_id,
+      const sensor::FixedFramePoseData& fixed_frame_pose_data);
 
   void FreezeTrajectory(int trajectory_id) override;
   void AddSubmapFromProto(int trajectory_id,

cartographer/sensor/data.h

@@ -29,7 +29,7 @@ namespace sensor {
 // filled in. It is only used for time ordering sensor data before passing it
 // on.
 struct Data {
-  enum class Type { kImu, kRangefinder, kOdometer };
+  enum class Type { kImu, kRangefinder, kOdometer, kFixedFramePose };
 
   struct Imu {
     Eigen::Vector3d linear_acceleration;
@@ -41,6 +41,10 @@ struct Data {
     PointCloud ranges;
   };
 
+  struct FixedFramePose {
+    transform::Rigid3d pose;
+  };
+
   Data(const common::Time time, const Imu& imu)
       : type(Type::kImu), time(time), imu(imu) {}
 
@@ -50,11 +54,17 @@ struct Data {
   Data(const common::Time time, const transform::Rigid3d& odometer_pose)
       : type(Type::kOdometer), time(time), odometer_pose(odometer_pose) {}
 
+  Data(const common::Time time, const FixedFramePose& fixed_frame_pose)
+      : type(Type::kFixedFramePose),
+        time(time),
+        fixed_frame_pose(fixed_frame_pose) {}
+
   Type type;
   common::Time time;
   Imu imu;
   Rangefinder rangefinder;
   transform::Rigid3d odometer_pose;
+  FixedFramePose fixed_frame_pose;
 };
 
 }  // namespace sensor

cartographer/sensor/fixed_frame_pose_data.cc

@@ -0,0 +1,37 @@
+/*
+ * Copyright 2017 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/sensor/fixed_frame_pose_data.h"
+
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace sensor {
+
+proto::FixedFramePoseData ToProto(const FixedFramePoseData& pose_data) {
+  proto::FixedFramePoseData proto;
+  proto.set_timestamp(common::ToUniversal(pose_data.time));
+  *proto.mutable_pose() = transform::ToProto(pose_data.pose);
+  return proto;
+}
+
+FixedFramePoseData FromProto(const proto::FixedFramePoseData& proto) {
+  return FixedFramePoseData{common::FromUniversal(proto.timestamp()),
+                            transform::ToRigid3(proto.pose())};
+}
+
+}  // namespace sensor
+}  // namespace cartographer

cartographer/sensor/fixed_frame_pose_data.h

@@ -0,0 +1,43 @@
+/*
+ * Copyright 2017 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_SENSOR_FIXED_FRAME_POSE_DATA_H_
+#define CARTOGRAPHER_SENSOR_FIXED_FRAME_POSE_DATA_H_
+
+#include "cartographer/common/time.h"
+#include "cartographer/sensor/proto/sensor.pb.h"
+#include "cartographer/transform/rigid_transform.h"
+
+namespace cartographer {
+namespace sensor {
+
+// The fixed frame pose data(like gps, pose, etc.) will be used in the
+// optimization.
+struct FixedFramePoseData {
+  common::Time time;
+  transform::Rigid3d pose;
+};
+
+// Converts 'pose_data' to a proto::FixedFramePoseData.
+proto::FixedFramePoseData ToProto(const FixedFramePoseData& pose_data);
+
+// Converts 'proto' to an FixedFramePoseData.
+FixedFramePoseData FromProto(const proto::FixedFramePoseData& proto);
+
+}  // namespace sensor
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_SENSOR_FIXED_FRAME_POSE_DATA_H_

cartographer/sensor/proto/sensor.proto

@@ -45,3 +45,9 @@ message OdometryData {
   optional int64 timestamp = 1;
   optional transform.proto.Rigid3d pose = 2;
 }
+
+// Proto representation of ::cartographer::sensor::FixedFramePoseData.
+message FixedFramePoseData {
+  optional int64 timestamp = 1;
+  optional transform.proto.Rigid3d pose = 2;
+}