[GenericPoseGraph] Added relative pose cost 3d. (#1303)

* [GenericPoseGraph] Added relative pose cost 3d.

* Added another case to the test.

* Removed ceres function from cost function.

* Ran clang-format.

* Minor renaming.

cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.cc

@@ -0,0 +1,37 @@
+/*
+ * 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/relative_pose_cost_3d.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+RelativePoseCost3D::RelativePoseCost3D(
+    const proto::RelativePose3D::Parameters& parameters)
+    : translation_weight_(parameters.translation_weight()),
+      rotation_weight_(parameters.rotation_weight()),
+      first_T_second_(transform::ToRigid3(parameters.first_t_second())) {}
+
+proto::RelativePose3D::Parameters RelativePoseCost3D::ToProto() const {
+  proto::RelativePose3D::Parameters parameters;
+  parameters.set_translation_weight(translation_weight_);
+  parameters.set_rotation_weight(rotation_weight_);
+  *parameters.mutable_first_t_second() = transform::ToProto(first_T_second_);
+  return parameters;
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h

@@ -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.
+ */
+
+#ifndef CARTOGRAPHER_RELATIVE_POSE_COST_3D_H
+#define CARTOGRAPHER_RELATIVE_POSE_COST_3D_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 RelativePoseCost3D {
+ public:
+  explicit RelativePoseCost3D(
+      const proto::RelativePose3D::Parameters& parameters);
+
+  proto::RelativePose3D::Parameters ToProto() const;
+
+  template <typename T>
+  bool operator()(const T* const c_i_translation, const T* const c_i_rotation,
+                  const T* const c_j_translation, const T* const c_j_rotation,
+                  T* const error_out) const {
+    const std::array<T, 6> error = mapping::optimization::ScaleError(
+        mapping::optimization::ComputeUnscaledError(
+            first_T_second_, c_i_rotation, c_i_translation, c_j_rotation,
+            c_j_translation),
+        translation_weight_, rotation_weight_);
+    std::copy(std::begin(error), std::end(error), error_out);
+    return true;
+  }
+
+ private:
+  const double translation_weight_;
+  const double rotation_weight_;
+  const transform::Rigid3d first_T_second_;
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_RELATIVE_POSE_COST_3D_H

cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d_test.cc

@@ -0,0 +1,84 @@
+/*
+ * 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/relative_pose_cost_3d.h"
+
+#include "cartographer/pose_graph/internal/testing/test_helpers.h"
+#include "gmock/gmock.h"
+#include "google/protobuf/text_format.h"
+
+namespace cartographer {
+namespace pose_graph {
+namespace {
+
+using ::google::protobuf::TextFormat;
+using ::testing::ElementsAre;
+using testing::EqualsProto;
+
+using PositionType = std::array<double, 3>;
+using RotationType = std::array<double, 4>;
+using ResidualType = std::array<double, 6>;
+
+::testing::Matcher<double> Near(double expected) {
+  constexpr double kPrecision = 1e-05;
+  return ::testing::DoubleNear(expected, kPrecision);
+}
+
+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: 10
+)PROTO";
+
+TEST(RelativePoseCost3DTest, SerializesCorrectly) {
+  proto::RelativePose3D::Parameters relative_pose_parameters;
+  ASSERT_TRUE(
+      TextFormat::ParseFromString(kParameters, &relative_pose_parameters));
+  RelativePoseCost3D relative_pose_cost_3d(relative_pose_parameters);
+  const auto actual_proto = relative_pose_cost_3d.ToProto();
+  EXPECT_THAT(actual_proto, EqualsProto(kParameters));
+}
+
+TEST(RelativePoseCost3DTest, EvaluatesCorrectly) {
+  proto::RelativePose3D::Parameters relative_pose_parameters;
+  ASSERT_TRUE(
+      TextFormat::ParseFromString(kParameters, &relative_pose_parameters));
+  RelativePoseCost3D relative_pose_cost_3d(relative_pose_parameters);
+
+  const PositionType kPosition1{{1., 1., 1.}};
+  const RotationType kRotation1{{1., 1., 1., 1.}};
+  ResidualType residuals;
+  EXPECT_TRUE(relative_pose_cost_3d(kPosition1.data(), kRotation1.data(),
+                                    kPosition1.data(), kRotation1.data(),
+                                    residuals.data()));
+  EXPECT_THAT(residuals, ElementsAre(Near(1), Near(2), Near(3), Near(0),
+                                     Near(19.1037), Near(6.3679)));
+
+  const PositionType kPosition2{{0., -1., -2.}};
+  const RotationType kRotation2{{.1, .2, .3, .4}};
+  EXPECT_TRUE(relative_pose_cost_3d(kPosition1.data(), kRotation1.data(),
+                                    kPosition2.data(), kRotation2.data(),
+                                    residuals.data()));
+  EXPECT_THAT(residuals, ElementsAre(Near(6), Near(8), Near(-2), Near(1.03544),
+                                     Near(11.38984), Near(3.10632)));
+}
+
+}  // namespace
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/proto/cost_function.proto

@@ -35,9 +35,12 @@ message RelativePose3D {
   NodeId first = 1;
   NodeId second = 2;
 
-  transform.proto.Rigid3d first_t_second = 3;
+  message Parameters {
-  double translation_weight = 4;
+    transform.proto.Rigid3d first_t_second = 1;
-  double rotation_weight = 5;
+    double translation_weight = 2;
+    double rotation_weight = 3;
+  }
+  Parameters parameters = 3;
 }
 
 message Acceleration3D {