12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Introduces mapping::MapById in the 2D pose graph for submaps. (#578) 

PAIR=cschuet
master
Wolfgang Hess 2017-10-12 11:58:59 +02:00 committed by GitHub
parent ea55e837d2
commit cb41777b9e
7 changed files with 107 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
cartographer/mapping/id.h
View File

@ -19,11 +19,13 @@
#include <algorithm>
#include <iterator>
#include <limits>
#include <map>
#include <ostream>
#include <tuple>
#include <vector>
#include "cartographer/common/port.h"
#include "glog/logging.h"
namespace cartographer {
@ -109,7 +111,8 @@ class NestedVectorsById {
std::vector<std::vector<ValueType>> data_;
};
// Like std::map, but indexed by 'IdType' which can be 'NodeId' or 'SubmapId'.
// Reminiscent of std::map, but indexed by 'IdType' which can be 'NodeId' or
// 'SubmapId'.
template <typename IdType, typename DataType>
class MapById {
private:
@ -121,25 +124,24 @@ class MapById {
const DataType& data;
};
class ConstIterator
: public std::iterator<std::forward_iterator_tag, IdDataReference> {
class ConstIterator {
public:
explicit ConstIterator(const MapById& map_by_id)
: current_trajectory_(map_by_id.trajectories_.begin()),
using iterator_category = std::bidirectional_iterator_tag;
using value_type = IdDataReference;
using difference_type = int64;
using pointer = const IdDataReference*;
using reference = const IdDataReference&;
explicit ConstIterator(const MapById& map_by_id, const int trajectory_id)
: current_trajectory_(
map_by_id.trajectories_.lower_bound(trajectory_id)),
end_trajectory_(map_by_id.trajectories_.end()) {
if (current_trajectory_ != end_trajectory_) {
current_data_ = current_trajectory_->second.data_.begin();
end_data_ = current_trajectory_->second.data_.end();
AdvanceToValidDataIterator();
}
}
static ConstIterator EndIterator(const MapById& map_by_id) {
auto it = ConstIterator(map_by_id);
it.current_trajectory_ = it.end_trajectory_;
return it;
}
IdDataReference operator*() const {
CHECK(current_trajectory_ != end_trajectory_);
return IdDataReference{
@ -154,6 +156,16 @@ class MapById {
return *this;
}
ConstIterator& operator--() {
while (current_trajectory_ == end_trajectory_ ||
current_data_ == current_trajectory_->second.data_.begin()) {
--current_trajectory_;
current_data_ = current_trajectory_->second.data_.end();
}
--current_data_;
return *this;
}
bool operator==(const ConstIterator& it) const {
if (current_trajectory_ == end_trajectory_ ||
it.current_trajectory_ == it.end_trajectory_) {
@ -168,20 +180,18 @@ class MapById {
private:
void AdvanceToValidDataIterator() {
CHECK(current_trajectory_ != end_trajectory_);
while (current_data_ == end_data_) {
while (current_data_ == current_trajectory_->second.data_.end()) {
++current_trajectory_;
if (current_trajectory_ == end_trajectory_) {
return;
}
current_data_ = current_trajectory_->second.data_.begin();
end_data_ = current_trajectory_->second.data_.end();
}
}
typename std::map<int, MapByIndex>::const_iterator current_trajectory_;
typename std::map<int, MapByIndex>::const_iterator end_trajectory_;
typename std::map<int, DataType>::const_iterator current_data_;
typename std::map<int, DataType>::const_iterator end_data_;
};
// Appends data to a 'trajectory_id', creating trajectories as needed.
@ -217,6 +227,11 @@ class MapById {
trajectory.data_.erase(it);
}
bool Contains(const IdType& id) const {
return trajectories_.count(id.trajectory_id) != 0 &&
trajectories_.at(id.trajectory_id).data_.count(GetIndex(id)) != 0;
}
const DataType& at(const IdType& id) const {
return trajectories_.at(id.trajectory_id).data_.at(GetIndex(id));
}
@ -225,8 +240,25 @@ class MapById {
return trajectories_.at(id.trajectory_id).data_.at(GetIndex(id));
}
ConstIterator begin() const { return ConstIterator(*this); }
ConstIterator end() const { return ConstIterator::EndIterator(*this); }
// Support querying by trajectory.
ConstIterator BeginOfTrajectory(const int trajectory_id) const {
return ConstIterator(*this, trajectory_id);
}
ConstIterator EndOfTrajectory(const int trajectory_id) const {
return BeginOfTrajectory(trajectory_id + 1);
}
// Returns 0 if 'trajectory_id' does not exist.
size_t SizeOfTrajectoryOrZero(const int trajectory_id) const {
return trajectories_.count(trajectory_id)
? trajectories_.at(trajectory_id).data_.size()
: 0;
}
ConstIterator begin() const { return BeginOfTrajectory(0); }
ConstIterator end() const {
return BeginOfTrajectory(std::numeric_limits<int>::max());
}
bool empty() const { return begin() == end(); }

21
cartographer/mapping/id_test.cc
View File

@ -17,6 +17,7 @@
#include "cartographer/mapping/id.h"
#include <deque>
#include <iterator>
#include <utility>
#include "gtest/gtest.h"
@ -40,6 +41,9 @@ TEST(IdTest, MapByIdIterator) {
map_by_id.Append(42, 3);
map_by_id.Append(0, 0);
map_by_id.Append(0, 1);
EXPECT_EQ(2, (*map_by_id.BeginOfTrajectory(7)).data);
EXPECT_TRUE(std::next(map_by_id.BeginOfTrajectory(7)) ==
map_by_id.EndOfTrajectory(7));
std::deque<std::pair<NodeId, int>> expected_id_data = {
{NodeId{0, 0}, 0},
{NodeId{0, 1}, 1},
@ -55,6 +59,23 @@ TEST(IdTest, MapByIdIterator) {
EXPECT_TRUE(expected_id_data.empty());
}
TEST(IdTest, MapByIdPrevIterator) {
MapById<NodeId, int> map_by_id;
map_by_id.Append(42, 42);
auto it = map_by_id.end();
ASSERT_TRUE(it != map_by_id.begin());
std::advance(it, -1);
EXPECT_TRUE(it == map_by_id.begin());
}
TEST(IdTest, InsertIntoMapById) {
MapById<NodeId, int> map_by_id;
EXPECT_EQ(0, map_by_id.SizeOfTrajectoryOrZero(42));
map_by_id.Append(42, 42);
map_by_id.Insert(NodeId{42, 5}, 42);
EXPECT_EQ(2, map_by_id.SizeOfTrajectoryOrZero(42));
}
} // namespace
} // namespace mapping
} // namespace cartographer

67
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -22,6 +22,7 @@
#include <functional>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <limits>
#include <memory>
#include <sstream>
@ -55,30 +56,27 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
CHECK(!insertion_submaps.empty());
auto submap_data = optimization_problem_.submap_data();
const auto& submap_data = optimization_problem_.submap_data();
if (insertion_submaps.size() == 1) {
// If we don't already have an entry for the first submap, add one.
if (static_cast<size_t>(trajectory_id) >= submap_data.size() ||
submap_data[trajectory_id].empty()) {
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
optimization_problem_.AddSubmap(
trajectory_id,
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0]));
submap_data = optimization_problem_.submap_data();
}
CHECK_EQ(submap_data[trajectory_id].size(), 1);
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
const mapping::SubmapId submap_id{trajectory_id, 0};
CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front());
return {submap_id};
}
CHECK_EQ(2, insertion_submaps.size());
CHECK(!submap_data.at(trajectory_id).empty());
const mapping::SubmapId last_submap_id{
trajectory_id, submap_data.at(trajectory_id).rbegin()->first};
const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
const mapping::SubmapId last_submap_id = (*std::prev(end_it)).id;
if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {
// In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()'
// and 'insertions_submaps.back()' is new.
const auto& first_submap_pose =
submap_data.at(trajectory_id).at(last_submap_id.submap_index).pose;
const auto& first_submap_pose = submap_data.at(last_submap_id).pose;
optimization_problem_.AddSubmap(
trajectory_id,
first_submap_pose *
@ -189,10 +187,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
// submap's trajectory, it suffices to do a match constrained to a local
// search window.
const transform::Rigid2d initial_relative_pose =
optimization_problem_.submap_data()
.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose.inverse() *
optimization_problem_.submap_data().at(submap_id).pose.inverse() *
optimization_problem_.node_data()
.at(node_id.trajectory_id)
.at(node_id.node_index)
@ -250,10 +245,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
constant_data->initial_pose *
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
const transform::Rigid2d optimized_pose =
optimization_problem_.submap_data()
.at(matching_id.trajectory_id)
.at(matching_id.submap_index)
.pose *
optimization_problem_.submap_data().at(matching_id).pose *
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front())
.inverse() *
pose;
@ -428,14 +420,9 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
submap_data_.Append(trajectory_id, SubmapData());
submap_data_.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the optimized pose.
CHECK_GE(static_cast<size_t>(submap_data_.num_trajectories()),
optimized_submap_transforms_.size());
optimized_submap_transforms_.resize(submap_data_.num_trajectories());
CHECK_EQ(optimized_submap_transforms_.at(trajectory_id).size(),
submap_id.submap_index);
optimized_submap_transforms_.at(trajectory_id)
.emplace(submap_id.submap_index,
sparse_pose_graph::SubmapData{initial_pose_2d});
CHECK_EQ(submap_id,
optimized_submap_transforms_.Append(
trajectory_id, sparse_pose_graph::SubmapData{initial_pose_2d}));
AddWorkItem([this, submap_id, initial_pose_2d]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
submap_data_.at(submap_id).state = SubmapState::kFinished;
@ -500,7 +487,7 @@ void SparsePoseGraph::RunOptimization() {
optimization_problem_.Solve(constraints_, frozen_trajectories_);
common::MutexLocker locker(&mutex_);
const auto submap_data = optimization_problem_.submap_data();
const auto& submap_data = optimization_problem_.submap_data();
const auto& node_data = optimization_problem_.node_data();
for (int trajectory_id = 0;
trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) {
@ -601,19 +588,18 @@ SparsePoseGraph::GetAllSubmapData() {
}
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
const std::vector<std::map<int, sparse_pose_graph::SubmapData>>&
const mapping::MapById<mapping::SubmapId, sparse_pose_graph::SubmapData>&
submap_transforms,
const int trajectory_id) const {
if (trajectory_id >= static_cast<int>(submap_transforms.size()) ||
submap_transforms.at(trajectory_id).empty()) {
auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id);
auto end_it = submap_transforms.EndOfTrajectory(trajectory_id);
if (begin_it == end_it) {
return transform::Rigid3d::Identity();
}
const int submap_index = submap_transforms.at(trajectory_id).rbegin()->first;
const mapping::SubmapId last_optimized_submap_id{trajectory_id, submap_index};
const mapping::SubmapId last_optimized_submap_id = (*std::prev(end_it)).id;
// Accessing 'local_pose' in Submap is okay, since the member is const.
return transform::Embed3D(
submap_transforms.at(trajectory_id).at(submap_index).pose) *
submap_transforms.at(last_optimized_submap_id).pose) *
submap_data_.at(last_optimized_submap_id)
.submap->local_pose()
.inverse();
@ -625,16 +611,10 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
return {};
}
auto submap = submap_data_.at(submap_id).submap;
if (submap_id.trajectory_id <
static_cast<int>(optimized_submap_transforms_.size()) &&
submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
.at(submap_id.trajectory_id)
.size())) {
if (optimized_submap_transforms_.Contains(submap_id)) {
// We already have an optimized pose.
return {submap, transform::Embed3D(
optimized_submap_transforms_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose)};
optimized_submap_transforms_.at(submap_id).pose)};
}
// We have to extrapolate.
return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,
@ -647,7 +627,8 @@ SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
int SparsePoseGraph::TrimmingHandle::num_submaps(
const int trajectory_id) const {
return parent_->optimization_problem_.submap_data().at(trajectory_id).size();
const auto& submap_data = parent_->optimization_problem_.submap_data();
return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
}
void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(

4
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -161,7 +161,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Computes the local to global frame transform based on the given optimized
// 'submap_transforms'.
transform::Rigid3d ComputeLocalToGlobalTransform(
const std::vector<std::map<int, sparse_pose_graph::SubmapData>>&
const mapping::MapById<mapping::SubmapId, sparse_pose_graph::SubmapData>&
submap_transforms,
int trajectory_id) const REQUIRES(mutex_);
@ -214,7 +214,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Current submap transforms used for displaying data.
std::vector<std::map<int, sparse_pose_graph::SubmapData>>
mapping::MapById<mapping::SubmapId, sparse_pose_graph::SubmapData>
optimized_submap_transforms_ GUARDED_BY(mutex_);
// List of all trimmers to consult when optimizations finish.

12
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
View File

@ -255,15 +255,9 @@ const std::vector<std::map<int, NodeData>>& OptimizationProblem::node_data()
return node_data_;
}
std::vector<std::map<int, SubmapData>> OptimizationProblem::submap_data()
const {
std::vector<std::map<int, SubmapData>> result;
for (const auto& submap_id_data : submap_data_) {
result.resize(submap_id_data.id.trajectory_id + 1);
result[submap_id_data.id.trajectory_id].emplace(
submap_id_data.id.submap_index, submap_id_data.data);
}
return result;
const mapping::MapById<mapping::SubmapId, SubmapData>&
OptimizationProblem::submap_data() const {
return submap_data_;
}
} // namespace sparse_pose_graph

2
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
View File

@ -80,7 +80,7 @@ class OptimizationProblem {
const std::set<int>& frozen_trajectories);
const std::vector<std::map<int, NodeData>>& node_data() const;
std::vector<std::map<int, SubmapData>> submap_data() const;
const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
private:
struct TrajectoryData {

9
cartographer/sensor/compressed_point_cloud.h
View File

@ -58,9 +58,14 @@ class CompressedPointCloud {
};
// Forward iterator for compressed point clouds.
class CompressedPointCloud::ConstIterator
: public std::iterator<std::forward_iterator_tag, Eigen::Vector3f> {
class CompressedPointCloud::ConstIterator {
public:
using iterator_category = std::forward_iterator_tag;
using value_type = Eigen::Vector3f;
using difference_type = int64;
using pointer = const Eigen::Vector3f*;
using reference = const Eigen::Vector3f&;
// Creates begin iterator.
explicit ConstIterator(const CompressedPointCloud* compressed_point_cloud);