[GenericPoseGraph] Add PoseGraphController. (#1335)

cartographer/pose_graph/node/imu_calibration.cc

@@ -20,12 +20,13 @@ namespace cartographer {
 namespace pose_graph {
 
 ImuCalibration::ImuCalibration(const NodeId& node_id, bool constant,
-                               double gravity_constant,
-                               const Eigen::Quaterniond& orientation)
+                               const proto::ImuCalibration& imu_calibration)
     : Node(node_id, constant),
-      gravity_constant_(gravity_constant),
-      orientation_{{orientation.x(), orientation.y(), orientation.z(),
-                    orientation.w()}} {}
+      gravity_constant_(imu_calibration.gravity_constant()),
+      orientation_{{imu_calibration.orientation().x(),
+                    imu_calibration.orientation().y(),
+                    imu_calibration.orientation().z(),
+                    imu_calibration.orientation().w()}} {}
 
 proto::Parameters ImuCalibration::ToParametersProto() const {
   proto::Parameters parameters;

cartographer/pose_graph/node/imu_calibration.h

@@ -28,8 +28,8 @@ namespace pose_graph {
 
 class ImuCalibration : public Node {
  public:
-  ImuCalibration(const NodeId& node_id, bool constant, double gravity_constant,
-                 const Eigen::Quaterniond& orientation);
+  ImuCalibration(const NodeId& node_id, bool constant,
+                 const proto::ImuCalibration& imu_calibration);
 
   double* mutable_gravity_constant() { return &gravity_constant_; }
   double gravity_constant() const { return gravity_constant_; }

cartographer/pose_graph/node/imu_calibration_test.cc

@@ -22,6 +22,8 @@ namespace cartographer {
 namespace pose_graph {
 namespace {
 
+using testing::ParseProto;
+
 constexpr char kExpectedNode[] = R"PROTO(
   id { object_id: "accelerometer" timestamp: 1 }
   constant: true
@@ -35,7 +37,10 @@ constexpr char kExpectedNode[] = R"PROTO(
 
 TEST(Pose3DTest, ToProto) {
   ImuCalibration imu_calibration({"accelerometer", common::FromUniversal(1)},
-                                 true, 10, Eigen::Quaterniond(0., 1., 2., 3.));
+                                 true, ParseProto<proto::ImuCalibration>(R"(
+                                     gravity_constant: 10
+                                     orientation: { w: 0 x: 1 y: 2 z: 3 }
+                                   )"));
   EXPECT_THAT(imu_calibration.ToProto(), testing::EqualsProto(kExpectedNode));
 }
 

cartographer/pose_graph/node/pose_2d.cc

@@ -27,9 +27,10 @@ constexpr size_t kRotationIndex = 2;
 }  // namespace
 
 Pose2D::Pose2D(const NodeId& node_id, bool constant,
-               const Eigen::Vector2d& translation, double rotation)
+               const proto::Pose2D& pose_2d)
     : Node(node_id, constant),
-      pose_2d_{{translation.x(), translation.y(), rotation}} {}
+      pose_2d_{{pose_2d.translation().x(), pose_2d.translation().y(),
+                pose_2d.rotation()}} {}
 
 proto::Parameters Pose2D::ToParametersProto() const {
   proto::Parameters parameters;

cartographer/pose_graph/node/pose_2d.h

@@ -28,8 +28,7 @@ namespace pose_graph {
 
 class Pose2D : public Node {
  public:
-  Pose2D(const NodeId& node_id, bool constant,
-         const Eigen::Vector2d& translation, double rotation);
+  Pose2D(const NodeId& node_id, bool constant, const proto::Pose2D& pose_2d);
 
   std::array<double, 3>* mutable_pose_2d() { return &pose_2d_; }
   const std::array<double, 3>& pose_2d() const { return pose_2d_; }

cartographer/pose_graph/node/pose_2d_test.cc

@@ -22,6 +22,8 @@ namespace cartographer {
 namespace pose_graph {
 namespace {
 
+using testing::ParseProto;
+
 constexpr char kExpectedNode[] = R"PROTO(
   id { object_id: "flat_world" timestamp: 1 }
   constant: true
@@ -35,7 +37,10 @@ constexpr char kExpectedNode[] = R"PROTO(
 
 TEST(Pose2DTest, ToProto) {
   Pose2D pose_2d({"flat_world", common::FromUniversal(1)}, true,
-                 Eigen::Vector2d(1., 2.), 5.);
+                 ParseProto<proto::Pose2D>(R"(
+                   translation { x: 1 y: 2 }
+                   rotation: 5
+                 )"));
   EXPECT_THAT(pose_2d.ToProto(), testing::EqualsProto(kExpectedNode));
 }
 

cartographer/pose_graph/node/pose_3d.cc

@@ -20,11 +20,12 @@ namespace cartographer {
 namespace pose_graph {
 
 Pose3D::Pose3D(const NodeId& node_id, bool constant,
-               const Eigen::Vector3d& translation,
-               const Eigen::Quaterniond& rotation)
+               const proto::Pose3D& pose_3d)
     : Node(node_id, constant),
-      translation_{{translation.x(), translation.y(), translation.z()}},
-      rotation_{{rotation.x(), rotation.y(), rotation.z(), rotation.w()}} {}
+      translation_{{pose_3d.translation().x(), pose_3d.translation().y(),
+                    pose_3d.translation().z()}},
+      rotation_{{pose_3d.rotation().x(), pose_3d.rotation().y(),
+                 pose_3d.rotation().z(), pose_3d.rotation().w()}} {}
 
 proto::Parameters Pose3D::ToParametersProto() const {
   proto::Parameters parameters;

cartographer/pose_graph/node/pose_3d.h

@@ -28,9 +28,7 @@ namespace pose_graph {
 
 class Pose3D : public Node {
  public:
-  Pose3D(const NodeId& node_id, bool constant,
-         const Eigen::Vector3d& translation,
-         const Eigen::Quaterniond& rotation);
+  Pose3D(const NodeId& node_id, bool constant, const proto::Pose3D& pose_3d);
 
   std::array<double, 3>* mutable_translation() { return &translation_; }
   const std::array<double, 3>& translation() const { return translation_; }

cartographer/pose_graph/node/pose_3d_test.cc

@@ -22,6 +22,8 @@ namespace cartographer {
 namespace pose_graph {
 namespace {
 
+using testing::ParseProto;
+
 constexpr char kExpectedNode[] = R"PROTO(
   id { object_id: "bumpy_world" timestamp: 1 }
   constant: true
@@ -35,8 +37,10 @@ constexpr char kExpectedNode[] = R"PROTO(
 
 TEST(Pose3DTest, ToProto) {
   Pose3D pose_3d({"bumpy_world", common::FromUniversal(1)}, true,
-                 Eigen::Vector3d(1., 2., 3.),
-                 Eigen::Quaterniond(0., 1., 2., 3.));
+                 ParseProto<proto::Pose3D>(R"(
+                   translation { x: 1 y: 2 z: 3 }
+                   rotation: { w: 0 x: 1 y: 2 z: 3 }
+                 )"));
   EXPECT_THAT(pose_3d.ToProto(), testing::EqualsProto(kExpectedNode));
 }
 

cartographer/pose_graph/optimizer/ceres_optimizer_test.cc

@@ -26,13 +26,6 @@ namespace {
 
 using testing::ParseProto;
 
-// 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()};
-}
-
 constexpr char kStartNode[] = R"PROTO(
   id { object_id: "start_node" timestamp: 1 }
   constant: false
@@ -56,30 +49,30 @@ constexpr char kEndNode[] = R"PROTO(
 )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
+  id: "constraint_1"
+  loss_function { quadratic_loss: {} }
+  cost_function {
+    relative_pose_2d {
+      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
+      }
     }
-    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(absl::make_unique<RelativePoseConstraint2D>(
-      "constraint_1", ParseProto<proto::LossFunction>(R"(quadratic_loss: {})"),
-      ParseProto<proto::RelativePose2D>(kRelativePose2D)));
-
+  AddNodeToPoseGraphData(ParseProto<proto::Node>(kStartNode), &data);
+  AddNodeToPoseGraphData(ParseProto<proto::Node>(kEndNode), &data);
+  AddConstraintToPoseGraphData(ParseProto<proto::Constraint>(kRelativePose2D),
+                               &data);
   CeresOptimizer optimizer(ceres::Solver::Options{});
   EXPECT_EQ(optimizer.Solve(&data), Optimizer::SolverStatus::CONVERGENCE);
 }

cartographer/pose_graph/pose_graph_controller.cc

@@ -0,0 +1,38 @@
+/*
+ * 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_graph_controller.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+void PoseGraphController::AddNode(const proto::Node& node) {
+  common::MutexLocker locker(&mutex_);
+  AddNodeToPoseGraphData(node, &data_);
+}
+
+void PoseGraphController::AddConstraint(const proto::Constraint& constraint) {
+  common::MutexLocker locker(&mutex_);
+  AddConstraintToPoseGraphData(constraint, &data_);
+}
+
+Optimizer::SolverStatus PoseGraphController::Optimize() {
+  common::MutexLocker locker(&mutex_);
+  return optimizer_->Solve(&data_);
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/pose_graph_controller.h

@@ -0,0 +1,49 @@
+/*
+ * 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_GRAPH_CONTROLLER_H_
+#define CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_
+
+#include "cartographer/common/mutex.h"
+#include "cartographer/pose_graph/optimizer/optimizer.h"
+#include "cartographer/pose_graph/pose_graph_data.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+class PoseGraphController {
+  PoseGraphController(std::unique_ptr<Optimizer> optimizer)
+      : optimizer_(std::move(optimizer)) {}
+
+  PoseGraphController(const PoseGraphController&) = delete;
+  PoseGraphController& operator=(const PoseGraphController&) = delete;
+
+  void AddNode(const proto::Node& node) EXCLUDES(mutex_);
+  void AddConstraint(const proto::Constraint& constraint) EXCLUDES(mutex_);
+
+  Optimizer::SolverStatus Optimize() EXCLUDES(mutex_);
+
+ private:
+  std::unique_ptr<Optimizer> optimizer_;
+
+  mutable common::Mutex mutex_;
+  PoseGraphData data_ GUARDED_BY(mutex_);
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_

cartographer/pose_graph/pose_graph_data.cc

@@ -0,0 +1,97 @@
+/*
+ * 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_graph_data.h"
+
+#include "absl/memory/memory.h"
+#include "cartographer/pose_graph/constraint/acceleration_constraint_3d.h"
+#include "cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h"
+#include "cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h"
+#include "cartographer/pose_graph/constraint/relative_pose_constraint_2d.h"
+#include "cartographer/pose_graph/constraint/relative_pose_constraint_3d.h"
+#include "cartographer/pose_graph/constraint/rotation_constraint_3d.h"
+
+namespace cartographer {
+namespace pose_graph {
+namespace {
+
+std::unique_ptr<Constraint> CreateConstraint(
+    const proto::Constraint& constraint) {
+  const auto& id = constraint.id();
+  const auto& loss = constraint.loss_function();
+  const auto& cost = constraint.cost_function();
+  switch (cost.type_case()) {
+    case (proto::CostFunction::kRelativePose2D):
+      return absl::make_unique<RelativePoseConstraint2D>(
+          id, loss, cost.relative_pose_2d());
+    case (proto::CostFunction::kRelativePose3D):
+      return absl::make_unique<RelativePoseConstraint3D>(
+          id, loss, cost.relative_pose_3d());
+    case (proto::CostFunction::kAcceleration3D):
+      return absl::make_unique<AccelerationConstraint3D>(
+          id, loss, cost.acceleration_3d());
+    case (proto::CostFunction::kRotation3D):
+      return absl::make_unique<RotationContraint3D>(id, loss,
+                                                    cost.rotation_3d());
+    case (proto::CostFunction::kInterpolatedRelativePose2D):
+      return absl::make_unique<InterpolatedRelativePoseConstraint2D>(
+          id, loss, cost.interpolated_relative_pose_2d());
+    case (proto::CostFunction::kInterpolatedRelativePose3D):
+      return absl::make_unique<InterpolatedRelativePoseConstraint3D>(
+          id, loss, cost.interpolated_relative_pose_3d());
+    case (proto::CostFunction::TYPE_NOT_SET):
+      LOG(FATAL) << "Constraint cost function type is not set.";
+  }
+  return nullptr;
+}
+
+}  // namespace
+
+void AddNodeToPoseGraphData(const proto::Node& node, PoseGraphData* data) {
+  NodeId node_id(node.id());
+  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()));
+      return;
+    }
+    case (proto::Parameters::kPose3D): {
+      data->nodes.pose_3d_nodes.emplace(
+          node_id,
+          Pose3D(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()));
+      return;
+    }
+    case (proto::Parameters::TYPE_NOT_SET): {
+      LOG(FATAL) << "Node parameter type is not set.";
+      return;
+    }
+  }
+}
+
+void AddConstraintToPoseGraphData(const proto::Constraint& constraint,
+                                  PoseGraphData* data) {
+  data->constraints.emplace_back(CreateConstraint(constraint));
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/pose_graph_data.h

@@ -28,6 +28,10 @@ struct PoseGraphData {
   std::vector<std::unique_ptr<Constraint>> constraints;
 };
 
+void AddNodeToPoseGraphData(const proto::Node& node, PoseGraphData* data);
+void AddConstraintToPoseGraphData(const proto::Constraint& constraint,
+                                  PoseGraphData* data);
+
 }  // namespace pose_graph
 }  // namespace cartographer
 

cartographer/pose_graph/proto/cost_function.proto

@@ -103,7 +103,7 @@ message InterpolatedRelativePose3D {
 }
 
 message CostFunction {
-  oneof Type {
+  oneof type {
     RelativePose2D relative_pose_2d = 1;
     RelativePose3D relative_pose_3d = 2;
     Acceleration3D acceleration_3d = 3;

cartographer/pose_graph/proto/node.proto

@@ -48,7 +48,7 @@ message NodeId {
 }
 
 message Parameters {
-  oneof Type {
+  oneof type {
     Pose2D pose_2d = 1;
     Pose3D pose_3d = 2;
     ImuCalibration imu_calibration = 3;