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

Fix gravity alignment of submaps in local SLAM (#1398) 

We were passing the gravity estimate of the current tracking frame
to intialize the local submap pose. Fixing this improves the alignment
of submaps in the global (and approx. gravity-aligned) frame.
master
danielsievers 2018-08-24 12:28:31 +02:00 committed by Wally B. Feed
parent 1c00e8a970
commit 81b75da9f4
4 changed files with 17 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cartographer/mapping/3d/submap_3d.cc
View File

@ -253,12 +253,13 @@ void Submap3D::ToResponseProto(
response->add_textures());
}
void Submap3D::InsertRangeData(const sensor::RangeData& range_data,
void Submap3D::InsertRangeData(const sensor::RangeData& range_data_in_local,
const RangeDataInserter3D& range_data_inserter,
const float high_resolution_max_range) {
CHECK(!insertion_finished());
// Transform range data into submap frame.
const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
range_data, local_pose().inverse().cast<float>());
range_data_in_local, local_pose().inverse().cast<float>());
range_data_inserter.Insert(
FilterRangeDataByMaxRange(transformed_range_data,
high_resolution_max_range),
@ -284,11 +285,11 @@ std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::submaps() const {
std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::InsertRangeData(
const sensor::RangeData& range_data,
const Eigen::Quaterniond& gravity_alignment) {
const Eigen::Quaterniond& local_from_gravity_aligned) {
if (submaps_.empty() ||
submaps_.back()->num_range_data() == options_.num_range_data()) {
AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
gravity_alignment));
local_from_gravity_aligned));
}
for (auto& submap : submaps_) {
submap->InsertRangeData(range_data, range_data_inserter_,

10
cartographer/mapping/3d/submap_3d.h
View File

@ -90,12 +90,12 @@ class ActiveSubmaps3D {
ActiveSubmaps3D(const ActiveSubmaps3D&) = delete;
ActiveSubmaps3D& operator=(const ActiveSubmaps3D&) = delete;
// Inserts 'range_data' into the Submap collection. 'gravity_alignment' is
// used for the orientation of new submaps so that the z axis approximately
// aligns with gravity.
// Inserts 'range_data_in_local' into the Submap collection.
// 'local_from_gravity_aligned' is used for the orientation of new submaps so
// that the z axis approximately aligns with gravity.
std::vector<std::shared_ptr<const Submap3D>> InsertRangeData(
const sensor::RangeData& range_data,
const Eigen::Quaterniond& gravity_alignment);
const sensor::RangeData& range_data_in_local,
const Eigen::Quaterniond& local_from_gravity_aligned);
std::vector<std::shared_ptr<const Submap3D>> submaps() const;

6
cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc
View File

@ -352,9 +352,13 @@ LocalTrajectoryBuilder3D::InsertIntoSubmap(
if (motion_filter_.IsSimilar(time, pose_estimate)) {
return nullptr;
}
const auto local_from_gravity_aligned =
pose_estimate.rotation() * gravity_alignment.inverse();
std::vector<std::shared_ptr<const mapping::Submap3D>> insertion_submaps =
active_submaps_.InsertRangeData(filtered_range_data_in_local,
gravity_alignment);
local_from_gravity_aligned);
const Eigen::VectorXf rotational_scan_matcher_histogram =
scan_matching::RotationalScanMatcher::ComputeHistogram(
sensor::TransformPointCloud(

3
cartographer/mapping/pose_extrapolator.h
View File

@ -54,7 +54,8 @@ class PoseExtrapolator {
void AddOdometryData(const sensor::OdometryData& odometry_data);
transform::Rigid3d ExtrapolatePose(common::Time time);
// Gravity alignment estimate.
// Returns the current gravity alignment estimate as a rotation from
// the tracking frame into a gravity aligned frame.
Eigen::Quaterniond EstimateGravityOrientation(common::Time time);
private: