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

Reduce transforms in LocalTrajectoryBuilder. (#668) 

This speeds up AddRangeData because we avoid one copy and loop over all points.
master
gaschler 2017-11-15 09:15:02 +01:00 committed by Wally B. Feed
parent 8c9fac4c69
commit 7904808d40
2 changed files with 16 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -97,20 +97,22 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
first_pose_estimate_.inverse() * first_pose_estimate_.inverse() *
extrapolator_->ExtrapolatePose(time).cast<float>(); extrapolator_->ExtrapolatePose(time).cast<float>();
// TODO(gaschler): Try to move this common behavior with 3D into helper. // TODO(gaschler): Try to move this common behavior with 3D into helper.
const sensor::TimedRangeData range_data_in_first_tracking = const Eigen::Vector3f origin_in_first_tracking =
sensor::TransformTimedRangeData(range_data, tracking_delta); tracking_delta * range_data.origin;
// Drop any returns below the minimum range and convert returns beyond the // Drop any returns below the minimum range and convert returns beyond the
// maximum range into misses. // maximum range into misses.
for (const Eigen::Vector4f& hit : range_data_in_first_tracking.returns) { for (const Eigen::Vector4f& hit : range_data.returns) {
const Eigen::Vector3f hit_in_first_tracking =
tracking_delta * hit.head<3>();
const Eigen::Vector3f delta = const Eigen::Vector3f delta =
hit.head<3>() - range_data_in_first_tracking.origin; hit_in_first_tracking - origin_in_first_tracking;
const float range = delta.norm(); const float range = delta.norm();
if (range >= options_.min_range()) { if (range >= options_.min_range()) {
if (range <= options_.max_range()) { if (range <= options_.max_range()) {
accumulated_range_data_.returns.push_back(hit.head<3>()); accumulated_range_data_.returns.push_back(hit_in_first_tracking);
} else { } else {
accumulated_range_data_.misses.push_back( accumulated_range_data_.misses.push_back(
range_data_in_first_tracking.origin + origin_in_first_tracking +
options_.missing_data_ray_length() / range * delta); options_.missing_data_ray_length() / range * delta);
} }
} }

15
cartographer/mapping_3d/local_trajectory_builder.cc
View File

@ -76,22 +76,23 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
const transform::Rigid3f tracking_delta = const transform::Rigid3f tracking_delta =
first_pose_estimate_.inverse() * first_pose_estimate_.inverse() *
extrapolator_->ExtrapolatePose(time).cast<float>(); extrapolator_->ExtrapolatePose(time).cast<float>();
const sensor::TimedRangeData range_data_in_first_tracking = const Eigen::Vector3f origin_in_first_tracking =
sensor::TransformTimedRangeData(range_data, tracking_delta); tracking_delta * range_data.origin;
for (const Eigen::Vector4f& hit : range_data_in_first_tracking.returns) { for (const Eigen::Vector4f& hit : range_data.returns) {
const Eigen::Vector3f hit_in_first_tracking =
tracking_delta * hit.head<3>();
const Eigen::Vector3f delta = const Eigen::Vector3f delta =
hit.head<3>() - range_data_in_first_tracking.origin; hit_in_first_tracking - origin_in_first_tracking;
const float range = delta.norm(); const float range = delta.norm();
if (range >= options_.min_range()) { if (range >= options_.min_range()) {
if (range <= options_.max_range()) { if (range <= options_.max_range()) {
accumulated_range_data_.returns.push_back(hit.head<3>()); accumulated_range_data_.returns.push_back(hit_in_first_tracking);
} else { } else {
// We insert a ray cropped to 'max_range' as a miss for hits beyond the // We insert a ray cropped to 'max_range' as a miss for hits beyond the
// maximum range. This way the free space up to the maximum range will // maximum range. This way the free space up to the maximum range will
// be updated. // be updated.
accumulated_range_data_.misses.push_back( accumulated_range_data_.misses.push_back(
range_data_in_first_tracking.origin + origin_in_first_tracking + options_.max_range() / range * delta);
options_.max_range() / range * delta);
} }
} }
} }