From a60b3e2d2de10dcbfc8565599465a0d9ab4c28ee Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Thu, 19 Jul 2018 12:41:14 +0200 Subject: [PATCH] [GenericPoseGraph] Add CeresOptimizer. (#1300) --- cartographer/pose_graph/node/node.h | 3 - .../pose_graph/optimizer/ceres_optimizer.cc | 62 ++++++++++++ .../pose_graph/optimizer/ceres_optimizer.h | 41 ++++++++ .../optimizer/ceres_optimizer_test.cc | 96 +++++++++++++++++++ cartographer/pose_graph/optimizer/optimizer.h | 45 +++++++++ cartographer/pose_graph/pose_graph_data.h | 2 +- 6 files changed, 245 insertions(+), 4 deletions(-) create mode 100644 cartographer/pose_graph/optimizer/ceres_optimizer.cc create mode 100644 cartographer/pose_graph/optimizer/ceres_optimizer.h create mode 100644 cartographer/pose_graph/optimizer/ceres_optimizer_test.cc create mode 100644 cartographer/pose_graph/optimizer/optimizer.h diff --git a/cartographer/pose_graph/node/node.h b/cartographer/pose_graph/node/node.h index 46e5b64..8e29ea6 100644 --- a/cartographer/pose_graph/node/node.h +++ b/cartographer/pose_graph/node/node.h @@ -32,9 +32,6 @@ class Node { ~Node() = default; - Node(const Node&) = delete; - Node& operator=(const Node&) = delete; - proto::Node ToProto() const; const NodeId node_id() const { return node_id_; } diff --git a/cartographer/pose_graph/optimizer/ceres_optimizer.cc b/cartographer/pose_graph/optimizer/ceres_optimizer.cc new file mode 100644 index 0000000..fc3f807 --- /dev/null +++ b/cartographer/pose_graph/optimizer/ceres_optimizer.cc @@ -0,0 +1,62 @@ +/* + * 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/optimizer/ceres_optimizer.h" + +namespace cartographer { +namespace pose_graph { +namespace { + +ceres::Problem::Options CreateCeresProblemOptions() { + ceres::Problem::Options problem_options; + problem_options.cost_function_ownership = + ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; + problem_options.loss_function_ownership = + ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; + return problem_options; +} + +Optimizer::SolverStatus ToSolverStatus( + const ceres::TerminationType& termination_type) { + switch (termination_type) { + case (ceres::TerminationType::CONVERGENCE): + return Optimizer::SolverStatus::CONVERGENCE; + case (ceres::TerminationType::NO_CONVERGENCE): + return Optimizer::SolverStatus::NO_CONVERGENCE; + default: + return Optimizer::SolverStatus::FAILURE; + } +} + +} // namespace + +CeresOptimizer::CeresOptimizer(const ceres::Solver::Options& options) + : problem_options_(CreateCeresProblemOptions()), solver_options_(options) {} + +Optimizer::SolverStatus CeresOptimizer::Solve(PoseGraphData* data) const { + ceres::Problem problem(problem_options_); + + for (const auto& constraint : data->constraints) { + constraint->AddToOptimizer(&data->nodes, &problem); + } + + ceres::Solver::Summary summary; + ceres::Solve(solver_options_, &problem, &summary); + return ToSolverStatus(summary.termination_type); +} + +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/optimizer/ceres_optimizer.h b/cartographer/pose_graph/optimizer/ceres_optimizer.h new file mode 100644 index 0000000..3689ff3 --- /dev/null +++ b/cartographer/pose_graph/optimizer/ceres_optimizer.h @@ -0,0 +1,41 @@ +/* + * 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_OPTIMIZER_CERES_OPTIMIZER_H_ +#define CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_OPTIMIZER_H_ + +#include "cartographer/pose_graph/optimizer/optimizer.h" +#include "ceres/problem.h" +#include "ceres/solver.h" + +namespace cartographer { +namespace pose_graph { + +class CeresOptimizer : public Optimizer { + public: + explicit CeresOptimizer(const ceres::Solver::Options& options); + + SolverStatus Solve(PoseGraphData* data) const final; + + private: + const ceres::Problem::Options problem_options_; + const ceres::Solver::Options solver_options_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_OPTIMIZER_H_ diff --git a/cartographer/pose_graph/optimizer/ceres_optimizer_test.cc b/cartographer/pose_graph/optimizer/ceres_optimizer_test.cc new file mode 100644 index 0000000..4f4c8fa --- /dev/null +++ b/cartographer/pose_graph/optimizer/ceres_optimizer_test.cc @@ -0,0 +1,96 @@ +/* + * 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/optimizer/ceres_optimizer.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/pose_graph/constraint/relative_pose_constraint_2d.h" +#include "google/protobuf/text_format.h" + +#include "gtest/gtest.h" + +namespace cartographer { +namespace pose_graph { +namespace { + +// TODO(pifon): Use the factory function, when the factory is done. +Pose2D GetPose2D(const proto::Node& proto) { + return {NodeId(proto.id()), proto.constant(), + transform::ToEigen(proto.parameters().pose_2d().translation()), + proto.parameters().pose_2d().rotation()}; +} + +template +ProtoType ParseProto(const std::string& proto_string) { + ProtoType proto; + EXPECT_TRUE( + ::google::protobuf::TextFormat::ParseFromString(proto_string, &proto)); + return proto; +} + +constexpr char kStartNode[] = R"PROTO( + id { object_id: "start_node" timestamp: 1 } + constant: false + parameters { + pose_2d { + translation { x: 1 y: 2 } + rotation: 5 + } + } +)PROTO"; + +constexpr char kEndNode[] = R"PROTO( + id { object_id: "end_node" timestamp: 1 } + constant: false + parameters { + pose_2d { + translation { x: 1 y: 2 } + rotation: 5 + } + } +)PROTO"; + +constexpr char kRelativePose2D[] = R"PROTO( + first { object_id: "start_node" timestamp: 1 } + second { object_id: "end_node" timestamp: 1 } + parameters { + first_t_second { + translation { x: 1 y: 1 } + rotation: 0 + } + translation_weight: 1 + rotation_weight: 1 + } +)PROTO"; + +TEST(CeresOptimizerTest, SmokeTest) { + PoseGraphData data; + data.nodes.pose_2d_nodes.emplace( + NodeId{"start_node", common::FromUniversal(1)}, + GetPose2D(ParseProto(kStartNode))); + data.nodes.pose_2d_nodes.emplace( + NodeId{"end_node", common::FromUniversal(1)}, + GetPose2D(ParseProto(kEndNode))); + data.constraints.emplace_back(common::make_unique( + "constraint_1", ParseProto(kRelativePose2D))); + + CeresOptimizer optimizer(ceres::Solver::Options{}); + EXPECT_EQ(optimizer.Solve(&data), Optimizer::SolverStatus::CONVERGENCE); +} + +} // namespace +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/optimizer/optimizer.h b/cartographer/pose_graph/optimizer/optimizer.h new file mode 100644 index 0000000..c6aa062 --- /dev/null +++ b/cartographer/pose_graph/optimizer/optimizer.h @@ -0,0 +1,45 @@ +/* + * 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_OPTIMIZER_OPTIMIZER_H_ +#define CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_OPTIMIZER_H_ + +#include "cartographer/pose_graph/pose_graph_data.h" + +namespace cartographer { +namespace pose_graph { + +class Optimizer { + public: + enum class SolverStatus { + CONVERGENCE, + NO_CONVERGENCE, + FAILURE, + }; + + Optimizer() = default; + virtual ~Optimizer() = default; + + Optimizer(const Optimizer&) = delete; + Optimizer& operator=(const Optimizer&) = delete; + + virtual SolverStatus Solve(PoseGraphData* data) const = 0; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_OPTIMIZER_H_ diff --git a/cartographer/pose_graph/pose_graph_data.h b/cartographer/pose_graph/pose_graph_data.h index 750c2d3..9d3aa64 100644 --- a/cartographer/pose_graph/pose_graph_data.h +++ b/cartographer/pose_graph/pose_graph_data.h @@ -18,7 +18,7 @@ #define CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_DATA_H_ #include "cartographer/pose_graph/constraint/constraint.h" -#include "cartographer/pose_graph/nodes/nodes.h" +#include "cartographer/pose_graph/node/nodes.h" namespace cartographer { namespace pose_graph {