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

Fix 3D yaw rotation in GenerateDiscreteScans. (#640) 

Correctly generates discrete scans rotating around yaw in the gravity-aligned global map frame.
Fixes #639.
PAIR=wohe
master
gaschler 2017-11-08 17:37:36 +01:00 committed by GitHub
parent eb96c91473
commit 386ee328d8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 92 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -157,7 +157,8 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
bool FastCorrelativeScanMatcher::Match(
const transform::Rigid3d& initial_pose_estimate,
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose,
const mapping::TrajectoryNode::Data& constant_data, const float min_score,
float* const score, transform::Rigid3d* const pose_estimate,
float* const rotational_score, float* const low_resolution_score) const {
@ -168,7 +169,8 @@ bool FastCorrelativeScanMatcher::Match(
common::RoundToInt(options_.linear_z_search_window() / resolution_),
options_.angular_search_window(), &low_resolution_matcher};
return MatchWithSearchParameters(
search_parameters, initial_pose_estimate,
search_parameters, global_node_pose.cast<float>(),
global_submap_pose.cast<float>(),
constant_data.high_resolution_point_cloud,
constant_data.rotational_scan_matcher_histogram,
constant_data.gravity_alignment, min_score, score, pose_estimate,
@ -176,12 +178,11 @@ bool FastCorrelativeScanMatcher::Match(
}
bool FastCorrelativeScanMatcher::MatchFullSubmap(
const Eigen::Quaterniond& gravity_alignment,
const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation,
const mapping::TrajectoryNode::Data& constant_data, const float min_score,
float* const score, transform::Rigid3d* const pose_estimate,
float* const rotational_score, float* const low_resolution_score) const {
const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(),
gravity_alignment);
float max_point_distance = 0.f;
for (const Eigen::Vector3f& point :
constant_data.high_resolution_point_cloud) {
@ -195,7 +196,9 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
const SearchParameters search_parameters{
linear_window_size, linear_window_size, M_PI, &low_resolution_matcher};
return MatchWithSearchParameters(
search_parameters, initial_pose_estimate,
search_parameters,
transform::Rigid3f::Rotation(global_node_rotation.cast<float>()),
transform::Rigid3f::Rotation(global_submap_rotation.cast<float>()),
constant_data.high_resolution_point_cloud,
constant_data.rotational_scan_matcher_histogram,
constant_data.gravity_alignment, min_score, score, pose_estimate,
@ -204,7 +207,8 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
const transform::Rigid3d& initial_pose_estimate,
const transform::Rigid3f& global_node_pose,
const transform::Rigid3f& global_submap_pose,
const sensor::PointCloud& point_cloud,
const Eigen::VectorXf& rotational_scan_matcher_histogram,
const Eigen::Quaterniond& gravity_alignment, const float min_score,
@ -215,7 +219,7 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
const std::vector<DiscreteScan> discrete_scans = GenerateDiscreteScans(
search_parameters, point_cloud, rotational_scan_matcher_histogram,
gravity_alignment, initial_pose_estimate.cast<float>());
gravity_alignment, global_node_pose, global_submap_pose);
const std::vector<Candidate> lowest_resolution_candidates =
ComputeLowestResolutionCandidates(search_parameters, discrete_scans);
@ -284,7 +288,8 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
const sensor::PointCloud& point_cloud,
const Eigen::VectorXf& rotational_scan_matcher_histogram,
const Eigen::Quaterniond& gravity_alignment,
const transform::Rigid3f& initial_pose) const {
const transform::Rigid3f& global_node_pose,
const transform::Rigid3f& global_submap_pose) const {
std::vector<DiscreteScan> result;
// We set this value to something on the order of resolution to make sure that
// the std::acos() below is defined.
@ -299,15 +304,15 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
(2.f * common::Pow2(max_scan_range)));
const int angular_window_size = common::RoundToInt(
search_parameters.angular_search_window / angular_step_size);
// TODO(whess): Should there be a small search window for rotations around
// x and y?
std::vector<float> angles;
for (int rz = -angular_window_size; rz <= angular_window_size; ++rz) {
angles.push_back(rz * angular_step_size);
}
const transform::Rigid3f node_to_submap =
global_submap_pose.inverse() * global_node_pose;
const std::vector<float> scores = rotational_scan_matcher_.Match(
rotational_scan_matcher_histogram,
transform::GetYaw(initial_pose.rotation() *
transform::GetYaw(node_to_submap.rotation() *
gravity_alignment.inverse().cast<float>()),
angles);
for (size_t i = 0; i != angles.size(); ++i) {
@ -319,9 +324,10 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
// and rotation of the 'initial_pose', so that the rotation is around the
// origin of the range data, and yaw is in map frame.
const transform::Rigid3f pose(
initial_pose.translation(),
node_to_submap.translation(),
global_submap_pose.rotation().inverse() *
transform::AngleAxisVectorToRotationQuaternion(angle_axis) *
initial_pose.rotation());
global_node_pose.rotation());
result.push_back(
DiscretizeScan(search_parameters, point_cloud, pose, scores[i]));
}

21
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h
View File

@ -61,20 +61,23 @@ class FastCorrelativeScanMatcher {
delete;
// Aligns the node with the given 'constant_data' within the 'hybrid_grid'
// given an 'initial_pose_estimate'. If a score above 'min_score' (excluding
// equality) is possible, true is returned, and 'score', 'pose_estimate',
// 'rotational_score', and 'low_resolution_score' are updated with the result.
bool Match(const transform::Rigid3d& initial_pose_estimate,
// given 'global_node_pose' and 'global_submap_pose'. If a score above
// 'min_score' (excluding equality) is possible, true is returned, and
// 'score', 'pose_estimate', 'rotational_score', and 'low_resolution_score'
// are updated with the result.
bool Match(const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose,
const mapping::TrajectoryNode::Data& constant_data,
float min_score, float* score, transform::Rigid3d* pose_estimate,
float* rotational_score, float* low_resolution_score) const;
// Aligns the node with the given 'constant_data' within the 'hybrid_grid'
// given a rotation which is expected to be approximately gravity aligned.
// given rotations which are expected to be approximately gravity aligned.
// If a score above 'min_score' (excluding equality) is possible, true is
// returned, and 'score', 'pose_estimate', 'rotational_score', and
// 'low_resolution_score' are updated with the result.
bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment,
bool MatchFullSubmap(const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation,
const mapping::TrajectoryNode::Data& constant_data,
float min_score, float* score,
transform::Rigid3d* pose_estimate,
@ -91,7 +94,8 @@ class FastCorrelativeScanMatcher {
bool MatchWithSearchParameters(
const SearchParameters& search_parameters,
const transform::Rigid3d& initial_pose_estimate,
const transform::Rigid3f& global_node_pose,
const transform::Rigid3f& global_submap_pose,
const sensor::PointCloud& point_cloud,
const Eigen::VectorXf& rotational_scan_matcher_histogram,
const Eigen::Quaterniond& gravity_alignment, float min_score,
@ -106,7 +110,8 @@ class FastCorrelativeScanMatcher {
const sensor::PointCloud& point_cloud,
const Eigen::VectorXf& rotational_scan_matcher_histogram,
const Eigen::Quaterniond& gravity_alignment,
const transform::Rigid3f& initial_pose) const;
const transform::Rigid3f& global_node_pose,
const transform::Rigid3f& global_submap_pose) const;
std::vector<Candidate> GenerateLowestResolutionCandidates(
const SearchParameters& search_parameters, int num_discrete_scans) const;
void ScoreCandidates(int depth,

16
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

@ -145,9 +145,9 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
float rotational_score = 0.f;
float low_resolution_score = 0.f;
EXPECT_TRUE(fast_correlative_scan_matcher->Match(
transform::Rigid3d::Identity(), CreateConstantData(point_cloud_),
kMinScore, &score, &pose_estimate, &rotational_score,
&low_resolution_score));
transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
CreateConstantData(point_cloud_), kMinScore, &score, &pose_estimate,
&rotational_score, &low_resolution_score));
EXPECT_LT(kMinScore, score);
EXPECT_LT(0.09f, rotational_score);
EXPECT_LT(0.14f, low_resolution_score);
@ -156,7 +156,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
<< "Actual: " << transform::ToProto(pose_estimate).DebugString()
<< "\nExpected: " << transform::ToProto(expected_pose).DebugString();
EXPECT_FALSE(fast_correlative_scan_matcher->Match(
transform::Rigid3d::Identity(),
transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore,
&score, &pose_estimate, &rotational_score, &low_resolution_score))
<< low_resolution_score;
@ -174,9 +174,9 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
float rotational_score = 0.f;
float low_resolution_score = 0.f;
EXPECT_TRUE(fast_correlative_scan_matcher->MatchFullSubmap(
Eigen::Quaterniond::Identity(), CreateConstantData(point_cloud_),
kMinScore, &score, &pose_estimate, &rotational_score,
&low_resolution_score));
Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
CreateConstantData(point_cloud_), kMinScore, &score, &pose_estimate,
&rotational_score, &low_resolution_score));
EXPECT_LT(kMinScore, score);
EXPECT_LT(0.09f, rotational_score);
EXPECT_LT(0.14f, low_resolution_score);
@ -185,7 +185,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
<< "Actual: " << transform::ToProto(pose_estimate).DebugString()
<< "\nExpected: " << transform::ToProto(expected_pose).DebugString();
EXPECT_FALSE(fast_correlative_scan_matcher->MatchFullSubmap(
Eigen::Quaterniond::Identity(),
Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore,
&score, &pose_estimate, &rotational_score, &low_resolution_score))
<< low_resolution_score;

35
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -179,19 +179,21 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) {
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
const transform::Rigid3d inverse_submap_pose =
optimization_problem_.submap_data().at(submap_id).pose.inverse();
const transform::Rigid3d initial_relative_pose =
inverse_submap_pose *
const transform::Rigid3d global_node_pose =
optimization_problem_.node_data().at(node_id).global_pose;
const transform::Rigid3d global_submap_pose =
optimization_problem_.submap_data().at(submap_id).pose;
const transform::Rigid3d global_submap_pose_inverse =
global_submap_pose.inverse();
std::vector<mapping::TrajectoryNode> submap_nodes;
for (const mapping::NodeId& submap_node_id :
submap_data_.at(submap_id).node_ids) {
submap_nodes.push_back(mapping::TrajectoryNode{
trajectory_nodes_.at(submap_node_id).constant_data,
inverse_submap_pose *
global_submap_pose_inverse *
trajectory_nodes_.at(submap_node_id).global_pose});
}
@ -211,24 +213,17 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
initial_relative_pose);
global_node_pose, global_submap_pose);
} else if (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.
// In this situation, 'global_node_pose' and 'global_submap_pose' have
// orientations agreeing on gravity. Their relationship regarding yaw is
// arbitrary. 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_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
initial_relative_pose.rotation());
global_node_pose.rotation(), global_submap_pose.rotation());
}
}

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

@ -59,8 +59,10 @@ void ConstraintBuilder::MaybeAddConstraint(
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const transform::Rigid3d& initial_pose) {
if (initial_pose.translation().norm() > options_.max_constraint_distance()) {
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose) {
if ((global_node_pose.translation() - global_submap_pose.translation())
.norm() > options_.max_constraint_distance()) {
return;
}
if (sampler_.Pulse()) {
@ -72,7 +74,8 @@ void ConstraintBuilder::MaybeAddConstraint(
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, node_id, false, /* match_full_submap */
constant_data, initial_pose, constraint);
constant_data, global_node_pose, global_submap_pose,
constraint);
FinishComputation(current_computation);
});
}
@ -83,7 +86,8 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& gravity_alignment) {
const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation) {
common::MutexLocker locker(&mutex_);
constraints_.emplace_back();
auto* const constraint = &constraints_.back();
@ -91,10 +95,10 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, node_id, true, /* match_full_submap */
constant_data,
transform::Rigid3d::Rotation(gravity_alignment),
constraint);
ComputeConstraint(
submap_id, node_id, true, /* match_full_submap */
constant_data, transform::Rigid3d::Rotation(global_node_rotation),
transform::Rigid3d::Rotation(global_submap_rotation), constraint);
FinishComputation(current_computation);
});
}
@ -166,7 +170,8 @@ void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::NodeId& node_id,
bool match_full_submap,
const mapping::TrajectoryNode::Data* const constant_data,
const transform::Rigid3d& initial_pose,
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose,
std::unique_ptr<OptimizationProblem::Constraint>* constraint) {
const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_id);
@ -178,6 +183,7 @@ void ConstraintBuilder::ComputeConstraint(
transform::Rigid3d pose_estimate;
float rotational_score = 0.f;
float low_resolution_score = 0.f;
// TODO(gaschler): Match methods should return unique_ptr<struct>.
// Compute 'pose_estimate' in three stages:
// 1. Fast estimate using the fast correlative scan matcher.
@ -185,9 +191,9 @@ void ConstraintBuilder::ComputeConstraint(
// 3. Refine.
if (match_full_submap) {
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
initial_pose.rotation(), *constant_data,
options_.global_localization_min_score(), &score, &pose_estimate,
&rotational_score, &low_resolution_score)) {
global_node_pose.rotation(), global_submap_pose.rotation(),
*constant_data, options_.global_localization_min_score(), &score,
&pose_estimate, &rotational_score, &low_resolution_score)) {
CHECK_GT(score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
@ -196,8 +202,9 @@ void ConstraintBuilder::ComputeConstraint(
}
} else {
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
initial_pose, *constant_data, options_.min_score(), &score,
&pose_estimate, &rotational_score, &low_resolution_score)) {
global_node_pose, global_submap_pose, *constant_data,
options_.min_score(), &score, &pose_estimate, &rotational_score,
&low_resolution_score)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
} else {
@ -238,8 +245,9 @@ void ConstraintBuilder::ComputeConstraint(
if (match_full_submap) {
info << " matches";
} else {
const transform::Rigid3d difference =
initial_pose.inverse() * constraint_transform;
const transform::Rigid3d difference = global_submap_pose.inverse() *
global_node_pose *
constraint_transform;
info << " differs by translation " << std::setprecision(2)
<< difference.translation().norm() << " rotation "
<< std::setprecision(3) << transform::GetAngle(difference);

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

@ -68,7 +68,9 @@ class ConstraintBuilder {
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id', and the 'compressed_point_cloud' for 'node_id'.
// The 'initial_pose' is relative to the 'submap'.
//
// 'global_node_pose' and 'global_submap_pose' are initial estimates of poses
// in the global map frame, i.e. both are gravity aligned.
//
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
@ -77,14 +79,15 @@ class ConstraintBuilder {
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const transform::Rigid3d& initial_pose);
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose);
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
// This performs full-submap matching.
//
// The 'gravity_alignment' is the rotation to apply to the point cloud data
// to make it approximately gravity aligned.
// 'global_node_rotation' and 'global_submap_rotation' are initial estimates
// of roll and pitch, i.e. their yaw is essentially ignored.
//
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
@ -93,7 +96,8 @@ class ConstraintBuilder {
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& gravity_alignment);
const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation);
// Must be called after all computations related to one node have been added.
void NotifyEndOfScan();
@ -141,7 +145,8 @@ class ConstraintBuilder {
const mapping::SubmapId& submap_id, const mapping::NodeId& node_id,
bool match_full_submap,
const mapping::TrajectoryNode::Data* const constant_data,
const transform::Rigid3d& initial_pose,
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose,
std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);
// Decrements the 'pending_computations_' count. If all computations are done,