From c4c5fab5b2dfd6f5a6efa15940fc440267284c13 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Thu, 12 Jul 2018 15:21:59 +0200 Subject: [PATCH] [GenericPoseGraph] Add a base class for all nodes and Pose2D node. (#1265) --- cartographer/pose_graph/node.cc | 31 +++++++++ cartographer/pose_graph/node.h | 83 +++++++++++++++++++++++++ cartographer/pose_graph/pose_2d.cc | 48 ++++++++++++++ cartographer/pose_graph/pose_2d.h | 42 +++++++++++++ cartographer/pose_graph/pose_2d_test.cc | 57 +++++++++++++++++ 5 files changed, 261 insertions(+) create mode 100644 cartographer/pose_graph/node.cc create mode 100644 cartographer/pose_graph/node.h create mode 100644 cartographer/pose_graph/pose_2d.cc create mode 100644 cartographer/pose_graph/pose_2d.h create mode 100644 cartographer/pose_graph/pose_2d_test.cc diff --git a/cartographer/pose_graph/node.cc b/cartographer/pose_graph/node.cc new file mode 100644 index 0000000..b52f915 --- /dev/null +++ b/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 diff --git a/cartographer/pose_graph/node.h b/cartographer/pose_graph/node.h new file mode 100644 index 0000000..69b677a --- /dev/null +++ b/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 +#include + +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& parameter_blocks() { return parameter_blocks_; } + + protected: + virtual proto::Parameters ToParametersProto() const = 0; + + template + void AddParameterBlock(Parameterization parameterization, + std::array* values) { + parameter_blocks_.emplace_back( + ParameterBlock{values->data(), values->size(), parameterization}); + } + + private: + NodeId node_id_; + bool constant_; + std::vector parameter_blocks_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_NODE_H_ diff --git a/cartographer/pose_graph/pose_2d.cc b/cartographer/pose_graph/pose_2d.cc new file mode 100644 index 0000000..0702069 --- /dev/null +++ b/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 diff --git a/cartographer/pose_graph/pose_2d.h b/cartographer/pose_graph/pose_2d.h new file mode 100644 index 0000000..5d634b2 --- /dev/null +++ b/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 pose_2d_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_POSE_2D_H_ diff --git a/cartographer/pose_graph/pose_2d_test.cc b/cartographer/pose_graph/pose_2d_test.cc new file mode 100644 index 0000000..1e40166 --- /dev/null +++ b/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::type; + + typename std::remove_cv::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