Add code to serialize a trajectory to proto. (#87)

cartographer/mapping/CMakeLists.txt

@@ -171,12 +171,16 @@ google_library(mapping_trajectory_connectivity
 
 google_library(mapping_trajectory_node
   USES_EIGEN
+  SRCS
+    trajectory_node.cc
   HDRS
     trajectory_node.h
   DEPENDS
     common_time
+    proto_trajectory
     sensor_laser
     transform_rigid_transform
+    transform_transform
 )
 
 google_test(mapping_probability_values_test

cartographer/mapping/trajectory_node.cc

@@ -0,0 +1,39 @@
+/*
+ * 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/mapping/trajectory_node.h"
+
+#include "cartographer/common/time.h"
+#include "cartographer/proto/trajectory.pb.h"
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace mapping {
+
+proto::Trajectory ToProto(const std::vector<TrajectoryNode>& nodes) {
+  proto::Trajectory trajectory;
+  for (const auto& node : nodes) {
+    const auto& data = *node.constant_data;
+    auto* node_proto = trajectory.add_node();
+    node_proto->set_timestamp(common::ToUniversal(data.time));
+    *node_proto->mutable_pose() =
+        transform::ToProto(node.pose * data.tracking_to_pose);
+  }
+  return trajectory;
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/trajectory_node.h

@@ -22,6 +22,7 @@
 
 #include "Eigen/Core"
 #include "cartographer/common/time.h"
+#include "cartographer/proto/trajectory.pb.h"
 #include "cartographer/sensor/laser.h"
 #include "cartographer/transform/rigid_transform.h"
 
@@ -66,6 +67,9 @@ struct TrajectoryNodes {
   std::vector<mapping::TrajectoryNode> trajectory_nodes;
 };
 
+::cartographer::proto::Trajectory ToProto(
+    const std::vector<TrajectoryNode>& nodes);
+
 }  // namespace mapping
 }  // namespace cartographer
 

cartographer/proto/trajectory.proto

@@ -24,8 +24,6 @@ message Trajectory {
   // NEXT_ID: 7
   message Node {
     optional int64 timestamp = 1;
-    // 2D Pose in map frame.
-    optional transform.proto.Rigid2d pose_2d = 2;
     // Transform from tracking to map frame.
     optional transform.proto.Rigid3d pose = 5;
   }