Transform submap cells to global frame correctly. (#1130)

master
Alexander Belyaev 2018-04-30 12:12:48 +02:00 committed by Wally B. Feed
parent a3b746ff67
commit ff18bae528
2 changed files with 105 additions and 35 deletions

View File

@ -30,30 +30,24 @@ class SubmapCoverageGrid2D {
using CellId = std::pair<int64 /* x cells */, int64 /* y cells */>; using CellId = std::pair<int64 /* x cells */, int64 /* y cells */>;
using StoredType = std::vector<std::pair<SubmapId, common::Time>>; using StoredType = std::vector<std::pair<SubmapId, common::Time>>;
explicit SubmapCoverageGrid2D(const Eigen::Vector2d& offset) SubmapCoverageGrid2D(const MapLimits& map_limits)
: offset_(offset) {} : offset_(map_limits.max()), resolution_(map_limits.resolution()) {}
void AddPoint(const Eigen::Vector2d& point, const SubmapId& submap_id, void AddPoint(const Eigen::Vector2d& point, const SubmapId& submap_id,
const common::Time& time) { const common::Time& time) {
CellId cell_id{common::RoundToInt64(offset_(0) - point(0)), CellId cell_id{common::RoundToInt64((offset_(0) - point(0)) / resolution_),
common::RoundToInt64(offset_(1) - point(1))}; common::RoundToInt64((offset_(1) - point(1)) / resolution_)};
cells_[cell_id].emplace_back(submap_id, time); cells_[cell_id].emplace_back(submap_id, time);
} }
const std::map<CellId, StoredType>& cells() const { return cells_; } const std::map<CellId, StoredType>& cells() const { return cells_; }
private: private:
const Eigen::Vector2d offset_; Eigen::Vector2d offset_;
double resolution_;
std::map<CellId, StoredType> cells_; 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->grid()->limits().max();
}
// Iterates over every cell in a submap, transforms the center of the cell to // 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 // the global frame and then adds the submap id and the timestamp of the most
// recent range data insertion into the global grid. // recent range data insertion into the global grid.
@ -78,14 +72,26 @@ std::set<SubmapId> AddSubmapsToSubmapCoverageGrid2D(
LOG(WARNING) << "Empty grid found in submap ID = " << submap.id; LOG(WARNING) << "Empty grid found in submap ID = " << submap.id;
continue; continue;
} }
const transform::Rigid2d projected_submap_pose =
transform::Project2D(submap.data.pose); const transform::Rigid3d& global_frame_from_submap_frame = submap.data.pose;
const transform::Rigid3d submap_frame_from_local_frame =
submap.data.submap->local_pose().inverse();
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) { for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
const Eigen::Array2i index = xy_index + offset; const Eigen::Array2i index = xy_index + offset;
if (!grid.IsKnown(index)) continue; if (!grid.IsKnown(index)) continue;
const transform::Rigid3d center_of_cell_in_local_frame =
transform::Rigid3d::Translation(Eigen::Vector3d(
grid.limits().max().x() -
grid.limits().resolution() * (index.y() + 0.5),
grid.limits().max().y() -
grid.limits().resolution() * (index.x() + 0.5),
0));
const transform::Rigid2d center_of_cell_in_global_frame = const transform::Rigid2d center_of_cell_in_global_frame =
projected_submap_pose * transform::Project2D(global_frame_from_submap_frame *
transform::Rigid2d::Translation({index.x() + 0.5, index.y() + 0.5}); submap_frame_from_local_frame *
center_of_cell_in_local_frame);
coverage_grid->AddPoint(center_of_cell_in_global_frame.translation(), coverage_grid->AddPoint(center_of_cell_in_global_frame.translation(),
submap.id, freshness->second); submap.id, freshness->second);
} }
@ -182,7 +188,11 @@ void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) {
return; return;
} }
SubmapCoverageGrid2D coverage_grid(GetCornerOfFirstSubmap(submap_data)); const MapLimits first_submap_map_limits =
std::static_pointer_cast<const Submap2D>(submap_data.begin()->data.submap)
->grid()
->limits();
SubmapCoverageGrid2D coverage_grid(first_submap_map_limits);
const std::map<SubmapId, common::Time> submap_freshness = const std::map<SubmapId, common::Time> submap_freshness =
ComputeSubmapFreshness(submap_data, pose_graph->GetTrajectoryNodes(), ComputeSubmapFreshness(submap_data, pose_graph->GetTrajectoryNodes(),
pose_graph->GetConstraints()); pose_graph->GetConstraints());

View File

@ -39,13 +39,15 @@ using ::testing::IsEmpty;
class OverlappingSubmapsTrimmer2DTest : public ::testing::Test { class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
protected: protected:
// Creates a submap with num_cells x num_cells grid. // Creates a submap with num_cells x num_cells grid.
void AddSquareSubmap(const Rigid2d& pose_2d, int submap_index, int num_cells, void AddSquareSubmap(const Rigid2d& global_to_submap_frame,
bool is_finished) { const Rigid2d& local_to_submap_frame,
const Rigid3d pose_3d = transform::Embed3D(pose_2d); const Eigen::Vector2d& submap_corner, int submap_index,
int num_cells, bool is_finished) {
proto::Submap2D submap_2d; proto::Submap2D submap_2d;
submap_2d.set_num_range_data(1); submap_2d.set_num_range_data(1);
submap_2d.set_finished(is_finished); submap_2d.set_finished(is_finished);
*submap_2d.mutable_local_pose() = transform::ToProto(pose_3d); *submap_2d.mutable_local_pose() =
transform::ToProto(transform::Embed3D(local_to_submap_frame));
auto* grid = submap_2d.mutable_grid(); auto* grid = submap_2d.mutable_grid();
for (int i = 0; i < num_cells * num_cells; ++i) { for (int i = 0; i < num_cells * num_cells; ++i) {
@ -54,8 +56,7 @@ class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
auto* map_limits = grid->mutable_limits(); auto* map_limits = grid->mutable_limits();
map_limits->set_resolution(1.0); map_limits->set_resolution(1.0);
*map_limits->mutable_max() = *map_limits->mutable_max() = transform::ToProto(submap_corner);
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_x_cells(num_cells);
map_limits->mutable_cell_limits()->set_num_y_cells(num_cells); map_limits->mutable_cell_limits()->set_num_y_cells(num_cells);
@ -68,7 +69,8 @@ class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
grid->mutable_probability_grid_2d(); grid->mutable_probability_grid_2d();
fake_pose_graph_.mutable_submap_data()->Insert( fake_pose_graph_.mutable_submap_data()->Insert(
{0 /* trajectory_id */, submap_index}, {0 /* trajectory_id */, submap_index},
{std::make_shared<const Submap2D>(submap_2d), pose_3d}); {std::make_shared<const Submap2D>(submap_2d),
transform::Embed3D(global_to_submap_frame)});
} }
void AddTrajectoryNode(int node_index, int64 timestamp) { void AddTrajectoryNode(int node_index, int64 timestamp) {
@ -107,9 +109,15 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, EmptyPoseGraph) {
} }
TEST_F(OverlappingSubmapsTrimmer2DTest, TrimOneOfTwoOverlappingSubmaps) { TEST_F(OverlappingSubmapsTrimmer2DTest, TrimOneOfTwoOverlappingSubmaps) {
AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */, AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
Rigid2d::Identity() /* local_from_submap_frame */,
Eigen::Vector2d(1., 1.) /* submap corner */,
0 /* submap_index */, 1 /* num_cells */,
true /* is_finished */); true /* is_finished */);
AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */, AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
Rigid2d::Identity() /* local_from_submap_frame */,
Eigen::Vector2d(1., 1.) /* submap corner */,
1 /* submap_index */, 1 /* num_cells */,
true /* is_finished */); true /* is_finished */);
AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */); AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */); AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
@ -125,9 +133,15 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TrimOneOfTwoOverlappingSubmaps) {
} }
TEST_F(OverlappingSubmapsTrimmer2DTest, TestMinAddedSubmapsCountParam) { TEST_F(OverlappingSubmapsTrimmer2DTest, TestMinAddedSubmapsCountParam) {
AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */, AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
Rigid2d::Identity() /* local_from_submap_frame */,
Eigen::Vector2d(1., 1.) /* submap corner */,
0 /* submap_index */, 1 /* num_cells */,
true /* is_finished */); true /* is_finished */);
AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */, AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
Rigid2d::Identity() /* local_from_submap_frame */,
Eigen::Vector2d(1., 1.) /* submap corner */,
1 /* submap_index */, 1 /* num_cells */,
true /* is_finished */); true /* is_finished */);
AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */); AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */); AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
@ -140,7 +154,10 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TestMinAddedSubmapsCountParam) {
trimmer.Trim(&fake_pose_graph_); trimmer.Trim(&fake_pose_graph_);
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty()); EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
AddSquareSubmap(Rigid2d::Identity(), 2 /* submap_index */, 1 /* num_cells */, AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
Rigid2d::Identity() /* local_from_submap_frame */,
Eigen::Vector2d(1., 1.) /* submap corner */,
2 /* submap_index */, 1 /* num_cells */,
true /* is_finished */); true /* is_finished */);
AddTrajectoryNode(2 /* node_index */, 3000 /* timestamp */); AddTrajectoryNode(2 /* node_index */, 3000 /* timestamp */);
AddConstraint(2 /*submap_index*/, 2 /*node_index*/, true); AddConstraint(2 /*submap_index*/, 2 /*node_index*/, true);
@ -150,9 +167,15 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TestMinAddedSubmapsCountParam) {
} }
TEST_F(OverlappingSubmapsTrimmer2DTest, DoNotTrimUnfinishedSubmap) { TEST_F(OverlappingSubmapsTrimmer2DTest, DoNotTrimUnfinishedSubmap) {
AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */, AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
Rigid2d::Identity() /* local_from_submap_frame */,
Eigen::Vector2d(1., 1.) /* submap corner */,
0 /* submap_index */, 1 /* num_cells */,
false /* is_finished */); false /* is_finished */);
AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */, AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
Rigid2d::Identity() /* local_from_submap_frame */,
Eigen::Vector2d(1., 1.) /* submap corner */,
1 /* submap_index */, 1 /* num_cells */,
true /* is_finished */); true /* is_finished */);
AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */); AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */); AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
@ -167,9 +190,15 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, DoNotTrimUnfinishedSubmap) {
} }
TEST_F(OverlappingSubmapsTrimmer2DTest, UseOnlyIntraSubmapsToComputeFreshness) { TEST_F(OverlappingSubmapsTrimmer2DTest, UseOnlyIntraSubmapsToComputeFreshness) {
AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */, AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
Rigid2d::Identity() /* local_from_submap_frame */,
Eigen::Vector2d(1., 1.) /* submap corner */,
0 /* submap_index */, 1 /* num_cells */,
true /* is_finished */); true /* is_finished */);
AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */, AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
Rigid2d::Identity() /* local_from_submap_frame */,
Eigen::Vector2d(1., 1.) /* submap corner */,
1 /* submap_index */, 1 /* num_cells */,
true /* is_finished */); true /* is_finished */);
AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */); AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */); AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
@ -195,9 +224,15 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, UseOnlyIntraSubmapsToComputeFreshness) {
// The background submap should be trimmed, since it has only 3 cells // The background submap should be trimmed, since it has only 3 cells
// not-covered by another submap. // not-covered by another submap.
TEST_F(OverlappingSubmapsTrimmer2DTest, TrimSubmapWithLowCoveredCellsCount) { TEST_F(OverlappingSubmapsTrimmer2DTest, TrimSubmapWithLowCoveredCellsCount) {
AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 2 /* num_cells */, AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
Rigid2d::Identity() /* local_from_submap_frame */,
Eigen::Vector2d(1., 1.) /* submap corner */,
0 /* submap_index */, 2 /* num_cells */,
true /* is_finished */); true /* is_finished */);
AddSquareSubmap(Rigid2d::Translation(Eigen::Vector2d(1., 1.)), AddSquareSubmap(Rigid2d::Translation(
Eigen::Vector2d(1., 1.)) /* global_from_submap_frame */,
Rigid2d::Identity() /* local_from_submap_frame */,
Eigen::Vector2d(1., 1.) /* submap corner */,
1 /* submap_index */, 2 /* num_cells */, 1 /* submap_index */, 2 /* num_cells */,
true /* is_finished */); true /* is_finished */);
AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */); AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
@ -213,6 +248,31 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TrimSubmapWithLowCoveredCellsCount) {
ElementsAre(EqualsSubmapId({0, 0}))); ElementsAre(EqualsSubmapId({0, 0})));
} }
TEST_F(OverlappingSubmapsTrimmer2DTest, TestTransformations) {
AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
Rigid2d::Identity() /* local_from_submap_frame */,
Eigen::Vector2d(1., 1.) /* submap corner */,
0 /* submap_index */, 1 /* num_cells */,
true /* is_finished */);
const Rigid2d transform(Eigen::Vector2d(1., 1.), M_PI / 2);
AddSquareSubmap(transform /* global_from_submap_frame */,
transform /* local_from_submap_frame */,
Eigen::Vector2d(1., 1.) /* submap corner */,
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 */,
0 /* min_added_submaps_count */);
trimmer.Trim(&fake_pose_graph_);
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
ElementsAre(EqualsSubmapId({0, 0})));
}
} // namespace } // namespace
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer