[GenericPoseGraph] Add CeresOptimizer. (#1300)

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_; }

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

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_

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 <typename ProtoType>
+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<proto::Node>(kStartNode)));
+  data.nodes.pose_2d_nodes.emplace(
+      NodeId{"end_node", common::FromUniversal(1)},
+      GetPose2D(ParseProto<proto::Node>(kEndNode)));
+  data.constraints.emplace_back(common::make_unique<RelativePoseConstraint2D>(
+      "constraint_1", ParseProto<proto::RelativePose2D>(kRelativePose2D)));
+
+  CeresOptimizer optimizer(ceres::Solver::Options{});
+  EXPECT_EQ(optimizer.Solve(&data), Optimizer::SolverStatus::CONVERGENCE);
+}
+
+}  // namespace
+}  // namespace pose_graph
+}  // namespace cartographer

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_

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 {