Use PoseExtrapolator in the 3D trajectory builder. (#450)
This replaces the Predict() logic in the 3D trajectory builder.master
parent
73b0e5e20a
commit
e79a918989
|
@ -30,6 +30,25 @@ PoseExtrapolator::PoseExtrapolator(const common::Duration pose_queue_duration,
|
||||||
: pose_queue_duration_(pose_queue_duration),
|
: pose_queue_duration_(pose_queue_duration),
|
||||||
gravity_time_constant_(gravity_time_constant) {}
|
gravity_time_constant_(gravity_time_constant) {}
|
||||||
|
|
||||||
|
std::unique_ptr<PoseExtrapolator> PoseExtrapolator::InitializeWithImu(
|
||||||
|
const common::Duration pose_queue_duration,
|
||||||
|
const double imu_gravity_time_constant, const sensor::ImuData& imu_data) {
|
||||||
|
auto extrapolator = common::make_unique<PoseExtrapolator>(
|
||||||
|
pose_queue_duration, imu_gravity_time_constant);
|
||||||
|
extrapolator->AddImuData(imu_data);
|
||||||
|
extrapolator->imu_tracker_ =
|
||||||
|
common::make_unique<ImuTracker>(imu_gravity_time_constant, imu_data.time);
|
||||||
|
extrapolator->imu_tracker_->AddImuLinearAccelerationObservation(
|
||||||
|
imu_data.linear_acceleration);
|
||||||
|
extrapolator->imu_tracker_->AddImuAngularVelocityObservation(
|
||||||
|
imu_data.angular_velocity);
|
||||||
|
extrapolator->imu_tracker_->Advance(imu_data.time);
|
||||||
|
extrapolator->AddPose(
|
||||||
|
imu_data.time,
|
||||||
|
transform::Rigid3d::Rotation(extrapolator->imu_tracker_->orientation()));
|
||||||
|
return extrapolator;
|
||||||
|
}
|
||||||
|
|
||||||
common::Time PoseExtrapolator::GetLastPoseTime() const {
|
common::Time PoseExtrapolator::GetLastPoseTime() const {
|
||||||
if (timed_pose_queue_.empty()) {
|
if (timed_pose_queue_.empty()) {
|
||||||
return common::Time::min();
|
return common::Time::min();
|
||||||
|
|
|
@ -40,6 +40,10 @@ class PoseExtrapolator {
|
||||||
PoseExtrapolator(const PoseExtrapolator&) = delete;
|
PoseExtrapolator(const PoseExtrapolator&) = delete;
|
||||||
PoseExtrapolator& operator=(const PoseExtrapolator&) = delete;
|
PoseExtrapolator& operator=(const PoseExtrapolator&) = delete;
|
||||||
|
|
||||||
|
static std::unique_ptr<PoseExtrapolator> InitializeWithImu(
|
||||||
|
common::Duration pose_queue_duration, double imu_gravity_time_constant,
|
||||||
|
const sensor::ImuData& imu_data);
|
||||||
|
|
||||||
// Returns the time of the last added pose or Time::min() if no pose was added
|
// Returns the time of the last added pose or Time::min() if no pose was added
|
||||||
// yet.
|
// yet.
|
||||||
common::Time GetLastPoseTime() const;
|
common::Time GetLastPoseTime() const;
|
||||||
|
@ -49,6 +53,11 @@ class PoseExtrapolator {
|
||||||
void AddOdometryData(const sensor::OdometryData& odometry_data);
|
void AddOdometryData(const sensor::OdometryData& odometry_data);
|
||||||
transform::Rigid3d ExtrapolatePose(common::Time time);
|
transform::Rigid3d ExtrapolatePose(common::Time time);
|
||||||
|
|
||||||
|
// Gravity alignment estimate.
|
||||||
|
Eigen::Quaterniond gravity_orientation() const {
|
||||||
|
return imu_tracker_->orientation();
|
||||||
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void UpdateVelocitiesFromPoses();
|
void UpdateVelocitiesFromPoses();
|
||||||
void TrimImuData();
|
void TrimImuData();
|
||||||
|
|
|
@ -37,46 +37,43 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
|
||||||
options_.real_time_correlative_scan_matcher_options())),
|
options_.real_time_correlative_scan_matcher_options())),
|
||||||
ceres_scan_matcher_(common::make_unique<scan_matching::CeresScanMatcher>(
|
ceres_scan_matcher_(common::make_unique<scan_matching::CeresScanMatcher>(
|
||||||
options_.ceres_scan_matcher_options())),
|
options_.ceres_scan_matcher_options())),
|
||||||
odometry_state_tracker_(options_.num_odometry_states()),
|
|
||||||
accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {}
|
accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {}
|
||||||
|
|
||||||
LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
|
LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
|
||||||
|
|
||||||
void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
|
void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
|
||||||
const bool initial_imu_data = (imu_tracker_ == nullptr);
|
if (extrapolator_ != nullptr) {
|
||||||
if (initial_imu_data) {
|
extrapolator_->AddImuData(imu_data);
|
||||||
imu_tracker_ = common::make_unique<mapping::ImuTracker>(
|
return;
|
||||||
options_.imu_gravity_time_constant(), imu_data.time);
|
|
||||||
}
|
|
||||||
Predict(imu_data.time);
|
|
||||||
imu_tracker_->AddImuLinearAccelerationObservation(
|
|
||||||
imu_data.linear_acceleration);
|
|
||||||
imu_tracker_->AddImuAngularVelocityObservation(imu_data.angular_velocity);
|
|
||||||
if (initial_imu_data) {
|
|
||||||
// This uses the first accelerometer measurement to approximately align the
|
|
||||||
// first pose to gravity.
|
|
||||||
pose_estimate_ = transform::Rigid3d::Rotation(imu_tracker_->orientation());
|
|
||||||
}
|
}
|
||||||
|
// We derive velocities from poses which are at least 1 ms apart for numerical
|
||||||
|
// 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;
|
||||||
|
extrapolator_ = mapping::PoseExtrapolator::InitializeWithImu(
|
||||||
|
::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
|
||||||
|
options_.imu_gravity_time_constant(), imu_data);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
|
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
|
||||||
LocalTrajectoryBuilder::AddRangefinderData(const common::Time time,
|
LocalTrajectoryBuilder::AddRangefinderData(const common::Time time,
|
||||||
const Eigen::Vector3f& origin,
|
const Eigen::Vector3f& origin,
|
||||||
const sensor::PointCloud& ranges) {
|
const sensor::PointCloud& ranges) {
|
||||||
if (imu_tracker_ == nullptr) {
|
if (extrapolator_ == nullptr) {
|
||||||
LOG(INFO) << "ImuTracker not yet initialized.";
|
// Until we've initialized the extrapolator with our first IMU message, we
|
||||||
|
// cannot compute the orientation of the rangefinder.
|
||||||
|
LOG(INFO) << "IMU not yet initialized.";
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
Predict(time);
|
|
||||||
if (num_accumulated_ == 0) {
|
if (num_accumulated_ == 0) {
|
||||||
first_pose_estimate_ = pose_estimate_.cast<float>();
|
first_pose_estimate_ = extrapolator_->ExtrapolatePose(time).cast<float>();
|
||||||
accumulated_range_data_ =
|
accumulated_range_data_ =
|
||||||
sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}};
|
sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}};
|
||||||
}
|
}
|
||||||
|
|
||||||
const transform::Rigid3f tracking_delta =
|
const transform::Rigid3f tracking_delta =
|
||||||
first_pose_estimate_.inverse() * pose_estimate_.cast<float>();
|
first_pose_estimate_.inverse() *
|
||||||
|
extrapolator_->ExtrapolatePose(time).cast<float>();
|
||||||
const sensor::RangeData range_data_in_first_tracking =
|
const sensor::RangeData range_data_in_first_tracking =
|
||||||
sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}},
|
sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}},
|
||||||
tracking_delta);
|
tracking_delta);
|
||||||
|
@ -107,24 +104,6 @@ LocalTrajectoryBuilder::AddRangefinderData(const common::Time time,
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
void LocalTrajectoryBuilder::Predict(const common::Time time) {
|
|
||||||
CHECK(imu_tracker_ != nullptr);
|
|
||||||
CHECK_LE(time_, time);
|
|
||||||
const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
|
|
||||||
imu_tracker_->Advance(time);
|
|
||||||
if (time_ > common::Time::min()) {
|
|
||||||
const double delta_t = common::ToSeconds(time - time_);
|
|
||||||
// Constant velocity model.
|
|
||||||
const Eigen::Vector3d translation =
|
|
||||||
pose_estimate_.translation() + delta_t * velocity_estimate_;
|
|
||||||
const Eigen::Quaterniond rotation = pose_estimate_.rotation() *
|
|
||||||
last_orientation.inverse() *
|
|
||||||
imu_tracker_->orientation();
|
|
||||||
pose_estimate_ = transform::Rigid3d(translation, rotation);
|
|
||||||
}
|
|
||||||
time_ = time;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
|
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
|
||||||
LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
||||||
const common::Time time, const sensor::RangeData& range_data_in_tracking) {
|
const common::Time time, const sensor::RangeData& range_data_in_tracking) {
|
||||||
|
@ -140,11 +119,8 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
Predict(time);
|
const transform::Rigid3d pose_prediction =
|
||||||
const transform::Rigid3d odometry_prediction =
|
extrapolator_->ExtrapolatePose(time);
|
||||||
pose_estimate_ * odometry_correction_;
|
|
||||||
const transform::Rigid3d model_prediction = pose_estimate_;
|
|
||||||
const transform::Rigid3d& pose_prediction = odometry_prediction;
|
|
||||||
|
|
||||||
std::shared_ptr<const Submap> matching_submap =
|
std::shared_ptr<const Submap> matching_submap =
|
||||||
active_submaps_.submaps().front();
|
active_submaps_.submaps().front();
|
||||||
|
@ -177,57 +153,26 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
||||||
{&low_resolution_point_cloud_in_tracking,
|
{&low_resolution_point_cloud_in_tracking,
|
||||||
&matching_submap->low_resolution_hybrid_grid()}},
|
&matching_submap->low_resolution_hybrid_grid()}},
|
||||||
&pose_observation_in_submap, &summary);
|
&pose_observation_in_submap, &summary);
|
||||||
pose_estimate_ = matching_submap->local_pose() * pose_observation_in_submap;
|
const transform::Rigid3d pose_estimate =
|
||||||
|
matching_submap->local_pose() * pose_observation_in_submap;
|
||||||
odometry_correction_ = transform::Rigid3d::Identity();
|
extrapolator_->AddPose(time, pose_estimate);
|
||||||
if (!odometry_state_tracker_.empty()) {
|
|
||||||
// We add an odometry state, so that the correction from the scan matching
|
|
||||||
// is not removed by the next odometry data we get.
|
|
||||||
odometry_state_tracker_.AddOdometryState(
|
|
||||||
{time, odometry_state_tracker_.newest().odometer_pose,
|
|
||||||
odometry_state_tracker_.newest().state_pose *
|
|
||||||
odometry_prediction.inverse() * pose_estimate_});
|
|
||||||
}
|
|
||||||
|
|
||||||
// Improve the velocity estimate.
|
|
||||||
if (last_scan_match_time_ > common::Time::min() &&
|
|
||||||
time > last_scan_match_time_) {
|
|
||||||
const double delta_t = common::ToSeconds(time - last_scan_match_time_);
|
|
||||||
// This adds the observed difference in velocity that would have reduced the
|
|
||||||
// error to zero.
|
|
||||||
velocity_estimate_ +=
|
|
||||||
(pose_estimate_.translation() - model_prediction.translation()) /
|
|
||||||
delta_t;
|
|
||||||
}
|
|
||||||
last_scan_match_time_ = time_;
|
|
||||||
|
|
||||||
last_pose_estimate_ = {
|
last_pose_estimate_ = {
|
||||||
time, pose_estimate_,
|
time, pose_estimate,
|
||||||
sensor::TransformPointCloud(filtered_range_data.returns,
|
sensor::TransformPointCloud(filtered_range_data.returns,
|
||||||
pose_estimate_.cast<float>())};
|
pose_estimate.cast<float>())};
|
||||||
|
|
||||||
return InsertIntoSubmap(time, filtered_range_data, pose_estimate_);
|
return InsertIntoSubmap(time, filtered_range_data, pose_estimate);
|
||||||
}
|
}
|
||||||
|
|
||||||
void LocalTrajectoryBuilder::AddOdometerData(
|
void LocalTrajectoryBuilder::AddOdometerData(
|
||||||
const common::Time time, const transform::Rigid3d& odometer_pose) {
|
const common::Time time, const transform::Rigid3d& odometer_pose) {
|
||||||
if (imu_tracker_ == nullptr) {
|
if (extrapolator_ == nullptr) {
|
||||||
// Until we've initialized the IMU tracker we do not want to call Predict().
|
// Until we've initialized the extrapolator we cannot add odometry data.
|
||||||
LOG(INFO) << "ImuTracker not yet initialized.";
|
LOG(INFO) << "Extrapolator not yet initialized.";
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
extrapolator_->AddOdometryData(sensor::OdometryData{time, odometer_pose});
|
||||||
Predict(time);
|
|
||||||
if (!odometry_state_tracker_.empty()) {
|
|
||||||
const auto& previous_odometry_state = odometry_state_tracker_.newest();
|
|
||||||
const transform::Rigid3d delta =
|
|
||||||
previous_odometry_state.odometer_pose.inverse() * odometer_pose;
|
|
||||||
const transform::Rigid3d new_pose =
|
|
||||||
previous_odometry_state.state_pose * delta;
|
|
||||||
odometry_correction_ = pose_estimate_.inverse() * new_pose;
|
|
||||||
}
|
|
||||||
odometry_state_tracker_.AddOdometryState(
|
|
||||||
{time, odometer_pose, pose_estimate_ * odometry_correction_});
|
|
||||||
}
|
}
|
||||||
|
|
||||||
const LocalTrajectoryBuilder::PoseEstimate&
|
const LocalTrajectoryBuilder::PoseEstimate&
|
||||||
|
@ -251,7 +196,7 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
|
||||||
active_submaps_.InsertRangeData(
|
active_submaps_.InsertRangeData(
|
||||||
sensor::TransformRangeData(range_data_in_tracking,
|
sensor::TransformRangeData(range_data_in_tracking,
|
||||||
pose_observation.cast<float>()),
|
pose_observation.cast<float>()),
|
||||||
imu_tracker_->orientation());
|
extrapolator_->gravity_orientation());
|
||||||
return std::unique_ptr<InsertionResult>(
|
return std::unique_ptr<InsertionResult>(
|
||||||
new InsertionResult{time, range_data_in_tracking, pose_observation,
|
new InsertionResult{time, range_data_in_tracking, pose_observation,
|
||||||
std::move(insertion_submaps)});
|
std::move(insertion_submaps)});
|
||||||
|
|
|
@ -21,8 +21,7 @@
|
||||||
|
|
||||||
#include "cartographer/common/time.h"
|
#include "cartographer/common/time.h"
|
||||||
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
||||||
#include "cartographer/mapping/imu_tracker.h"
|
#include "cartographer/mapping/pose_extrapolator.h"
|
||||||
#include "cartographer/mapping/odometry_state_tracker.h"
|
|
||||||
#include "cartographer/mapping_3d/motion_filter.h"
|
#include "cartographer/mapping_3d/motion_filter.h"
|
||||||
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
|
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
|
||||||
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
|
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
|
||||||
|
@ -64,8 +63,6 @@ class LocalTrajectoryBuilder {
|
||||||
const PoseEstimate& pose_estimate() const;
|
const PoseEstimate& pose_estimate() const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void Predict(common::Time time);
|
|
||||||
|
|
||||||
std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
|
std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
|
||||||
common::Time time, const sensor::RangeData& range_data_in_tracking);
|
common::Time time, const sensor::RangeData& range_data_in_tracking);
|
||||||
|
|
||||||
|
@ -83,17 +80,7 @@ class LocalTrajectoryBuilder {
|
||||||
real_time_correlative_scan_matcher_;
|
real_time_correlative_scan_matcher_;
|
||||||
std::unique_ptr<scan_matching::CeresScanMatcher> ceres_scan_matcher_;
|
std::unique_ptr<scan_matching::CeresScanMatcher> ceres_scan_matcher_;
|
||||||
|
|
||||||
// Current 'pose_estimate_' and 'velocity_estimate_' at 'time_'.
|
std::unique_ptr<mapping::PoseExtrapolator> extrapolator_;
|
||||||
common::Time time_ = common::Time::min();
|
|
||||||
transform::Rigid3d pose_estimate_ = transform::Rigid3d::Identity();
|
|
||||||
Eigen::Vector3d velocity_estimate_ = Eigen::Vector3d::Zero();
|
|
||||||
common::Time last_scan_match_time_ = common::Time::min();
|
|
||||||
// This is the difference between the model (constant velocity, IMU)
|
|
||||||
// prediction 'pose_estimate_' and the odometry prediction. To get the
|
|
||||||
// odometry prediction, right-multiply this to 'pose_estimate_'.
|
|
||||||
transform::Rigid3d odometry_correction_ = transform::Rigid3d::Identity();
|
|
||||||
std::unique_ptr<mapping::ImuTracker> imu_tracker_;
|
|
||||||
mapping::OdometryStateTracker odometry_state_tracker_;
|
|
||||||
|
|
||||||
int num_accumulated_ = 0;
|
int num_accumulated_ = 0;
|
||||||
transform::Rigid3f first_pose_estimate_ = transform::Rigid3f::Identity();
|
transform::Rigid3f first_pose_estimate_ = transform::Rigid3f::Identity();
|
||||||
|
|
Loading…
Reference in New Issue