From 31d0a6acc744e06d2b82797ea27d81f72d01df66 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Mon, 16 Jul 2018 11:07:37 +0200 Subject: [PATCH] [GenericPoseGraph] Add IMU Calibration and Pose3D node types. (#1278) --- .../pose_graph/node/imu_calibration.cc | 48 +++++++++++++++++ .../pose_graph/node/imu_calibration.h | 51 ++++++++++++++++++ .../pose_graph/node/imu_calibration_test.cc | 44 ++++++++++++++++ cartographer/pose_graph/{ => node}/node.cc | 2 +- cartographer/pose_graph/{ => node}/node.h | 30 ++--------- cartographer/pose_graph/node/nodes.h | 39 ++++++++++++++ cartographer/pose_graph/{ => node}/pose_2d.cc | 6 +-- cartographer/pose_graph/{ => node}/pose_2d.h | 12 +++-- .../pose_graph/{ => node}/pose_2d_test.cc | 2 +- cartographer/pose_graph/node/pose_3d.cc | 48 +++++++++++++++++ cartographer/pose_graph/node/pose_3d.h | 52 +++++++++++++++++++ cartographer/pose_graph/node/pose_3d_test.cc | 44 ++++++++++++++++ cartographer/pose_graph/proto/node.proto | 2 +- 13 files changed, 342 insertions(+), 38 deletions(-) create mode 100644 cartographer/pose_graph/node/imu_calibration.cc create mode 100644 cartographer/pose_graph/node/imu_calibration.h create mode 100644 cartographer/pose_graph/node/imu_calibration_test.cc rename cartographer/pose_graph/{ => node}/node.cc (95%) rename cartographer/pose_graph/{ => node}/node.h (64%) create mode 100644 cartographer/pose_graph/node/nodes.h rename cartographer/pose_graph/{ => node}/pose_2d.cc (88%) rename cartographer/pose_graph/{ => node}/pose_2d.h (75%) rename cartographer/pose_graph/{ => node}/pose_2d_test.cc (95%) create mode 100644 cartographer/pose_graph/node/pose_3d.cc create mode 100644 cartographer/pose_graph/node/pose_3d.h create mode 100644 cartographer/pose_graph/node/pose_3d_test.cc diff --git a/cartographer/pose_graph/node/imu_calibration.cc b/cartographer/pose_graph/node/imu_calibration.cc new file mode 100644 index 0000000..9f2c32f --- /dev/null +++ b/cartographer/pose_graph/node/imu_calibration.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/node/imu_calibration.h" + +namespace cartographer { +namespace pose_graph { + +ImuCalibration::ImuCalibration(const NodeId& node_id, bool constant, + double gravity_constant, + const Eigen::Quaterniond& orientation) + : Node(node_id, constant), + gravity_constant_(gravity_constant), + orientation_{{orientation.x(), orientation.y(), orientation.z(), + orientation.w()}} {} + +proto::Parameters ImuCalibration::ToParametersProto() const { + proto::Parameters parameters; + auto* imu_calibration = parameters.mutable_imu_calibration(); + + imu_calibration->set_gravity_constant(gravity_constant_); + + // TODO(pifon): Use a common method to convert from Eigen::Quaterniond to + // proto. Probably, the one defined in transform.h. + auto* orientation = imu_calibration->mutable_orientation(); + orientation->set_x(orientation_[0]); + orientation->set_y(orientation_[1]); + orientation->set_z(orientation_[2]); + orientation->set_w(orientation_[3]); + + return parameters; +} + +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/node/imu_calibration.h b/cartographer/pose_graph/node/imu_calibration.h new file mode 100644 index 0000000..7cfa41f --- /dev/null +++ b/cartographer/pose_graph/node/imu_calibration.h @@ -0,0 +1,51 @@ +/* + * 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_IMU_CALIBRATION_H_ +#define CARTOGRAPHER_POSE_GRAPH_NODE_IMU_CALIBRATION_H_ + +#include "cartographer/pose_graph/node/node.h" + +#include + +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace pose_graph { + +class ImuCalibration : public Node { + public: + ImuCalibration(const NodeId& node_id, bool constant, double gravity_constant, + const Eigen::Quaterniond& orientation); + + double* mutable_gravity_constant() { return &gravity_constant_; } + double gravity_constant() const { return gravity_constant_; } + + std::array* mutable_orientation() { return &orientation_; } + const std::array& orientation() const { return orientation_; } + + protected: + proto::Parameters ToParametersProto() const final; + + private: + double gravity_constant_; + std::array orientation_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_NODE_IMU_CALIBRATION_H_ diff --git a/cartographer/pose_graph/node/imu_calibration_test.cc b/cartographer/pose_graph/node/imu_calibration_test.cc new file mode 100644 index 0000000..4e74e8c --- /dev/null +++ b/cartographer/pose_graph/node/imu_calibration_test.cc @@ -0,0 +1,44 @@ +/* + * 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/imu_calibration.h" + +#include "cartographer/pose_graph/internal/testing/test_helpers.h" + +namespace cartographer { +namespace pose_graph { +namespace { + +constexpr char kExpectedNode[] = R"PROTO( + id { object_id: "accelerometer" } + constant: true + parameters { + imu_calibration { + gravity_constant: 10 + orientation: { w: 0 x: 1 y: 2 z: 3 } + } + } +)PROTO"; + +TEST(Pose3DTest, ToProto) { + ImuCalibration imu_calibration("accelerometer", true, 10, + Eigen::Quaterniond(0., 1., 2., 3.)); + EXPECT_THAT(imu_calibration.ToProto(), testing::EqualsProto(kExpectedNode)); +} + +} // namespace +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/node.cc b/cartographer/pose_graph/node/node.cc similarity index 95% rename from cartographer/pose_graph/node.cc rename to cartographer/pose_graph/node/node.cc index b52f915..cb1bc6a 100644 --- a/cartographer/pose_graph/node.cc +++ b/cartographer/pose_graph/node/node.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/pose_graph/node.h" +#include "cartographer/pose_graph/node/node.h" namespace cartographer { namespace pose_graph { diff --git a/cartographer/pose_graph/node.h b/cartographer/pose_graph/node/node.h similarity index 64% rename from cartographer/pose_graph/node.h rename to cartographer/pose_graph/node/node.h index f353229..c5e63d1 100644 --- a/cartographer/pose_graph/node.h +++ b/cartographer/pose_graph/node/node.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_H_ -#define CARTOGRAPHER_POSE_GRAPH_NODE_H_ +#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_NODE_H_ +#define CARTOGRAPHER_POSE_GRAPH_NODE_NODE_H_ #include "cartographer/pose_graph/proto/node.pb.h" @@ -32,20 +32,6 @@ 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; @@ -60,25 +46,15 @@ class Node { 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_ +#endif // CARTOGRAPHER_POSE_GRAPH_NODE_NODE_H_ diff --git a/cartographer/pose_graph/node/nodes.h b/cartographer/pose_graph/node/nodes.h new file mode 100644 index 0000000..b42e934 --- /dev/null +++ b/cartographer/pose_graph/node/nodes.h @@ -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. + */ + +#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_NODES_H_ +#define CARTOGRAPHER_POSE_GRAPH_NODE_NODES_H_ + +#include "cartographer/pose_graph/node/imu_calibration.h" +#include "cartographer/pose_graph/node/pose_2d.h" +#include "cartographer/pose_graph/node/pose_3d.h" + +#include + +namespace cartographer { +namespace pose_graph { + +struct Nodes { + // TODO(pifon): Should it really be an std::map or smth else? + std::map pose_2d_nodes; + std::map pose_3d_nodes; + std::map imu_calibration_nodes; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_NODE_NODES_H_ diff --git a/cartographer/pose_graph/pose_2d.cc b/cartographer/pose_graph/node/pose_2d.cc similarity index 88% rename from cartographer/pose_graph/pose_2d.cc rename to cartographer/pose_graph/node/pose_2d.cc index 0702069..0298b9c 100644 --- a/cartographer/pose_graph/pose_2d.cc +++ b/cartographer/pose_graph/node/pose_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/pose_graph/pose_2d.h" +#include "cartographer/pose_graph/node/pose_2d.h" namespace cartographer { namespace pose_graph { @@ -29,9 +29,7 @@ constexpr size_t kRotationIndex = 2; 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_); -} + pose_2d_{{translation.x(), translation.y(), rotation}} {} proto::Parameters Pose2D::ToParametersProto() const { proto::Parameters parameters; diff --git a/cartographer/pose_graph/pose_2d.h b/cartographer/pose_graph/node/pose_2d.h similarity index 75% rename from cartographer/pose_graph/pose_2d.h rename to cartographer/pose_graph/node/pose_2d.h index 1b89d30..7ce5101 100644 --- a/cartographer/pose_graph/pose_2d.h +++ b/cartographer/pose_graph/node/pose_2d.h @@ -14,12 +14,13 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_POSE_GRAPH_POSE_2D_H_ -#define CARTOGRAPHER_POSE_GRAPH_POSE_2D_H_ +#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_POSE_2D_H_ +#define CARTOGRAPHER_POSE_GRAPH_NODE_POSE_2D_H_ -#include "cartographer/pose_graph/node.h" +#include "cartographer/pose_graph/node/node.h" #include + #include "Eigen/Core" namespace cartographer { @@ -30,6 +31,9 @@ class Pose2D : public Node { Pose2D(const NodeId& node_id, bool constant, const Eigen::Vector2d& translation, double rotation); + std::array* mutable_pose_2d() { return &pose_2d_; } + const std::array& pose_2d() const { return pose_2d_; } + protected: proto::Parameters ToParametersProto() const final; @@ -40,4 +44,4 @@ class Pose2D : public Node { } // namespace pose_graph } // namespace cartographer -#endif // CARTOGRAPHER_POSE_GRAPH_POSE_2D_H_ +#endif // CARTOGRAPHER_POSE_GRAPH_NODE_POSE_2D_H_ diff --git a/cartographer/pose_graph/pose_2d_test.cc b/cartographer/pose_graph/node/pose_2d_test.cc similarity index 95% rename from cartographer/pose_graph/pose_2d_test.cc rename to cartographer/pose_graph/node/pose_2d_test.cc index a8c57cc..0eeaa39 100644 --- a/cartographer/pose_graph/pose_2d_test.cc +++ b/cartographer/pose_graph/node/pose_2d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/pose_graph/pose_2d.h" +#include "cartographer/pose_graph/node/pose_2d.h" #include "cartographer/pose_graph/internal/testing/test_helpers.h" diff --git a/cartographer/pose_graph/node/pose_3d.cc b/cartographer/pose_graph/node/pose_3d.cc new file mode 100644 index 0000000..17be4e6 --- /dev/null +++ b/cartographer/pose_graph/node/pose_3d.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/node/pose_3d.h" + +namespace cartographer { +namespace pose_graph { + +Pose3D::Pose3D(const NodeId& node_id, bool constant, + const Eigen::Vector3d& translation, + const Eigen::Quaterniond& rotation) + : Node(node_id, constant), + translation_{{translation.x(), translation.y(), translation.z()}}, + rotation_{{rotation.x(), rotation.y(), rotation.z(), rotation.w()}} {} + +proto::Parameters Pose3D::ToParametersProto() const { + proto::Parameters parameters; + auto* pose_3d = parameters.mutable_pose_3d(); + + auto* translation = pose_3d->mutable_translation(); + translation->set_x(translation_[0]); + translation->set_y(translation_[1]); + translation->set_z(translation_[2]); + + auto* rotation = pose_3d->mutable_rotation(); + rotation->set_x(rotation_[0]); + rotation->set_y(rotation_[1]); + rotation->set_z(rotation_[2]); + rotation->set_w(rotation_[3]); + + return parameters; +} + +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/node/pose_3d.h b/cartographer/pose_graph/node/pose_3d.h new file mode 100644 index 0000000..a0a1ee1 --- /dev/null +++ b/cartographer/pose_graph/node/pose_3d.h @@ -0,0 +1,52 @@ +/* + * 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_POSE_3D_H_ +#define CARTOGRAPHER_POSE_GRAPH_NODE_POSE_3D_H_ + +#include "cartographer/pose_graph/node/node.h" + +#include + +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace pose_graph { + +class Pose3D : public Node { + public: + Pose3D(const NodeId& node_id, bool constant, + const Eigen::Vector3d& translation, + const Eigen::Quaterniond& rotation); + + std::array* mutable_translation() { return &translation_; } + const std::array& translation() const { return translation_; } + + std::array* mutable_rotation() { return &rotation_; } + const std::array& rotation() const { return rotation_; } + + protected: + proto::Parameters ToParametersProto() const final; + + private: + std::array translation_; + std::array rotation_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_NODE_POSE_3D_H_ diff --git a/cartographer/pose_graph/node/pose_3d_test.cc b/cartographer/pose_graph/node/pose_3d_test.cc new file mode 100644 index 0000000..2377aba --- /dev/null +++ b/cartographer/pose_graph/node/pose_3d_test.cc @@ -0,0 +1,44 @@ +/* + * 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/pose_3d.h" + +#include "cartographer/pose_graph/internal/testing/test_helpers.h" + +namespace cartographer { +namespace pose_graph { +namespace { + +constexpr char kExpectedNode[] = R"PROTO( + id { object_id: "bumpy_world" } + constant: true + parameters { + pose_3d { + translation { x: 1 y: 2 z: 3 } + rotation: { w: 0 x: 1 y: 2 z: 3 } + } + } +)PROTO"; + +TEST(Pose3DTest, ToProto) { + Pose3D pose_3d("bumpy_world", true, Eigen::Vector3d(1., 2., 3.), + Eigen::Quaterniond(0., 1., 2., 3.)); + EXPECT_THAT(pose_3d.ToProto(), testing::EqualsProto(kExpectedNode)); +} + +} // namespace +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/proto/node.proto b/cartographer/pose_graph/proto/node.proto index 47749a6..8e6f368 100644 --- a/cartographer/pose_graph/proto/node.proto +++ b/cartographer/pose_graph/proto/node.proto @@ -38,7 +38,7 @@ message Pose3D { message ImuCalibration { double gravity_constant = 1; - transform.proto.Quaterniond imu_calibration = 2; + transform.proto.Quaterniond orientation = 2; Parameterization rotation_parameterization = 3; }