diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h index 42fa9ab..502aa80 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/cartographer/mapping/global_trajectory_builder_interface.h @@ -78,13 +78,13 @@ class GlobalTrajectoryBuilderInterface { virtual kalman_filter::PoseTracker* pose_tracker() const = 0; virtual const PoseEstimate& pose_estimate() const = 0; - virtual void AddHorizontalLaserFan(common::Time, - const sensor::LaserFan3D& laser_fan) = 0; + virtual void AddHorizontalLaserFan(common::Time time, + const sensor::LaserFan& laser_fan) = 0; virtual void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) = 0; virtual void AddLaserFan3D(common::Time time, - const sensor::LaserFan3D& laser_fan) = 0; + const sensor::LaserFan& laser_fan) = 0; virtual void AddOdometerPose( common::Time time, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) = 0; diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index 047f3eb..61220e9 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -35,10 +35,10 @@ struct TrajectoryNode { common::Time time; // LaserFan in 'pose' frame. Only used in the 2D case. - sensor::LaserFan3D laser_fan_2d; + sensor::LaserFan laser_fan_2d; // LaserFan in 'pose' frame. Only used in the 3D case. - sensor::CompressedLaserFan3D laser_fan_3d; + sensor::CompressedLaserFan laser_fan_3d; // Trajectory this node belongs to. // TODO(jmason): The naming here is confusing because 'trajectory' doesn't diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index a413335..f32c575 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -41,7 +41,7 @@ kalman_filter::PoseTracker* GlobalTrajectoryBuilder::pose_tracker() const { } void GlobalTrajectoryBuilder::AddHorizontalLaserFan( - const common::Time time, const sensor::LaserFan3D& laser_fan) { + const common::Time time, const sensor::LaserFan& laser_fan) { std::unique_ptr insertion_result = local_trajectory_builder_.AddHorizontalLaserFan(time, laser_fan); if (insertion_result != nullptr) { diff --git a/cartographer/mapping_2d/global_trajectory_builder.h b/cartographer/mapping_2d/global_trajectory_builder.h index ba5022c..996cb99 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.h +++ b/cartographer/mapping_2d/global_trajectory_builder.h @@ -41,13 +41,13 @@ class GlobalTrajectoryBuilder const override; void AddHorizontalLaserFan(common::Time time, - const sensor::LaserFan3D& laser_fan) override; + const sensor::LaserFan& laser_fan) override; void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; void AddOdometerPose( common::Time time, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) override; - void AddLaserFan3D(common::Time, const sensor::LaserFan3D&) override { + void AddLaserFan3D(common::Time, const sensor::LaserFan&) override { LOG(FATAL) << "Not implemented."; }; diff --git a/cartographer/mapping_2d/laser_fan_inserter.cc b/cartographer/mapping_2d/laser_fan_inserter.cc index f3d2750..053343a 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::LaserFan3D& laser_fan, +void LaserFanInserter::Insert(const sensor::LaserFan& 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 2a22036..daae856 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::LaserFan3D& laser_fan, + void Insert(const sensor::LaserFan& 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 413577c..569e63c 100644 --- a/cartographer/mapping_2d/laser_fan_inserter_test.cc +++ b/cartographer/mapping_2d/laser_fan_inserter_test.cc @@ -44,7 +44,7 @@ class LaserFanInserterTest : public ::testing::Test { } void InsertPointCloud() { - sensor::LaserFan3D laser_fan; + sensor::LaserFan 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); diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 7fcc6f2..77101c2 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -78,13 +78,13 @@ kalman_filter::PoseTracker* LocalTrajectoryBuilder::pose_tracker() const { return pose_tracker_.get(); } -sensor::LaserFan3D LocalTrajectoryBuilder::BuildCroppedLaserFan( +sensor::LaserFan LocalTrajectoryBuilder::BuildCroppedLaserFan( const transform::Rigid3f& tracking_to_tracking_2d, - const sensor::LaserFan3D& laser_fan) const { - const sensor::LaserFan3D cropped_fan = sensor::CropLaserFan( - sensor::TransformLaserFan3D(laser_fan, tracking_to_tracking_2d), + const sensor::LaserFan& laser_fan) const { + const sensor::LaserFan cropped_fan = sensor::CropLaserFan( + sensor::TransformLaserFan(laser_fan, tracking_to_tracking_2d), options_.horizontal_laser_min_z(), options_.horizontal_laser_max_z()); - return sensor::LaserFan3D{ + return sensor::LaserFan{ cropped_fan.origin, sensor::VoxelFiltered(cropped_fan.returns, options_.horizontal_laser_voxel_filter_size()), @@ -95,7 +95,7 @@ sensor::LaserFan3D LocalTrajectoryBuilder::BuildCroppedLaserFan( void LocalTrajectoryBuilder::ScanMatch( common::Time time, const transform::Rigid3d& pose_prediction, const transform::Rigid3d& tracking_to_tracking_2d, - const sensor::LaserFan3D& laser_fan_in_tracking_2d, + const sensor::LaserFan& laser_fan_in_tracking_2d, transform::Rigid3d* pose_observation, kalman_filter::PoseCovariance* covariance_observation) { const ProbabilityGrid& probability_grid = @@ -140,7 +140,7 @@ void LocalTrajectoryBuilder::ScanMatch( std::unique_ptr LocalTrajectoryBuilder::AddHorizontalLaserFan( - const common::Time time, const sensor::LaserFan3D& laser_fan) { + const common::Time time, const sensor::LaserFan& laser_fan) { // Initialize pose tracker now if we do not ever use an IMU. if (!options_.use_imu_data()) { InitializePoseTracker(time); @@ -165,7 +165,7 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan( -transform::GetYaw(pose_prediction), Eigen::Vector3d::UnitZ())) * pose_prediction.rotation()); - const sensor::LaserFan3D laser_fan_in_tracking_2d = + const sensor::LaserFan laser_fan_in_tracking_2d = BuildCroppedLaserFan(tracking_to_tracking_2d.cast(), laser_fan); if (laser_fan_in_tracking_2d.returns.empty()) { @@ -212,8 +212,8 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan( for (int insertion_index : submaps_.insertion_indices()) { insertion_submaps.push_back(submaps_.Get(insertion_index)); } - submaps_.InsertLaserFan(TransformLaserFan3D( - laser_fan_in_tracking_2d, tracking_2d_to_map.cast())); + submaps_.InsertLaserFan(TransformLaserFan(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/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index 11e42bf..a7c6829 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -49,7 +49,7 @@ class LocalTrajectoryBuilder { std::vector insertion_submaps; transform::Rigid3d tracking_to_tracking_2d; transform::Rigid3d tracking_2d_to_map; - sensor::LaserFan3D laser_fan_in_tracking_2d; + sensor::LaserFan laser_fan_in_tracking_2d; transform::Rigid2d pose_estimate_2d; kalman_filter::PoseCovariance covariance_estimate; }; @@ -64,7 +64,7 @@ class LocalTrajectoryBuilder { const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate() const; std::unique_ptr AddHorizontalLaserFan( - common::Time, const sensor::LaserFan3D& laser_fan); + common::Time, const sensor::LaserFan& laser_fan); void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); void AddOdometerPose(common::Time time, const transform::Rigid3d& pose, @@ -76,15 +76,15 @@ class LocalTrajectoryBuilder { private: // Transforms 'laser_scan', crops and voxel filters. - sensor::LaserFan3D BuildCroppedLaserFan( + sensor::LaserFan BuildCroppedLaserFan( const transform::Rigid3f& tracking_to_tracking_2d, - const sensor::LaserFan3D& laser_fan) const; + const sensor::LaserFan& laser_fan) const; // Scan match 'laser_fan_in_tracking_2d' and fill in the // 'pose_observation' and 'covariance_observation' with the result. void ScanMatch(common::Time time, const transform::Rigid3d& pose_prediction, const transform::Rigid3d& tracking_to_tracking_2d, - const sensor::LaserFan3D& laser_fan_in_tracking_2d, + const sensor::LaserFan& laser_fan_in_tracking_2d, transform::Rigid3d* pose_observation, kalman_filter::PoseCovariance* covariance_observation); diff --git a/cartographer/mapping_2d/map_limits.h b/cartographer/mapping_2d/map_limits.h index d7806fd..1b9b2ba 100644 --- a/cartographer/mapping_2d/map_limits.h +++ b/cartographer/mapping_2d/map_limits.h @@ -102,14 +102,14 @@ 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 = sensor::TransformLaserFan3D( + const sensor::LaserFan laser_fan = sensor::TransformLaserFan( Decompress(data.laser_fan_3d), 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>()); } } else { - const sensor::LaserFan3D laser_fan = sensor::TransformLaserFan3D( + const sensor::LaserFan laser_fan = sensor::TransformLaserFan( data.laser_fan_2d, node.pose.cast()); bounding_box.extend(laser_fan.origin.head<2>()); for (const Eigen::Vector3f& hit : laser_fan.returns) { diff --git a/cartographer/mapping_2d/map_limits_test.cc b/cartographer/mapping_2d/map_limits_test.cc index 046cf27..168c4f0 100644 --- a/cartographer/mapping_2d/map_limits_test.cc +++ b/cartographer/mapping_2d/map_limits_test.cc @@ -39,12 +39,12 @@ TEST(MapLimitsTest, ConstructAndGet) { TEST(MapLimitsTest, ComputeMapLimits) { const mapping::TrajectoryNode::ConstantData constant_data{ common::FromUniversal(52), - sensor::LaserFan3D{ + sensor::LaserFan{ 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()}; + Compress(sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}, {}}), nullptr, + transform::Rigid3d::Identity()}; const mapping::TrajectoryNode trajectory_node{&constant_data, transform::Rigid3d::Identity()}; constexpr double kResolution = 0.05; diff --git a/cartographer/mapping_2d/ray_casting.cc b/cartographer/mapping_2d/ray_casting.cc index 59268dd..e78c689 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::LaserFan3D& laser_fan, const MapLimits& limits, +void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits, const std::function& hit_visitor, const std::function& miss_visitor) { const double superscaled_resolution = limits.resolution() / kSubpixelScale; diff --git a/cartographer/mapping_2d/ray_casting.h b/cartographer/mapping_2d/ray_casting.h index acbd6bb..7e12121 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::LaserFan3D& laser_fan, const MapLimits& limits, +void CastRays(const sensor::LaserFan& 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 11cdfb8..e7ca0e9 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,7 +150,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); probability_grid.StartUpdate(); laser_fan_inserter.Insert( - sensor::LaserFan3D{ + sensor::LaserFan{ Eigen::Vector3f(expected_pose.translation().x(), expected_pose.translation().y(), 0.f), sensor::TransformPointCloud( @@ -203,7 +203,7 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); probability_grid.StartUpdate(); laser_fan_inserter.Insert( - sensor::LaserFan3D{ + sensor::LaserFan{ transform::Embed3D(expected_pose * perturbation).translation(), sensor::TransformPointCloud(point_cloud, transform::Embed3D(expected_pose)), 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 d956823..dbab5e5 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,7 +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::LaserFan3D{Eigen::Vector3f::Zero(), point_cloud_, {}}, + sensor::LaserFan{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 11650b7..dad9dd0 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::LaserFan3D& laser_fan_in_pose, const transform::Rigid2d& pose, + const sensor::LaserFan& laser_fan_in_pose, const transform::Rigid2d& pose, const kalman_filter::Pose2DCovariance& covariance, const mapping::Submaps* submaps, const mapping::Submap* const matching_submap, @@ -100,8 +100,8 @@ void SparsePoseGraph::AddScan( constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{ time, laser_fan_in_pose, - Compress(sensor::LaserFan3D{Eigen::Vector3f::Zero(), {}, {}, {}}), - submaps, transform::Rigid3d(tracking_to_pose)}); + Compress(sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}, {}}), submaps, + transform::Rigid3d(tracking_to_pose)}); trajectory_nodes_.push_back(mapping::TrajectoryNode{ &constant_node_data_->back(), optimized_pose, }); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index e549f7a..84a1c16 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::LaserFan3D& laser_fan_in_pose, + const sensor::LaserFan& 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_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index b6f061e..c5fdda2 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -157,10 +157,10 @@ class SparsePoseGraphTest : public ::testing::Test { for (int insertion_index : submaps_->insertion_indices()) { insertion_submaps.push_back(submaps_->Get(insertion_index)); } - const sensor::LaserFan3D laser_fan{ + const sensor::LaserFan laser_fan{ Eigen::Vector3f::Zero(), new_point_cloud, {}}; const transform::Rigid2d pose_estimate = noise * current_pose_; - submaps_->InsertLaserFan(TransformLaserFan3D( + submaps_->InsertLaserFan(TransformLaserFan( laser_fan, transform::Embed3D(pose_estimate.cast()))); sparse_pose_graph_->AddScan(common::FromUniversal(0), transform::Rigid3d::Identity(), laser_fan, diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index ae67d3c..a56df15 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::LaserFan3D& laser_fan) { +void Submaps::InsertLaserFan(const sensor::LaserFan& laser_fan) { CHECK_LT(num_laser_fans_, std::numeric_limits::max()); ++num_laser_fans_; for (const int index : insertion_indices()) { diff --git a/cartographer/mapping_2d/submaps.h b/cartographer/mapping_2d/submaps.h index 1c397af..d035ea9 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::LaserFan3D& laser_fan); + void InsertLaserFan(const sensor::LaserFan& laser_fan); private: void FinishSubmap(int index); diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index 77f058f..a38b175 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -49,8 +49,8 @@ void GlobalTrajectoryBuilder::AddImuData( sparse_pose_graph_->AddImuData(time, linear_acceleration, angular_velocity); } -void GlobalTrajectoryBuilder::AddLaserFan3D( - const common::Time time, const sensor::LaserFan3D& laser_fan) { +void GlobalTrajectoryBuilder::AddLaserFan3D(const common::Time time, + const sensor::LaserFan& laser_fan) { auto insertion_result = local_trajectory_builder_->AddLaserFan3D(time, laser_fan); diff --git a/cartographer/mapping_3d/global_trajectory_builder.h b/cartographer/mapping_3d/global_trajectory_builder.h index 79537c5..b96db59 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.h +++ b/cartographer/mapping_3d/global_trajectory_builder.h @@ -43,13 +43,13 @@ class GlobalTrajectoryBuilder void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; void AddLaserFan3D(common::Time time, - const sensor::LaserFan3D& laser_fan) override; + const sensor::LaserFan& laser_fan) override; void AddOdometerPose( common::Time time, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) override; const PoseEstimate& pose_estimate() const override; - void AddHorizontalLaserFan(common::Time, const sensor::LaserFan3D&) override { + void AddHorizontalLaserFan(common::Time, const sensor::LaserFan&) override { LOG(FATAL) << "Not implemented."; } diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index 7f3d829..81e26ad 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -74,8 +74,8 @@ void KalmanLocalTrajectoryBuilder::AddImuData( } std::unique_ptr -KalmanLocalTrajectoryBuilder::AddLaserFan3D( - const common::Time time, const sensor::LaserFan3D& laser_fan) { +KalmanLocalTrajectoryBuilder::AddLaserFan3D(const common::Time time, + const sensor::LaserFan& laser_fan) { if (!pose_tracker_) { LOG(INFO) << "PoseTracker not yet initialized."; return nullptr; @@ -87,14 +87,13 @@ KalmanLocalTrajectoryBuilder::AddLaserFan3D( time, &pose_prediction, &unused_covariance_prediction); if (num_accumulated_ == 0) { first_pose_prediction_ = pose_prediction.cast(); - accumulated_laser_fan_ = - sensor::LaserFan3D{Eigen::Vector3f::Zero(), {}, {}}; + accumulated_laser_fan_ = sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}}; } const transform::Rigid3f tracking_delta = first_pose_prediction_.inverse() * pose_prediction.cast(); - const sensor::LaserFan3D laser_fan_in_first_tracking = - sensor::TransformLaserFan3D(laser_fan, tracking_delta); + const sensor::LaserFan laser_fan_in_first_tracking = + sensor::TransformLaserFan(laser_fan, tracking_delta); for (const Eigen::Vector3f& laser_return : laser_fan_in_first_tracking.returns) { const Eigen::Vector3f delta = @@ -117,17 +116,17 @@ KalmanLocalTrajectoryBuilder::AddLaserFan3D( if (num_accumulated_ >= options_.scans_per_accumulation()) { num_accumulated_ = 0; - return AddAccumulatedLaserFan3D( - time, sensor::TransformLaserFan3D(accumulated_laser_fan_, - tracking_delta.inverse())); + return AddAccumulatedLaserFan( + time, sensor::TransformLaserFan(accumulated_laser_fan_, + tracking_delta.inverse())); } return nullptr; } std::unique_ptr -KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan3D( - const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking) { - const sensor::LaserFan3D filtered_laser_fan = { +KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan( + const common::Time time, const sensor::LaserFan& laser_fan_in_tracking) { + const sensor::LaserFan filtered_laser_fan = { laser_fan_in_tracking.origin, sensor::VoxelFiltered(laser_fan_in_tracking.returns, options_.laser_voxel_filter_size()), @@ -215,7 +214,7 @@ void KalmanLocalTrajectoryBuilder::AddTrajectoryNodeIndex( std::unique_ptr KalmanLocalTrajectoryBuilder::InsertIntoSubmap( - const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking, + const common::Time time, const sensor::LaserFan& laser_fan_in_tracking, const transform::Rigid3d& pose_observation, const kalman_filter::PoseCovariance& covariance_estimate) { if (motion_filter_.IsSimilar(time, pose_observation)) { @@ -227,7 +226,7 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap( for (int insertion_index : submaps_->insertion_indices()) { insertion_submaps.push_back(submaps_->Get(insertion_index)); } - submaps_->InsertLaserFan(sensor::TransformLaserFan3D( + submaps_->InsertLaserFan(sensor::TransformLaserFan( laser_fan_in_tracking, pose_observation.cast())); return std::unique_ptr(new InsertionResult{ time, laser_fan_in_tracking, pose_observation, covariance_estimate, diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.h b/cartographer/mapping_3d/kalman_local_trajectory_builder.h index cbc6c76..87ba388 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.h +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.h @@ -49,7 +49,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; std::unique_ptr AddLaserFan3D( - common::Time time, const sensor::LaserFan3D& laser_fan) override; + common::Time time, const sensor::LaserFan& laser_fan) override; void AddOdometerPose( common::Time time, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) override; @@ -60,11 +60,11 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { kalman_filter::PoseTracker* pose_tracker() const override; private: - std::unique_ptr AddAccumulatedLaserFan3D( - common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking); + std::unique_ptr AddAccumulatedLaserFan( + common::Time time, const sensor::LaserFan& laser_fan_in_tracking); std::unique_ptr InsertIntoSubmap( - const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking, + const common::Time time, const sensor::LaserFan& laser_fan_in_tracking, const transform::Rigid3d& pose_observation, const kalman_filter::PoseCovariance& covariance_estimate); @@ -85,7 +85,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { int num_accumulated_; transform::Rigid3f first_pose_prediction_; - sensor::LaserFan3D accumulated_laser_fan_; + sensor::LaserFan accumulated_laser_fan_; }; } // namespace mapping_3d diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc index 505b868..ffa99ce 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc @@ -186,7 +186,7 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { return first * (to - from) + from; } - sensor::LaserFan3D GenerateLaserFan(const transform::Rigid3d& pose) { + sensor::LaserFan GenerateLaserFan(const transform::Rigid3d& pose) { // 360 degree rays at 16 angles. sensor::PointCloud directions_in_laser_frame; for (int r = -8; r != 8; ++r) { diff --git a/cartographer/mapping_3d/laser_fan_inserter.cc b/cartographer/mapping_3d/laser_fan_inserter.cc index 979b358..58a9d92 100644 --- a/cartographer/mapping_3d/laser_fan_inserter.cc +++ b/cartographer/mapping_3d/laser_fan_inserter.cc @@ -76,7 +76,7 @@ LaserFanInserter::LaserFanInserter( miss_table_(mapping::ComputeLookupTableToApplyOdds( mapping::Odds(options_.miss_probability()))) {} -void LaserFanInserter::Insert(const sensor::LaserFan3D& laser_fan, +void LaserFanInserter::Insert(const sensor::LaserFan& laser_fan, HybridGrid* hybrid_grid) const { CHECK_NOTNULL(hybrid_grid)->StartUpdate(); diff --git a/cartographer/mapping_3d/laser_fan_inserter.h b/cartographer/mapping_3d/laser_fan_inserter.h index 05736e0..8b95bd5 100644 --- a/cartographer/mapping_3d/laser_fan_inserter.h +++ b/cartographer/mapping_3d/laser_fan_inserter.h @@ -36,8 +36,7 @@ class LaserFanInserter { LaserFanInserter& operator=(const LaserFanInserter&) = delete; // Inserts 'laser_fan' into 'hybrid_grid'. - void Insert(const sensor::LaserFan3D& laser_fan, - HybridGrid* hybrid_grid) const; + void Insert(const sensor::LaserFan& laser_fan, HybridGrid* hybrid_grid) const; private: const proto::LaserFanInserterOptions options_; diff --git a/cartographer/mapping_3d/laser_fan_inserter_test.cc b/cartographer/mapping_3d/laser_fan_inserter_test.cc index a2cbfcb..d333267 100644 --- a/cartographer/mapping_3d/laser_fan_inserter_test.cc +++ b/cartographer/mapping_3d/laser_fan_inserter_test.cc @@ -46,7 +46,7 @@ class LaserFanInserterTest : public ::testing::Test { {-1.5f, 0.5f, 4.5f}, {-0.5f, 1.5f, 4.5f}, {0.5f, 2.5f, 4.5f}}; - laser_fan_inserter_->Insert(sensor::LaserFan3D{origin, laser_returns, {}}, + laser_fan_inserter_->Insert(sensor::LaserFan{origin, laser_returns, {}}, &hybrid_grid_); } diff --git a/cartographer/mapping_3d/local_trajectory_builder_interface.h b/cartographer/mapping_3d/local_trajectory_builder_interface.h index ec4d6b7..e8918ff 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_interface.h +++ b/cartographer/mapping_3d/local_trajectory_builder_interface.h @@ -35,7 +35,7 @@ class LocalTrajectoryBuilderInterface { struct InsertionResult { common::Time time; - sensor::LaserFan3D laser_fan_in_tracking; + sensor::LaserFan laser_fan_in_tracking; transform::Rigid3d pose_observation; kalman_filter::PoseCovariance covariance_estimate; const Submaps* submaps; @@ -54,7 +54,7 @@ class LocalTrajectoryBuilderInterface { const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) = 0; virtual std::unique_ptr AddLaserFan3D( - common::Time time, const sensor::LaserFan3D& laser_fan) = 0; + common::Time time, const sensor::LaserFan& laser_fan) = 0; virtual void AddOdometerPose( common::Time time, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) = 0; diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc index cc4c43f..9bd78eb 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc @@ -134,7 +134,7 @@ void OptimizingLocalTrajectoryBuilder::AddOdometerPose( std::unique_ptr OptimizingLocalTrajectoryBuilder::AddLaserFan3D( - const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking) { + const common::Time time, const sensor::LaserFan& laser_fan_in_tracking) { CHECK_GT(laser_fan_in_tracking.returns.size(), 0); // TODO(hrapp): Handle misses. @@ -335,7 +335,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { num_accumulated_ = 0; const transform::Rigid3d optimized_pose = batches_.back().state.ToRigid(); - sensor::LaserFan3D accumulated_laser_fan_in_tracking = { + sensor::LaserFan accumulated_laser_fan_in_tracking = { Eigen::Vector3f::Zero(), {}, {}}; for (const auto& batch : batches_) { @@ -346,15 +346,15 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { } } - return AddAccumulatedLaserFan3D(time, optimized_pose, - accumulated_laser_fan_in_tracking); + return AddAccumulatedLaserFan(time, optimized_pose, + accumulated_laser_fan_in_tracking); } std::unique_ptr -OptimizingLocalTrajectoryBuilder::AddAccumulatedLaserFan3D( +OptimizingLocalTrajectoryBuilder::AddAccumulatedLaserFan( const common::Time time, const transform::Rigid3d& optimized_pose, - const sensor::LaserFan3D& laser_fan_in_tracking) { - const sensor::LaserFan3D filtered_laser_fan = { + const sensor::LaserFan& laser_fan_in_tracking) { + const sensor::LaserFan filtered_laser_fan = { laser_fan_in_tracking.origin, sensor::VoxelFiltered(laser_fan_in_tracking.returns, options_.laser_voxel_filter_size()), @@ -393,7 +393,7 @@ void OptimizingLocalTrajectoryBuilder::AddTrajectoryNodeIndex( std::unique_ptr OptimizingLocalTrajectoryBuilder::InsertIntoSubmap( - const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking, + const common::Time time, const sensor::LaserFan& laser_fan_in_tracking, const transform::Rigid3d& pose_observation, const kalman_filter::PoseCovariance& covariance_estimate) { if (motion_filter_.IsSimilar(time, pose_observation)) { @@ -405,7 +405,7 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap( for (int insertion_index : submaps_->insertion_indices()) { insertion_submaps.push_back(submaps_->Get(insertion_index)); } - submaps_->InsertLaserFan(sensor::TransformLaserFan3D( + submaps_->InsertLaserFan(sensor::TransformLaserFan( laser_fan_in_tracking, pose_observation.cast())); return std::unique_ptr(new InsertionResult{ time, laser_fan_in_tracking, pose_observation, covariance_estimate, diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h index 28411d0..1d5d9bd 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h @@ -54,7 +54,7 @@ class OptimizingLocalTrajectoryBuilder const Eigen::Vector3d& angular_velocity) override; std::unique_ptr AddLaserFan3D( common::Time time, - const sensor::LaserFan3D& laser_fan_in_tracking) override; + const sensor::LaserFan& laser_fan_in_tracking) override; void AddOdometerPose( const common::Time time, const transform::Rigid3d& pose, @@ -106,12 +106,12 @@ class OptimizingLocalTrajectoryBuilder void RemoveObsoleteSensorData(); - std::unique_ptr AddAccumulatedLaserFan3D( + std::unique_ptr AddAccumulatedLaserFan( common::Time time, const transform::Rigid3d& pose_observation, - const sensor::LaserFan3D& laser_fan_in_tracking); + const sensor::LaserFan& laser_fan_in_tracking); std::unique_ptr InsertIntoSubmap( - const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking, + const common::Time time, const sensor::LaserFan& laser_fan_in_tracking, const transform::Rigid3d& pose_observation, const kalman_filter::PoseCovariance& covariance_estimate); diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc index e10f434..f41487d 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -89,12 +89,11 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { HybridGrid hybrid_grid(0.05f /* resolution */, Eigen::Vector3f(0.5f, 1.5f, 2.5f) /* origin */); hybrid_grid.StartUpdate(); - laser_fan_inserter.Insert( - sensor::LaserFan3D{ - expected_pose.translation(), - sensor::TransformPointCloud(point_cloud, expected_pose), - {}}, - &hybrid_grid); + laser_fan_inserter.Insert(sensor::LaserFan{expected_pose.translation(), + sensor::TransformPointCloud( + point_cloud, expected_pose), + {}}, + &hybrid_grid); FastCorrelativeScanMatcher fast_correlative_scan_matcher(hybrid_grid, {}, options); diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index e5e5a63..995f7c9 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -83,7 +83,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded( } int SparsePoseGraph::AddScan( - common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking, + common::Time time, const sensor::LaserFan& laser_fan_in_tracking, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance, const Submaps* submaps, const Submap* const matching_submap, @@ -96,7 +96,7 @@ int SparsePoseGraph::AddScan( CHECK_LT(j, std::numeric_limits::max()); constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{ - time, sensor::LaserFan3D{Eigen::Vector3f::Zero(), {}, {}}, + time, sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}}, sensor::Compress(laser_fan_in_tracking), submaps, transform::Rigid3d::Identity()}); trajectory_nodes_.push_back( diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 3e2b3bb..def9e0c 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -70,8 +70,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // against the 'matching_submap' and the scan was inserted into the // 'insertion_submaps'. The index into the vector of trajectory nodes as // used with GetTrajectoryNodes() is returned. - int AddScan(common::Time time, - const sensor::LaserFan3D& laser_fan_in_tracking, + int AddScan(common::Time time, const sensor::LaserFan& laser_fan_in_tracking, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& pose_covariance, const Submaps* submaps, const Submap* matching_submap, diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc index d41d3de..0e532c9 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/cartographer/mapping_3d/submaps.cc @@ -39,16 +39,17 @@ struct LaserSegment { // We compute a slice around the xy-plane. 'transform' is applied to the laser // rays in global map frame to allow choosing an arbitrary slice. -void GenerateSegmentForSlice(const sensor::LaserFan3D& laser_fan_3d, +void GenerateSegmentForSlice(const sensor::LaserFan& laser_fan, const transform::Rigid3f& pose, const transform::Rigid3f& transform, std::vector* segments) { - const sensor::LaserFan3D laser_fan = - sensor::TransformLaserFan3D(laser_fan_3d, transform * pose); - segments->reserve(laser_fan.returns.size()); - for (const Eigen::Vector3f& hit : laser_fan.returns) { - const Eigen::Vector2f laser_origin_xy = laser_fan.origin.head<2>(); - const float laser_origin_z = laser_fan.origin.z(); + const sensor::LaserFan transformed_laser_fan = + sensor::TransformLaserFan(laser_fan, transform * pose); + segments->reserve(transformed_laser_fan.returns.size()); + for (const Eigen::Vector3f& hit : transformed_laser_fan.returns) { + const Eigen::Vector2f laser_origin_xy = + transformed_laser_fan.origin.head<2>(); + const float laser_origin_z = transformed_laser_fan.origin.z(); const float delta_z = hit.z() - laser_origin_z; const Eigen::Vector2f delta_xy = hit.head<2>() - laser_origin_xy; if (laser_origin_z < -kSliceHalfHeight) { @@ -181,12 +182,12 @@ void InsertSegmentsIntoProbabilityGrid( } // namespace void InsertIntoProbabilityGrid( - const sensor::LaserFan3D& laser_fan_3d, const transform::Rigid3f& pose, + const sensor::LaserFan& laser_fan, const transform::Rigid3f& pose, const float slice_z, const mapping_2d::LaserFanInserter& laser_fan_inserter, mapping_2d::ProbabilityGrid* result) { std::vector segments; GenerateSegmentForSlice( - laser_fan_3d, pose, + laser_fan, pose, transform::Rigid3f::Translation(-slice_z * Eigen::Vector3f::UnitZ()), &segments); InsertSegmentsIntoProbabilityGrid(segments, laser_fan_inserter.hit_table(), @@ -265,7 +266,7 @@ void Submaps::SubmapToProto( global_submap_pose.translation().z()))); } -void Submaps::InsertLaserFan(const sensor::LaserFan3D& laser_fan) { +void Submaps::InsertLaserFan(const sensor::LaserFan& laser_fan) { CHECK_LT(num_laser_fans_, std::numeric_limits::max()); ++num_laser_fans_; for (const int index : insertion_indices()) { diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h index d71553d..961946f 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submaps.h @@ -36,7 +36,7 @@ namespace cartographer { namespace mapping_3d { void InsertIntoProbabilityGrid( - const sensor::LaserFan3D& laser_fan_3d, const transform::Rigid3f& pose, + const sensor::LaserFan& laser_fan, const transform::Rigid3f& pose, const float slice_z, const mapping_2d::LaserFanInserter& laser_fan_inserter, mapping_2d::ProbabilityGrid* result); @@ -69,7 +69,7 @@ class Submaps : public mapping::Submaps { mapping::proto::SubmapQuery::Response* response) override; // Inserts 'laser_fan' into the Submap collection. - void InsertLaserFan(const sensor::LaserFan3D& laser_fan); + void InsertLaserFan(const sensor::LaserFan& laser_fan); // Returns the 'high_resolution' HybridGrid to be used for matching. const HybridGrid& high_resolution_matching_grid() const; diff --git a/cartographer/sensor/data.h b/cartographer/sensor/data.h index 44084bf..1b693fb 100644 --- a/cartographer/sensor/data.h +++ b/cartographer/sensor/data.h @@ -30,7 +30,7 @@ namespace sensor { // filled in. It is only used for time ordering sensor data before passing it // on. struct Data { - enum class Type { kImu, kLaserFan3D, kOdometry }; + enum class Type { kImu, kLaserFan, kOdometry }; struct Odometry { transform::Rigid3d pose; @@ -46,10 +46,8 @@ struct Data { : type(Type::kImu), frame_id(frame_id), imu(imu) {} Data(const string& frame_id, - const ::cartographer::sensor::LaserFan3D& laser_fan_3d) - : type(Type::kLaserFan3D), - frame_id(frame_id), - laser_fan_3d(laser_fan_3d) {} + const ::cartographer::sensor::LaserFan& laser_fan) + : type(Type::kLaserFan), frame_id(frame_id), laser_fan(laser_fan) {} Data(const string& frame_id, const Odometry& odometry) : type(Type::kOdometry), frame_id(frame_id), odometry(odometry) {} @@ -57,7 +55,7 @@ struct Data { Type type; string frame_id; Imu imu; - sensor::LaserFan3D laser_fan_3d; + sensor::LaserFan laser_fan; Odometry odometry; }; diff --git a/cartographer/sensor/laser.cc b/cartographer/sensor/laser.cc index 2373ae5..46a81e6 100644 --- a/cartographer/sensor/laser.cc +++ b/cartographer/sensor/laser.cc @@ -37,13 +37,13 @@ std::vector ReorderReflectivities( } // namespace -LaserFan3D ToLaserFan3D(const proto::LaserScan& proto, const float min_range, - const float max_range, - const float missing_echo_ray_length) { +LaserFan ToLaserFan(const proto::LaserScan& proto, const float min_range, + const float max_range, + const float missing_echo_ray_length) { CHECK_GE(min_range, 0.f); CHECK_GT(proto.angle_increment(), 0.f); CHECK_GT(proto.angle_max(), proto.angle_min()); - LaserFan3D laser_fan = {Eigen::Vector3f::Zero(), {}, {}}; + LaserFan laser_fan = {Eigen::Vector3f::Zero(), {}, {}}; float angle = proto.angle_min(); for (const auto& range : proto.range()) { if (range.value_size() > 0) { @@ -64,8 +64,8 @@ LaserFan3D ToLaserFan3D(const proto::LaserScan& proto, const float min_range, return laser_fan; } -proto::LaserFan3D ToProto(const LaserFan3D& laser_fan) { - proto::LaserFan3D proto; +proto::LaserFan ToProto(const LaserFan& laser_fan) { + proto::LaserFan proto; *proto.mutable_origin() = transform::ToProto(laser_fan.origin); *proto.mutable_point_cloud() = ToProto(laser_fan.returns); *proto.mutable_missing_echo_point_cloud() = ToProto(laser_fan.misses); @@ -74,19 +74,19 @@ proto::LaserFan3D ToProto(const LaserFan3D& laser_fan) { return proto; } -LaserFan3D FromProto(const proto::LaserFan3D& proto) { - auto laser_fan_3d = LaserFan3D{ +LaserFan FromProto(const proto::LaserFan& proto) { + auto laser_fan = LaserFan{ transform::ToEigen(proto.origin()), ToPointCloud(proto.point_cloud()), ToPointCloud(proto.missing_echo_point_cloud()), }; std::copy(proto.reflectivity().begin(), proto.reflectivity().end(), - std::back_inserter(laser_fan_3d.reflectivities)); - return laser_fan_3d; + std::back_inserter(laser_fan.reflectivities)); + return laser_fan; } -LaserFan3D TransformLaserFan3D(const LaserFan3D& laser_fan, - const transform::Rigid3f& transform) { - return LaserFan3D{ +LaserFan TransformLaserFan(const LaserFan& laser_fan, + const transform::Rigid3f& transform) { + return LaserFan{ transform * laser_fan.origin, TransformPointCloud(laser_fan.returns, transform), TransformPointCloud(laser_fan.misses, transform), @@ -94,9 +94,9 @@ LaserFan3D TransformLaserFan3D(const LaserFan3D& laser_fan, }; } -LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan, - const float max_range) { - LaserFan3D result{laser_fan.origin, {}, {}, {}}; +LaserFan FilterLaserFanByMaxRange(const LaserFan& laser_fan, + const float max_range) { + LaserFan result{laser_fan.origin, {}, {}, {}}; for (const Eigen::Vector3f& return_ : laser_fan.returns) { if ((return_ - laser_fan.origin).norm() <= max_range) { result.returns.push_back(return_); @@ -105,28 +105,28 @@ LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan, return result; } -LaserFan3D CropLaserFan(const LaserFan3D& laser_fan, const float min_z, - const float max_z) { - return LaserFan3D{laser_fan.origin, Crop(laser_fan.returns, min_z, max_z), - Crop(laser_fan.misses, min_z, max_z)}; +LaserFan CropLaserFan(const LaserFan& laser_fan, const float min_z, + const float max_z) { + return LaserFan{laser_fan.origin, Crop(laser_fan.returns, min_z, max_z), + Crop(laser_fan.misses, min_z, max_z)}; } -CompressedLaserFan3D Compress(const LaserFan3D& laser_fan) { +CompressedLaserFan Compress(const LaserFan& laser_fan) { std::vector new_to_old; CompressedPointCloud compressed_returns = CompressedPointCloud::CompressAndReturnOrder(laser_fan.returns, &new_to_old); - return CompressedLaserFan3D{ + return CompressedLaserFan{ laser_fan.origin, std::move(compressed_returns), CompressedPointCloud(laser_fan.misses), ReorderReflectivities(laser_fan.reflectivities, new_to_old)}; } -LaserFan3D Decompress(const CompressedLaserFan3D& compressed_laser_fan) { - return LaserFan3D{compressed_laser_fan.origin, - compressed_laser_fan.returns.Decompress(), - compressed_laser_fan.misses.Decompress(), - compressed_laser_fan.reflectivities}; +LaserFan Decompress(const CompressedLaserFan& compressed_laser_fan) { + return LaserFan{compressed_laser_fan.origin, + compressed_laser_fan.returns.Decompress(), + compressed_laser_fan.misses.Decompress(), + compressed_laser_fan.reflectivities}; } } // namespace sensor diff --git a/cartographer/sensor/laser.h b/cartographer/sensor/laser.h index 5bec2dd..0d511f3 100644 --- a/cartographer/sensor/laser.h +++ b/cartographer/sensor/laser.h @@ -29,7 +29,7 @@ namespace sensor { // 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 // distance. It is assumed that between the 'origin' and 'misses' is free space. -struct LaserFan3D { +struct LaserFan { Eigen::Vector3f origin; PointCloud returns; PointCloud misses; @@ -42,30 +42,29 @@ struct LaserFan3D { // the range ['min_range', 'max_range']. Beams beyond 'max_range' are inserted // into the 'misses' point cloud with length 'missing_echo_ray_length'. The // points in both clouds are stored in scan order. -LaserFan3D ToLaserFan3D(const proto::LaserScan& proto, float min_range, - float max_range, float missing_echo_ray_length); +LaserFan ToLaserFan(const proto::LaserScan& proto, float min_range, + float max_range, float missing_echo_ray_length); -// Converts 3D 'laser_fan' to a proto::LaserFan3D. -proto::LaserFan3D ToProto(const LaserFan3D& laser_fan); +// Converts 3D 'laser_fan' to a proto::LaserFan. +proto::LaserFan ToProto(const LaserFan& laser_fan); -// Converts 'proto' to a LaserFan3D. -LaserFan3D FromProto(const proto::LaserFan3D& proto); +// Converts 'proto' to a LaserFan. +LaserFan FromProto(const proto::LaserFan& proto); -LaserFan3D TransformLaserFan3D(const LaserFan3D& laser_fan, - const transform::Rigid3f& transform); +LaserFan TransformLaserFan(const LaserFan& laser_fan, + const transform::Rigid3f& transform); // Filter a 'laser_fan', retaining only the returns that have no more than // 'max_range' distance from the laser origin. Removes misses and reflectivity // information. -LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan, - float max_range); +LaserFan FilterLaserFanByMaxRange(const LaserFan& laser_fan, float max_range); // 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); +LaserFan CropLaserFan(const LaserFan& laser_fan, float min_z, float max_z); -// Like LaserFan3D but with compressed point clouds. The point order changes -// when converting from LaserFan3D. -struct CompressedLaserFan3D { +// Like LaserFan but with compressed point clouds. The point order changes +// when converting from LaserFan. +struct CompressedLaserFan { Eigen::Vector3f origin; CompressedPointCloud returns; CompressedPointCloud misses; @@ -74,9 +73,9 @@ struct CompressedLaserFan3D { std::vector reflectivities; }; -CompressedLaserFan3D Compress(const LaserFan3D& laser_fan); +CompressedLaserFan Compress(const LaserFan& laser_fan); -LaserFan3D Decompress(const CompressedLaserFan3D& compressed_laser_fan); +LaserFan Decompress(const CompressedLaserFan& compressed_laser_fan); } // namespace sensor } // namespace cartographer diff --git a/cartographer/sensor/laser_test.cc b/cartographer/sensor/laser_test.cc index d031a05..6331e45 100644 --- a/cartographer/sensor/laser_test.cc +++ b/cartographer/sensor/laser_test.cc @@ -28,7 +28,7 @@ namespace { using ::testing::Contains; using ::testing::PrintToString; -TEST(ProjectorTest, ToLaserFan3D) { +TEST(ProjectorTest, ToLaserFan) { proto::LaserScan laser_scan; for (int i = 0; i < 8; ++i) { laser_scan.add_range()->add_value(1.f); @@ -37,7 +37,7 @@ TEST(ProjectorTest, ToLaserFan3D) { laser_scan.set_angle_max(8.f * static_cast(M_PI_4)); laser_scan.set_angle_increment(static_cast(M_PI_4)); - const LaserFan3D fan = ToLaserFan3D(laser_scan, 0.f, 10.f, 1.f); + const LaserFan fan = ToLaserFan(laser_scan, 0.f, 10.f, 1.f); EXPECT_TRUE(fan.returns[0].isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), 1e-6)); EXPECT_TRUE(fan.returns[1].isApprox( Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6)); @@ -53,7 +53,7 @@ TEST(ProjectorTest, ToLaserFan3D) { Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), 1e-6)); } -TEST(ProjectorTest, ToLaserFan3DWithInfinityAndNaN) { +TEST(ProjectorTest, ToLaserFanWithInfinityAndNaN) { proto::LaserScan laser_scan; laser_scan.add_range()->add_value(1.f); laser_scan.add_range()->add_value(std::numeric_limits::infinity()); @@ -64,7 +64,7 @@ TEST(ProjectorTest, ToLaserFan3DWithInfinityAndNaN) { laser_scan.set_angle_max(3.f * static_cast(M_PI_4)); laser_scan.set_angle_increment(static_cast(M_PI_4)); - const LaserFan3D fan = ToLaserFan3D(laser_scan, 2.f, 10.f, 1.f); + const LaserFan fan = ToLaserFan(laser_scan, 2.f, 10.f, 1.f); ASSERT_EQ(2, fan.returns.size()); EXPECT_TRUE(fan.returns[0].isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), 1e-6)); EXPECT_TRUE(fan.returns[1].isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), 1e-6)); @@ -82,12 +82,12 @@ MATCHER_P(PairApproximatelyEquals, expected, } TEST(LaserTest, Compression) { - LaserFan3D fan = {Eigen::Vector3f(1, 1, 1), - {Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6), - Eigen::Vector3f(0, 1, 2)}, - {Eigen::Vector3f(7, 8, 9)}, - {1, 2, 3}}; - LaserFan3D actual = Decompress(Compress(fan)); + LaserFan fan = {Eigen::Vector3f(1, 1, 1), + {Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6), + Eigen::Vector3f(0, 1, 2)}, + {Eigen::Vector3f(7, 8, 9)}, + {1, 2, 3}}; + LaserFan actual = Decompress(Compress(fan)); EXPECT_TRUE(actual.origin.isApprox(Eigen::Vector3f(1, 1, 1), 1e-6)); EXPECT_EQ(3, actual.returns.size()); EXPECT_EQ(1, actual.misses.size()); diff --git a/cartographer/sensor/proto/sensor.proto b/cartographer/sensor/proto/sensor.proto index 614fb7b..fcb2079 100644 --- a/cartographer/sensor/proto/sensor.proto +++ b/cartographer/sensor/proto/sensor.proto @@ -69,8 +69,8 @@ message LaserScan { repeated Values intensity = 10; } -// Proto representation of ::cartographer::sensor::LaserFan3D -message LaserFan3D { +// Proto representation of ::cartographer::sensor::LaserFan +message LaserFan { optional transform.proto.Vector3f origin = 1; optional PointCloud point_cloud = 2; optional PointCloud missing_echo_point_cloud = 3;