[GenericPoseGraph] Add rotation 3d constraint. (#1317)

cartographer/pose_graph/constraint/rotation_constraint_3d.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/constraint/rotation_constraint_3d.h"
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer/common/utils.h"
+
+namespace cartographer {
+namespace pose_graph {
+namespace {
+
+void AddRotationParameters(Pose3D* pose, ceres::Problem* problem) {
+  auto rotation = pose->mutable_rotation();
+  problem->AddParameterBlock(rotation->data(), rotation->size());
+  if (pose->constant()) {
+    problem->SetParameterBlockConstant(rotation->data());
+  }
+}
+
+}  // namespace
+
+RotationContraint3D::RotationContraint3D(
+    const ConstraintId& id, const proto::LossFunction& loss_function_proto,
+    const proto::Rotation3D& proto)
+    : Constraint(id, loss_function_proto),
+      first_(proto.first()),
+      second_(proto.second()),
+      imu_calibration_(proto.imu_calibration()),
+      cost_(new RotationCost3D(proto.parameters())),
+      ceres_cost_(common::make_unique<AutoDiffFunction>(cost_)) {}
+
+void RotationContraint3D::AddToOptimizer(Nodes* nodes,
+                                         ceres::Problem* problem) const {
+  auto first_node = common::FindOrNull(nodes->pose_3d_nodes, first_);
+  if (first_node == nullptr) {
+    LOG(INFO) << "First node was not found in pose_3d_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->constant() && second_node->constant()) {
+    LOG(INFO) << "Both nodes are constant, skipping the constraint.";
+    return;
+  }
+
+  auto imu_calibration_node =
+      common::FindOrNull(nodes->imu_calibration_nodes, imu_calibration_);
+  if (imu_calibration_node == nullptr) {
+    LOG(INFO) << "Imu calibration node was not found.";
+    return;
+  }
+
+  AddRotationParameters(first_node, problem);
+  AddRotationParameters(second_node, problem);
+  auto imu_orientation = imu_calibration_node->mutable_orientation();
+  problem->AddParameterBlock(imu_orientation->data(), imu_orientation->size());
+  if (imu_calibration_node->constant()) {
+    problem->SetParameterBlockConstant(imu_orientation->data());
+  }
+
+  problem->AddResidualBlock(ceres_cost_.get(), nullptr /* loss function */,
+                            first_node->mutable_rotation()->data(),
+                            second_node->mutable_rotation()->data(),
+                            imu_orientation->data());
+}
+
+proto::CostFunction RotationContraint3D::ToCostFunctionProto() const {
+  proto::CostFunction cost_function;
+  auto* rotation_3d = cost_function.mutable_rotation_3d();
+  *rotation_3d->mutable_first() = first_.ToProto();
+  *rotation_3d->mutable_second() = second_.ToProto();
+  *rotation_3d->mutable_imu_calibration() = imu_calibration_.ToProto();
+  *rotation_3d->mutable_parameters() = cost_->ToProto();
+  return cost_function;
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/constraint/rotation_constraint_3d.h

@@ -0,0 +1,52 @@
+/*
+ * 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_ROTATION_CONSTRAINT_3D_H_
+#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ROTATION_CONSTRAINT_3D_H_
+
+#include "cartographer/pose_graph/constraint/constraint.h"
+#include "cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+class RotationContraint3D : public Constraint {
+ public:
+  RotationContraint3D(const ConstraintId& id,
+                      const proto::LossFunction& loss_function_proto,
+                      const proto::Rotation3D& proto);
+
+  void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final;
+
+ protected:
+  proto::CostFunction ToCostFunctionProto() const final;
+
+ private:
+  using AutoDiffFunction = ceres::AutoDiffCostFunction<
+      RotationCost3D, 3 /* number of residuals */, 4 /* rotation first pose */,
+      4 /* rotation second pose */, 4 /* imu calibration */>;
+  NodeId first_;
+  NodeId second_;
+  NodeId imu_calibration_;
+  // The cost function is owned by the ceres cost function.
+  RotationCost3D* const cost_;
+  std::unique_ptr<AutoDiffFunction> ceres_cost_;
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ROTATION_CONSTRAINT_3D_H_

cartographer/pose_graph/constraint/rotation_constraint_3d_test.cc

@@ -0,0 +1,60 @@
+/*
+ * 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/rotation_constraint_3d.h"
+
+#include "cartographer/pose_graph/internal/testing/test_helpers.h"
+
+namespace cartographer {
+namespace pose_graph {
+namespace {
+
+using ::testing::ElementsAre;
+using testing::EqualsProto;
+using testing::Near;
+using testing::ParseProto;
+
+using RotationType = std::array<double, 4>;
+using ResidualType = std::array<double, 3>;
+
+constexpr char kConstraint[] = R"PROTO(
+  id: "narf"
+  cost_function {
+    rotation_3d {
+      first { object_id: "node0" }
+      second { object_id: "node1" }
+      imu_calibration { object_id: "imu_node" }
+      parameters {
+        delta_rotation_imu_frame { x: 0 y: 0.1 z: 0.2 w: 0.3 }
+        scaling_factor: 0.4
+      }
+    }
+  }
+
+  loss_function { quadratic_loss {} }
+)PROTO";
+
+TEST(RotationConstraint3DTest, SerializesCorrectly) {
+  const auto proto = ParseProto<proto::Constraint>(kConstraint);
+  RotationContraint3D constraint(proto.id(), proto.loss_function(),
+                                 proto.cost_function().rotation_3d());
+  const auto actual_proto = constraint.ToProto();
+  EXPECT_THAT(actual_proto, EqualsProto(kConstraint));
+}
+
+}  // namespace
+}  // namespace pose_graph
+}  // namespace cartographer