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

Add TimedPointCloud and TimedRangeData. (#601) 

Adds a type TimedPointCloud that holds 4-dimensional vectors where measurement time of individual points are stored in the fourth entry.
Uses TimedRangeData to pass TimedPointCloud of incoming measurements to LocalTrajectoryBuilder.

Fixes #573.
master
gaschler 2017-10-24 11:47:35 +02:00 committed by GitHub
parent 638aee7c2a
commit d4db1e79a6
15 changed files with 151 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/mapping/global_trajectory_builder.h
View File

@ -46,10 +46,10 @@ class GlobalTrajectoryBuilder
void AddRangefinderData(const common::Time time,
const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) override {
const sensor::TimedPointCloud& ranges) override {
std::unique_ptr<typename LocalTrajectoryBuilder::InsertionResult>
insertion_result = local_trajectory_builder_.AddRangeData(
time, sensor::RangeData{origin, ranges, {}});
time, sensor::TimedRangeData{origin, ranges, {}});
if (insertion_result == nullptr) {
return;
}

2
cartographer/mapping/global_trajectory_builder_interface.h
View File

@ -53,7 +53,7 @@ class GlobalTrajectoryBuilderInterface {
virtual void AddRangefinderData(common::Time time,
const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) = 0;
const sensor::TimedPointCloud& ranges) = 0;
virtual void AddSensorData(const sensor::ImuData& imu_data) = 0;
virtual void AddSensorData(const sensor::OdometryData& odometry_data) = 0;
virtual void AddSensorData(

2
cartographer/mapping/trajectory_builder.h
View File

@ -56,7 +56,7 @@ class TrajectoryBuilder {
void AddRangefinderData(const string& sensor_id, common::Time time,
const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) {
const sensor::TimedPointCloud& ranges) {
AddSensorData(sensor_id,
common::make_unique<sensor::DispatchableRangefinderData>(
time, origin, ranges));

15
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -75,7 +75,7 @@ void LocalTrajectoryBuilder::ScanMatch(
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
LocalTrajectoryBuilder::AddRangeData(const common::Time time,
const sensor::RangeData& range_data) {
const sensor::TimedRangeData& range_data) {
// Initialize extrapolator now if we do not ever use an IMU.
if (!options_.use_imu_data()) {
InitializeExtrapolator(time);
@ -93,19 +93,22 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}};
}
// TODO(gaschler): Take time delta of individual points into account.
const transform::Rigid3f tracking_delta =
first_pose_estimate_.inverse() *
extrapolator_->ExtrapolatePose(time).cast<float>();
const sensor::RangeData range_data_in_first_tracking =
sensor::TransformRangeData(range_data, tracking_delta);
// TODO(gaschler): Try to move this common behavior with 3D into helper.
const sensor::TimedRangeData range_data_in_first_tracking =
sensor::TransformTimedRangeData(range_data, tracking_delta);
// Drop any returns below the minimum range and convert returns beyond the
// maximum range into misses.
for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) {
const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin;
for (const Eigen::Vector4f& hit : range_data_in_first_tracking.returns) {
const Eigen::Vector3f delta =
hit.head<3>() - range_data_in_first_tracking.origin;
const float range = delta.norm();
if (range >= options_.min_range()) {
if (range <= options_.max_range()) {
accumulated_range_data_.returns.push_back(hit);
accumulated_range_data_.returns.push_back(hit.head<3>());
} else {
accumulated_range_data_.misses.push_back(
range_data_in_first_tracking.origin +

3
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -38,6 +38,7 @@ namespace mapping_2d {
// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
// without loop closure.
// TODO(gaschler): Add test for this class similar to the 3D test.
class LocalTrajectoryBuilder {
public:
struct InsertionResult {
@ -56,7 +57,7 @@ class LocalTrajectoryBuilder {
// Range data must be approximately horizontal for 2D SLAM.
std::unique_ptr<InsertionResult> AddRangeData(
common::Time, const sensor::RangeData& range_data);
common::Time, const sensor::TimedRangeData& range_data);
void AddImuData(const sensor::ImuData& imu_data);
void AddOdometerData(const sensor::OdometryData& odometry_data);

14
cartographer/mapping_3d/local_trajectory_builder.cc
View File

@ -60,7 +60,7 @@ void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
LocalTrajectoryBuilder::AddRangeData(const common::Time time,
const sensor::RangeData& range_data) {
const sensor::TimedRangeData& range_data) {
if (extrapolator_ == nullptr) {
// Until we've initialized the extrapolator with our first IMU message, we
// cannot compute the orientation of the rangefinder.
@ -73,17 +73,19 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}};
}
// TODO(gaschler): Take time delta of individual points into account.
const transform::Rigid3f tracking_delta =
first_pose_estimate_.inverse() *
extrapolator_->ExtrapolatePose(time).cast<float>();
const sensor::RangeData range_data_in_first_tracking =
sensor::TransformRangeData(range_data, tracking_delta);
for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) {
const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin;
const sensor::TimedRangeData range_data_in_first_tracking =
sensor::TransformTimedRangeData(range_data, tracking_delta);
for (const Eigen::Vector4f& hit : range_data_in_first_tracking.returns) {
const Eigen::Vector3f delta =
hit.head<3>() - range_data_in_first_tracking.origin;
const float range = delta.norm();
if (range >= options_.min_range()) {
if (range <= options_.max_range()) {
accumulated_range_data_.returns.push_back(hit);
accumulated_range_data_.returns.push_back(hit.head<3>());
} 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

2
cartographer/mapping_3d/local_trajectory_builder.h
View File

@ -54,7 +54,7 @@ class LocalTrajectoryBuilder {
void AddImuData(const sensor::ImuData& imu_data);
std::unique_ptr<InsertionResult> AddRangeData(
common::Time time, const sensor::RangeData& range_data);
common::Time time, const sensor::TimedRangeData& range_data);
void AddOdometerData(const sensor::OdometryData& odometry_data);
const mapping::PoseEstimate& pose_estimate() const;

53
cartographer/mapping_3d/local_trajectory_builder_test.cc
View File

@ -170,37 +170,48 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
return first * (to - from) + from;
}
sensor::RangeData GenerateRangeData(const transform::Rigid3d& pose) {
sensor::TimedRangeData GenerateRangeData(const transform::Rigid3d& pose) {
// 360 degree rays at 16 angles.
sensor::PointCloud directions_in_rangefinder_frame;
sensor::TimedPointCloud directions_in_rangefinder_frame;
for (int r = -8; r != 8; ++r) {
for (int s = -250; s != 250; ++s) {
directions_in_rangefinder_frame.push_back(
Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(M_PI / 12. * r / 8., Eigen::Vector3f::UnitY()) *
Eigen::Vector3f::UnitX());
Eigen::Vector4f first_point;
first_point << Eigen::AngleAxisf(M_PI * s / 250.,
Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(M_PI / 12. * r / 8.,
Eigen::Vector3f::UnitY()) *
Eigen::Vector3f::UnitX(),
0.;
directions_in_rangefinder_frame.push_back(first_point);
// Second orthogonal rangefinder.
directions_in_rangefinder_frame.push_back(
Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) *
Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(M_PI / 12. * r / 8., Eigen::Vector3f::UnitY()) *
Eigen::Vector3f::UnitX());
Eigen::Vector4f second_point;
second_point << Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) *
Eigen::AngleAxisf(M_PI * s / 250.,
Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(M_PI / 12. * r / 8.,
Eigen::Vector3f::UnitY()) *
Eigen::Vector3f::UnitX(),
0.;
directions_in_rangefinder_frame.push_back(second_point);
}
}
// We simulate a 30 m edge length box around the origin, also containing
// 50 cm radius spheres.
sensor::PointCloud returns_in_world_frame;
for (const Eigen::Vector3f& direction_in_world_frame :
sensor::TransformPointCloud(directions_in_rangefinder_frame,
pose.cast<float>())) {
sensor::TimedPointCloud returns_in_world_frame;
for (const Eigen::Vector4f& direction_in_world_frame :
sensor::TransformTimedPointCloud(directions_in_rangefinder_frame,
pose.cast<float>())) {
const Eigen::Vector3f origin =
pose.cast<float>() * Eigen::Vector3f::Zero();
returns_in_world_frame.push_back(CollideWithBubbles(
origin, CollideWithBox(origin, direction_in_world_frame)));
Eigen::Vector4f return_point;
return_point << CollideWithBubbles(
origin, CollideWithBox(origin, direction_in_world_frame.head<3>())),
0.;
returns_in_world_frame.push_back(return_point);
}
return {Eigen::Vector3f::Zero(),
sensor::TransformPointCloud(returns_in_world_frame,
pose.inverse().cast<float>()),
sensor::TransformTimedPointCloud(returns_in_world_frame,
pose.inverse().cast<float>()),
{}};
}
@ -242,8 +253,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
const auto range_data = GenerateRangeData(node.pose);
if (local_trajectory_builder_->AddRangeData(
node.time,
sensor::RangeData{range_data.origin, range_data.returns, {}}) !=
nullptr) {
sensor::TimedRangeData{
range_data.origin, range_data.returns, {}}) != nullptr) {
const auto pose_estimate = local_trajectory_builder_->pose_estimate();
EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));
++num_poses;

2
cartographer/sensor/compressed_point_cloud.h
View File

@ -29,7 +29,7 @@ namespace cartographer {
namespace sensor {
// A compressed representation of a point cloud consisting of a collection of
// points (Vector3f).
// points (Vector3f) without time information.
// Internally, points are grouped by blocks. Each block encodes a bit of meta
// data (number of points in block, coordinates of the block) and encodes each
// point with a fixed bit rate in relation to the block.

4
cartographer/sensor/data.h
View File

@ -42,7 +42,7 @@ class DispatchableRangefinderData : public Data {
public:
DispatchableRangefinderData(const common::Time time,
const Eigen::Vector3f& origin,
const PointCloud& ranges)
const TimedPointCloud& ranges)
: time_(time), origin_(origin), ranges_(ranges) {}
common::Time GetTime() const override { return time_; }
@ -54,7 +54,7 @@ class DispatchableRangefinderData : public Data {
private:
const common::Time time_;
const Eigen::Vector3f origin_;
const PointCloud ranges_;
const TimedPointCloud ranges_;
};
template <typename DataType>

28
cartographer/sensor/point_cloud.cc
View File

@ -32,8 +32,21 @@ PointCloud TransformPointCloud(const PointCloud& point_cloud,
return result;
}
PointCloud Crop(const PointCloud& point_cloud, const float min_z,
const float max_z) {
TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud,
const transform::Rigid3f& transform) {
TimedPointCloud result;
result.reserve(point_cloud.size());
for (const Eigen::Vector4f& point : point_cloud) {
Eigen::Vector4f result_point;
result_point.head<3>() = transform * point.head<3>();
result_point[3] = point[3];
result.emplace_back(result_point);
}
return result;
}
PointCloud CropPointCloud(const PointCloud& point_cloud, const float min_z,
const float max_z) {
PointCloud cropped_point_cloud;
for (const auto& point : point_cloud) {
if (min_z <= point.z() && point.z() <= max_z) {
@ -43,5 +56,16 @@ PointCloud Crop(const PointCloud& point_cloud, const float min_z,
return cropped_point_cloud;
}
TimedPointCloud CropTimedPointCloud(const TimedPointCloud& point_cloud,
const float min_z, const float max_z) {
TimedPointCloud cropped_point_cloud;
for (const auto& point : point_cloud) {
if (min_z <= point.z() && point.z() <= max_z) {
cropped_point_cloud.push_back(point);
}
}
return cropped_point_cloud;
}
} // namespace sensor
} // namespace cartographer

29
cartographer/sensor/point_cloud.h
View File

@ -27,25 +27,38 @@
namespace cartographer {
namespace sensor {
// Stores 3D positions of points.
// For 2D points, the third entry is 0.f.
typedef std::vector<Eigen::Vector3f> PointCloud;
struct PointCloudWithIntensities {
PointCloud points;
std::vector<float> intensities;
// Stores 3D positions of points with their measurement time in the fourth
// entry. Time is in seconds, increasing and relative to the moment when
// 'points[0]' was acquired. If timing is not available, all fourth entries
// are 0.f. For 2D points, the third entry is 0.f and the fourth entry is time.
typedef std::vector<Eigen::Vector4f> TimedPointCloud;
// For each item in 'points', contains the time delta of when it was acquired
// after points[0], i.e. the first entry is always 0.f. If timing
// information is not available all entries will be 0.f.
std::vector<float> offset_seconds;
struct PointCloudWithIntensities {
TimedPointCloud points;
std::vector<float> intensities;
};
// Transforms 'point_cloud' according to 'transform'.
PointCloud TransformPointCloud(const PointCloud& point_cloud,
const transform::Rigid3f& transform);
// Transforms 'point_cloud' according to 'transform'.
TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud,
const transform::Rigid3f& transform);
// Returns a new point cloud without points that fall outside the region defined
// by 'min_z' and 'max_z'.
PointCloud Crop(const PointCloud& point_cloud, float min_z, float max_z);
PointCloud CropPointCloud(const PointCloud& point_cloud, float min_z,
float max_z);
// Returns a new point cloud without points that fall outside the region defined
// by 'min_z' and 'max_z'.
TimedPointCloud CropTimedPointCloud(const TimedPointCloud& point_cloud,
float min_z, float max_z);
} // namespace sensor
} // namespace cartographer

12
cartographer/sensor/point_cloud_test.cc
View File

@ -37,6 +37,18 @@ TEST(PointCloudTest, TransformPointCloud) {
EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6);
}
TEST(PointCloudTest, TransformTimedPointCloud) {
TimedPointCloud point_cloud;
point_cloud.emplace_back(0.5f, 0.5f, 1.f, 0.f);
point_cloud.emplace_back(3.5f, 0.5f, 42.f, 0.f);
const TimedPointCloud transformed_point_cloud = TransformTimedPointCloud(
point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2)));
EXPECT_NEAR(-0.5f, transformed_point_cloud[0].x(), 1e-6);
EXPECT_NEAR(0.5f, transformed_point_cloud[0].y(), 1e-6);
EXPECT_NEAR(-0.5f, transformed_point_cloud[1].x(), 1e-6);
EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6);
}
} // namespace
} // namespace sensor
} // namespace cartographer

21
cartographer/sensor/range_data.cc
View File

@ -31,10 +31,27 @@ RangeData TransformRangeData(const RangeData& range_data,
};
}
TimedRangeData TransformTimedRangeData(const TimedRangeData& range_data,
const transform::Rigid3f& transform) {
return TimedRangeData{
transform * range_data.origin,
TransformTimedPointCloud(range_data.returns, transform),
TransformTimedPointCloud(range_data.misses, transform),
};
}
RangeData CropRangeData(const RangeData& range_data, const float min_z,
const float max_z) {
return RangeData{range_data.origin, Crop(range_data.returns, min_z, max_z),
Crop(range_data.misses, min_z, max_z)};
return RangeData{range_data.origin,
CropPointCloud(range_data.returns, min_z, max_z),
CropPointCloud(range_data.misses, min_z, max_z)};
}
TimedRangeData CropTimedRangeData(const TimedRangeData& range_data,
const float min_z, const float max_z) {
return TimedRangeData{range_data.origin,
CropTimedPointCloud(range_data.returns, min_z, max_z),
CropTimedPointCloud(range_data.misses, min_z, max_z)};
}
} // namespace sensor

14
cartographer/sensor/range_data.h
View File

@ -35,12 +35,26 @@ struct RangeData {
PointCloud misses;
};
// Like 'RangeData', but with 'TimedPointClouds'.
struct TimedRangeData {
Eigen::Vector3f origin;
TimedPointCloud returns;
TimedPointCloud misses;
};
RangeData TransformRangeData(const RangeData& range_data,
const transform::Rigid3f& transform);
TimedRangeData TransformTimedRangeData(const TimedRangeData& range_data,
const transform::Rigid3f& transform);
// Crops 'range_data' according to the region defined by 'min_z' and 'max_z'.
RangeData CropRangeData(const RangeData& range_data, float min_z, float max_z);
// Crops 'range_data' according to the region defined by 'min_z' and 'max_z'.
TimedRangeData CropTimedRangeData(const TimedRangeData& range_data, float min_z,
float max_z);
} // namespace sensor
} // namespace cartographer