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

Remove LaserFan in favor of LaserFan3D. (#62) 

LaserFan3D will be renamed in the next PR.
master
Wolfgang Hess 2016-10-14 11:26:53 +02:00 committed by GitHub
parent cac501cdb1
commit 9020f71605
27 changed files with 78 additions and 154 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/trajectory_node.h
View File

@ -35,7 +35,7 @@ struct TrajectoryNode {
common::Time time;
// LaserFan in 'pose' frame. Only used in the 2D case.
sensor::LaserFan laser_fan;
sensor::LaserFan3D laser_fan_2d;
// LaserFan in 'pose' frame. Only used in the 3D case.
sensor::CompressedLaserFan3D laser_fan_3d;

2
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -47,7 +47,7 @@ void GlobalTrajectoryBuilder::AddHorizontalLaserFan(
if (insertion_result != nullptr) {
sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->tracking_to_tracking_2d,
sensor::ProjectLaserFan(insertion_result->laser_fan_in_tracking_2d),
insertion_result->laser_fan_in_tracking_2d,
insertion_result->pose_estimate_2d,
kalman_filter::Project2D(insertion_result->covariance_estimate),
insertion_result->submaps, insertion_result->matching_submap,

2
cartographer/mapping_2d/laser_fan_inserter.cc
View File

@ -51,7 +51,7 @@ LaserFanInserter::LaserFanInserter(
miss_table_(mapping::ComputeLookupTableToApplyOdds(
mapping::Odds(options.miss_probability()))) {}
void LaserFanInserter::Insert(const sensor::LaserFan& laser_fan,
void LaserFanInserter::Insert(const sensor::LaserFan3D& laser_fan,
ProbabilityGrid* const probability_grid) const {
CHECK_NOTNULL(probability_grid)->StartUpdate();

2
cartographer/mapping_2d/laser_fan_inserter.h
View File

@ -42,7 +42,7 @@ class LaserFanInserter {
LaserFanInserter& operator=(const LaserFanInserter&) = delete;
// Inserts 'laser_fan' into 'probability_grid'.
void Insert(const sensor::LaserFan& laser_fan,
void Insert(const sensor::LaserFan3D& laser_fan,
ProbabilityGrid* probability_grid) const;
const std::vector<uint16>& hit_table() const { return hit_table_; }

10
cartographer/mapping_2d/laser_fan_inserter_test.cc
View File

@ -44,11 +44,11 @@ class LaserFanInserterTest : public ::testing::Test {
}
void InsertPointCloud() {
sensor::LaserFan laser_fan;
laser_fan.point_cloud.emplace_back(-3.5, 0.5);
laser_fan.point_cloud.emplace_back(-2.5, 1.5);
laser_fan.point_cloud.emplace_back(-1.5, 2.5);
laser_fan.point_cloud.emplace_back(-0.5, 3.5);
sensor::LaserFan3D laser_fan;
laser_fan.returns.emplace_back(-3.5, 0.5, 0.f);
laser_fan.returns.emplace_back(-2.5, 1.5, 0.f);
laser_fan.returns.emplace_back(-1.5, 2.5, 0.f);
laser_fan.returns.emplace_back(-0.5, 3.5, 0.f);
laser_fan.origin.x() = -0.5;
laser_fan.origin.y() = 0.5;
probability_grid_.StartUpdate();

4
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -212,8 +212,8 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
for (int insertion_index : submaps_.insertion_indices()) {
insertion_submaps.push_back(submaps_.Get(insertion_index));
}
submaps_.InsertLaserFan(sensor::ProjectLaserFan(TransformLaserFan3D(
laser_fan_in_tracking_2d, tracking_2d_to_map.cast<float>())));
submaps_.InsertLaserFan(TransformLaserFan3D(
laser_fan_in_tracking_2d, tracking_2d_to_map.cast<float>()));
return common::make_unique<InsertionResult>(InsertionResult{
time, &submaps_, matching_submap, insertion_submaps,

20
cartographer/mapping_2d/map_limits.h
View File

@ -102,21 +102,21 @@ class MapLimits {
for (const auto& node : trajectory_nodes) {
const auto& data = *node.constant_data;
if (!data.laser_fan_3d.returns.empty()) {
const sensor::LaserFan3D laser_fan_3d = sensor::TransformLaserFan3D(
const sensor::LaserFan3D laser_fan = sensor::TransformLaserFan3D(
Decompress(data.laser_fan_3d), node.pose.cast<float>());
bounding_box.extend(laser_fan_3d.origin.head<2>());
for (const Eigen::Vector3f& hit : laser_fan_3d.returns) {
bounding_box.extend(laser_fan.origin.head<2>());
for (const Eigen::Vector3f& hit : laser_fan.returns) {
bounding_box.extend(hit.head<2>());
}
} else {
const sensor::LaserFan laser_fan = sensor::TransformLaserFan(
data.laser_fan, transform::Project2D(node.pose).cast<float>());
bounding_box.extend(laser_fan.origin);
for (const Eigen::Vector2f& hit : laser_fan.point_cloud) {
bounding_box.extend(hit);
const sensor::LaserFan3D laser_fan = sensor::TransformLaserFan3D(
data.laser_fan_2d, node.pose.cast<float>());
bounding_box.extend(laser_fan.origin.head<2>());
for (const Eigen::Vector3f& hit : laser_fan.returns) {
bounding_box.extend(hit.head<2>());
}
for (const Eigen::Vector2f& miss : laser_fan.missing_echo_point_cloud) {
bounding_box.extend(miss);
for (const Eigen::Vector3f& miss : laser_fan.misses) {
bounding_box.extend(miss.head<2>());
}
}
}

6
cartographer/mapping_2d/map_limits_test.cc
View File

@ -39,9 +39,9 @@ TEST(MapLimitsTest, ConstructAndGet) {
TEST(MapLimitsTest, ComputeMapLimits) {
const mapping::TrajectoryNode::ConstantData constant_data{
common::FromUniversal(52),
sensor::LaserFan{
Eigen::Vector2f::Zero(),
{Eigen::Vector2f(-30.f, 1.f), Eigen::Vector2f(50.f, -10.f)},
sensor::LaserFan3D{
Eigen::Vector3f::Zero(),
{Eigen::Vector3f(-30.f, 1.f, 0.f), Eigen::Vector3f(50.f, -10.f, 0.f)},
{}},
Compress(sensor::LaserFan3D{Eigen::Vector3f::Zero(), {}, {}, {}}),
nullptr, transform::Rigid3d::Identity()};

9
cartographer/mapping_2d/ray_casting.cc
View File

@ -147,7 +147,7 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
} // namespace
void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
void CastRays(const sensor::LaserFan3D& laser_fan, const MapLimits& limits,
const std::function<void(const Eigen::Array2i&)>& hit_visitor,
const std::function<void(const Eigen::Array2i&)>& miss_visitor) {
const double superscaled_resolution = limits.resolution() / kSubpixelScale;
@ -161,8 +161,8 @@ void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
// Compute and add the end points.
std::vector<Eigen::Array2i> ends;
ends.reserve(laser_fan.point_cloud.size());
for (const Eigen::Vector2f& laser_return : laser_fan.point_cloud) {
ends.reserve(laser_fan.returns.size());
for (const Eigen::Vector3f& laser_return : laser_fan.returns) {
ends.push_back(superscaled_limits.GetXYIndexOfCellContainingPoint(
laser_return.x(), laser_return.y()));
hit_visitor(ends.back() / kSubpixelScale);
@ -174,8 +174,7 @@ void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
}
// Finally, compute and add empty rays based on missing echos in the scan.
for (const Eigen::Vector2f& missing_echo :
laser_fan.missing_echo_point_cloud) {
for (const Eigen::Vector3f& missing_echo : laser_fan.misses) {
CastRay(begin, superscaled_limits.GetXYIndexOfCellContainingPoint(
missing_echo.x(), missing_echo.y()),
miss_visitor);

2
cartographer/mapping_2d/ray_casting.h
View File

@ -30,7 +30,7 @@ namespace mapping_2d {
// For each ray in 'laser_fan', calls 'hit_visitor' and 'miss_visitor' on the
// appropriate cells. Hits are handled before misses.
void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
void CastRays(const sensor::LaserFan3D& laser_fan, const MapLimits& limits,
const std::function<void(const Eigen::Array2i&)>& hit_visitor,
const std::function<void(const Eigen::Array2i&)>& miss_visitor);

23
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

@ -150,10 +150,11 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
probability_grid.StartUpdate();
laser_fan_inserter.Insert(
sensor::LaserFan{
expected_pose.translation(),
sensor::TransformPointCloud2D(
sensor::ProjectToPointCloud2D(point_cloud), expected_pose),
sensor::LaserFan3D{
Eigen::Vector3f(expected_pose.translation().x(),
expected_pose.translation().y(), 0.f),
sensor::TransformPointCloud(
point_cloud, transform::Embed3D(expected_pose.cast<float>())),
{}},
&probability_grid);
@ -192,11 +193,7 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
{10. * distribution(prng), 10. * distribution(prng)},
1.6 * distribution(prng));
const sensor::PointCloud point_cloud = sensor::TransformPointCloud(
unperturbed_point_cloud,
transform::Rigid3f(Eigen::Vector3f(perturbation.translation().x(),
perturbation.translation().y(), 0.f),
Eigen::AngleAxisf(perturbation.rotation().angle(),
Eigen::Vector3f::UnitZ())));
unperturbed_point_cloud, transform::Embed3D(perturbation));
const transform::Rigid2f expected_pose =
transform::Rigid2f({2. * distribution(prng), 2. * distribution(prng)},
0.5 * distribution(prng)) *
@ -206,10 +203,10 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
probability_grid.StartUpdate();
laser_fan_inserter.Insert(
sensor::LaserFan{
(expected_pose * perturbation).translation(),
sensor::TransformPointCloud2D(
sensor::ProjectToPointCloud2D(point_cloud), expected_pose),
sensor::LaserFan3D{
transform::Embed3D(expected_pose * perturbation).translation(),
sensor::TransformPointCloud(point_cloud,
transform::Embed3D(expected_pose)),
{}},
&probability_grid);

4
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc
View File

@ -58,9 +58,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
point_cloud_.emplace_back(-0.125f, 0.025f, 0.f);
probability_grid_.StartUpdate();
laser_fan_inserter_->Insert(
sensor::LaserFan{Eigen::Vector2f::Zero(),
sensor::ProjectToPointCloud2D(point_cloud_),
{}},
sensor::LaserFan3D{Eigen::Vector3f::Zero(), point_cloud_, {}},
&probability_grid_);
{
auto parameter_dictionary = common::MakeDictionary(

9
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -86,7 +86,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
void SparsePoseGraph::AddScan(
common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::LaserFan& laser_fan_in_pose, const transform::Rigid2d& pose,
const sensor::LaserFan3D& laser_fan_in_pose, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance,
const mapping::Submaps* submaps,
const mapping::Submap* const matching_submap,
@ -145,7 +145,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
point_cloud_poses_[scan_index];
constraint_builder_.MaybeAddConstraint(
submap_index, submap, scan_index,
&trajectory_nodes_[scan_index].constant_data->laser_fan.point_cloud,
&trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns,
relative_pose);
}
}
@ -202,8 +202,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
constraint_builder_.MaybeAddGlobalConstraint(
submap_index, submap_states_[submap_index].submap, scan_index,
scan_trajectory, submap_trajectory, &trajectory_connectivity_,
&trajectory_nodes_[scan_index]
.constant_data->laser_fan.point_cloud);
&trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns);
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(scan_trajectory) > 0 &&
@ -215,7 +214,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
constraint_builder_.MaybeAddConstraint(
submap_index, submap_states_[submap_index].submap, scan_index,
&trajectory_nodes_[scan_index]
.constant_data->laser_fan.point_cloud,
.constant_data->laser_fan_2d.returns,
relative_pose);
}
}

2
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -70,7 +70,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// by scan matching against the 'matching_submap' and the scan was inserted
// into the 'insertion_submaps'.
void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::LaserFan& laser_fan_in_pose,
const sensor::LaserFan3D& laser_fan_in_pose,
const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& pose_covariance,
const mapping::Submaps* submaps,

8
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
View File

@ -62,7 +62,7 @@ ConstraintBuilder::~ConstraintBuilder() {
void ConstraintBuilder::MaybeAddConstraint(
const int submap_index, const mapping::Submap* const submap,
const int scan_index, const sensor::PointCloud2D* const point_cloud,
const int scan_index, const sensor::PointCloud* const point_cloud,
const transform::Rigid2d& initial_relative_pose) {
if (initial_relative_pose.translation().norm() >
options_.max_constraint_distance()) {
@ -94,7 +94,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
const int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud2D* const point_cloud) {
const sensor::PointCloud* const point_cloud) {
common::MutexLocker locker(&mutex_);
CHECK_LE(scan_index, current_computation_);
constraints_.emplace_back();
@ -174,7 +174,7 @@ void ConstraintBuilder::ComputeConstraint(
const int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud2D* const point_cloud,
const sensor::PointCloud* const point_cloud,
const transform::Rigid2d& initial_relative_pose,
std::unique_ptr<OptimizationProblem::Constraint>* constraint) {
const transform::Rigid2d initial_pose =
@ -182,7 +182,7 @@ void ConstraintBuilder::ComputeConstraint(
const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_index);
const sensor::PointCloud filtered_point_cloud =
adaptive_voxel_filter_.Filter(sensor::ToPointCloud(*point_cloud));
adaptive_voxel_filter_.Filter(*point_cloud);
// The 'constraint_transform' (i <- j) is computed from:
// - a 'filtered_point_cloud' in j,

7
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h
View File

@ -78,8 +78,7 @@ class ConstraintBuilder {
// The pointees of 'submap' and 'point_cloud' must stay valid until all
// computations are finished.
void MaybeAddConstraint(int submap_index, const mapping::Submap* submap,
int scan_index,
const sensor::PointCloud2D* point_cloud,
int scan_index, const sensor::PointCloud* point_cloud,
const transform::Rigid2d& initial_relative_pose);
// Schedules exploring a new constraint between 'submap' identified by
@ -97,7 +96,7 @@ class ConstraintBuilder {
const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud2D* point_cloud);
const sensor::PointCloud* point_cloud);
// Must be called after all computations related to 'scan_index' are added.
void NotifyEndOfScan(const int scan_index);
@ -142,7 +141,7 @@ class ConstraintBuilder {
const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud2D* point_cloud,
const sensor::PointCloud* point_cloud,
const transform::Rigid2d& initial_relative_pose,
std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);

17
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -44,7 +44,7 @@ class SparsePoseGraphTest : public ::testing::Test {
// kMinProbability) to unknown space (== kMinProbability).
for (float t = 0.f; t < 2.f * M_PI; t += 0.005f) {
const float r = (std::sin(20.f * t) + 2.f) * std::sin(t + 2.f);
point_cloud_.emplace_back(r * std::sin(t), r * std::cos(t));
point_cloud_.emplace_back(r * std::sin(t), r * std::cos(t), 0.f);
}
{
@ -146,8 +146,9 @@ class SparsePoseGraphTest : public ::testing::Test {
void MoveRelativeWithNoise(const transform::Rigid2d& movement,
const transform::Rigid2d& noise) {
current_pose_ = current_pose_ * movement;
const sensor::PointCloud2D new_point_cloud = sensor::TransformPointCloud2D(
point_cloud_, current_pose_.inverse().cast<float>());
const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
point_cloud_,
transform::Embed3D(current_pose_.inverse().cast<float>()));
kalman_filter::Pose2DCovariance covariance =
kalman_filter::Pose2DCovariance::Identity();
const mapping::Submap* const matching_submap =
@ -156,11 +157,11 @@ class SparsePoseGraphTest : public ::testing::Test {
for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index));
}
const sensor::LaserFan laser_fan{
Eigen::Vector2f::Zero(), new_point_cloud, {}};
const sensor::LaserFan3D laser_fan{
Eigen::Vector3f::Zero(), new_point_cloud, {}};
const transform::Rigid2d pose_estimate = noise * current_pose_;
submaps_->InsertLaserFan(
TransformLaserFan(laser_fan, pose_estimate.cast<float>()));
submaps_->InsertLaserFan(TransformLaserFan3D(
laser_fan, transform::Embed3D(pose_estimate.cast<float>())));
sparse_pose_graph_->AddScan(common::FromUniversal(0),
transform::Rigid3d::Identity(), laser_fan,
pose_estimate, covariance, submaps_.get(),
@ -171,7 +172,7 @@ class SparsePoseGraphTest : public ::testing::Test {
MoveRelativeWithNoise(movement, transform::Rigid2d::Identity());
}
sensor::PointCloud2D point_cloud_;
sensor::PointCloud point_cloud_;
std::unique_ptr<Submaps> submaps_;
std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
common::ThreadPool thread_pool_;

4
cartographer/mapping_2d/submaps.cc
View File

@ -114,7 +114,7 @@ Submaps::Submaps(const proto::SubmapsOptions& options)
AddSubmap(Eigen::Vector2f::Zero());
}
void Submaps::InsertLaserFan(const sensor::LaserFan& laser_fan) {
void Submaps::InsertLaserFan(const sensor::LaserFan3D& laser_fan) {
CHECK_LT(num_laser_fans_, std::numeric_limits<int>::max());
++num_laser_fans_;
for (const int index : insertion_indices()) {
@ -125,7 +125,7 @@ void Submaps::InsertLaserFan(const sensor::LaserFan& laser_fan) {
}
++num_laser_fans_in_last_submap_;
if (num_laser_fans_in_last_submap_ == options_.num_laser_fans()) {
AddSubmap(laser_fan.origin);
AddSubmap(laser_fan.origin.head<2>());
}
}

2
cartographer/mapping_2d/submaps.h
View File

@ -64,7 +64,7 @@ class Submaps : public mapping::Submaps {
mapping::proto::SubmapQuery::Response* response) override;
// Inserts 'laser_fan' into the Submap collection.
void InsertLaserFan(const sensor::LaserFan& laser_fan);
void InsertLaserFan(const sensor::LaserFan3D& laser_fan);
private:
void FinishSubmap(int index);

2
cartographer/mapping_2d/submaps_test.cc
View File

@ -51,7 +51,7 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
submaps.Get(i)->begin_laser_fan_index;
};
for (int i = 0; i != 1000; ++i) {
submaps.InsertLaserFan({Eigen::Vector2f::Zero(), {}, {}});
submaps.InsertLaserFan({Eigen::Vector3f::Zero(), {}, {}});
const int matching = submaps.matching_index();
// Except for the first, maps should only be returned after enough scans.
if (matching != 0) {

2
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -96,7 +96,7 @@ int SparsePoseGraph::AddScan(
CHECK_LT(j, std::numeric_limits<int>::max());
constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{
time, sensor::LaserFan{Eigen::Vector2f::Zero(), {}, {}},
time, sensor::LaserFan3D{Eigen::Vector3f::Zero(), {}, {}},
sensor::Compress(laser_fan_in_tracking), submaps,
transform::Rigid3d::Identity()});
trajectory_nodes_.push_back(

1
cartographer/sensor/CMakeLists.txt
View File

@ -121,6 +121,7 @@ google_test(sensor_point_cloud_test
point_cloud_test.cc
DEPENDS
sensor_point_cloud
transform_transform
)
google_test(sensor_voxel_filter_test

14
cartographer/sensor/laser.cc
View File

@ -37,14 +37,6 @@ std::vector<uint8> ReorderReflectivities(
} // namespace
LaserFan TransformLaserFan(const LaserFan& laser_fan,
const transform::Rigid2f& transform) {
return LaserFan{
transform * laser_fan.origin,
TransformPointCloud2D(laser_fan.point_cloud, transform),
TransformPointCloud2D(laser_fan.missing_echo_point_cloud, transform)};
}
LaserFan3D ToLaserFan3D(const proto::LaserScan& proto, const float min_range,
const float max_range,
const float missing_echo_ray_length) {
@ -119,12 +111,6 @@ LaserFan3D CropLaserFan(const LaserFan3D& laser_fan, const float min_z,
Crop(laser_fan.misses, min_z, max_z)};
}
LaserFan ProjectLaserFan(const LaserFan3D& laser_fan) {
return LaserFan{laser_fan.origin.head<2>(),
ProjectToPointCloud2D(laser_fan.returns),
ProjectToPointCloud2D(laser_fan.misses)};
}
CompressedLaserFan3D Compress(const LaserFan3D& laser_fan) {
std::vector<int> new_to_old;
CompressedPointCloud compressed_returns =

13
cartographer/sensor/laser.h
View File

@ -25,16 +25,6 @@
namespace cartographer {
namespace sensor {
struct LaserFan {
Eigen::Vector2f origin;
PointCloud2D point_cloud;
PointCloud2D missing_echo_point_cloud;
};
// Transforms 'laser_fan' according to 'transform'.
LaserFan TransformLaserFan(const LaserFan& laser_fan,
const transform::Rigid2f& transform);
// A 3D variant of LaserFan. Rays begin at 'origin'. 'returns' are the points
// where laser returns were detected. 'misses' are points in the direction of
// rays for which no return was detected, and were inserted at a configured
@ -73,9 +63,6 @@ LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan,
// Crops 'laser_fan' according to the region defined by 'min_z' and 'max_z'.
LaserFan3D CropLaserFan(const LaserFan3D& laser_fan, float min_z, float max_z);
// Projects 'laser_fan' into 2D.
LaserFan ProjectLaserFan(const LaserFan3D& laser_fan);
// Like LaserFan3D but with compressed point clouds. The point order changes
// when converting from LaserFan3D.
struct CompressedLaserFan3D {

41
cartographer/sensor/point_cloud.cc
View File

@ -22,49 +22,16 @@
namespace cartographer {
namespace sensor {
namespace {
template <typename PointCloudType, typename TransformType>
PointCloudType Transform(const PointCloudType& point_cloud,
const TransformType& transform) {
PointCloudType result;
PointCloud TransformPointCloud(const PointCloud& point_cloud,
const transform::Rigid3f& transform) {
PointCloud result;
result.reserve(point_cloud.size());
for (const auto& point : point_cloud) {
for (const Eigen::Vector3f& point : point_cloud) {
result.emplace_back(transform * point);
}
return result;
}
} // namespace
PointCloud TransformPointCloud(const PointCloud& point_cloud,
const transform::Rigid3f& transform) {
return Transform(point_cloud, transform);
}
PointCloud2D TransformPointCloud2D(const PointCloud2D& point_cloud_2d,
const transform::Rigid2f& transform) {
return Transform(point_cloud_2d, transform);
}
PointCloud ToPointCloud(const PointCloud2D& point_cloud_2d) {
sensor::PointCloud point_cloud;
point_cloud.reserve(point_cloud_2d.size());
for (const auto& point : point_cloud_2d) {
point_cloud.emplace_back(point.x(), point.y(), 0.f);
}
return point_cloud;
}
PointCloud2D ProjectToPointCloud2D(const PointCloud& point_cloud) {
sensor::PointCloud2D point_cloud_2d;
point_cloud_2d.reserve(point_cloud.size());
for (const auto& point : point_cloud) {
point_cloud_2d.emplace_back(point.x(), point.y());
}
return point_cloud_2d;
}
PointCloud Crop(const PointCloud& point_cloud, const float min_z,
const float max_z) {
PointCloud cropped_point_cloud;

11
cartographer/sensor/point_cloud.h
View File

@ -29,22 +29,11 @@ namespace cartographer {
namespace sensor {
typedef std::vector<Eigen::Vector3f> PointCloud;
typedef std::vector<Eigen::Vector2f> PointCloud2D;
// Transforms 'point_cloud' according to 'transform'.
PointCloud TransformPointCloud(const PointCloud& point_cloud,
const transform::Rigid3f& transform);
// Transforms 'point_cloud_2d' according to 'transform'.
PointCloud2D TransformPointCloud2D(const PointCloud2D& point_cloud_2d,
const transform::Rigid2f& transform);
// Converts 'point_cloud_2d' to a 3D point cloud.
PointCloud ToPointCloud(const PointCloud2D& point_cloud_2d);
// Converts 'point_cloud' to a 2D point cloud by removing the z component.
PointCloud2D ProjectToPointCloud2D(const PointCloud& point_cloud);
// 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);

13
cartographer/sensor/point_cloud_test.cc
View File

@ -15,6 +15,7 @@
*/
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/transform.h"
#include <cmath>
@ -24,12 +25,12 @@ namespace cartographer {
namespace sensor {
namespace {
TEST(PointCloudTest, TransformPointCloud2D) {
PointCloud2D point_cloud;
point_cloud.emplace_back(0.5f, 0.5f);
point_cloud.emplace_back(3.5f, 0.5f);
const PointCloud2D transformed_point_cloud =
TransformPointCloud2D(point_cloud, transform::Rigid2f::Rotation(M_PI_2));
TEST(PointCloudTest, TransformPointCloud) {
PointCloud point_cloud;
point_cloud.emplace_back(0.5f, 0.5f, 1.f);
point_cloud.emplace_back(3.5f, 0.5f, 42.f);
const PointCloud transformed_point_cloud = TransformPointCloud(
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);