From 9020f716056621ee57f3cf1c7a7a9a2d81833817 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 14 Oct 2016 11:26:53 +0200 Subject: [PATCH] Remove LaserFan in favor of LaserFan3D. (#62) LaserFan3D will be renamed in the next PR. --- cartographer/mapping/trajectory_node.h | 2 +- .../mapping_2d/global_trajectory_builder.cc | 2 +- cartographer/mapping_2d/laser_fan_inserter.cc | 2 +- cartographer/mapping_2d/laser_fan_inserter.h | 2 +- .../mapping_2d/laser_fan_inserter_test.cc | 10 ++--- .../mapping_2d/local_trajectory_builder.cc | 4 +- cartographer/mapping_2d/map_limits.h | 20 ++++----- cartographer/mapping_2d/map_limits_test.cc | 6 +-- cartographer/mapping_2d/ray_casting.cc | 9 ++-- cartographer/mapping_2d/ray_casting.h | 2 +- .../fast_correlative_scan_matcher_test.cc | 23 +++++------ ...real_time_correlative_scan_matcher_test.cc | 4 +- cartographer/mapping_2d/sparse_pose_graph.cc | 9 ++-- cartographer/mapping_2d/sparse_pose_graph.h | 2 +- .../sparse_pose_graph/constraint_builder.cc | 8 ++-- .../sparse_pose_graph/constraint_builder.h | 7 ++-- .../mapping_2d/sparse_pose_graph_test.cc | 17 ++++---- cartographer/mapping_2d/submaps.cc | 4 +- cartographer/mapping_2d/submaps.h | 2 +- cartographer/mapping_2d/submaps_test.cc | 2 +- cartographer/mapping_3d/sparse_pose_graph.cc | 2 +- cartographer/sensor/CMakeLists.txt | 1 + cartographer/sensor/laser.cc | 14 ------- cartographer/sensor/laser.h | 13 ------ cartographer/sensor/point_cloud.cc | 41 ++----------------- cartographer/sensor/point_cloud.h | 11 ----- cartographer/sensor/point_cloud_test.cc | 13 +++--- 27 files changed, 78 insertions(+), 154 deletions(-) diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index e09ff22..047f3eb 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -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; diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index 5df7a25..a413335 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -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, diff --git a/cartographer/mapping_2d/laser_fan_inserter.cc b/cartographer/mapping_2d/laser_fan_inserter.cc index 053343a..f3d2750 100644 --- a/cartographer/mapping_2d/laser_fan_inserter.cc +++ b/cartographer/mapping_2d/laser_fan_inserter.cc @@ -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(); diff --git a/cartographer/mapping_2d/laser_fan_inserter.h b/cartographer/mapping_2d/laser_fan_inserter.h index daae856..2a22036 100644 --- a/cartographer/mapping_2d/laser_fan_inserter.h +++ b/cartographer/mapping_2d/laser_fan_inserter.h @@ -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& hit_table() const { return hit_table_; } diff --git a/cartographer/mapping_2d/laser_fan_inserter_test.cc b/cartographer/mapping_2d/laser_fan_inserter_test.cc index d9c6beb..413577c 100644 --- a/cartographer/mapping_2d/laser_fan_inserter_test.cc +++ b/cartographer/mapping_2d/laser_fan_inserter_test.cc @@ -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(); diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 6f10d11..7fcc6f2 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -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()))); + submaps_.InsertLaserFan(TransformLaserFan3D( + laser_fan_in_tracking_2d, tracking_2d_to_map.cast())); return common::make_unique(InsertionResult{ time, &submaps_, matching_submap, insertion_submaps, diff --git a/cartographer/mapping_2d/map_limits.h b/cartographer/mapping_2d/map_limits.h index cf2eacd..d7806fd 100644 --- a/cartographer/mapping_2d/map_limits.h +++ b/cartographer/mapping_2d/map_limits.h @@ -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()); - 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()); - 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()); + 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>()); } } } diff --git a/cartographer/mapping_2d/map_limits_test.cc b/cartographer/mapping_2d/map_limits_test.cc index b338aad..046cf27 100644 --- a/cartographer/mapping_2d/map_limits_test.cc +++ b/cartographer/mapping_2d/map_limits_test.cc @@ -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()}; diff --git a/cartographer/mapping_2d/ray_casting.cc b/cartographer/mapping_2d/ray_casting.cc index 16231fe..59268dd 100644 --- a/cartographer/mapping_2d/ray_casting.cc +++ b/cartographer/mapping_2d/ray_casting.cc @@ -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& hit_visitor, const std::function& 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 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); diff --git a/cartographer/mapping_2d/ray_casting.h b/cartographer/mapping_2d/ray_casting.h index 7e12121..acbd6bb 100644 --- a/cartographer/mapping_2d/ray_casting.h +++ b/cartographer/mapping_2d/ray_casting.h @@ -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& hit_visitor, const std::function& miss_visitor); diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc index eb51ffb..11cdfb8 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -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())), {}}, &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); diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc index fb039c2..d956823 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc @@ -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( diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 6522998..11650b7 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -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); } } diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 84a1c16..e549f7a 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -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, diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 0bef976..4a96406 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -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* 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, diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h index ddf74f5..b485b76 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h @@ -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) EXCLUDES(mutex_); diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index 3fd818c..b6f061e 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -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()); + const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud( + point_cloud_, + transform::Embed3D(current_pose_.inverse().cast())); 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())); + submaps_->InsertLaserFan(TransformLaserFan3D( + laser_fan, transform::Embed3D(pose_estimate.cast()))); 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_; std::deque constant_node_data_; common::ThreadPool thread_pool_; diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index 4525975..ae67d3c 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/cartographer/mapping_2d/submaps.cc @@ -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::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>()); } } diff --git a/cartographer/mapping_2d/submaps.h b/cartographer/mapping_2d/submaps.h index d035ea9..1c397af 100644 --- a/cartographer/mapping_2d/submaps.h +++ b/cartographer/mapping_2d/submaps.h @@ -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); diff --git a/cartographer/mapping_2d/submaps_test.cc b/cartographer/mapping_2d/submaps_test.cc index 302cfff..77426f7 100644 --- a/cartographer/mapping_2d/submaps_test.cc +++ b/cartographer/mapping_2d/submaps_test.cc @@ -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) { diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index b36b9df..e5e5a63 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -96,7 +96,7 @@ int SparsePoseGraph::AddScan( CHECK_LT(j, std::numeric_limits::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( diff --git a/cartographer/sensor/CMakeLists.txt b/cartographer/sensor/CMakeLists.txt index 18147a6..eb838d3 100644 --- a/cartographer/sensor/CMakeLists.txt +++ b/cartographer/sensor/CMakeLists.txt @@ -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 diff --git a/cartographer/sensor/laser.cc b/cartographer/sensor/laser.cc index 0348b8a..2373ae5 100644 --- a/cartographer/sensor/laser.cc +++ b/cartographer/sensor/laser.cc @@ -37,14 +37,6 @@ std::vector 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 new_to_old; CompressedPointCloud compressed_returns = diff --git a/cartographer/sensor/laser.h b/cartographer/sensor/laser.h index a67f46b..5bec2dd 100644 --- a/cartographer/sensor/laser.h +++ b/cartographer/sensor/laser.h @@ -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 { diff --git a/cartographer/sensor/point_cloud.cc b/cartographer/sensor/point_cloud.cc index dc3fbd8..345a821 100644 --- a/cartographer/sensor/point_cloud.cc +++ b/cartographer/sensor/point_cloud.cc @@ -22,49 +22,16 @@ namespace cartographer { namespace sensor { -namespace { - -template -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; diff --git a/cartographer/sensor/point_cloud.h b/cartographer/sensor/point_cloud.h index 6879f84..87479e3 100644 --- a/cartographer/sensor/point_cloud.h +++ b/cartographer/sensor/point_cloud.h @@ -29,22 +29,11 @@ namespace cartographer { namespace sensor { typedef std::vector PointCloud; -typedef std::vector 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); diff --git a/cartographer/sensor/point_cloud_test.cc b/cartographer/sensor/point_cloud_test.cc index 0b928a4..394a46b 100644 --- a/cartographer/sensor/point_cloud_test.cc +++ b/cartographer/sensor/point_cloud_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/transform.h" #include @@ -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);