From 049f30d824c8e4a4c3dd1b10f633814b33d0e349 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 6 Nov 2017 11:00:33 +0100 Subject: [PATCH] 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 | 10 ++--- cartographer/mapping_3d/imu_integration.cc | 32 -------------- cartographer/mapping_3d/imu_integration.h | 44 +++++++++++-------- .../sparse_pose_graph/optimization_problem.cc | 6 +-- 4 files changed, 33 insertions(+), 59 deletions(-) delete mode 100644 cartographer/mapping_3d/imu_integration.cc diff --git a/cartographer/mapping/id_test.cc b/cartographer/mapping/id_test.cc index 68decca..84ab862 100644 --- a/cartographer/mapping/id_test.cc +++ b/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 map_by_id = CreateTestMapById(); std::deque 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(); } } diff --git a/cartographer/mapping_3d/imu_integration.cc b/cartographer/mapping_3d/imu_integration.cc deleted file mode 100644 index 827e8ee..0000000 --- a/cartographer/mapping_3d/imu_integration.cc +++ /dev/null @@ -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 IntegrateImu( - const std::deque& imu_data, const common::Time start_time, - const common::Time end_time, - std::deque::const_iterator* it) { - return IntegrateImu(imu_data, Eigen::Affine3d::Identity(), - Eigen::Affine3d::Identity(), start_time, end_time, - it); -} - -} // namespace mapping_3d -} // namespace cartographer diff --git a/cartographer/mapping_3d/imu_integration.h b/cartographer/mapping_3d/imu_integration.h index f3125ad..4358e1a 100644 --- a/cartographer/mapping_3d/imu_integration.h +++ b/cartographer/mapping_3d/imu_integration.h @@ -36,25 +36,19 @@ struct IntegrateImuResult { Eigen::Quaternion delta_rotation; }; -// Returns velocity delta in map frame. -IntegrateImuResult IntegrateImu( - const std::deque& imu_data, const common::Time start_time, - const common::Time end_time, - std::deque::const_iterator* it); - -template +template IntegrateImuResult IntegrateImu( - const std::deque& imu_data, + const RangeType& imu_data, const Eigen::Transform& linear_acceleration_calibration, const Eigen::Transform& angular_velocity_calibration, const common::Time start_time, const common::Time end_time, - std::deque::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()) { - CHECK_GT(((*it) + 1)->time, start_time); + if (std::next(*it) != imu_data.end()) { + CHECK_GT(std::next(*it)->time, start_time); } common::Time current_time = start_time; @@ -63,21 +57,22 @@ IntegrateImuResult IntegrateImu( Eigen::Quaterniond::Identity().cast()}; while (current_time < end_time) { common::Time next_imu_data = common::Time::max(); - if ((*it) + 1 != imu_data.cend()) { - next_imu_data = ((*it) + 1)->time; + if (std::next(*it) != imu_data.end()) { + 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 delta_angle = - (angular_velocity_calibration * (*it)->angular_velocity.cast()) * + (angular_velocity_calibration * + (*it)->angular_velocity.template cast()) * delta_t; result.delta_rotation *= transform::AngleAxisVectorToRotationQuaternion(delta_angle); - result.delta_velocity += - result.delta_rotation * ((linear_acceleration_calibration * - (*it)->linear_acceleration.cast()) * - delta_t); + result.delta_velocity += result.delta_rotation * + ((linear_acceleration_calibration * + (*it)->linear_acceleration.template cast()) * + delta_t); current_time = next_time; if (current_time == next_imu_data) { ++(*it); @@ -86,6 +81,17 @@ IntegrateImuResult IntegrateImu( return result; } +// Returns velocity delta in map frame. +template +IntegrateImuResult IntegrateImu(const RangeType& imu_data, + const common::Time start_time, + const common::Time end_time, + IteratorType* const it) { + return IntegrateImu( + imu_data, Eigen::Affine3d::Identity(), Eigen::Affine3d::Identity(), + start_time, end_time, it); +} + } // namespace mapping_3d } // namespace cartographer diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index f3f3d35..ee39c3e 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -236,7 +236,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, const std::deque& 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& constraints, } // Skip IMU data before the node. - while ((imu_it + 1) != imu_data.cend() && - (imu_it + 1)->time <= first_node_data.time) { + while (std::next(imu_it) != imu_data.end() && + std::next(imu_it)->time <= first_node_data.time) { ++imu_it; }