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

Add 'gravity_alignment' rotation to each node. (#500) 

In 2D this replaces the 'tracking_to_tracking_2d' transform.
Also changes the 2D SparsePoseGraph to get the full 3D pose.
master
Wolfgang Hess 2017-09-04 16:24:26 +02:00 committed by GitHub
parent 3948943b64
commit c8de50bd2b
9 changed files with 43 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
cartographer/mapping/trajectory_node.h
View File

@ -35,16 +35,17 @@ struct TrajectoryNode {
// Range data in 'tracking' frame. Only used in 3D.
sensor::CompressedRangeData range_data;
// Used for loop closure in 2D: voxel filtered returns in 'pose' frame.
sensor::PointCloud filtered_point_cloud;
// Transform to approximately gravity align the tracking frame as
// determined by local SLAM.
Eigen::Quaterniond gravity_alignment;
// Used for loop closure in 2D: voxel filtered returns in the
// 'gravity_alignment' frame.
sensor::PointCloud filtered_gravity_aligned_point_cloud;
// Used for loop closure in 3D.
sensor::PointCloud high_resolution_point_cloud;
sensor::PointCloud low_resolution_point_cloud;
// Transforms the 'tracking' frame into a gravity-aligned 'tracking_2d'
// frame. Only used in 2D.
transform::Rigid3d tracking_to_tracking_2d;
};
common::Time time() const { return constant_data->time; }

5
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -181,11 +181,12 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
mapping::TrajectoryNode::Data{
time,
{}, // 'range_data' is only used in 3D.
gravity_alignment.rotation(),
filtered_gravity_aligned_point_cloud,
{}, // 'high_resolution_point_cloud' is only used in 3D.
{}, // 'low_resolution_point_cloud' is only used in 3D.
gravity_alignment}),
pose_estimate_2d, std::move(insertion_submaps)});
}),
pose_estimate, std::move(insertion_submaps)});
}
const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {

2
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -42,7 +42,7 @@ class LocalTrajectoryBuilder {
public:
struct InsertionResult {
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
transform::Rigid2d pose_observation;
transform::Rigid3d pose_observation;
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
};

24
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -95,11 +95,10 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
void SparsePoseGraph::AddScan(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
const transform::Rigid2d& pose, const int trajectory_id,
const transform::Rigid3d& pose, const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose) *
constant_data->tracking_to_tracking_2d);
GetLocalToGlobalTransform(trajectory_id) * pose);
common::MutexLocker locker(&mutex_);
trajectory_nodes_.Append(
@ -132,8 +131,11 @@ void SparsePoseGraph::AddScan(
// execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(trajectory_id, insertion_submaps,
newly_finished_submap, pose);
ComputeConstraintsForScan(
trajectory_id, insertion_submaps, newly_finished_submap,
transform::Project2D(pose *
transform::Rigid3d::Rotation(
constant_data->gravity_alignment.inverse())));
});
}
@ -445,9 +447,10 @@ void SparsePoseGraph::RunOptimization() {
for (; node_data_index != static_cast<int>(node_data[trajectory_id].size());
++node_data_index, ++node_index) {
const mapping::NodeId node_id{trajectory_id, node_index};
trajectory_nodes_.at(node_id).pose =
auto& node = trajectory_nodes_.at(node_id);
node.pose =
transform::Embed3D(node_data[trajectory_id][node_data_index].pose) *
trajectory_nodes_.at(node_id).constant_data->tracking_to_tracking_2d;
transform::Rigid3d::Rotation(node.constant_data->gravity_alignment);
}
// Extrapolate all point cloud poses that were added later.
const auto local_to_new_global =
@ -458,8 +461,8 @@ void SparsePoseGraph::RunOptimization() {
local_to_new_global * local_to_old_global.inverse();
for (; node_index < num_nodes; ++node_index) {
const mapping::NodeId node_id{trajectory_id, node_index};
trajectory_nodes_.at(node_id).pose =
old_global_to_new_global * trajectory_nodes_.at(node_id).pose;
auto& node_pose = trajectory_nodes_.at(node_id).pose;
node_pose = old_global_to_new_global * node_pose;
}
}
optimized_submap_transforms_ = submap_data;
@ -490,8 +493,9 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
result.push_back(Constraint{
constraint.submap_id, constraint.node_id,
Constraint::Pose{constraint.pose.zbar_ij *
transform::Rigid3d::Rotation(
trajectory_nodes_.at(constraint.node_id)
.constant_data->tracking_to_tracking_2d,
.constant_data->gravity_alignment),
constraint.pose.translation_weight,
constraint.pose.rotation_weight},
constraint.tag});

2
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -71,7 +71,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// 'true', this submap was inserted into for the last time.
void AddScan(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
const transform::Rigid2d& pose, int trajectory_id,
const transform::Rigid3d& pose, int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
EXCLUDES(mutex_);

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

@ -172,7 +172,7 @@ void ConstraintBuilder::ComputeConstraint(
GetSubmapScanMatcher(submap_id);
// The 'constraint_transform' (submap i <- scan j) is computed from:
// - a 'filtered_point_cloud' in scan j,
// - a 'filtered_gravity_aligned_point_cloud' in scan j,
// - the initial guess 'initial_pose' for (map <- scan j),
// - the result 'pose_estimate' of Match() (map <- scan j).
// - the ComputeSubmapPose() (map <- submap i)
@ -185,7 +185,7 @@ void ConstraintBuilder::ComputeConstraint(
// 3. Refine.
if (match_full_submap) {
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
constant_data->filtered_point_cloud,
constant_data->filtered_gravity_aligned_point_cloud,
options_.global_localization_min_score(), &score, &pose_estimate)) {
CHECK_GT(score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, 0);
@ -197,7 +197,7 @@ void ConstraintBuilder::ComputeConstraint(
}
} else {
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
initial_pose, constant_data->filtered_point_cloud,
initial_pose, constant_data->filtered_gravity_aligned_point_cloud,
options_.min_score(), &score, &pose_estimate)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
@ -214,9 +214,10 @@ void ConstraintBuilder::ComputeConstraint(
// effect that, in the absence of better information, we prefer the original
// CSM estimate.
ceres::Solver::Summary unused_summary;
ceres_scan_matcher_.Match(
pose_estimate, pose_estimate, constant_data->filtered_point_cloud,
*submap_scan_matcher->probability_grid, &pose_estimate, &unused_summary);
ceres_scan_matcher_.Match(pose_estimate, pose_estimate,
constant_data->filtered_gravity_aligned_point_cloud,
*submap_scan_matcher->probability_grid,
&pose_estimate, &unused_summary);
const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate;
@ -230,8 +231,8 @@ void ConstraintBuilder::ComputeConstraint(
if (options_.log_matches()) {
std::ostringstream info;
info << "Node " << node_id << " with "
<< constant_data->filtered_point_cloud.size() << " points on submap "
<< submap_id << std::fixed;
<< constant_data->filtered_gravity_aligned_point_cloud.size()
<< " points on submap " << submap_id << std::fixed;
if (match_full_submap) {
info << " matches";
} else {

6
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -161,11 +161,11 @@ class SparsePoseGraphTest : public ::testing::Test {
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{common::FromUniversal(0),
Compress(range_data),
Eigen::Quaterniond::Identity(),
range_data.returns,
{},
{},
transform::Rigid3d::Identity()}),
pose_estimate, kTrajectoryId, insertion_submaps);
{}}),
transform::Embed3D(pose_estimate), kTrajectoryId, insertion_submaps);
}
void MoveRelative(const transform::Rigid2d& movement) {

4
cartographer/mapping_3d/local_trajectory_builder.cc
View File

@ -210,10 +210,10 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
mapping::TrajectoryNode::Data{
time,
sensor::Compress(range_data_in_tracking),
gravity_alignment,
{}, // 'filtered_point_cloud' is only used in 2D.
high_resolution_point_cloud,
low_resolution_point_cloud,
{}}), // 'tracking_to_tracking_2d' in only used in 2D.
low_resolution_point_cloud}),
pose_observation, std::move(insertion_submaps)});
}

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

@ -114,10 +114,10 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
return mapping::TrajectoryNode::Data{
common::FromUniversal(0),
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}}),
Eigen::Quaterniond::Identity(),
{},
point_cloud_,
low_resolution_point_cloud,
transform::Rigid3d::Identity()};
low_resolution_point_cloud};
}
std::mt19937 prng_ = std::mt19937(42);