[GenericPoseGraph] Add parameterization. (#1385)

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_

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) {
+void AddImuCalibration(ImuCalibration* imu_calibration,
-  auto gravity = pose->mutable_gravity_constant();
+                       ceres::Problem* problem) {
-  auto orientation = pose->mutable_orientation();
+  auto gravity = imu_calibration->mutable_gravity_constant();
+  auto orientation = imu_calibration->mutable_orientation();
+
   problem->AddParameterBlock(gravity, 1);
-  problem->AddParameterBlock(orientation->data(), orientation->size());
+  problem->AddParameterBlock(orientation->data(), orientation->size(),
-  if (pose->constant()) {
+                             imu_calibration->orientation_parameterization());
+  if (imu_calibration->constant()) {
     problem->SetParameterBlockConstant(gravity);
     problem->SetParameterBlockConstant(orientation->data());
   }

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 <typename MapType,
+          typename UniquePtrType = typename MapType::mapped_type,
+          typename ValueTyper = typename UniquePtrType::element_type>
+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;                                                       \

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()),
+      gravity_constant_(imu.gravity_constant()),
-      orientation_{{imu_calibration.orientation().x(),
+      orientation_{{imu.orientation().x(), imu.orientation().y(),
-                    imu_calibration.orientation().y(),
+                    imu.orientation().z(), imu.orientation().w()}},
-                    imu_calibration.orientation().z(),
+      orientation_parameterization_(imu.orientation_parameterization()) {}
-                    imu_calibration.orientation().w()}} {}
 
 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;
 }

cartographer/pose_graph/node/imu_calibration.h

@@ -21,6 +21,7 @@
 
 #include <array>
 
+#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<double, 4>* mutable_orientation() { return &orientation_; }
   const std::array<double, 4>& 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<double, 4> orientation_;
+  Parameterization orientation_parameterization_;
 };
 
 }  // namespace pose_graph

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<proto::ImuCalibration>(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));
 }

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?
+  // TODO(pifon): Migrate to Swiss Tables when they are out.
-  std::map<NodeId, Pose2D> pose_2d_nodes;
+  std::map<NodeId, std::unique_ptr<Pose2D>> pose_2d_nodes;
-  std::map<NodeId, Pose3D> pose_3d_nodes;
+  std::map<NodeId, std::unique_ptr<Pose3D>> pose_3d_nodes;
-  std::map<NodeId, ImuCalibration> imu_calibration_nodes;
+  std::map<NodeId, std::unique_ptr<ImuCalibration>> imu_calibration_nodes;
 };
 
 }  // namespace pose_graph

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<LocalParameterization> CeresParameterizationFromProto(
+    const proto::Parameterization& parameterization) {
+  switch (parameterization) {
+    case (proto::Parameterization::NONE):
+      return nullptr;
+    case (proto::Parameterization::YAW_ONLY):
+      return make_unique<AutoDiffLocalParameterization<
+          mapping::YawOnlyQuaternionPlus, 4, 1>>();
+    case (proto::Parameterization::YAW_CONSTANT):
+      return make_unique<AutoDiffLocalParameterization<
+          mapping::ConstantYawQuaternionPlus, 4, 1>>();
+    case (proto::Parameterization::FIX_Z):
+      return make_unique<ceres::SubsetParameterization>(
+          3, /* constant parameters */ std::vector<int>{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

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::LocalParameterization> ceres_parameterization_;
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_

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

cartographer/pose_graph/node/pose_3d.h

@@ -21,6 +21,7 @@
 
 #include <array>
 
+#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<double, 3>* mutable_translation() { return &translation_; }
   const std::array<double, 3>& translation() const { return translation_; }
+  ceres::LocalParameterization* translation_parameterization() const {
+    return translation_parameterization_.ceres_parameterization();
+  }
 
   std::array<double, 4>* mutable_rotation() { return &rotation_; }
   const std::array<double, 4>& rotation() const { return rotation_; }
+  ceres::LocalParameterization* rotation_parameterization() const {
+    return rotation_parameterization_.ceres_parameterization();
+  }
 
  protected:
   proto::Parameters ToParametersProto() const final;
 
  private:
   std::array<double, 3> translation_;
+  Parameterization translation_parameterization_;
+
   std::array<double, 4> rotation_;
+  Parameterization rotation_parameterization_;
 };
 
 }  // namespace pose_graph

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<proto::Pose3D>(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));
 }

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

cartographer/pose_graph/pose_graph_data.cc

@@ -28,6 +28,8 @@ namespace cartographer {
 namespace pose_graph {
 namespace {
 
+using absl::make_unique;
+
 std::unique_ptr<Constraint> CreateConstraint(
     const proto::Constraint& constraint) {
   const auto& id = constraint.id();
@@ -35,22 +37,21 @@ std::unique_ptr<Constraint> CreateConstraint(
   const auto& cost = constraint.cost_function();
   switch (cost.type_case()) {
     case (proto::CostFunction::kRelativePose2D):
-      return absl::make_unique<RelativePoseConstraint2D>(
+      return make_unique<RelativePoseConstraint2D>(id, loss,
-          id, loss, cost.relative_pose_2d());
+                                                   cost.relative_pose_2d());
     case (proto::CostFunction::kRelativePose3D):
-      return absl::make_unique<RelativePoseConstraint3D>(
+      return make_unique<RelativePoseConstraint3D>(id, loss,
-          id, loss, cost.relative_pose_3d());
+                                                   cost.relative_pose_3d());
     case (proto::CostFunction::kAcceleration3D):
-      return absl::make_unique<AccelerationConstraint3D>(
+      return make_unique<AccelerationConstraint3D>(id, loss,
-          id, loss, cost.acceleration_3d());
+                                                   cost.acceleration_3d());
     case (proto::CostFunction::kRotation3D):
-      return absl::make_unique<RotationContraint3D>(id, loss,
+      return make_unique<RotationContraint3D>(id, loss, cost.rotation_3d());
-                                                    cost.rotation_3d());
     case (proto::CostFunction::kInterpolatedRelativePose2D):
-      return absl::make_unique<InterpolatedRelativePoseConstraint2D>(
+      return make_unique<InterpolatedRelativePoseConstraint2D>(
           id, loss, cost.interpolated_relative_pose_2d());
     case (proto::CostFunction::kInterpolatedRelativePose3D):
-      return absl::make_unique<InterpolatedRelativePoseConstraint3D>(
+      return 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.";
@@ -65,19 +66,21 @@ 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,
+          node_id, make_unique<Pose2D>(node_id, node.constant(),
-          Pose2D(node_id, node.constant(), node.parameters().pose_2d()));
+                                       node.parameters().pose_2d()));
       return;
     }
     case (proto::Parameters::kPose3D): {
       data->nodes.pose_3d_nodes.emplace(
-          node_id,
+          node_id, make_unique<Pose3D>(node_id, node.constant(),
-          Pose3D(node_id, node.constant(), node.parameters().pose_3d()));
+                                       node.parameters().pose_3d()));
+
       return;
     }
     case (proto::Parameters::kImuCalibration): {
       data->nodes.imu_calibration_nodes.emplace(
-          node_id, ImuCalibration(node_id, node.constant(),
+          node_id,
+          make_unique<ImuCalibration>(node_id, node.constant(),
                                       node.parameters().imu_calibration()));
       return;
     }

cartographer/pose_graph/proto/node.proto

@@ -20,8 +20,9 @@ import "cartographer/transform/proto/transform.proto";
 
 enum Parameterization {
   NONE = 0;
-  YAW_ONLY = 1;
+  FIX_Z = 1;
-  YAW_CONSTANT = 2;
+  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 {