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
parent
7c31291b2b
commit
049f30d824
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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
|
|
@ -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,20 +57,21 @@ 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>()) *
|
||||
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) {
|
||||
|
@ -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
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue