Transform submap cells to global frame correctly. (#1130)
Alexander Belyaev
Wally B. Feed
parent
a3b746ff67
commit
ff18bae528
|
|
@ -30,30 +30,24 @@ class SubmapCoverageGrid2D {
|
|||
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) {}
|
||||
SubmapCoverageGrid2D(const MapLimits& map_limits)
|
||||
: offset_(map_limits.max()), resolution_(map_limits.resolution()) {}
|
||||
|
||||
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))};
|
||||
CellId cell_id{common::RoundToInt64((offset_(0) - point(0)) / resolution_),
|
||||
common::RoundToInt64((offset_(1) - point(1)) / resolution_)};
|
||||
cells_[cell_id].emplace_back(submap_id, time);
|
||||
}
|
||||
|
||||
const std::map<CellId, StoredType>& cells() const { return cells_; }
|
||||
|
||||
private:
|
||||
const Eigen::Vector2d offset_;
|
||||
Eigen::Vector2d offset_;
|
||||
double resolution_;
|
||||
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
|
||||
// the global frame and then adds the submap id and the timestamp of the most
|
||||
// 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;
|
||||
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)) {
|
||||
const Eigen::Array2i index = xy_index + offset;
|
||||
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 =
|
||||
projected_submap_pose *
|
||||
transform::Rigid2d::Translation({index.x() + 0.5, index.y() + 0.5});
|
||||
transform::Project2D(global_frame_from_submap_frame *
|
||||
submap_frame_from_local_frame *
|
||||
center_of_cell_in_local_frame);
|
||||
coverage_grid->AddPoint(center_of_cell_in_global_frame.translation(),
|
||||
submap.id, freshness->second);
|
||||
}
|
||||
|
|
@ -182,7 +188,11 @@ void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) {
|
|||
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 =
|
||||
ComputeSubmapFreshness(submap_data, pose_graph->GetTrajectoryNodes(),
|
||||
pose_graph->GetConstraints());
|
||||
|
|
|
|||
|
|
@ -39,13 +39,15 @@ 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);
|
||||
void AddSquareSubmap(const Rigid2d& global_to_submap_frame,
|
||||
const Rigid2d& local_to_submap_frame,
|
||||
const Eigen::Vector2d& submap_corner, int submap_index,
|
||||
int num_cells, bool is_finished) {
|
||||
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);
|
||||
*submap_2d.mutable_local_pose() =
|
||||
transform::ToProto(transform::Embed3D(local_to_submap_frame));
|
||||
|
||||
auto* grid = submap_2d.mutable_grid();
|
||||
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();
|
||||
map_limits->set_resolution(1.0);
|
||||
*map_limits->mutable_max() =
|
||||
transform::ToProto(Eigen::Vector2d(num_cells, num_cells));
|
||||
*map_limits->mutable_max() = transform::ToProto(submap_corner);
|
||||
map_limits->mutable_cell_limits()->set_num_x_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();
|
||||
fake_pose_graph_.mutable_submap_data()->Insert(
|
||||
{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) {
|
||||
|
|
@ -107,9 +109,15 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, EmptyPoseGraph) {
|
|||
}
|
||||
|
||||
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 */);
|
||||
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 */);
|
||||
AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
|
||||
AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
|
||||
|
|
@ -125,9 +133,15 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TrimOneOfTwoOverlappingSubmaps) {
|
|||
}
|
||||
|
||||
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 */);
|
||||
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 */);
|
||||
AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
|
||||
AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
|
||||
|
|
@ -140,7 +154,10 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TestMinAddedSubmapsCountParam) {
|
|||
trimmer.Trim(&fake_pose_graph_);
|
||||
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 */);
|
||||
AddTrajectoryNode(2 /* node_index */, 3000 /* timestamp */);
|
||||
AddConstraint(2 /*submap_index*/, 2 /*node_index*/, true);
|
||||
|
|
@ -150,9 +167,15 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TestMinAddedSubmapsCountParam) {
|
|||
}
|
||||
|
||||
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 */);
|
||||
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 */);
|
||||
AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
|
||||
AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
|
||||
|
|
@ -167,9 +190,15 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, DoNotTrimUnfinishedSubmap) {
|
|||
}
|
||||
|
||||
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 */);
|
||||
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 */);
|
||||
AddTrajectoryNode(0 /* node_index */, 1000 /* 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
|
||||
// not-covered by another submap.
|
||||
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 */);
|
||||
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 */,
|
||||
true /* is_finished */);
|
||||
AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
|
||||
|
|
@ -213,6 +248,31 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TrimSubmapWithLowCoveredCellsCount) {
|
|||
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 mapping
|
||||
} // namespace cartographer
|
||||
|
|
|
|||
Loading…
Reference in New Issue