Adds a 'LandmarkData' struct and proto. (#628)

cartographer/sensor/landmark_data.cc

@@ -0,0 +1,52 @@
+/*
+ * 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/landmark_data.h"
+
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace sensor {
+
+proto::LandmarkData ToProto(const LandmarkData& landmark_data) {
+  proto::LandmarkData proto;
+  proto.set_timestamp(common::ToUniversal(landmark_data.time));
+  for (const Landmark& landmark : landmark_data.landmarks) {
+    auto* item = proto.add_landmarks();
+    item->set_id(landmark.id);
+    *item->mutable_transform() = transform::ToProto(landmark.transform);
+    item->set_translation_weight(landmark.translation_weight);
+    item->set_rotation_weight(landmark.rotation_weight);
+  }
+  return proto;
+}
+
+LandmarkData FromProto(const proto::LandmarkData& proto) {
+  LandmarkData landmark_data;
+  landmark_data.time = common::FromUniversal(proto.timestamp());
+  for (const auto& item : proto.landmarks()) {
+    landmark_data.landmarks.push_back({
+        item.id(),
+        transform::ToRigid3(item.transform()),
+        item.translation_weight(),
+        item.rotation_weight(),
+    });
+  }
+  return landmark_data;
+}
+
+}  // namespace sensor
+}  // namespace cartographer

cartographer/sensor/landmark_data.h

@@ -0,0 +1,53 @@
+/*
+ * 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_LANDMARK_DATA_H_
+#define CARTOGRAPHER_SENSOR_LANDMARK_DATA_H_
+
+#include <string>
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "cartographer/common/port.h"
+#include "cartographer/common/time.h"
+#include "cartographer/sensor/proto/sensor.pb.h"
+#include "cartographer/transform/rigid_transform.h"
+
+namespace cartographer {
+namespace sensor {
+
+struct Landmark {
+  string id;
+  transform::Rigid3d transform;
+  double translation_weight;
+  double rotation_weight;
+};
+
+struct LandmarkData {
+  common::Time time;
+  std::vector<Landmark> landmarks;
+};
+
+// Converts 'landmark_data' to a proto::LandmarkData.
+proto::LandmarkData ToProto(const LandmarkData& landmark_data);
+
+// Converts 'proto' to an LandmarkData.
+LandmarkData FromProto(const proto::LandmarkData& proto);
+
+}  // namespace sensor
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_SENSOR_LANDMARK_DATA_H_

cartographer/sensor/landmark_data_test.cc

@@ -0,0 +1,75 @@
+/*
+ * 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/sensor/landmark_data.h"
+
+#include <cmath>
+
+#include "cartographer/sensor/test_helpers.h"
+#include "cartographer/transform/rigid_transform_test_helpers.h"
+#include "cartographer/transform/transform.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace sensor {
+namespace {
+
+using ::testing::DoubleNear;
+using ::testing::Field;
+
+::testing::Matcher<const Landmark&> EqualsLandmark(const Landmark& expected) {
+  return ::testing::AllOf(Field(&Landmark::id, expected.id),
+                          Field(&Landmark::transform,
+                                transform::IsNearly(expected.transform, 1e-2)),
+                          Field(&Landmark::translation_weight,
+                                DoubleNear(expected.translation_weight, 0.01)),
+                          Field(&Landmark::rotation_weight,
+                                DoubleNear(expected.rotation_weight, 0.01)));
+}
+
+class LandmarkDataTest : public ::testing::Test {
+ protected:
+  LandmarkDataTest()
+      : landmarks_(
+            {{
+                 "ID1",
+                 transform::Rigid3d(Eigen::Vector3d(1., 1., 1.),
+                                    Eigen::Quaterniond(1., 1., -1., -1.)),
+                 1.f,
+                 3.f,
+             },
+             {
+                 "ID2",
+                 transform::Rigid3d(Eigen::Vector3d(2., 2., 2.),
+                                    Eigen::Quaterniond(2., 2., -2., -2.)),
+                 2.f,
+                 4.f,
+             }}) {}
+  std::vector<Landmark> landmarks_;
+};
+
+TEST_F(LandmarkDataTest, LandmarkDataToAndFromProto) {
+  const auto expected = LandmarkData{common::FromUniversal(50), landmarks_};
+  const auto actual = FromProto(ToProto(expected));
+  EXPECT_EQ(expected.time, actual.time);
+  EXPECT_THAT(actual.landmarks,
+              ElementsAre(EqualsLandmark(expected.landmarks[0]),
+                          EqualsLandmark(expected.landmarks[1])));
+}
+
+}  // namespace
+}  // namespace sensor
+}  // namespace cartographer

cartographer/sensor/proto/sensor.proto

@@ -44,3 +44,15 @@ message FixedFramePoseData {
   optional int64 timestamp = 1;
   optional transform.proto.Rigid3d pose = 2;
 }
+
+// Proto representation of ::cartographer::sensor::LandmarkData.
+message LandmarkData {
+  message Landmark {
+    optional bytes id = 1;
+    optional transform.proto.Rigid3d transform = 2;
+    optional double translation_weight = 3;
+    optional double rotation_weight = 4;
+  }
+  optional int64 timestamp = 1;
+  repeated Landmark landmarks = 2;
+}

cartographer/sensor/range_data_test.cc

@@ -19,6 +19,7 @@
 #include <tuple>
 #include <vector>
 
+#include "cartographer/sensor/test_helpers.h"
 #include "gmock/gmock.h"
 
 namespace cartographer {
@@ -27,14 +28,6 @@ namespace {
 
 using ::testing::Contains;
 
-MATCHER(NearPointwise, std::string(negation ? "Doesn't" : "Does") + " match.") {
-  return std::get<0>(arg).isApprox(std::get<1>(arg), 0.001f);
-}
-
-MATCHER_P(Near, point, std::string(negation ? "Doesn't" : "Does") + " match.") {
-  return arg.isApprox(point, 0.001f);
-}
-
 class RangeDataTest : public ::testing::Test {
  protected:
   RangeDataTest() : origin_(Eigen::Vector3f(1, 1, 1)) {

cartographer/sensor/test_helpers.h

@@ -0,0 +1,35 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_SENSOR_TEST_HELPERS_H_
+#define CARTOGRAPHER_SENSOR_TEST_HELPERS_H_
+
+#include <string>
+#include <tuple>
+
+#include "gmock/gmock.h"
+
+namespace cartographer {
+namespace sensor {
+
+MATCHER_P(Near, point, std::string(negation ? "Doesn't" : "Does") + " match.") {
+  return arg.isApprox(point, 0.001f);
+}
+
+}  // namespace sensor
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_SENSOR_TEST_HELPERS_H_