[GenericPoseGraph] Add acceleration constraint. (#1319)

cartographer/pose_graph/constraint/acceleration_constraint_3d.cc

@@ -0,0 +1,118 @@
+/*
+ * 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/acceleration_constraint_3d.h"
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer/common/utils.h"
+
+namespace cartographer {
+namespace pose_graph {
+namespace {
+
+void AddPoseParameters(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 AddImuParameters(ImuCalibration* pose, ceres::Problem* problem) {
+  auto gravity = pose->mutable_gravity_constant();
+  auto orientation = pose->mutable_orientation();
+  problem->AddParameterBlock(gravity, 1);
+  problem->AddParameterBlock(orientation->data(), orientation->size());
+  if (pose->constant()) {
+    problem->SetParameterBlockConstant(gravity);
+    problem->SetParameterBlockConstant(orientation->data());
+  }
+}
+
+}  // namespace
+
+AccelerationConstraint3D::AccelerationConstraint3D(
+    const ConstraintId& id, const proto::LossFunction& loss_function_proto,
+    const proto::Acceleration3D& proto)
+    : Constraint(id, loss_function_proto),
+      first_(proto.first()),
+      second_(proto.second()),
+      third_(proto.third()),
+      imu_(proto.imu_calibration()),
+      cost_(new AccelerationCost3D(proto.parameters())),
+      ceres_cost_(common::make_unique<AutoDiffFunction>(cost_)) {}
+
+void AccelerationConstraint3D::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;
+  }
+
+  auto third_node = common::FindOrNull(nodes->pose_3d_nodes, third_);
+  if (third_node == nullptr) {
+    LOG(INFO) << "Third node was not found in pose_3d_nodes.";
+    return;
+  }
+
+  auto imu_node = common::FindOrNull(nodes->imu_calibration_nodes, imu_);
+  if (imu_node == nullptr) {
+    LOG(INFO) << "IMU calibration node was not found in imu_calibration_nodes.";
+    return;
+  }
+
+  if (first_node->constant() && second_node->constant() &&
+      third_node->constant() && imu_node->constant()) {
+    LOG(INFO) << "All nodes are constant, skipping the constraint.";
+    return;
+  }
+
+  AddPoseParameters(first_node, problem);
+  AddPoseParameters(second_node, problem);
+  AddPoseParameters(third_node, problem);
+  AddImuParameters(imu_node, problem);
+  problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
+                            second_node->mutable_rotation()->data(),
+                            second_node->mutable_translation()->data(),
+                            first_node->mutable_translation()->data(),
+                            third_node->mutable_translation()->data(),
+                            imu_node->mutable_gravity_constant(),
+                            imu_node->mutable_orientation()->data());
+}
+
+proto::CostFunction AccelerationConstraint3D::ToCostFunctionProto() const {
+  proto::CostFunction cost_function;
+  auto* acceleration_3d = cost_function.mutable_acceleration_3d();
+  *acceleration_3d->mutable_first() = first_.ToProto();
+  *acceleration_3d->mutable_second() = second_.ToProto();
+  *acceleration_3d->mutable_third() = third_.ToProto();
+  *acceleration_3d->mutable_imu_calibration() = imu_.ToProto();
+  *acceleration_3d->mutable_parameters() = cost_->ToProto();
+  return cost_function;
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/constraint/acceleration_constraint_3d.h

@@ -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.
+ */
+
+#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ACCELERATION_CONSTRAINT_3D_H_
+#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ACCELERATION_CONSTRAINT_3D_H_
+
+#include "cartographer/pose_graph/constraint/constraint.h"
+#include "cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+class AccelerationConstraint3D : public Constraint {
+ public:
+  AccelerationConstraint3D(const ConstraintId& id,
+                           const proto::LossFunction& loss_function_proto,
+                           const proto::Acceleration3D& proto);
+
+  void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final;
+
+ protected:
+  proto::CostFunction ToCostFunctionProto() const final;
+
+ private:
+  // clang-format off
+  using AutoDiffFunction =
+      ceres::AutoDiffCostFunction<AccelerationCost3D,
+                                  3 /* residuals */,
+                                  4 /* rotation variables second pose*/,
+                                  3 /* position variables second pose*/,
+                                  3 /* position variables first pose */,
+                                  3 /* position variables second pose */,
+                                  1 /* gravity variable */,
+                                  4 /* imu orientation */>;
+  // clang-format on
+  NodeId first_;
+  NodeId second_;
+  NodeId third_;
+  NodeId imu_;
+  // The cost function is owned by the ceres cost function.
+  AccelerationCost3D* const cost_;
+  std::unique_ptr<AutoDiffFunction> ceres_cost_;
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_3D_H_

cartographer/pose_graph/constraint/acceleration_constraint_3d_test.cc

@@ -0,0 +1,57 @@
+/*
+ * 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/acceleration_constraint_3d.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: "hal_acceleration"
+  cost_function {
+    acceleration_3d {
+      first { object_id: "hal9000" timestamp: 100 }
+      second { object_id: "hal9000" timestamp: 200 }
+      third { object_id: "hal9000" timestamp: 300 }
+      imu_calibration { object_id: "hal_imu" }
+      parameters {
+        delta_velocity_imu_frame { x: 1 y: 1 z: 1 }
+        first_to_second_delta_time_seconds: 10.0
+        second_to_third_delta_time_seconds: 20.0
+        scaling_factor: 2.0
+      }
+    }
+  }
+  loss_function { quadratic_loss {} }
+)PROTO";
+
+TEST(AccelerationConstraint3DTest, SerializesCorrectly) {
+  const auto proto = ParseProto<proto::Constraint>(kConstraint);
+  AccelerationConstraint3D constraint(proto.id(), proto.loss_function(),
+                                      proto.cost_function().acceleration_3d());
+  const auto actual_proto = constraint.ToProto();
+  EXPECT_THAT(actual_proto, EqualsProto(kConstraint));
+}
+
+}  // namespace
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.cc

@@ -0,0 +1,47 @@
+/*
+ * 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/acceleration_cost_3d.h"
+
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+AccelerationCost3D::AccelerationCost3D(
+    const proto::Acceleration3D::Parameters& parameters)
+    : delta_velocity_imu_frame_(
+          transform::ToEigen(parameters.delta_velocity_imu_frame())),
+      first_to_second_delta_time_seconds_(
+          parameters.first_to_second_delta_time_seconds()),
+      second_to_third_delta_time_seconds_(
+          parameters.second_to_third_delta_time_seconds()),
+      scaling_factor_(parameters.scaling_factor()) {}
+
+proto::Acceleration3D::Parameters AccelerationCost3D::ToProto() const {
+  proto::Acceleration3D::Parameters parameters;
+  *parameters.mutable_delta_velocity_imu_frame() =
+      transform::ToProto(delta_velocity_imu_frame_);
+  parameters.set_first_to_second_delta_time_seconds(
+      first_to_second_delta_time_seconds_);
+  parameters.set_second_to_third_delta_time_seconds(
+      second_to_third_delta_time_seconds_);
+  parameters.set_scaling_factor(scaling_factor_);
+  return parameters;
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h

@@ -0,0 +1,75 @@
+/*
+ * 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_ACCELERATION_COST_3D_H_
+#define CARTOGRAPHER_ACCELERATION_COST_3D_H_
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "cartographer/pose_graph/proto/cost_function.pb.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+// Penalizes differences between IMU data and optimized accelerations.
+class AccelerationCost3D {
+ public:
+  explicit AccelerationCost3D(
+      const proto::Acceleration3D::Parameters& parameters);
+
+  proto::Acceleration3D::Parameters ToProto() const;
+
+  template <typename T>
+  bool operator()(const T* const middle_rotation, const T* const start_position,
+                  const T* const middle_position, const T* const end_position,
+                  const T* const gravity_constant,
+                  const T* const imu_calibration, T* residual) const {
+    using Vector3T = Eigen::Matrix<T, 3, 1>;
+    using TranslationMap = Eigen::Map<const Vector3T>;
+    using RotationMap = Eigen::Map<const Eigen::Quaternion<T>>;
+
+    const Vector3T imu_delta_velocity =
+        RotationMap(middle_rotation) * RotationMap(imu_calibration) *
+            delta_velocity_imu_frame_.cast<T>() -
+        (*gravity_constant * 0.5 *
+         (first_to_second_delta_time_seconds_ +
+          second_to_third_delta_time_seconds_)) *
+            Eigen::Vector3d::UnitZ().cast<T>();
+
+    const Vector3T start_velocity =
+        (TranslationMap(middle_position) - TranslationMap(start_position)) /
+        T(first_to_second_delta_time_seconds_);
+    const Vector3T end_velocity =
+        (TranslationMap(end_position) - TranslationMap(middle_position)) /
+        T(second_to_third_delta_time_seconds_);
+    const Vector3T delta_velocity = end_velocity - start_velocity;
+
+    (Eigen::Map<Vector3T>(residual) =
+         T(scaling_factor_) * (imu_delta_velocity - delta_velocity));
+    return true;
+  }
+
+ private:
+  const Eigen::Vector3d delta_velocity_imu_frame_;
+  const double first_to_second_delta_time_seconds_;
+  const double second_to_third_delta_time_seconds_;
+  const double scaling_factor_;
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_ACCELERATION_COST_3D_H_

cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d_test.cc

@@ -0,0 +1,71 @@
+/*
+ * 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/acceleration_cost_3d.h"
+
+#include "cartographer/testing/test_helpers.h"
+
+namespace cartographer {
+namespace pose_graph {
+namespace {
+
+using ::google::protobuf::TextFormat;
+using ::testing::ElementsAre;
+using testing::EqualsProto;
+using testing::Near;
+using testing::ParseProto;
+
+using PositionType = std::array<double, 3>;
+using RotationType = std::array<double, 4>;
+using ResidualType = std::array<double, 3>;
+
+constexpr char kParameters[] = R"PROTO(
+  delta_velocity_imu_frame { x: 1 y: 1 z: 1 }
+  first_to_second_delta_time_seconds: 10.0
+  second_to_third_delta_time_seconds: 20.0
+  scaling_factor: 2.0
+)PROTO";
+
+TEST(AccelerationCost3DTest, SerializesCorrectly) {
+  AccelerationCost3D acceleration_cost_3d(
+      ParseProto<proto::Acceleration3D::Parameters>(kParameters));
+  EXPECT_THAT(acceleration_cost_3d.ToProto(), EqualsProto(kParameters));
+}
+
+TEST(AccelerationCost3DTest, EvaluatesCorrectly) {
+  AccelerationCost3D acceleration_cost_3d(
+      ParseProto<proto::Acceleration3D::Parameters>(kParameters));
+  const PositionType kStartPosition{{1., 1., 1.}};
+
+  const PositionType kMiddlePosition{{2., 2., 2.}};
+  const RotationType kMiddleRotation{{0., 0., 0., 1.}};
+
+  const PositionType kEndPosition{{3., 3., 3.}};
+
+  const double kGravityConstant{10.};
+  const RotationType kImuCalibration{{0., 0., 0., 1.0}};
+
+  ResidualType residuals;
+  EXPECT_TRUE(acceleration_cost_3d(
+      kMiddleRotation.data(), kStartPosition.data(), kMiddlePosition.data(),
+      kEndPosition.data(), &kGravityConstant, kImuCalibration.data(),
+      residuals.data()));
+  EXPECT_THAT(residuals, ElementsAre(Near(2.1), Near(2.1), Near(-297.9)));
+}
+
+}  // namespace
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/proto/cost_function.proto

@@ -51,8 +51,8 @@ message Acceleration3D {
 
   message Parameters {
     transform.proto.Vector3d delta_velocity_imu_frame = 1;
-    int64 first_to_second_time_delta = 2;
+    double first_to_second_delta_time_seconds = 2;
-    int64 second_to_third_time_delta = 3;
+    double second_to_third_delta_time_seconds = 3;
     double scaling_factor = 4;
   }
   Parameters parameters = 5;