Serialize trajectory node data. (#504)

Related to #253.

cartographer/mapping/map_builder.cc

@@ -168,7 +168,7 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
       }
     }
   }
-  // Next we serialize all range data.
+  // Next we serialize all node data.
   {
     const auto node_data = sparse_pose_graph_->GetTrajectoryNodes();
     for (int trajectory_id = 0;
@@ -177,11 +177,12 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
            node_index != static_cast<int>(node_data[trajectory_id].size());
            ++node_index) {
         proto::SerializedData proto;
-        auto* const range_data_proto = proto.mutable_range_data();
+        auto* const node_data_proto = proto.mutable_node_data();
         // TODO(whess): Handle trimmed data.
-        range_data_proto->mutable_node_id()->set_trajectory_id(trajectory_id);
-        range_data_proto->mutable_node_id()->set_node_index(node_index);
-        range_data_proto->mutable_range_data();
+        node_data_proto->mutable_node_id()->set_trajectory_id(trajectory_id);
+        node_data_proto->mutable_node_id()->set_node_index(node_index);
+        *node_data_proto->mutable_trajectory_node() =
+            ToProto(*node_data[trajectory_id][node_index].constant_data);
         // TODO(whess): Only enable optionally? Resulting pbstream files will be
         // a lot larger now.
         writer->WriteProto(proto);

cartographer/mapping/proto/serialization.proto

@@ -18,7 +18,7 @@ package cartographer.mapping.proto;
 
 import "cartographer/mapping/proto/sparse_pose_graph.proto";
 import "cartographer/mapping/proto/submap.proto";
-import "cartographer/sensor/proto/sensor.proto";
+import "cartographer/mapping/proto/trajectory_node.proto";
 
 message Submap {
   optional SubmapId submap_id = 1;
@@ -26,13 +26,13 @@ message Submap {
   optional Submap3D submap_3d = 3;
 }
 
-message RangeData {
+message NodeData {
   optional NodeId node_id = 1;
-  optional sensor.proto.CompressedRangeData range_data = 4;
+  optional TrajectoryNode trajectory_node = 5;
 }
 
 message SerializedData {
   optional Submap submap = 1;
-  optional RangeData range_data = 2;
+  optional NodeData node_data = 2;
   // TODO(whess): Add IMU data, odometry.
 }

cartographer/mapping/proto/trajectory_node.proto

@@ -0,0 +1,31 @@
+// 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.
+
+syntax = "proto2";
+
+package cartographer.mapping.proto;
+
+import "cartographer/sensor/proto/sensor.proto";
+import "cartographer/transform/proto/transform.proto";
+
+// Serialized state of a mapping::TrajectoryNode.
+message TrajectoryNode {
+  optional int64 timestamp = 1;
+  optional transform.proto.Quaterniond gravity_alignment = 2;
+  optional sensor.proto.CompressedPointCloud
+      filtered_gravity_aligned_point_cloud = 3;
+  optional sensor.proto.CompressedPointCloud high_resolution_point_cloud = 4;
+  optional sensor.proto.CompressedPointCloud low_resolution_point_cloud = 5;
+  repeated float rotational_scan_matcher_histogram = 6;
+}

cartographer/mapping/trajectory_node.cc

@@ -0,0 +1,70 @@
+/*
+ * 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/mapping/trajectory_node.h"
+
+#include "Eigen/Core"
+#include "cartographer/common/time.h"
+#include "cartographer/sensor/compressed_point_cloud.h"
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace mapping {
+
+proto::TrajectoryNode ToProto(const TrajectoryNode::Data& constant_data) {
+  proto::TrajectoryNode proto;
+  proto.set_timestamp(common::ToUniversal(constant_data.time));
+  *proto.mutable_gravity_alignment() =
+      transform::ToProto(constant_data.gravity_alignment);
+  *proto.mutable_filtered_gravity_aligned_point_cloud() =
+      sensor::CompressedPointCloud(
+          constant_data.filtered_gravity_aligned_point_cloud)
+          .ToProto();
+  *proto.mutable_high_resolution_point_cloud() =
+      sensor::CompressedPointCloud(constant_data.high_resolution_point_cloud)
+          .ToProto();
+  *proto.mutable_low_resolution_point_cloud() =
+      sensor::CompressedPointCloud(constant_data.low_resolution_point_cloud)
+          .ToProto();
+  for (Eigen::VectorXf::Index i = 0;
+       i != constant_data.rotational_scan_matcher_histogram.size(); ++i) {
+    proto.add_rotational_scan_matcher_histogram(
+        constant_data.rotational_scan_matcher_histogram(i));
+  }
+  return proto;
+}
+
+TrajectoryNode::Data FromProto(const proto::TrajectoryNode& proto) {
+  Eigen::VectorXf rotational_scan_matcher_histogram(
+      proto.rotational_scan_matcher_histogram_size());
+  for (int i = 0; i != proto.rotational_scan_matcher_histogram_size(); ++i) {
+    rotational_scan_matcher_histogram(i) =
+        proto.rotational_scan_matcher_histogram(i);
+  }
+  return TrajectoryNode::Data{
+      common::FromUniversal(proto.timestamp()),
+      transform::ToEigen(proto.gravity_alignment()),
+      sensor::CompressedPointCloud(proto.filtered_gravity_aligned_point_cloud())
+          .Decompress(),
+      sensor::CompressedPointCloud(proto.high_resolution_point_cloud())
+          .Decompress(),
+      sensor::CompressedPointCloud(proto.low_resolution_point_cloud())
+          .Decompress(),
+      rotational_scan_matcher_histogram};
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/trajectory_node.h

@@ -22,6 +22,7 @@
 
 #include "Eigen/Core"
 #include "cartographer/common/time.h"
+#include "cartographer/mapping/proto/trajectory_node.pb.h"
 #include "cartographer/sensor/range_data.h"
 #include "cartographer/transform/rigid_transform.h"
 
@@ -56,6 +57,9 @@ struct TrajectoryNode {
   transform::Rigid3d pose;
 };
 
+proto::TrajectoryNode ToProto(const TrajectoryNode::Data& constant_data);
+TrajectoryNode::Data FromProto(const proto::TrajectoryNode& proto);
+
 }  // namespace mapping
 }  // namespace cartographer
 

cartographer/mapping/trajectory_node_test.cc

@@ -0,0 +1,56 @@
+/*
+ * 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/mapping/trajectory_node.h"
+
+#include <limits>
+
+#include "Eigen/Core"
+#include "cartographer/common/time.h"
+#include "cartographer/mapping/proto/trajectory_node.pb.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+TEST(TrajectoryNodeTest, ToAndFromProto) {
+  const TrajectoryNode::Data expected{
+      common::FromUniversal(42),
+      Eigen::Quaterniond(1., 2., -3., -4.),
+      sensor::CompressedPointCloud({{1.f, 2.f, 0.f}, {0.f, 0.f, 1.f}})
+          .Decompress(),
+      sensor::CompressedPointCloud({{2.f, 3.f, 4.f}}).Decompress(),
+      sensor::CompressedPointCloud({{-1.f, 2.f, 0.f}}).Decompress(),
+      Eigen::VectorXf::Unit(20, 4),
+  };
+  const proto::TrajectoryNode proto = ToProto(expected);
+  const TrajectoryNode::Data actual = FromProto(proto);
+  EXPECT_EQ(expected.time, actual.time);
+  EXPECT_TRUE(actual.gravity_alignment.isApprox(expected.gravity_alignment));
+  EXPECT_EQ(expected.filtered_gravity_aligned_point_cloud,
+            actual.filtered_gravity_aligned_point_cloud);
+  EXPECT_EQ(expected.high_resolution_point_cloud,
+            actual.high_resolution_point_cloud);
+  EXPECT_EQ(expected.low_resolution_point_cloud,
+            actual.low_resolution_point_cloud);
+  EXPECT_EQ(expected.rotational_scan_matcher_histogram,
+            actual.rotational_scan_matcher_histogram);
+}
+
+}  // namespace
+}  // namespace mapping
+}  // namespace cartographer