Add serialization for TimestampedTransform. (#1495)

cartographer/transform/proto/timestamped_transform.proto

@@ -0,0 +1,25 @@
+// Copyright 2018 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 = "proto3";
+
+package cartographer.transform.proto;
+
+import "cartographer/transform/proto/transform.proto";
+
+// NEXT ID: 3
+message TimestampedTransform {
+  int64 time = 1;
+  transform.proto.Rigid3d transform = 2;
+}

cartographer/transform/timestamped_transform.cc

@@ -16,6 +16,8 @@
 
 #include "cartographer/transform/timestamped_transform.h"
 
+#include "cartographer/transform/transform.h"
+
 namespace cartographer {
 namespace transform {
 
@@ -36,5 +38,17 @@ TimestampedTransform Interpolate(const TimestampedTransform& start,
   return TimestampedTransform{time, transform::Rigid3d(origin, rotation)};
 }
 
+TimestampedTransform FromProto(const proto::TimestampedTransform& proto) {
+  return TimestampedTransform{common::FromUniversal(proto.time()),
+                              ToRigid3(proto.transform())};
+}
+
+proto::TimestampedTransform ToProto(const TimestampedTransform& transform) {
+  proto::TimestampedTransform proto;
+  proto.set_time(common::ToUniversal(transform.time));
+  *proto.mutable_transform() = ToProto(transform.transform);
+  return proto;
+}
+
 }  // namespace transform
 }  // namespace cartographer

cartographer/transform/timestamped_transform.h

@@ -18,6 +18,7 @@
 #define CARTOGRAPHER_TRANSFORM_TIMESTAMPED_TRANSFORM_H_
 
 #include "cartographer/common/time.h"
+#include "cartographer/transform/proto/timestamped_transform.pb.h"
 #include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
@@ -28,6 +29,9 @@ struct TimestampedTransform {
   transform::Rigid3d transform;
 };
 
+TimestampedTransform FromProto(const proto::TimestampedTransform& proto);
+proto::TimestampedTransform ToProto(const TimestampedTransform& transform);
+
 TimestampedTransform Interpolate(const TimestampedTransform& start,
                                  const TimestampedTransform& end,
                                  const common::Time time);

cartographer/transform/timestamped_transform_test.cc

@@ -0,0 +1,39 @@
+/*
+ * Copyright 2018 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/transform/timestamped_transform.h"
+
+#include "cartographer/transform/rigid_transform_test_helpers.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace transform {
+namespace {
+
+TEST(TimestampedTransformTest, ToProtoAndBack) {
+  const TimestampedTransform expected{
+      common::FromUniversal(12345678),
+      Rigid3d(Eigen::Vector3d(1., 2., 3.),
+              Eigen::Quaterniond(1., 2., 3., 4.).normalized())};
+  const TimestampedTransform actual = FromProto(ToProto(expected));
+  EXPECT_EQ(expected.time, actual.time);
+  EXPECT_THAT(actual.transform, IsNearly(expected.transform, 1e-6));
+}
+
+}  // namespace
+}  // namespace transform
+}  // namespace cartographer