[GenericPoseGraph] Add InterpolatedRelativePose2D constraint. (#1337)

cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.cc

@@ -0,0 +1,48 @@
+/*
+ * 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/cost_function/interpolated_relative_pose_cost_2d.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+InterpolatedRelativePoseCost2D::InterpolatedRelativePoseCost2D(
+    const proto::InterpolatedRelativePose2D::Parameters& parameters)
+    : translation_weight_(parameters.translation_weight()),
+      rotation_weight_(parameters.rotation_weight()),
+      interpolation_factor_(parameters.interpolation_factor()),
+      first_T_second_(transform::ToRigid3(parameters.first_t_second())),
+      gravity_alignment_first_start_(
+          transform::ToEigen(parameters.gravity_alignment_first_start())),
+      gravity_alignment_first_end_(
+          transform::ToEigen(parameters.gravity_alignment_first_end())) {}
+
+proto::InterpolatedRelativePose2D::Parameters
+InterpolatedRelativePoseCost2D::ToProto() const {
+  proto::InterpolatedRelativePose2D::Parameters parameters;
+  parameters.set_translation_weight(translation_weight_);
+  parameters.set_rotation_weight(rotation_weight_);
+  parameters.set_interpolation_factor(interpolation_factor_);
+  *parameters.mutable_first_t_second() = transform::ToProto(first_T_second_);
+  *parameters.mutable_gravity_alignment_first_start() =
+      transform::ToProto(gravity_alignment_first_start_);
+  *parameters.mutable_gravity_alignment_first_end() =
+      transform::ToProto(gravity_alignment_first_end_);
+  return parameters;
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h

@@ -0,0 +1,66 @@
+/*
+ * 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_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_2D_H
+#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_2D_H
+
+#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
+#include "cartographer/pose_graph/proto/cost_function.pb.h"
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+// Provides cost function for relative pose and de/serialization methods.
+class InterpolatedRelativePoseCost2D {
+ public:
+  explicit InterpolatedRelativePoseCost2D(
+      const proto::InterpolatedRelativePose2D::Parameters& parameters);
+
+  proto::InterpolatedRelativePose2D::Parameters ToProto() const;
+
+  template <typename T>
+  bool operator()(const T* const first_start_pose,
+                  const T* const first_end_pose,
+                  const T* const second_translation,
+                  const T* const second_rotation, T* const e) const {
+    const std::tuple<std::array<T, 4>, std::array<T, 3>>
+        interpolated_first_pose = mapping::optimization::InterpolateNodes2D(
+            first_start_pose, gravity_alignment_first_start_, first_end_pose,
+            gravity_alignment_first_end_, interpolation_factor_);
+    const std::array<T, 6> error = mapping::optimization::ScaleError(
+        mapping::optimization::ComputeUnscaledError(
+            first_T_second_, std::get<0>(interpolated_first_pose).data(),
+            std::get<1>(interpolated_first_pose).data(), second_rotation,
+            second_translation),
+        translation_weight_, rotation_weight_);
+    std::copy(std::begin(error), std::end(error), e);
+    return true;
+  }
+
+ private:
+  const double translation_weight_;
+  const double rotation_weight_;
+  const double interpolation_factor_;
+  const transform::Rigid3d first_T_second_;
+  const Eigen::Quaterniond gravity_alignment_first_start_;
+  const Eigen::Quaterniond gravity_alignment_first_end_;
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_2D_H

cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d_test.cc

@@ -0,0 +1,76 @@
+/*
+ * 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/cost_function/interpolated_relative_pose_cost_2d.h"
+
+#include "cartographer/testing/test_helpers.h"
+
+namespace cartographer {
+namespace pose_graph {
+namespace {
+
+using ::testing::ElementsAre;
+using testing::EqualsProto;
+using testing::Near;
+using testing::ParseProto;
+
+using Position2DType = std::array<double, 3>;
+using TranslationType = std::array<double, 3>;
+using RotationType = std::array<double, 4>;
+using ResidualType = std::array<double, 6>;
+
+constexpr char kParameters[] = R"PROTO(
+  first_t_second {
+    translation: { x: 1 y: 2 z: 3 }
+    rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 }
+  }
+  translation_weight: 1
+  rotation_weight: 2
+  gravity_alignment_first_start { x: 0.4 y: 0.1 z: 0.3 w: 0.2 }
+  gravity_alignment_first_end { x: 0.3 y: 0.4 z: 0.2 w: 0.1 }
+  interpolation_factor: 0.3
+)PROTO";
+
+TEST(InterpolatedRelativePoseCost2DTest, SerializesCorrectly) {
+  const auto parameters =
+      ParseProto<proto::InterpolatedRelativePose2D::Parameters>(kParameters);
+  InterpolatedRelativePoseCost2D interpolated_relative_pose_cost_2d(parameters);
+  const auto actual_proto = interpolated_relative_pose_cost_2d.ToProto();
+  EXPECT_THAT(actual_proto, EqualsProto(kParameters));
+}
+
+TEST(InterpolatedRelativePoseCost2DTest, EvaluatesCorrectly) {
+  const auto parameters =
+      ParseProto<proto::InterpolatedRelativePose2D::Parameters>(kParameters);
+  InterpolatedRelativePoseCost2D interpolated_relative_pose_cost_2d(parameters);
+
+  const Position2DType kFirstStartPose{{1., 1., 1.}};
+  const Position2DType kFirstEndPose{{2., 3., 4.}};
+  const TranslationType kSecondTranslation{{0., -1., -2.}};
+  const RotationType kSecondRotation{{.4, .2, .3, .1}};
+
+  ResidualType residuals;
+  EXPECT_TRUE(interpolated_relative_pose_cost_2d(
+      kFirstStartPose.data(), kFirstEndPose.data(), kSecondTranslation.data(),
+      kSecondRotation.data(), residuals.data()));
+  EXPECT_THAT(residuals,
+              ElementsAre(Near(4.49044), Near(1.8527), Near(3.49511),
+                          Near(-1.93746), Near(3.54854), Near(4.50243)));
+}
+
+}  // namespace
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc

@@ -0,0 +1,108 @@
+/*
+ * 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/interpolated_relative_pose_constraint_2d.h"
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer/common/utils.h"
+
+namespace cartographer {
+namespace pose_graph {
+namespace {
+
+void AddPose3DParameters(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());
+  if (pose->constant()) {
+    problem->SetParameterBlockConstant(transation->data());
+    problem->SetParameterBlockConstant(rotation->data());
+  }
+}
+
+void AddPose2DParameters(Pose2D* pose, ceres::Problem* problem) {
+  auto pose_2d = pose->mutable_pose_2d();
+  problem->AddParameterBlock(pose_2d->data(), pose_2d->size());
+  if (pose->constant()) {
+    problem->SetParameterBlockConstant(pose_2d->data());
+  }
+}
+
+}  // namespace
+
+InterpolatedRelativePoseConstraint2D::InterpolatedRelativePoseConstraint2D(
+    const ConstraintId& id, const proto::LossFunction& loss_function_proto,
+    const proto::InterpolatedRelativePose2D& proto)
+    : Constraint(id, loss_function_proto),
+      first_start_(proto.first_start()),
+      first_end_(proto.first_end()),
+      second_(proto.second()),
+      cost_(new InterpolatedRelativePoseCost2D(proto.parameters())),
+      ceres_cost_(common::make_unique<AutoDiffFunction>(cost_)) {}
+
+void InterpolatedRelativePoseConstraint2D::AddToOptimizer(
+    Nodes* nodes, ceres::Problem* problem) const {
+  auto first_node_start =
+      common::FindOrNull(nodes->pose_2d_nodes, first_start_);
+  if (first_node_start == nullptr) {
+    LOG(INFO) << "First node (start) was not found in pose_2d_nodes.";
+    return;
+  }
+
+  auto first_node_end = common::FindOrNull(nodes->pose_2d_nodes, first_end_);
+  if (first_node_end == nullptr) {
+    LOG(INFO) << "First node (end) was not found in pose_2d_nodes.";
+    return;
+  }
+
+  auto second_node = common::FindOrNull(nodes->pose_3d_nodes, second_);
+  if (second_node == nullptr) {
+    LOG(INFO) << "Second node was not found in pose_3d_nodes.";
+    return;
+  }
+
+  if (first_node_start->constant() && first_node_end->constant() &&
+      second_node->constant()) {
+    LOG(INFO) << "All nodes are constant, skipping the constraint.";
+    return;
+  }
+
+  AddPose2DParameters(first_node_start, problem);
+  AddPose2DParameters(first_node_end, problem);
+  AddPose3DParameters(second_node, problem);
+  problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
+                            first_node_start->mutable_pose_2d()->data(),
+                            first_node_end->mutable_pose_2d()->data(),
+                            second_node->mutable_translation()->data(),
+                            second_node->mutable_rotation()->data());
+}
+
+proto::CostFunction InterpolatedRelativePoseConstraint2D::ToCostFunctionProto()
+    const {
+  proto::CostFunction cost_function;
+  auto* interpolated_relative_pose_2d =
+      cost_function.mutable_interpolated_relative_pose_2d();
+  *interpolated_relative_pose_2d->mutable_first_start() =
+      first_start_.ToProto();
+  *interpolated_relative_pose_2d->mutable_first_end() = first_end_.ToProto();
+  *interpolated_relative_pose_2d->mutable_second() = second_.ToProto();
+  *interpolated_relative_pose_2d->mutable_parameters() = cost_->ToProto();
+  return cost_function;
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h

@@ -0,0 +1,59 @@
+/*
+ * 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_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_2D_H_
+#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_2D_H_
+
+#include "cartographer/pose_graph/constraint/constraint.h"
+#include "cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+class InterpolatedRelativePoseConstraint2D : public Constraint {
+ public:
+  InterpolatedRelativePoseConstraint2D(
+      const ConstraintId& id, const proto::LossFunction& loss_function_proto,
+      const proto::InterpolatedRelativePose2D& proto);
+
+  void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final;
+
+ protected:
+  proto::CostFunction ToCostFunctionProto() const final;
+
+ private:
+  // clang-format off
+  using AutoDiffFunction = ceres::AutoDiffCostFunction<
+      InterpolatedRelativePoseCost2D,
+      6 /* residuals */,
+      3 /* 2d pose variables of first start pose */,
+      3 /* 2d pose variables of first end pose */,
+      3 /* translation of second pose */,
+      4 /* rotation of second pose */>;
+  // clang-format on
+  NodeId first_start_;
+  NodeId first_end_;
+  NodeId second_;
+
+  // The cost function is owned by the ceres cost function.
+  InterpolatedRelativePoseCost2D* const cost_;
+  std::unique_ptr<AutoDiffFunction> ceres_cost_;
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_2D_H_

cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d_test.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/constraint/interpolated_relative_pose_constraint_2d.h"
+
+#include "cartographer/testing/test_helpers.h"
+
+namespace cartographer {
+namespace pose_graph {
+namespace {
+
+using testing::EqualsProto;
+using testing::ParseProto;
+
+constexpr char kConstraint[] = R"PROTO(
+  id: "narf"
+  cost_function {
+    interpolated_relative_pose_2d {
+      first_start { object_id: "node0_start" }
+      first_end { object_id: "node0_end" }
+      second { object_id: "node1" }
+      parameters {
+        first_t_second {
+          translation: { x: 1 y: 2 z: 3 }
+          rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 }
+        }
+        translation_weight: 0.5
+        rotation_weight: 1.0
+        gravity_alignment_first_start { x: 0.4 y: 0.2 z: 0.3 w: 0.2 }
+        gravity_alignment_first_end { x: 0.3 y: 0.4 z: 0.2 w: 0.1 }
+        interpolation_factor: 0.3
+      }
+    }
+  }
+  loss_function { quadratic_loss {} }
+)PROTO";
+
+TEST(InterpolatedRelativePostConstraint2DTest, SerializesCorrectly) {
+  const auto proto = ParseProto<proto::Constraint>(kConstraint);
+  InterpolatedRelativePoseConstraint2D constraint(
+      proto.id(), proto.loss_function(),
+      proto.cost_function().interpolated_relative_pose_2d());
+  const auto actual_proto = constraint.ToProto();
+  EXPECT_THAT(actual_proto, EqualsProto(kConstraint));
+}
+
+}  // namespace
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/proto/cost_function.proto

@@ -70,22 +70,21 @@ message Rotation3D {
   Parameters parameters = 4;
 }
 
-message InterpolatedNode {
-  NodeId start = 1;
-  NodeId end = 2;
-  double factor = 3;
-}
-
-message RelativePose2DInterpolated {
-  InterpolatedNode first = 1;
-  NodeId second = 2;
+message InterpolatedRelativePose2D {
+  NodeId first_start = 1;
+  NodeId first_end = 2;
+  NodeId second = 3;
 
   message Parameters {
     transform.proto.Rigid3d first_t_second = 1;
     double translation_weight = 2;
     double rotation_weight = 3;
+    transform.proto.Quaterniond gravity_alignment_first_start = 4;
+    transform.proto.Quaterniond gravity_alignment_first_end = 5;
+     // Interpolates between first_start and first_end.
+    double interpolation_factor = 6;
   }
-  Parameters parameters = 3;
+  Parameters parameters = 4;
 }
 
 message InterpolatedRelativePose3D {
@@ -109,7 +108,7 @@ message CostFunction {
     RelativePose3D relative_pose_3d = 2;
     Acceleration3D acceleration_3d = 3;
     Rotation3D rotation_3d = 4;
-    RelativePose2DInterpolated relative_pose_2d_interpolated = 5;
+    InterpolatedRelativePose2D interpolated_relative_pose_2d = 5;
     InterpolatedRelativePose3D interpolated_relative_pose_3d = 6;
   }
 }