Reduce transforms in LocalTrajectoryBuilder. (#668)
This speeds up AddRangeData because we avoid one copy and loop over all points.master
parent
8c9fac4c69
commit
7904808d40
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -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);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue