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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
cartographer/internal/mapping_2d/local_trajectory_builder.cc
View File

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

5
cartographer/internal/mapping_2d/local_trajectory_builder.h
View File

@ -61,7 +61,9 @@ class LocalTrajectoryBuilder {
// Returns 'MatchingResult' when range data accumulation completed,
// 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(
common::Time, const sensor::TimedRangeData& range_data);
void AddImuData(const sensor::ImuData& imu_data);
@ -101,7 +103,6 @@ class LocalTrajectoryBuilder {
std::unique_ptr<mapping::PoseExtrapolator> extrapolator_;
int num_accumulated_ = 0;
transform::Rigid3f first_pose_estimate_ = transform::Rigid3f::Identity();
sensor::RangeData accumulated_range_data_;
};

53
cartographer/internal/mapping_3d/local_trajectory_builder.cc
View File

@ -67,32 +67,47 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
LOG(INFO) << "IMU not yet initialized.";
return nullptr;
}
if (num_accumulated_ == 0) {
first_pose_estimate_ = extrapolator_->ExtrapolatePose(time).cast<float>();
accumulated_range_data_ = sensor::RangeData{range_data.origin, {}, {}};
CHECK(!range_data.returns.empty());
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.
const transform::Rigid3f tracking_delta =
first_pose_estimate_.inverse() *
extrapolator_->ExtrapolatePose(time).cast<float>();
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_in_first_tracking - origin_in_first_tracking;
sensor::TimedPointCloud hits =
sensor::VoxelFilter(0.5f * options_.voxel_filter_size())
.Filter(range_data.returns);
std::vector<transform::Rigid3f> hits_poses;
hits_poses.reserve(hits.size());
for (const Eigen::Vector4f& hit : hits) {
const common::Time time_point = time + common::FromSeconds(hit[3]);
hits_poses.push_back(
extrapolator_->ExtrapolatePose(time_point).cast<float>());
}
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();
if (range >= options_.min_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 {
// 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(
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()) {
num_accumulated_ = 0;
transform::Rigid3f current_pose =
extrapolator_->ExtrapolatePose(time).cast<float>();
const sensor::RangeData filtered_range_data = {
accumulated_range_data_.origin,
current_pose * range_data.origin,
sensor::VoxelFilter(options_.voxel_filter_size())
.Filter(accumulated_range_data_.returns),
sensor::VoxelFilter(options_.voxel_filter_size())
.Filter(accumulated_range_data_.misses)};
return AddAccumulatedRangeData(
time, sensor::TransformRangeData(filtered_range_data,
tracking_delta.inverse()));
current_pose.inverse()));
}
return nullptr;
}

5
cartographer/internal/mapping_3d/local_trajectory_builder.h
View File

@ -60,7 +60,9 @@ class LocalTrajectoryBuilder {
void AddImuData(const sensor::ImuData& imu_data);
// 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(
common::Time time, const sensor::TimedRangeData& range_data);
void AddOdometryData(const sensor::OdometryData& odometry_data);
@ -89,7 +91,6 @@ class LocalTrajectoryBuilder {
std::unique_ptr<mapping::PoseExtrapolator> extrapolator_;
int num_accumulated_ = 0;
transform::Rigid3f first_pose_estimate_ = transform::Rigid3f::Identity();
sensor::RangeData accumulated_range_data_;
};

21
cartographer/mapping/pose_extrapolator.cc
View File

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

1
cartographer/mapping/pose_extrapolator.h
View File

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