From c7a8c5fda90ec9fe66c2329e5324acb2a42af079 Mon Sep 17 00:00:00 2001
From: Susanne Pielawa <32822068+spielawa@users.noreply.github.com>
Date: Wed, 10 Jan 2018 16:01:27 +0100
Subject: [PATCH] Keep fixed frame across optimizations. (#807)

PAIR=wohe
---
 .../pose_graph/optimization_problem.cc        | 45 ++++++++++++-------
 .../pose_graph/optimization_problem.h         |  2 +
 2 files changed, 31 insertions(+), 16 deletions(-)

diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.cc b/cartographer/mapping_3d/pose_graph/optimization_problem.cc
index 6c6de43..cb5a545 100644
--- a/cartographer/mapping_3d/pose_graph/optimization_problem.cc
+++ b/cartographer/mapping_3d/pose_graph/optimization_problem.cc
@@ -378,7 +378,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   }
 
   // Add fixed frame pose constraints.
-  std::deque<CeresPose> C_fixed_frames;
+  std::map<int, CeresPose> C_fixed_frames;
   for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
     const int trajectory_id = node_it->id.trajectory_id;
     const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
@@ -387,6 +387,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
       continue;
     }
 
+    const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
     bool fixed_frame_pose_initialized = false;
     for (; node_it != trajectory_end; ++node_it) {
       const mapping::NodeId node_id = node_it->id;
@@ -403,26 +404,34 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
           options_.fixed_frame_pose_rotation_weight()};
 
       if (!fixed_frame_pose_initialized) {
-        const transform::Rigid3d fixed_frame_pose_in_map =
-            node_data.global_pose * constraint_pose.zbar_ij.inverse();
-        C_fixed_frames.emplace_back(
-            transform::Rigid3d(
-                fixed_frame_pose_in_map.translation(),
-                Eigen::AngleAxisd(
-                    transform::GetYaw(fixed_frame_pose_in_map.rotation()),
-                    Eigen::Vector3d::UnitZ())),
-            nullptr,
-            common::make_unique<ceres::AutoDiffLocalParameterization<
-                YawOnlyQuaternionPlus, 4, 1>>(),
-            &problem);
+        transform::Rigid3d fixed_frame_pose_in_map;
+        if (trajectory_data.fixed_frame.has_value()) {
+          fixed_frame_pose_in_map = trajectory_data.fixed_frame.value();
+        } else {
+          fixed_frame_pose_in_map =
+              node_data.global_pose * constraint_pose.zbar_ij.inverse();
+        }
+        C_fixed_frames.emplace(
+            std::piecewise_construct, std::forward_as_tuple(trajectory_id),
+            std::forward_as_tuple(
+                transform::Rigid3d(
+                    fixed_frame_pose_in_map.translation(),
+                    Eigen::AngleAxisd(
+                        transform::GetYaw(fixed_frame_pose_in_map.rotation()),
+                        Eigen::Vector3d::UnitZ())),
+                nullptr,
+                common::make_unique<ceres::AutoDiffLocalParameterization<
+                    YawOnlyQuaternionPlus, 4, 1>>(),
+                &problem));
         fixed_frame_pose_initialized = true;
       }
 
       problem.AddResidualBlock(
           SpaCostFunction::CreateAutoDiffCostFunction(constraint_pose),
-          nullptr /* loss function */, C_fixed_frames.back().rotation(),
-          C_fixed_frames.back().translation(), C_nodes.at(node_id).rotation(),
-          C_nodes.at(node_id).translation());
+          nullptr /* loss function */,
+          C_fixed_frames.at(trajectory_id).rotation(),
+          C_fixed_frames.at(trajectory_id).translation(),
+          C_nodes.at(node_id).rotation(), C_nodes.at(node_id).translation());
     }
   }
 
@@ -458,6 +467,10 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
     node_data_.at(C_node_id_data.id).global_pose =
         C_node_id_data.data.ToRigid();
   }
+  for (const auto& C_fixed_frame : C_fixed_frames) {
+    trajectory_data_.at(C_fixed_frame.first).fixed_frame =
+        C_fixed_frame.second.ToRigid();
+  }
 }
 
 const mapping::MapById<mapping::NodeId, NodeData>&
diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.h b/cartographer/mapping_3d/pose_graph/optimization_problem.h
index aa42132..863274f 100644
--- a/cartographer/mapping_3d/pose_graph/optimization_problem.h
+++ b/cartographer/mapping_3d/pose_graph/optimization_problem.h
@@ -24,6 +24,7 @@
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
+#include "cartographer/common/optional.h"
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/id.h"
@@ -105,6 +106,7 @@ class OptimizationProblem {
   struct TrajectoryData {
     double gravity_constant = 9.8;
     std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
+    common::optional<transform::Rigid3d> fixed_frame;
   };
 
   mapping::pose_graph::proto::OptimizationProblemOptions options_;