Interpolate trajectory node to compute starting point for landmarks. (#997)

[Landmarks RFC](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)

cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc

@@ -30,6 +30,7 @@
 #include "cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h"
 #include "cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h"
 #include "cartographer/mapping/internal/pose_graph/ceres_pose.h"
+#include "cartographer/mapping/internal/pose_graph/cost_helpers.h"
 #include "cartographer/sensor/odometry_data.h"
 #include "cartographer/transform/transform.h"
 #include "ceres/ceres.h"
@@ -62,13 +63,20 @@ transform::Rigid2d ToPose(const std::array<double, 3>& values) {
 // applies a relative transform from it.
 transform::Rigid3d GetInitialLandmarkPose(
     const LandmarkNode::LandmarkObservation& observation,
-    const NodeData& prev_node, const NodeData& next_node) {
+    const NodeData& prev_node, const NodeData& next_node,
-  const NodeData& closest_node =
+    const std::array<double, 3>& prev_node_pose,
-      observation.time - prev_node.time < next_node.time - observation.time
+    const std::array<double, 3>& next_node_pose) {
-          ? prev_node
+  const double interpolation_parameter =
-          : next_node;
+      common::ToSeconds(observation.time - prev_node.time) /
-  return transform::Embed3D(closest_node.pose) *
+      common::ToSeconds(next_node.time - prev_node.time);
-         transform::Rigid3d::Rotation(closest_node.gravity_alignment) *
+
+  const std::tuple<std::array<double, 4>, std::array<double, 3>>
+      rotation_and_translation =
+          InterpolateNodes2D(prev_node_pose.data(), prev_node.gravity_alignment,
+                             next_node_pose.data(), next_node.gravity_alignment,
+                             interpolation_parameter);
+  return transform::Rigid3d::FromArrays(std::get<0>(rotation_and_translation),
+                                        std::get<1>(rotation_and_translation)) *
          observation.landmark_to_tracking_transform;
 }
 
@@ -104,11 +112,14 @@ void AddLandmarkCostFunctions(
       }
       auto prev = std::prev(next);
       // Add parameter blocks for the landmark ID if they were not added before.
+      std::array<double, 3>* prev_node_pose = &C_nodes->at(prev->id);
+      std::array<double, 3>* next_node_pose = &C_nodes->at(next->id);
       if (!C_landmarks->count(landmark_id)) {
         const transform::Rigid3d starting_point =
             landmark_node.second.global_landmark_pose.has_value()
                 ? landmark_node.second.global_landmark_pose.value()
-                : GetInitialLandmarkPose(observation, prev->data, next->data);
+                : GetInitialLandmarkPose(observation, prev->data, next->data,
+                                         *prev_node_pose, *next_node_pose);
         C_landmarks->emplace(
             landmark_id,
             CeresPose(starting_point, nullptr /* translation_parametrization */,
@@ -124,8 +135,8 @@ void AddLandmarkCostFunctions(
       problem->AddResidualBlock(
           LandmarkCostFunction2D::CreateAutoDiffCostFunction(
               observation, prev->data, next->data),
-          nullptr /* loss function */, C_nodes->at(prev->id).data(),
+          nullptr /* loss function */, prev_node_pose->data(),
-          C_nodes->at(next->id).data(), C_landmarks->at(landmark_id).rotation(),
+          next_node_pose->data(), C_landmarks->at(landmark_id).rotation(),
           C_landmarks->at(landmark_id).translation());
     }
   }

cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc

@@ -107,12 +107,20 @@ std::unique_ptr<transform::Rigid3d> Interpolate(
 // applies a relative transform from it.
 transform::Rigid3d GetInitialLandmarkPose(
     const LandmarkNode::LandmarkObservation& observation,
-    const NodeData& prev_node, const NodeData& next_node) {
+    const NodeData& prev_node, const NodeData& next_node,
-  const NodeData& closest_node =
+    const CeresPose& prev_node_pose, const CeresPose& next_node_pose) {
-      observation.time - prev_node.time < next_node.time - observation.time
+  const double interpolation_parameter =
-          ? prev_node
+      common::ToSeconds(observation.time - prev_node.time) /
-          : next_node;
+      common::ToSeconds(next_node.time - prev_node.time);
-  return closest_node.global_pose * observation.landmark_to_tracking_transform;
+
+  const std::tuple<std::array<double, 4>, std::array<double, 3>>
+      rotation_and_translation = InterpolateNodes3D(
+          prev_node_pose.rotation(), prev_node_pose.translation(),
+          next_node_pose.rotation(), next_node_pose.translation(),
+          interpolation_parameter);
+  return transform::Rigid3d::FromArrays(std::get<0>(rotation_and_translation),
+                                        std::get<1>(rotation_and_translation)) *
+         observation.landmark_to_tracking_transform;
 }
 
 void AddLandmarkCostFunctions(
@@ -147,11 +155,14 @@ void AddLandmarkCostFunctions(
       }
       auto prev = std::prev(next);
       // Add parameter blocks for the landmark ID if they were not added before.
+      CeresPose* prev_node_pose = &C_nodes->at(prev->id);
+      CeresPose* next_node_pose = &C_nodes->at(next->id);
       if (!C_landmarks->count(landmark_id)) {
         const transform::Rigid3d starting_point =
             landmark_node.second.global_landmark_pose.has_value()
                 ? landmark_node.second.global_landmark_pose.value()
-                : GetInitialLandmarkPose(observation, prev->data, next->data);
+                : GetInitialLandmarkPose(observation, prev->data, next->data,
+                                         *prev_node_pose, *next_node_pose);
         C_landmarks->emplace(
             landmark_id,
             CeresPose(starting_point, nullptr /* translation_parametrization */,
@@ -167,9 +178,9 @@ void AddLandmarkCostFunctions(
       problem->AddResidualBlock(
           LandmarkCostFunction3D::CreateAutoDiffCostFunction(
               observation, prev->data, next->data),
-          nullptr /* loss function */, C_nodes->at(prev->id).rotation(),
+          nullptr /* loss function */, prev_node_pose->rotation(),
-          C_nodes->at(prev->id).translation(), C_nodes->at(next->id).rotation(),
+          prev_node_pose->translation(), next_node_pose->rotation(),
-          C_nodes->at(next->id).translation(),
+          next_node_pose->translation(),
           C_landmarks->at(landmark_id).rotation(),
           C_landmarks->at(landmark_id).translation());
     }

cartographer/mapping/internal/pose_graph/ceres_pose.cc

@@ -37,10 +37,7 @@ CeresPose::CeresPose(
 }
 
 const transform::Rigid3d CeresPose::ToRigid() const {
-  const auto& rotation = data_->rotation;
+  return transform::Rigid3d::FromArrays(data_->rotation, data_->translation);
-  return transform::Rigid3d(
-      Eigen::Map<const Eigen::Vector3d>(data_->translation.data()),
-      Eigen::Quaterniond(rotation[0], rotation[1], rotation[2], rotation[3]));
 }
 
 }  // namespace pose_graph

cartographer/mapping/internal/pose_graph/ceres_pose.h

@@ -39,7 +39,10 @@ class CeresPose {
   const transform::Rigid3d ToRigid() const;
 
   double* translation() { return data_->translation.data(); }
+  const double* translation() const { return data_->translation.data(); }
+
   double* rotation() { return data_->rotation.data(); }
+  const double* rotation() const { return data_->rotation.data(); }
 
  private:
   struct Data {

cartographer/transform/rigid_transform.h

@@ -144,6 +144,13 @@ class Rigid3 {
     return Rigid3(vector, Quaternion::Identity());
   }
 
+  static Rigid3 FromArrays(const std::array<FloatType, 4>& rotation,
+                           const std::array<FloatType, 3>& translation) {
+    return Rigid3(Eigen::Map<const Vector>(translation.data()),
+                  Eigen::Quaternion<FloatType>(rotation[0], rotation[1],
+                                               rotation[2], rotation[3]));
+  }
+
   static Rigid3<FloatType> Identity() { return Rigid3<FloatType>(); }
 
   template <typename OtherType>