From 5261c90c3421fd6e7eab570e13f99a46dd6d8c07 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Tue, 14 Aug 2018 14:37:27 +0200 Subject: [PATCH] [GenericPoseGraph] Add parameterization. (#1385) --- cartographer/common/utils.h | 5 ++ .../pose_graph/constraint/constraint_utils.cc | 36 ++++++++--- .../pose_graph/constraint/constraint_utils.h | 11 +++- .../pose_graph/node/imu_calibration.cc | 13 ++-- .../pose_graph/node/imu_calibration.h | 6 ++ .../pose_graph/node/imu_calibration_test.cc | 2 + cartographer/pose_graph/node/nodes.h | 8 +-- .../node/parameterization/parameterization.cc | 61 +++++++++++++++++++ .../node/parameterization/parameterization.h | 44 +++++++++++++ cartographer/pose_graph/node/pose_3d.cc | 7 ++- cartographer/pose_graph/node/pose_3d.h | 10 +++ cartographer/pose_graph/node/pose_3d_test.cc | 3 + .../pose_graph/optimizer/ceres_optimizer.cc | 2 + cartographer/pose_graph/pose_graph_data.cc | 35 ++++++----- cartographer/pose_graph/proto/node.proto | 7 ++- 15 files changed, 212 insertions(+), 38 deletions(-) create mode 100644 cartographer/pose_graph/node/parameterization/parameterization.cc create mode 100644 cartographer/pose_graph/node/parameterization/parameterization.h diff --git a/cartographer/common/utils.h b/cartographer/common/utils.h index 47fce47..229ad69 100644 --- a/cartographer/common/utils.h +++ b/cartographer/common/utils.h @@ -14,6 +14,9 @@ * limitations under the License. */ +#ifndef CARTOGRAPHER_COMMON_UTILS_H_ +#define CARTOGRAPHER_COMMON_UTILS_H_ + namespace cartographer { namespace common { @@ -27,3 +30,5 @@ ValueType* FindOrNull(MapType& map, const KeyType& key) { } // namespace common } // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_UTILS_H_ diff --git a/cartographer/pose_graph/constraint/constraint_utils.cc b/cartographer/pose_graph/constraint/constraint_utils.cc index 216f81b..89e5dff 100644 --- a/cartographer/pose_graph/constraint/constraint_utils.cc +++ b/cartographer/pose_graph/constraint/constraint_utils.cc @@ -1,3 +1,19 @@ +/* + * 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/constraint/constraint_utils.h" namespace cartographer { @@ -14,20 +30,26 @@ void AddPose2D(Pose2D* pose, ceres::Problem* problem) { void AddPose3D(Pose3D* pose, ceres::Problem* problem) { auto transation = pose->mutable_translation(); auto rotation = pose->mutable_rotation(); - problem->AddParameterBlock(transation->data(), transation->size()); - problem->AddParameterBlock(rotation->data(), rotation->size()); + + problem->AddParameterBlock(transation->data(), transation->size(), + pose->translation_parameterization()); + problem->AddParameterBlock(rotation->data(), rotation->size(), + pose->rotation_parameterization()); if (pose->constant()) { problem->SetParameterBlockConstant(transation->data()); problem->SetParameterBlockConstant(rotation->data()); } } -void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem) { - auto gravity = pose->mutable_gravity_constant(); - auto orientation = pose->mutable_orientation(); +void AddImuCalibration(ImuCalibration* imu_calibration, + ceres::Problem* problem) { + auto gravity = imu_calibration->mutable_gravity_constant(); + auto orientation = imu_calibration->mutable_orientation(); + problem->AddParameterBlock(gravity, 1); - problem->AddParameterBlock(orientation->data(), orientation->size()); - if (pose->constant()) { + problem->AddParameterBlock(orientation->data(), orientation->size(), + imu_calibration->orientation_parameterization()); + if (imu_calibration->constant()) { problem->SetParameterBlockConstant(gravity); problem->SetParameterBlockConstant(orientation->data()); } diff --git a/cartographer/pose_graph/constraint/constraint_utils.h b/cartographer/pose_graph/constraint/constraint_utils.h index b46f4df..67cbaed 100644 --- a/cartographer/pose_graph/constraint/constraint_utils.h +++ b/cartographer/pose_graph/constraint/constraint_utils.h @@ -17,6 +17,7 @@ #ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_ #define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_ +#include "cartographer/common/utils.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" @@ -31,8 +32,16 @@ void AddPose3D(Pose3D* pose, ceres::Problem* problem); void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem); +template +ValueTyper* FindNodeOrNull(MapType& map, const NodeId& node_id) { + auto node = common::FindOrNull(map, node_id); + return node == nullptr ? nullptr : node->get(); +} + #define FIND_NODE_OR_RETURN(node_name, node_id, map, log_message) \ - auto node_name = common::FindOrNull(map, node_id); \ + auto node_name = FindNodeOrNull(map, node_id); \ if (node_name == nullptr) { \ LOG(INFO) << log_message; \ return; \ diff --git a/cartographer/pose_graph/node/imu_calibration.cc b/cartographer/pose_graph/node/imu_calibration.cc index a8793ec..3389c7d 100644 --- a/cartographer/pose_graph/node/imu_calibration.cc +++ b/cartographer/pose_graph/node/imu_calibration.cc @@ -20,13 +20,12 @@ namespace cartographer { namespace pose_graph { ImuCalibration::ImuCalibration(const NodeId& node_id, bool constant, - const proto::ImuCalibration& imu_calibration) + const proto::ImuCalibration& imu) : Node(node_id, constant), - gravity_constant_(imu_calibration.gravity_constant()), - orientation_{{imu_calibration.orientation().x(), - imu_calibration.orientation().y(), - imu_calibration.orientation().z(), - imu_calibration.orientation().w()}} {} + gravity_constant_(imu.gravity_constant()), + orientation_{{imu.orientation().x(), imu.orientation().y(), + imu.orientation().z(), imu.orientation().w()}}, + orientation_parameterization_(imu.orientation_parameterization()) {} proto::Parameters ImuCalibration::ToParametersProto() const { proto::Parameters parameters; @@ -41,6 +40,8 @@ proto::Parameters ImuCalibration::ToParametersProto() const { orientation->set_y(orientation_[1]); orientation->set_z(orientation_[2]); orientation->set_w(orientation_[3]); + imu_calibration->set_orientation_parameterization( + orientation_parameterization_.ToProto()); return parameters; } diff --git a/cartographer/pose_graph/node/imu_calibration.h b/cartographer/pose_graph/node/imu_calibration.h index 6fd062d..6febd5b 100644 --- a/cartographer/pose_graph/node/imu_calibration.h +++ b/cartographer/pose_graph/node/imu_calibration.h @@ -21,6 +21,7 @@ #include +#include "cartographer/pose_graph/node/parameterization/parameterization.h" #include "cartographer/transform/transform.h" namespace cartographer { @@ -37,12 +38,17 @@ class ImuCalibration : public Node { std::array* mutable_orientation() { return &orientation_; } const std::array& orientation() const { return orientation_; } + ceres::LocalParameterization* orientation_parameterization() const { + return orientation_parameterization_.ceres_parameterization(); + } + protected: proto::Parameters ToParametersProto() const final; private: double gravity_constant_; std::array orientation_; + Parameterization orientation_parameterization_; }; } // namespace pose_graph diff --git a/cartographer/pose_graph/node/imu_calibration_test.cc b/cartographer/pose_graph/node/imu_calibration_test.cc index fbbb3dc..1894043 100644 --- a/cartographer/pose_graph/node/imu_calibration_test.cc +++ b/cartographer/pose_graph/node/imu_calibration_test.cc @@ -31,6 +31,7 @@ constexpr char kExpectedNode[] = R"PROTO( imu_calibration { gravity_constant: 10 orientation: { w: 0 x: 1 y: 2 z: 3 } + orientation_parameterization: YAW_ONLY } } )PROTO"; @@ -40,6 +41,7 @@ TEST(Pose3DTest, ToProto) { true, ParseProto(R"( gravity_constant: 10 orientation: { w: 0 x: 1 y: 2 z: 3 } + orientation_parameterization: YAW_ONLY )")); EXPECT_THAT(imu_calibration.ToProto(), testing::EqualsProto(kExpectedNode)); } diff --git a/cartographer/pose_graph/node/nodes.h b/cartographer/pose_graph/node/nodes.h index f5036d0..21f9860 100644 --- a/cartographer/pose_graph/node/nodes.h +++ b/cartographer/pose_graph/node/nodes.h @@ -27,10 +27,10 @@ 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; + // TODO(pifon): Migrate to Swiss Tables when they are out. + std::map> pose_2d_nodes; + std::map> pose_3d_nodes; + std::map> imu_calibration_nodes; }; } // namespace pose_graph diff --git a/cartographer/pose_graph/node/parameterization/parameterization.cc b/cartographer/pose_graph/node/parameterization/parameterization.cc new file mode 100644 index 0000000..44a5be4 --- /dev/null +++ b/cartographer/pose_graph/node/parameterization/parameterization.cc @@ -0,0 +1,61 @@ +/* + * 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/parameterization/parameterization.h" + +#include "absl/memory/memory.h" +#include "cartographer/common/math.h" +#include "cartographer/mapping/internal/3d/rotation_parameterization.h" +#include "ceres/autodiff_local_parameterization.h" +#include "ceres/rotation.h" + +namespace cartographer { +namespace pose_graph { +namespace { + +using absl::make_unique; +using ceres::AutoDiffLocalParameterization; +using ceres::LocalParameterization; + +// TODO(pifon): Check if the functors are compatible with our quaternions. Test! +std::unique_ptr CeresParameterizationFromProto( + const proto::Parameterization& parameterization) { + switch (parameterization) { + case (proto::Parameterization::NONE): + return nullptr; + case (proto::Parameterization::YAW_ONLY): + return make_unique>(); + case (proto::Parameterization::YAW_CONSTANT): + return make_unique>(); + case (proto::Parameterization::FIX_Z): + return make_unique( + 3, /* constant parameters */ std::vector{2}); + default: + LOG(FATAL) << "Parameterization is not known."; + } + return nullptr; +} + +} // namespace + +Parameterization::Parameterization(const proto::Parameterization& proto) + : proto_(proto), + ceres_parameterization_(CeresParameterizationFromProto(proto_)) {} + +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/node/parameterization/parameterization.h b/cartographer/pose_graph/node/parameterization/parameterization.h new file mode 100644 index 0000000..eab1e58 --- /dev/null +++ b/cartographer/pose_graph/node/parameterization/parameterization.h @@ -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. + */ + +#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_ +#define CARTOGRAPHER_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_ + +#include "cartographer/pose_graph/proto/node.pb.h" +#include "ceres/local_parameterization.h" + +namespace cartographer { +namespace pose_graph { + +class Parameterization { + public: + explicit Parameterization(const proto::Parameterization& proto); + + const proto::Parameterization& ToProto() const { return proto_; } + + ceres::LocalParameterization* ceres_parameterization() const { + return ceres_parameterization_.get(); + } + + private: + const proto::Parameterization proto_; + const std::unique_ptr ceres_parameterization_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_ diff --git a/cartographer/pose_graph/node/pose_3d.cc b/cartographer/pose_graph/node/pose_3d.cc index 9c30621..d743e72 100644 --- a/cartographer/pose_graph/node/pose_3d.cc +++ b/cartographer/pose_graph/node/pose_3d.cc @@ -24,8 +24,10 @@ Pose3D::Pose3D(const NodeId& node_id, bool constant, : Node(node_id, constant), translation_{{pose_3d.translation().x(), pose_3d.translation().y(), pose_3d.translation().z()}}, + translation_parameterization_(pose_3d.translation_parameterization()), rotation_{{pose_3d.rotation().x(), pose_3d.rotation().y(), - pose_3d.rotation().z(), pose_3d.rotation().w()}} {} + pose_3d.rotation().z(), pose_3d.rotation().w()}}, + rotation_parameterization_(pose_3d.rotation_parameterization()) {} proto::Parameters Pose3D::ToParametersProto() const { proto::Parameters parameters; @@ -35,12 +37,15 @@ proto::Parameters Pose3D::ToParametersProto() const { translation->set_x(translation_[0]); translation->set_y(translation_[1]); translation->set_z(translation_[2]); + pose_3d->set_translation_parameterization( + translation_parameterization_.ToProto()); 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]); + pose_3d->set_rotation_parameterization(rotation_parameterization_.ToProto()); return parameters; } diff --git a/cartographer/pose_graph/node/pose_3d.h b/cartographer/pose_graph/node/pose_3d.h index 32487a7..1b09781 100644 --- a/cartographer/pose_graph/node/pose_3d.h +++ b/cartographer/pose_graph/node/pose_3d.h @@ -21,6 +21,7 @@ #include +#include "cartographer/pose_graph/node/parameterization/parameterization.h" #include "cartographer/transform/transform.h" namespace cartographer { @@ -32,16 +33,25 @@ class Pose3D : public Node { std::array* mutable_translation() { return &translation_; } const std::array& translation() const { return translation_; } + ceres::LocalParameterization* translation_parameterization() const { + return translation_parameterization_.ceres_parameterization(); + } std::array* mutable_rotation() { return &rotation_; } const std::array& rotation() const { return rotation_; } + ceres::LocalParameterization* rotation_parameterization() const { + return rotation_parameterization_.ceres_parameterization(); + } protected: proto::Parameters ToParametersProto() const final; private: std::array translation_; + Parameterization translation_parameterization_; + std::array rotation_; + Parameterization rotation_parameterization_; }; } // namespace pose_graph diff --git a/cartographer/pose_graph/node/pose_3d_test.cc b/cartographer/pose_graph/node/pose_3d_test.cc index 7f62474..25b1059 100644 --- a/cartographer/pose_graph/node/pose_3d_test.cc +++ b/cartographer/pose_graph/node/pose_3d_test.cc @@ -30,6 +30,7 @@ constexpr char kExpectedNode[] = R"PROTO( parameters { pose_3d { translation { x: 1 y: 2 z: 3 } + translation_parameterization: FIX_Z rotation: { w: 0 x: 1 y: 2 z: 3 } } } @@ -39,7 +40,9 @@ TEST(Pose3DTest, ToProto) { Pose3D pose_3d({"bumpy_world", common::FromUniversal(1)}, true, ParseProto(R"( translation { x: 1 y: 2 z: 3 } + translation_parameterization: FIX_Z rotation: { w: 0 x: 1 y: 2 z: 3 } + rotation_parameterization: NONE )")); EXPECT_THAT(pose_3d.ToProto(), testing::EqualsProto(kExpectedNode)); } diff --git a/cartographer/pose_graph/optimizer/ceres_optimizer.cc b/cartographer/pose_graph/optimizer/ceres_optimizer.cc index fc3f807..d08347d 100644 --- a/cartographer/pose_graph/optimizer/ceres_optimizer.cc +++ b/cartographer/pose_graph/optimizer/ceres_optimizer.cc @@ -26,6 +26,8 @@ ceres::Problem::Options CreateCeresProblemOptions() { ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; problem_options.loss_function_ownership = ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; + problem_options.local_parameterization_ownership = + ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; return problem_options; } diff --git a/cartographer/pose_graph/pose_graph_data.cc b/cartographer/pose_graph/pose_graph_data.cc index b450a72..87686c9 100644 --- a/cartographer/pose_graph/pose_graph_data.cc +++ b/cartographer/pose_graph/pose_graph_data.cc @@ -28,6 +28,8 @@ namespace cartographer { namespace pose_graph { namespace { +using absl::make_unique; + std::unique_ptr CreateConstraint( const proto::Constraint& constraint) { const auto& id = constraint.id(); @@ -35,22 +37,21 @@ std::unique_ptr CreateConstraint( const auto& cost = constraint.cost_function(); switch (cost.type_case()) { case (proto::CostFunction::kRelativePose2D): - return absl::make_unique( - id, loss, cost.relative_pose_2d()); + return make_unique(id, loss, + cost.relative_pose_2d()); case (proto::CostFunction::kRelativePose3D): - return absl::make_unique( - id, loss, cost.relative_pose_3d()); + return make_unique(id, loss, + cost.relative_pose_3d()); case (proto::CostFunction::kAcceleration3D): - return absl::make_unique( - id, loss, cost.acceleration_3d()); + return make_unique(id, loss, + cost.acceleration_3d()); case (proto::CostFunction::kRotation3D): - return absl::make_unique(id, loss, - cost.rotation_3d()); + return make_unique(id, loss, cost.rotation_3d()); case (proto::CostFunction::kInterpolatedRelativePose2D): - return absl::make_unique( + return make_unique( id, loss, cost.interpolated_relative_pose_2d()); case (proto::CostFunction::kInterpolatedRelativePose3D): - return absl::make_unique( + return make_unique( id, loss, cost.interpolated_relative_pose_3d()); case (proto::CostFunction::TYPE_NOT_SET): LOG(FATAL) << "Constraint cost function type is not set."; @@ -65,20 +66,22 @@ void AddNodeToPoseGraphData(const proto::Node& node, PoseGraphData* data) { switch (node.parameters().type_case()) { case (proto::Parameters::kPose2D): { data->nodes.pose_2d_nodes.emplace( - node_id, - Pose2D(node_id, node.constant(), node.parameters().pose_2d())); + node_id, make_unique(node_id, node.constant(), + node.parameters().pose_2d())); return; } case (proto::Parameters::kPose3D): { data->nodes.pose_3d_nodes.emplace( - node_id, - Pose3D(node_id, node.constant(), node.parameters().pose_3d())); + node_id, make_unique(node_id, node.constant(), + node.parameters().pose_3d())); + return; } case (proto::Parameters::kImuCalibration): { data->nodes.imu_calibration_nodes.emplace( - node_id, ImuCalibration(node_id, node.constant(), - node.parameters().imu_calibration())); + node_id, + make_unique(node_id, node.constant(), + node.parameters().imu_calibration())); return; } case (proto::Parameters::TYPE_NOT_SET): { diff --git a/cartographer/pose_graph/proto/node.proto b/cartographer/pose_graph/proto/node.proto index e48e066..8b73b29 100644 --- a/cartographer/pose_graph/proto/node.proto +++ b/cartographer/pose_graph/proto/node.proto @@ -20,8 +20,9 @@ import "cartographer/transform/proto/transform.proto"; enum Parameterization { NONE = 0; - YAW_ONLY = 1; - YAW_CONSTANT = 2; + FIX_Z = 1; + YAW_ONLY = 2; + YAW_CONSTANT = 3; } message Pose2D { @@ -39,7 +40,7 @@ message Pose3D { message ImuCalibration { double gravity_constant = 1; transform.proto.Quaterniond orientation = 2; - Parameterization rotation_parameterization = 3; + Parameterization orientation_parameterization = 3; } message NodeId {