Add support for odometry to the 3D pose graph optimization. (#570)

Not used yet. Intended to experiment with the 3D pose graph optimization
in 2D SLAM.

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -223,10 +223,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
 
       const bool odometry_available =
           trajectory_id < odometry_data_.size() &&
-          odometry_data_[trajectory_id].Has(
+          odometry_data_[trajectory_id].Has(next_node_data.time) &&
-              node_data_[trajectory_id][next_node_index].time) &&
+          odometry_data_[trajectory_id].Has(node_data.time);
-          odometry_data_[trajectory_id].Has(
-              node_data_[trajectory_id][node_index].time);
       const transform::Rigid3d relative_pose =
           odometry_available
               ? transform::Rigid3d::Rotation(node_data.gravity_alignment) *
@@ -245,8 +243,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
                   options_.consecutive_scan_translation_penalty_factor(),
                   options_.consecutive_scan_rotation_penalty_factor()})),
           nullptr /* loss function */,
-          C_nodes[trajectory_id][node_index].data(),
+          C_nodes[trajectory_id].at(node_index).data(),
-          C_nodes[trajectory_id][next_node_index].data());
+          C_nodes[trajectory_id].at(next_node_index).data());
     }
   }
 

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -233,6 +233,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
 
   // Add constraints based on IMU observations of angular velocities and
   // linear acceleration.
+  if (fix_z_ == FixZ::kNo) {
     trajectory_data_.resize(imu_data_.size());
     CHECK_GE(trajectory_data_.size(), node_data_.size());
     for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
@@ -289,10 +290,10 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
           const IntegrateImuResult<double> result_center_to_center =
               IntegrateImu(imu_data, first_center, second_center, &imu_it2);
           // 'delta_velocity' is the change in velocity from the point in time
-        // halfway between the first and second poses to halfway between second
+          // halfway between the first and second poses to halfway between
-        // and third pose. It is computed from IMU data and still contains a
+          // second and third pose. It is computed from IMU data and still
-        // delta due to gravity. The orientation of this vector is in the IMU
+          // contains a delta due to gravity. The orientation of this vector is
-        // frame at the second pose.
+          // in the IMU frame at the second pose.
           const Eigen::Vector3d delta_velocity =
               (result.delta_rotation.inverse() *
                result_to_first_center.delta_rotation) *
@@ -320,6 +321,57 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
             trajectory_data.imu_calibration.data());
       }
     }
+  }
+
+  if (fix_z_ == FixZ::kYes) {
+    // Add penalties for violating odometry or changes between consecutive scans
+    // if odometry is not available.
+    for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
+         ++trajectory_id) {
+      if (node_data_[trajectory_id].empty()) {
+        continue;
+      }
+      for (auto node_it = node_data_[trajectory_id].begin();;) {
+        const int node_index = node_it->first;
+        const NodeData& node_data = node_it->second;
+        ++node_it;
+        if (node_it == node_data_[trajectory_id].end()) {
+          break;
+        }
+
+        const int next_node_index = node_it->first;
+        const NodeData& next_node_data = node_it->second;
+
+        if (next_node_index != node_index + 1) {
+          continue;
+        }
+
+        const bool odometry_available =
+            trajectory_id < odometry_data_.size() &&
+            odometry_data_[trajectory_id].Has(next_node_data.time) &&
+            odometry_data_[trajectory_id].Has(node_data.time);
+        const transform::Rigid3d relative_pose =
+            odometry_available
+                ? odometry_data_[trajectory_id]
+                          .Lookup(node_data.time)
+                          .inverse() *
+                      odometry_data_[trajectory_id].Lookup(next_node_data.time)
+                : node_data.initial_pose.inverse() *
+                      next_node_data.initial_pose;
+        problem.AddResidualBlock(
+            new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
+                new SpaCostFunction(Constraint::Pose{
+                    relative_pose,
+                    options_.consecutive_scan_translation_penalty_factor(),
+                    options_.consecutive_scan_rotation_penalty_factor()})),
+            nullptr /* loss function */,
+            C_nodes[trajectory_id].at(node_index).rotation(),
+            C_nodes[trajectory_id].at(node_index).translation(),
+            C_nodes[trajectory_id].at(next_node_index).rotation(),
+            C_nodes[trajectory_id].at(next_node_index).translation());
+      }
+    }
+  }
 
   // Add fixed frame pose constraints.
   std::deque<CeresPose> C_fixed_frames;