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

LocalTrajectoryBuilder uses RangeDataCollator (#996) 

Local SLAM accepts and unwarps time-overlapping range data from multiple sensors.

[RFC=0017](https://github.com/googlecartographer/rfcs/blob/master/text/0017-synchronize-points.md)
FIXES=#910
master
gaschler 2018-03-15 16:34:51 +01:00 committed by Wally B. Feed
parent 55065a2108
commit 37a68cd7f4
7 changed files with 98 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
View File

@ -34,13 +34,15 @@ static auto* kScanMatcherResidualDistanceMetric = metrics::Histogram::Null();
static auto* kScanMatcherResidualAngleMetric = metrics::Histogram::Null();
LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D(
const proto::LocalTrajectoryBuilderOptions2D& options)
const proto::LocalTrajectoryBuilderOptions2D& 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_(
options_.real_time_correlative_scan_matcher_options()),
ceres_scan_matcher_(options_.ceres_scan_matcher_options()) {}
ceres_scan_matcher_(options_.ceres_scan_matcher_options()),
range_data_collator_(expected_range_sensor_ids) {}
LocalTrajectoryBuilder2D::~LocalTrajectoryBuilder2D() {}
@ -101,7 +103,16 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
const common::Time time, const sensor::TimedRangeData& range_data) {
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;
// Initialize extrapolator now if we do not ever use an IMU.
if (!options_.use_imu_data()) {
InitializeExtrapolator(time);
@ -114,10 +125,12 @@ LocalTrajectoryBuilder2D::AddRangeData(
return nullptr;
}
CHECK(!range_data.returns.empty());
CHECK_EQ(range_data.returns.back()[3], 0);
CHECK(!synchronized_data.ranges.empty());
// TODO(gaschler): Check if this can strictly be 0.
CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f);
const common::Time time_first_point =
time + common::FromSeconds(range_data.returns.front()[3]);
time +
common::FromSeconds(synchronized_data.ranges.front().point_time[3]);
if (time_first_point < extrapolator_->GetLastPoseTime()) {
LOG(INFO) << "Extrapolator is still initializing.";
return nullptr;
@ -128,10 +141,10 @@ LocalTrajectoryBuilder2D::AddRangeData(
}
std::vector<transform::Rigid3f> range_data_poses;
range_data_poses.reserve(range_data.returns.size());
range_data_poses.reserve(synchronized_data.ranges.size());
bool warned = false;
for (const Eigen::Vector4f& hit : range_data.returns) {
common::Time time_point = time + common::FromSeconds(hit[3]);
for (const auto& range : synchronized_data.ranges) {
common::Time time_point = time + common::FromSeconds(range.point_time[3]);
if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
if (!warned) {
LOG(ERROR)
@ -153,10 +166,11 @@ LocalTrajectoryBuilder2D::AddRangeData(
// Drop any returns below the minimum range and convert returns beyond the
// maximum range into misses.
for (size_t i = 0; i < range_data.returns.size(); ++i) {
const Eigen::Vector4f& hit = range_data.returns[i];
for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
const Eigen::Vector4f& hit = synchronized_data.ranges[i].point_time;
const Eigen::Vector3f origin_in_local =
range_data_poses[i] * range_data.origin;
range_data_poses[i] *
synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
const Eigen::Vector3f hit_in_local = range_data_poses[i] * hit.head<3>();
const Eigen::Vector3f delta = hit_in_local - origin_in_local;
const float range = delta.norm();
@ -176,8 +190,9 @@ LocalTrajectoryBuilder2D::AddRangeData(
num_accumulated_ = 0;
const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
extrapolator_->EstimateGravityOrientation(time));
accumulated_range_data_.origin =
range_data_poses.back() * range_data.origin;
// TODO(gaschler): This assumes that 'range_data_poses.back()' is at time
// 'time'.
accumulated_range_data_.origin = range_data_poses.back().translation();
return AddAccumulatedRangeData(
time,
TransformToGravityAlignedFrameAndFilter(

15
cartographer/mapping/internal/2d/local_trajectory_builder_2d.h
View File

@ -26,6 +26,7 @@
#include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h"
#include "cartographer/mapping/internal/motion_filter.h"
#include "cartographer/mapping/internal/range_data_collator.h"
#include "cartographer/mapping/pose_extrapolator.h"
#include "cartographer/metrics/family_factory.h"
#include "cartographer/sensor/imu_data.h"
@ -55,7 +56,8 @@ class LocalTrajectoryBuilder2D {
};
explicit LocalTrajectoryBuilder2D(
const proto::LocalTrajectoryBuilderOptions2D& options);
const proto::LocalTrajectoryBuilderOptions2D& options,
const std::vector<std::string>& expected_range_sensor_ids);
~LocalTrajectoryBuilder2D();
LocalTrajectoryBuilder2D(const LocalTrajectoryBuilder2D&) = delete;
@ -63,11 +65,12 @@ class LocalTrajectoryBuilder2D {
// Returns 'MatchingResult' when range data accumulation completed,
// otherwise 'nullptr'. Range data must be approximately horizontal
// for 2D SLAM. `time` is when the last point in `range_data` was
// acquired, `range_data` contains the relative time of point with
// respect to `time`.
// for 2D SLAM. `TimedPointCloudData::time` is when the last point in
// `range_data` was acquired, `TimedPointCloudData::ranges` contains the
// relative time of point with respect to `TimedPointCloudData::time`.
std::unique_ptr<MatchingResult> AddRangeData(
common::Time, const sensor::TimedRangeData& range_data);
const std::string& sensor_id,
const sensor::TimedPointCloudData& range_data);
void AddImuData(const sensor::ImuData& imu_data);
void AddOdometryData(const sensor::OdometryData& odometry_data);
@ -109,6 +112,8 @@ class LocalTrajectoryBuilder2D {
int num_accumulated_ = 0;
sensor::RangeData accumulated_range_data_;
std::chrono::steady_clock::time_point accumulation_started_;
RangeDataCollator range_data_collator_;
};
} // namespace mapping

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

@ -38,7 +38,8 @@ static auto* kScanMatcherResidualDistanceMetric = metrics::Histogram::Null();
static auto* kScanMatcherResidualAngleMetric = metrics::Histogram::Null();
LocalTrajectoryBuilder3D::LocalTrajectoryBuilder3D(
const mapping::proto::LocalTrajectoryBuilderOptions3D& options)
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()),
@ -48,7 +49,8 @@ LocalTrajectoryBuilder3D::LocalTrajectoryBuilder3D(
ceres_scan_matcher_(
common::make_unique<scan_matching::CeresScanMatcher3D>(
options_.ceres_scan_matcher_options())),
accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {}
accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}},
range_data_collator_(expected_range_sensor_ids) {}
LocalTrajectoryBuilder3D::~LocalTrajectoryBuilder3D() {}
@ -68,7 +70,16 @@ void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) {
std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
LocalTrajectoryBuilder3D::AddRangeData(
const common::Time time, const sensor::TimedRangeData& range_data) {
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.
@ -76,10 +87,11 @@ LocalTrajectoryBuilder3D::AddRangeData(
return nullptr;
}
CHECK(!range_data.returns.empty());
CHECK_EQ(range_data.returns.back()[3], 0);
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(range_data.returns.front()[3]);
time +
common::FromSeconds(synchronized_data.ranges.front().point_time[3]);
if (time_first_point < extrapolator_->GetLastPoseTime()) {
LOG(INFO) << "Extrapolator is still initializing.";
return nullptr;
@ -89,15 +101,15 @@ LocalTrajectoryBuilder3D::AddRangeData(
accumulation_started_ = std::chrono::steady_clock::now();
}
sensor::TimedPointCloud hits =
std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> hits =
sensor::VoxelFilter(0.5f * options_.voxel_filter_size())
.Filter(range_data.returns);
.Filter(synchronized_data.ranges);
std::vector<transform::Rigid3f> hits_poses;
hits_poses.reserve(hits.size());
bool warned = false;
for (const Eigen::Vector4f& hit : hits) {
common::Time time_point = time + common::FromSeconds(hit[3]);
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)
@ -117,8 +129,10 @@ LocalTrajectoryBuilder3D::AddRangeData(
}
for (size_t i = 0; i < hits.size(); ++i) {
const Eigen::Vector3f hit_in_local = hits_poses[i] * hits[i].head<3>();
const Eigen::Vector3f origin_in_local = hits_poses[i] * range_data.origin;
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()) {
@ -140,7 +154,7 @@ LocalTrajectoryBuilder3D::AddRangeData(
transform::Rigid3f current_pose =
extrapolator_->ExtrapolatePose(time).cast<float>();
const sensor::RangeData filtered_range_data = {
current_pose * range_data.origin,
current_pose.translation(),
sensor::VoxelFilter(options_.voxel_filter_size())
.Filter(accumulated_range_data_.returns),
sensor::VoxelFilter(options_.voxel_filter_size())

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

@ -26,6 +26,7 @@
#include "cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h"
#include "cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.h"
#include "cartographer/mapping/internal/motion_filter.h"
#include "cartographer/mapping/internal/range_data_collator.h"
#include "cartographer/mapping/pose_extrapolator.h"
#include "cartographer/metrics/family_factory.h"
#include "cartographer/sensor/imu_data.h"
@ -54,7 +55,8 @@ class LocalTrajectoryBuilder3D {
};
explicit LocalTrajectoryBuilder3D(
const mapping::proto::LocalTrajectoryBuilderOptions3D& options);
const mapping::proto::LocalTrajectoryBuilderOptions3D& options,
const std::vector<std::string>& expected_range_sensor_ids);
~LocalTrajectoryBuilder3D();
LocalTrajectoryBuilder3D(const LocalTrajectoryBuilder3D&) = delete;
@ -62,11 +64,12 @@ class LocalTrajectoryBuilder3D {
void AddImuData(const sensor::ImuData& imu_data);
// Returns 'MatchingResult' when range data accumulation completed,
// otherwise 'nullptr'. `time` is when the last point in `range_data`
// was acquired, `range_data` contains the relative time of point with
// respect to `time`.
// otherwise 'nullptr'. `TimedPointCloudData::time` is when the last point in
// `range_data` was acquired, `TimedPointCloudData::ranges` contains the
// relative time of point with respect to `TimedPointCloudData::time`.
std::unique_ptr<MatchingResult> AddRangeData(
common::Time time, const sensor::TimedRangeData& range_data);
const std::string& sensor_id,
const sensor::TimedPointCloudData& range_data);
void AddOdometryData(const sensor::OdometryData& odometry_data);
static void RegisterMetrics(metrics::FamilyFactory* family_factory);
@ -97,6 +100,8 @@ class LocalTrajectoryBuilder3D {
int num_accumulated_ = 0;
sensor::RangeData accumulated_range_data_;
std::chrono::steady_clock::time_point accumulation_started_;
RangeDataCollator range_data_collator_;
};
} // namespace mapping

10
cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc
View File

@ -35,6 +35,8 @@ namespace cartographer {
namespace mapping {
namespace {
constexpr char kSensorId[] = "sensor_id";
class LocalTrajectoryBuilderTest : public ::testing::Test {
protected:
struct TrajectoryNode {
@ -255,8 +257,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
const auto range_data = GenerateRangeData(node.pose);
const std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
matching_result = local_trajectory_builder_->AddRangeData(
node.time, sensor::TimedRangeData{
range_data.origin, range_data.returns, {}});
kSensorId, sensor::TimedPointCloudData{
node.time, range_data.origin, range_data.returns});
if (matching_result != nullptr) {
EXPECT_THAT(matching_result->local_pose,
transform::IsNearly(node.pose, 1e-1));
@ -271,8 +273,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
};
TEST_F(LocalTrajectoryBuilderTest, MoveInsideCubeUsingOnlyCeresScanMatcher) {
local_trajectory_builder_.reset(
new LocalTrajectoryBuilder3D(CreateTrajectoryBuilderOptions3D()));
local_trajectory_builder_.reset(new LocalTrajectoryBuilder3D(
CreateTrajectoryBuilderOptions3D(), {kSensorId}));
VerifyAccuracy(GenerateCorkscrewTrajectory(), 1e-1);
}

5
cartographer/mapping/internal/global_trajectory_builder.cc
View File

@ -56,10 +56,7 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
<< "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
matching_result = local_trajectory_builder_->AddRangeData(
timed_point_cloud_data.time,
sensor::TimedRangeData{timed_point_cloud_data.origin,
timed_point_cloud_data.ranges,
{}});
sensor_id, timed_point_cloud_data);
if (matching_result == nullptr) {
// The range data has not been fully accumulated yet.
return;

17
cartographer/mapping/map_builder.cc
View File

@ -49,6 +49,17 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
return options;
}
std::vector<std::string> SelectRangeSensorIds(
const std::set<MapBuilder::SensorId>& expected_sensor_ids) {
std::vector<std::string> range_sensor_ids;
for (const MapBuilder::SensorId& sensor_id : expected_sensor_ids) {
if (sensor_id.type == MapBuilder::SensorId::SensorType::RANGE) {
range_sensor_ids.push_back(sensor_id.id);
}
}
return range_sensor_ids;
}
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
: options_(options), thread_pool_(options.num_background_threads()) {
if (options.use_trajectory_builder_2d()) {
@ -81,7 +92,8 @@ int MapBuilder::AddTrajectoryBuilder(
std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder;
if (trajectory_options.has_trajectory_builder_3d_options()) {
local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder3D>(
trajectory_options.trajectory_builder_3d_options());
trajectory_options.trajectory_builder_3d_options(),
SelectRangeSensorIds(expected_sensor_ids));
}
DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get()));
trajectory_builders_.push_back(
@ -95,7 +107,8 @@ int MapBuilder::AddTrajectoryBuilder(
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
if (trajectory_options.has_trajectory_builder_2d_options()) {
local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder2D>(
trajectory_options.trajectory_builder_2d_options());
trajectory_options.trajectory_builder_2d_options(),
SelectRangeSensorIds(expected_sensor_ids));
}
DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
trajectory_builders_.push_back(