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.
master
Wolfgang Hess 2017-11-06 11:00:33 +01:00 committed by GitHub
parent 7c31291b2b
commit 049f30d824
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 33 additions and 59 deletions

View File

@ -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();
}
}

View File

@ -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

View File

@ -36,25 +36,19 @@ struct IntegrateImuResult {
Eigen::Quaternion<T> delta_rotation;
};
// Returns velocity delta in map frame.
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>
template <typename T, typename RangeType, typename IteratorType>
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()) {
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<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()) {
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<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_rotation * ((linear_acceleration_calibration *
(*it)->linear_acceleration.cast<T>()) *
delta_t);
result.delta_velocity += result.delta_rotation *
((linear_acceleration_calibration *
(*it)->linear_acceleration.template cast<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

View File

@ -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() &&
(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;
}