Generalize IntegrateImu(). (#627)

This is in preparation of changing the data structure
for IMU data away from a deque. Needed for localization
and life-long mapping.

cartographer/mapping/id_test.cc

@@ -35,7 +35,7 @@ common::Time CreateTime(const int milliseconds) {
 
 class Data {
  public:
-  Data(int milliseconds) : time_(CreateTime(milliseconds)) {}
+  explicit Data(int milliseconds) : time_(CreateTime(milliseconds)) {}
 
   const common::Time& time() const { return time_; }
 
@@ -74,9 +74,9 @@ TEST(IdTest, MapByIdIterator) {
       {NodeId{42, 0}, 3},
   };
   for (const auto& id_data : map_by_id) {
+    ASSERT_FALSE(expected_id_data.empty());
     EXPECT_EQ(expected_id_data.front().first, id_data.id);
     EXPECT_EQ(expected_id_data.front().second, id_data.data);
-    ASSERT_FALSE(expected_id_data.empty());
     expected_id_data.pop_front();
   }
   EXPECT_TRUE(expected_id_data.empty());
@@ -89,9 +89,9 @@ TEST(IdTest, MapByIdTrajectoryRange) {
       {NodeId{0, 1}, 1},
   };
   for (const auto& entry : map_by_id.trajectory(0)) {
+    ASSERT_FALSE(expected_data.empty());
     EXPECT_EQ(expected_data.front().first, entry.id);
     EXPECT_EQ(expected_data.front().second, entry.data);
-    ASSERT_FALSE(expected_data.empty());
     expected_data.pop_front();
   }
   EXPECT_TRUE(expected_data.empty());
@@ -101,8 +101,8 @@ TEST(IdTest, MapByIdTrajectoryIdRange) {
   MapById<NodeId, int> map_by_id = CreateTestMapById<NodeId>();
   std::deque<int> expected_data = {0, 7, 42};
   for (const int trajectory_id : map_by_id.trajectory_ids()) {
-    EXPECT_EQ(expected_data.front(), trajectory_id);
     ASSERT_FALSE(expected_data.empty());
+    EXPECT_EQ(expected_data.front(), trajectory_id);
     expected_data.pop_front();
   }
   EXPECT_TRUE(expected_data.empty());
@@ -118,9 +118,9 @@ TEST(IdTest, MapByIdIterateByTrajectories) {
   };
   for (int trajectory_id : map_by_id.trajectory_ids()) {
     for (const auto& entry : map_by_id.trajectory(trajectory_id)) {
+      ASSERT_FALSE(expected_id_data.empty());
       EXPECT_EQ(expected_id_data.front().first, entry.id);
       EXPECT_EQ(expected_id_data.front().second, entry.data);
-      ASSERT_FALSE(expected_id_data.empty());
       expected_id_data.pop_front();
     }
   }

cartographer/mapping_3d/imu_integration.cc

@@ -1,32 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "cartographer/mapping_3d/imu_integration.h"
-
-namespace cartographer {
-namespace mapping_3d {
-
-IntegrateImuResult<double> IntegrateImu(
-    const std::deque<sensor::ImuData>& imu_data, const common::Time start_time,
-    const common::Time end_time,
-    std::deque<sensor::ImuData>::const_iterator* it) {
-  return IntegrateImu<double>(imu_data, Eigen::Affine3d::Identity(),
-                              Eigen::Affine3d::Identity(), start_time, end_time,
-                              it);
-}
-
-}  // namespace mapping_3d
-}  // namespace cartographer

cartographer/mapping_3d/imu_integration.h

@@ -36,25 +36,19 @@ struct IntegrateImuResult {
   Eigen::Quaternion<T> delta_rotation;
 };
 
-// Returns velocity delta in map frame.
+template <typename T, typename RangeType, typename IteratorType>
-IntegrateImuResult<double> IntegrateImu(
-    const std::deque<sensor::ImuData>& imu_data, const common::Time start_time,
-    const common::Time end_time,
-    std::deque<sensor::ImuData>::const_iterator* it);
-
-template <typename T>
 IntegrateImuResult<T> IntegrateImu(
-    const std::deque<sensor::ImuData>& imu_data,
+    const RangeType& imu_data,
     const Eigen::Transform<T, 3, Eigen::Affine>&
         linear_acceleration_calibration,
     const Eigen::Transform<T, 3, Eigen::Affine>& angular_velocity_calibration,
     const common::Time start_time, const common::Time end_time,
-    std::deque<sensor::ImuData>::const_iterator* it) {
+    IteratorType* const it) {
   CHECK_LE(start_time, end_time);
-  CHECK(*it != imu_data.cend());
+  CHECK(*it != imu_data.end());
   CHECK_LE((*it)->time, start_time);
-  if ((*it) + 1 != imu_data.cend()) {
+  if (std::next(*it) != imu_data.end()) {
-    CHECK_GT(((*it) + 1)->time, start_time);
+    CHECK_GT(std::next(*it)->time, start_time);
   }
 
   common::Time current_time = start_time;
@@ -63,21 +57,22 @@ IntegrateImuResult<T> IntegrateImu(
                                   Eigen::Quaterniond::Identity().cast<T>()};
   while (current_time < end_time) {
     common::Time next_imu_data = common::Time::max();
-    if ((*it) + 1 != imu_data.cend()) {
+    if (std::next(*it) != imu_data.end()) {
-      next_imu_data = ((*it) + 1)->time;
+      next_imu_data = std::next(*it)->time;
     }
     common::Time next_time = std::min(next_imu_data, end_time);
     const T delta_t(common::ToSeconds(next_time - current_time));
 
     const Eigen::Matrix<T, 3, 1> delta_angle =
-        (angular_velocity_calibration * (*it)->angular_velocity.cast<T>()) *
+        (angular_velocity_calibration *
+         (*it)->angular_velocity.template cast<T>()) *
         delta_t;
     result.delta_rotation *=
         transform::AngleAxisVectorToRotationQuaternion(delta_angle);
-    result.delta_velocity +=
+    result.delta_velocity += result.delta_rotation *
-        result.delta_rotation * ((linear_acceleration_calibration *
+                             ((linear_acceleration_calibration *
-                                  (*it)->linear_acceleration.cast<T>()) *
+                               (*it)->linear_acceleration.template cast<T>()) *
-                                 delta_t);
+                              delta_t);
     current_time = next_time;
     if (current_time == next_imu_data) {
       ++(*it);
@@ -86,6 +81,17 @@ IntegrateImuResult<T> IntegrateImu(
   return result;
 }
 
+// Returns velocity delta in map frame.
+template <typename RangeType, typename IteratorType>
+IntegrateImuResult<double> IntegrateImu(const RangeType& imu_data,
+                                        const common::Time start_time,
+                                        const common::Time end_time,
+                                        IteratorType* const it) {
+  return IntegrateImu<double, RangeType, IteratorType>(
+      imu_data, Eigen::Affine3d::Identity(), Eigen::Affine3d::Identity(),
+      start_time, end_time, it);
+}
+
 }  // namespace mapping_3d
 }  // namespace cartographer
 

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -236,7 +236,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
       const std::deque<sensor::ImuData>& imu_data = imu_data_.at(trajectory_id);
       CHECK(!imu_data.empty());
 
-      auto imu_it = imu_data.cbegin();
+      auto imu_it = imu_data.begin();
       auto prev_node_it = node_it;
       for (++node_it; node_it != trajectory_end; ++node_it) {
         const mapping::NodeId first_node_id = prev_node_it->id;
@@ -250,8 +250,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
         }
 
         // Skip IMU data before the node.
-        while ((imu_it + 1) != imu_data.cend() &&
+        while (std::next(imu_it) != imu_data.end() &&
-               (imu_it + 1)->time <= first_node_data.time) {
+               std::next(imu_it)->time <= first_node_data.time) {
           ++imu_it;
         }