From b72bbb20c3fd45f972889ef2ad7dd49734027b7f Mon Sep 17 00:00:00 2001
From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com>
Date: Fri, 9 Feb 2018 13:13:17 +0100
Subject: [PATCH] Fix the size of residuals, add test for jacobians. (#895)

[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)
---
 .../pose_graph/landmark_cost_function.h       |  2 +-
 .../pose_graph/landmark_cost_function_test.cc | 20 ++++++++------
 .../pose_graph/landmark_cost_function_test.cc | 27 ++++++++++---------
 3 files changed, 28 insertions(+), 21 deletions(-)

diff --git a/cartographer/mapping_2d/pose_graph/landmark_cost_function.h b/cartographer/mapping_2d/pose_graph/landmark_cost_function.h
index a746cd7..238015c 100644
--- a/cartographer/mapping_2d/pose_graph/landmark_cost_function.h
+++ b/cartographer/mapping_2d/pose_graph/landmark_cost_function.h
@@ -43,7 +43,7 @@ class LandmarkCostFunction {
       const LandmarkObservation& observation, const NodeData& prev_node,
       const NodeData& next_node) {
     return new ceres::AutoDiffCostFunction<
-        LandmarkCostFunction, 3 /* residuals */,
+        LandmarkCostFunction, 6 /* residuals */,
         3 /* previous node pose variables */, 3 /* next node pose variables */,
         4 /* landmark rotation variables */,
         3 /* landmark translation variables */>(
diff --git a/cartographer/mapping_2d/pose_graph/landmark_cost_function_test.cc b/cartographer/mapping_2d/pose_graph/landmark_cost_function_test.cc
index 7a47126..ef386e1 100644
--- a/cartographer/mapping_2d/pose_graph/landmark_cost_function_test.cc
+++ b/cartographer/mapping_2d/pose_graph/landmark_cost_function_test.cc
@@ -49,17 +49,21 @@ TEST(LandmarkCostFunctionTest, SmokeTest) {
       },
       prev_node, next_node);
 
-  std::array<double, 3> prev_node_pose{{2., 0., 0.}};
-  std::array<double, 3> next_node_pose{{0., 2., 0.}};
-  std::array<double, 4> landmark_rotation{{1., 0., 0., 0.}};
-  std::array<double, 3> landmark_translation{{1., 2., 1.}};
-  const double* parameter_blocks[] = {
-      prev_node_pose.data(), next_node_pose.data(), landmark_rotation.data(),
-      landmark_translation.data()};
+  const std::array<double, 3> prev_node_pose{{2., 0., 0.}};
+  const std::array<double, 3> next_node_pose{{0., 2., 0.}};
+  const std::array<double, 4> landmark_rotation{{1., 0., 0., 0.}};
+  const std::array<double, 3> landmark_translation{{1., 2., 1.}};
+  const std::array<const double*, 4> parameter_blocks{
+      {prev_node_pose.data(), next_node_pose.data(), landmark_rotation.data(),
+       landmark_translation.data()}};
 
   std::array<double, 6> residuals;
-  cost_function->Evaluate(parameter_blocks, residuals.data(), nullptr);
+  std::array<std::array<double, 13>, 6> jacobians;
+  std::array<double*, 6> jacobians_ptrs;
+  for (int i = 0; i < 6; ++i) jacobians_ptrs[i] = jacobians[i].data();
 
+  cost_function->Evaluate(parameter_blocks.data(), residuals.data(),
+                          jacobians_ptrs.data());
   EXPECT_THAT(residuals, ElementsAre(DoubleEq(1.), DoubleEq(0.), DoubleEq(0.),
                                      DoubleEq(0.), DoubleEq(0.), DoubleEq(0.)));
 }
diff --git a/cartographer/mapping_3d/pose_graph/landmark_cost_function_test.cc b/cartographer/mapping_3d/pose_graph/landmark_cost_function_test.cc
index 47cfc08..9fb4694 100644
--- a/cartographer/mapping_3d/pose_graph/landmark_cost_function_test.cc
+++ b/cartographer/mapping_3d/pose_graph/landmark_cost_function_test.cc
@@ -47,20 +47,23 @@ TEST(LandmarkCostFunctionTest, SmokeTest) {
       },
       prev_node, next_node);
 
-  std::array<double, 4> prev_node_rotation{{1., 0., 0., 0.}};
-  std::array<double, 3> prev_node_translation{{0., 0., 0.}};
-  std::array<double, 4> next_node_rotation{{1., 0., 0., 0.}};
-  std::array<double, 3> next_node_translation{{2., 2., 2.}};
-  std::array<double, 4> landmark_rotation{{1., 0., 0., 0.}};
-  std::array<double, 3> landmark_translation{{1., 2., 2.}};
-  const double* parameter_blocks[] = {
-      prev_node_rotation.data(), prev_node_translation.data(),
-      next_node_rotation.data(), next_node_translation.data(),
-      landmark_rotation.data(),  landmark_translation.data()};
+  const std::array<double, 4> prev_node_rotation{{1., 0., 0., 0.}};
+  const std::array<double, 3> prev_node_translation{{0., 0., 0.}};
+  const std::array<double, 4> next_node_rotation{{1., 0., 0., 0.}};
+  const std::array<double, 3> next_node_translation{{2., 2., 2.}};
+  const std::array<double, 4> landmark_rotation{{1., 0., 0., 0.}};
+  const std::array<double, 3> landmark_translation{{1., 2., 2.}};
+  const std::array<const double*, 6> parameter_blocks{
+      {prev_node_rotation.data(), prev_node_translation.data(),
+       next_node_rotation.data(), next_node_translation.data(),
+       landmark_rotation.data(), landmark_translation.data()}};
 
   std::array<double, 6> residuals;
-  cost_function->Evaluate(parameter_blocks, residuals.data(), nullptr);
-
+  std::array<std::array<double, 21>, 6> jacobians;
+  std::array<double*, 6> jacobians_ptrs;
+  for (int i = 0; i < 6; ++i) jacobians_ptrs[i] = jacobians[i].data();
+  cost_function->Evaluate(parameter_blocks.data(), residuals.data(),
+                          jacobians_ptrs.data());
   EXPECT_THAT(residuals, ElementsAre(DoubleEq(1.), DoubleEq(0.), DoubleEq(0.),
                                      DoubleEq(0.), DoubleEq(0.), DoubleEq(0.)));
 }