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

Fix 3D SLAM loop closure issues. (#296) 

3D submaps are now oriented approximately gravity aligned.
This is so that accumulating error in the local SLAM frame is
no longer a problem for finding loop closures. It also ensures
that the z search window size is approximately in the gravity
direction.

We now also pass an estimate of gravity orientation when doing
multi-trajectory matches. Otherwise trajectories starting with
different orientation relative to gravity could not be connected.

The gravity alignment is currently derived from the ImuTracker.
It might be possible to further improve on this by using the
latest gravity direction from the optimized poses.
master
Wolfgang Hess 2017-05-18 16:55:11 +02:00 committed by GitHub
parent 22f673655f
commit 56fc2a9a92
13 changed files with 93 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/kalman_filter/pose_tracker.h
View File

@ -122,6 +122,10 @@ class PoseTracker {
const mapping::OdometryStateTracker::OdometryStates& odometry_states() const;
Eigen::Quaterniond gravity_orientation() const {
return imu_tracker_.orientation();
}
private:
// Returns the distribution required to initialize the KalmanFilter.
static Distribution KalmanFilterInit();

2
cartographer/mapping/imu_tracker.h
View File

@ -41,7 +41,7 @@ class ImuTracker {
const Eigen::Vector3d& imu_angular_velocity);
// Query the current orientation estimate.
Eigen::Quaterniond orientation() { return orientation_; }
Eigen::Quaterniond orientation() const { return orientation_; }
private:
const double imu_gravity_time_constant_;

14
cartographer/mapping/submaps.h
View File

@ -52,19 +52,15 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) {
return value;
}
// An individual submap, which has an initial position 'origin', keeps track of
// how many range data were inserted into it, and sets the
// An individual submap, which has a 'local_pose' in the local SLAM frame, keeps
// track of how many range data were inserted into it, and sets the
// 'finished_probability_grid' to be used for loop closing once the map no
// longer changes.
struct Submap {
Submap(const Eigen::Vector3f& origin) : origin(origin) {}
Submap(const transform::Rigid3d& local_pose) : local_pose(local_pose) {}
transform::Rigid3d local_pose() const {
return transform::Rigid3d::Translation(origin.cast<double>());
}
// Origin of this submap.
const Eigen::Vector3f origin;
// Local SLAM pose of this submap.
const transform::Rigid3d local_pose;
// Number of RangeData inserted.
int num_range_data = 0;

10
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -441,8 +441,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
return extrapolated_submap_transforms.back() *
submap_states_.at(trajectory_id)
.at(extrapolated_submap_transforms.size() - 1)
.submap->local_pose()
.inverse();
.submap->local_pose.inverse();
}
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
@ -476,14 +475,13 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
} else if (result.empty()) {
result.push_back(transform::Rigid3d::Identity());
} else {
// Extrapolate to the remaining submaps. Accessing local_pose() in Submaps
// Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps
// is okay, since the member is const.
result.push_back(result.back() *
submap_states_.at(trajectory_id)
.at(result.size() - 1)
.submap->local_pose()
.inverse() *
state.submap->local_pose());
.submap->local_pose.inverse() *
state.submap->local_pose);
}
}

2
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
View File

@ -40,7 +40,7 @@ namespace mapping_2d {
namespace sparse_pose_graph {
transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap) {
return transform::Project2D(submap.local_pose());
return transform::Project2D(submap.local_pose);
}
ConstraintBuilder::ConstraintBuilder(

5
cartographer/mapping_2d/submaps.cc
View File

@ -102,7 +102,8 @@ proto::SubmapsOptions CreateSubmapsOptions(
}
Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
: mapping::Submap(Eigen::Vector3f(origin.x(), origin.y(), 0.)),
: mapping::Submap(transform::Rigid3d::Translation(
Eigen::Vector3d(origin.x(), origin.y(), 0.))),
probability_grid(limits) {}
Submaps::Submaps(const proto::SubmapsOptions& options)
@ -137,7 +138,7 @@ int Submaps::size() const { return submaps_.size(); }
void Submaps::SubmapToProto(
const int index, const transform::Rigid3d&,
mapping::proto::SubmapQuery::Response* const response) const {
AddProbabilityGridToResponse(Get(index)->local_pose(),
AddProbabilityGridToResponse(Get(index)->local_pose,
Get(index)->probability_grid, response);
}

10
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -143,7 +143,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
const Submap* const matching_submap =
submaps_->Get(submaps_->matching_index());
transform::Rigid3d initial_ceres_pose =
matching_submap->local_pose().inverse() * pose_prediction;
matching_submap->local_pose.inverse() * pose_prediction;
sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.high_resolution_adaptive_voxel_filter_options());
const sensor::PointCloud filtered_point_cloud_in_tracking =
@ -171,7 +171,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
&matching_submap->low_resolution_hybrid_grid}},
&pose_observation_in_submap, &summary);
const transform::Rigid3d pose_observation =
matching_submap->local_pose() * pose_observation_in_submap;
matching_submap->local_pose * pose_observation_in_submap;
pose_tracker_->AddPoseObservation(
time, pose_observation,
options_.kalman_local_trajectory_builder_options()
@ -227,8 +227,10 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index));
}
submaps_->InsertRangeData(sensor::TransformRangeData(
range_data_in_tracking, pose_observation.cast<float>()));
submaps_->InsertRangeData(
sensor::TransformRangeData(range_data_in_tracking,
pose_observation.cast<float>()),
pose_tracker_->gravity_orientation());
return std::unique_ptr<InsertionResult>(new InsertionResult{
time, range_data_in_tracking, pose_observation, covariance_estimate,
matching_submap, insertion_submaps});

13
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -242,7 +242,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
// We transform the states in 'batches_' in place to be in the submap frame as
// expected by the OccupiedSpaceCostFunctor. This is reverted after solving
// the optimization problem.
TransformStates(matching_submap->local_pose().inverse());
TransformStates(matching_submap->local_pose.inverse());
for (size_t i = 0; i < batches_.size(); ++i) {
Batch& batch = batches_[i];
problem.AddResidualBlock(
@ -351,7 +351,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
ceres::Solve(ceres_solver_options_, &problem, &summary);
// The optimized states in 'batches_' are in the submap frame and we transform
// them in place to be in the local SLAM frame again.
TransformStates(matching_submap->local_pose());
TransformStates(matching_submap->local_pose);
if (num_accumulated_ < options_.scans_per_accumulation()) {
return nullptr;
}
@ -416,8 +416,13 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index));
}
submaps_->InsertRangeData(sensor::TransformRangeData(
range_data_in_tracking, pose_observation.cast<float>()));
// TODO(whess): Use an ImuTracker to track the gravity direction.
const Eigen::Quaterniond kFakeGravityOrientation =
Eigen::Quaterniond::Identity();
submaps_->InsertRangeData(
sensor::TransformRangeData(range_data_in_tracking,
pose_observation.cast<float>()),
kFakeGravityOrientation);
const kalman_filter::PoseCovariance kCovariance =
1e-7 * kalman_filter::PoseCovariance::Identity();

44
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -81,8 +81,8 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
submap_data.at(trajectory_id).at(first_submap_id.submap_index).pose;
optimization_problem_.AddSubmap(
trajectory_id, first_submap_pose *
insertion_submaps[0]->local_pose().inverse() *
insertion_submaps[1]->local_pose());
insertion_submaps[0]->local_pose.inverse() *
insertion_submaps[1]->local_pose);
}
}
@ -164,6 +164,12 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
.at(submap_id.submap_index)
.pose.inverse();
const transform::Rigid3d initial_relative_pose =
inverse_submap_pose * optimization_problem_.node_data()
.at(node_id.trajectory_id)
.at(node_id.node_index)
.point_cloud_pose;
std::vector<mapping::TrajectoryNode> submap_nodes;
for (const mapping::NodeId& submap_node_id :
submap_states_.at(submap_id.trajectory_id)
@ -181,6 +187,18 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
// Only globally match against submaps not in this trajectory.
if (node_id.trajectory_id != submap_id.trajectory_id &&
global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
// In this situation, 'initial_relative_pose' is:
//
// submap <- global map 2 <- global map 1 <- tracking
// (agreeing on gravity)
//
// Since they possibly came from two disconnected trajectories, the only
// guaranteed connection between the tracking and the submap frames is
// an agreement on the direction of gravity. Therefore, excluding yaw,
// 'initial_relative_pose.rotation()' is a good estimate of the relative
// orientation of the point cloud in the submap frame. Finding the correct
// yaw component will be handled by the matching procedure in the
// FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
constraint_builder_.MaybeAddGlobalConstraint(
submap_id,
submap_states_.at(submap_id.trajectory_id)
@ -190,7 +208,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
&trajectory_nodes_.at(node_id.trajectory_id)
.at(node_id.node_index)
.constant_data->range_data_3d.returns,
submap_nodes, &trajectory_connectivity_);
submap_nodes, initial_relative_pose.rotation(),
&trajectory_connectivity_);
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(node_id.trajectory_id) > 0 &&
@ -199,11 +218,6 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
reverse_connected_components_.at(submap_id.trajectory_id);
if (node_id.trajectory_id == submap_id.trajectory_id ||
scan_and_submap_trajectories_connected) {
const transform::Rigid3d initial_relative_pose =
inverse_submap_pose * optimization_problem_.node_data()
.at(node_id.trajectory_id)
.at(node_id.node_index)
.point_cloud_pose;
constraint_builder_.MaybeAddConstraint(
submap_id,
submap_states_.at(submap_id.trajectory_id)
@ -242,7 +256,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(matching_id.trajectory_id)
.at(matching_id.submap_index)
.pose *
matching_submap->local_pose().inverse() * pose;
matching_submap->local_pose.inverse() * pose;
CHECK_EQ(scan_index, scan_index_to_node_id_.size());
const mapping::NodeId node_id{
matching_id.trajectory_id,
@ -266,7 +280,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.node_ids.emplace(node_id);
// Unchanged covariance as (submap <- map) is a translation.
const transform::Rigid3d constraint_transform =
submap->local_pose().inverse() * pose;
submap->local_pose.inverse() * pose;
constraints_.push_back(
Constraint{submap_id,
node_id,
@ -447,8 +461,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
return extrapolated_submap_transforms.back() *
submap_states_.at(trajectory_id)
.at(extrapolated_submap_transforms.size() - 1)
.submap->local_pose()
.inverse();
.submap->local_pose.inverse();
}
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
@ -482,14 +495,13 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
} else if (result.empty()) {
result.push_back(transform::Rigid3d::Identity());
} else {
// Extrapolate to the remaining submaps. Accessing local_pose() in Submaps
// Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps
// is okay, since the member is const.
result.push_back(result.back() *
submap_states_.at(trajectory_id)
.at(result.size() - 1)
.submap->local_pose()
.inverse() *
state.submap->local_pose());
.submap->local_pose.inverse() *
state.submap->local_pose);
}
}

7
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -88,6 +88,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::NodeId& node_id,
const sensor::CompressedPointCloud* const compressed_point_cloud,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& gravity_alignment,
mapping::TrajectoryConnectivity* const trajectory_connectivity) {
common::MutexLocker locker(&mutex_);
constraints_.emplace_back();
@ -97,10 +98,10 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
[=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id,
true, /* match_full_submap */
ComputeConstraint(
submap_id, submap, node_id, true, /* match_full_submap */
trajectory_connectivity, compressed_point_cloud,
transform::Rigid3d::Identity(), constraint);
transform::Rigid3d::Rotation(gravity_alignment), constraint);
FinishComputation(current_computation);
});
}

5
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
View File

@ -84,7 +84,9 @@ class ConstraintBuilder {
// 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
// This performs full-submap matching.
//
// The 'trajectory_connectivity' is updated if the full-submap match succeeds.
// The 'gravity_alignment' is the rotation to apply to the point cloud data
// to make it approximately gravity aligned. The 'trajectory_connectivity' is
// updated if the full-submap match succeeds.
//
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
@ -93,6 +95,7 @@ class ConstraintBuilder {
const mapping::NodeId& node_id,
const sensor::CompressedPointCloud* compressed_point_cloud,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& gravity_alignment,
mapping::TrajectoryConnectivity* trajectory_connectivity);
// Must be called after all computations related to one node have been added.

25
cartographer/mapping_3d/submaps.cc
View File

@ -217,17 +217,20 @@ proto::SubmapsOptions CreateSubmapsOptions(
}
Submap::Submap(const float high_resolution, const float low_resolution,
const Eigen::Vector3f& origin)
: mapping::Submap(origin),
const transform::Rigid3d& local_pose)
: mapping::Submap(local_pose),
high_resolution_hybrid_grid(high_resolution),
low_resolution_hybrid_grid(low_resolution) {}
Submaps::Submaps(const proto::SubmapsOptions& options)
: options_(options),
range_data_inserter_(options.range_data_inserter_options()) {
// We always want to have at least one likelihood field which we can return,
// and will create it at the origin in absence of a better choice.
AddSubmap(Eigen::Vector3f::Zero());
// We always want to have at least one submap which we can return and will
// create it at the origin in absence of a better choice.
//
// TODO(whess): Start with no submaps, so that all of them can be
// approximately gravity aligned.
AddSubmap(transform::Rigid3d::Identity());
}
const Submap* Submaps::Get(int index) const {
@ -271,11 +274,12 @@ void Submaps::SubmapToProto(
global_submap_pose.translation().z())));
}
void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
void Submaps::InsertRangeData(const sensor::RangeData& range_data,
const Eigen::Quaterniond& gravity_alignment) {
for (const int index : insertion_indices()) {
Submap* submap = submaps_[index].get();
const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
range_data, submap->local_pose().inverse().cast<float>());
range_data, submap->local_pose.inverse().cast<float>());
range_data_inserter_.Insert(
FilterRangeDataByMaxRange(transformed_range_data,
options_.high_resolution_max_range()),
@ -286,18 +290,19 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
}
const Submap* const last_submap = Get(size() - 1);
if (last_submap->num_range_data == options_.num_range_data()) {
AddSubmap(range_data.origin);
AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
gravity_alignment));
}
}
void Submaps::AddSubmap(const Eigen::Vector3f& origin) {
void Submaps::AddSubmap(const transform::Rigid3d& local_pose) {
if (size() > 1) {
Submap* submap = submaps_[size() - 2].get();
CHECK(!submap->finished);
submap->finished = true;
}
submaps_.emplace_back(new Submap(options_.high_resolution(),
options_.low_resolution(), origin));
options_.low_resolution(), local_pose));
LOG(INFO) << "Added submap " << size();
}

12
cartographer/mapping_3d/submaps.h
View File

@ -32,6 +32,7 @@
#include "cartographer/mapping_3d/proto/submaps_options.pb.h"
#include "cartographer/mapping_3d/range_data_inserter.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
@ -48,7 +49,7 @@ proto::SubmapsOptions CreateSubmapsOptions(
struct Submap : public mapping::Submap {
Submap(float high_resolution, float low_resolution,
const Eigen::Vector3f& origin);
const transform::Rigid3d& local_pose);
HybridGrid high_resolution_hybrid_grid;
HybridGrid low_resolution_hybrid_grid;
@ -69,8 +70,11 @@ class Submaps : public mapping::Submaps {
int index, const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* response) const override;
// Inserts 'range_data' into the Submap collection.
void InsertRangeData(const sensor::RangeData& range_data);
// 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.
void InsertRangeData(const sensor::RangeData& range_data,
const Eigen::Quaterniond& gravity_alignment);
private:
struct PixelData {
@ -81,7 +85,7 @@ class Submaps : public mapping::Submaps {
float max_probability = 0.5f;
};
void AddSubmap(const Eigen::Vector3f& origin);
void AddSubmap(const transform::Rigid3d& local_pose);
std::vector<PixelData> AccumulatePixelData(
const int width, const int height, const Eigen::Array2i& min_index,