Introduce sensor::MapByTime<>. (#631)
This data structure is used for keeping IMU data. This allows trimming IMU data in the middle which is needed for life-long mapping.master
parent
049f30d824
commit
77fb50fd76
|
@ -64,10 +64,7 @@ OptimizationProblem::~OptimizationProblem() {}
|
|||
|
||||
void OptimizationProblem::AddImuData(const int trajectory_id,
|
||||
const sensor::ImuData& imu_data) {
|
||||
CHECK_GE(trajectory_id, 0);
|
||||
imu_data_.resize(
|
||||
std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
|
||||
imu_data_[trajectory_id].push_back(imu_data);
|
||||
imu_data_.Append(trajectory_id, imu_data);
|
||||
}
|
||||
|
||||
void OptimizationProblem::AddOdometerData(
|
||||
|
@ -95,18 +92,8 @@ void OptimizationProblem::InsertTrajectoryNode(
|
|||
}
|
||||
|
||||
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
|
||||
imu_data_.Trim(node_data_, node_id);
|
||||
node_data_.Trim(node_id);
|
||||
|
||||
const int trajectory_id = node_id.trajectory_id;
|
||||
if (node_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 &&
|
||||
trajectory_id < static_cast<int>(imu_data_.size())) {
|
||||
const common::Time node_time =
|
||||
node_data_.BeginOfTrajectory(trajectory_id)->data.time;
|
||||
auto& imu_data = imu_data_.at(trajectory_id);
|
||||
while (imu_data.size() > 1 && imu_data[1].time <= node_time) {
|
||||
imu_data.pop_front();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void OptimizationProblem::AddSubmap(const int trajectory_id,
|
||||
|
|
|
@ -31,6 +31,7 @@
|
|||
#include "cartographer/mapping/sparse_pose_graph.h"
|
||||
#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
|
||||
#include "cartographer/sensor/imu_data.h"
|
||||
#include "cartographer/sensor/map_by_time.h"
|
||||
#include "cartographer/sensor/odometry_data.h"
|
||||
#include "cartographer/transform/transform_interpolation_buffer.h"
|
||||
|
||||
|
@ -90,7 +91,7 @@ class OptimizationProblem {
|
|||
|
||||
private:
|
||||
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
|
||||
std::vector<std::deque<sensor::ImuData>> imu_data_;
|
||||
sensor::MapByTime<sensor::ImuData> imu_data_;
|
||||
mapping::MapById<mapping::NodeId, NodeData> node_data_;
|
||||
std::vector<transform::TransformInterpolationBuffer> odometry_data_;
|
||||
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
|
||||
|
|
|
@ -56,10 +56,7 @@ OptimizationProblem::~OptimizationProblem() {}
|
|||
|
||||
void OptimizationProblem::AddImuData(const int trajectory_id,
|
||||
const sensor::ImuData& imu_data) {
|
||||
CHECK_GE(trajectory_id, 0);
|
||||
imu_data_.resize(
|
||||
std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
|
||||
imu_data_[trajectory_id].push_back(imu_data);
|
||||
imu_data_.Append(trajectory_id, imu_data);
|
||||
}
|
||||
|
||||
void OptimizationProblem::AddOdometerData(
|
||||
|
@ -101,18 +98,8 @@ void OptimizationProblem::InsertTrajectoryNode(
|
|||
}
|
||||
|
||||
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
|
||||
imu_data_.Trim(node_data_, node_id);
|
||||
node_data_.Trim(node_id);
|
||||
|
||||
const int trajectory_id = node_id.trajectory_id;
|
||||
if (node_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 &&
|
||||
trajectory_id < static_cast<int>(imu_data_.size())) {
|
||||
const common::Time node_time =
|
||||
node_data_.BeginOfTrajectory(trajectory_id)->data.time;
|
||||
auto& imu_data = imu_data_.at(trajectory_id);
|
||||
while (imu_data.size() > 1 && imu_data[1].time <= node_time) {
|
||||
imu_data.pop_front();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void OptimizationProblem::AddSubmap(const int trajectory_id,
|
||||
|
@ -233,8 +220,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
|
|||
|
||||
problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4,
|
||||
new ceres::QuaternionParameterization());
|
||||
const std::deque<sensor::ImuData>& imu_data = imu_data_.at(trajectory_id);
|
||||
CHECK(!imu_data.empty());
|
||||
CHECK(imu_data_.HasTrajectory(trajectory_id));
|
||||
const auto imu_data = imu_data_.trajectory(trajectory_id);
|
||||
CHECK(imu_data.begin() != imu_data.end());
|
||||
|
||||
auto imu_it = imu_data.begin();
|
||||
auto prev_node_it = node_it;
|
||||
|
|
|
@ -32,6 +32,7 @@
|
|||
#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
|
||||
#include "cartographer/sensor/fixed_frame_pose_data.h"
|
||||
#include "cartographer/sensor/imu_data.h"
|
||||
#include "cartographer/sensor/map_by_time.h"
|
||||
#include "cartographer/sensor/odometry_data.h"
|
||||
#include "cartographer/transform/transform_interpolation_buffer.h"
|
||||
|
||||
|
@ -100,7 +101,7 @@ class OptimizationProblem {
|
|||
|
||||
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
|
||||
FixZ fix_z_;
|
||||
std::vector<std::deque<sensor::ImuData>> imu_data_;
|
||||
sensor::MapByTime<sensor::ImuData> imu_data_;
|
||||
mapping::MapById<mapping::NodeId, NodeData> node_data_;
|
||||
std::vector<transform::TransformInterpolationBuffer> odometry_data_;
|
||||
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
|
||||
|
|
|
@ -0,0 +1,156 @@
|
|||
/*
|
||||
* Copyright 2017 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.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_SENSOR_MAP_BY_TIME_H_
|
||||
#define CARTOGRAPHER_SENSOR_MAP_BY_TIME_H_
|
||||
|
||||
#include <algorithm>
|
||||
#include <iterator>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/mapping/id.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace sensor {
|
||||
|
||||
// 'DataType' must contain a 'time' member of type common::Time.
|
||||
template <typename DataType>
|
||||
class MapByTime {
|
||||
public:
|
||||
// Appends data to a 'trajectory_id', creating trajectories as needed.
|
||||
void Append(const int trajectory_id, const DataType& data) {
|
||||
CHECK_GE(trajectory_id, 0);
|
||||
auto& trajectory = data_[trajectory_id];
|
||||
if (!trajectory.empty()) {
|
||||
CHECK_GT(data.time, std::prev(trajectory.end())->first);
|
||||
}
|
||||
trajectory.emplace(data.time, data);
|
||||
}
|
||||
|
||||
// Removes data no longer needed once 'node_id' gets removed from 'nodes'.
|
||||
// 'NodeType' must contain a 'time' member of type common::Time.
|
||||
template <typename NodeType>
|
||||
void Trim(const mapping::MapById<mapping::NodeId, NodeType>& nodes,
|
||||
const mapping::NodeId& node_id) {
|
||||
const int trajectory_id = node_id.trajectory_id;
|
||||
CHECK_GE(trajectory_id, 0);
|
||||
|
||||
// Data only important between 'gap_start' and 'gap_end' is no longer
|
||||
// needed. We retain the first and last data of the gap so that
|
||||
// interpolation with the adjacent data outside the gap is still possible.
|
||||
const auto node_it = nodes.find(node_id);
|
||||
CHECK(node_it != nodes.end());
|
||||
const common::Time gap_start =
|
||||
node_it != nodes.BeginOfTrajectory(trajectory_id)
|
||||
? std::prev(node_it)->data.time
|
||||
: common::Time::min();
|
||||
const auto next_it = std::next(node_it);
|
||||
const common::Time gap_end = next_it != nodes.EndOfTrajectory(trajectory_id)
|
||||
? next_it->data.time
|
||||
: common::Time::max();
|
||||
CHECK_LT(gap_start, gap_end);
|
||||
|
||||
auto& trajectory = data_[trajectory_id];
|
||||
auto data_it = trajectory.lower_bound(gap_start);
|
||||
auto data_end = trajectory.upper_bound(gap_end);
|
||||
if (data_it == data_end) {
|
||||
return;
|
||||
}
|
||||
if (gap_end != common::Time::max()) {
|
||||
// Retain the last data inside the gap.
|
||||
data_end = std::prev(data_end);
|
||||
if (data_it == data_end) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
if (gap_start != common::Time::min()) {
|
||||
// Retain the first data inside the gap.
|
||||
data_it = std::next(data_it);
|
||||
}
|
||||
while (data_it != data_end) {
|
||||
data_it = trajectory.erase(data_it);
|
||||
}
|
||||
if (trajectory.empty()) {
|
||||
data_.erase(trajectory_id);
|
||||
}
|
||||
}
|
||||
|
||||
bool HasTrajectory(const int trajectory_id) const {
|
||||
return data_.count(trajectory_id) != 0;
|
||||
}
|
||||
|
||||
class ConstIterator {
|
||||
public:
|
||||
using iterator_category = std::bidirectional_iterator_tag;
|
||||
using value_type = DataType;
|
||||
using difference_type = int64;
|
||||
using pointer = const DataType*;
|
||||
using reference = const DataType&;
|
||||
|
||||
explicit ConstIterator(
|
||||
typename std::map<common::Time, DataType>::const_iterator iterator)
|
||||
: iterator_(iterator) {}
|
||||
|
||||
const DataType& operator*() const { return iterator_->second; }
|
||||
|
||||
const DataType* operator->() const { return &iterator_->second; }
|
||||
|
||||
ConstIterator& operator++() {
|
||||
++iterator_;
|
||||
return *this;
|
||||
}
|
||||
|
||||
ConstIterator& operator--() {
|
||||
--iterator_;
|
||||
return *this;
|
||||
}
|
||||
|
||||
bool operator==(const ConstIterator& it) const {
|
||||
return iterator_ == it.iterator_;
|
||||
}
|
||||
|
||||
bool operator!=(const ConstIterator& it) const { return !operator==(it); }
|
||||
|
||||
private:
|
||||
typename std::map<common::Time, DataType>::const_iterator iterator_;
|
||||
};
|
||||
|
||||
ConstIterator BeginOfTrajectory(const int trajectory_id) const {
|
||||
return ConstIterator(data_.at(trajectory_id).begin());
|
||||
}
|
||||
|
||||
ConstIterator EndOfTrajectory(const int trajectory_id) const {
|
||||
return ConstIterator(data_.at(trajectory_id).end());
|
||||
}
|
||||
|
||||
mapping::Range<ConstIterator> trajectory(const int trajectory_id) const {
|
||||
return mapping::Range<ConstIterator>(BeginOfTrajectory(trajectory_id),
|
||||
EndOfTrajectory(trajectory_id));
|
||||
}
|
||||
|
||||
private:
|
||||
std::map<int, std::map<common::Time, DataType>> data_;
|
||||
};
|
||||
|
||||
} // namespace sensor
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_SENSOR_MAP_BY_TIME_H_
|
|
@ -0,0 +1,93 @@
|
|||
/*
|
||||
* Copyright 2017 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/sensor/map_by_time.h"
|
||||
|
||||
#include <deque>
|
||||
|
||||
#include "cartographer/common/time.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace sensor {
|
||||
namespace {
|
||||
|
||||
common::Time CreateTime(const int milliseconds) {
|
||||
return common::Time(common::FromMilliseconds(milliseconds));
|
||||
}
|
||||
|
||||
struct Data {
|
||||
common::Time time;
|
||||
};
|
||||
|
||||
struct NodeData {
|
||||
common::Time time;
|
||||
};
|
||||
|
||||
TEST(MapByTimeTest, AppendAndViewTrajectory) {
|
||||
MapByTime<Data> map_by_time;
|
||||
map_by_time.Append(0, Data{CreateTime(10)});
|
||||
map_by_time.Append(42, Data{CreateTime(42)});
|
||||
map_by_time.Append(42, Data{CreateTime(43)});
|
||||
std::deque<Data> expected_data = {Data{CreateTime(42)}, Data{CreateTime(43)}};
|
||||
for (const Data& data : map_by_time.trajectory(42)) {
|
||||
ASSERT_FALSE(expected_data.empty());
|
||||
EXPECT_EQ(expected_data.front().time, data.time);
|
||||
expected_data.pop_front();
|
||||
}
|
||||
EXPECT_TRUE(expected_data.empty());
|
||||
}
|
||||
|
||||
TEST(MapByTimeTest, Trimming) {
|
||||
MapByTime<Data> map_by_time;
|
||||
EXPECT_FALSE(map_by_time.HasTrajectory(42));
|
||||
map_by_time.Append(42, Data{CreateTime(1)});
|
||||
map_by_time.Append(42, Data{CreateTime(41)});
|
||||
map_by_time.Append(42, Data{CreateTime(42)});
|
||||
map_by_time.Append(42, Data{CreateTime(43)});
|
||||
map_by_time.Append(42, Data{CreateTime(47)});
|
||||
map_by_time.Append(42, Data{CreateTime(48)});
|
||||
map_by_time.Append(42, Data{CreateTime(49)});
|
||||
map_by_time.Append(42, Data{CreateTime(5000)});
|
||||
EXPECT_TRUE(map_by_time.HasTrajectory(42));
|
||||
// Trim one node.
|
||||
mapping::MapById<mapping::NodeId, NodeData> map_by_id;
|
||||
map_by_id.Append(42, NodeData{CreateTime(42)});
|
||||
map_by_id.Append(42, NodeData{CreateTime(46)});
|
||||
map_by_id.Append(42, NodeData{CreateTime(48)});
|
||||
map_by_time.Trim(map_by_id, mapping::NodeId{42, 1});
|
||||
map_by_id.Trim(mapping::NodeId{42, 1});
|
||||
ASSERT_TRUE(map_by_time.HasTrajectory(42));
|
||||
std::deque<Data> expected_data = {
|
||||
Data{CreateTime(1)}, Data{CreateTime(41)}, Data{CreateTime(42)},
|
||||
Data{CreateTime(48)}, Data{CreateTime(49)}, Data{CreateTime(5000)}};
|
||||
for (const Data& data : map_by_time.trajectory(42)) {
|
||||
ASSERT_FALSE(expected_data.empty());
|
||||
EXPECT_EQ(expected_data.front().time, data.time);
|
||||
expected_data.pop_front();
|
||||
}
|
||||
EXPECT_TRUE(expected_data.empty());
|
||||
// Trim everything.
|
||||
map_by_time.Trim(map_by_id, mapping::NodeId{42, 2});
|
||||
map_by_id.Trim(mapping::NodeId{42, 2});
|
||||
map_by_time.Trim(map_by_id, mapping::NodeId{42, 0});
|
||||
map_by_id.Trim(mapping::NodeId{42, 0});
|
||||
EXPECT_FALSE(map_by_time.HasTrajectory(42));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace sensor
|
||||
} // namespace cartographer
|
Loading…
Reference in New Issue