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

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,21 +83,24 @@ 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;
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);
}
}
}
return proto;
}

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

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

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_;

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

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);

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 {

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()),

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);

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 {