12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
cartographer/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc

409 lines
18 KiB
C++
Raw Blame History

/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/mapping/internal/3d/local_trajectory_builder_3d.h"
#include <memory>
#include "cartographer/common/make_unique.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h"
#include "cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.pb.h"
#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 "glog/logging.h"
namespace cartographer {
namespace mapping {
static auto* kLocalSlamLatencyMetric = metrics::Gauge::Null();
static auto* kLocalSlamVoxelFilterFraction = metrics::Gauge::Null();
static auto* kLocalSlamScanMatcherFraction = metrics::Gauge::Null();
static auto* kLocalSlamInsertIntoSubmapFraction = metrics::Gauge::Null();
static auto* kRealTimeCorrelativeScanMatcherScoreMetric =
metrics::Histogram::Null();
static auto* kCeresScanMatcherCostMetric = metrics::Histogram::Null();
static auto* kScanMatcherResidualDistanceMetric = metrics::Histogram::Null();
static auto* kScanMatcherResidualAngleMetric = metrics::Histogram::Null();
LocalTrajectoryBuilder3D::LocalTrajectoryBuilder3D(
const mapping::proto::LocalTrajectoryBuilderOptions3D& options,
const std::vector<std::string>& expected_range_sensor_ids)
: options_(options),
active_submaps_(options.submaps_options()),
motion_filter_(options.motion_filter_options()),
real_time_correlative_scan_matcher_(
common::make_unique<scan_matching::RealTimeCorrelativeScanMatcher3D>(
options_.real_time_correlative_scan_matcher_options())),
ceres_scan_matcher_(
common::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() {}
std::unique_ptr<transform::Rigid3d> LocalTrajectoryBuilder3D::ScanMatch(
const transform::Rigid3d& pose_prediction,
const sensor::PointCloud& low_resolution_point_cloud_in_tracking,
const sensor::PointCloud& high_resolution_point_cloud_in_tracking) {
std::shared_ptr<const mapping::Submap3D> matching_submap =
active_submaps_.submaps().front();
transform::Rigid3d initial_ceres_pose =
matching_submap->local_pose().inverse() * pose_prediction;
if (options_.use_online_correlative_scan_matching()) {
// We take a copy since we use 'initial_ceres_pose' as an output argument.
const transform::Rigid3d initial_pose = initial_ceres_pose;
const double score = real_time_correlative_scan_matcher_->Match(
initial_pose, high_resolution_point_cloud_in_tracking,
matching_submap->high_resolution_hybrid_grid(), &initial_ceres_pose);
kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
}
transform::Rigid3d pose_observation_in_submap;
ceres::Solver::Summary summary;
ceres_scan_matcher_->Match(
(matching_submap->local_pose().inverse() * pose_prediction).translation(),
initial_ceres_pose,
{{&high_resolution_point_cloud_in_tracking,
&matching_submap->high_resolution_hybrid_grid()},
{&low_resolution_point_cloud_in_tracking,
&matching_submap->low_resolution_hybrid_grid()}},
&pose_observation_in_submap, &summary);
kCeresScanMatcherCostMetric->Observe(summary.final_cost);
const double residual_distance = (pose_observation_in_submap.translation() -
initial_ceres_pose.translation())
.norm();
kScanMatcherResidualDistanceMetric->Observe(residual_distance);
const double residual_angle =
pose_observation_in_submap.rotation().angularDistance(
initial_ceres_pose.rotation());
kScanMatcherResidualAngleMetric->Observe(residual_angle);
return common::make_unique<transform::Rigid3d>(matching_submap->local_pose() *
pose_observation_in_submap);
}
void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) {
if (extrapolator_ != nullptr) {
extrapolator_->AddImuData(imu_data);
return;
}
// 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<LocalTrajectoryBuilder3D::MatchingResult>
LocalTrajectoryBuilder3D::AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& unsynchronized_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& 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.
LOG(INFO) << "IMU not yet initialized.";
return nullptr;
}
CHECK(!synchronized_data.ranges.empty());
CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f);
const common::Time time_first_point =
time +
common::FromSeconds(synchronized_data.ranges.front().point_time[3]);
if (time_first_point < extrapolator_->GetLastPoseTime()) {
LOG(INFO) << "Extrapolator is still initializing.";
return nullptr;
}
if (num_accumulated_ == 0) {
accumulation_started_ = std::chrono::steady_clock::now();
}
std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> hits =
sensor::VoxelFilter(0.5f * options_.voxel_filter_size())
.Filter(synchronized_data.ranges);
std::vector<transform::Rigid3f> hits_poses;
hits_poses.reserve(hits.size());
bool warned = false;
for (const auto& hit : hits) {
common::Time time_point = time + common::FromSeconds(hit.point_time[3]);
if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
if (!warned) {
LOG(ERROR)
<< "Timestamp of individual range data point jumps backwards from "
<< extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;
warned = true;
}
time_point = extrapolator_->GetLastExtrapolatedTime();
}
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].point_time.head<3>();
const Eigen::Vector3f origin_in_local =
hits_poses[i] * synchronized_data.origins.at(hits[i].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);
} 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_local + options_.max_range() / range * delta);
}
}
}
++num_accumulated_;
if (num_accumulated_ >= options_.num_accumulated_range_data()) {
const common::Time current_sensor_time = synchronized_data.time;
common::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(time).cast<float>();
const auto voxel_filter_start = std::chrono::steady_clock::now();
const sensor::RangeData filtered_range_data = {
current_pose.translation(),
sensor::VoxelFilter(options_.voxel_filter_size())
.Filter(accumulated_range_data_.returns),
sensor::VoxelFilter(options_.voxel_filter_size())
.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;
if (sensor_duration.has_value()) {
const double voxel_filter_fraction =
common::ToSeconds(voxel_filter_duration) /
common::ToSeconds(sensor_duration.value());
kLocalSlamVoxelFilterFraction->Set(voxel_filter_fraction);
}
return AddAccumulatedRangeData(
time,
sensor::TransformRangeData(filtered_range_data, current_pose.inverse()),
sensor_duration);
}
return nullptr;
}
std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
const common::Time time,
const sensor::RangeData& filtered_range_data_in_tracking,
const common::optional<common::Duration>& sensor_duration) {
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();
sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.high_resolution_adaptive_voxel_filter_options());
const sensor::PointCloud high_resolution_point_cloud_in_tracking =
adaptive_voxel_filter.Filter(filtered_range_data_in_tracking.returns);
if (high_resolution_point_cloud_in_tracking.empty()) {
LOG(WARNING) << "Dropped empty high resolution point cloud data.";
return nullptr;
}
sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
options_.low_resolution_adaptive_voxel_filter_options());
const sensor::PointCloud low_resolution_point_cloud_in_tracking =
low_resolution_adaptive_voxel_filter.Filter(
filtered_range_data_in_tracking.returns);
if (low_resolution_point_cloud_in_tracking.empty()) {
LOG(WARNING) << "Dropped empty low resolution point cloud data.";
return nullptr;
}
std::unique_ptr<transform::Rigid3d> pose_estimate =
ScanMatch(pose_prediction, low_resolution_point_cloud_in_tracking,
high_resolution_point_cloud_in_tracking);
if (pose_estimate == nullptr) {
LOG(WARNING) << "Scan matching failed.";
return nullptr;
}
extrapolator_->AddPose(time, *pose_estimate);
const auto scan_matcher_stop = std::chrono::steady_clock::now();
const auto scan_matcher_duration = scan_matcher_stop - scan_matcher_start;
if (sensor_duration.has_value()) {
const double scan_matcher_fraction =
common::ToSeconds(scan_matcher_duration) /
common::ToSeconds(sensor_duration.value());
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>());
const auto insert_into_submap_start = std::chrono::steady_clock::now();
std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
time, filtered_range_data_in_local, filtered_range_data_in_tracking,
high_resolution_point_cloud_in_tracking,
low_resolution_point_cloud_in_tracking, *pose_estimate,
gravity_alignment);
const auto insert_into_submap_stop = std::chrono::steady_clock::now();
const auto insert_into_submap_duration =
insert_into_submap_stop - insert_into_submap_start;
if (sensor_duration.has_value()) {
const double insert_into_submap_fraction =
common::ToSeconds(insert_into_submap_duration) /
common::ToSeconds(sensor_duration.value());
kLocalSlamInsertIntoSubmapFraction->Set(insert_into_submap_fraction);
}
const auto accumulation_duration =
std::chrono::steady_clock::now() - accumulation_started_;
kLocalSlamLatencyMetric->Set(common::ToSeconds(accumulation_duration));
return common::make_unique<MatchingResult>(MatchingResult{
time, *pose_estimate, std::move(filtered_range_data_in_local),
std::move(insertion_result)});
}
void LocalTrajectoryBuilder3D::AddOdometryData(
const sensor::OdometryData& odometry_data) {
if (extrapolator_ == nullptr) {
// Until we've initialized the extrapolator we cannot add odometry data.
LOG(INFO) << "Extrapolator not yet initialized.";
return;
}
extrapolator_->AddOdometryData(odometry_data);
}
std::unique_ptr<LocalTrajectoryBuilder3D::InsertionResult>
LocalTrajectoryBuilder3D::InsertIntoSubmap(
const common::Time time,
const sensor::RangeData& filtered_range_data_in_local,
const sensor::RangeData& filtered_range_data_in_tracking,
const sensor::PointCloud& high_resolution_point_cloud_in_tracking,
const sensor::PointCloud& low_resolution_point_cloud_in_tracking,
const transform::Rigid3d& pose_estimate,
const Eigen::Quaterniond& gravity_alignment) {
if (motion_filter_.IsSimilar(time, pose_estimate)) {
return nullptr;
}
// Querying the active submaps must be done here before calling
// InsertRangeData() since the queried values are valid for next insertion.
std::vector<std::shared_ptr<const mapping::Submap3D>> insertion_submaps;
for (const std::shared_ptr<mapping::Submap3D>& submap :
active_submaps_.submaps()) {
insertion_submaps.push_back(submap);
}
active_submaps_.InsertRangeData(filtered_range_data_in_local,
gravity_alignment);
const Eigen::VectorXf rotational_scan_matcher_histogram =
scan_matching::RotationalScanMatcher::ComputeHistogram(
sensor::TransformPointCloud(
filtered_range_data_in_tracking.returns,
transform::Rigid3f::Rotation(gravity_alignment.cast<float>())),
options_.rotational_histogram_size());
return common::make_unique<InsertionResult>(
InsertionResult{std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{
time,
gravity_alignment,
{}, // 'filtered_point_cloud' is only used in 2D.
high_resolution_point_cloud_in_tracking,
low_resolution_point_cloud_in_tracking,
rotational_scan_matcher_histogram,
pose_estimate}),
std::move(insertion_submaps)});
}
void LocalTrajectoryBuilder3D::RegisterMetrics(
metrics::FamilyFactory* family_factory) {
auto* latency = family_factory->NewGaugeFamily(
"mapping_3d_local_trajectory_builder_latency",
"Duration from first incoming point cloud in accumulation to local slam "
"result");
auto* voxel_filter_fraction = family_factory->NewGaugeFamily(
"mapping_3d_local_trajectory_builder_voxel_filter_fraction",
"Fraction of total sensor time taken up by voxel filter.");
kLocalSlamVoxelFilterFraction = voxel_filter_fraction->Add({});
auto* scan_matcher_fraction = family_factory->NewGaugeFamily(
"mapping_3d_local_trajectory_builder_scan_matcher_fraction",
"Fraction of total sensor time taken up by scan matcher.");
kLocalSlamScanMatcherFraction = scan_matcher_fraction->Add({});
auto* insert_into_submap_fraction = family_factory->NewGaugeFamily(
"mapping_3d_local_trajectory_builder"
"insert_into_submap_fraction",
"Fraction of total sensor time taken up by inserting into submap.");
kLocalSlamInsertIntoSubmapFraction = insert_into_submap_fraction->Add({});
kLocalSlamLatencyMetric = latency->Add({});
auto score_boundaries = metrics::Histogram::FixedWidth(0.05, 20);
auto* scores = family_factory->NewHistogramFamily(
"mapping_3d_local_trajectory_builder_scores", "Local scan matcher scores",
score_boundaries);
kRealTimeCorrelativeScanMatcherScoreMetric =
scores->Add({{"scan_matcher", "real_time_correlative"}});
auto cost_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 100);
auto* costs = family_factory->NewHistogramFamily(
"mapping_3d_local_trajectory_builder_costs", "Local scan matcher costs",
cost_boundaries);
kCeresScanMatcherCostMetric = costs->Add({{"scan_matcher", "ceres"}});
auto distance_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 10);
auto* residuals = family_factory->NewHistogramFamily(
"mapping_3d_local_trajectory_builder_residuals",
"Local scan matcher residuals", distance_boundaries);
kScanMatcherResidualDistanceMetric =
residuals->Add({{"component", "distance"}});
kScanMatcherResidualAngleMetric = residuals->Add({{"component", "angle"}});
}
} // namespace mapping
} // namespace cartographer
Reference in New Issue View Git Blame Copy Permalink