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

Extrapolator Result for motion compensation (#1731) 

This makes use of the ExtrapolationResult returned by the pose extrapolator. ExtrapolationResult contains poses corresponding to the timestamps of range data measurements to be used for motion compensation. When using the PoseExtrapolator implementation there is no functional difference. This PR prepares the integration of ImuBasedPoseExtrapolator, which requires using ExtrapolationResult returned by ExtrapolatePosesWithGravity to run efficiently.

Signed-off-by: mschworer <mschworer@lyft.com>
master
Martin Schwörer 2020-07-30 09:14:02 +02:00 committed by GitHub
parent efc64934d6
commit d3794a420a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 115 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

126
cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc
View File

@ -25,6 +25,7 @@
#include "cartographer/mapping/proto/3d/submaps_options_3d.pb.h"
#include "cartographer/mapping/proto/scan_matching//ceres_scan_matcher_options_3d.pb.h"
#include "cartographer/mapping/proto/scan_matching//real_time_correlative_scan_matcher_options.pb.h"
#include "cartographer/transform/timestamped_transform.h"
#include "glog/logging.h"
namespace cartographer {
@ -55,7 +56,6 @@ LocalTrajectoryBuilder3D::LocalTrajectoryBuilder3D(
options_.real_time_correlative_scan_matcher_options())),
ceres_scan_matcher_(absl::make_unique<scan_matching::CeresScanMatcher3D>(
options_.ceres_scan_matcher_options())),
accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}},
range_data_collator_(expected_range_sensor_ids) {}
LocalTrajectoryBuilder3D::~LocalTrajectoryBuilder3D() {}
@ -116,14 +116,13 @@ std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
LocalTrajectoryBuilder3D::AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& unsynchronized_data) {
const auto synchronized_data =
auto synchronized_data =
range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
if (synchronized_data.ranges.empty()) {
LOG(INFO) << "Range data collator filling buffer.";
return nullptr;
}
const common::Time& current_sensor_time = synchronized_data.time;
if (extrapolator_ == nullptr) {
// Until we've initialized the extrapolator with our first IMU message, we
// cannot compute the orientation of the rangefinder.
@ -134,82 +133,104 @@ LocalTrajectoryBuilder3D::AddRangeData(
CHECK(!synchronized_data.ranges.empty());
CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
const common::Time time_first_point =
current_sensor_time +
synchronized_data.time +
common::FromSeconds(synchronized_data.ranges.front().point_time.time);
if (time_first_point < extrapolator_->GetLastPoseTime()) {
LOG(INFO) << "Extrapolator is still initializing.";
return nullptr;
}
std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> hits =
if (num_accumulated_ == 0) {
accumulated_point_cloud_origin_data_.clear();
}
synchronized_data.ranges =
sensor::VoxelFilter(0.5f * options_.voxel_filter_size())
.Filter(synchronized_data.ranges);
accumulated_point_cloud_origin_data_.emplace_back(
std::move(synchronized_data));
++num_accumulated_;
if (num_accumulated_ < options_.num_accumulated_range_data()) {
return nullptr;
}
num_accumulated_ = 0;
std::vector<transform::Rigid3f> hits_poses;
hits_poses.reserve(hits.size());
bool warned = false;
for (const auto& hit : hits) {
common::Time time_point =
current_sensor_time + common::FromSeconds(hit.point_time.time);
if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
std::vector<common::Time> hit_times;
common::Time prev_time_point = extrapolator_->GetLastExtrapolatedTime();
for (const auto& point_cloud_origin_data :
accumulated_point_cloud_origin_data_) {
for (const auto& hit : point_cloud_origin_data.ranges) {
common::Time time_point = point_cloud_origin_data.time +
common::FromSeconds(hit.point_time.time);
if (time_point < prev_time_point) {
if (!warned) {
LOG(ERROR)
<< "Timestamp of individual range data point jumps backwards from "
<< extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;
LOG(ERROR) << "Timestamp of individual range data point jumps "
"backwards from "
<< prev_time_point << " to " << time_point;
warned = true;
}
time_point = extrapolator_->GetLastExtrapolatedTime();
}
hits_poses.push_back(
extrapolator_->ExtrapolatePose(time_point).cast<float>());
time_point = prev_time_point;
}
if (num_accumulated_ == 0) {
// 'accumulated_range_data_.origin' is not used.
accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
hit_times.push_back(time_point);
prev_time_point = time_point;
}
}
hit_times.push_back(accumulated_point_cloud_origin_data_.back().time);
for (size_t i = 0; i < hits.size(); ++i) {
sensor::RangefinderPoint hit_in_local =
hits_poses[i] * sensor::ToRangefinderPoint(hits[i].point_time);
const PoseExtrapolatorInterface::ExtrapolationResult extrapolation_result =
extrapolator_->ExtrapolatePosesWithGravity(hit_times);
std::vector<transform::Rigid3f> hits_poses(
std::move(extrapolation_result.previous_poses));
hits_poses.push_back(extrapolation_result.current_pose.cast<float>());
CHECK_EQ(hits_poses.size(), hit_times.size());
sensor::RangeData accumulated_range_data;
std::vector<transform::Rigid3f>::const_iterator hits_poses_it =
hits_poses.begin();
for (const auto& point_cloud_origin_data :
accumulated_point_cloud_origin_data_) {
for (const auto& hit : point_cloud_origin_data.ranges) {
const Eigen::Vector3f hit_in_local =
*hits_poses_it * hit.point_time.position;
const Eigen::Vector3f origin_in_local =
hits_poses[i] * synchronized_data.origins.at(hits[i].origin_index);
const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
*hits_poses_it * point_cloud_origin_data.origins.at(hit.origin_index);
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_local);
accumulated_range_data.returns.push_back(
sensor::RangefinderPoint{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.
hit_in_local.position =
origin_in_local + options_.max_range() / range * delta;
accumulated_range_data_.misses.push_back(hit_in_local);
// 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(sensor::RangefinderPoint{
origin_in_local + options_.max_range() / range * delta});
}
}
++hits_poses_it;
}
++num_accumulated_;
}
CHECK(std::next(hits_poses_it) == hits_poses.end());
if (num_accumulated_ >= options_.num_accumulated_range_data()) {
const common::Time current_sensor_time = synchronized_data.time;
absl::optional<common::Duration> sensor_duration;
if (last_sensor_time_.has_value()) {
sensor_duration = current_sensor_time - last_sensor_time_.value();
}
last_sensor_time_ = current_sensor_time;
num_accumulated_ = 0;
transform::Rigid3f current_pose =
extrapolator_->ExtrapolatePose(current_sensor_time).cast<float>();
const common::Time current_time = hit_times.back();
const auto voxel_filter_start = std::chrono::steady_clock::now();
const sensor::RangeData filtered_range_data = {
current_pose.translation(),
extrapolation_result.current_pose.translation().cast<float>(),
sensor::VoxelFilter(options_.voxel_filter_size())
.Filter(accumulated_range_data_.returns),
.Filter(accumulated_range_data.returns),
sensor::VoxelFilter(options_.voxel_filter_size())
.Filter(accumulated_range_data_.misses)};
.Filter(accumulated_range_data.misses)};
const auto voxel_filter_stop = std::chrono::steady_clock::now();
const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start;
@ -221,24 +242,25 @@ LocalTrajectoryBuilder3D::AddRangeData(
}
return AddAccumulatedRangeData(
current_sensor_time,
sensor::TransformRangeData(filtered_range_data, current_pose.inverse()),
sensor_duration);
}
return nullptr;
current_time,
sensor::TransformRangeData(
filtered_range_data,
extrapolation_result.current_pose.inverse().cast<float>()),
sensor_duration, extrapolation_result.current_pose,
extrapolation_result.gravity_from_tracking);
}
std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
const common::Time time,
const sensor::RangeData& filtered_range_data_in_tracking,
const absl::optional<common::Duration>& sensor_duration) {
const absl::optional<common::Duration>& sensor_duration,
const transform::Rigid3d& pose_prediction,
const Eigen::Quaterniond& gravity_alignment) {
if (filtered_range_data_in_tracking.returns.empty()) {
LOG(WARNING) << "Dropped empty range data.";
return nullptr;
}
const transform::Rigid3d pose_prediction =
extrapolator_->ExtrapolatePose(time);
const auto scan_matcher_start = std::chrono::steady_clock::now();
@ -278,8 +300,6 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
kLocalSlamScanMatcherFraction->Set(scan_matcher_fraction);
}
const Eigen::Quaterniond gravity_alignment =
extrapolator_->EstimateGravityOrientation(time);
sensor::RangeData filtered_range_data_in_local = sensor::TransformRangeData(
filtered_range_data_in_tracking, pose_estimate->cast<float>());

8
cartographer/mapping/internal/3d/local_trajectory_builder_3d.h
View File

@ -33,6 +33,7 @@
#include "cartographer/sensor/internal/voxel_filter.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/sensor/timed_point_cloud_data.h"
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {
@ -78,7 +79,9 @@ class LocalTrajectoryBuilder3D {
std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
common::Time time,
const sensor::RangeData& filtered_range_data_in_tracking,
const absl::optional<common::Duration>& sensor_duration);
const absl::optional<common::Duration>& sensor_duration,
const transform::Rigid3d& pose_prediction,
const Eigen::Quaterniond& gravity_alignment);
std::unique_ptr<InsertionResult> InsertIntoSubmap(
common::Time time, const sensor::RangeData& filtered_range_data_in_local,
@ -106,7 +109,8 @@ class LocalTrajectoryBuilder3D {
std::unique_ptr<mapping::PoseExtrapolatorInterface> extrapolator_;
int num_accumulated_ = 0;
sensor::RangeData accumulated_range_data_;
std::vector<sensor::TimedPointCloudOriginData>
accumulated_point_cloud_origin_data_;
absl::optional<std::chrono::steady_clock::time_point> last_wall_time_;
absl::optional<double> last_thread_cpu_time_seconds_;