[GenericPoseGraph] Add a base class for all nodes and Pose2D node. (#1265)

cartographer/pose_graph/node.cc

@@ -0,0 +1,31 @@
+/*
+ * 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/pose_graph/node.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+proto::Node Node::ToProto() const {
+  proto::Node node;
+  node.set_constant(constant_);
+  node.mutable_id()->set_object_id(node_id_);
+  *node.mutable_parameters() = ToParametersProto();
+  return node;
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/node.h

@@ -0,0 +1,83 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_H_
+#define CARTOGRAPHER_POSE_GRAPH_NODE_H_
+
+#include "cartographer/pose_graph/proto/node.pb.h"
+
+#include <string>
+#include <vector>
+
+namespace cartographer {
+namespace pose_graph {
+
+// TODO(pifon): Change it to struct { string, timestamp} and introduce the
+// necessary operators.
+using NodeId = std::string;
+
+class Node {
+ public:
+  enum class Parameterization {
+    NONE,
+    YAW_ONLY,
+    CONSTANT_YAW,
+  };
+
+  struct ParameterBlock {
+    // Non-owning pointer to values corresponding to a single parameter block.
+    double* const values;
+    // Size of the parameter block.
+    const size_t size;
+    const Parameterization parameterization;
+  };
+
+  explicit Node(NodeId id, bool constant) : node_id_(id), constant_(constant) {}
+  ~Node() = default;
+
+  Node(const Node&) = delete;
+  Node& operator=(const Node&) = delete;
+
+  proto::Node ToProto() const;
+
+  const NodeId node_id() const { return node_id_; }
+  void set_node_id(const NodeId& id) { node_id_ = id; }
+
+  bool constant() const { return constant_; }
+  void set_constant(bool constant) { constant_ = constant; }
+
+  std::vector<ParameterBlock>& parameter_blocks() { return parameter_blocks_; }
+
+ protected:
+  virtual proto::Parameters ToParametersProto() const = 0;
+
+  template <std::size_t ArraySize>
+  void AddParameterBlock(Parameterization parameterization,
+                         std::array<double, ArraySize>* values) {
+    parameter_blocks_.emplace_back(
+        ParameterBlock{values->data(), values->size(), parameterization});
+  }
+
+ private:
+  NodeId node_id_;
+  bool constant_;
+  std::vector<ParameterBlock> parameter_blocks_;
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_POSE_GRAPH_NODE_H_

cartographer/pose_graph/pose_2d.cc

@@ -0,0 +1,48 @@
+/*
+ * 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/pose_graph/pose_2d.h"
+
+namespace cartographer {
+namespace pose_graph {
+namespace {
+
+constexpr size_t kXIndex = 0;
+constexpr size_t kYIndex = 1;
+constexpr size_t kRotationIndex = 2;
+
+}  // namespace
+
+Pose2D::Pose2D(const NodeId& node_id, bool constant,
+               const Eigen::Vector2d& translation, double rotation)
+    : Node(node_id, constant),
+      pose_2d_{{translation.x(), translation.y(), rotation}} {
+  AddParameterBlock(Parameterization::NONE, &pose_2d_);
+}
+
+proto::Parameters Pose2D::ToParametersProto() const {
+  proto::Parameters parameters;
+  auto* pose_2d = parameters.mutable_pose_2d();
+  pose_2d->set_rotation(pose_2d_[kRotationIndex]);
+
+  auto* translation = pose_2d->mutable_translation();
+  translation->set_x(pose_2d_[kXIndex]);
+  translation->set_y(pose_2d_[kYIndex]);
+  return parameters;
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/pose_2d.h

@@ -0,0 +1,42 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_POSE_GRAPH_POSE_2D_H_
+#define CARTOGRAPHER_POSE_GRAPH_POSE_2D_H_
+
+#include "cartographer/pose_graph/node.h"
+
+#include "Eigen/Core"
+
+namespace cartographer {
+namespace pose_graph {
+
+class Pose2D : public Node {
+ public:
+  Pose2D(const NodeId& node_id, bool constant,
+         const Eigen::Vector2d& translation, double rotation);
+
+ protected:
+  proto::Parameters ToParametersProto() const final;
+
+ private:
+  std::array<double, 3> pose_2d_;
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_POSE_GRAPH_POSE_2D_H_

cartographer/pose_graph/pose_2d_test.cc

@@ -0,0 +1,57 @@
+/*
+ * 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/pose_graph/pose_2d.h"
+
+#include "gmock/gmock.h"
+#include "google/protobuf/text_format.h"
+#include "google/protobuf/util/message_differencer.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace pose_graph {
+namespace {
+
+constexpr char kExpectedNode[] = R"PROTO(
+  id { object_id: "flat_world" }
+  constant: true
+  parameters {
+    pose_2d {
+      translation { x: 1 y: 2 }
+      rotation: 5
+    }
+  }
+)PROTO";
+
+// TODO(pifon): Move this to a separate header.
+MATCHER_P(EqualsProto, expected_proto_string, "") {
+  using ConstProtoType = typename std::remove_reference<decltype(arg)>::type;
+
+  typename std::remove_cv<ConstProtoType>::type expected_proto;
+  EXPECT_TRUE(google::protobuf::TextFormat::ParseFromString(
+      expected_proto_string, &expected_proto));
+  return google::protobuf::util::MessageDifferencer::Equals(arg,
+                                                            expected_proto);
+}
+
+TEST(Pose2DTest, SerializeToProto) {
+  Pose2D pose_2d("flat_world", true, Eigen::Vector2d(1., 2.), 5.);
+  EXPECT_THAT(pose_2d.ToProto(), EqualsProto(kExpectedNode));
+}
+
+}  // namespace
+}  // namespace pose_graph
+}  // namespace cartographer