Tiny improvement of the gravity estimation. (#494)
parent
d3c49c8585
commit
5673334f0e
|
@ -130,6 +130,13 @@ transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
|
|||
transform::Rigid3d::Rotation(ExtrapolateRotation(time));
|
||||
}
|
||||
|
||||
Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation(
|
||||
const common::Time time) {
|
||||
ImuTracker imu_tracker = *imu_tracker_;
|
||||
AdvanceImuTracker(time, &imu_tracker);
|
||||
return imu_tracker.orientation();
|
||||
}
|
||||
|
||||
void PoseExtrapolator::UpdateVelocitiesFromPoses() {
|
||||
if (timed_pose_queue_.size() < 2) {
|
||||
// We need two poses to estimate velocities.
|
||||
|
|
|
@ -54,9 +54,7 @@ class PoseExtrapolator {
|
|||
transform::Rigid3d ExtrapolatePose(common::Time time);
|
||||
|
||||
// Gravity alignment estimate.
|
||||
Eigen::Quaterniond gravity_orientation() const {
|
||||
return imu_tracker_->orientation();
|
||||
}
|
||||
Eigen::Quaterniond EstimateGravityOrientation(common::Time time);
|
||||
|
||||
private:
|
||||
void UpdateVelocitiesFromPoses();
|
||||
|
|
|
@ -156,15 +156,18 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
|||
const transform::Rigid3d pose_estimate =
|
||||
matching_submap->local_pose() * pose_observation_in_submap;
|
||||
extrapolator_->AddPose(time, pose_estimate);
|
||||
const Eigen::Quaterniond gravity_alignment =
|
||||
extrapolator_->EstimateGravityOrientation(time);
|
||||
|
||||
last_pose_estimate_ = {
|
||||
time, pose_estimate,
|
||||
sensor::TransformPointCloud(filtered_range_data.returns,
|
||||
pose_estimate.cast<float>())};
|
||||
|
||||
return InsertIntoSubmap(
|
||||
time, filtered_range_data, filtered_point_cloud_in_tracking,
|
||||
low_resolution_point_cloud_in_tracking, pose_estimate);
|
||||
return InsertIntoSubmap(time, filtered_range_data, gravity_alignment,
|
||||
filtered_point_cloud_in_tracking,
|
||||
low_resolution_point_cloud_in_tracking,
|
||||
pose_estimate);
|
||||
}
|
||||
|
||||
void LocalTrajectoryBuilder::AddOdometerData(
|
||||
|
@ -184,6 +187,7 @@ const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {
|
|||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
|
||||
LocalTrajectoryBuilder::InsertIntoSubmap(
|
||||
const common::Time time, const sensor::RangeData& range_data_in_tracking,
|
||||
const Eigen::Quaterniond& gravity_alignment,
|
||||
const sensor::PointCloud& high_resolution_point_cloud,
|
||||
const sensor::PointCloud& low_resolution_point_cloud,
|
||||
const transform::Rigid3d& pose_observation) {
|
||||
|
@ -199,7 +203,7 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
|
|||
active_submaps_.InsertRangeData(
|
||||
sensor::TransformRangeData(range_data_in_tracking,
|
||||
pose_observation.cast<float>()),
|
||||
extrapolator_->gravity_orientation());
|
||||
gravity_alignment);
|
||||
return std::unique_ptr<InsertionResult>(new InsertionResult{
|
||||
|
||||
std::make_shared<const mapping::TrajectoryNode::Data>(
|
||||
|
|
|
@ -65,6 +65,7 @@ class LocalTrajectoryBuilder {
|
|||
|
||||
std::unique_ptr<InsertionResult> InsertIntoSubmap(
|
||||
common::Time time, const sensor::RangeData& range_data_in_tracking,
|
||||
const Eigen::Quaterniond& gravity_alignment,
|
||||
const sensor::PointCloud& high_resolution_point_cloud,
|
||||
const sensor::PointCloud& low_resolution_point_cloud,
|
||||
const transform::Rigid3d& pose_observation);
|
||||
|
|
|
@ -275,8 +275,7 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
|
|||
}
|
||||
const float kSafetyMargin = 1.f - 1e-2f;
|
||||
const float angular_step_size =
|
||||
kSafetyMargin * std::acos(1.f -
|
||||
common::Pow2(resolution_) /
|
||||
kSafetyMargin * std::acos(1.f - common::Pow2(resolution_) /
|
||||
(2.f * common::Pow2(max_scan_range)));
|
||||
const int angular_window_size = common::RoundToInt(
|
||||
search_parameters.angular_search_window / angular_step_size);
|
||||
|
|
|
@ -35,8 +35,7 @@ class Rigid2 {
|
|||
using Vector = Eigen::Matrix<FloatType, 2, 1>;
|
||||
using Rotation2D = Eigen::Rotation2D<FloatType>;
|
||||
|
||||
Rigid2()
|
||||
: translation_(Vector::Zero()), rotation_(Rotation2D::Identity()) {}
|
||||
Rigid2() : translation_(Vector::Zero()), rotation_(Rotation2D::Identity()) {}
|
||||
Rigid2(const Vector& translation, const Rotation2D& rotation)
|
||||
: translation_(translation), rotation_(rotation) {}
|
||||
Rigid2(const Vector& translation, const double rotation)
|
||||
|
@ -54,9 +53,7 @@ class Rigid2 {
|
|||
return Rigid2(vector, Rotation2D::Identity());
|
||||
}
|
||||
|
||||
static Rigid2<FloatType> Identity() {
|
||||
return Rigid2<FloatType>();
|
||||
}
|
||||
static Rigid2<FloatType> Identity() { return Rigid2<FloatType>(); }
|
||||
|
||||
template <typename OtherType>
|
||||
Rigid2<OtherType> cast() const {
|
||||
|
@ -128,8 +125,7 @@ class Rigid3 {
|
|||
using Quaternion = Eigen::Quaternion<FloatType>;
|
||||
using AngleAxis = Eigen::AngleAxis<FloatType>;
|
||||
|
||||
Rigid3()
|
||||
: translation_(Vector::Zero()), rotation_(Quaternion::Identity()) {}
|
||||
Rigid3() : translation_(Vector::Zero()), rotation_(Quaternion::Identity()) {}
|
||||
Rigid3(const Vector& translation, const Quaternion& rotation)
|
||||
: translation_(translation), rotation_(rotation) {}
|
||||
Rigid3(const Vector& translation, const AngleAxis& rotation)
|
||||
|
@ -147,9 +143,7 @@ class Rigid3 {
|
|||
return Rigid3(vector, Quaternion::Identity());
|
||||
}
|
||||
|
||||
static Rigid3<FloatType> Identity() {
|
||||
return Rigid3<FloatType>();
|
||||
}
|
||||
static Rigid3<FloatType> Identity() { return Rigid3<FloatType>(); }
|
||||
|
||||
template <typename OtherType>
|
||||
Rigid3<OtherType> cast() const {
|
||||
|
|
Loading…
Reference in New Issue