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

Tiny improvement of the gravity estimation. (#494)

master
Wolfgang Hess 2017-09-01 10:22:13 +02:00 committed by GitHub
parent d3c49c8585
commit 5673334f0e
7 changed files with 24 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cartographer/mapping/pose_extrapolator.cc
View File

@ -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.

4
cartographer/mapping/pose_extrapolator.h
View File

@ -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();

12
cartographer/mapping_3d/local_trajectory_builder.cc
View File

@ -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>(

1
cartographer/mapping_3d/local_trajectory_builder.h
View File

@ -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);

5
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -275,9 +275,8 @@ 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_) /
(2.f * common::Pow2(max_scan_range)));
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);
// TODO(whess): Should there be a small search window for rotations around

2
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -266,7 +266,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) {
CHECK_EQ(submap_queued_work_items_.size(), 0);
if (when_done_ != nullptr) {
for (const std::unique_ptr<OptimizationProblem::Constraint>&
constraint : constraints_) {
constraint : constraints_) {
if (constraint != nullptr) {
result.push_back(*constraint);
}

14
cartographer/transform/rigid_transform.h
View File

@ -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 {