[GenericPoseGraph] Added rotation cost 3d. (#1313)

cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.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/constraint/cost_function/rotation_cost_3d.h"
+
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+RotationCost3D::RotationCost3D(const proto::Rotation3D::Parameters& parameters)
+    : scaling_factor_(parameters.scaling_factor()),
+      delta_rotation_imu_frame_(
+          transform::ToEigen(parameters.delta_rotation_imu_frame())) {}
+
+proto::Rotation3D::Parameters RotationCost3D::ToProto() const {
+  proto::Rotation3D::Parameters parameters;
+  parameters.set_scaling_factor(scaling_factor_);
+  *parameters.mutable_delta_rotation_imu_frame() =
+      transform::ToProto(delta_rotation_imu_frame_);
+  return parameters;
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h

@@ -0,0 +1,63 @@
+/*
+ * 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_ROTATION_COST_3D_H
+#define CARTOGRAPHER_ROTATION_COST_3D_H
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "cartographer/pose_graph/proto/cost_function.pb.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+// Provides cost function for 3D rotation and de/serialization methods. The cost
+// function penalizes differences between IMU data and optimized orientations.
+class RotationCost3D {
+ public:
+  explicit RotationCost3D(const proto::Rotation3D::Parameters& parameters);
+
+  proto::Rotation3D::Parameters ToProto() const;
+
+  // Cost function expecting three quaternions as input and 3D vector as output.
+  template <typename T>
+  bool operator()(const T* const start_rotation, const T* const end_rotation,
+                  const T* const imu_calibration, T* residual) const {
+    const Eigen::Quaternion<T> start(start_rotation[0], start_rotation[1],
+                                     start_rotation[2], start_rotation[3]);
+    const Eigen::Quaternion<T> end(end_rotation[0], end_rotation[1],
+                                   end_rotation[2], end_rotation[3]);
+    const Eigen::Quaternion<T> eigen_imu_calibration(
+        imu_calibration[0], imu_calibration[1], imu_calibration[2],
+        imu_calibration[3]);
+    const Eigen::Quaternion<T> error =
+        end.conjugate() * start * eigen_imu_calibration *
+        delta_rotation_imu_frame_.cast<T>() * eigen_imu_calibration.conjugate();
+    residual[0] = scaling_factor_ * error.x();
+    residual[1] = scaling_factor_ * error.y();
+    residual[2] = scaling_factor_ * error.z();
+    return true;
+  }
+
+ private:
+  const double scaling_factor_;
+  const Eigen::Quaterniond delta_rotation_imu_frame_;
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_ROTATION_COST_3D_H

cartographer/pose_graph/constraint/cost_function/rotation_cost_3d_test.cc

@@ -0,0 +1,64 @@
+/*
+ * 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/rotation_cost_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 kParameters[] = R"PROTO(
+  delta_rotation_imu_frame { x: 0 y: 0.1 z: 0.2 w: 0.3 }
+  scaling_factor: 0.4
+)PROTO";
+
+TEST(RotationCost3DTest, SerializesCorrectly) {
+  const auto parameters =
+      ParseProto<proto::Rotation3D::Parameters>(kParameters);
+  RotationCost3D rotation_cost_3d(parameters);
+  const auto actual_proto = rotation_cost_3d.ToProto();
+  EXPECT_THAT(actual_proto, EqualsProto(kParameters));
+}
+
+TEST(RotationCost3DTest, EvaluatesCorrectly) {
+  const auto parameters =
+      ParseProto<proto::Rotation3D::Parameters>(kParameters);
+  RotationCost3D rotation_cost_3d(parameters);
+
+  const RotationType kStartRotation{{1., 1., 1., 1.}};
+  const RotationType kEndRotation{{1.1, 1.2, 1.3, 1.4}};
+  const RotationType kImuCalibration{{0.1, 0.1, 0.1, 0.1}};
+  ResidualType residuals;
+  rotation_cost_3d(kStartRotation.data(), kEndRotation.data(),
+                   kImuCalibration.data(), residuals.data());
+
+  EXPECT_THAT(residuals,
+              ElementsAre(Near(0.01536), Near(-0.00256), Near(0.00832)));
+}
+
+}  // namespace
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/proto/cost_function.proto

@@ -60,8 +60,10 @@ message Rotation3D {
   NodeId second = 2;
   NodeId imu_calibration = 3;
 
+  message Parameters {
     transform.proto.Quaterniond delta_rotation_imu_frame = 4;
     double scaling_factor = 5;
+  }
 }
 
 message CostFunction {