diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h index 12b97e2..8414d95 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/cartographer/mapping/global_trajectory_builder_interface.h @@ -24,8 +24,8 @@ #include "cartographer/common/time.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_builder.h" -#include "cartographer/sensor/laser.h" #include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index a42ad8e..1743a44 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -27,7 +27,7 @@ #include "cartographer/mapping_2d/global_trajectory_builder.h" #include "cartographer/mapping_3d/global_trajectory_builder.h" #include "cartographer/mapping_3d/local_trajectory_builder_options.h" -#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/range_data.h" #include "cartographer/sensor/voxel_filter.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" @@ -141,7 +141,8 @@ string MapBuilder::SubmapToProto(const int trajectory_id, " submaps in this trajectory."; } - response->set_submap_version(submaps->Get(submap_index)->end_laser_fan_index); + response->set_submap_version( + submaps->Get(submap_index)->end_range_data_index); const std::vector submap_transforms = sparse_pose_graph_->GetSubmapTransforms(*submaps); CHECK_EQ(submap_transforms.size(), submaps->size()); diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h index faa6bfc..1857fb2 100644 --- a/cartographer/mapping/submaps.h +++ b/cartographer/mapping/submaps.h @@ -64,14 +64,14 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) { } // An individual submap, which has an initial position 'origin', keeps track of -// which laser fans where inserted into it, and sets the +// which range data were inserted into it, and sets the // 'finished_probability_grid' to be used for loop closing once the map no // longer changes. struct Submap { - Submap(const Eigen::Vector3f& origin, int begin_laser_fan_index) + Submap(const Eigen::Vector3f& origin, int begin_range_data_index) : origin(origin), - begin_laser_fan_index(begin_laser_fan_index), - end_laser_fan_index(begin_laser_fan_index) {} + begin_range_data_index(begin_range_data_index), + end_range_data_index(begin_range_data_index) {} transform::Rigid3d local_pose() const { return transform::Rigid3d::Translation(origin.cast()); @@ -80,14 +80,14 @@ struct Submap { // Origin of this submap. Eigen::Vector3f origin; - // This Submap contains LaserFans with indices in the range - // ['begin_laser_fan_index', 'end_laser_fan_index'). - int begin_laser_fan_index; - int end_laser_fan_index; + // This Submap contains RangeData with indices in the range + // ['begin_range_data_index', 'end_range_data_index'). + int begin_range_data_index; + int end_range_data_index; // The 'finished_probability_grid' when this submap is finished and will not // change anymore. Otherwise, this is nullptr and the next call to - // InsertLaserFan() will change the submap. + // InsertRangeData() will change the submap. const mapping_2d::ProbabilityGrid* finished_probability_grid = nullptr; }; diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h index 2a227b4..8c870b2 100644 --- a/cartographer/mapping/trajectory_builder.h +++ b/cartographer/mapping/trajectory_builder.h @@ -26,8 +26,8 @@ #include "cartographer/common/time.h" #include "cartographer/mapping/submaps.h" #include "cartographer/sensor/data.h" -#include "cartographer/sensor/laser.h" #include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/trajectory_connectivity_test.cc b/cartographer/mapping/trajectory_connectivity_test.cc index 7b2c780..b640e78 100644 --- a/cartographer/mapping/trajectory_connectivity_test.cc +++ b/cartographer/mapping/trajectory_connectivity_test.cc @@ -36,9 +36,9 @@ class TrajectoryConnectivityTest : public ::testing::Test { return { resolution = 0.05, half_length = 10., - num_laser_fans = 10, + num_range_data = 10, output_debug_images = false, - laser_fan_inserter = { + range_data_inserter = { insert_free_space = true, hit_probability = 0.53, miss_probability = 0.495, diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index 4a6b9e8..a5d8a1a 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -23,7 +23,7 @@ #include "Eigen/Core" #include "cartographer/common/time.h" #include "cartographer/mapping/proto/trajectory.pb.h" -#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { @@ -35,8 +35,8 @@ struct TrajectoryNode { struct ConstantData { common::Time time; - // LaserFan in 'pose' frame. Only used in the 2D case. - sensor::LaserFan laser_fan_2d; + // Range data in 'pose' frame. Only used in the 2D case. + sensor::RangeData range_data_2d; // Range data in 'pose' frame. Only used in the 3D case. sensor::CompressedRangeData range_data_3d; diff --git a/cartographer/mapping_2d/CMakeLists.txt b/cartographer/mapping_2d/CMakeLists.txt index f1254f3..76f6cc0 100644 --- a/cartographer/mapping_2d/CMakeLists.txt +++ b/cartographer/mapping_2d/CMakeLists.txt @@ -14,9 +14,9 @@ add_subdirectory("scan_matching") -google_test(mapping_2d_laser_fan_inserter_test +google_test(mapping_2d_range_data_inserter_test SRCS - laser_fan_inserter_test.cc + range_data_inserter_test.cc ) google_test(mapping_2d_map_limits_test diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index fc18b0a..c06a257 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -36,12 +36,12 @@ void GlobalTrajectoryBuilder::AddRangefinderData( const common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) { std::unique_ptr insertion_result = - local_trajectory_builder_.AddHorizontalLaserFan( - time, sensor::LaserFan{origin, ranges, {}, {}}); + local_trajectory_builder_.AddHorizontalRangeData( + time, sensor::RangeData{origin, ranges, {}, {}}); if (insertion_result != nullptr) { sparse_pose_graph_->AddScan( insertion_result->time, insertion_result->tracking_to_tracking_2d, - insertion_result->laser_fan_in_tracking_2d, + insertion_result->range_data_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/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index f282239..ede972d 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -19,7 +19,7 @@ #include #include "cartographer/common/make_unique.h" -#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/range_data.h" namespace cartographer { namespace mapping_2d { @@ -78,28 +78,28 @@ LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {} const Submaps* LocalTrajectoryBuilder::submaps() const { return &submaps_; } -sensor::LaserFan LocalTrajectoryBuilder::TransformAndFilterLaserFan( +sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData( const transform::Rigid3f& tracking_to_tracking_2d, - const sensor::LaserFan& laser_fan) const { + const sensor::RangeData& range_data) const { // Drop any returns below the minimum range and convert returns beyond the // maximum range into misses. - sensor::LaserFan returns_and_misses{laser_fan.origin, {}, {}, {}}; - for (const Eigen::Vector3f& return_ : laser_fan.returns) { - const float range = (return_ - laser_fan.origin).norm(); + sensor::RangeData returns_and_misses{range_data.origin, {}, {}, {}}; + for (const Eigen::Vector3f& hit : range_data.returns) { + const float range = (hit - range_data.origin).norm(); if (range >= options_.laser_min_range()) { if (range <= options_.laser_max_range()) { - returns_and_misses.returns.push_back(return_); + returns_and_misses.returns.push_back(hit); } else { returns_and_misses.misses.push_back( - laser_fan.origin + options_.laser_missing_echo_ray_length() * - (return_ - laser_fan.origin).normalized()); + range_data.origin + options_.laser_missing_echo_ray_length() * + (hit - range_data.origin).normalized()); } } } - const sensor::LaserFan cropped = sensor::CropLaserFan( - sensor::TransformLaserFan(returns_and_misses, tracking_to_tracking_2d), + const sensor::RangeData cropped = sensor::CropRangeData( + sensor::TransformRangeData(returns_and_misses, tracking_to_tracking_2d), options_.laser_min_z(), options_.laser_max_z()); - return sensor::LaserFan{ + return sensor::RangeData{ cropped.origin, sensor::VoxelFiltered(cropped.returns, options_.laser_voxel_filter_size()), @@ -110,7 +110,7 @@ sensor::LaserFan LocalTrajectoryBuilder::TransformAndFilterLaserFan( void LocalTrajectoryBuilder::ScanMatch( common::Time time, const transform::Rigid3d& pose_prediction, const transform::Rigid3d& tracking_to_tracking_2d, - const sensor::LaserFan& laser_fan_in_tracking_2d, + const sensor::RangeData& range_data_in_tracking_2d, transform::Rigid3d* pose_observation, kalman_filter::PoseCovariance* covariance_observation) { const ProbabilityGrid& probability_grid = @@ -123,7 +123,7 @@ void LocalTrajectoryBuilder::ScanMatch( sensor::AdaptiveVoxelFilter adaptive_voxel_filter( options_.adaptive_voxel_filter_options()); const sensor::PointCloud filtered_point_cloud_in_tracking_2d = - adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.returns); + adaptive_voxel_filter.Filter(range_data_in_tracking_2d.returns); if (options_.use_online_correlative_scan_matching()) { real_time_correlative_scan_matcher_.Match( pose_prediction_2d, filtered_point_cloud_in_tracking_2d, @@ -149,8 +149,8 @@ void LocalTrajectoryBuilder::ScanMatch( } std::unique_ptr -LocalTrajectoryBuilder::AddHorizontalLaserFan( - const common::Time time, const sensor::LaserFan& laser_fan) { +LocalTrajectoryBuilder::AddHorizontalRangeData( + const common::Time time, const sensor::RangeData& range_data) { // Initialize IMU tracker now if we do not ever use an IMU. if (!options_.use_imu_data()) { InitializeImuTracker(time); @@ -177,17 +177,19 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan( -transform::GetYaw(pose_prediction), Eigen::Vector3d::UnitZ())) * pose_prediction.rotation()); - const sensor::LaserFan laser_fan_in_tracking_2d = TransformAndFilterLaserFan( - tracking_to_tracking_2d.cast(), laser_fan); + const sensor::RangeData range_data_in_tracking_2d = + TransformAndFilterRangeData(tracking_to_tracking_2d.cast(), + range_data); - if (laser_fan_in_tracking_2d.returns.empty()) { - LOG(WARNING) << "Dropped empty horizontal laser point cloud."; + if (range_data_in_tracking_2d.returns.empty()) { + LOG(WARNING) << "Dropped empty horizontal range data."; return nullptr; } kalman_filter::PoseCovariance covariance_observation; ScanMatch(time, pose_prediction, tracking_to_tracking_2d, - laser_fan_in_tracking_2d, &pose_estimate_, &covariance_observation); + range_data_in_tracking_2d, &pose_estimate_, + &covariance_observation); odometry_correction_ = transform::Rigid3d::Identity(); if (!odometry_state_tracker_.empty()) { // We add an odometry state, so that the correction from the scan matching @@ -217,7 +219,7 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan( pose_estimate_ * tracking_to_tracking_2d.inverse(); last_pose_estimate_ = { time, pose_estimate_, - sensor::TransformPointCloud(laser_fan_in_tracking_2d.returns, + sensor::TransformPointCloud(range_data_in_tracking_2d.returns, tracking_2d_to_map.cast())}; const transform::Rigid2d pose_estimate_2d = @@ -232,13 +234,13 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan( for (int insertion_index : submaps_.insertion_indices()) { insertion_submaps.push_back(submaps_.Get(insertion_index)); } - submaps_.InsertLaserFan( - TransformLaserFan(laser_fan_in_tracking_2d, - transform::Embed3D(pose_estimate_2d.cast()))); + submaps_.InsertRangeData( + TransformRangeData(range_data_in_tracking_2d, + transform::Embed3D(pose_estimate_2d.cast()))); return common::make_unique(InsertionResult{ time, &submaps_, matching_submap, insertion_submaps, - tracking_to_tracking_2d, tracking_2d_to_map, laser_fan_in_tracking_2d, + tracking_to_tracking_2d, tracking_2d_to_map, range_data_in_tracking_2d, pose_estimate_2d, covariance_observation}); } diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index ee9c6b9..eba303c 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -50,7 +50,7 @@ class LocalTrajectoryBuilder { std::vector insertion_submaps; transform::Rigid3d tracking_to_tracking_2d; transform::Rigid3d tracking_2d_to_map; - sensor::LaserFan laser_fan_in_tracking_2d; + sensor::RangeData range_data_in_tracking_2d; transform::Rigid2d pose_estimate_2d; kalman_filter::PoseCovariance covariance_estimate; }; @@ -64,8 +64,8 @@ class LocalTrajectoryBuilder { const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate() const; - std::unique_ptr AddHorizontalLaserFan( - common::Time, const sensor::LaserFan& laser_fan); + std::unique_ptr AddHorizontalRangeData( + common::Time, const sensor::RangeData& range_data); void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); void AddOdometerData(common::Time time, const transform::Rigid3d& pose); @@ -73,15 +73,15 @@ class LocalTrajectoryBuilder { const Submaps* submaps() const; private: - sensor::LaserFan TransformAndFilterLaserFan( + sensor::RangeData TransformAndFilterRangeData( const transform::Rigid3f& tracking_to_tracking_2d, - const sensor::LaserFan& laser_fan) const; + const sensor::RangeData& range_data) const; - // Scan match 'laser_fan_in_tracking_2d' and fill in the + // Scan match 'range_data_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::LaserFan& laser_fan_in_tracking_2d, + const sensor::RangeData& range_data_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 b46db3a..4fda7fa 100644 --- a/cartographer/mapping_2d/map_limits.h +++ b/cartographer/mapping_2d/map_limits.h @@ -26,8 +26,8 @@ #include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping_2d/proto/map_limits.pb.h" #include "cartographer/mapping_2d/xy_index.h" -#include "cartographer/sensor/laser.h" #include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" @@ -85,7 +85,7 @@ class MapLimits { } // Computes MapLimits that contain the origin, and all laser rays (both - // returns and missing echoes) in the 'trajectory'. + // returns and misses) in the 'trajectory'. static MapLimits ComputeMapLimits( const double resolution, const std::vector& trajectory_nodes) { @@ -108,20 +108,20 @@ class MapLimits { for (const auto& node : trajectory_nodes) { const auto& data = *node.constant_data; if (!data.range_data_3d.returns.empty()) { - const sensor::LaserFan laser_fan = sensor::TransformLaserFan( + const sensor::RangeData range_data = sensor::TransformRangeData( Decompress(data.range_data_3d), node.pose.cast()); - bounding_box.extend(laser_fan.origin.head<2>()); - for (const Eigen::Vector3f& hit : laser_fan.returns) { + bounding_box.extend(range_data.origin.head<2>()); + for (const Eigen::Vector3f& hit : range_data.returns) { bounding_box.extend(hit.head<2>()); } } else { - 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) { + const sensor::RangeData range_data = sensor::TransformRangeData( + data.range_data_2d, node.pose.cast()); + bounding_box.extend(range_data.origin.head<2>()); + for (const Eigen::Vector3f& hit : range_data.returns) { bounding_box.extend(hit.head<2>()); } - for (const Eigen::Vector3f& miss : laser_fan.misses) { + for (const Eigen::Vector3f& miss : range_data.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 4f7d2a6..c952866 100644 --- a/cartographer/mapping_2d/map_limits_test.cc +++ b/cartographer/mapping_2d/map_limits_test.cc @@ -65,11 +65,11 @@ TEST(MapLimitsTest, ConstructAndGet) { TEST(MapLimitsTest, ComputeMapLimits) { const mapping::TrajectoryNode::ConstantData constant_data{ common::FromUniversal(52), - sensor::LaserFan{ + sensor::RangeData{ Eigen::Vector3f::Zero(), {Eigen::Vector3f(-30.f, 1.f, 0.f), Eigen::Vector3f(50.f, -10.f, 0.f)}, {}}, - Compress(sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}, {}}), nullptr, + Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}, {}}), nullptr, transform::Rigid3d::Identity()}; const mapping::TrajectoryNode trajectory_node{&constant_data, transform::Rigid3d::Identity()}; diff --git a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto b/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto index 03db854..3435469 100644 --- a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto +++ b/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto @@ -33,7 +33,7 @@ message LocalTrajectoryBuilderOptions { // empty space. optional float laser_missing_echo_ray_length = 16; - // Voxel filter that gets applied to the horizontal laser immediately after + // Voxel filter that gets applied to the range data immediately after // cropping. optional float laser_voxel_filter_size = 3; diff --git a/cartographer/mapping_2d/proto/probability_grid.proto b/cartographer/mapping_2d/proto/probability_grid.proto index 7b15a3e..d4f0e5e 100644 --- a/cartographer/mapping_2d/proto/probability_grid.proto +++ b/cartographer/mapping_2d/proto/probability_grid.proto @@ -20,7 +20,8 @@ package cartographer.mapping_2d.proto; message ProbabilityGrid { optional MapLimits limits = 1; - // These values are actually int16s, but protos don't have a native int16 type. + // These values are actually int16s, but protos don't have a native int16 + // type. repeated int32 cells = 2; repeated int32 update_indices = 3; optional int32 max_x = 4; diff --git a/cartographer/mapping_2d/proto/laser_fan_inserter_options.proto b/cartographer/mapping_2d/proto/range_data_inserter_options.proto similarity index 96% rename from cartographer/mapping_2d/proto/laser_fan_inserter_options.proto rename to cartographer/mapping_2d/proto/range_data_inserter_options.proto index 673df2d..15b435c 100644 --- a/cartographer/mapping_2d/proto/laser_fan_inserter_options.proto +++ b/cartographer/mapping_2d/proto/range_data_inserter_options.proto @@ -16,7 +16,7 @@ syntax = "proto2"; package cartographer.mapping_2d.proto; -message LaserFanInserterOptions { +message RangeDataInserterOptions { // Probability change for a hit (this will be converted to odds and therefore // must be greater than 0.5). optional double hit_probability = 1; diff --git a/cartographer/mapping_2d/proto/submaps_options.proto b/cartographer/mapping_2d/proto/submaps_options.proto index 894ed8a..81d78fa 100644 --- a/cartographer/mapping_2d/proto/submaps_options.proto +++ b/cartographer/mapping_2d/proto/submaps_options.proto @@ -14,7 +14,7 @@ syntax = "proto2"; -import "cartographer/mapping_2d/proto/laser_fan_inserter_options.proto"; +import "cartographer/mapping_2d/proto/range_data_inserter_options.proto"; package cartographer.mapping_2d.proto; @@ -28,10 +28,10 @@ message SubmapsOptions { // Number of scans before adding a new submap. Each submap will get twice the // number of scans inserted: First for initialization without being matched // against, then while being matched. - optional int32 num_laser_fans = 3; + optional int32 num_range_data = 3; // If enabled, submap%d.png images are written for debugging. optional bool output_debug_images = 4; - optional LaserFanInserterOptions laser_fan_inserter_options = 5; + optional RangeDataInserterOptions range_data_inserter_options = 5; } diff --git a/cartographer/mapping_2d/laser_fan_inserter.cc b/cartographer/mapping_2d/range_data_inserter.cc similarity index 83% rename from cartographer/mapping_2d/laser_fan_inserter.cc rename to cartographer/mapping_2d/range_data_inserter.cc index 053343a..0974b66 100644 --- a/cartographer/mapping_2d/laser_fan_inserter.cc +++ b/cartographer/mapping_2d/range_data_inserter.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/laser_fan_inserter.h" +#include "cartographer/mapping_2d/range_data_inserter.h" #include @@ -27,9 +27,9 @@ namespace cartographer { namespace mapping_2d { -proto::LaserFanInserterOptions CreateLaserFanInserterOptions( +proto::RangeDataInserterOptions CreateRangeDataInserterOptions( common::LuaParameterDictionary* const parameter_dictionary) { - proto::LaserFanInserterOptions options; + proto::RangeDataInserterOptions options; options.set_hit_probability( parameter_dictionary->GetDouble("hit_probability")); options.set_miss_probability( @@ -43,21 +43,21 @@ proto::LaserFanInserterOptions CreateLaserFanInserterOptions( return options; } -LaserFanInserter::LaserFanInserter( - const proto::LaserFanInserterOptions& options) +RangeDataInserter::RangeDataInserter( + const proto::RangeDataInserterOptions& options) : options_(options), hit_table_(mapping::ComputeLookupTableToApplyOdds( mapping::Odds(options.hit_probability()))), miss_table_(mapping::ComputeLookupTableToApplyOdds( mapping::Odds(options.miss_probability()))) {} -void LaserFanInserter::Insert(const sensor::LaserFan& laser_fan, - ProbabilityGrid* const probability_grid) const { +void RangeDataInserter::Insert(const sensor::RangeData& range_data, + ProbabilityGrid* const probability_grid) const { CHECK_NOTNULL(probability_grid)->StartUpdate(); // By not starting a new update after hits are inserted, we give hits priority // (i.e. no hits will be ignored because of a miss in the same cell). - CastRays(laser_fan, probability_grid->limits(), + CastRays(range_data, probability_grid->limits(), [this, &probability_grid](const Eigen::Array2i& hit) { probability_grid->ApplyLookupTable(hit, hit_table_); }, diff --git a/cartographer/mapping_2d/laser_fan_inserter.h b/cartographer/mapping_2d/range_data_inserter.h similarity index 65% rename from cartographer/mapping_2d/laser_fan_inserter.h rename to cartographer/mapping_2d/range_data_inserter.h index daae856..b0c0db6 100644 --- a/cartographer/mapping_2d/laser_fan_inserter.h +++ b/cartographer/mapping_2d/range_data_inserter.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_LASER_FAN_INSERTER_H_ -#define CARTOGRAPHER_MAPPING_2D_LASER_FAN_INSERTER_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_H_ +#define CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_H_ #include #include @@ -23,33 +23,33 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/port.h" #include "cartographer/mapping_2d/probability_grid.h" -#include "cartographer/mapping_2d/proto/laser_fan_inserter_options.pb.h" +#include "cartographer/mapping_2d/proto/range_data_inserter_options.pb.h" #include "cartographer/mapping_2d/xy_index.h" -#include "cartographer/sensor/laser.h" #include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" namespace cartographer { namespace mapping_2d { -proto::LaserFanInserterOptions CreateLaserFanInserterOptions( +proto::RangeDataInserterOptions CreateRangeDataInserterOptions( common::LuaParameterDictionary* parameter_dictionary); -class LaserFanInserter { +class RangeDataInserter { public: - explicit LaserFanInserter(const proto::LaserFanInserterOptions& options); + explicit RangeDataInserter(const proto::RangeDataInserterOptions& options); - LaserFanInserter(const LaserFanInserter&) = delete; - LaserFanInserter& operator=(const LaserFanInserter&) = delete; + RangeDataInserter(const RangeDataInserter&) = delete; + RangeDataInserter& operator=(const RangeDataInserter&) = delete; - // Inserts 'laser_fan' into 'probability_grid'. - void Insert(const sensor::LaserFan& laser_fan, + // Inserts 'range_data' into 'probability_grid'. + void Insert(const sensor::RangeData& range_data, ProbabilityGrid* probability_grid) const; const std::vector& hit_table() const { return hit_table_; } const std::vector& miss_table() const { return miss_table_; } private: - const proto::LaserFanInserterOptions options_; + const proto::RangeDataInserterOptions options_; const std::vector hit_table_; const std::vector miss_table_; }; @@ -57,4 +57,4 @@ class LaserFanInserter { } // namespace mapping_2d } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_LASER_FAN_INSERTER_H_ +#endif // CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_H_ diff --git a/cartographer/mapping_2d/laser_fan_inserter_test.cc b/cartographer/mapping_2d/range_data_inserter_test.cc similarity index 79% rename from cartographer/mapping_2d/laser_fan_inserter_test.cc rename to cartographer/mapping_2d/range_data_inserter_test.cc index 569e63c..7add520 100644 --- a/cartographer/mapping_2d/laser_fan_inserter_test.cc +++ b/cartographer/mapping_2d/range_data_inserter_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/laser_fan_inserter.h" +#include "cartographer/mapping_2d/range_data_inserter.h" #include @@ -28,9 +28,9 @@ namespace cartographer { namespace mapping_2d { namespace { -class LaserFanInserterTest : public ::testing::Test { +class RangeDataInserterTest : public ::testing::Test { protected: - LaserFanInserterTest() + RangeDataInserterTest() : probability_grid_( MapLimits(1., Eigen::Vector2d(1., 5.), CellLimits(5, 5))) { auto parameter_dictionary = common::MakeDictionary( @@ -39,28 +39,28 @@ class LaserFanInserterTest : public ::testing::Test { "hit_probability = 0.7, " "miss_probability = 0.4, " "}"); - options_ = CreateLaserFanInserterOptions(parameter_dictionary.get()); - laser_fan_inserter_ = common::make_unique(options_); + options_ = CreateRangeDataInserterOptions(parameter_dictionary.get()); + range_data_inserter_ = common::make_unique(options_); } void InsertPointCloud() { - 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); - laser_fan.returns.emplace_back(-0.5, 3.5, 0.f); - laser_fan.origin.x() = -0.5; - laser_fan.origin.y() = 0.5; + sensor::RangeData range_data; + range_data.returns.emplace_back(-3.5, 0.5, 0.f); + range_data.returns.emplace_back(-2.5, 1.5, 0.f); + range_data.returns.emplace_back(-1.5, 2.5, 0.f); + range_data.returns.emplace_back(-0.5, 3.5, 0.f); + range_data.origin.x() = -0.5; + range_data.origin.y() = 0.5; probability_grid_.StartUpdate(); - laser_fan_inserter_->Insert(laser_fan, &probability_grid_); + range_data_inserter_->Insert(range_data, &probability_grid_); } ProbabilityGrid probability_grid_; - std::unique_ptr laser_fan_inserter_; - proto::LaserFanInserterOptions options_; + std::unique_ptr range_data_inserter_; + proto::RangeDataInserterOptions options_; }; -TEST_F(LaserFanInserterTest, InsertPointCloud) { +TEST_F(RangeDataInserterTest, InsertPointCloud) { InsertPointCloud(); EXPECT_NEAR(1., probability_grid_.limits().max().x(), 1e-9); @@ -100,7 +100,7 @@ TEST_F(LaserFanInserterTest, InsertPointCloud) { } } -TEST_F(LaserFanInserterTest, ProbabilityProgression) { +TEST_F(RangeDataInserterTest, ProbabilityProgression) { InsertPointCloud(); EXPECT_NEAR(options_.hit_probability(), probability_grid_.GetProbability(-3.5, 0.5), 1e-4); diff --git a/cartographer/mapping_2d/ray_casting.cc b/cartographer/mapping_2d/ray_casting.cc index 7131eec..1923dea 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::RangeData& range_data, const MapLimits& limits, const std::function& hit_visitor, const std::function& miss_visitor) { const double superscaled_resolution = limits.resolution() / kSubpixelScale; @@ -156,15 +156,15 @@ void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits, CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale, limits.cell_limits().num_y_cells * kSubpixelScale)); const Eigen::Array2i begin = - superscaled_limits.GetXYIndexOfCellContainingPoint(laser_fan.origin.x(), - laser_fan.origin.y()); + superscaled_limits.GetXYIndexOfCellContainingPoint(range_data.origin.x(), + range_data.origin.y()); // Compute and add the end points. std::vector ends; - 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())); + ends.reserve(range_data.returns.size()); + for (const Eigen::Vector3f& hit : range_data.returns) { + ends.push_back( + superscaled_limits.GetXYIndexOfCellContainingPoint(hit.x(), hit.y())); hit_visitor(ends.back() / kSubpixelScale); } @@ -173,8 +173,8 @@ void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits, CastRay(begin, end, miss_visitor); } - // Finally, compute and add empty rays based on missing echos in the scan. - for (const Eigen::Vector3f& missing_echo : laser_fan.misses) { + // Finally, compute and add empty rays based on misses in the scan. + for (const Eigen::Vector3f& missing_echo : range_data.misses) { CastRay(begin, superscaled_limits.GetXYIndexOfCellContainingPoint( missing_echo.x(), missing_echo.y()), diff --git a/cartographer/mapping_2d/ray_casting.h b/cartographer/mapping_2d/ray_casting.h index 7e12121..b829ad1 100644 --- a/cartographer/mapping_2d/ray_casting.h +++ b/cartographer/mapping_2d/ray_casting.h @@ -21,16 +21,16 @@ #include "cartographer/mapping_2d/map_limits.h" #include "cartographer/mapping_2d/xy_index.h" -#include "cartographer/sensor/laser.h" #include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" #include "cartographer/transform/transform.h" namespace cartographer { namespace mapping_2d { -// For each ray in 'laser_fan', calls 'hit_visitor' and 'miss_visitor' on the +// For each ray in 'range_data', 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::RangeData& range_data, 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 cab2044..6a45faa 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 @@ -23,8 +23,8 @@ #include #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" -#include "cartographer/mapping_2d/laser_fan_inserter.h" #include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/mapping_2d/range_data_inserter.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/transform.h" #include "gtest/gtest.h" @@ -118,20 +118,21 @@ CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) { return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); } -mapping_2d::proto::LaserFanInserterOptions CreateLaserFanInserterTestOptions() { +mapping_2d::proto::RangeDataInserterOptions +CreateRangeDataInserterTestOptions() { auto parameter_dictionary = common::MakeDictionary(R"text( return { insert_free_space = true, hit_probability = 0.7, miss_probability = 0.4, })text"); - return mapping_2d::CreateLaserFanInserterOptions(parameter_dictionary.get()); + return mapping_2d::CreateRangeDataInserterOptions(parameter_dictionary.get()); } TEST(FastCorrelativeScanMatcherTest, CorrectPose) { std::mt19937 prng(42); std::uniform_real_distribution distribution(-1.f, 1.f); - LaserFanInserter laser_fan_inserter(CreateLaserFanInserterTestOptions()); + RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions()); constexpr float kMinScore = 0.1f; const auto options = CreateFastCorrelativeScanMatcherTestOptions(3); @@ -151,8 +152,8 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { ProbabilityGrid probability_grid( MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); probability_grid.StartUpdate(); - laser_fan_inserter.Insert( - sensor::LaserFan{ + range_data_inserter.Insert( + sensor::RangeData{ Eigen::Vector3f(expected_pose.translation().x(), expected_pose.translation().y(), 0.f), sensor::TransformPointCloud( @@ -178,7 +179,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { std::mt19937 prng(42); std::uniform_real_distribution distribution(-1.f, 1.f); - LaserFanInserter laser_fan_inserter(CreateLaserFanInserterTestOptions()); + RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions()); constexpr float kMinScore = 0.1f; const auto options = CreateFastCorrelativeScanMatcherTestOptions(6); @@ -204,8 +205,8 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { ProbabilityGrid probability_grid( MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); probability_grid.StartUpdate(); - laser_fan_inserter.Insert( - sensor::LaserFan{ + range_data_inserter.Insert( + sensor::RangeData{ 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 dbab5e5..a78d5a2 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 @@ -23,8 +23,8 @@ #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" #include "cartographer/kalman_filter/pose_tracker.h" -#include "cartographer/mapping_2d/laser_fan_inserter.h" #include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/mapping_2d/range_data_inserter.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/transform.h" #include "gtest/gtest.h" @@ -46,8 +46,8 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { "hit_probability = 0.7, " "miss_probability = 0.4, " "}"); - laser_fan_inserter_ = common::make_unique( - CreateLaserFanInserterOptions(parameter_dictionary.get())); + range_data_inserter_ = common::make_unique( + CreateRangeDataInserterOptions(parameter_dictionary.get())); } point_cloud_.emplace_back(0.025f, 0.175f, 0.f); point_cloud_.emplace_back(-0.025f, 0.175f, 0.f); @@ -57,8 +57,8 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { point_cloud_.emplace_back(-0.125f, 0.075f, 0.f); point_cloud_.emplace_back(-0.125f, 0.025f, 0.f); probability_grid_.StartUpdate(); - laser_fan_inserter_->Insert( - sensor::LaserFan{Eigen::Vector3f::Zero(), point_cloud_, {}}, + range_data_inserter_->Insert( + sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}}, &probability_grid_); { auto parameter_dictionary = common::MakeDictionary( @@ -76,7 +76,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { } ProbabilityGrid probability_grid_; - std::unique_ptr laser_fan_inserter_; + std::unique_ptr range_data_inserter_; sensor::PointCloud point_cloud_; std::unique_ptr real_time_correlative_scan_matcher_; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index c9a9750..ef35a11 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -85,7 +85,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::RangeData& range_data_in_pose, const transform::Rigid2d& pose, const kalman_filter::Pose2DCovariance& covariance, const mapping::Submaps* submaps, const mapping::Submap* const matching_submap, @@ -98,8 +98,8 @@ void SparsePoseGraph::AddScan( CHECK_LT(j, std::numeric_limits::max()); constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{ - time, laser_fan_in_pose, - Compress(sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}, {}}), submaps, + time, range_data_in_pose, + Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}, {}}), submaps, tracking_to_pose}); trajectory_nodes_.push_back(mapping::TrajectoryNode{ &constant_node_data_->back(), optimized_pose, @@ -167,7 +167,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, 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_2d.returns); + &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns); } else { const bool scan_and_submap_trajectories_connected = reverse_connected_components_.count(scan_trajectory) > 0 && @@ -178,7 +178,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, scan_and_submap_trajectories_connected) { constraint_builder_.MaybeAddConstraint( submap_index, submap_states_[submap_index].submap, scan_index, - &trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns, + &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns, relative_pose); } } diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 2136f38..f67885a 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -66,13 +66,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; - // Adds a new 'laser_fan_in_pose' observation at 'time', and a 'pose' + // Adds a new 'range_data_in_pose' observation at 'time', and a 'pose' // that will later be optimized. The 'tracking_to_pose' is remembered so // that the optimized pose can be embedded into 3D. The 'pose' was determined // 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::RangeData& range_data_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 d46b85e..7c2ed15 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -24,7 +24,7 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" -#include "cartographer/mapping_2d/laser_fan_inserter.h" +#include "cartographer/mapping_2d/range_data_inserter.h" #include "cartographer/mapping_2d/submaps.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform_test_helpers.h" @@ -52,9 +52,9 @@ class SparsePoseGraphTest : public ::testing::Test { return { resolution = 0.05, half_length = 21., - num_laser_fans = 1, + num_range_data = 1, output_debug_images = false, - laser_fan_inserter = { + range_data_inserter = { insert_free_space = true, hit_probability = 0.53, miss_probability = 0.495, @@ -155,13 +155,13 @@ 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{ + const sensor::RangeData range_data{ Eigen::Vector3f::Zero(), new_point_cloud, {}}; const transform::Rigid2d pose_estimate = noise * current_pose_; - submaps_->InsertLaserFan(TransformLaserFan( - laser_fan, transform::Embed3D(pose_estimate.cast()))); + submaps_->InsertRangeData(TransformRangeData( + range_data, transform::Embed3D(pose_estimate.cast()))); sparse_pose_graph_->AddScan(common::FromUniversal(0), - transform::Rigid3d::Identity(), laser_fan, + transform::Rigid3d::Identity(), range_data, pose_estimate, covariance, submaps_.get(), matching_submap, insertion_submaps); } diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index e1362c6..a221940 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/cartographer/mapping_2d/submaps.cc @@ -90,42 +90,43 @@ proto::SubmapsOptions CreateSubmapsOptions( proto::SubmapsOptions options; options.set_resolution(parameter_dictionary->GetDouble("resolution")); options.set_half_length(parameter_dictionary->GetDouble("half_length")); - options.set_num_laser_fans( - parameter_dictionary->GetNonNegativeInt("num_laser_fans")); + options.set_num_range_data( + parameter_dictionary->GetNonNegativeInt("num_range_data")); options.set_output_debug_images( parameter_dictionary->GetBool("output_debug_images")); - *options.mutable_laser_fan_inserter_options() = CreateLaserFanInserterOptions( - parameter_dictionary->GetDictionary("laser_fan_inserter").get()); - CHECK_GT(options.num_laser_fans(), 0); + *options.mutable_range_data_inserter_options() = + CreateRangeDataInserterOptions( + parameter_dictionary->GetDictionary("range_data_inserter").get()); + CHECK_GT(options.num_range_data(), 0); return options; } Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin, - const int begin_laser_fan_index) + const int begin_range_data_index) : mapping::Submap(Eigen::Vector3f(origin.x(), origin.y(), 0.), - begin_laser_fan_index), + begin_range_data_index), probability_grid(limits) {} Submaps::Submaps(const proto::SubmapsOptions& options) : options_(options), - laser_fan_inserter_(options.laser_fan_inserter_options()) { + range_data_inserter_(options.range_data_inserter_options()) { // We always want to have at least one likelihood field which we can return, // and will create it at the origin in absence of a better choice. AddSubmap(Eigen::Vector2f::Zero()); } -void Submaps::InsertLaserFan(const sensor::LaserFan& laser_fan) { - CHECK_LT(num_laser_fans_, std::numeric_limits::max()); - ++num_laser_fans_; +void Submaps::InsertRangeData(const sensor::RangeData& range_data) { + CHECK_LT(num_range_data_, std::numeric_limits::max()); + ++num_range_data_; for (const int index : insertion_indices()) { Submap* submap = submaps_[index].get(); CHECK(submap->finished_probability_grid == nullptr); - laser_fan_inserter_.Insert(laser_fan, &submap->probability_grid); - submap->end_laser_fan_index = num_laser_fans_; + range_data_inserter_.Insert(range_data, &submap->probability_grid); + submap->end_range_data_index = num_range_data_; } - ++num_laser_fans_in_last_submap_; - if (num_laser_fans_in_last_submap_ == options_.num_laser_fans()) { - AddSubmap(laser_fan.origin.head<2>()); + ++num_range_data_in_last_submap_; + if (num_range_data_in_last_submap_ == options_.num_range_data()) { + AddSubmap(range_data.origin.head<2>()); } } @@ -172,9 +173,9 @@ void Submaps::AddSubmap(const Eigen::Vector2f& origin) { origin.cast() + options_.half_length() * Eigen::Vector2d::Ones(), CellLimits(num_cells_per_dimension, num_cells_per_dimension)), - origin, num_laser_fans_)); + origin, num_range_data_)); LOG(INFO) << "Added submap " << size(); - num_laser_fans_in_last_submap_ = 0; + num_range_data_in_last_submap_ = 0; } } // namespace mapping_2d diff --git a/cartographer/mapping_2d/submaps.h b/cartographer/mapping_2d/submaps.h index b152310..e119b71 100644 --- a/cartographer/mapping_2d/submaps.h +++ b/cartographer/mapping_2d/submaps.h @@ -25,11 +25,11 @@ #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_node.h" -#include "cartographer/mapping_2d/laser_fan_inserter.h" #include "cartographer/mapping_2d/map_limits.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/proto/submaps_options.pb.h" -#include "cartographer/sensor/laser.h" +#include "cartographer/mapping_2d/range_data_inserter.h" +#include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { @@ -43,7 +43,7 @@ proto::SubmapsOptions CreateSubmapsOptions( struct Submap : public mapping::Submap { Submap(const MapLimits& limits, const Eigen::Vector2f& origin, - int begin_laser_fan_index); + int begin_range_data_index); ProbabilityGrid probability_grid; }; @@ -63,8 +63,8 @@ class Submaps : public mapping::Submaps { const transform::Rigid3d& global_submap_pose, mapping::proto::SubmapQuery::Response* response) const override; - // Inserts 'laser_fan' into the Submap collection. - void InsertLaserFan(const sensor::LaserFan& laser_fan); + // Inserts 'range_data' into the Submap collection. + void InsertRangeData(const sensor::RangeData& range_data); private: void FinishSubmap(int index); @@ -73,13 +73,13 @@ class Submaps : public mapping::Submaps { const proto::SubmapsOptions options_; std::vector> submaps_; - LaserFanInserter laser_fan_inserter_; + RangeDataInserter range_data_inserter_; - // Number of LaserFans inserted. - int num_laser_fans_ = 0; + // Number of RangeData inserted. + int num_range_data_ = 0; - // Number of LaserFans inserted since the last Submap was added. - int num_laser_fans_in_last_submap_ = 0; + // Number of RangeData inserted since the last Submap was added. + int num_range_data_in_last_submap_ = 0; }; } // namespace mapping_2d diff --git a/cartographer/mapping_2d/submaps_test.cc b/cartographer/mapping_2d/submaps_test.cc index 77426f7..c818ae0 100644 --- a/cartographer/mapping_2d/submaps_test.cc +++ b/cartographer/mapping_2d/submaps_test.cc @@ -30,16 +30,16 @@ namespace mapping_2d { namespace { TEST(SubmapsTest, TheRightNumberOfScansAreInserted) { - constexpr int kNumLaserFans = 10; + constexpr int kNumRangeData = 10; auto parameter_dictionary = common::MakeDictionary( "return {" "resolution = 0.05, " "half_length = 10., " - "num_laser_fans = " + - std::to_string(kNumLaserFans) + + "num_range_data = " + + std::to_string(kNumRangeData) + ", " "output_debug_images = false, " - "laser_fan_inserter = {" + "range_data_inserter = {" "insert_free_space = true, " "hit_probability = 0.53, " "miss_probability = 0.495, " @@ -47,22 +47,22 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) { "}"); Submaps submaps{CreateSubmapsOptions(parameter_dictionary.get())}; auto num_inserted = [&submaps](const int i) { - return submaps.Get(i)->end_laser_fan_index - - submaps.Get(i)->begin_laser_fan_index; + return submaps.Get(i)->end_range_data_index - + submaps.Get(i)->begin_range_data_index; }; for (int i = 0; i != 1000; ++i) { - submaps.InsertLaserFan({Eigen::Vector3f::Zero(), {}, {}}); + submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}}); const int matching = submaps.matching_index(); // Except for the first, maps should only be returned after enough scans. if (matching != 0) { - EXPECT_LE(kNumLaserFans, num_inserted(matching)); + EXPECT_LE(kNumRangeData, num_inserted(matching)); } } for (int i = 0; i != submaps.size() - 2; ++i) { // Submaps should not be left without the right number of scans in them. - EXPECT_EQ(kNumLaserFans * 2, num_inserted(i)); - EXPECT_EQ(i * kNumLaserFans, submaps.Get(i)->begin_laser_fan_index); - EXPECT_EQ((i + 2) * kNumLaserFans, submaps.Get(i)->end_laser_fan_index); + EXPECT_EQ(kNumRangeData * 2, num_inserted(i)); + EXPECT_EQ(i * kNumRangeData, submaps.Get(i)->begin_range_data_index); + EXPECT_EQ((i + 2) * kNumRangeData, submaps.Get(i)->end_range_data_index); } } diff --git a/cartographer/mapping_3d/CMakeLists.txt b/cartographer/mapping_3d/CMakeLists.txt index 84c501a..986c680 100644 --- a/cartographer/mapping_3d/CMakeLists.txt +++ b/cartographer/mapping_3d/CMakeLists.txt @@ -25,9 +25,9 @@ google_test(mapping_3d_kalman_local_trajectory_builder_test kalman_local_trajectory_builder_test.cc ) -google_test(mapping_3d_laser_fan_inserter_test +google_test(mapping_3d_range_data_inserter_test SRCS - laser_fan_inserter_test.cc + range_data_inserter_test.cc ) google_test(mapping_3d_motion_filter_test diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index 74c5862..39d6e18 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -53,7 +53,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData( } const int trajectory_node_index = sparse_pose_graph_->AddScan( - insertion_result->time, insertion_result->laser_fan_in_tracking, + insertion_result->time, insertion_result->range_data_in_tracking, insertion_result->pose_observation, insertion_result->covariance_estimate, insertion_result->submaps, insertion_result->matching_submap, insertion_result->insertion_submaps); diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index bf6e023..5e5bbcf 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -42,7 +42,7 @@ KalmanLocalTrajectoryBuilder::KalmanLocalTrajectoryBuilder( options_.ceres_scan_matcher_options())), num_accumulated_(0), first_pose_prediction_(transform::Rigid3f::Identity()), - accumulated_laser_fan_{Eigen::Vector3f::Zero(), {}, {}} {} + accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {} KalmanLocalTrajectoryBuilder::~KalmanLocalTrajectoryBuilder() {} @@ -84,28 +84,27 @@ KalmanLocalTrajectoryBuilder::AddRangefinderData( time, &pose_prediction, &unused_covariance_prediction); if (num_accumulated_ == 0) { first_pose_prediction_ = pose_prediction.cast(); - accumulated_laser_fan_ = sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}}; + accumulated_range_data_ = + sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}; } const transform::Rigid3f tracking_delta = first_pose_prediction_.inverse() * pose_prediction.cast(); - const sensor::LaserFan laser_fan_in_first_tracking = - sensor::TransformLaserFan(sensor::LaserFan{origin, ranges, {}, {}}, - tracking_delta); - for (const Eigen::Vector3f& laser_return : - laser_fan_in_first_tracking.returns) { - const Eigen::Vector3f delta = - laser_return - laser_fan_in_first_tracking.origin; + const sensor::RangeData range_data_in_first_tracking = + sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}, {}}, + tracking_delta); + for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) { + const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin; const float range = delta.norm(); if (range >= options_.laser_min_range()) { if (range <= options_.laser_max_range()) { - accumulated_laser_fan_.returns.push_back(laser_return); + accumulated_range_data_.returns.push_back(hit); } else { // We insert a ray cropped to 'laser_max_range' as a miss for hits // beyond the maximum range. This way the free space up to the maximum // range will be updated. - accumulated_laser_fan_.misses.push_back( - laser_fan_in_first_tracking.origin + + accumulated_range_data_.misses.push_back( + range_data_in_first_tracking.origin + options_.laser_max_range() / range * delta); } } @@ -114,25 +113,25 @@ KalmanLocalTrajectoryBuilder::AddRangefinderData( if (num_accumulated_ >= options_.scans_per_accumulation()) { num_accumulated_ = 0; - return AddAccumulatedLaserFan( - time, sensor::TransformLaserFan(accumulated_laser_fan_, - tracking_delta.inverse())); + return AddAccumulatedRangeData( + time, sensor::TransformRangeData(accumulated_range_data_, + tracking_delta.inverse())); } return nullptr; } std::unique_ptr -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, +KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData( + const common::Time time, const sensor::RangeData& range_data_in_tracking) { + const sensor::RangeData filtered_range_data = { + range_data_in_tracking.origin, + sensor::VoxelFiltered(range_data_in_tracking.returns, options_.laser_voxel_filter_size()), - sensor::VoxelFiltered(laser_fan_in_tracking.misses, + sensor::VoxelFiltered(range_data_in_tracking.misses, options_.laser_voxel_filter_size())}; - if (filtered_laser_fan.returns.empty()) { - LOG(WARNING) << "Dropped empty laser scanner point cloud."; + if (filtered_range_data.returns.empty()) { + LOG(WARNING) << "Dropped empty range data."; return nullptr; } @@ -145,7 +144,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan( sensor::AdaptiveVoxelFilter adaptive_voxel_filter( options_.high_resolution_adaptive_voxel_filter_options()); const sensor::PointCloud filtered_point_cloud_in_tracking = - adaptive_voxel_filter.Filter(filtered_laser_fan.returns); + adaptive_voxel_filter.Filter(filtered_range_data.returns); if (options_.kalman_local_trajectory_builder_options() .use_online_correlative_scan_matching()) { real_time_correlative_scan_matcher_->Match( @@ -159,7 +158,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan( sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( options_.low_resolution_adaptive_voxel_filter_options()); const sensor::PointCloud low_resolution_point_cloud_in_tracking = - low_resolution_adaptive_voxel_filter.Filter(filtered_laser_fan.returns); + low_resolution_adaptive_voxel_filter.Filter(filtered_range_data.returns); ceres_scan_matcher_->Match(scan_matcher_pose_estimate_, initial_ceres_pose, {{&filtered_point_cloud_in_tracking, &submaps_->high_resolution_matching_grid()}, @@ -178,10 +177,10 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan( last_pose_estimate_ = { time, scan_matcher_pose_estimate_, - sensor::TransformPointCloud(filtered_laser_fan.returns, + sensor::TransformPointCloud(filtered_range_data.returns, pose_observation.cast())}; - return InsertIntoSubmap(time, filtered_laser_fan, pose_observation, + return InsertIntoSubmap(time, filtered_range_data, pose_observation, covariance_estimate); } @@ -214,7 +213,7 @@ void KalmanLocalTrajectoryBuilder::AddTrajectoryNodeIndex( std::unique_ptr KalmanLocalTrajectoryBuilder::InsertIntoSubmap( - const common::Time time, const sensor::LaserFan& laser_fan_in_tracking, + const common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose_observation, const kalman_filter::PoseCovariance& covariance_estimate) { if (motion_filter_.IsSimilar(time, pose_observation)) { @@ -226,10 +225,10 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap( for (int insertion_index : submaps_->insertion_indices()) { insertion_submaps.push_back(submaps_->Get(insertion_index)); } - submaps_->InsertLaserFan(sensor::TransformLaserFan( - laser_fan_in_tracking, pose_observation.cast())); + submaps_->InsertRangeData(sensor::TransformRangeData( + range_data_in_tracking, pose_observation.cast())); return std::unique_ptr(new InsertionResult{ - time, laser_fan_in_tracking, pose_observation, covariance_estimate, + time, range_data_in_tracking, pose_observation, covariance_estimate, submaps_.get(), matching_submap, insertion_submaps}); } diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.h b/cartographer/mapping_3d/kalman_local_trajectory_builder.h index b57003d..34d7291 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.h +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.h @@ -28,7 +28,7 @@ #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/mapping_3d/submaps.h" -#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/range_data.h" #include "cartographer/sensor/voxel_filter.h" namespace cartographer { @@ -58,11 +58,11 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { const PoseEstimate& pose_estimate() const override; private: - std::unique_ptr AddAccumulatedLaserFan( - common::Time time, const sensor::LaserFan& laser_fan_in_tracking); + std::unique_ptr AddAccumulatedRangeData( + common::Time time, const sensor::RangeData& range_data_in_tracking); std::unique_ptr InsertIntoSubmap( - const common::Time time, const sensor::LaserFan& laser_fan_in_tracking, + const common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose_observation, const kalman_filter::PoseCovariance& covariance_estimate); @@ -83,7 +83,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { int num_accumulated_; transform::Rigid3f first_pose_prediction_; - sensor::LaserFan accumulated_laser_fan_; + sensor::RangeData accumulated_range_data_; }; } // 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 d251f92..b3963a1 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc @@ -24,7 +24,7 @@ #include "cartographer/common/time.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/local_trajectory_builder_options.h" -#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/transform.h" @@ -85,8 +85,8 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { high_resolution = 0.2, high_resolution_max_range = 50., low_resolution = 0.5, - num_laser_fans = 45000, - laser_fan_inserter = { + num_range_data = 45000, + range_data_inserter = { hit_probability = 0.7, miss_probability = 0.4, num_free_space_voxels = 0, @@ -189,7 +189,7 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { return first * (to - from) + from; } - sensor::LaserFan GenerateLaserFan(const transform::Rigid3d& pose) { + sensor::RangeData GenerateRangeData(const transform::Rigid3d& pose) { // 360 degree rays at 16 angles. sensor::PointCloud directions_in_laser_frame; for (int r = -8; r != 8; ++r) { @@ -259,9 +259,9 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { int num_poses = 0; for (const TrajectoryNode& node : expected_trajectory) { AddLinearOnlyImuObservation(node.time, node.pose); - const auto laser_fan = GenerateLaserFan(node.pose); + const auto range_data = GenerateRangeData(node.pose); if (local_trajectory_builder_->AddRangefinderData( - node.time, laser_fan.origin, laser_fan.returns) != nullptr) { + node.time, range_data.origin, range_data.returns) != nullptr) { const auto pose_estimate = local_trajectory_builder_->pose_estimate(); EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1)); ++num_poses; diff --git a/cartographer/mapping_3d/local_trajectory_builder_interface.h b/cartographer/mapping_3d/local_trajectory_builder_interface.h index a04fcd7..8410bf2 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_interface.h +++ b/cartographer/mapping_3d/local_trajectory_builder_interface.h @@ -23,7 +23,7 @@ #include "cartographer/common/time.h" #include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping_3d/submaps.h" -#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { @@ -35,7 +35,7 @@ class LocalTrajectoryBuilderInterface { struct InsertionResult { common::Time time; - sensor::LaserFan laser_fan_in_tracking; + sensor::RangeData range_data_in_tracking; transform::Rigid3d pose_observation; kalman_filter::PoseCovariance covariance_estimate; const Submaps* submaps; diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc index 211ec5d..f4f8362 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc @@ -139,14 +139,14 @@ OptimizingLocalTrajectoryBuilder::AddRangefinderData( CHECK_GT(ranges.size(), 0); // TODO(hrapp): Handle misses. - // TODO(hrapp): Where are NaNs in laser_fan_in_tracking coming from? + // TODO(hrapp): Where are NaNs in range_data_in_tracking coming from? sensor::PointCloud point_cloud; - for (const Eigen::Vector3f& laser_return : ranges) { - const Eigen::Vector3f delta = laser_return - origin; + for (const Eigen::Vector3f& hit : ranges) { + const Eigen::Vector3f delta = hit - origin; const float range = delta.norm(); if (range >= options_.laser_min_range()) { if (range <= options_.laser_max_range()) { - point_cloud.push_back(laser_return); + point_cloud.push_back(hit); } } } @@ -172,7 +172,7 @@ OptimizingLocalTrajectoryBuilder::AddRangefinderData( low_resolution_adaptive_voxel_filter.Filter(point_cloud); if (batches_.empty()) { - // First laser ever. Initialize to the origin. + // First rangefinder data ever. Initialize to the origin. batches_.push_back( Batch{time, point_cloud, high_resolution_filtered_points, low_resolution_filtered_points, @@ -299,8 +299,8 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { interpolation_buffer.Push(odometer_data.time, odometer_data.pose); } for (size_t i = 1; i < batches_.size(); ++i) { - // Only add constraints for this laser if we have bracketing data from - // the odometer. + // Only add constraints for this range data if we have bracketing data + // from the odometer. if (!(interpolation_buffer.earliest_time() <= batches_[i - 1].time && batches_[i].time <= interpolation_buffer.latest_time())) { continue; @@ -336,43 +336,43 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { num_accumulated_ = 0; const transform::Rigid3d optimized_pose = batches_.back().state.ToRigid(); - sensor::LaserFan accumulated_laser_fan_in_tracking = { + sensor::RangeData accumulated_range_data_in_tracking = { Eigen::Vector3f::Zero(), {}, {}}; for (const auto& batch : batches_) { const transform::Rigid3f transform = (optimized_pose.inverse() * batch.state.ToRigid()).cast(); for (const Eigen::Vector3f& point : batch.points) { - accumulated_laser_fan_in_tracking.returns.push_back(transform * point); + accumulated_range_data_in_tracking.returns.push_back(transform * point); } } - return AddAccumulatedLaserFan(time, optimized_pose, - accumulated_laser_fan_in_tracking); + return AddAccumulatedRangeData(time, optimized_pose, + accumulated_range_data_in_tracking); } std::unique_ptr -OptimizingLocalTrajectoryBuilder::AddAccumulatedLaserFan( +OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData( const common::Time time, const transform::Rigid3d& optimized_pose, - 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, + const sensor::RangeData& range_data_in_tracking) { + const sensor::RangeData filtered_range_data = { + range_data_in_tracking.origin, + sensor::VoxelFiltered(range_data_in_tracking.returns, options_.laser_voxel_filter_size()), - sensor::VoxelFiltered(laser_fan_in_tracking.misses, + sensor::VoxelFiltered(range_data_in_tracking.misses, options_.laser_voxel_filter_size())}; - if (filtered_laser_fan.returns.empty()) { - LOG(WARNING) << "Dropped empty laser scanner point cloud."; + if (filtered_range_data.returns.empty()) { + LOG(WARNING) << "Dropped empty range data."; return nullptr; } last_pose_estimate_ = { time, optimized_pose, - sensor::TransformPointCloud(filtered_laser_fan.returns, + sensor::TransformPointCloud(filtered_range_data.returns, optimized_pose.cast())}; - return InsertIntoSubmap(time, filtered_laser_fan, optimized_pose); + return InsertIntoSubmap(time, filtered_range_data, optimized_pose); } const OptimizingLocalTrajectoryBuilder::PoseEstimate& @@ -387,7 +387,7 @@ void OptimizingLocalTrajectoryBuilder::AddTrajectoryNodeIndex( std::unique_ptr OptimizingLocalTrajectoryBuilder::InsertIntoSubmap( - const common::Time time, const sensor::LaserFan& laser_fan_in_tracking, + const common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose_observation) { if (motion_filter_.IsSimilar(time, pose_observation)) { return nullptr; @@ -398,14 +398,14 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap( for (int insertion_index : submaps_->insertion_indices()) { insertion_submaps.push_back(submaps_->Get(insertion_index)); } - submaps_->InsertLaserFan(sensor::TransformLaserFan( - laser_fan_in_tracking, pose_observation.cast())); + submaps_->InsertRangeData(sensor::TransformRangeData( + range_data_in_tracking, pose_observation.cast())); const kalman_filter::PoseCovariance kCovariance = 1e-7 * kalman_filter::PoseCovariance::Identity(); return std::unique_ptr(new InsertionResult{ - time, laser_fan_in_tracking, pose_observation, kCovariance, + time, range_data_in_tracking, pose_observation, kCovariance, submaps_.get(), matching_submap, insertion_submaps}); } diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h index 348490b..7b90bc2 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h @@ -28,7 +28,7 @@ #include "cartographer/mapping_3d/motion_filter.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/submaps.h" -#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/range_data.h" #include "cartographer/sensor/voxel_filter.h" #include "cartographer/transform/rigid_transform.h" @@ -100,12 +100,12 @@ class OptimizingLocalTrajectoryBuilder void RemoveObsoleteSensorData(); - std::unique_ptr AddAccumulatedLaserFan( + std::unique_ptr AddAccumulatedRangeData( common::Time time, const transform::Rigid3d& pose_observation, - const sensor::LaserFan& laser_fan_in_tracking); + const sensor::RangeData& range_data_in_tracking); std::unique_ptr InsertIntoSubmap( - const common::Time time, const sensor::LaserFan& laser_fan_in_tracking, + const common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose_observation); std::unique_ptr MaybeOptimize(common::Time time); diff --git a/cartographer/mapping_3d/proto/laser_fan_inserter_options.proto b/cartographer/mapping_3d/proto/range_data_inserter_options.proto similarity index 96% rename from cartographer/mapping_3d/proto/laser_fan_inserter_options.proto rename to cartographer/mapping_3d/proto/range_data_inserter_options.proto index f03fddb..0ea20c5 100644 --- a/cartographer/mapping_3d/proto/laser_fan_inserter_options.proto +++ b/cartographer/mapping_3d/proto/range_data_inserter_options.proto @@ -16,7 +16,7 @@ syntax = "proto2"; package cartographer.mapping_3d.proto; -message LaserFanInserterOptions { +message RangeDataInserterOptions { // Probability change for a hit (this will be converted to odds and therefore // must be greater than 0.5). optional double hit_probability = 1; diff --git a/cartographer/mapping_3d/proto/submaps_options.proto b/cartographer/mapping_3d/proto/submaps_options.proto index 2353a9d..abc6696 100644 --- a/cartographer/mapping_3d/proto/submaps_options.proto +++ b/cartographer/mapping_3d/proto/submaps_options.proto @@ -14,7 +14,7 @@ syntax = "proto2"; -import "cartographer/mapping_3d/proto/laser_fan_inserter_options.proto"; +import "cartographer/mapping_3d/proto/range_data_inserter_options.proto"; package cartographer.mapping_3d.proto; @@ -34,7 +34,7 @@ message SubmapsOptions { // Number of scans before adding a new submap. Each submap will get twice the // number of scans inserted: First for initialization without being matched // against, then while being matched. - optional int32 num_laser_fans = 2; + optional int32 num_range_data = 2; - optional LaserFanInserterOptions laser_fan_inserter_options = 3; + optional RangeDataInserterOptions range_data_inserter_options = 3; } diff --git a/cartographer/mapping_3d/laser_fan_inserter.cc b/cartographer/mapping_3d/range_data_inserter.cc similarity index 74% rename from cartographer/mapping_3d/laser_fan_inserter.cc rename to cartographer/mapping_3d/range_data_inserter.cc index 58a9d92..0516dc4 100644 --- a/cartographer/mapping_3d/laser_fan_inserter.cc +++ b/cartographer/mapping_3d/range_data_inserter.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/laser_fan_inserter.h" +#include "cartographer/mapping_3d/range_data_inserter.h" #include "Eigen/Core" #include "cartographer/mapping/probability_values.h" @@ -25,13 +25,13 @@ namespace mapping_3d { namespace { -void InsertLaserMissesIntoGrid(const std::vector& miss_table, - const Eigen::Vector3f& origin, - const sensor::PointCloud& laser_returns, - HybridGrid* hybrid_grid, - const int num_free_space_voxels) { +void InsertMissesIntoGrid(const std::vector& miss_table, + const Eigen::Vector3f& origin, + const sensor::PointCloud& returns, + HybridGrid* hybrid_grid, + const int num_free_space_voxels) { const Eigen::Array3i origin_cell = hybrid_grid->GetCellIndex(origin); - for (const Eigen::Vector3f& hit : laser_returns) { + for (const Eigen::Vector3f& hit : returns) { const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit); const Eigen::Array3i delta = hit_cell - origin_cell; @@ -54,9 +54,9 @@ void InsertLaserMissesIntoGrid(const std::vector& miss_table, } // namespace -proto::LaserFanInserterOptions CreateLaserFanInserterOptions( +proto::RangeDataInserterOptions CreateRangeDataInserterOptions( common::LuaParameterDictionary* parameter_dictionary) { - proto::LaserFanInserterOptions options; + proto::RangeDataInserterOptions options; options.set_hit_probability( parameter_dictionary->GetDouble("hit_probability")); options.set_miss_probability( @@ -68,27 +68,27 @@ proto::LaserFanInserterOptions CreateLaserFanInserterOptions( return options; } -LaserFanInserter::LaserFanInserter( - const proto::LaserFanInserterOptions& options) +RangeDataInserter::RangeDataInserter( + const proto::RangeDataInserterOptions& options) : options_(options), hit_table_(mapping::ComputeLookupTableToApplyOdds( mapping::Odds(options_.hit_probability()))), miss_table_(mapping::ComputeLookupTableToApplyOdds( mapping::Odds(options_.miss_probability()))) {} -void LaserFanInserter::Insert(const sensor::LaserFan& laser_fan, - HybridGrid* hybrid_grid) const { +void RangeDataInserter::Insert(const sensor::RangeData& range_data, + HybridGrid* hybrid_grid) const { CHECK_NOTNULL(hybrid_grid)->StartUpdate(); - for (const Eigen::Vector3f& hit : laser_fan.returns) { + for (const Eigen::Vector3f& hit : range_data.returns) { const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit); hybrid_grid->ApplyLookupTable(hit_cell, hit_table_); } // By not starting a new update after hits are inserted, we give hits priority // (i.e. no hits will be ignored because of a miss in the same cell). - InsertLaserMissesIntoGrid(miss_table_, laser_fan.origin, laser_fan.returns, - hybrid_grid, options_.num_free_space_voxels()); + InsertMissesIntoGrid(miss_table_, range_data.origin, range_data.returns, + hybrid_grid, options_.num_free_space_voxels()); } } // namespace mapping_3d diff --git a/cartographer/mapping_3d/laser_fan_inserter.h b/cartographer/mapping_3d/range_data_inserter.h similarity index 55% rename from cartographer/mapping_3d/laser_fan_inserter.h rename to cartographer/mapping_3d/range_data_inserter.h index 8b95bd5..d03ae3a 100644 --- a/cartographer/mapping_3d/laser_fan_inserter.h +++ b/cartographer/mapping_3d/range_data_inserter.h @@ -14,32 +14,33 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_3D_LASER_FAN_INSERTER_H_ -#define CARTOGRAPHER_MAPPING_3D_LASER_FAN_INSERTER_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_ +#define CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_ #include "cartographer/mapping_3d/hybrid_grid.h" -#include "cartographer/mapping_3d/proto/laser_fan_inserter_options.pb.h" -#include "cartographer/sensor/laser.h" +#include "cartographer/mapping_3d/proto/range_data_inserter_options.pb.h" #include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" namespace cartographer { namespace mapping_3d { -proto::LaserFanInserterOptions CreateLaserFanInserterOptions( +proto::RangeDataInserterOptions CreateRangeDataInserterOptions( common::LuaParameterDictionary* parameter_dictionary); -class LaserFanInserter { +class RangeDataInserter { public: - explicit LaserFanInserter(const proto::LaserFanInserterOptions& options); + explicit RangeDataInserter(const proto::RangeDataInserterOptions& options); - LaserFanInserter(const LaserFanInserter&) = delete; - LaserFanInserter& operator=(const LaserFanInserter&) = delete; + RangeDataInserter(const RangeDataInserter&) = delete; + RangeDataInserter& operator=(const RangeDataInserter&) = delete; - // Inserts 'laser_fan' into 'hybrid_grid'. - void Insert(const sensor::LaserFan& laser_fan, HybridGrid* hybrid_grid) const; + // Inserts 'range_data' into 'hybrid_grid'. + void Insert(const sensor::RangeData& range_data, + HybridGrid* hybrid_grid) const; private: - const proto::LaserFanInserterOptions options_; + const proto::RangeDataInserterOptions options_; const std::vector hit_table_; const std::vector miss_table_; }; @@ -47,4 +48,4 @@ class LaserFanInserter { } // namespace mapping_3d } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_LASER_FAN_INSERTER_H_ +#endif // CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_ diff --git a/cartographer/mapping_3d/laser_fan_inserter_test.cc b/cartographer/mapping_3d/range_data_inserter_test.cc similarity index 76% rename from cartographer/mapping_3d/laser_fan_inserter_test.cc rename to cartographer/mapping_3d/range_data_inserter_test.cc index d333267..c1fb5ef 100644 --- a/cartographer/mapping_3d/laser_fan_inserter_test.cc +++ b/cartographer/mapping_3d/range_data_inserter_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/laser_fan_inserter.h" +#include "cartographer/mapping_3d/range_data_inserter.h" #include #include @@ -26,9 +26,9 @@ namespace cartographer { namespace mapping_3d { namespace { -class LaserFanInserterTest : public ::testing::Test { +class RangeDataInserterTest : public ::testing::Test { protected: - LaserFanInserterTest() + RangeDataInserterTest() : hybrid_grid_(1.f, Eigen::Vector3f(0.5f, 0.5f, 0.5f)) { auto parameter_dictionary = common::MakeDictionary( "return { " @@ -36,18 +36,18 @@ class LaserFanInserterTest : public ::testing::Test { "miss_probability = 0.4, " "num_free_space_voxels = 1000, " "}"); - options_ = CreateLaserFanInserterOptions(parameter_dictionary.get()); - laser_fan_inserter_.reset(new LaserFanInserter(options_)); + options_ = CreateRangeDataInserterOptions(parameter_dictionary.get()); + range_data_inserter_.reset(new RangeDataInserter(options_)); } void InsertPointCloud() { const Eigen::Vector3f origin = Eigen::Vector3f(0.5f, 0.5f, -3.5f); - sensor::PointCloud laser_returns = {{-2.5f, -0.5f, 4.5f}, - {-1.5f, 0.5f, 4.5f}, - {-0.5f, 1.5f, 4.5f}, - {0.5f, 2.5f, 4.5f}}; - laser_fan_inserter_->Insert(sensor::LaserFan{origin, laser_returns, {}}, - &hybrid_grid_); + sensor::PointCloud returns = {{-2.5f, -0.5f, 4.5f}, + {-1.5f, 0.5f, 4.5f}, + {-0.5f, 1.5f, 4.5f}, + {0.5f, 2.5f, 4.5f}}; + range_data_inserter_->Insert(sensor::RangeData{origin, returns, {}}, + &hybrid_grid_); } float GetProbability(float x, float y, float z) const { @@ -60,15 +60,15 @@ class LaserFanInserterTest : public ::testing::Test { hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); } - const proto::LaserFanInserterOptions& options() const { return options_; } + const proto::RangeDataInserterOptions& options() const { return options_; } private: HybridGrid hybrid_grid_; - std::unique_ptr laser_fan_inserter_; - proto::LaserFanInserterOptions options_; + std::unique_ptr range_data_inserter_; + proto::RangeDataInserterOptions options_; }; -TEST_F(LaserFanInserterTest, InsertPointCloud) { +TEST_F(RangeDataInserterTest, InsertPointCloud) { InsertPointCloud(); EXPECT_NEAR(options().miss_probability(), GetProbability(0.5f, 0.5f, -3.5f), 1e-4); @@ -88,7 +88,7 @@ TEST_F(LaserFanInserterTest, InsertPointCloud) { } } -TEST_F(LaserFanInserterTest, ProbabilityProgression) { +TEST_F(RangeDataInserterTest, ProbabilityProgression) { InsertPointCloud(); EXPECT_NEAR(options().hit_probability(), GetProbability(-1.5f, 0.5f, 4.5f), 1e-4); 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 f41487d..a680c8c 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 @@ -22,7 +22,7 @@ #include #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" -#include "cartographer/mapping_3d/laser_fan_inserter.h" +#include "cartographer/mapping_3d/range_data_inserter.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/transform.h" #include "gtest/gtest.h" @@ -51,20 +51,21 @@ CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) { return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); } -mapping_3d::proto::LaserFanInserterOptions CreateLaserFanInserterTestOptions() { +mapping_3d::proto::RangeDataInserterOptions +CreateRangeDataInserterTestOptions() { auto parameter_dictionary = common::MakeDictionary( "return { " "hit_probability = 0.7, " "miss_probability = 0.4, " "num_free_space_voxels = 5, " "}"); - return CreateLaserFanInserterOptions(parameter_dictionary.get()); + return CreateRangeDataInserterOptions(parameter_dictionary.get()); } TEST(FastCorrelativeScanMatcherTest, CorrectPose) { std::mt19937 prng(42); std::uniform_real_distribution distribution(-1.f, 1.f); - LaserFanInserter laser_fan_inserter(CreateLaserFanInserterTestOptions()); + RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions()); constexpr float kMinScore = 0.1f; const auto options = CreateFastCorrelativeScanMatcherTestOptions(5); @@ -89,11 +90,12 @@ 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::LaserFan{expected_pose.translation(), - sensor::TransformPointCloud( - point_cloud, expected_pose), - {}}, - &hybrid_grid); + range_data_inserter.Insert( + sensor::RangeData{ + 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 9cb35a2..050e5a9 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -84,7 +84,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded( } int SparsePoseGraph::AddScan( - common::Time time, const sensor::LaserFan& laser_fan_in_tracking, + common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance, const Submaps* submaps, const Submap* const matching_submap, @@ -97,8 +97,8 @@ int SparsePoseGraph::AddScan( CHECK_LT(j, std::numeric_limits::max()); constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{ - time, sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}}, - sensor::Compress(laser_fan_in_tracking), submaps, + time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}, + sensor::Compress(range_data_in_tracking), submaps, transform::Rigid3d::Identity()}); trajectory_nodes_.push_back( mapping::TrajectoryNode{&constant_node_data_->back(), optimized_pose}); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 4cd6b08..7403e23 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -66,12 +66,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; - // Adds a new 'laser_fan_in_tracking' observation at 'time', and a 'pose' + // Adds a new 'range_data_in_tracking' observation at 'time', and a 'pose' // that will later be optimized. The 'pose' was determined by scan matching // 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::LaserFan& laser_fan_in_tracking, + int AddScan(common::Time time, + const sensor::RangeData& range_data_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 b1a5d13..d047a85 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/cartographer/mapping_3d/submaps.cc @@ -20,7 +20,7 @@ #include #include "cartographer/common/math.h" -#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/range_data.h" #include "glog/logging.h" namespace cartographer { @@ -30,86 +30,77 @@ namespace { constexpr float kSliceHalfHeight = 0.1f; -struct LaserSegment { +struct RaySegment { Eigen::Vector2f from; Eigen::Vector2f to; - bool hit; // Whether there is a laser return at 'to'. + bool hit; // Whether there is a hit at 'to'. }; -// 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::LaserFan& laser_fan, +// We compute a slice around the xy-plane. 'transform' is applied to the rays in +// global map frame to allow choosing an arbitrary slice. +void GenerateSegmentForSlice(const sensor::RangeData& range_data, const transform::Rigid3f& pose, const transform::Rigid3f& transform, - std::vector* segments) { - 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) { - // Laser ray originates below the slice. + std::vector* segments) { + const sensor::RangeData transformed_range_data = + sensor::TransformRangeData(range_data, transform * pose); + segments->reserve(transformed_range_data.returns.size()); + for (const Eigen::Vector3f& hit : transformed_range_data.returns) { + const Eigen::Vector2f origin_xy = transformed_range_data.origin.head<2>(); + const float origin_z = transformed_range_data.origin.z(); + const float delta_z = hit.z() - origin_z; + const Eigen::Vector2f delta_xy = hit.head<2>() - origin_xy; + if (origin_z < -kSliceHalfHeight) { + // Ray originates below the slice. if (hit.z() > kSliceHalfHeight) { - // Laser ray is cutting through the slice. - segments->push_back(LaserSegment{ - laser_origin_xy + - (-kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, - laser_origin_xy + - (kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, + // Ray is cutting through the slice. + segments->push_back(RaySegment{ + origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy, + origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy, false}); } else if (hit.z() > -kSliceHalfHeight) { - // Laser return is inside the slice. - segments->push_back(LaserSegment{ - laser_origin_xy + - (-kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, + // Hit is inside the slice. + segments->push_back(RaySegment{ + origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy, hit.head<2>(), true}); } - } else if (laser_origin_z < kSliceHalfHeight) { - // Laser ray originates inside the slice. + } else if (origin_z < kSliceHalfHeight) { + // Ray originates inside the slice. if (hit.z() < -kSliceHalfHeight) { - // Laser hit is below. - segments->push_back(LaserSegment{ - laser_origin_xy, - laser_origin_xy + - (-kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, + // Hit is below. + segments->push_back(RaySegment{ + origin_xy, + origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy, false}); } else if (hit.z() < kSliceHalfHeight) { // Full ray is inside the slice. - segments->push_back(LaserSegment{laser_origin_xy, hit.head<2>(), true}); + segments->push_back(RaySegment{origin_xy, hit.head<2>(), true}); } else { - // Laser hit is above. - segments->push_back( - LaserSegment{laser_origin_xy, - laser_origin_xy + (kSliceHalfHeight - laser_origin_z) / - delta_z * delta_xy, - false}); + // Hit is above. + segments->push_back(RaySegment{ + origin_xy, + origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy, + false}); } } else { - // Laser ray originates above the slice. + // Ray originates above the slice. if (hit.z() < -kSliceHalfHeight) { - // Laser ray is cutting through the slice. - segments->push_back(LaserSegment{ - laser_origin_xy + - (kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, - laser_origin_xy + - (-kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, + // Ray is cutting through the slice. + segments->push_back(RaySegment{ + origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy, + origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy, false}); } else if (hit.z() < kSliceHalfHeight) { - // Laser return is inside the slice. - segments->push_back( - LaserSegment{laser_origin_xy + (kSliceHalfHeight - laser_origin_z) / - delta_z * delta_xy, - hit.head<2>(), true}); + // Hit is inside the slice. + segments->push_back(RaySegment{ + origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy, + hit.head<2>(), true}); } } } } -void UpdateFreeSpaceFromSegment(const LaserSegment& segment, +void UpdateFreeSpaceFromSegment(const RaySegment& segment, const std::vector& miss_table, mapping_2d::ProbabilityGrid* result) { Eigen::Array2i from = result->limits().GetXYIndexOfCellContainingPoint( @@ -144,17 +135,17 @@ void UpdateFreeSpaceFromSegment(const LaserSegment& segment, } } -void InsertSegmentsIntoProbabilityGrid( - const std::vector& segments, - const std::vector& hit_table, const std::vector& miss_table, - mapping_2d::ProbabilityGrid* result) { +void InsertSegmentsIntoProbabilityGrid(const std::vector& segments, + const std::vector& hit_table, + const std::vector& miss_table, + mapping_2d::ProbabilityGrid* result) { result->StartUpdate(); if (segments.empty()) { return; } Eigen::Vector2f min = segments.front().from; Eigen::Vector2f max = min; - for (const LaserSegment& segment : segments) { + for (const RaySegment& segment : segments) { min = min.cwiseMin(segment.from); min = min.cwiseMin(segment.to); max = max.cwiseMax(segment.from); @@ -166,27 +157,27 @@ void InsertSegmentsIntoProbabilityGrid( result->GrowLimits(min.x(), min.y()); result->GrowLimits(max.x(), max.y()); - for (const LaserSegment& segment : segments) { + for (const RaySegment& segment : segments) { if (segment.hit) { result->ApplyLookupTable(result->limits().GetXYIndexOfCellContainingPoint( segment.to.x(), segment.to.y()), hit_table); } } - for (const LaserSegment& segment : segments) { + for (const RaySegment& segment : segments) { UpdateFreeSpaceFromSegment(segment, miss_table, result); } } -// Filters 'laser_fan', retaining only the returns that have no more than -// 'max_range' distance from the laser origin. Removes misses and reflectivity +// Filters 'range_data', retaining only the returns that have no more than +// 'max_range' distance from the origin. Removes misses and reflectivity // information. -sensor::LaserFan FilterLaserFanByMaxRange(const sensor::LaserFan& laser_fan, - const float max_range) { - sensor::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_); +sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data, + const float max_range) { + sensor::RangeData result{range_data.origin, {}, {}, {}}; + for (const Eigen::Vector3f& hit : range_data.returns) { + if ((hit - range_data.origin).norm() <= max_range) { + result.returns.push_back(hit); } } return result; @@ -195,16 +186,17 @@ sensor::LaserFan FilterLaserFanByMaxRange(const sensor::LaserFan& laser_fan, } // namespace void InsertIntoProbabilityGrid( - const sensor::LaserFan& laser_fan, const transform::Rigid3f& pose, - const float slice_z, const mapping_2d::LaserFanInserter& laser_fan_inserter, + const sensor::RangeData& range_data, const transform::Rigid3f& pose, + const float slice_z, + const mapping_2d::RangeDataInserter& range_data_inserter, mapping_2d::ProbabilityGrid* result) { - std::vector segments; + std::vector segments; GenerateSegmentForSlice( - laser_fan, pose, + range_data, pose, transform::Rigid3f::Translation(-slice_z * Eigen::Vector3f::UnitZ()), &segments); - InsertSegmentsIntoProbabilityGrid(segments, laser_fan_inserter.hit_table(), - laser_fan_inserter.miss_table(), result); + InsertSegmentsIntoProbabilityGrid(segments, range_data_inserter.hit_table(), + range_data_inserter.miss_table(), result); } proto::SubmapsOptions CreateSubmapsOptions( @@ -215,23 +207,24 @@ proto::SubmapsOptions CreateSubmapsOptions( options.set_high_resolution_max_range( parameter_dictionary->GetDouble("high_resolution_max_range")); options.set_low_resolution(parameter_dictionary->GetDouble("low_resolution")); - options.set_num_laser_fans( - parameter_dictionary->GetNonNegativeInt("num_laser_fans")); - *options.mutable_laser_fan_inserter_options() = CreateLaserFanInserterOptions( - parameter_dictionary->GetDictionary("laser_fan_inserter").get()); - CHECK_GT(options.num_laser_fans(), 0); + options.set_num_range_data( + parameter_dictionary->GetNonNegativeInt("num_range_data")); + *options.mutable_range_data_inserter_options() = + CreateRangeDataInserterOptions( + parameter_dictionary->GetDictionary("range_data_inserter").get()); + CHECK_GT(options.num_range_data(), 0); return options; } Submap::Submap(const float high_resolution, const float low_resolution, - const Eigen::Vector3f& origin, const int begin_laser_fan_index) - : mapping::Submap(origin, begin_laser_fan_index), + const Eigen::Vector3f& origin, const int begin_range_data_index) + : mapping::Submap(origin, begin_range_data_index), high_resolution_hybrid_grid(high_resolution, origin), low_resolution_hybrid_grid(low_resolution, origin) {} Submaps::Submaps(const proto::SubmapsOptions& options) : options_(options), - laser_fan_inserter_(options.laser_fan_inserter_options()) { + range_data_inserter_(options.range_data_inserter_options()) { // We always want to have at least one likelihood field which we can return, // and will create it at the origin in absence of a better choice. AddSubmap(Eigen::Vector3f::Zero()); @@ -281,21 +274,22 @@ void Submaps::SubmapToProto( global_submap_pose.translation().z()))); } -void Submaps::InsertLaserFan(const sensor::LaserFan& laser_fan) { - CHECK_LT(num_laser_fans_, std::numeric_limits::max()); - ++num_laser_fans_; +void Submaps::InsertRangeData(const sensor::RangeData& range_data) { + CHECK_LT(num_range_data_, std::numeric_limits::max()); + ++num_range_data_; for (const int index : insertion_indices()) { Submap* submap = submaps_[index].get(); - laser_fan_inserter_.Insert( - FilterLaserFanByMaxRange(laser_fan, - options_.high_resolution_max_range()), + range_data_inserter_.Insert( + FilterRangeDataByMaxRange(range_data, + options_.high_resolution_max_range()), &submap->high_resolution_hybrid_grid); - laser_fan_inserter_.Insert(laser_fan, &submap->low_resolution_hybrid_grid); - submap->end_laser_fan_index = num_laser_fans_; + range_data_inserter_.Insert(range_data, + &submap->low_resolution_hybrid_grid); + submap->end_range_data_index = num_range_data_; } - ++num_laser_fans_in_last_submap_; - if (num_laser_fans_in_last_submap_ == options_.num_laser_fans()) { - AddSubmap(laser_fan.origin); + ++num_range_data_in_last_submap_; + if (num_range_data_in_last_submap_ == options_.num_range_data()) { + AddSubmap(range_data.origin); } } @@ -310,8 +304,8 @@ const HybridGrid& Submaps::low_resolution_matching_grid() const { void Submaps::AddTrajectoryNodeIndex(const int trajectory_node_index) { for (int i = 0; i != size(); ++i) { Submap& submap = *submaps_[i]; - if (submap.end_laser_fan_index == num_laser_fans_ && - submap.begin_laser_fan_index <= num_laser_fans_ - 1) { + if (submap.end_range_data_index == num_range_data_ && + submap.begin_range_data_index <= num_range_data_ - 1) { submap.trajectory_node_indices.push_back(trajectory_node_index); } } @@ -325,9 +319,9 @@ void Submaps::AddSubmap(const Eigen::Vector3f& origin) { } submaps_.emplace_back(new Submap(options_.high_resolution(), options_.low_resolution(), origin, - num_laser_fans_)); + num_range_data_)); LOG(INFO) << "Added submap " << size(); - num_laser_fans_in_last_submap_ = 0; + num_range_data_in_last_submap_ = 0; } std::vector Submaps::AccumulatePixelData( diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h index e1cfe37..6b90727 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submaps.h @@ -25,20 +25,21 @@ #include "cartographer/common/port.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/submaps.h" -#include "cartographer/mapping_2d/laser_fan_inserter.h" #include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/mapping_2d/range_data_inserter.h" #include "cartographer/mapping_3d/hybrid_grid.h" -#include "cartographer/mapping_3d/laser_fan_inserter.h" #include "cartographer/mapping_3d/proto/submaps_options.pb.h" -#include "cartographer/sensor/laser.h" +#include "cartographer/mapping_3d/range_data_inserter.h" +#include "cartographer/sensor/range_data.h" #include "cartographer/transform/transform.h" namespace cartographer { namespace mapping_3d { void InsertIntoProbabilityGrid( - const sensor::LaserFan& laser_fan, const transform::Rigid3f& pose, - const float slice_z, const mapping_2d::LaserFanInserter& laser_fan_inserter, + const sensor::RangeData& range_data, const transform::Rigid3f& pose, + const float slice_z, + const mapping_2d::RangeDataInserter& range_data_inserter, mapping_2d::ProbabilityGrid* result); proto::SubmapsOptions CreateSubmapsOptions( @@ -46,7 +47,7 @@ proto::SubmapsOptions CreateSubmapsOptions( struct Submap : public mapping::Submap { Submap(float high_resolution, float low_resolution, - const Eigen::Vector3f& origin, int begin_laser_fan_index); + const Eigen::Vector3f& origin, int begin_range_data_index); HybridGrid high_resolution_hybrid_grid; HybridGrid low_resolution_hybrid_grid; @@ -69,8 +70,8 @@ class Submaps : public mapping::Submaps { const transform::Rigid3d& global_submap_pose, mapping::proto::SubmapQuery::Response* response) const override; - // Inserts 'laser_fan' into the Submap collection. - void InsertLaserFan(const sensor::LaserFan& laser_fan); + // Inserts 'range_data' into the Submap collection. + void InsertRangeData(const sensor::RangeData& range_data); // Returns the 'high_resolution' HybridGrid to be used for matching. const HybridGrid& high_resolution_matching_grid() const; @@ -110,13 +111,13 @@ class Submaps : public mapping::Submaps { const proto::SubmapsOptions options_; std::vector> submaps_; - LaserFanInserter laser_fan_inserter_; + RangeDataInserter range_data_inserter_; - // Number of LaserFans inserted. - int num_laser_fans_ = 0; + // Number of RangeData inserted. + int num_range_data_ = 0; - // Number of LaserFans inserted since the last Submap was added. - int num_laser_fans_in_last_submap_ = 0; + // Number of RangeData inserted since the last Submap was added. + int num_range_data_in_last_submap_ = 0; }; } // namespace mapping_3d diff --git a/cartographer/sensor/CMakeLists.txt b/cartographer/sensor/CMakeLists.txt index 6fcf2ed..c687dec 100644 --- a/cartographer/sensor/CMakeLists.txt +++ b/cartographer/sensor/CMakeLists.txt @@ -22,9 +22,9 @@ google_test(sensor_compressed_point_cloud_test compressed_point_cloud_test.cc ) -google_test(sensor_laser_test +google_test(sensor_range_data_test SRCS - laser_test.cc + range_data_test.cc ) google_test(sensor_ordered_multi_queue_test diff --git a/cartographer/sensor/data.h b/cartographer/sensor/data.h index 81a0b64..68e2ace 100644 --- a/cartographer/sensor/data.h +++ b/cartographer/sensor/data.h @@ -19,8 +19,8 @@ #include "cartographer/common/time.h" #include "cartographer/kalman_filter/pose_tracker.h" -#include "cartographer/sensor/laser.h" #include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { diff --git a/cartographer/sensor/proto/sensor.proto b/cartographer/sensor/proto/sensor.proto index c57aff2..7b5c04c 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::LaserFan -message LaserFan { +// Proto representation of ::cartographer::sensor::RangeData +message RangeData { optional transform.proto.Vector3f origin = 1; optional PointCloud point_cloud = 2; optional PointCloud missing_echo_point_cloud = 3; diff --git a/cartographer/sensor/laser.cc b/cartographer/sensor/range_data.cc similarity index 62% rename from cartographer/sensor/laser.cc rename to cartographer/sensor/range_data.cc index 94b6e23..14c1955 100644 --- a/cartographer/sensor/laser.cc +++ b/cartographer/sensor/range_data.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/range_data.h" #include "cartographer/sensor/proto/sensor.pb.h" #include "cartographer/transform/transform.h" @@ -72,58 +72,58 @@ PointCloudWithIntensities ToPointCloudWithIntensities( return point_cloud; } -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); - std::copy(laser_fan.reflectivities.begin(), laser_fan.reflectivities.end(), +proto::RangeData ToProto(const RangeData& range_data) { + proto::RangeData proto; + *proto.mutable_origin() = transform::ToProto(range_data.origin); + *proto.mutable_point_cloud() = ToProto(range_data.returns); + *proto.mutable_missing_echo_point_cloud() = ToProto(range_data.misses); + std::copy(range_data.reflectivities.begin(), range_data.reflectivities.end(), RepeatedFieldBackInserter(proto.mutable_reflectivity())); return proto; } -LaserFan FromProto(const proto::LaserFan& proto) { - auto laser_fan = LaserFan{ +RangeData FromProto(const proto::RangeData& proto) { + auto range_data = RangeData{ 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.reflectivities)); - return laser_fan; + std::back_inserter(range_data.reflectivities)); + return range_data; } -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), - laser_fan.reflectivities, +RangeData TransformRangeData(const RangeData& range_data, + const transform::Rigid3f& transform) { + return RangeData{ + transform * range_data.origin, + TransformPointCloud(range_data.returns, transform), + TransformPointCloud(range_data.misses, transform), + range_data.reflectivities, }; } -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)}; +RangeData CropRangeData(const RangeData& range_data, const float min_z, + const float max_z) { + return RangeData{range_data.origin, Crop(range_data.returns, min_z, max_z), + Crop(range_data.misses, min_z, max_z)}; } -CompressedRangeData Compress(const LaserFan& laser_fan) { +CompressedRangeData Compress(const RangeData& range_data) { std::vector new_to_old; CompressedPointCloud compressed_returns = - CompressedPointCloud::CompressAndReturnOrder(laser_fan.returns, + CompressedPointCloud::CompressAndReturnOrder(range_data.returns, &new_to_old); return CompressedRangeData{ - laser_fan.origin, std::move(compressed_returns), - CompressedPointCloud(laser_fan.misses), - ReorderReflectivities(laser_fan.reflectivities, new_to_old)}; + range_data.origin, std::move(compressed_returns), + CompressedPointCloud(range_data.misses), + ReorderReflectivities(range_data.reflectivities, new_to_old)}; } -LaserFan Decompress(const CompressedRangeData& compressed_range_data) { - return LaserFan{compressed_range_data.origin, - compressed_range_data.returns.Decompress(), - compressed_range_data.misses.Decompress(), - compressed_range_data.reflectivities}; +RangeData Decompress(const CompressedRangeData& compressed_range_data) { + return RangeData{compressed_range_data.origin, + compressed_range_data.returns.Decompress(), + compressed_range_data.misses.Decompress(), + compressed_range_data.reflectivities}; } } // namespace sensor diff --git a/cartographer/sensor/laser.h b/cartographer/sensor/range_data.h similarity index 70% rename from cartographer/sensor/laser.h rename to cartographer/sensor/range_data.h index b0fcfb7..9960f25 100644 --- a/cartographer/sensor/laser.h +++ b/cartographer/sensor/range_data.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_SENSOR_LASER_H_ -#define CARTOGRAPHER_SENSOR_LASER_H_ +#ifndef CARTOGRAPHER_SENSOR_RANGE_DATA_H_ +#define CARTOGRAPHER_SENSOR_RANGE_DATA_H_ #include "cartographer/common/port.h" #include "cartographer/sensor/compressed_point_cloud.h" @@ -29,7 +29,7 @@ namespace sensor { // 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 LaserFan { +struct RangeData { Eigen::Vector3f origin; PointCloud returns; PointCloud misses; @@ -49,20 +49,20 @@ PointCloud ToPointCloud(const proto::LaserScan& proto); PointCloudWithIntensities ToPointCloudWithIntensities( const proto::LaserScan& proto); -// Converts 'laser_fan' to a proto::LaserFan. -proto::LaserFan ToProto(const LaserFan& laser_fan); +// Converts 'range_data' to a proto::RangeData. +proto::RangeData ToProto(const RangeData& range_data); -// Converts 'proto' to a LaserFan. -LaserFan FromProto(const proto::LaserFan& proto); +// Converts 'proto' to a RangeData. +RangeData FromProto(const proto::RangeData& proto); -LaserFan TransformLaserFan(const LaserFan& laser_fan, - const transform::Rigid3f& transform); +RangeData TransformRangeData(const RangeData& range_data, + const transform::Rigid3f& transform); -// Crops 'laser_fan' according to the region defined by 'min_z' and 'max_z'. -LaserFan CropLaserFan(const LaserFan& laser_fan, float min_z, float max_z); +// Crops 'range_data' according to the region defined by 'min_z' and 'max_z'. +RangeData CropRangeData(const RangeData& range_data, float min_z, float max_z); -// Like LaserFan but with compressed point clouds. The point order changes -// when converting from LaserFan. +// Like RangeData but with compressed point clouds. The point order changes +// when converting from RangeData. struct CompressedRangeData { Eigen::Vector3f origin; CompressedPointCloud returns; @@ -72,11 +72,11 @@ struct CompressedRangeData { std::vector reflectivities; }; -CompressedRangeData Compress(const LaserFan& laser_fan); +CompressedRangeData Compress(const RangeData& range_data); -LaserFan Decompress(const CompressedRangeData& compressed_range_Data); +RangeData Decompress(const CompressedRangeData& compressed_range_Data); } // namespace sensor } // namespace cartographer -#endif // CARTOGRAPHER_SENSOR_LASER_H_ +#endif // CARTOGRAPHER_SENSOR_RANGE_DATA_H_ diff --git a/cartographer/sensor/laser_test.cc b/cartographer/sensor/range_data_test.cc similarity index 96% rename from cartographer/sensor/laser_test.cc rename to cartographer/sensor/range_data_test.cc index e1bfd6d..f439f1c 100644 --- a/cartographer/sensor/laser_test.cc +++ b/cartographer/sensor/range_data_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/range_data.h" #include #include @@ -81,14 +81,14 @@ MATCHER_P(PairApproximatelyEquals, expected, arg.second == expected.second; } -TEST(LaserTest, Compression) { - const LaserFan laser_fan = { +TEST(RangeDataTest, Compression) { + const RangeData range_data = { 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}}; - const LaserFan actual = Decompress(Compress(laser_fan)); + const RangeData actual = Decompress(Compress(range_data)); 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/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index 3e1e33e..f0127d9 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -59,9 +59,9 @@ TRAJECTORY_BUILDER_2D = { submaps = { resolution = 0.05, half_length = 200., - num_laser_fans = 90, + num_range_data = 90, output_debug_images = false, - laser_fan_inserter = { + range_data_inserter = { insert_free_space = true, hit_probability = 0.55, miss_probability = 0.49, diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index bd795ce..fb74794 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -55,8 +55,8 @@ TRAJECTORY_BUILDER_3D = { high_resolution = 0.10, high_resolution_max_range = 20., low_resolution = 0.45, - num_laser_fans = 160, - laser_fan_inserter = { + num_range_data = 160, + range_data_inserter = { hit_probability = 0.55, miss_probability = 0.49, num_free_space_voxels = 2, diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 3100096..4e22ea9 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -164,22 +164,6 @@ cartographer.common.proto.CeresSolverOptions ceres_solver_options Not yet documented. -cartographer.mapping_2d.proto.LaserFanInserterOptions -===================================================== - -double hit_probability - Probability change for a hit (this will be converted to odds and therefore - must be greater than 0.5). - -double miss_probability - Probability change for a miss (this will be converted to odds and therefore - must be less than 0.5). - -bool insert_free_space - If 'false', free space will not change the probabilities in the occupancy - grid. - - cartographer.mapping_2d.proto.LocalTrajectoryBuilderOptions =========================================================== @@ -200,7 +184,7 @@ float laser_missing_echo_ray_length empty space. float laser_voxel_filter_size - Voxel filter that gets applied to the horizontal laser immediately after + Voxel filter that gets applied to the range data immediately after cropping. bool use_online_correlative_scan_matching @@ -237,6 +221,22 @@ bool use_imu_data True if IMU data should be expected and used. +cartographer.mapping_2d.proto.RangeDataInserterOptions +====================================================== + +double hit_probability + Probability change for a hit (this will be converted to odds and therefore + must be greater than 0.5). + +double miss_probability + Probability change for a miss (this will be converted to odds and therefore + must be less than 0.5). + +bool insert_free_space + If 'false', free space will not change the probabilities in the occupancy + grid. + + cartographer.mapping_2d.proto.SubmapsOptions ============================================ @@ -246,7 +246,7 @@ double resolution double half_length Half the width/height of each submap, its "radius". -int32 num_laser_fans +int32 num_range_data Number of scans before adding a new submap. Each submap will get twice the number of scans inserted: First for initialization without being matched against, then while being matched. @@ -254,7 +254,7 @@ int32 num_laser_fans bool output_debug_images If enabled, submap%d.png images are written for debugging. -cartographer.mapping_2d.proto.LaserFanInserterOptions laser_fan_inserter_options +cartographer.mapping_2d.proto.RangeDataInserterOptions range_data_inserter_options Not yet documented. @@ -334,22 +334,6 @@ double odometer_rotational_variance Not yet documented. -cartographer.mapping_3d.proto.LaserFanInserterOptions -===================================================== - -double hit_probability - Probability change for a hit (this will be converted to odds and therefore - must be greater than 0.5). - -double miss_probability - Probability change for a miss (this will be converted to odds and therefore - must be less than 0.5). - -int32 num_free_space_voxels - Up to how many free space voxels are updated for scan matching. - 0 disables free space. - - cartographer.mapping_3d.proto.LocalTrajectoryBuilderOptions =========================================================== @@ -392,6 +376,22 @@ double odometry_rotation_weight Not yet documented. +cartographer.mapping_3d.proto.RangeDataInserterOptions +====================================================== + +double hit_probability + Probability change for a hit (this will be converted to odds and therefore + must be greater than 0.5). + +double miss_probability + Probability change for a miss (this will be converted to odds and therefore + must be less than 0.5). + +int32 num_free_space_voxels + Up to how many free space voxels are updated for scan matching. + 0 disables free space. + + cartographer.mapping_3d.proto.SubmapsOptions ============================================ @@ -407,12 +407,12 @@ double low_resolution Resolution of the 'low_resolution' version of the map in meters used for local SLAM only. -int32 num_laser_fans +int32 num_range_data Number of scans before adding a new submap. Each submap will get twice the number of scans inserted: First for initialization without being matched against, then while being matched. -cartographer.mapping_3d.proto.LaserFanInserterOptions laser_fan_inserter_options +cartographer.mapping_3d.proto.RangeDataInserterOptions range_data_inserter_options Not yet documented.