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() *
extrapolator_->ExtrapolatePose(time).cast<float>();
// TODO(gaschler): Try to move this common behavior with 3D into helper.
const sensor::TimedRangeData range_data_in_first_tracking =
sensor::TransformTimedRangeData(range_data, tracking_delta);
const Eigen::Vector3f origin_in_first_tracking =
tracking_delta * range_data.origin;
// Drop any returns below the minimum range and convert returns beyond the
// 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 =
hit.head<3>() - range_data_in_first_tracking.origin;
hit_in_first_tracking - origin_in_first_tracking;
const float range = delta.norm();
if (range >= options_.min_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 {
accumulated_range_data_.misses.push_back(
range_data_in_first_tracking.origin +
origin_in_first_tracking +
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 =
first_pose_estimate_.inverse() *
extrapolator_->ExtrapolatePose(time).cast<float>();
const sensor::TimedRangeData range_data_in_first_tracking =
sensor::TransformTimedRangeData(range_data, tracking_delta);
for (const Eigen::Vector4f& hit : range_data_in_first_tracking.returns) {
const Eigen::Vector3f origin_in_first_tracking =
tracking_delta * range_data.origin;
for (const Eigen::Vector4f& hit : range_data.returns) {
const Eigen::Vector3f hit_in_first_tracking =
tracking_delta * hit.head<3>();
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();
if (range >= options_.min_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 {
// 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
// be updated.
accumulated_range_data_.misses.push_back(
range_data_in_first_tracking.origin +
options_.max_range() / range * delta);
origin_in_first_tracking + options_.max_range() / range * delta);
}
}
}