From d3c49c8585a1e7fa8d87d93e22306c603a7a5fe5 Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@google.com>
Date: Fri, 1 Sep 2017 09:58:24 +0200
Subject: [PATCH] Pass odometry data to the 3D optimization problem. (#495)

Not yet used.
---
 cartographer/mapping_3d/sparse_pose_graph.cc              | 6 ++++--
 .../mapping_3d/sparse_pose_graph/optimization_problem.cc  | 8 ++++++++
 .../mapping_3d/sparse_pose_graph/optimization_problem.h   | 4 ++++
 3 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc
index 80c835d..2c59b0f 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph.cc
@@ -152,8 +152,10 @@ void SparsePoseGraph::AddImuData(const int trajectory_id,
 
 void SparsePoseGraph::AddOdometerData(
     const int trajectory_id, const sensor::OdometryData& odometry_data) {
-  // TODO(cschuet): Add support for handling OdometryData in the optimization
-  // problem.
+  common::MutexLocker locker(&mutex_);
+  AddWorkItem([=]() REQUIRES(mutex_) {
+    optimization_problem_.AddOdometerData(trajectory_id, odometry_data);
+  });
 }
 
 void SparsePoseGraph::AddFixedFramePoseData(
diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
index c4314cc..732247e 100644
--- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
@@ -61,6 +61,14 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
   imu_data_[trajectory_id].push_back(imu_data);
 }
 
+void OptimizationProblem::AddOdometerData(
+    const int trajectory_id, const sensor::OdometryData& odometry_data) {
+  CHECK_GE(trajectory_id, 0);
+  odometry_data_.resize(
+      std::max(odometry_data_.size(), static_cast<size_t>(trajectory_id) + 1));
+  odometry_data_[trajectory_id].Push(odometry_data.time, odometry_data.pose);
+}
+
 void OptimizationProblem::AddFixedFramePoseData(
     const int trajectory_id,
     const sensor::FixedFramePoseData& fixed_frame_pose_data) {
diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
index dfa48fd..3745f93 100644
--- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
+++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
@@ -31,6 +31,7 @@
 #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
 #include "cartographer/sensor/imu_data.h"
+#include "cartographer/sensor/odometry_data.h"
 #include "cartographer/transform/transform_interpolation_buffer.h"
 
 namespace cartographer {
@@ -64,6 +65,8 @@ class OptimizationProblem {
   OptimizationProblem& operator=(const OptimizationProblem&) = delete;
 
   void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
+  void AddOdometerData(int trajectory_id,
+                       const sensor::OdometryData& odometry_data);
   void AddFixedFramePoseData(
       int trajectory_id,
       const sensor::FixedFramePoseData& fixed_frame_pose_data);
@@ -91,6 +94,7 @@ class OptimizationProblem {
   FixZ fix_z_;
   std::vector<std::deque<sensor::ImuData>> imu_data_;
   std::vector<std::vector<NodeData>> node_data_;
+  std::vector<transform::TransformInterpolationBuffer> odometry_data_;
   std::vector<std::vector<SubmapData>> submap_data_;
   std::vector<TrajectoryData> trajectory_data_;
   std::vector<transform::TransformInterpolationBuffer> fixed_frame_pose_data_;