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

Remove flat indices from SparsePoseGraph::GetTrajectoryNodes(). (#288) 

Related to #256.

Also removes ComputeMapLimits with is moved to Cartographer ROS.

PAIR=SirVer
master
Wolfgang Hess 2017-05-16 10:51:08 +02:00 committed by GitHub
parent 690d1893a7
commit 5effc4dac7
10 changed files with 48 additions and 238 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
cartographer/mapping/sparse_pose_graph.cc
View File

@ -36,23 +36,6 @@ proto::SparsePoseGraph::Constraint::Tag ToProto(
LOG(FATAL) << "Unsupported tag.";
}
void GroupTrajectoryNodes(
const std::vector<TrajectoryNode>& trajectory_nodes,
const std::unordered_map<const Submaps*, int>& trajectory_ids,
std::vector<std::vector<TrajectoryNode>>* grouped_nodes,
std::vector<std::pair<int, int>>* new_indices) {
CHECK_NOTNULL(grouped_nodes)->clear();
CHECK_NOTNULL(new_indices)->clear();
grouped_nodes->resize(trajectory_ids.size());
for (const auto& node : trajectory_nodes) {
const int id = trajectory_ids.at(node.constant_data->trajectory);
new_indices->emplace_back(id, (*grouped_nodes)[id].size());
(*grouped_nodes)[id].push_back(node);
}
}
proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::SparsePoseGraphOptions options;
@ -75,11 +58,6 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
proto::SparsePoseGraph SparsePoseGraph::ToProto() {
proto::SparsePoseGraph proto;
std::vector<std::vector<TrajectoryNode>> grouped_nodes;
std::vector<std::pair<int, int>> grouped_node_indices;
GroupTrajectoryNodes(GetTrajectoryNodes(), trajectory_ids(), &grouped_nodes,
&grouped_node_indices);
for (const auto& constraint : constraints()) {
auto* const constraint_proto = proto.add_constraint();
*constraint_proto->mutable_relative_pose() =
@ -105,19 +83,22 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
constraint_proto->set_tag(mapping::ToProto(constraint.tag));
}
for (const auto& group : grouped_nodes) {
for (const auto& trajectory_nodes : GetTrajectoryNodes()) {
auto* trajectory_proto = proto.add_trajectory();
for (const auto& node : group) {
for (const auto& node : trajectory_nodes) {
auto* node_proto = trajectory_proto->add_node();
node_proto->set_timestamp(common::ToUniversal(node.constant_data->time));
*node_proto->mutable_pose() =
transform::ToProto(node.pose * node.constant_data->tracking_to_pose);
}
const Submaps* const trajectory = group[0].constant_data->trajectory;
for (const auto& transform : GetSubmapTransforms(trajectory)) {
*trajectory_proto->add_submap()->mutable_pose() =
transform::ToProto(transform);
if (!trajectory_nodes.empty()) {
const Submaps* const trajectory =
trajectory_nodes[0].constant_data->trajectory;
for (const auto& transform : GetSubmapTransforms(trajectory)) {
*trajectory_proto->add_submap()->mutable_pose() =
transform::ToProto(transform);
}
}
}

20
cartographer/mapping/sparse_pose_graph.h
View File

@ -36,18 +36,6 @@ namespace mapping {
proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
common::LuaParameterDictionary* const parameter_dictionary);
// TrajectoryNodes are provided in a flat vector, but serialization requires
// that we group them by trajectory. This groups the elements of
// 'trajectory_nodes' into 'grouped_nodes' (so that (*grouped_nodes)[i]
// contains a complete single trajectory). The re-indexing done is stored in
// 'new_indices', such that 'trajectory_nodes[i]' landed in
// '(*grouped_nodes)[new_indices[i].first][new_indices[i].second]'.
void GroupTrajectoryNodes(
const std::vector<TrajectoryNode>& trajectory_nodes,
const std::unordered_map<const Submaps*, int>& trajectory_ids,
std::vector<std::vector<TrajectoryNode>>* grouped_nodes,
std::vector<std::pair<int, int>>* new_indices);
class SparsePoseGraph {
public:
// A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse
@ -93,18 +81,14 @@ class SparsePoseGraph {
virtual transform::Rigid3d GetLocalToGlobalTransform(
const Submaps* submaps) = 0;
// Returns the current optimized trajectory.
virtual std::vector<TrajectoryNode> GetTrajectoryNodes() = 0;
// Returns the current optimized trajectories.
virtual std::vector<std::vector<TrajectoryNode>> GetTrajectoryNodes() = 0;
// Serializes the constraints and trajectories.
proto::SparsePoseGraph ToProto();
// Returns the collection of constraints.
virtual std::vector<Constraint> constraints() = 0;
protected:
// Returns the mapping from Submaps* to trajectory IDs.
virtual const std::unordered_map<const Submaps*, int>& trajectory_ids() = 0;
};
} // namespace mapping

87
cartographer/mapping/sparse_pose_graph_test.cc
View File

@ -1,87 +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/sparse_pose_graph.h"
#include <algorithm>
#include <deque>
#include <vector>
#include "glog/logging.h"
#include "gmock/gmock.h"
namespace cartographer {
namespace mapping {
namespace {
class FakeSubmaps : public Submaps {
public:
~FakeSubmaps() override {}
const Submap* Get(int) const override { LOG(FATAL) << "Not implemented."; }
int size() const override { LOG(FATAL) << "Not implemented."; }
void SubmapToProto(int, const transform::Rigid3d&,
proto::SubmapQuery::Response*) const override {
LOG(FATAL) << "Not implemented.";
}
};
TEST(SparsePoseGraphTest, TrajectoryFunctions) {
std::vector<TrajectoryNode> trajectory_nodes;
const std::vector<FakeSubmaps> submaps(5);
std::deque<TrajectoryNode::ConstantData> constant_data;
constexpr int kNumTrajectoryNodes = 10;
for (int j = 0; j < kNumTrajectoryNodes; ++j) {
for (size_t i = 0; i < submaps.size(); ++i) {
constant_data.push_back({});
constant_data.back().trajectory = &submaps[i];
TrajectoryNode node;
node.constant_data = &constant_data.back();
trajectory_nodes.push_back(node);
}
}
const std::unordered_map<const Submaps*, int> index_map = {{&submaps[0], 0},
{&submaps[1], 1},
{&submaps[2], 2},
{&submaps[3], 3},
{&submaps[4], 4}};
std::vector<std::vector<TrajectoryNode>> grouped_nodes;
std::vector<std::pair<int, int>> new_indices;
GroupTrajectoryNodes(trajectory_nodes, index_map, &grouped_nodes,
&new_indices);
ASSERT_EQ(grouped_nodes.size(), submaps.size());
for (size_t i = 0; i < submaps.size(); ++i) {
EXPECT_EQ(grouped_nodes[i].size(), kNumTrajectoryNodes);
for (const auto& node : grouped_nodes[i]) {
EXPECT_EQ(node.constant_data->trajectory, &submaps[i]);
}
}
ASSERT_EQ(trajectory_nodes.size(), new_indices.size());
for (size_t i = 0; i < new_indices.size(); ++i) {
const auto index_pair = new_indices[i];
EXPECT_EQ(trajectory_nodes[i].constant_data->trajectory,
grouped_nodes[index_pair.first][index_pair.second]
.constant_data->trajectory);
}
}
} // namespace
} // namespace mapping
} // namespace cartographer

45
cartographer/mapping_2d/map_limits.h
View File

@ -84,51 +84,6 @@ class MapLimits {
.all();
}
// Computes MapLimits that contain the origin, and all rays (both returns and
// misses) in the 'trajectory'.
static MapLimits ComputeMapLimits(
const double resolution,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
Eigen::AlignedBox2f bounding_box = ComputeMapBoundingBox(trajectory_nodes);
// Add some padding to ensure all rays are still contained in the map after
// discretization.
const float kPadding = 3.f * resolution;
bounding_box.min() -= kPadding * Eigen::Vector2f::Ones();
bounding_box.max() += kPadding * Eigen::Vector2f::Ones();
const Eigen::Vector2d pixel_sizes =
bounding_box.sizes().cast<double>() / resolution;
return MapLimits(resolution, bounding_box.max().cast<double>(),
CellLimits(common::RoundToInt(pixel_sizes.y()),
common::RoundToInt(pixel_sizes.x())));
}
static Eigen::AlignedBox2f ComputeMapBoundingBox(
const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
Eigen::AlignedBox2f bounding_box(Eigen::Vector2f::Zero());
for (const auto& node : trajectory_nodes) {
const auto& data = *node.constant_data;
if (!data.range_data_3d.returns.empty()) {
const sensor::RangeData range_data = sensor::TransformRangeData(
Decompress(data.range_data_3d), node.pose.cast<float>());
bounding_box.extend(range_data.origin.head<2>());
for (const Eigen::Vector3f& hit : range_data.returns) {
bounding_box.extend(hit.head<2>());
}
} else {
const sensor::RangeData range_data = sensor::TransformRangeData(
data.range_data_2d, node.pose.cast<float>());
bounding_box.extend(range_data.origin.head<2>());
for (const Eigen::Vector3f& hit : range_data.returns) {
bounding_box.extend(hit.head<2>());
}
for (const Eigen::Vector3f& miss : range_data.misses) {
bounding_box.extend(miss.head<2>());
}
}
}
return bounding_box;
}
private:
double resolution_;
Eigen::Vector2d max_;

23
cartographer/mapping_2d/map_limits_test.cc
View File

@ -62,29 +62,6 @@ TEST(MapLimitsTest, ConstructAndGet) {
EXPECT_EQ(42., limits.resolution());
}
TEST(MapLimitsTest, ComputeMapLimits) {
const mapping::TrajectoryNode::ConstantData constant_data{
common::FromUniversal(52),
sensor::RangeData{
Eigen::Vector3f::Zero(),
{Eigen::Vector3f(-30.f, 1.f, 0.f), Eigen::Vector3f(50.f, -10.f, 0.f)},
{}},
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), nullptr,
transform::Rigid3d::Identity()};
const mapping::TrajectoryNode trajectory_node{&constant_data,
transform::Rigid3d::Identity()};
constexpr double kResolution = 0.05;
const MapLimits limits =
MapLimits::ComputeMapLimits(kResolution, {trajectory_node});
constexpr float kPaddingAwareTolerance = 5 * kResolution;
EXPECT_NEAR(50.f, limits.max().x(), kPaddingAwareTolerance);
EXPECT_NEAR(1.f, limits.max().y(), kPaddingAwareTolerance);
EXPECT_LT(200, limits.cell_limits().num_x_cells);
EXPECT_LT(1600, limits.cell_limits().num_y_cells);
EXPECT_GT(400, limits.cell_limits().num_x_cells);
EXPECT_GT(2000, limits.cell_limits().num_y_cells);
}
} // namespace
} // namespace mapping_2d
} // namespace cartographer

17
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -424,9 +424,16 @@ void SparsePoseGraph::RunOptimization() {
}
}
std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
std::vector<std::vector<mapping::TrajectoryNode>>
SparsePoseGraph::GetTrajectoryNodes() {
common::MutexLocker locker(&mutex_);
return trajectory_nodes_;
std::vector<std::vector<mapping::TrajectoryNode>> result(
trajectory_ids_.size());
for (const auto& node : trajectory_nodes_) {
result.at(trajectory_ids_.at(node.constant_data->trajectory))
.push_back(node);
}
return result;
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
@ -434,12 +441,6 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
return constraints_;
}
const std::unordered_map<const mapping::Submaps*, int>&
SparsePoseGraph::trajectory_ids() {
common::MutexLocker locker(&mutex_);
return trajectory_ids_;
}
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
const mapping::Submaps* const submaps) {
const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps);

8
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -86,13 +86,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
EXCLUDES(mutex_) override;
std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
protected:
const std::unordered_map<const mapping::Submaps*, int>& trajectory_ids()
std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
override EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
private:
struct SubmapState {

20
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -189,12 +189,13 @@ TEST_F(SparsePoseGraphTest, NoMovement) {
MoveRelative(transform::Rigid2d::Identity());
sparse_pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
EXPECT_THAT(nodes.size(), ::testing::Eq(3));
EXPECT_THAT(nodes[0].pose,
ASSERT_THAT(nodes.size(), ::testing::Eq(1));
EXPECT_THAT(nodes[0].size(), ::testing::Eq(3));
EXPECT_THAT(nodes[0][0].pose,
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
EXPECT_THAT(nodes[1].pose,
EXPECT_THAT(nodes[0][1].pose,
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
EXPECT_THAT(nodes[2].pose,
EXPECT_THAT(nodes[0][2].pose,
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
}
@ -208,8 +209,10 @@ TEST_F(SparsePoseGraphTest, NoOverlappingScans) {
}
sparse_pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
ASSERT_THAT(nodes.size(), ::testing::Eq(1));
for (int i = 0; i != 4; ++i) {
EXPECT_THAT(poses[i], IsNearly(transform::Project2D(nodes[i].pose), 1e-2))
EXPECT_THAT(poses[i],
IsNearly(transform::Project2D(nodes[0][i].pose), 1e-2))
<< i;
}
}
@ -224,8 +227,10 @@ TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingScans) {
}
sparse_pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
ASSERT_THAT(nodes.size(), ::testing::Eq(1));
for (int i = 0; i != 5; ++i) {
EXPECT_THAT(poses[i], IsNearly(transform::Project2D(nodes[i].pose), 1e-2))
EXPECT_THAT(poses[i],
IsNearly(transform::Project2D(nodes[0][i].pose), 1e-2))
<< i;
}
}
@ -247,12 +252,13 @@ TEST_F(SparsePoseGraphTest, OverlappingScans) {
}
sparse_pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
ASSERT_THAT(nodes.size(), ::testing::Eq(1));
transform::Rigid2d true_movement =
ground_truth.front().inverse() * ground_truth.back();
transform::Rigid2d movement_before = poses.front().inverse() * poses.back();
transform::Rigid2d error_before = movement_before.inverse() * true_movement;
transform::Rigid3d optimized_movement =
nodes.front().pose.inverse() * nodes.back().pose;
nodes[0].front().pose.inverse() * nodes[0].back().pose;
transform::Rigid2d optimized_error =
transform::Project2D(optimized_movement).inverse() * true_movement;
EXPECT_THAT(std::abs(optimized_error.normalized_angle()),

17
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -415,9 +415,16 @@ void SparsePoseGraph::RunOptimization() {
}
}
std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
std::vector<std::vector<mapping::TrajectoryNode>>
SparsePoseGraph::GetTrajectoryNodes() {
common::MutexLocker locker(&mutex_);
return trajectory_nodes_;
std::vector<std::vector<mapping::TrajectoryNode>> result(
trajectory_ids_.size());
for (const auto& node : trajectory_nodes_) {
result.at(trajectory_ids_.at(node.constant_data->trajectory))
.push_back(node);
}
return result;
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
@ -425,12 +432,6 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
return constraints_;
}
const std::unordered_map<const mapping::Submaps*, int>&
SparsePoseGraph::trajectory_ids() {
common::MutexLocker locker(&mutex_);
return trajectory_ids_;
}
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
const mapping::Submaps* const submaps) {
const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps);

12
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -73,8 +73,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const std::vector<const Submap*>& insertion_submaps)
EXCLUDES(mutex_);
// The index into the vector of trajectory nodes as used with
// GetTrajectoryNodes() for the next node added with AddScan() is returned.
// The index into the flat vector of trajectory nodes used internally for the
// next node added with AddScan() is returned.
int GetNextTrajectoryNodeIndex() EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization.
@ -88,13 +88,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
EXCLUDES(mutex_) override;
std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
protected:
const std::unordered_map<const mapping::Submaps*, int>& trajectory_ids()
std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
override EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
private:
struct SubmapState {