Add overlapping submaps trimmer. (#1027)
Trims submaps that have less than 'min_covered_cells_count' cells not overlapped by at least 'fresh_submaps_count` submaps.master
Alexander Belyaev
GitHub
parent
e2966ca156
commit
771336b3c9
|
|
@ -331,6 +331,15 @@ class MapById {
|
|||
: 0;
|
||||
}
|
||||
|
||||
// Returns count of all elements.
|
||||
size_t size() const {
|
||||
size_t size = 0;
|
||||
for (const auto& item : trajectories_) {
|
||||
size += item.second.data_.size();
|
||||
}
|
||||
return size;
|
||||
}
|
||||
|
||||
// Returns Range object for range-based loops over the nodes of a trajectory.
|
||||
Range<ConstIterator> trajectory(const int trajectory_id) const {
|
||||
return Range<ConstIterator>(BeginOfTrajectory(trajectory_id),
|
||||
|
|
|
|||
|
|
@ -60,10 +60,12 @@ TEST(IdTest, EmptyMapById) {
|
|||
EXPECT_FALSE(map_by_id.empty());
|
||||
map_by_id.Trim(id);
|
||||
EXPECT_TRUE(map_by_id.empty());
|
||||
EXPECT_EQ(0, map_by_id.size());
|
||||
}
|
||||
|
||||
TEST(IdTest, MapByIdIterator) {
|
||||
MapById<NodeId, int> map_by_id = CreateTestMapById<NodeId>();
|
||||
EXPECT_EQ(4, map_by_id.size());
|
||||
EXPECT_EQ(2, map_by_id.BeginOfTrajectory(7)->data);
|
||||
EXPECT_TRUE(std::next(map_by_id.BeginOfTrajectory(7)) ==
|
||||
map_by_id.EndOfTrajectory(7));
|
||||
|
|
|
|||
|
|
@ -0,0 +1,203 @@
|
|||
/*
|
||||
* Copyright 2018 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/internal/2d/overlapping_submaps_trimmer_2d.h"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
#include "cartographer/mapping/2d/submap_2d.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
namespace {
|
||||
|
||||
class SubmapCoverageGrid2D {
|
||||
public:
|
||||
// Aliases for documentation only (no type-safety).
|
||||
using CellId = std::pair<int64 /* x cells */, int64 /* y cells */>;
|
||||
using StoredType = std::vector<std::pair<SubmapId, common::Time>>;
|
||||
|
||||
explicit SubmapCoverageGrid2D(const Eigen::Vector2d& offset)
|
||||
: offset_(offset) {}
|
||||
|
||||
void AddPoint(const Eigen::Vector2d& point, const SubmapId& submap_id,
|
||||
const common::Time& time) {
|
||||
CellId cell_id{common::RoundToInt64(offset_(0) - point(0)),
|
||||
common::RoundToInt64(offset_(1) - point(1))};
|
||||
cells_[cell_id].emplace_back(submap_id, time);
|
||||
}
|
||||
|
||||
const std::map<CellId, StoredType>& cells() const { return cells_; }
|
||||
|
||||
private:
|
||||
const Eigen::Vector2d offset_;
|
||||
std::map<CellId, StoredType> cells_;
|
||||
};
|
||||
|
||||
Eigen::Vector2d GetCornerOfFirstSubmap(
|
||||
const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data) {
|
||||
auto submap_2d = std::static_pointer_cast<const Submap2D>(
|
||||
submap_data.begin()->data.submap);
|
||||
return submap_2d->probability_grid().limits().max();
|
||||
}
|
||||
|
||||
// Iterates over every cell in a submap, transforms the center of the cell to
|
||||
// the global frame and then adds the submap id and the timestamp of the most
|
||||
// recent range data insertion into the global grid.
|
||||
std::set<SubmapId> AddSubmapsToSubmapCoverageGrid2D(
|
||||
const std::map<SubmapId, common::Time>& submap_freshness,
|
||||
const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data,
|
||||
SubmapCoverageGrid2D* coverage_grid) {
|
||||
std::set<SubmapId> all_submap_ids;
|
||||
|
||||
for (const auto& submap : submap_data) {
|
||||
auto freshness = submap_freshness.find(submap.id);
|
||||
if (freshness == submap_freshness.end()) continue;
|
||||
if (!submap.data.submap->finished()) continue;
|
||||
all_submap_ids.insert(submap.id);
|
||||
const ProbabilityGrid& probability_grid =
|
||||
std::static_pointer_cast<const Submap2D>(submap.data.submap)
|
||||
->probability_grid();
|
||||
// Iterate over every cell in a submap.
|
||||
Eigen::Array2i offset;
|
||||
CellLimits cell_limits;
|
||||
probability_grid.ComputeCroppedLimits(&offset, &cell_limits);
|
||||
if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) {
|
||||
LOG(WARNING) << "Empty grid found in submap ID = " << submap.id;
|
||||
continue;
|
||||
}
|
||||
const transform::Rigid2d projected_submap_pose =
|
||||
transform::Project2D(submap.data.pose);
|
||||
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
|
||||
const Eigen::Array2i index = xy_index + offset;
|
||||
if (!probability_grid.IsKnown(index)) continue;
|
||||
const transform::Rigid2d center_of_cell_in_global_frame =
|
||||
projected_submap_pose *
|
||||
transform::Rigid2d::Translation({index.x() + 0.5, index.y() + 0.5});
|
||||
coverage_grid->AddPoint(center_of_cell_in_global_frame.translation(),
|
||||
submap.id, freshness->second);
|
||||
}
|
||||
}
|
||||
return all_submap_ids;
|
||||
}
|
||||
|
||||
// Uses intra-submap constraints and trajectory node timestamps to identify time
|
||||
// of the last range data insertion to the submap.
|
||||
std::map<SubmapId, common::Time> ComputeSubmapFreshness(
|
||||
const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data,
|
||||
const MapById<NodeId, TrajectoryNode>& trajectory_nodes,
|
||||
const std::vector<PoseGraphInterface::Constraint>& constraints) {
|
||||
std::map<SubmapId, common::Time> submap_freshness;
|
||||
|
||||
// Find the node with the largest NodeId per SubmapId.
|
||||
std::map<SubmapId, NodeId> submap_to_latest_node;
|
||||
for (const PoseGraphInterface::Constraint& constraint : constraints) {
|
||||
if (constraint.tag != PoseGraphInterface::Constraint::INTRA_SUBMAP) {
|
||||
continue;
|
||||
}
|
||||
auto submap_to_node = submap_to_latest_node.find(constraint.submap_id);
|
||||
if (submap_to_node == submap_to_latest_node.end()) {
|
||||
submap_to_latest_node.insert(
|
||||
std::make_pair(constraint.submap_id, constraint.node_id));
|
||||
continue;
|
||||
}
|
||||
submap_to_node->second =
|
||||
std::max(submap_to_node->second, constraint.node_id);
|
||||
}
|
||||
|
||||
// Find timestamp of every latest node.
|
||||
for (const auto& submap_id_to_node_id : submap_to_latest_node) {
|
||||
auto submap_data_item = submap_data.find(submap_id_to_node_id.first);
|
||||
if (submap_data_item == submap_data.end()) {
|
||||
LOG(WARNING) << "Intra-submap constraint between SubmapID = "
|
||||
<< submap_id_to_node_id.first << " and NodeID "
|
||||
<< submap_id_to_node_id.second << " is missing submap data";
|
||||
continue;
|
||||
}
|
||||
auto latest_node_id = trajectory_nodes.find(submap_id_to_node_id.second);
|
||||
if (latest_node_id == trajectory_nodes.end()) continue;
|
||||
submap_freshness[submap_data_item->id] = latest_node_id->data.time();
|
||||
}
|
||||
return submap_freshness;
|
||||
}
|
||||
|
||||
// Returns IDs of submaps that have less than 'min_covered_cells_count' cells
|
||||
// not overlapped by at least 'fresh_submaps_count' submaps.
|
||||
std::vector<SubmapId> FindSubmapIdsToTrim(
|
||||
const SubmapCoverageGrid2D& coverage_grid,
|
||||
const std::set<SubmapId>& all_submap_ids, uint16 fresh_submaps_count,
|
||||
uint16 min_covered_cells_count) {
|
||||
std::map<SubmapId, uint16> cells_covered_by_submap;
|
||||
for (const auto& cell : coverage_grid.cells()) {
|
||||
std::vector<std::pair<SubmapId, common::Time>> submaps_per_cell(
|
||||
cell.second);
|
||||
|
||||
// In case there are several submaps covering the cell, only the freshest
|
||||
// submaps are kept.
|
||||
if (submaps_per_cell.size() > fresh_submaps_count) {
|
||||
// Sort by time in descending order.
|
||||
std::sort(submaps_per_cell.begin(), submaps_per_cell.end(),
|
||||
[](const std::pair<SubmapId, common::Time>& left,
|
||||
const std::pair<SubmapId, common::Time>& right) {
|
||||
return left.second > right.second;
|
||||
});
|
||||
submaps_per_cell.erase(submaps_per_cell.begin() + fresh_submaps_count,
|
||||
submaps_per_cell.end());
|
||||
}
|
||||
for (const std::pair<SubmapId, common::Time>& submap : submaps_per_cell) {
|
||||
++cells_covered_by_submap[submap.first];
|
||||
}
|
||||
}
|
||||
std::vector<SubmapId> submap_ids_to_keep;
|
||||
for (const auto& id_to_cells_count : cells_covered_by_submap) {
|
||||
if (id_to_cells_count.second < min_covered_cells_count) continue;
|
||||
submap_ids_to_keep.push_back(id_to_cells_count.first);
|
||||
}
|
||||
|
||||
DCHECK(std::is_sorted(submap_ids_to_keep.begin(), submap_ids_to_keep.end()));
|
||||
|
||||
std::vector<SubmapId> result;
|
||||
std::set_difference(all_submap_ids.begin(), all_submap_ids.end(),
|
||||
submap_ids_to_keep.begin(), submap_ids_to_keep.end(),
|
||||
std::inserter(result, result.begin()));
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) {
|
||||
const auto submap_data = pose_graph->GetAllSubmapData();
|
||||
if (submap_data.size() == 0) return;
|
||||
|
||||
const auto constraints = pose_graph->GetConstraints();
|
||||
const auto trajectory_nodes = pose_graph->GetTrajectoryNodes();
|
||||
|
||||
SubmapCoverageGrid2D coverage_grid(GetCornerOfFirstSubmap(submap_data));
|
||||
|
||||
const std::set<SubmapId> all_submap_ids = AddSubmapsToSubmapCoverageGrid2D(
|
||||
ComputeSubmapFreshness(submap_data, trajectory_nodes, constraints),
|
||||
submap_data, &coverage_grid);
|
||||
const std::vector<SubmapId> submap_ids_to_remove =
|
||||
FindSubmapIdsToTrim(coverage_grid, all_submap_ids, fresh_submaps_count_,
|
||||
min_covered_cells_count_);
|
||||
for (const SubmapId& id : submap_ids_to_remove) {
|
||||
pose_graph->MarkSubmapAsTrimmed(id);
|
||||
}
|
||||
finished_ = true;
|
||||
}
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,51 @@
|
|||
/*
|
||||
* Copyright 2018 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_MAPPING_INTERNAL_2D_OVERLAPPING_SUBMAPS_TRIMMER_H_
|
||||
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_OVERLAPPING_SUBMAPS_TRIMMER_H_
|
||||
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/mapping/pose_graph_trimmer.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
|
||||
// Trims submaps that have less than 'min_covered_cells_count' cells not
|
||||
// overlapped by at least 'fresh_submaps_count` submaps.
|
||||
class OverlappingSubmapsTrimmer2D : public PoseGraphTrimmer {
|
||||
public:
|
||||
OverlappingSubmapsTrimmer2D(uint16 fresh_submaps_count,
|
||||
uint16 min_covered_cells_count)
|
||||
: fresh_submaps_count_(fresh_submaps_count),
|
||||
min_covered_cells_count_(min_covered_cells_count) {}
|
||||
~OverlappingSubmapsTrimmer2D() override = default;
|
||||
|
||||
void Trim(Trimmable* pose_graph) override;
|
||||
bool IsFinished() override { return finished_; }
|
||||
|
||||
private:
|
||||
// Number of the most recent submaps to keep.
|
||||
const uint16 fresh_submaps_count_;
|
||||
// Minimal number of covered cells to keep submap from trimming.
|
||||
const uint16 min_covered_cells_count_;
|
||||
|
||||
bool finished_ = false;
|
||||
};
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_OVERLAPPING_SUBMAPS_TRIMMER_H_
|
||||
|
|
@ -0,0 +1,187 @@
|
|||
/*
|
||||
* Copyright 2018 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/internal/2d/overlapping_submaps_trimmer_2d.h"
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/mapping/2d/submap_2d.h"
|
||||
#include "cartographer/mapping/id.h"
|
||||
#include "cartographer/mapping/internal/testing/fake_trimmable.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
#include "gmock/gmock.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
namespace {
|
||||
|
||||
using ::cartographer::transform::Rigid2d;
|
||||
using ::cartographer::transform::Rigid3d;
|
||||
using ::testing::ElementsAre;
|
||||
using ::testing::Field;
|
||||
using ::testing::IsEmpty;
|
||||
|
||||
class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
|
||||
protected:
|
||||
// Creates a submap with num_cells x num_cells grid.
|
||||
void AddSquareSubmap(const Rigid2d& pose_2d, int submap_index, int num_cells,
|
||||
bool is_finished) {
|
||||
const Rigid3d pose_3d = transform::Embed3D(pose_2d);
|
||||
proto::Submap2D submap_2d;
|
||||
submap_2d.set_num_range_data(1);
|
||||
submap_2d.set_finished(is_finished);
|
||||
*submap_2d.mutable_local_pose() = transform::ToProto(pose_3d);
|
||||
|
||||
auto* probability_grid = submap_2d.mutable_probability_grid();
|
||||
for (int i = 0; i < num_cells * num_cells; ++i) {
|
||||
probability_grid->add_cells(1);
|
||||
}
|
||||
|
||||
auto* map_limits = probability_grid->mutable_limits();
|
||||
map_limits->set_resolution(1.0);
|
||||
*map_limits->mutable_max() =
|
||||
transform::ToProto(Eigen::Vector2d(num_cells, num_cells));
|
||||
map_limits->mutable_cell_limits()->set_num_x_cells(num_cells);
|
||||
map_limits->mutable_cell_limits()->set_num_y_cells(num_cells);
|
||||
|
||||
auto* know_cells_box = probability_grid->mutable_known_cells_box();
|
||||
know_cells_box->set_min_x(0);
|
||||
know_cells_box->set_min_y(0);
|
||||
know_cells_box->set_max_x(num_cells - 1);
|
||||
know_cells_box->set_max_y(num_cells - 1);
|
||||
|
||||
fake_pose_graph_.mutable_submap_data()->Insert(
|
||||
{0 /* trajectory_id */, submap_index},
|
||||
{std::make_shared<const Submap2D>(submap_2d), pose_3d});
|
||||
}
|
||||
|
||||
void AddTrajectoryNode(int node_index, int64 timestamp) {
|
||||
TrajectoryNode::Data data;
|
||||
data.time = common::FromUniversal(timestamp);
|
||||
|
||||
fake_pose_graph_.mutable_trajectory_nodes()->Insert(
|
||||
NodeId{0 /* trajectory_id */, node_index},
|
||||
{std::make_shared<const TrajectoryNode::Data>(data), {} /* pose */});
|
||||
}
|
||||
|
||||
void AddConstraint(int submap_index, int node_index, bool is_intra_submap) {
|
||||
fake_pose_graph_.mutable_constraints()->push_back(
|
||||
{SubmapId{0 /* trajectory_id */, submap_index},
|
||||
NodeId{0 /* trajectory_id */, node_index},
|
||||
{} /* pose */,
|
||||
is_intra_submap ? PoseGraphInterface::Constraint::INTRA_SUBMAP
|
||||
: PoseGraphInterface::Constraint::INTER_SUBMAP});
|
||||
}
|
||||
|
||||
testing::FakeTrimmable fake_pose_graph_;
|
||||
};
|
||||
|
||||
::testing::Matcher<const SubmapId&> EqualsSubmapId(const SubmapId& expected) {
|
||||
return ::testing::AllOf(
|
||||
Field(&SubmapId::trajectory_id, expected.trajectory_id),
|
||||
Field(&SubmapId::submap_index, expected.submap_index));
|
||||
}
|
||||
|
||||
TEST_F(OverlappingSubmapsTrimmer2DTest, EmptyPoseGraph) {
|
||||
OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
|
||||
0 /* min_covered_cells_count */);
|
||||
trimmer.Trim(&fake_pose_graph_);
|
||||
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
|
||||
}
|
||||
|
||||
TEST_F(OverlappingSubmapsTrimmer2DTest, TrimOneOfTwoOverlappingSubmaps) {
|
||||
AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */,
|
||||
true /* is_finished */);
|
||||
AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */,
|
||||
true /* is_finished */);
|
||||
AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
|
||||
AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
|
||||
AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
|
||||
AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
|
||||
|
||||
OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
|
||||
0 /* min_covered_cells_count */);
|
||||
trimmer.Trim(&fake_pose_graph_);
|
||||
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
|
||||
ElementsAre(EqualsSubmapId({0, 0})));
|
||||
}
|
||||
|
||||
TEST_F(OverlappingSubmapsTrimmer2DTest, DoNotTrimUnfinishedSubmap) {
|
||||
AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */,
|
||||
false /* is_finished */);
|
||||
AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */,
|
||||
true /* is_finished */);
|
||||
AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
|
||||
AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
|
||||
AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
|
||||
AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
|
||||
|
||||
OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
|
||||
0 /* min_covered_cells_count */);
|
||||
trimmer.Trim(&fake_pose_graph_);
|
||||
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
|
||||
}
|
||||
|
||||
TEST_F(OverlappingSubmapsTrimmer2DTest, UseOnlyIntraSubmapsToComputeFreshness) {
|
||||
AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */,
|
||||
true /* is_finished */);
|
||||
AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */,
|
||||
true /* is_finished */);
|
||||
AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
|
||||
AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
|
||||
AddTrajectoryNode(2 /* node_index */, 3000 /* timestamp */);
|
||||
AddConstraint(0 /*submap_index*/, 0 /*node_index*/, false);
|
||||
AddConstraint(0 /*submap_index*/, 2 /*node_index*/, true);
|
||||
AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
|
||||
|
||||
OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
|
||||
0 /* min_covered_cells_count */);
|
||||
trimmer.Trim(&fake_pose_graph_);
|
||||
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
|
||||
ElementsAre(EqualsSubmapId({0, 1})));
|
||||
}
|
||||
|
||||
// This test covers two 4-cells submaps that overlap each other displaced like:
|
||||
// ___
|
||||
// _| |
|
||||
// | |_ _|
|
||||
// |___|
|
||||
//
|
||||
// The background submap should be trimmed, since it has only 3 cells
|
||||
// not-covered by another submap.
|
||||
TEST_F(OverlappingSubmapsTrimmer2DTest, ) {
|
||||
AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 2 /* num_cells */,
|
||||
true /* is_finished */);
|
||||
AddSquareSubmap(Rigid2d::Translation(Eigen::Vector2d(1., 1.)),
|
||||
1 /* submap_index */, 2 /* num_cells */,
|
||||
true /* is_finished */);
|
||||
AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
|
||||
AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
|
||||
AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
|
||||
AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
|
||||
|
||||
OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
|
||||
4 /* min_covered_cells_count */);
|
||||
trimmer.Trim(&fake_pose_graph_);
|
||||
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
|
||||
ElementsAre(EqualsSubmapId({0, 0})));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
|
@ -27,32 +27,66 @@ namespace testing {
|
|||
|
||||
class FakeTrimmable : public Trimmable {
|
||||
public:
|
||||
FakeTrimmable() = default;
|
||||
|
||||
// Populates dummy SubmapIDs.
|
||||
FakeTrimmable(int trajectory_id, int num_submaps) {
|
||||
for (int index = 0; index < num_submaps; ++index) {
|
||||
submaps_.push_back(SubmapId{trajectory_id, index});
|
||||
submap_data_.Insert(SubmapId{trajectory_id, index}, {});
|
||||
}
|
||||
}
|
||||
~FakeTrimmable() override {}
|
||||
|
||||
int num_submaps(const int trajectory_id) const override {
|
||||
return submaps_.size() - trimmed_submaps_.size();
|
||||
return submap_data_.size() - trimmed_submaps_.size();
|
||||
}
|
||||
|
||||
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override {
|
||||
return submaps_;
|
||||
std::vector<SubmapId> submap_ids;
|
||||
for (const auto& submap : submap_data_) {
|
||||
submap_ids.push_back(submap.id);
|
||||
}
|
||||
return submap_ids;
|
||||
}
|
||||
|
||||
void set_submap_data(
|
||||
const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data) {
|
||||
submap_data_ = submap_data;
|
||||
}
|
||||
|
||||
MapById<SubmapId, PoseGraphInterface::SubmapData>* mutable_submap_data() {
|
||||
return &submap_data_;
|
||||
}
|
||||
|
||||
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
|
||||
const override {
|
||||
return {};
|
||||
return submap_data_;
|
||||
}
|
||||
|
||||
void set_trajectory_nodes(
|
||||
const MapById<NodeId, TrajectoryNode>& trajectory_nodes) {
|
||||
trajectory_nodes_ = trajectory_nodes;
|
||||
}
|
||||
|
||||
MapById<NodeId, TrajectoryNode>* mutable_trajectory_nodes() {
|
||||
return &trajectory_nodes_;
|
||||
}
|
||||
|
||||
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override {
|
||||
return {};
|
||||
return trajectory_nodes_;
|
||||
}
|
||||
|
||||
void set_constraints(
|
||||
const std::vector<PoseGraphInterface::Constraint>& constraints) {
|
||||
constraints_ = constraints;
|
||||
}
|
||||
|
||||
std::vector<PoseGraphInterface::Constraint>* mutable_constraints() {
|
||||
return &constraints_;
|
||||
}
|
||||
|
||||
std::vector<PoseGraphInterface::Constraint> GetConstraints() const override {
|
||||
return {};
|
||||
return constraints_;
|
||||
}
|
||||
|
||||
void MarkSubmapAsTrimmed(const SubmapId& submap_id) override {
|
||||
|
|
@ -64,8 +98,11 @@ class FakeTrimmable : public Trimmable {
|
|||
std::vector<SubmapId> trimmed_submaps() { return trimmed_submaps_; }
|
||||
|
||||
private:
|
||||
std::vector<SubmapId> submaps_;
|
||||
std::vector<SubmapId> trimmed_submaps_;
|
||||
|
||||
std::vector<PoseGraphInterface::Constraint> constraints_;
|
||||
MapById<NodeId, TrajectoryNode> trajectory_nodes_;
|
||||
MapById<SubmapId, PoseGraphInterface::SubmapData> submap_data_;
|
||||
};
|
||||
|
||||
} // namespace testing
|
||||
|
|
|
|||
Loading…
Reference in New Issue