From 91034eaf58e0151e6ddb01475e2e4c413dee342b Mon Sep 17 00:00:00 2001
From: Susanne Pielawa <32822068+spielawa@users.noreply.github.com>
Date: Mon, 8 Jan 2018 11:37:07 +0100
Subject: [PATCH] Making pose in FixedFramePoseData optional. (#792)

This is for the case that the GPS signal is not available.
---
 .../mapping/global_trajectory_builder.h       |  5 ++-
 .../pose_graph/optimization_problem.cc        | 34 ++++++++++++++++---
 cartographer/sensor/fixed_frame_pose_data.cc  | 10 ++++--
 cartographer/sensor/fixed_frame_pose_data.h   |  7 ++--
 4 files changed, 47 insertions(+), 9 deletions(-)

diff --git a/cartographer/internal/mapping/global_trajectory_builder.h b/cartographer/internal/mapping/global_trajectory_builder.h
index e2db4f0..0d2d12f 100644
--- a/cartographer/internal/mapping/global_trajectory_builder.h
+++ b/cartographer/internal/mapping/global_trajectory_builder.h
@@ -85,7 +85,10 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
   void AddSensorData(
       const std::string& sensor_id,
       const sensor::FixedFramePoseData& fixed_frame_pose) override {
-    CHECK(fixed_frame_pose.pose.IsValid()) << fixed_frame_pose.pose;
+    if (fixed_frame_pose.pose.has_value()) {
+      CHECK(fixed_frame_pose.pose.value().IsValid())
+          << fixed_frame_pose.pose.value();
+    }
     pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
   }
 
diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.cc b/cartographer/mapping_3d/pose_graph/optimization_problem.cc
index 2f640de..925868c 100644
--- a/cartographer/mapping_3d/pose_graph/optimization_problem.cc
+++ b/cartographer/mapping_3d/pose_graph/optimization_problem.cc
@@ -49,11 +49,10 @@ namespace pose_graph {
 
 namespace {
 
-// For odometry and fixed frame pose.
-template <typename MapByTimeType>
+// For odometry.
 std::unique_ptr<transform::Rigid3d> Interpolate(
-    const MapByTimeType& map_by_time, const int trajectory_id,
-    const common::Time time) {
+    const sensor::MapByTime<sensor::OdometryData>& map_by_time,
+    const int trajectory_id, const common::Time time) {
   const auto it = map_by_time.lower_bound(trajectory_id, time);
   if (it == map_by_time.EndOfTrajectory(trajectory_id)) {
     return nullptr;
@@ -71,6 +70,33 @@ std::unique_ptr<transform::Rigid3d> Interpolate(
           .transform);
 }
 
+// For fixed frame pose.
+std::unique_ptr<transform::Rigid3d> Interpolate(
+    const sensor::MapByTime<sensor::FixedFramePoseData>& map_by_time,
+    const int trajectory_id, const common::Time time) {
+  const auto it = map_by_time.lower_bound(trajectory_id, time);
+  if (it == map_by_time.EndOfTrajectory(trajectory_id) ||
+      !it->pose.has_value()) {
+    return nullptr;
+  }
+  if (it == map_by_time.BeginOfTrajectory(trajectory_id)) {
+    if (it->time == time) {
+      return common::make_unique<transform::Rigid3d>(it->pose.value());
+    }
+    return nullptr;
+  }
+  const auto prev_it = std::prev(it);
+  if (prev_it->pose.has_value()) {
+    return common::make_unique<transform::Rigid3d>(
+        Interpolate(transform::TimestampedTransform{prev_it->time,
+                                                    prev_it->pose.value()},
+                    transform::TimestampedTransform{it->time, it->pose.value()},
+                    time)
+            .transform);
+  }
+  return nullptr;
+}
+
 }  // namespace
 
 OptimizationProblem::OptimizationProblem(
diff --git a/cartographer/sensor/fixed_frame_pose_data.cc b/cartographer/sensor/fixed_frame_pose_data.cc
index 87626cf..7bceea3 100644
--- a/cartographer/sensor/fixed_frame_pose_data.cc
+++ b/cartographer/sensor/fixed_frame_pose_data.cc
@@ -16,6 +16,7 @@
 
 #include "cartographer/sensor/fixed_frame_pose_data.h"
 
+#include "cartographer/common/optional.h"
 #include "cartographer/transform/transform.h"
 
 namespace cartographer {
@@ -24,13 +25,18 @@ namespace sensor {
 proto::FixedFramePoseData ToProto(const FixedFramePoseData& pose_data) {
   proto::FixedFramePoseData proto;
   proto.set_timestamp(common::ToUniversal(pose_data.time));
-  *proto.mutable_pose() = transform::ToProto(pose_data.pose);
+  if (pose_data.pose.has_value()) {
+    *proto.mutable_pose() = transform::ToProto(pose_data.pose.value());
+  }
   return proto;
 }
 
 FixedFramePoseData FromProto(const proto::FixedFramePoseData& proto) {
   return FixedFramePoseData{common::FromUniversal(proto.timestamp()),
-                            transform::ToRigid3(proto.pose())};
+                            proto.has_pose()
+                                ? common::optional<transform::Rigid3d>(
+                                      transform::ToRigid3(proto.pose()))
+                                : common::optional<transform::Rigid3d>()};
 }
 
 }  // namespace sensor
diff --git a/cartographer/sensor/fixed_frame_pose_data.h b/cartographer/sensor/fixed_frame_pose_data.h
index 19cdb41..382bd60 100644
--- a/cartographer/sensor/fixed_frame_pose_data.h
+++ b/cartographer/sensor/fixed_frame_pose_data.h
@@ -17,6 +17,9 @@
 #ifndef CARTOGRAPHER_SENSOR_FIXED_FRAME_POSE_DATA_H_
 #define CARTOGRAPHER_SENSOR_FIXED_FRAME_POSE_DATA_H_
 
+#include <memory>
+
+#include "cartographer/common/optional.h"
 #include "cartographer/common/time.h"
 #include "cartographer/sensor/proto/sensor.pb.h"
 #include "cartographer/transform/rigid_transform.h"
@@ -24,11 +27,11 @@
 namespace cartographer {
 namespace sensor {
 
-// The fixed frame pose data(like gps, pose, etc.) will be used in the
+// The fixed frame pose data (like GPS, pose, etc.) will be used in the
 // optimization.
 struct FixedFramePoseData {
   common::Time time;
-  transform::Rigid3d pose;
+  common::optional<transform::Rigid3d> pose;
 };
 
 // Converts 'pose_data' to a proto::FixedFramePoseData.