Unwarp by point in LocalTrajectoryBuilder. (#636)

master
gaschler 2017-12-13 18:29:42 +01:00 committed by Wally B. Feed
parent e16d1b1207
commit 59d1b968bc
6 changed files with 89 additions and 48 deletions

View File

@ -94,32 +94,45 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
LOG(INFO) << "Extrapolator not yet initialized."; LOG(INFO) << "Extrapolator not yet initialized.";
return nullptr; return nullptr;
} }
if (num_accumulated_ == 0) {
first_pose_estimate_ = extrapolator_->ExtrapolatePose(time).cast<float>(); CHECK(!range_data.returns.empty());
accumulated_range_data_ = sensor::RangeData{range_data.origin, {}, {}}; CHECK_EQ(range_data.returns.back()[3], 0);
const common::Time time_first_point =
time + common::FromSeconds(range_data.returns.front()[3]);
if (time_first_point < extrapolator_->GetLastPoseTime()) {
LOG(INFO) << "Extrapolator is still initializing.";
return nullptr;
}
std::vector<transform::Rigid3f> range_data_poses;
range_data_poses.reserve(range_data.returns.size());
for (const Eigen::Vector4f& hit : range_data.returns) {
const common::Time time_point = time + common::FromSeconds(hit[3]);
range_data_poses.push_back(
extrapolator_->ExtrapolatePose(time_point).cast<float>());
}
if (num_accumulated_ == 0) {
// 'accumulated_range_data_.origin' is uninitialized until the last
// accumulation.
accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
} }
// TODO(gaschler): Take time delta of individual points into account.
const transform::Rigid3f tracking_delta =
first_pose_estimate_.inverse() *
extrapolator_->ExtrapolatePose(time).cast<float>();
// TODO(gaschler): Try to move this common behavior with 3D into helper.
const Eigen::Vector3f origin_in_first_tracking =
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.returns) { for (size_t i = 0; i < range_data.returns.size(); ++i) {
const Eigen::Vector3f hit_in_first_tracking = const Eigen::Vector4f& hit = range_data.returns[i];
tracking_delta * hit.head<3>(); const Eigen::Vector3f origin_in_local =
const Eigen::Vector3f delta = range_data_poses[i] * range_data.origin;
hit_in_first_tracking - origin_in_first_tracking; const Eigen::Vector3f hit_in_local = range_data_poses[i] * hit.head<3>();
const Eigen::Vector3f delta = hit_in_local - origin_in_local;
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_in_first_tracking); accumulated_range_data_.returns.push_back(hit_in_local);
} else { } else {
accumulated_range_data_.misses.push_back( accumulated_range_data_.misses.push_back(
origin_in_first_tracking + origin_in_local +
options_.missing_data_ray_length() / range * delta); options_.missing_data_ray_length() / range * delta);
} }
} }
@ -130,10 +143,12 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
num_accumulated_ = 0; num_accumulated_ = 0;
const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation( const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
extrapolator_->EstimateGravityOrientation(time)); extrapolator_->EstimateGravityOrientation(time));
accumulated_range_data_.origin =
range_data_poses.back() * range_data.origin;
return AddAccumulatedRangeData( return AddAccumulatedRangeData(
time, time,
TransformToGravityAlignedFrameAndFilter( TransformToGravityAlignedFrameAndFilter(
gravity_alignment.cast<float>() * tracking_delta.inverse(), gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
accumulated_range_data_), accumulated_range_data_),
gravity_alignment); gravity_alignment);
} }
@ -238,6 +253,7 @@ void LocalTrajectoryBuilder::InitializeExtrapolator(const common::Time time) {
// stability. Usually poses known to the extrapolator will be further apart // stability. Usually poses known to the extrapolator will be further apart
// in time and thus the last two are used. // in time and thus the last two are used.
constexpr double kExtrapolationEstimationTimeSec = 0.001; constexpr double kExtrapolationEstimationTimeSec = 0.001;
// TODO(gaschler): Consider using InitializeWithImu as 3D does.
extrapolator_ = common::make_unique<mapping::PoseExtrapolator>( extrapolator_ = common::make_unique<mapping::PoseExtrapolator>(
::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
options_.imu_gravity_time_constant()); options_.imu_gravity_time_constant());

View File

@ -61,7 +61,9 @@ class LocalTrajectoryBuilder {
// Returns 'MatchingResult' when range data accumulation completed, // Returns 'MatchingResult' when range data accumulation completed,
// otherwise 'nullptr'. Range data must be approximately horizontal // otherwise 'nullptr'. Range data must be approximately horizontal
// for 2D SLAM. // for 2D SLAM. `time` is when the last point in `range_data` was
// acquired, `range_data` contains the relative time of point with
// respect to `time`.
std::unique_ptr<MatchingResult> AddRangeData( std::unique_ptr<MatchingResult> AddRangeData(
common::Time, const sensor::TimedRangeData& range_data); common::Time, const sensor::TimedRangeData& range_data);
void AddImuData(const sensor::ImuData& imu_data); void AddImuData(const sensor::ImuData& imu_data);
@ -101,7 +103,6 @@ class LocalTrajectoryBuilder {
std::unique_ptr<mapping::PoseExtrapolator> extrapolator_; std::unique_ptr<mapping::PoseExtrapolator> extrapolator_;
int num_accumulated_ = 0; int num_accumulated_ = 0;
transform::Rigid3f first_pose_estimate_ = transform::Rigid3f::Identity();
sensor::RangeData accumulated_range_data_; sensor::RangeData accumulated_range_data_;
}; };

View File

@ -67,32 +67,47 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
LOG(INFO) << "IMU not yet initialized."; LOG(INFO) << "IMU not yet initialized.";
return nullptr; return nullptr;
} }
if (num_accumulated_ == 0) {
first_pose_estimate_ = extrapolator_->ExtrapolatePose(time).cast<float>(); CHECK(!range_data.returns.empty());
accumulated_range_data_ = sensor::RangeData{range_data.origin, {}, {}}; CHECK_EQ(range_data.returns.back()[3], 0);
const common::Time time_first_point =
time + common::FromSeconds(range_data.returns.front()[3]);
if (time_first_point < extrapolator_->GetLastPoseTime()) {
LOG(INFO) << "Extrapolator is still initializing.";
return nullptr;
} }
// TODO(gaschler): Take time delta of individual points into account. sensor::TimedPointCloud hits =
const transform::Rigid3f tracking_delta = sensor::VoxelFilter(0.5f * options_.voxel_filter_size())
first_pose_estimate_.inverse() * .Filter(range_data.returns);
extrapolator_->ExtrapolatePose(time).cast<float>();
const Eigen::Vector3f origin_in_first_tracking = std::vector<transform::Rigid3f> hits_poses;
tracking_delta * range_data.origin; hits_poses.reserve(hits.size());
for (const Eigen::Vector4f& hit : range_data.returns) { for (const Eigen::Vector4f& hit : hits) {
const Eigen::Vector3f hit_in_first_tracking = const common::Time time_point = time + common::FromSeconds(hit[3]);
tracking_delta * hit.head<3>(); hits_poses.push_back(
const Eigen::Vector3f delta = extrapolator_->ExtrapolatePose(time_point).cast<float>());
hit_in_first_tracking - origin_in_first_tracking; }
if (num_accumulated_ == 0) {
// 'accumulated_range_data_.origin' is not used.
accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
}
for (size_t i = 0; i < hits.size(); ++i) {
const Eigen::Vector3f hit_in_local = hits_poses[i] * hits[i].head<3>();
const Eigen::Vector3f origin_in_local = hits_poses[i] * range_data.origin;
const Eigen::Vector3f delta = hit_in_local - origin_in_local;
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_in_first_tracking); accumulated_range_data_.returns.push_back(hit_in_local);
} 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(
origin_in_first_tracking + options_.max_range() / range * delta); origin_in_local + options_.max_range() / range * delta);
} }
} }
} }
@ -100,15 +115,17 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
if (num_accumulated_ >= options_.num_accumulated_range_data()) { if (num_accumulated_ >= options_.num_accumulated_range_data()) {
num_accumulated_ = 0; num_accumulated_ = 0;
transform::Rigid3f current_pose =
extrapolator_->ExtrapolatePose(time).cast<float>();
const sensor::RangeData filtered_range_data = { const sensor::RangeData filtered_range_data = {
accumulated_range_data_.origin, current_pose * range_data.origin,
sensor::VoxelFilter(options_.voxel_filter_size()) sensor::VoxelFilter(options_.voxel_filter_size())
.Filter(accumulated_range_data_.returns), .Filter(accumulated_range_data_.returns),
sensor::VoxelFilter(options_.voxel_filter_size()) sensor::VoxelFilter(options_.voxel_filter_size())
.Filter(accumulated_range_data_.misses)}; .Filter(accumulated_range_data_.misses)};
return AddAccumulatedRangeData( return AddAccumulatedRangeData(
time, sensor::TransformRangeData(filtered_range_data, time, sensor::TransformRangeData(filtered_range_data,
tracking_delta.inverse())); current_pose.inverse()));
} }
return nullptr; return nullptr;
} }

View File

@ -60,7 +60,9 @@ class LocalTrajectoryBuilder {
void AddImuData(const sensor::ImuData& imu_data); void AddImuData(const sensor::ImuData& imu_data);
// Returns 'MatchingResult' when range data accumulation completed, // Returns 'MatchingResult' when range data accumulation completed,
// otherwise 'nullptr'. // otherwise 'nullptr'. `time` is when the last point in `range_data`
// was acquired, `range_data` contains the relative time of point with
// respect to `time`.
std::unique_ptr<MatchingResult> AddRangeData( std::unique_ptr<MatchingResult> AddRangeData(
common::Time time, const sensor::TimedRangeData& range_data); common::Time time, const sensor::TimedRangeData& range_data);
void AddOdometryData(const sensor::OdometryData& odometry_data); void AddOdometryData(const sensor::OdometryData& odometry_data);
@ -89,7 +91,6 @@ class LocalTrajectoryBuilder {
std::unique_ptr<mapping::PoseExtrapolator> extrapolator_; std::unique_ptr<mapping::PoseExtrapolator> extrapolator_;
int num_accumulated_ = 0; int num_accumulated_ = 0;
transform::Rigid3f first_pose_estimate_ = transform::Rigid3f::Identity();
sensor::RangeData accumulated_range_data_; sensor::RangeData accumulated_range_data_;
}; };

View File

@ -28,7 +28,9 @@ namespace mapping {
PoseExtrapolator::PoseExtrapolator(const common::Duration pose_queue_duration, PoseExtrapolator::PoseExtrapolator(const common::Duration pose_queue_duration,
double imu_gravity_time_constant) double imu_gravity_time_constant)
: pose_queue_duration_(pose_queue_duration), : pose_queue_duration_(pose_queue_duration),
gravity_time_constant_(imu_gravity_time_constant) {} gravity_time_constant_(imu_gravity_time_constant),
cached_extrapolated_pose_{common::Time::min(),
transform::Rigid3d::Identity()} {}
std::unique_ptr<PoseExtrapolator> PoseExtrapolator::InitializeWithImu( std::unique_ptr<PoseExtrapolator> PoseExtrapolator::InitializeWithImu(
const common::Duration pose_queue_duration, const common::Duration pose_queue_duration,
@ -130,15 +132,18 @@ void PoseExtrapolator::AddOdometryData(
} }
transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) { transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
// TODO(whess): Keep the last extrapolated pose.
const TimedPose& newest_timed_pose = timed_pose_queue_.back(); const TimedPose& newest_timed_pose = timed_pose_queue_.back();
CHECK_GE(time, newest_timed_pose.time); CHECK_GE(time, newest_timed_pose.time);
const Eigen::Vector3d translation = if (cached_extrapolated_pose_.time != time) {
ExtrapolateTranslation(time) + newest_timed_pose.pose.translation(); const Eigen::Vector3d translation =
const Eigen::Quaterniond rotation = ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
newest_timed_pose.pose.rotation() * const Eigen::Quaterniond rotation =
ExtrapolateRotation(time, extrapolation_imu_tracker_.get()); newest_timed_pose.pose.rotation() *
return transform::Rigid3d(translation, rotation); ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
cached_extrapolated_pose_ =
TimedPose{time, transform::Rigid3d{translation, rotation}};
}
return cached_extrapolated_pose_.pose;
} }
Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation( Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation(

View File

@ -80,6 +80,7 @@ class PoseExtrapolator {
std::unique_ptr<ImuTracker> imu_tracker_; std::unique_ptr<ImuTracker> imu_tracker_;
std::unique_ptr<ImuTracker> odometry_imu_tracker_; std::unique_ptr<ImuTracker> odometry_imu_tracker_;
std::unique_ptr<ImuTracker> extrapolation_imu_tracker_; std::unique_ptr<ImuTracker> extrapolation_imu_tracker_;
TimedPose cached_extrapolated_pose_;
std::deque<sensor::OdometryData> odometry_data_; std::deque<sensor::OdometryData> odometry_data_;
Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero(); Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();