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

View File

@ -35,7 +35,7 @@ struct TrajectoryNode {
common::Time time; common::Time time;
// LaserFan in 'pose' frame. Only used in the 2D case. // 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. // LaserFan in 'pose' frame. Only used in the 3D case.
sensor::CompressedLaserFan3D laser_fan_3d; sensor::CompressedLaserFan3D laser_fan_3d;

View File

@ -47,7 +47,7 @@ void GlobalTrajectoryBuilder::AddHorizontalLaserFan(
if (insertion_result != nullptr) { if (insertion_result != nullptr) {
sparse_pose_graph_->AddScan( sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->tracking_to_tracking_2d, 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, insertion_result->pose_estimate_2d,
kalman_filter::Project2D(insertion_result->covariance_estimate), kalman_filter::Project2D(insertion_result->covariance_estimate),
insertion_result->submaps, insertion_result->matching_submap, insertion_result->submaps, insertion_result->matching_submap,

View File

@ -51,7 +51,7 @@ LaserFanInserter::LaserFanInserter(
miss_table_(mapping::ComputeLookupTableToApplyOdds( miss_table_(mapping::ComputeLookupTableToApplyOdds(
mapping::Odds(options.miss_probability()))) {} 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 { ProbabilityGrid* const probability_grid) const {
CHECK_NOTNULL(probability_grid)->StartUpdate(); CHECK_NOTNULL(probability_grid)->StartUpdate();

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -96,7 +96,7 @@ int SparsePoseGraph::AddScan(
CHECK_LT(j, std::numeric_limits<int>::max()); CHECK_LT(j, std::numeric_limits<int>::max());
constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{ 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, sensor::Compress(laser_fan_in_tracking), submaps,
transform::Rigid3d::Identity()}); transform::Rigid3d::Identity()});
trajectory_nodes_.push_back( trajectory_nodes_.push_back(

View File

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

View File

@ -37,14 +37,6 @@ std::vector<uint8> ReorderReflectivities(
} // namespace } // 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, LaserFan3D ToLaserFan3D(const proto::LaserScan& proto, const float min_range,
const float max_range, const float max_range,
const float missing_echo_ray_length) { 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)}; 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) { CompressedLaserFan3D Compress(const LaserFan3D& laser_fan) {
std::vector<int> new_to_old; std::vector<int> new_to_old;
CompressedPointCloud compressed_returns = CompressedPointCloud compressed_returns =

View File

@ -25,16 +25,6 @@
namespace cartographer { namespace cartographer {
namespace sensor { 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 // 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 // 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 // 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'. // 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); 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 // Like LaserFan3D but with compressed point clouds. The point order changes
// when converting from LaserFan3D. // when converting from LaserFan3D.
struct CompressedLaserFan3D { struct CompressedLaserFan3D {

View File

@ -22,49 +22,16 @@
namespace cartographer { namespace cartographer {
namespace sensor { namespace sensor {
namespace { PointCloud TransformPointCloud(const PointCloud& point_cloud,
const transform::Rigid3f& transform) {
template <typename PointCloudType, typename TransformType> PointCloud result;
PointCloudType Transform(const PointCloudType& point_cloud,
const TransformType& transform) {
PointCloudType result;
result.reserve(point_cloud.size()); result.reserve(point_cloud.size());
for (const auto& point : point_cloud) { for (const Eigen::Vector3f& point : point_cloud) {
result.emplace_back(transform * point); result.emplace_back(transform * point);
} }
return result; 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, PointCloud Crop(const PointCloud& point_cloud, const float min_z,
const float max_z) { const float max_z) {
PointCloud cropped_point_cloud; PointCloud cropped_point_cloud;

View File

@ -29,22 +29,11 @@ namespace cartographer {
namespace sensor { namespace sensor {
typedef std::vector<Eigen::Vector3f> PointCloud; typedef std::vector<Eigen::Vector3f> PointCloud;
typedef std::vector<Eigen::Vector2f> PointCloud2D;
// Transforms 'point_cloud' according to 'transform'. // Transforms 'point_cloud' according to 'transform'.
PointCloud TransformPointCloud(const PointCloud& point_cloud, PointCloud TransformPointCloud(const PointCloud& point_cloud,
const transform::Rigid3f& transform); 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 // Returns a new point cloud without points that fall outside the region defined
// by 'min_z' and 'max_z'. // by 'min_z' and 'max_z'.
PointCloud Crop(const PointCloud& point_cloud, float min_z, float max_z); PointCloud Crop(const PointCloud& point_cloud, float min_z, float max_z);

View File

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