Rename LaserFan to RangeData. (#224)

LaserFan is misleading since range data can come from
various sensor types.
master
Wolfgang Hess 2017-03-23 14:56:18 +01:00 committed by GitHub
parent a2abe45542
commit 4fa190d316
56 changed files with 545 additions and 541 deletions

View File

@ -24,8 +24,8 @@
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {

View File

@ -27,7 +27,7 @@
#include "cartographer/mapping_2d/global_trajectory_builder.h" #include "cartographer/mapping_2d/global_trajectory_builder.h"
#include "cartographer/mapping_3d/global_trajectory_builder.h" #include "cartographer/mapping_3d/global_trajectory_builder.h"
#include "cartographer/mapping_3d/local_trajectory_builder_options.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/sensor/voxel_filter.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
@ -141,7 +141,8 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
" submaps in this trajectory."; " 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<transform::Rigid3d> submap_transforms = const std::vector<transform::Rigid3d> submap_transforms =
sparse_pose_graph_->GetSubmapTransforms(*submaps); sparse_pose_graph_->GetSubmapTransforms(*submaps);
CHECK_EQ(submap_transforms.size(), submaps->size()); CHECK_EQ(submap_transforms.size(), submaps->size());

View File

@ -64,14 +64,14 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) {
} }
// An individual submap, which has an initial position 'origin', keeps track of // 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 // 'finished_probability_grid' to be used for loop closing once the map no
// longer changes. // longer changes.
struct Submap { struct Submap {
Submap(const Eigen::Vector3f& origin, int begin_laser_fan_index) Submap(const Eigen::Vector3f& origin, int begin_range_data_index)
: origin(origin), : origin(origin),
begin_laser_fan_index(begin_laser_fan_index), begin_range_data_index(begin_range_data_index),
end_laser_fan_index(begin_laser_fan_index) {} end_range_data_index(begin_range_data_index) {}
transform::Rigid3d local_pose() const { transform::Rigid3d local_pose() const {
return transform::Rigid3d::Translation(origin.cast<double>()); return transform::Rigid3d::Translation(origin.cast<double>());
@ -80,14 +80,14 @@ struct Submap {
// Origin of this submap. // Origin of this submap.
Eigen::Vector3f origin; Eigen::Vector3f origin;
// This Submap contains LaserFans with indices in the range // This Submap contains RangeData with indices in the range
// ['begin_laser_fan_index', 'end_laser_fan_index'). // ['begin_range_data_index', 'end_range_data_index').
int begin_laser_fan_index; int begin_range_data_index;
int end_laser_fan_index; int end_range_data_index;
// The 'finished_probability_grid' when this submap is finished and will not // The 'finished_probability_grid' when this submap is finished and will not
// change anymore. Otherwise, this is nullptr and the next call to // 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; const mapping_2d::ProbabilityGrid* finished_probability_grid = nullptr;
}; };

View File

@ -26,8 +26,8 @@
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
#include "cartographer/sensor/data.h" #include "cartographer/sensor/data.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {

View File

@ -36,9 +36,9 @@ class TrajectoryConnectivityTest : public ::testing::Test {
return { return {
resolution = 0.05, resolution = 0.05,
half_length = 10., half_length = 10.,
num_laser_fans = 10, num_range_data = 10,
output_debug_images = false, output_debug_images = false,
laser_fan_inserter = { range_data_inserter = {
insert_free_space = true, insert_free_space = true,
hit_probability = 0.53, hit_probability = 0.53,
miss_probability = 0.495, miss_probability = 0.495,

View File

@ -23,7 +23,7 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/proto/trajectory.pb.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" #include "cartographer/transform/rigid_transform.h"
namespace cartographer { namespace cartographer {
@ -35,8 +35,8 @@ struct TrajectoryNode {
struct ConstantData { struct ConstantData {
common::Time time; common::Time time;
// LaserFan in 'pose' frame. Only used in the 2D case. // Range data in 'pose' frame. Only used in the 2D case.
sensor::LaserFan laser_fan_2d; sensor::RangeData range_data_2d;
// Range data in 'pose' frame. Only used in the 3D case. // Range data in 'pose' frame. Only used in the 3D case.
sensor::CompressedRangeData range_data_3d; sensor::CompressedRangeData range_data_3d;

View File

@ -14,9 +14,9 @@
add_subdirectory("scan_matching") add_subdirectory("scan_matching")
google_test(mapping_2d_laser_fan_inserter_test google_test(mapping_2d_range_data_inserter_test
SRCS SRCS
laser_fan_inserter_test.cc range_data_inserter_test.cc
) )
google_test(mapping_2d_map_limits_test google_test(mapping_2d_map_limits_test

View File

@ -36,12 +36,12 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
const common::Time time, const Eigen::Vector3f& origin, const common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) { const sensor::PointCloud& ranges) {
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result = std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
local_trajectory_builder_.AddHorizontalLaserFan( local_trajectory_builder_.AddHorizontalRangeData(
time, sensor::LaserFan{origin, ranges, {}, {}}); time, sensor::RangeData{origin, ranges, {}, {}});
if (insertion_result != nullptr) { if (insertion_result != nullptr) {
sparse_pose_graph_->AddScan( sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->tracking_to_tracking_2d, insertion_result->time, insertion_result->tracking_to_tracking_2d,
insertion_result->laser_fan_in_tracking_2d, insertion_result->range_data_in_tracking_2d,
insertion_result->pose_estimate_2d, insertion_result->pose_estimate_2d,
kalman_filter::Project2D(insertion_result->covariance_estimate), kalman_filter::Project2D(insertion_result->covariance_estimate),
insertion_result->submaps, insertion_result->matching_submap, insertion_result->submaps, insertion_result->matching_submap,

View File

@ -19,7 +19,7 @@
#include <limits> #include <limits>
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/sensor/laser.h" #include "cartographer/sensor/range_data.h"
namespace cartographer { namespace cartographer {
namespace mapping_2d { namespace mapping_2d {
@ -78,28 +78,28 @@ LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
const Submaps* LocalTrajectoryBuilder::submaps() const { return &submaps_; } const Submaps* LocalTrajectoryBuilder::submaps() const { return &submaps_; }
sensor::LaserFan LocalTrajectoryBuilder::TransformAndFilterLaserFan( sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData(
const transform::Rigid3f& tracking_to_tracking_2d, 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 // Drop any returns below the minimum range and convert returns beyond the
// maximum range into misses. // maximum range into misses.
sensor::LaserFan returns_and_misses{laser_fan.origin, {}, {}, {}}; sensor::RangeData returns_and_misses{range_data.origin, {}, {}, {}};
for (const Eigen::Vector3f& return_ : laser_fan.returns) { for (const Eigen::Vector3f& hit : range_data.returns) {
const float range = (return_ - laser_fan.origin).norm(); const float range = (hit - range_data.origin).norm();
if (range >= options_.laser_min_range()) { if (range >= options_.laser_min_range()) {
if (range <= options_.laser_max_range()) { if (range <= options_.laser_max_range()) {
returns_and_misses.returns.push_back(return_); returns_and_misses.returns.push_back(hit);
} else { } else {
returns_and_misses.misses.push_back( returns_and_misses.misses.push_back(
laser_fan.origin + options_.laser_missing_echo_ray_length() * range_data.origin + options_.laser_missing_echo_ray_length() *
(return_ - laser_fan.origin).normalized()); (hit - range_data.origin).normalized());
} }
} }
} }
const sensor::LaserFan cropped = sensor::CropLaserFan( const sensor::RangeData cropped = sensor::CropRangeData(
sensor::TransformLaserFan(returns_and_misses, tracking_to_tracking_2d), sensor::TransformRangeData(returns_and_misses, tracking_to_tracking_2d),
options_.laser_min_z(), options_.laser_max_z()); options_.laser_min_z(), options_.laser_max_z());
return sensor::LaserFan{ return sensor::RangeData{
cropped.origin, cropped.origin,
sensor::VoxelFiltered(cropped.returns, sensor::VoxelFiltered(cropped.returns,
options_.laser_voxel_filter_size()), options_.laser_voxel_filter_size()),
@ -110,7 +110,7 @@ sensor::LaserFan LocalTrajectoryBuilder::TransformAndFilterLaserFan(
void LocalTrajectoryBuilder::ScanMatch( void LocalTrajectoryBuilder::ScanMatch(
common::Time time, const transform::Rigid3d& pose_prediction, common::Time time, const transform::Rigid3d& pose_prediction,
const transform::Rigid3d& tracking_to_tracking_2d, 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, transform::Rigid3d* pose_observation,
kalman_filter::PoseCovariance* covariance_observation) { kalman_filter::PoseCovariance* covariance_observation) {
const ProbabilityGrid& probability_grid = const ProbabilityGrid& probability_grid =
@ -123,7 +123,7 @@ void LocalTrajectoryBuilder::ScanMatch(
sensor::AdaptiveVoxelFilter adaptive_voxel_filter( sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.adaptive_voxel_filter_options()); options_.adaptive_voxel_filter_options());
const sensor::PointCloud filtered_point_cloud_in_tracking_2d = 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()) { if (options_.use_online_correlative_scan_matching()) {
real_time_correlative_scan_matcher_.Match( real_time_correlative_scan_matcher_.Match(
pose_prediction_2d, filtered_point_cloud_in_tracking_2d, pose_prediction_2d, filtered_point_cloud_in_tracking_2d,
@ -149,8 +149,8 @@ void LocalTrajectoryBuilder::ScanMatch(
} }
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
LocalTrajectoryBuilder::AddHorizontalLaserFan( LocalTrajectoryBuilder::AddHorizontalRangeData(
const common::Time time, const sensor::LaserFan& laser_fan) { const common::Time time, const sensor::RangeData& range_data) {
// Initialize IMU tracker now if we do not ever use an IMU. // Initialize IMU tracker now if we do not ever use an IMU.
if (!options_.use_imu_data()) { if (!options_.use_imu_data()) {
InitializeImuTracker(time); InitializeImuTracker(time);
@ -177,17 +177,19 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
-transform::GetYaw(pose_prediction), Eigen::Vector3d::UnitZ())) * -transform::GetYaw(pose_prediction), Eigen::Vector3d::UnitZ())) *
pose_prediction.rotation()); pose_prediction.rotation());
const sensor::LaserFan laser_fan_in_tracking_2d = TransformAndFilterLaserFan( const sensor::RangeData range_data_in_tracking_2d =
tracking_to_tracking_2d.cast<float>(), laser_fan); TransformAndFilterRangeData(tracking_to_tracking_2d.cast<float>(),
range_data);
if (laser_fan_in_tracking_2d.returns.empty()) { if (range_data_in_tracking_2d.returns.empty()) {
LOG(WARNING) << "Dropped empty horizontal laser point cloud."; LOG(WARNING) << "Dropped empty horizontal range data.";
return nullptr; return nullptr;
} }
kalman_filter::PoseCovariance covariance_observation; kalman_filter::PoseCovariance covariance_observation;
ScanMatch(time, pose_prediction, tracking_to_tracking_2d, 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(); odometry_correction_ = transform::Rigid3d::Identity();
if (!odometry_state_tracker_.empty()) { if (!odometry_state_tracker_.empty()) {
// We add an odometry state, so that the correction from the scan matching // 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(); pose_estimate_ * tracking_to_tracking_2d.inverse();
last_pose_estimate_ = { last_pose_estimate_ = {
time, 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<float>())}; tracking_2d_to_map.cast<float>())};
const transform::Rigid2d pose_estimate_2d = const transform::Rigid2d pose_estimate_2d =
@ -232,13 +234,13 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
for (int insertion_index : submaps_.insertion_indices()) { for (int insertion_index : submaps_.insertion_indices()) {
insertion_submaps.push_back(submaps_.Get(insertion_index)); insertion_submaps.push_back(submaps_.Get(insertion_index));
} }
submaps_.InsertLaserFan( submaps_.InsertRangeData(
TransformLaserFan(laser_fan_in_tracking_2d, TransformRangeData(range_data_in_tracking_2d,
transform::Embed3D(pose_estimate_2d.cast<float>()))); transform::Embed3D(pose_estimate_2d.cast<float>())));
return common::make_unique<InsertionResult>(InsertionResult{ return common::make_unique<InsertionResult>(InsertionResult{
time, &submaps_, matching_submap, insertion_submaps, time, &submaps_, matching_submap, insertion_submaps,
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}); pose_estimate_2d, covariance_observation});
} }

View File

@ -50,7 +50,7 @@ class LocalTrajectoryBuilder {
std::vector<const mapping::Submap*> insertion_submaps; std::vector<const mapping::Submap*> insertion_submaps;
transform::Rigid3d tracking_to_tracking_2d; transform::Rigid3d tracking_to_tracking_2d;
transform::Rigid3d tracking_2d_to_map; 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; transform::Rigid2d pose_estimate_2d;
kalman_filter::PoseCovariance covariance_estimate; kalman_filter::PoseCovariance covariance_estimate;
}; };
@ -64,8 +64,8 @@ class LocalTrajectoryBuilder {
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate() const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
const; const;
std::unique_ptr<InsertionResult> AddHorizontalLaserFan( std::unique_ptr<InsertionResult> AddHorizontalRangeData(
common::Time, const sensor::LaserFan& laser_fan); common::Time, const sensor::RangeData& range_data);
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity); const Eigen::Vector3d& angular_velocity);
void AddOdometerData(common::Time time, const transform::Rigid3d& pose); void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
@ -73,15 +73,15 @@ class LocalTrajectoryBuilder {
const Submaps* submaps() const; const Submaps* submaps() const;
private: private:
sensor::LaserFan TransformAndFilterLaserFan( sensor::RangeData TransformAndFilterRangeData(
const transform::Rigid3f& tracking_to_tracking_2d, 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. // 'pose_observation' and 'covariance_observation' with the result.
void ScanMatch(common::Time time, const transform::Rigid3d& pose_prediction, void ScanMatch(common::Time time, const transform::Rigid3d& pose_prediction,
const transform::Rigid3d& tracking_to_tracking_2d, 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, transform::Rigid3d* pose_observation,
kalman_filter::PoseCovariance* covariance_observation); kalman_filter::PoseCovariance* covariance_observation);

View File

@ -26,8 +26,8 @@
#include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping/trajectory_node.h"
#include "cartographer/mapping_2d/proto/map_limits.pb.h" #include "cartographer/mapping_2d/proto/map_limits.pb.h"
#include "cartographer/mapping_2d/xy_index.h" #include "cartographer/mapping_2d/xy_index.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -85,7 +85,7 @@ class MapLimits {
} }
// Computes MapLimits that contain the origin, and all laser rays (both // 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( static MapLimits ComputeMapLimits(
const double resolution, const double resolution,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes) { const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
@ -108,20 +108,20 @@ class MapLimits {
for (const auto& node : trajectory_nodes) { for (const auto& node : trajectory_nodes) {
const auto& data = *node.constant_data; const auto& data = *node.constant_data;
if (!data.range_data_3d.returns.empty()) { 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<float>()); Decompress(data.range_data_3d), node.pose.cast<float>());
bounding_box.extend(laser_fan.origin.head<2>()); bounding_box.extend(range_data.origin.head<2>());
for (const Eigen::Vector3f& hit : laser_fan.returns) { for (const Eigen::Vector3f& hit : range_data.returns) {
bounding_box.extend(hit.head<2>()); bounding_box.extend(hit.head<2>());
} }
} else { } else {
const sensor::LaserFan laser_fan = sensor::TransformLaserFan( const sensor::RangeData range_data = sensor::TransformRangeData(
data.laser_fan_2d, node.pose.cast<float>()); data.range_data_2d, node.pose.cast<float>());
bounding_box.extend(laser_fan.origin.head<2>()); bounding_box.extend(range_data.origin.head<2>());
for (const Eigen::Vector3f& hit : laser_fan.returns) { for (const Eigen::Vector3f& hit : range_data.returns) {
bounding_box.extend(hit.head<2>()); 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>()); bounding_box.extend(miss.head<2>());
} }
} }

View File

@ -65,11 +65,11 @@ TEST(MapLimitsTest, ConstructAndGet) {
TEST(MapLimitsTest, ComputeMapLimits) { TEST(MapLimitsTest, ComputeMapLimits) {
const mapping::TrajectoryNode::ConstantData constant_data{ const mapping::TrajectoryNode::ConstantData constant_data{
common::FromUniversal(52), common::FromUniversal(52),
sensor::LaserFan{ sensor::RangeData{
Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(),
{Eigen::Vector3f(-30.f, 1.f, 0.f), Eigen::Vector3f(50.f, -10.f, 0.f)}, {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()}; transform::Rigid3d::Identity()};
const mapping::TrajectoryNode trajectory_node{&constant_data, const mapping::TrajectoryNode trajectory_node{&constant_data,
transform::Rigid3d::Identity()}; transform::Rigid3d::Identity()};

View File

@ -33,7 +33,7 @@ message LocalTrajectoryBuilderOptions {
// empty space. // empty space.
optional float laser_missing_echo_ray_length = 16; 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. // cropping.
optional float laser_voxel_filter_size = 3; optional float laser_voxel_filter_size = 3;

View File

@ -20,7 +20,8 @@ package cartographer.mapping_2d.proto;
message ProbabilityGrid { message ProbabilityGrid {
optional MapLimits limits = 1; 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 cells = 2;
repeated int32 update_indices = 3; repeated int32 update_indices = 3;
optional int32 max_x = 4; optional int32 max_x = 4;

View File

@ -16,7 +16,7 @@ syntax = "proto2";
package cartographer.mapping_2d.proto; package cartographer.mapping_2d.proto;
message LaserFanInserterOptions { message RangeDataInserterOptions {
// Probability change for a hit (this will be converted to odds and therefore // Probability change for a hit (this will be converted to odds and therefore
// must be greater than 0.5). // must be greater than 0.5).
optional double hit_probability = 1; optional double hit_probability = 1;

View File

@ -14,7 +14,7 @@
syntax = "proto2"; 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; 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 before adding a new submap. Each submap will get twice the
// number of scans inserted: First for initialization without being matched // number of scans inserted: First for initialization without being matched
// against, then while 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. // If enabled, submap%d.png images are written for debugging.
optional bool output_debug_images = 4; optional bool output_debug_images = 4;
optional LaserFanInserterOptions laser_fan_inserter_options = 5; optional RangeDataInserterOptions range_data_inserter_options = 5;
} }

View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_2d/laser_fan_inserter.h" #include "cartographer/mapping_2d/range_data_inserter.h"
#include <cstdlib> #include <cstdlib>
@ -27,9 +27,9 @@
namespace cartographer { namespace cartographer {
namespace mapping_2d { namespace mapping_2d {
proto::LaserFanInserterOptions CreateLaserFanInserterOptions( proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
common::LuaParameterDictionary* const parameter_dictionary) { common::LuaParameterDictionary* const parameter_dictionary) {
proto::LaserFanInserterOptions options; proto::RangeDataInserterOptions options;
options.set_hit_probability( options.set_hit_probability(
parameter_dictionary->GetDouble("hit_probability")); parameter_dictionary->GetDouble("hit_probability"));
options.set_miss_probability( options.set_miss_probability(
@ -43,21 +43,21 @@ proto::LaserFanInserterOptions CreateLaserFanInserterOptions(
return options; return options;
} }
LaserFanInserter::LaserFanInserter( RangeDataInserter::RangeDataInserter(
const proto::LaserFanInserterOptions& options) const proto::RangeDataInserterOptions& options)
: options_(options), : options_(options),
hit_table_(mapping::ComputeLookupTableToApplyOdds( hit_table_(mapping::ComputeLookupTableToApplyOdds(
mapping::Odds(options.hit_probability()))), mapping::Odds(options.hit_probability()))),
miss_table_(mapping::ComputeLookupTableToApplyOdds( miss_table_(mapping::ComputeLookupTableToApplyOdds(
mapping::Odds(options.miss_probability()))) {} mapping::Odds(options.miss_probability()))) {}
void LaserFanInserter::Insert(const sensor::LaserFan& laser_fan, void RangeDataInserter::Insert(const sensor::RangeData& range_data,
ProbabilityGrid* const probability_grid) const { ProbabilityGrid* const probability_grid) const {
CHECK_NOTNULL(probability_grid)->StartUpdate(); CHECK_NOTNULL(probability_grid)->StartUpdate();
// By not starting a new update after hits are inserted, we give hits priority // 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). // (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) { [this, &probability_grid](const Eigen::Array2i& hit) {
probability_grid->ApplyLookupTable(hit, hit_table_); probability_grid->ApplyLookupTable(hit, hit_table_);
}, },

View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_2D_LASER_FAN_INSERTER_H_ #ifndef CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_H_
#define CARTOGRAPHER_MAPPING_2D_LASER_FAN_INSERTER_H_ #define CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_H_
#include <utility> #include <utility>
#include <vector> #include <vector>
@ -23,33 +23,33 @@
#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/mapping_2d/probability_grid.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/mapping_2d/xy_index.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
namespace cartographer { namespace cartographer {
namespace mapping_2d { namespace mapping_2d {
proto::LaserFanInserterOptions CreateLaserFanInserterOptions( proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
common::LuaParameterDictionary* parameter_dictionary); common::LuaParameterDictionary* parameter_dictionary);
class LaserFanInserter { class RangeDataInserter {
public: public:
explicit LaserFanInserter(const proto::LaserFanInserterOptions& options); explicit RangeDataInserter(const proto::RangeDataInserterOptions& options);
LaserFanInserter(const LaserFanInserter&) = delete; RangeDataInserter(const RangeDataInserter&) = delete;
LaserFanInserter& operator=(const LaserFanInserter&) = delete; RangeDataInserter& operator=(const RangeDataInserter&) = delete;
// Inserts 'laser_fan' into 'probability_grid'. // Inserts 'range_data' into 'probability_grid'.
void Insert(const sensor::LaserFan& laser_fan, void Insert(const sensor::RangeData& range_data,
ProbabilityGrid* probability_grid) const; ProbabilityGrid* probability_grid) const;
const std::vector<uint16>& hit_table() const { return hit_table_; } const std::vector<uint16>& hit_table() const { return hit_table_; }
const std::vector<uint16>& miss_table() const { return miss_table_; } const std::vector<uint16>& miss_table() const { return miss_table_; }
private: private:
const proto::LaserFanInserterOptions options_; const proto::RangeDataInserterOptions options_;
const std::vector<uint16> hit_table_; const std::vector<uint16> hit_table_;
const std::vector<uint16> miss_table_; const std::vector<uint16> miss_table_;
}; };
@ -57,4 +57,4 @@ class LaserFanInserter {
} // namespace mapping_2d } // namespace mapping_2d
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_LASER_FAN_INSERTER_H_ #endif // CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_H_

View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_2d/laser_fan_inserter.h" #include "cartographer/mapping_2d/range_data_inserter.h"
#include <memory> #include <memory>
@ -28,9 +28,9 @@ namespace cartographer {
namespace mapping_2d { namespace mapping_2d {
namespace { namespace {
class LaserFanInserterTest : public ::testing::Test { class RangeDataInserterTest : public ::testing::Test {
protected: protected:
LaserFanInserterTest() RangeDataInserterTest()
: probability_grid_( : probability_grid_(
MapLimits(1., Eigen::Vector2d(1., 5.), CellLimits(5, 5))) { MapLimits(1., Eigen::Vector2d(1., 5.), CellLimits(5, 5))) {
auto parameter_dictionary = common::MakeDictionary( auto parameter_dictionary = common::MakeDictionary(
@ -39,28 +39,28 @@ class LaserFanInserterTest : public ::testing::Test {
"hit_probability = 0.7, " "hit_probability = 0.7, "
"miss_probability = 0.4, " "miss_probability = 0.4, "
"}"); "}");
options_ = CreateLaserFanInserterOptions(parameter_dictionary.get()); options_ = CreateRangeDataInserterOptions(parameter_dictionary.get());
laser_fan_inserter_ = common::make_unique<LaserFanInserter>(options_); range_data_inserter_ = common::make_unique<RangeDataInserter>(options_);
} }
void InsertPointCloud() { void InsertPointCloud() {
sensor::LaserFan laser_fan; sensor::RangeData range_data;
laser_fan.returns.emplace_back(-3.5, 0.5, 0.f); range_data.returns.emplace_back(-3.5, 0.5, 0.f);
laser_fan.returns.emplace_back(-2.5, 1.5, 0.f); range_data.returns.emplace_back(-2.5, 1.5, 0.f);
laser_fan.returns.emplace_back(-1.5, 2.5, 0.f); range_data.returns.emplace_back(-1.5, 2.5, 0.f);
laser_fan.returns.emplace_back(-0.5, 3.5, 0.f); range_data.returns.emplace_back(-0.5, 3.5, 0.f);
laser_fan.origin.x() = -0.5; range_data.origin.x() = -0.5;
laser_fan.origin.y() = 0.5; range_data.origin.y() = 0.5;
probability_grid_.StartUpdate(); probability_grid_.StartUpdate();
laser_fan_inserter_->Insert(laser_fan, &probability_grid_); range_data_inserter_->Insert(range_data, &probability_grid_);
} }
ProbabilityGrid probability_grid_; ProbabilityGrid probability_grid_;
std::unique_ptr<LaserFanInserter> laser_fan_inserter_; std::unique_ptr<RangeDataInserter> range_data_inserter_;
proto::LaserFanInserterOptions options_; proto::RangeDataInserterOptions options_;
}; };
TEST_F(LaserFanInserterTest, InsertPointCloud) { TEST_F(RangeDataInserterTest, InsertPointCloud) {
InsertPointCloud(); InsertPointCloud();
EXPECT_NEAR(1., probability_grid_.limits().max().x(), 1e-9); 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(); InsertPointCloud();
EXPECT_NEAR(options_.hit_probability(), EXPECT_NEAR(options_.hit_probability(),
probability_grid_.GetProbability(-3.5, 0.5), 1e-4); probability_grid_.GetProbability(-3.5, 0.5), 1e-4);

View File

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

View File

@ -21,16 +21,16 @@
#include "cartographer/mapping_2d/map_limits.h" #include "cartographer/mapping_2d/map_limits.h"
#include "cartographer/mapping_2d/xy_index.h" #include "cartographer/mapping_2d/xy_index.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
namespace cartographer { namespace cartographer {
namespace mapping_2d { 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. // 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<void(const Eigen::Array2i&)>& hit_visitor, const std::function<void(const Eigen::Array2i&)>& hit_visitor,
const std::function<void(const Eigen::Array2i&)>& miss_visitor); const std::function<void(const Eigen::Array2i&)>& miss_visitor);

View File

@ -23,8 +23,8 @@
#include <string> #include <string>
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #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/probability_grid.h"
#include "cartographer/mapping_2d/range_data_inserter.h"
#include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/rigid_transform_test_helpers.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
@ -118,20 +118,21 @@ CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) {
return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
} }
mapping_2d::proto::LaserFanInserterOptions CreateLaserFanInserterTestOptions() { mapping_2d::proto::RangeDataInserterOptions
CreateRangeDataInserterTestOptions() {
auto parameter_dictionary = common::MakeDictionary(R"text( auto parameter_dictionary = common::MakeDictionary(R"text(
return { return {
insert_free_space = true, insert_free_space = true,
hit_probability = 0.7, hit_probability = 0.7,
miss_probability = 0.4, miss_probability = 0.4,
})text"); })text");
return mapping_2d::CreateLaserFanInserterOptions(parameter_dictionary.get()); return mapping_2d::CreateRangeDataInserterOptions(parameter_dictionary.get());
} }
TEST(FastCorrelativeScanMatcherTest, CorrectPose) { TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
std::mt19937 prng(42); std::mt19937 prng(42);
std::uniform_real_distribution<float> distribution(-1.f, 1.f); std::uniform_real_distribution<float> distribution(-1.f, 1.f);
LaserFanInserter laser_fan_inserter(CreateLaserFanInserterTestOptions()); RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions());
constexpr float kMinScore = 0.1f; constexpr float kMinScore = 0.1f;
const auto options = CreateFastCorrelativeScanMatcherTestOptions(3); const auto options = CreateFastCorrelativeScanMatcherTestOptions(3);
@ -151,8 +152,8 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
ProbabilityGrid probability_grid( ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
probability_grid.StartUpdate(); probability_grid.StartUpdate();
laser_fan_inserter.Insert( range_data_inserter.Insert(
sensor::LaserFan{ sensor::RangeData{
Eigen::Vector3f(expected_pose.translation().x(), Eigen::Vector3f(expected_pose.translation().x(),
expected_pose.translation().y(), 0.f), expected_pose.translation().y(), 0.f),
sensor::TransformPointCloud( sensor::TransformPointCloud(
@ -178,7 +179,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
std::mt19937 prng(42); std::mt19937 prng(42);
std::uniform_real_distribution<float> distribution(-1.f, 1.f); std::uniform_real_distribution<float> distribution(-1.f, 1.f);
LaserFanInserter laser_fan_inserter(CreateLaserFanInserterTestOptions()); RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions());
constexpr float kMinScore = 0.1f; constexpr float kMinScore = 0.1f;
const auto options = CreateFastCorrelativeScanMatcherTestOptions(6); const auto options = CreateFastCorrelativeScanMatcherTestOptions(6);
@ -204,8 +205,8 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
ProbabilityGrid probability_grid( ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
probability_grid.StartUpdate(); probability_grid.StartUpdate();
laser_fan_inserter.Insert( range_data_inserter.Insert(
sensor::LaserFan{ sensor::RangeData{
transform::Embed3D(expected_pose * perturbation).translation(), transform::Embed3D(expected_pose * perturbation).translation(),
sensor::TransformPointCloud(point_cloud, sensor::TransformPointCloud(point_cloud,
transform::Embed3D(expected_pose)), transform::Embed3D(expected_pose)),

View File

@ -23,8 +23,8 @@
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/kalman_filter/pose_tracker.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/probability_grid.h"
#include "cartographer/mapping_2d/range_data_inserter.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
@ -46,8 +46,8 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
"hit_probability = 0.7, " "hit_probability = 0.7, "
"miss_probability = 0.4, " "miss_probability = 0.4, "
"}"); "}");
laser_fan_inserter_ = common::make_unique<LaserFanInserter>( range_data_inserter_ = common::make_unique<RangeDataInserter>(
CreateLaserFanInserterOptions(parameter_dictionary.get())); CreateRangeDataInserterOptions(parameter_dictionary.get()));
} }
point_cloud_.emplace_back(0.025f, 0.175f, 0.f); point_cloud_.emplace_back(0.025f, 0.175f, 0.f);
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.075f, 0.f);
point_cloud_.emplace_back(-0.125f, 0.025f, 0.f); point_cloud_.emplace_back(-0.125f, 0.025f, 0.f);
probability_grid_.StartUpdate(); probability_grid_.StartUpdate();
laser_fan_inserter_->Insert( range_data_inserter_->Insert(
sensor::LaserFan{Eigen::Vector3f::Zero(), point_cloud_, {}}, sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}},
&probability_grid_); &probability_grid_);
{ {
auto parameter_dictionary = common::MakeDictionary( auto parameter_dictionary = common::MakeDictionary(
@ -76,7 +76,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
} }
ProbabilityGrid probability_grid_; ProbabilityGrid probability_grid_;
std::unique_ptr<LaserFanInserter> laser_fan_inserter_; std::unique_ptr<RangeDataInserter> range_data_inserter_;
sensor::PointCloud point_cloud_; sensor::PointCloud point_cloud_;
std::unique_ptr<RealTimeCorrelativeScanMatcher> std::unique_ptr<RealTimeCorrelativeScanMatcher>
real_time_correlative_scan_matcher_; real_time_correlative_scan_matcher_;

View File

@ -85,7 +85,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
void SparsePoseGraph::AddScan( void SparsePoseGraph::AddScan(
common::Time time, const transform::Rigid3d& tracking_to_pose, common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::LaserFan& laser_fan_in_pose, const transform::Rigid2d& pose, const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance, const kalman_filter::Pose2DCovariance& covariance,
const mapping::Submaps* submaps, const mapping::Submaps* submaps,
const mapping::Submap* const matching_submap, const mapping::Submap* const matching_submap,
@ -98,8 +98,8 @@ void SparsePoseGraph::AddScan(
CHECK_LT(j, std::numeric_limits<int>::max()); CHECK_LT(j, std::numeric_limits<int>::max());
constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{ constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{
time, laser_fan_in_pose, time, range_data_in_pose,
Compress(sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}, {}}), submaps, Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}, {}}), submaps,
tracking_to_pose}); tracking_to_pose});
trajectory_nodes_.push_back(mapping::TrajectoryNode{ trajectory_nodes_.push_back(mapping::TrajectoryNode{
&constant_node_data_->back(), optimized_pose, &constant_node_data_->back(), optimized_pose,
@ -167,7 +167,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
constraint_builder_.MaybeAddGlobalConstraint( constraint_builder_.MaybeAddGlobalConstraint(
submap_index, submap_states_[submap_index].submap, scan_index, submap_index, submap_states_[submap_index].submap, scan_index,
scan_trajectory, submap_trajectory, &trajectory_connectivity_, scan_trajectory, submap_trajectory, &trajectory_connectivity_,
&trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns); &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns);
} else { } else {
const bool scan_and_submap_trajectories_connected = const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(scan_trajectory) > 0 && reverse_connected_components_.count(scan_trajectory) > 0 &&
@ -178,7 +178,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
scan_and_submap_trajectories_connected) { scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint( constraint_builder_.MaybeAddConstraint(
submap_index, submap_states_[submap_index].submap, scan_index, submap_index, submap_states_[submap_index].submap, scan_index,
&trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns, &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns,
relative_pose); relative_pose);
} }
} }

View File

@ -66,13 +66,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph(const SparsePoseGraph&) = delete;
SparsePoseGraph& operator=(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 will later be optimized. The 'tracking_to_pose' is remembered so
// that the optimized pose can be embedded into 3D. The 'pose' was determined // 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 // by scan matching against the 'matching_submap' and the scan was inserted
// into the 'insertion_submaps'. // into the 'insertion_submaps'.
void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose, void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::LaserFan& laser_fan_in_pose, const sensor::RangeData& range_data_in_pose,
const transform::Rigid2d& pose, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& pose_covariance, const kalman_filter::Pose2DCovariance& pose_covariance,
const mapping::Submaps* submaps, const mapping::Submaps* submaps,

View File

@ -24,7 +24,7 @@
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.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/mapping_2d/submaps.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/rigid_transform_test_helpers.h"
@ -52,9 +52,9 @@ class SparsePoseGraphTest : public ::testing::Test {
return { return {
resolution = 0.05, resolution = 0.05,
half_length = 21., half_length = 21.,
num_laser_fans = 1, num_range_data = 1,
output_debug_images = false, output_debug_images = false,
laser_fan_inserter = { range_data_inserter = {
insert_free_space = true, insert_free_space = true,
hit_probability = 0.53, hit_probability = 0.53,
miss_probability = 0.495, miss_probability = 0.495,
@ -155,13 +155,13 @@ class SparsePoseGraphTest : public ::testing::Test {
for (int insertion_index : submaps_->insertion_indices()) { for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index)); insertion_submaps.push_back(submaps_->Get(insertion_index));
} }
const sensor::LaserFan laser_fan{ const sensor::RangeData range_data{
Eigen::Vector3f::Zero(), new_point_cloud, {}}; Eigen::Vector3f::Zero(), new_point_cloud, {}};
const transform::Rigid2d pose_estimate = noise * current_pose_; const transform::Rigid2d pose_estimate = noise * current_pose_;
submaps_->InsertLaserFan(TransformLaserFan( submaps_->InsertRangeData(TransformRangeData(
laser_fan, transform::Embed3D(pose_estimate.cast<float>()))); range_data, transform::Embed3D(pose_estimate.cast<float>())));
sparse_pose_graph_->AddScan(common::FromUniversal(0), sparse_pose_graph_->AddScan(common::FromUniversal(0),
transform::Rigid3d::Identity(), laser_fan, transform::Rigid3d::Identity(), range_data,
pose_estimate, covariance, submaps_.get(), pose_estimate, covariance, submaps_.get(),
matching_submap, insertion_submaps); matching_submap, insertion_submaps);
} }

View File

@ -90,42 +90,43 @@ proto::SubmapsOptions CreateSubmapsOptions(
proto::SubmapsOptions options; proto::SubmapsOptions options;
options.set_resolution(parameter_dictionary->GetDouble("resolution")); options.set_resolution(parameter_dictionary->GetDouble("resolution"));
options.set_half_length(parameter_dictionary->GetDouble("half_length")); options.set_half_length(parameter_dictionary->GetDouble("half_length"));
options.set_num_laser_fans( options.set_num_range_data(
parameter_dictionary->GetNonNegativeInt("num_laser_fans")); parameter_dictionary->GetNonNegativeInt("num_range_data"));
options.set_output_debug_images( options.set_output_debug_images(
parameter_dictionary->GetBool("output_debug_images")); parameter_dictionary->GetBool("output_debug_images"));
*options.mutable_laser_fan_inserter_options() = CreateLaserFanInserterOptions( *options.mutable_range_data_inserter_options() =
parameter_dictionary->GetDictionary("laser_fan_inserter").get()); CreateRangeDataInserterOptions(
CHECK_GT(options.num_laser_fans(), 0); parameter_dictionary->GetDictionary("range_data_inserter").get());
CHECK_GT(options.num_range_data(), 0);
return options; return options;
} }
Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin, 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.), : mapping::Submap(Eigen::Vector3f(origin.x(), origin.y(), 0.),
begin_laser_fan_index), begin_range_data_index),
probability_grid(limits) {} probability_grid(limits) {}
Submaps::Submaps(const proto::SubmapsOptions& options) Submaps::Submaps(const proto::SubmapsOptions& options)
: options_(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, // 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. // and will create it at the origin in absence of a better choice.
AddSubmap(Eigen::Vector2f::Zero()); AddSubmap(Eigen::Vector2f::Zero());
} }
void Submaps::InsertLaserFan(const sensor::LaserFan& laser_fan) { void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
CHECK_LT(num_laser_fans_, std::numeric_limits<int>::max()); CHECK_LT(num_range_data_, std::numeric_limits<int>::max());
++num_laser_fans_; ++num_range_data_;
for (const int index : insertion_indices()) { for (const int index : insertion_indices()) {
Submap* submap = submaps_[index].get(); Submap* submap = submaps_[index].get();
CHECK(submap->finished_probability_grid == nullptr); CHECK(submap->finished_probability_grid == nullptr);
laser_fan_inserter_.Insert(laser_fan, &submap->probability_grid); range_data_inserter_.Insert(range_data, &submap->probability_grid);
submap->end_laser_fan_index = num_laser_fans_; submap->end_range_data_index = num_range_data_;
} }
++num_laser_fans_in_last_submap_; ++num_range_data_in_last_submap_;
if (num_laser_fans_in_last_submap_ == options_.num_laser_fans()) { if (num_range_data_in_last_submap_ == options_.num_range_data()) {
AddSubmap(laser_fan.origin.head<2>()); AddSubmap(range_data.origin.head<2>());
} }
} }
@ -172,9 +173,9 @@ void Submaps::AddSubmap(const Eigen::Vector2f& origin) {
origin.cast<double>() + origin.cast<double>() +
options_.half_length() * Eigen::Vector2d::Ones(), options_.half_length() * Eigen::Vector2d::Ones(),
CellLimits(num_cells_per_dimension, num_cells_per_dimension)), CellLimits(num_cells_per_dimension, num_cells_per_dimension)),
origin, num_laser_fans_)); origin, num_range_data_));
LOG(INFO) << "Added submap " << size(); LOG(INFO) << "Added submap " << size();
num_laser_fans_in_last_submap_ = 0; num_range_data_in_last_submap_ = 0;
} }
} // namespace mapping_2d } // namespace mapping_2d

View File

@ -25,11 +25,11 @@
#include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_node.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/map_limits.h"
#include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/proto/submaps_options.pb.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" #include "cartographer/transform/rigid_transform.h"
namespace cartographer { namespace cartographer {
@ -43,7 +43,7 @@ proto::SubmapsOptions CreateSubmapsOptions(
struct Submap : public mapping::Submap { struct Submap : public mapping::Submap {
Submap(const MapLimits& limits, const Eigen::Vector2f& origin, Submap(const MapLimits& limits, const Eigen::Vector2f& origin,
int begin_laser_fan_index); int begin_range_data_index);
ProbabilityGrid probability_grid; ProbabilityGrid probability_grid;
}; };
@ -63,8 +63,8 @@ class Submaps : public mapping::Submaps {
const transform::Rigid3d& global_submap_pose, const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* response) const override; mapping::proto::SubmapQuery::Response* response) const override;
// Inserts 'laser_fan' into the Submap collection. // Inserts 'range_data' into the Submap collection.
void InsertLaserFan(const sensor::LaserFan& laser_fan); void InsertRangeData(const sensor::RangeData& range_data);
private: private:
void FinishSubmap(int index); void FinishSubmap(int index);
@ -73,13 +73,13 @@ class Submaps : public mapping::Submaps {
const proto::SubmapsOptions options_; const proto::SubmapsOptions options_;
std::vector<std::unique_ptr<Submap>> submaps_; std::vector<std::unique_ptr<Submap>> submaps_;
LaserFanInserter laser_fan_inserter_; RangeDataInserter range_data_inserter_;
// Number of LaserFans inserted. // Number of RangeData inserted.
int num_laser_fans_ = 0; int num_range_data_ = 0;
// Number of LaserFans inserted since the last Submap was added. // Number of RangeData inserted since the last Submap was added.
int num_laser_fans_in_last_submap_ = 0; int num_range_data_in_last_submap_ = 0;
}; };
} // namespace mapping_2d } // namespace mapping_2d

View File

@ -30,16 +30,16 @@ namespace mapping_2d {
namespace { namespace {
TEST(SubmapsTest, TheRightNumberOfScansAreInserted) { TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
constexpr int kNumLaserFans = 10; constexpr int kNumRangeData = 10;
auto parameter_dictionary = common::MakeDictionary( auto parameter_dictionary = common::MakeDictionary(
"return {" "return {"
"resolution = 0.05, " "resolution = 0.05, "
"half_length = 10., " "half_length = 10., "
"num_laser_fans = " + "num_range_data = " +
std::to_string(kNumLaserFans) + std::to_string(kNumRangeData) +
", " ", "
"output_debug_images = false, " "output_debug_images = false, "
"laser_fan_inserter = {" "range_data_inserter = {"
"insert_free_space = true, " "insert_free_space = true, "
"hit_probability = 0.53, " "hit_probability = 0.53, "
"miss_probability = 0.495, " "miss_probability = 0.495, "
@ -47,22 +47,22 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
"}"); "}");
Submaps submaps{CreateSubmapsOptions(parameter_dictionary.get())}; Submaps submaps{CreateSubmapsOptions(parameter_dictionary.get())};
auto num_inserted = [&submaps](const int i) { auto num_inserted = [&submaps](const int i) {
return submaps.Get(i)->end_laser_fan_index - return submaps.Get(i)->end_range_data_index -
submaps.Get(i)->begin_laser_fan_index; submaps.Get(i)->begin_range_data_index;
}; };
for (int i = 0; i != 1000; ++i) { for (int i = 0; i != 1000; ++i) {
submaps.InsertLaserFan({Eigen::Vector3f::Zero(), {}, {}}); submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}});
const int matching = submaps.matching_index(); const int matching = submaps.matching_index();
// Except for the first, maps should only be returned after enough scans. // Except for the first, maps should only be returned after enough scans.
if (matching != 0) { if (matching != 0) {
EXPECT_LE(kNumLaserFans, num_inserted(matching)); EXPECT_LE(kNumRangeData, num_inserted(matching));
} }
} }
for (int i = 0; i != submaps.size() - 2; ++i) { for (int i = 0; i != submaps.size() - 2; ++i) {
// Submaps should not be left without the right number of scans in them. // Submaps should not be left without the right number of scans in them.
EXPECT_EQ(kNumLaserFans * 2, num_inserted(i)); EXPECT_EQ(kNumRangeData * 2, num_inserted(i));
EXPECT_EQ(i * kNumLaserFans, submaps.Get(i)->begin_laser_fan_index); EXPECT_EQ(i * kNumRangeData, submaps.Get(i)->begin_range_data_index);
EXPECT_EQ((i + 2) * kNumLaserFans, submaps.Get(i)->end_laser_fan_index); EXPECT_EQ((i + 2) * kNumRangeData, submaps.Get(i)->end_range_data_index);
} }
} }

View File

@ -25,9 +25,9 @@ google_test(mapping_3d_kalman_local_trajectory_builder_test
kalman_local_trajectory_builder_test.cc kalman_local_trajectory_builder_test.cc
) )
google_test(mapping_3d_laser_fan_inserter_test google_test(mapping_3d_range_data_inserter_test
SRCS SRCS
laser_fan_inserter_test.cc range_data_inserter_test.cc
) )
google_test(mapping_3d_motion_filter_test google_test(mapping_3d_motion_filter_test

View File

@ -53,7 +53,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
} }
const int trajectory_node_index = sparse_pose_graph_->AddScan( 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->pose_observation, insertion_result->covariance_estimate,
insertion_result->submaps, insertion_result->matching_submap, insertion_result->submaps, insertion_result->matching_submap,
insertion_result->insertion_submaps); insertion_result->insertion_submaps);

View File

@ -42,7 +42,7 @@ KalmanLocalTrajectoryBuilder::KalmanLocalTrajectoryBuilder(
options_.ceres_scan_matcher_options())), options_.ceres_scan_matcher_options())),
num_accumulated_(0), num_accumulated_(0),
first_pose_prediction_(transform::Rigid3f::Identity()), first_pose_prediction_(transform::Rigid3f::Identity()),
accumulated_laser_fan_{Eigen::Vector3f::Zero(), {}, {}} {} accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {}
KalmanLocalTrajectoryBuilder::~KalmanLocalTrajectoryBuilder() {} KalmanLocalTrajectoryBuilder::~KalmanLocalTrajectoryBuilder() {}
@ -84,28 +84,27 @@ KalmanLocalTrajectoryBuilder::AddRangefinderData(
time, &pose_prediction, &unused_covariance_prediction); time, &pose_prediction, &unused_covariance_prediction);
if (num_accumulated_ == 0) { if (num_accumulated_ == 0) {
first_pose_prediction_ = pose_prediction.cast<float>(); first_pose_prediction_ = pose_prediction.cast<float>();
accumulated_laser_fan_ = sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}}; accumulated_range_data_ =
sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}};
} }
const transform::Rigid3f tracking_delta = const transform::Rigid3f tracking_delta =
first_pose_prediction_.inverse() * pose_prediction.cast<float>(); first_pose_prediction_.inverse() * pose_prediction.cast<float>();
const sensor::LaserFan laser_fan_in_first_tracking = const sensor::RangeData range_data_in_first_tracking =
sensor::TransformLaserFan(sensor::LaserFan{origin, ranges, {}, {}}, sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}, {}},
tracking_delta); tracking_delta);
for (const Eigen::Vector3f& laser_return : for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) {
laser_fan_in_first_tracking.returns) { const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin;
const Eigen::Vector3f delta =
laser_return - laser_fan_in_first_tracking.origin;
const float range = delta.norm(); const float range = delta.norm();
if (range >= options_.laser_min_range()) { if (range >= options_.laser_min_range()) {
if (range <= options_.laser_max_range()) { if (range <= options_.laser_max_range()) {
accumulated_laser_fan_.returns.push_back(laser_return); accumulated_range_data_.returns.push_back(hit);
} else { } else {
// We insert a ray cropped to 'laser_max_range' as a miss for hits // 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 // beyond the maximum range. This way the free space up to the maximum
// range will be updated. // range will be updated.
accumulated_laser_fan_.misses.push_back( accumulated_range_data_.misses.push_back(
laser_fan_in_first_tracking.origin + range_data_in_first_tracking.origin +
options_.laser_max_range() / range * delta); options_.laser_max_range() / range * delta);
} }
} }
@ -114,25 +113,25 @@ KalmanLocalTrajectoryBuilder::AddRangefinderData(
if (num_accumulated_ >= options_.scans_per_accumulation()) { if (num_accumulated_ >= options_.scans_per_accumulation()) {
num_accumulated_ = 0; num_accumulated_ = 0;
return AddAccumulatedLaserFan( return AddAccumulatedRangeData(
time, sensor::TransformLaserFan(accumulated_laser_fan_, time, sensor::TransformRangeData(accumulated_range_data_,
tracking_delta.inverse())); tracking_delta.inverse()));
} }
return nullptr; return nullptr;
} }
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult> std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan( KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
const common::Time time, const sensor::LaserFan& laser_fan_in_tracking) { const common::Time time, const sensor::RangeData& range_data_in_tracking) {
const sensor::LaserFan filtered_laser_fan = { const sensor::RangeData filtered_range_data = {
laser_fan_in_tracking.origin, range_data_in_tracking.origin,
sensor::VoxelFiltered(laser_fan_in_tracking.returns, sensor::VoxelFiltered(range_data_in_tracking.returns,
options_.laser_voxel_filter_size()), options_.laser_voxel_filter_size()),
sensor::VoxelFiltered(laser_fan_in_tracking.misses, sensor::VoxelFiltered(range_data_in_tracking.misses,
options_.laser_voxel_filter_size())}; options_.laser_voxel_filter_size())};
if (filtered_laser_fan.returns.empty()) { if (filtered_range_data.returns.empty()) {
LOG(WARNING) << "Dropped empty laser scanner point cloud."; LOG(WARNING) << "Dropped empty range data.";
return nullptr; return nullptr;
} }
@ -145,7 +144,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan(
sensor::AdaptiveVoxelFilter adaptive_voxel_filter( sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.high_resolution_adaptive_voxel_filter_options()); options_.high_resolution_adaptive_voxel_filter_options());
const sensor::PointCloud filtered_point_cloud_in_tracking = 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() if (options_.kalman_local_trajectory_builder_options()
.use_online_correlative_scan_matching()) { .use_online_correlative_scan_matching()) {
real_time_correlative_scan_matcher_->Match( real_time_correlative_scan_matcher_->Match(
@ -159,7 +158,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan(
sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
options_.low_resolution_adaptive_voxel_filter_options()); options_.low_resolution_adaptive_voxel_filter_options());
const sensor::PointCloud low_resolution_point_cloud_in_tracking = 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, ceres_scan_matcher_->Match(scan_matcher_pose_estimate_, initial_ceres_pose,
{{&filtered_point_cloud_in_tracking, {{&filtered_point_cloud_in_tracking,
&submaps_->high_resolution_matching_grid()}, &submaps_->high_resolution_matching_grid()},
@ -178,10 +177,10 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan(
last_pose_estimate_ = { last_pose_estimate_ = {
time, scan_matcher_pose_estimate_, time, scan_matcher_pose_estimate_,
sensor::TransformPointCloud(filtered_laser_fan.returns, sensor::TransformPointCloud(filtered_range_data.returns,
pose_observation.cast<float>())}; pose_observation.cast<float>())};
return InsertIntoSubmap(time, filtered_laser_fan, pose_observation, return InsertIntoSubmap(time, filtered_range_data, pose_observation,
covariance_estimate); covariance_estimate);
} }
@ -214,7 +213,7 @@ void KalmanLocalTrajectoryBuilder::AddTrajectoryNodeIndex(
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult> std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::InsertIntoSubmap( 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 transform::Rigid3d& pose_observation,
const kalman_filter::PoseCovariance& covariance_estimate) { const kalman_filter::PoseCovariance& covariance_estimate) {
if (motion_filter_.IsSimilar(time, pose_observation)) { if (motion_filter_.IsSimilar(time, pose_observation)) {
@ -226,10 +225,10 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
for (int insertion_index : submaps_->insertion_indices()) { for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index)); insertion_submaps.push_back(submaps_->Get(insertion_index));
} }
submaps_->InsertLaserFan(sensor::TransformLaserFan( submaps_->InsertRangeData(sensor::TransformRangeData(
laser_fan_in_tracking, pose_observation.cast<float>())); range_data_in_tracking, pose_observation.cast<float>()));
return std::unique_ptr<InsertionResult>(new InsertionResult{ return std::unique_ptr<InsertionResult>(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}); submaps_.get(), matching_submap, insertion_submaps});
} }

View File

@ -28,7 +28,7 @@
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #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/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping_3d/submaps.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/sensor/voxel_filter.h"
namespace cartographer { namespace cartographer {
@ -58,11 +58,11 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
const PoseEstimate& pose_estimate() const override; const PoseEstimate& pose_estimate() const override;
private: private:
std::unique_ptr<InsertionResult> AddAccumulatedLaserFan( std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
common::Time time, const sensor::LaserFan& laser_fan_in_tracking); common::Time time, const sensor::RangeData& range_data_in_tracking);
std::unique_ptr<InsertionResult> InsertIntoSubmap( std::unique_ptr<InsertionResult> 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 transform::Rigid3d& pose_observation,
const kalman_filter::PoseCovariance& covariance_estimate); const kalman_filter::PoseCovariance& covariance_estimate);
@ -83,7 +83,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
int num_accumulated_; int num_accumulated_;
transform::Rigid3f first_pose_prediction_; transform::Rigid3f first_pose_prediction_;
sensor::LaserFan accumulated_laser_fan_; sensor::RangeData accumulated_range_data_;
}; };
} // namespace mapping_3d } // namespace mapping_3d

View File

@ -24,7 +24,7 @@
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/mapping_3d/local_trajectory_builder_options.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.h"
#include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/rigid_transform_test_helpers.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
@ -85,8 +85,8 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
high_resolution = 0.2, high_resolution = 0.2,
high_resolution_max_range = 50., high_resolution_max_range = 50.,
low_resolution = 0.5, low_resolution = 0.5,
num_laser_fans = 45000, num_range_data = 45000,
laser_fan_inserter = { range_data_inserter = {
hit_probability = 0.7, hit_probability = 0.7,
miss_probability = 0.4, miss_probability = 0.4,
num_free_space_voxels = 0, num_free_space_voxels = 0,
@ -189,7 +189,7 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
return first * (to - from) + from; 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. // 360 degree rays at 16 angles.
sensor::PointCloud directions_in_laser_frame; sensor::PointCloud directions_in_laser_frame;
for (int r = -8; r != 8; ++r) { for (int r = -8; r != 8; ++r) {
@ -259,9 +259,9 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
int num_poses = 0; int num_poses = 0;
for (const TrajectoryNode& node : expected_trajectory) { for (const TrajectoryNode& node : expected_trajectory) {
AddLinearOnlyImuObservation(node.time, node.pose); AddLinearOnlyImuObservation(node.time, node.pose);
const auto laser_fan = GenerateLaserFan(node.pose); const auto range_data = GenerateRangeData(node.pose);
if (local_trajectory_builder_->AddRangefinderData( 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(); const auto pose_estimate = local_trajectory_builder_->pose_estimate();
EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1)); EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));
++num_poses; ++num_poses;

View File

@ -23,7 +23,7 @@
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping_3d/submaps.h" #include "cartographer/mapping_3d/submaps.h"
#include "cartographer/sensor/laser.h" #include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
namespace cartographer { namespace cartographer {
@ -35,7 +35,7 @@ class LocalTrajectoryBuilderInterface {
struct InsertionResult { struct InsertionResult {
common::Time time; common::Time time;
sensor::LaserFan laser_fan_in_tracking; sensor::RangeData range_data_in_tracking;
transform::Rigid3d pose_observation; transform::Rigid3d pose_observation;
kalman_filter::PoseCovariance covariance_estimate; kalman_filter::PoseCovariance covariance_estimate;
const Submaps* submaps; const Submaps* submaps;

View File

@ -139,14 +139,14 @@ OptimizingLocalTrajectoryBuilder::AddRangefinderData(
CHECK_GT(ranges.size(), 0); CHECK_GT(ranges.size(), 0);
// TODO(hrapp): Handle misses. // 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; sensor::PointCloud point_cloud;
for (const Eigen::Vector3f& laser_return : ranges) { for (const Eigen::Vector3f& hit : ranges) {
const Eigen::Vector3f delta = laser_return - origin; const Eigen::Vector3f delta = hit - origin;
const float range = delta.norm(); const float range = delta.norm();
if (range >= options_.laser_min_range()) { if (range >= options_.laser_min_range()) {
if (range <= options_.laser_max_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); low_resolution_adaptive_voxel_filter.Filter(point_cloud);
if (batches_.empty()) { if (batches_.empty()) {
// First laser ever. Initialize to the origin. // First rangefinder data ever. Initialize to the origin.
batches_.push_back( batches_.push_back(
Batch{time, point_cloud, high_resolution_filtered_points, Batch{time, point_cloud, high_resolution_filtered_points,
low_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); interpolation_buffer.Push(odometer_data.time, odometer_data.pose);
} }
for (size_t i = 1; i < batches_.size(); ++i) { for (size_t i = 1; i < batches_.size(); ++i) {
// Only add constraints for this laser if we have bracketing data from // Only add constraints for this range data if we have bracketing data
// the odometer. // from the odometer.
if (!(interpolation_buffer.earliest_time() <= batches_[i - 1].time && if (!(interpolation_buffer.earliest_time() <= batches_[i - 1].time &&
batches_[i].time <= interpolation_buffer.latest_time())) { batches_[i].time <= interpolation_buffer.latest_time())) {
continue; continue;
@ -336,43 +336,43 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
num_accumulated_ = 0; num_accumulated_ = 0;
const transform::Rigid3d optimized_pose = batches_.back().state.ToRigid(); 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(), {}, {}}; Eigen::Vector3f::Zero(), {}, {}};
for (const auto& batch : batches_) { for (const auto& batch : batches_) {
const transform::Rigid3f transform = const transform::Rigid3f transform =
(optimized_pose.inverse() * batch.state.ToRigid()).cast<float>(); (optimized_pose.inverse() * batch.state.ToRigid()).cast<float>();
for (const Eigen::Vector3f& point : batch.points) { 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, return AddAccumulatedRangeData(time, optimized_pose,
accumulated_laser_fan_in_tracking); accumulated_range_data_in_tracking);
} }
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult> std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::AddAccumulatedLaserFan( OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData(
const common::Time time, const transform::Rigid3d& optimized_pose, const common::Time time, const transform::Rigid3d& optimized_pose,
const sensor::LaserFan& laser_fan_in_tracking) { const sensor::RangeData& range_data_in_tracking) {
const sensor::LaserFan filtered_laser_fan = { const sensor::RangeData filtered_range_data = {
laser_fan_in_tracking.origin, range_data_in_tracking.origin,
sensor::VoxelFiltered(laser_fan_in_tracking.returns, sensor::VoxelFiltered(range_data_in_tracking.returns,
options_.laser_voxel_filter_size()), options_.laser_voxel_filter_size()),
sensor::VoxelFiltered(laser_fan_in_tracking.misses, sensor::VoxelFiltered(range_data_in_tracking.misses,
options_.laser_voxel_filter_size())}; options_.laser_voxel_filter_size())};
if (filtered_laser_fan.returns.empty()) { if (filtered_range_data.returns.empty()) {
LOG(WARNING) << "Dropped empty laser scanner point cloud."; LOG(WARNING) << "Dropped empty range data.";
return nullptr; return nullptr;
} }
last_pose_estimate_ = { last_pose_estimate_ = {
time, optimized_pose, time, optimized_pose,
sensor::TransformPointCloud(filtered_laser_fan.returns, sensor::TransformPointCloud(filtered_range_data.returns,
optimized_pose.cast<float>())}; optimized_pose.cast<float>())};
return InsertIntoSubmap(time, filtered_laser_fan, optimized_pose); return InsertIntoSubmap(time, filtered_range_data, optimized_pose);
} }
const OptimizingLocalTrajectoryBuilder::PoseEstimate& const OptimizingLocalTrajectoryBuilder::PoseEstimate&
@ -387,7 +387,7 @@ void OptimizingLocalTrajectoryBuilder::AddTrajectoryNodeIndex(
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult> std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::InsertIntoSubmap( 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) { const transform::Rigid3d& pose_observation) {
if (motion_filter_.IsSimilar(time, pose_observation)) { if (motion_filter_.IsSimilar(time, pose_observation)) {
return nullptr; return nullptr;
@ -398,14 +398,14 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
for (int insertion_index : submaps_->insertion_indices()) { for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index)); insertion_submaps.push_back(submaps_->Get(insertion_index));
} }
submaps_->InsertLaserFan(sensor::TransformLaserFan( submaps_->InsertRangeData(sensor::TransformRangeData(
laser_fan_in_tracking, pose_observation.cast<float>())); range_data_in_tracking, pose_observation.cast<float>()));
const kalman_filter::PoseCovariance kCovariance = const kalman_filter::PoseCovariance kCovariance =
1e-7 * kalman_filter::PoseCovariance::Identity(); 1e-7 * kalman_filter::PoseCovariance::Identity();
return std::unique_ptr<InsertionResult>(new InsertionResult{ return std::unique_ptr<InsertionResult>(new InsertionResult{
time, laser_fan_in_tracking, pose_observation, kCovariance, time, range_data_in_tracking, pose_observation, kCovariance,
submaps_.get(), matching_submap, insertion_submaps}); submaps_.get(), matching_submap, insertion_submaps});
} }

View File

@ -28,7 +28,7 @@
#include "cartographer/mapping_3d/motion_filter.h" #include "cartographer/mapping_3d/motion_filter.h"
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
#include "cartographer/mapping_3d/submaps.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/sensor/voxel_filter.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
@ -100,12 +100,12 @@ class OptimizingLocalTrajectoryBuilder
void RemoveObsoleteSensorData(); void RemoveObsoleteSensorData();
std::unique_ptr<InsertionResult> AddAccumulatedLaserFan( std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
common::Time time, const transform::Rigid3d& pose_observation, 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<InsertionResult> InsertIntoSubmap( std::unique_ptr<InsertionResult> 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 transform::Rigid3d& pose_observation);
std::unique_ptr<InsertionResult> MaybeOptimize(common::Time time); std::unique_ptr<InsertionResult> MaybeOptimize(common::Time time);

View File

@ -16,7 +16,7 @@ syntax = "proto2";
package cartographer.mapping_3d.proto; package cartographer.mapping_3d.proto;
message LaserFanInserterOptions { message RangeDataInserterOptions {
// Probability change for a hit (this will be converted to odds and therefore // Probability change for a hit (this will be converted to odds and therefore
// must be greater than 0.5). // must be greater than 0.5).
optional double hit_probability = 1; optional double hit_probability = 1;

View File

@ -14,7 +14,7 @@
syntax = "proto2"; 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; 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 before adding a new submap. Each submap will get twice the
// number of scans inserted: First for initialization without being matched // number of scans inserted: First for initialization without being matched
// against, then while 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;
} }

View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_3d/laser_fan_inserter.h" #include "cartographer/mapping_3d/range_data_inserter.h"
#include "Eigen/Core" #include "Eigen/Core"
#include "cartographer/mapping/probability_values.h" #include "cartographer/mapping/probability_values.h"
@ -25,13 +25,13 @@ namespace mapping_3d {
namespace { namespace {
void InsertLaserMissesIntoGrid(const std::vector<uint16>& miss_table, void InsertMissesIntoGrid(const std::vector<uint16>& miss_table,
const Eigen::Vector3f& origin, const Eigen::Vector3f& origin,
const sensor::PointCloud& laser_returns, const sensor::PointCloud& returns,
HybridGrid* hybrid_grid, HybridGrid* hybrid_grid,
const int num_free_space_voxels) { const int num_free_space_voxels) {
const Eigen::Array3i origin_cell = hybrid_grid->GetCellIndex(origin); 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 hit_cell = hybrid_grid->GetCellIndex(hit);
const Eigen::Array3i delta = hit_cell - origin_cell; const Eigen::Array3i delta = hit_cell - origin_cell;
@ -54,9 +54,9 @@ void InsertLaserMissesIntoGrid(const std::vector<uint16>& miss_table,
} // namespace } // namespace
proto::LaserFanInserterOptions CreateLaserFanInserterOptions( proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
common::LuaParameterDictionary* parameter_dictionary) { common::LuaParameterDictionary* parameter_dictionary) {
proto::LaserFanInserterOptions options; proto::RangeDataInserterOptions options;
options.set_hit_probability( options.set_hit_probability(
parameter_dictionary->GetDouble("hit_probability")); parameter_dictionary->GetDouble("hit_probability"));
options.set_miss_probability( options.set_miss_probability(
@ -68,26 +68,26 @@ proto::LaserFanInserterOptions CreateLaserFanInserterOptions(
return options; return options;
} }
LaserFanInserter::LaserFanInserter( RangeDataInserter::RangeDataInserter(
const proto::LaserFanInserterOptions& options) const proto::RangeDataInserterOptions& options)
: options_(options), : options_(options),
hit_table_(mapping::ComputeLookupTableToApplyOdds( hit_table_(mapping::ComputeLookupTableToApplyOdds(
mapping::Odds(options_.hit_probability()))), mapping::Odds(options_.hit_probability()))),
miss_table_(mapping::ComputeLookupTableToApplyOdds( miss_table_(mapping::ComputeLookupTableToApplyOdds(
mapping::Odds(options_.miss_probability()))) {} mapping::Odds(options_.miss_probability()))) {}
void LaserFanInserter::Insert(const sensor::LaserFan& laser_fan, void RangeDataInserter::Insert(const sensor::RangeData& range_data,
HybridGrid* hybrid_grid) const { HybridGrid* hybrid_grid) const {
CHECK_NOTNULL(hybrid_grid)->StartUpdate(); 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); const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit);
hybrid_grid->ApplyLookupTable(hit_cell, hit_table_); hybrid_grid->ApplyLookupTable(hit_cell, hit_table_);
} }
// By not starting a new update after hits are inserted, we give hits priority // 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). // (i.e. no hits will be ignored because of a miss in the same cell).
InsertLaserMissesIntoGrid(miss_table_, laser_fan.origin, laser_fan.returns, InsertMissesIntoGrid(miss_table_, range_data.origin, range_data.returns,
hybrid_grid, options_.num_free_space_voxels()); hybrid_grid, options_.num_free_space_voxels());
} }

View File

@ -14,32 +14,33 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_3D_LASER_FAN_INSERTER_H_ #ifndef CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_
#define CARTOGRAPHER_MAPPING_3D_LASER_FAN_INSERTER_H_ #define CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_
#include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/mapping_3d/proto/laser_fan_inserter_options.pb.h" #include "cartographer/mapping_3d/proto/range_data_inserter_options.pb.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping_3d {
proto::LaserFanInserterOptions CreateLaserFanInserterOptions( proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
common::LuaParameterDictionary* parameter_dictionary); common::LuaParameterDictionary* parameter_dictionary);
class LaserFanInserter { class RangeDataInserter {
public: public:
explicit LaserFanInserter(const proto::LaserFanInserterOptions& options); explicit RangeDataInserter(const proto::RangeDataInserterOptions& options);
LaserFanInserter(const LaserFanInserter&) = delete; RangeDataInserter(const RangeDataInserter&) = delete;
LaserFanInserter& operator=(const LaserFanInserter&) = delete; RangeDataInserter& operator=(const RangeDataInserter&) = delete;
// Inserts 'laser_fan' into 'hybrid_grid'. // Inserts 'range_data' into 'hybrid_grid'.
void Insert(const sensor::LaserFan& laser_fan, HybridGrid* hybrid_grid) const; void Insert(const sensor::RangeData& range_data,
HybridGrid* hybrid_grid) const;
private: private:
const proto::LaserFanInserterOptions options_; const proto::RangeDataInserterOptions options_;
const std::vector<uint16> hit_table_; const std::vector<uint16> hit_table_;
const std::vector<uint16> miss_table_; const std::vector<uint16> miss_table_;
}; };
@ -47,4 +48,4 @@ class LaserFanInserter {
} // namespace mapping_3d } // namespace mapping_3d
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_LASER_FAN_INSERTER_H_ #endif // CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_

View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_3d/laser_fan_inserter.h" #include "cartographer/mapping_3d/range_data_inserter.h"
#include <memory> #include <memory>
#include <vector> #include <vector>
@ -26,9 +26,9 @@ namespace cartographer {
namespace mapping_3d { namespace mapping_3d {
namespace { namespace {
class LaserFanInserterTest : public ::testing::Test { class RangeDataInserterTest : public ::testing::Test {
protected: protected:
LaserFanInserterTest() RangeDataInserterTest()
: hybrid_grid_(1.f, Eigen::Vector3f(0.5f, 0.5f, 0.5f)) { : hybrid_grid_(1.f, Eigen::Vector3f(0.5f, 0.5f, 0.5f)) {
auto parameter_dictionary = common::MakeDictionary( auto parameter_dictionary = common::MakeDictionary(
"return { " "return { "
@ -36,17 +36,17 @@ class LaserFanInserterTest : public ::testing::Test {
"miss_probability = 0.4, " "miss_probability = 0.4, "
"num_free_space_voxels = 1000, " "num_free_space_voxels = 1000, "
"}"); "}");
options_ = CreateLaserFanInserterOptions(parameter_dictionary.get()); options_ = CreateRangeDataInserterOptions(parameter_dictionary.get());
laser_fan_inserter_.reset(new LaserFanInserter(options_)); range_data_inserter_.reset(new RangeDataInserter(options_));
} }
void InsertPointCloud() { void InsertPointCloud() {
const Eigen::Vector3f origin = Eigen::Vector3f(0.5f, 0.5f, -3.5f); const Eigen::Vector3f origin = Eigen::Vector3f(0.5f, 0.5f, -3.5f);
sensor::PointCloud laser_returns = {{-2.5f, -0.5f, 4.5f}, sensor::PointCloud returns = {{-2.5f, -0.5f, 4.5f},
{-1.5f, 0.5f, 4.5f}, {-1.5f, 0.5f, 4.5f},
{-0.5f, 1.5f, 4.5f}, {-0.5f, 1.5f, 4.5f},
{0.5f, 2.5f, 4.5f}}; {0.5f, 2.5f, 4.5f}};
laser_fan_inserter_->Insert(sensor::LaserFan{origin, laser_returns, {}}, range_data_inserter_->Insert(sensor::RangeData{origin, returns, {}},
&hybrid_grid_); &hybrid_grid_);
} }
@ -60,15 +60,15 @@ class LaserFanInserterTest : public ::testing::Test {
hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
} }
const proto::LaserFanInserterOptions& options() const { return options_; } const proto::RangeDataInserterOptions& options() const { return options_; }
private: private:
HybridGrid hybrid_grid_; HybridGrid hybrid_grid_;
std::unique_ptr<LaserFanInserter> laser_fan_inserter_; std::unique_ptr<RangeDataInserter> range_data_inserter_;
proto::LaserFanInserterOptions options_; proto::RangeDataInserterOptions options_;
}; };
TEST_F(LaserFanInserterTest, InsertPointCloud) { TEST_F(RangeDataInserterTest, InsertPointCloud) {
InsertPointCloud(); InsertPointCloud();
EXPECT_NEAR(options().miss_probability(), GetProbability(0.5f, 0.5f, -3.5f), EXPECT_NEAR(options().miss_probability(), GetProbability(0.5f, 0.5f, -3.5f),
1e-4); 1e-4);
@ -88,7 +88,7 @@ TEST_F(LaserFanInserterTest, InsertPointCloud) {
} }
} }
TEST_F(LaserFanInserterTest, ProbabilityProgression) { TEST_F(RangeDataInserterTest, ProbabilityProgression) {
InsertPointCloud(); InsertPointCloud();
EXPECT_NEAR(options().hit_probability(), GetProbability(-1.5f, 0.5f, 4.5f), EXPECT_NEAR(options().hit_probability(), GetProbability(-1.5f, 0.5f, 4.5f),
1e-4); 1e-4);

View File

@ -22,7 +22,7 @@
#include <string> #include <string>
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #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/rigid_transform_test_helpers.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
@ -51,20 +51,21 @@ CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) {
return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
} }
mapping_3d::proto::LaserFanInserterOptions CreateLaserFanInserterTestOptions() { mapping_3d::proto::RangeDataInserterOptions
CreateRangeDataInserterTestOptions() {
auto parameter_dictionary = common::MakeDictionary( auto parameter_dictionary = common::MakeDictionary(
"return { " "return { "
"hit_probability = 0.7, " "hit_probability = 0.7, "
"miss_probability = 0.4, " "miss_probability = 0.4, "
"num_free_space_voxels = 5, " "num_free_space_voxels = 5, "
"}"); "}");
return CreateLaserFanInserterOptions(parameter_dictionary.get()); return CreateRangeDataInserterOptions(parameter_dictionary.get());
} }
TEST(FastCorrelativeScanMatcherTest, CorrectPose) { TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
std::mt19937 prng(42); std::mt19937 prng(42);
std::uniform_real_distribution<float> distribution(-1.f, 1.f); std::uniform_real_distribution<float> distribution(-1.f, 1.f);
LaserFanInserter laser_fan_inserter(CreateLaserFanInserterTestOptions()); RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions());
constexpr float kMinScore = 0.1f; constexpr float kMinScore = 0.1f;
const auto options = CreateFastCorrelativeScanMatcherTestOptions(5); const auto options = CreateFastCorrelativeScanMatcherTestOptions(5);
@ -89,9 +90,10 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
HybridGrid hybrid_grid(0.05f /* resolution */, HybridGrid hybrid_grid(0.05f /* resolution */,
Eigen::Vector3f(0.5f, 1.5f, 2.5f) /* origin */); Eigen::Vector3f(0.5f, 1.5f, 2.5f) /* origin */);
hybrid_grid.StartUpdate(); hybrid_grid.StartUpdate();
laser_fan_inserter.Insert(sensor::LaserFan{expected_pose.translation(), range_data_inserter.Insert(
sensor::TransformPointCloud( sensor::RangeData{
point_cloud, expected_pose), expected_pose.translation(),
sensor::TransformPointCloud(point_cloud, expected_pose),
{}}, {}},
&hybrid_grid); &hybrid_grid);

View File

@ -84,7 +84,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
} }
int SparsePoseGraph::AddScan( 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 transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance, const Submaps* submaps, const kalman_filter::PoseCovariance& covariance, const Submaps* submaps,
const Submap* const matching_submap, const Submap* const matching_submap,
@ -97,8 +97,8 @@ int SparsePoseGraph::AddScan(
CHECK_LT(j, std::numeric_limits<int>::max()); CHECK_LT(j, std::numeric_limits<int>::max());
constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{ constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{
time, sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}}, time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
sensor::Compress(laser_fan_in_tracking), submaps, sensor::Compress(range_data_in_tracking), submaps,
transform::Rigid3d::Identity()}); transform::Rigid3d::Identity()});
trajectory_nodes_.push_back( trajectory_nodes_.push_back(
mapping::TrajectoryNode{&constant_node_data_->back(), optimized_pose}); mapping::TrajectoryNode{&constant_node_data_->back(), optimized_pose});

View File

@ -66,12 +66,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph(const SparsePoseGraph&) = delete;
SparsePoseGraph& operator=(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 // that will later be optimized. The 'pose' was determined by scan matching
// against the 'matching_submap' and the scan was inserted into the // against the 'matching_submap' and the scan was inserted into the
// 'insertion_submaps'. The index into the vector of trajectory nodes as // 'insertion_submaps'. The index into the vector of trajectory nodes as
// used with GetTrajectoryNodes() is returned. // 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 transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& pose_covariance, const kalman_filter::PoseCovariance& pose_covariance,
const Submaps* submaps, const Submap* matching_submap, const Submaps* submaps, const Submap* matching_submap,

View File

@ -20,7 +20,7 @@
#include <limits> #include <limits>
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/sensor/laser.h" #include "cartographer/sensor/range_data.h"
#include "glog/logging.h" #include "glog/logging.h"
namespace cartographer { namespace cartographer {
@ -30,86 +30,77 @@ namespace {
constexpr float kSliceHalfHeight = 0.1f; constexpr float kSliceHalfHeight = 0.1f;
struct LaserSegment { struct RaySegment {
Eigen::Vector2f from; Eigen::Vector2f from;
Eigen::Vector2f to; 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 // We compute a slice around the xy-plane. 'transform' is applied to the rays in
// rays in global map frame to allow choosing an arbitrary slice. // global map frame to allow choosing an arbitrary slice.
void GenerateSegmentForSlice(const sensor::LaserFan& laser_fan, void GenerateSegmentForSlice(const sensor::RangeData& range_data,
const transform::Rigid3f& pose, const transform::Rigid3f& pose,
const transform::Rigid3f& transform, const transform::Rigid3f& transform,
std::vector<LaserSegment>* segments) { std::vector<RaySegment>* segments) {
const sensor::LaserFan transformed_laser_fan = const sensor::RangeData transformed_range_data =
sensor::TransformLaserFan(laser_fan, transform * pose); sensor::TransformRangeData(range_data, transform * pose);
segments->reserve(transformed_laser_fan.returns.size()); segments->reserve(transformed_range_data.returns.size());
for (const Eigen::Vector3f& hit : transformed_laser_fan.returns) { for (const Eigen::Vector3f& hit : transformed_range_data.returns) {
const Eigen::Vector2f laser_origin_xy = const Eigen::Vector2f origin_xy = transformed_range_data.origin.head<2>();
transformed_laser_fan.origin.head<2>(); const float origin_z = transformed_range_data.origin.z();
const float laser_origin_z = transformed_laser_fan.origin.z(); const float delta_z = hit.z() - origin_z;
const float delta_z = hit.z() - laser_origin_z; const Eigen::Vector2f delta_xy = hit.head<2>() - origin_xy;
const Eigen::Vector2f delta_xy = hit.head<2>() - laser_origin_xy; if (origin_z < -kSliceHalfHeight) {
if (laser_origin_z < -kSliceHalfHeight) { // Ray originates below the slice.
// Laser ray originates below the slice.
if (hit.z() > kSliceHalfHeight) { if (hit.z() > kSliceHalfHeight) {
// Laser ray is cutting through the slice. // Ray is cutting through the slice.
segments->push_back(LaserSegment{ segments->push_back(RaySegment{
laser_origin_xy + origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy,
(-kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy,
laser_origin_xy +
(kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy,
false}); false});
} else if (hit.z() > -kSliceHalfHeight) { } else if (hit.z() > -kSliceHalfHeight) {
// Laser return is inside the slice. // Hit is inside the slice.
segments->push_back(LaserSegment{ segments->push_back(RaySegment{
laser_origin_xy + origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy,
(-kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy,
hit.head<2>(), true}); hit.head<2>(), true});
} }
} else if (laser_origin_z < kSliceHalfHeight) { } else if (origin_z < kSliceHalfHeight) {
// Laser ray originates inside the slice. // Ray originates inside the slice.
if (hit.z() < -kSliceHalfHeight) { if (hit.z() < -kSliceHalfHeight) {
// Laser hit is below. // Hit is below.
segments->push_back(LaserSegment{ segments->push_back(RaySegment{
laser_origin_xy, origin_xy,
laser_origin_xy + origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy,
(-kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy,
false}); false});
} else if (hit.z() < kSliceHalfHeight) { } else if (hit.z() < kSliceHalfHeight) {
// Full ray is inside the slice. // 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 { } else {
// Laser hit is above. // Hit is above.
segments->push_back( segments->push_back(RaySegment{
LaserSegment{laser_origin_xy, origin_xy,
laser_origin_xy + (kSliceHalfHeight - laser_origin_z) / origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy,
delta_z * delta_xy,
false}); false});
} }
} else { } else {
// Laser ray originates above the slice. // Ray originates above the slice.
if (hit.z() < -kSliceHalfHeight) { if (hit.z() < -kSliceHalfHeight) {
// Laser ray is cutting through the slice. // Ray is cutting through the slice.
segments->push_back(LaserSegment{ segments->push_back(RaySegment{
laser_origin_xy + origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy,
(kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy,
laser_origin_xy +
(-kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy,
false}); false});
} else if (hit.z() < kSliceHalfHeight) { } else if (hit.z() < kSliceHalfHeight) {
// Laser return is inside the slice. // Hit is inside the slice.
segments->push_back( segments->push_back(RaySegment{
LaserSegment{laser_origin_xy + (kSliceHalfHeight - laser_origin_z) / origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy,
delta_z * delta_xy,
hit.head<2>(), true}); hit.head<2>(), true});
} }
} }
} }
} }
void UpdateFreeSpaceFromSegment(const LaserSegment& segment, void UpdateFreeSpaceFromSegment(const RaySegment& segment,
const std::vector<uint16>& miss_table, const std::vector<uint16>& miss_table,
mapping_2d::ProbabilityGrid* result) { mapping_2d::ProbabilityGrid* result) {
Eigen::Array2i from = result->limits().GetXYIndexOfCellContainingPoint( Eigen::Array2i from = result->limits().GetXYIndexOfCellContainingPoint(
@ -144,9 +135,9 @@ void UpdateFreeSpaceFromSegment(const LaserSegment& segment,
} }
} }
void InsertSegmentsIntoProbabilityGrid( void InsertSegmentsIntoProbabilityGrid(const std::vector<RaySegment>& segments,
const std::vector<LaserSegment>& segments, const std::vector<uint16>& hit_table,
const std::vector<uint16>& hit_table, const std::vector<uint16>& miss_table, const std::vector<uint16>& miss_table,
mapping_2d::ProbabilityGrid* result) { mapping_2d::ProbabilityGrid* result) {
result->StartUpdate(); result->StartUpdate();
if (segments.empty()) { if (segments.empty()) {
@ -154,7 +145,7 @@ void InsertSegmentsIntoProbabilityGrid(
} }
Eigen::Vector2f min = segments.front().from; Eigen::Vector2f min = segments.front().from;
Eigen::Vector2f max = min; Eigen::Vector2f max = min;
for (const LaserSegment& segment : segments) { for (const RaySegment& segment : segments) {
min = min.cwiseMin(segment.from); min = min.cwiseMin(segment.from);
min = min.cwiseMin(segment.to); min = min.cwiseMin(segment.to);
max = max.cwiseMax(segment.from); max = max.cwiseMax(segment.from);
@ -166,27 +157,27 @@ void InsertSegmentsIntoProbabilityGrid(
result->GrowLimits(min.x(), min.y()); result->GrowLimits(min.x(), min.y());
result->GrowLimits(max.x(), max.y()); result->GrowLimits(max.x(), max.y());
for (const LaserSegment& segment : segments) { for (const RaySegment& segment : segments) {
if (segment.hit) { if (segment.hit) {
result->ApplyLookupTable(result->limits().GetXYIndexOfCellContainingPoint( result->ApplyLookupTable(result->limits().GetXYIndexOfCellContainingPoint(
segment.to.x(), segment.to.y()), segment.to.x(), segment.to.y()),
hit_table); hit_table);
} }
} }
for (const LaserSegment& segment : segments) { for (const RaySegment& segment : segments) {
UpdateFreeSpaceFromSegment(segment, miss_table, result); UpdateFreeSpaceFromSegment(segment, miss_table, result);
} }
} }
// Filters 'laser_fan', retaining only the returns that have no more than // Filters 'range_data', retaining only the returns that have no more than
// 'max_range' distance from the laser origin. Removes misses and reflectivity // 'max_range' distance from the origin. Removes misses and reflectivity
// information. // information.
sensor::LaserFan FilterLaserFanByMaxRange(const sensor::LaserFan& laser_fan, sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data,
const float max_range) { const float max_range) {
sensor::LaserFan result{laser_fan.origin, {}, {}, {}}; sensor::RangeData result{range_data.origin, {}, {}, {}};
for (const Eigen::Vector3f& return_ : laser_fan.returns) { for (const Eigen::Vector3f& hit : range_data.returns) {
if ((return_ - laser_fan.origin).norm() <= max_range) { if ((hit - range_data.origin).norm() <= max_range) {
result.returns.push_back(return_); result.returns.push_back(hit);
} }
} }
return result; return result;
@ -195,16 +186,17 @@ sensor::LaserFan FilterLaserFanByMaxRange(const sensor::LaserFan& laser_fan,
} // namespace } // namespace
void InsertIntoProbabilityGrid( void InsertIntoProbabilityGrid(
const sensor::LaserFan& laser_fan, const transform::Rigid3f& pose, const sensor::RangeData& range_data, const transform::Rigid3f& pose,
const float slice_z, const mapping_2d::LaserFanInserter& laser_fan_inserter, const float slice_z,
const mapping_2d::RangeDataInserter& range_data_inserter,
mapping_2d::ProbabilityGrid* result) { mapping_2d::ProbabilityGrid* result) {
std::vector<LaserSegment> segments; std::vector<RaySegment> segments;
GenerateSegmentForSlice( GenerateSegmentForSlice(
laser_fan, pose, range_data, pose,
transform::Rigid3f::Translation(-slice_z * Eigen::Vector3f::UnitZ()), transform::Rigid3f::Translation(-slice_z * Eigen::Vector3f::UnitZ()),
&segments); &segments);
InsertSegmentsIntoProbabilityGrid(segments, laser_fan_inserter.hit_table(), InsertSegmentsIntoProbabilityGrid(segments, range_data_inserter.hit_table(),
laser_fan_inserter.miss_table(), result); range_data_inserter.miss_table(), result);
} }
proto::SubmapsOptions CreateSubmapsOptions( proto::SubmapsOptions CreateSubmapsOptions(
@ -215,23 +207,24 @@ proto::SubmapsOptions CreateSubmapsOptions(
options.set_high_resolution_max_range( options.set_high_resolution_max_range(
parameter_dictionary->GetDouble("high_resolution_max_range")); parameter_dictionary->GetDouble("high_resolution_max_range"));
options.set_low_resolution(parameter_dictionary->GetDouble("low_resolution")); options.set_low_resolution(parameter_dictionary->GetDouble("low_resolution"));
options.set_num_laser_fans( options.set_num_range_data(
parameter_dictionary->GetNonNegativeInt("num_laser_fans")); parameter_dictionary->GetNonNegativeInt("num_range_data"));
*options.mutable_laser_fan_inserter_options() = CreateLaserFanInserterOptions( *options.mutable_range_data_inserter_options() =
parameter_dictionary->GetDictionary("laser_fan_inserter").get()); CreateRangeDataInserterOptions(
CHECK_GT(options.num_laser_fans(), 0); parameter_dictionary->GetDictionary("range_data_inserter").get());
CHECK_GT(options.num_range_data(), 0);
return options; return options;
} }
Submap::Submap(const float high_resolution, const float low_resolution, Submap::Submap(const float high_resolution, const float low_resolution,
const Eigen::Vector3f& origin, const int begin_laser_fan_index) const Eigen::Vector3f& origin, const int begin_range_data_index)
: mapping::Submap(origin, begin_laser_fan_index), : mapping::Submap(origin, begin_range_data_index),
high_resolution_hybrid_grid(high_resolution, origin), high_resolution_hybrid_grid(high_resolution, origin),
low_resolution_hybrid_grid(low_resolution, origin) {} low_resolution_hybrid_grid(low_resolution, origin) {}
Submaps::Submaps(const proto::SubmapsOptions& options) Submaps::Submaps(const proto::SubmapsOptions& options)
: options_(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, // 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. // and will create it at the origin in absence of a better choice.
AddSubmap(Eigen::Vector3f::Zero()); AddSubmap(Eigen::Vector3f::Zero());
@ -281,21 +274,22 @@ void Submaps::SubmapToProto(
global_submap_pose.translation().z()))); global_submap_pose.translation().z())));
} }
void Submaps::InsertLaserFan(const sensor::LaserFan& laser_fan) { void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
CHECK_LT(num_laser_fans_, std::numeric_limits<int>::max()); CHECK_LT(num_range_data_, std::numeric_limits<int>::max());
++num_laser_fans_; ++num_range_data_;
for (const int index : insertion_indices()) { for (const int index : insertion_indices()) {
Submap* submap = submaps_[index].get(); Submap* submap = submaps_[index].get();
laser_fan_inserter_.Insert( range_data_inserter_.Insert(
FilterLaserFanByMaxRange(laser_fan, FilterRangeDataByMaxRange(range_data,
options_.high_resolution_max_range()), options_.high_resolution_max_range()),
&submap->high_resolution_hybrid_grid); &submap->high_resolution_hybrid_grid);
laser_fan_inserter_.Insert(laser_fan, &submap->low_resolution_hybrid_grid); range_data_inserter_.Insert(range_data,
submap->end_laser_fan_index = num_laser_fans_; &submap->low_resolution_hybrid_grid);
submap->end_range_data_index = num_range_data_;
} }
++num_laser_fans_in_last_submap_; ++num_range_data_in_last_submap_;
if (num_laser_fans_in_last_submap_ == options_.num_laser_fans()) { if (num_range_data_in_last_submap_ == options_.num_range_data()) {
AddSubmap(laser_fan.origin); AddSubmap(range_data.origin);
} }
} }
@ -310,8 +304,8 @@ const HybridGrid& Submaps::low_resolution_matching_grid() const {
void Submaps::AddTrajectoryNodeIndex(const int trajectory_node_index) { void Submaps::AddTrajectoryNodeIndex(const int trajectory_node_index) {
for (int i = 0; i != size(); ++i) { for (int i = 0; i != size(); ++i) {
Submap& submap = *submaps_[i]; Submap& submap = *submaps_[i];
if (submap.end_laser_fan_index == num_laser_fans_ && if (submap.end_range_data_index == num_range_data_ &&
submap.begin_laser_fan_index <= num_laser_fans_ - 1) { submap.begin_range_data_index <= num_range_data_ - 1) {
submap.trajectory_node_indices.push_back(trajectory_node_index); 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(), submaps_.emplace_back(new Submap(options_.high_resolution(),
options_.low_resolution(), origin, options_.low_resolution(), origin,
num_laser_fans_)); num_range_data_));
LOG(INFO) << "Added submap " << size(); LOG(INFO) << "Added submap " << size();
num_laser_fans_in_last_submap_ = 0; num_range_data_in_last_submap_ = 0;
} }
std::vector<Submaps::PixelData> Submaps::AccumulatePixelData( std::vector<Submaps::PixelData> Submaps::AccumulatePixelData(

View File

@ -25,20 +25,21 @@
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/submaps.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/probability_grid.h"
#include "cartographer/mapping_2d/range_data_inserter.h"
#include "cartographer/mapping_3d/hybrid_grid.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/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" #include "cartographer/transform/transform.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping_3d {
void InsertIntoProbabilityGrid( void InsertIntoProbabilityGrid(
const sensor::LaserFan& laser_fan, const transform::Rigid3f& pose, const sensor::RangeData& range_data, const transform::Rigid3f& pose,
const float slice_z, const mapping_2d::LaserFanInserter& laser_fan_inserter, const float slice_z,
const mapping_2d::RangeDataInserter& range_data_inserter,
mapping_2d::ProbabilityGrid* result); mapping_2d::ProbabilityGrid* result);
proto::SubmapsOptions CreateSubmapsOptions( proto::SubmapsOptions CreateSubmapsOptions(
@ -46,7 +47,7 @@ proto::SubmapsOptions CreateSubmapsOptions(
struct Submap : public mapping::Submap { struct Submap : public mapping::Submap {
Submap(float high_resolution, float low_resolution, 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 high_resolution_hybrid_grid;
HybridGrid low_resolution_hybrid_grid; HybridGrid low_resolution_hybrid_grid;
@ -69,8 +70,8 @@ class Submaps : public mapping::Submaps {
const transform::Rigid3d& global_submap_pose, const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* response) const override; mapping::proto::SubmapQuery::Response* response) const override;
// Inserts 'laser_fan' into the Submap collection. // Inserts 'range_data' into the Submap collection.
void InsertLaserFan(const sensor::LaserFan& laser_fan); void InsertRangeData(const sensor::RangeData& range_data);
// Returns the 'high_resolution' HybridGrid to be used for matching. // Returns the 'high_resolution' HybridGrid to be used for matching.
const HybridGrid& high_resolution_matching_grid() const; const HybridGrid& high_resolution_matching_grid() const;
@ -110,13 +111,13 @@ class Submaps : public mapping::Submaps {
const proto::SubmapsOptions options_; const proto::SubmapsOptions options_;
std::vector<std::unique_ptr<Submap>> submaps_; std::vector<std::unique_ptr<Submap>> submaps_;
LaserFanInserter laser_fan_inserter_; RangeDataInserter range_data_inserter_;
// Number of LaserFans inserted. // Number of RangeData inserted.
int num_laser_fans_ = 0; int num_range_data_ = 0;
// Number of LaserFans inserted since the last Submap was added. // Number of RangeData inserted since the last Submap was added.
int num_laser_fans_in_last_submap_ = 0; int num_range_data_in_last_submap_ = 0;
}; };
} // namespace mapping_3d } // namespace mapping_3d

View File

@ -22,9 +22,9 @@ google_test(sensor_compressed_point_cloud_test
compressed_point_cloud_test.cc compressed_point_cloud_test.cc
) )
google_test(sensor_laser_test google_test(sensor_range_data_test
SRCS SRCS
laser_test.cc range_data_test.cc
) )
google_test(sensor_ordered_multi_queue_test google_test(sensor_ordered_multi_queue_test

View File

@ -19,8 +19,8 @@
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
namespace cartographer { namespace cartographer {

View File

@ -69,8 +69,8 @@ message LaserScan {
repeated Values intensity = 10; repeated Values intensity = 10;
} }
// Proto representation of ::cartographer::sensor::LaserFan // Proto representation of ::cartographer::sensor::RangeData
message LaserFan { message RangeData {
optional transform.proto.Vector3f origin = 1; optional transform.proto.Vector3f origin = 1;
optional PointCloud point_cloud = 2; optional PointCloud point_cloud = 2;
optional PointCloud missing_echo_point_cloud = 3; optional PointCloud missing_echo_point_cloud = 3;

View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/sensor/laser.h" #include "cartographer/sensor/range_data.h"
#include "cartographer/sensor/proto/sensor.pb.h" #include "cartographer/sensor/proto/sensor.pb.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
@ -72,55 +72,55 @@ PointCloudWithIntensities ToPointCloudWithIntensities(
return point_cloud; return point_cloud;
} }
proto::LaserFan ToProto(const LaserFan& laser_fan) { proto::RangeData ToProto(const RangeData& range_data) {
proto::LaserFan proto; proto::RangeData proto;
*proto.mutable_origin() = transform::ToProto(laser_fan.origin); *proto.mutable_origin() = transform::ToProto(range_data.origin);
*proto.mutable_point_cloud() = ToProto(laser_fan.returns); *proto.mutable_point_cloud() = ToProto(range_data.returns);
*proto.mutable_missing_echo_point_cloud() = ToProto(laser_fan.misses); *proto.mutable_missing_echo_point_cloud() = ToProto(range_data.misses);
std::copy(laser_fan.reflectivities.begin(), laser_fan.reflectivities.end(), std::copy(range_data.reflectivities.begin(), range_data.reflectivities.end(),
RepeatedFieldBackInserter(proto.mutable_reflectivity())); RepeatedFieldBackInserter(proto.mutable_reflectivity()));
return proto; return proto;
} }
LaserFan FromProto(const proto::LaserFan& proto) { RangeData FromProto(const proto::RangeData& proto) {
auto laser_fan = LaserFan{ auto range_data = RangeData{
transform::ToEigen(proto.origin()), ToPointCloud(proto.point_cloud()), transform::ToEigen(proto.origin()), ToPointCloud(proto.point_cloud()),
ToPointCloud(proto.missing_echo_point_cloud()), ToPointCloud(proto.missing_echo_point_cloud()),
}; };
std::copy(proto.reflectivity().begin(), proto.reflectivity().end(), std::copy(proto.reflectivity().begin(), proto.reflectivity().end(),
std::back_inserter(laser_fan.reflectivities)); std::back_inserter(range_data.reflectivities));
return laser_fan; return range_data;
} }
LaserFan TransformLaserFan(const LaserFan& laser_fan, RangeData TransformRangeData(const RangeData& range_data,
const transform::Rigid3f& transform) { const transform::Rigid3f& transform) {
return LaserFan{ return RangeData{
transform * laser_fan.origin, transform * range_data.origin,
TransformPointCloud(laser_fan.returns, transform), TransformPointCloud(range_data.returns, transform),
TransformPointCloud(laser_fan.misses, transform), TransformPointCloud(range_data.misses, transform),
laser_fan.reflectivities, range_data.reflectivities,
}; };
} }
LaserFan CropLaserFan(const LaserFan& laser_fan, const float min_z, RangeData CropRangeData(const RangeData& range_data, const float min_z,
const float max_z) { const float max_z) {
return LaserFan{laser_fan.origin, Crop(laser_fan.returns, min_z, max_z), return RangeData{range_data.origin, Crop(range_data.returns, min_z, max_z),
Crop(laser_fan.misses, 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<int> new_to_old; std::vector<int> new_to_old;
CompressedPointCloud compressed_returns = CompressedPointCloud compressed_returns =
CompressedPointCloud::CompressAndReturnOrder(laser_fan.returns, CompressedPointCloud::CompressAndReturnOrder(range_data.returns,
&new_to_old); &new_to_old);
return CompressedRangeData{ return CompressedRangeData{
laser_fan.origin, std::move(compressed_returns), range_data.origin, std::move(compressed_returns),
CompressedPointCloud(laser_fan.misses), CompressedPointCloud(range_data.misses),
ReorderReflectivities(laser_fan.reflectivities, new_to_old)}; ReorderReflectivities(range_data.reflectivities, new_to_old)};
} }
LaserFan Decompress(const CompressedRangeData& compressed_range_data) { RangeData Decompress(const CompressedRangeData& compressed_range_data) {
return LaserFan{compressed_range_data.origin, return RangeData{compressed_range_data.origin,
compressed_range_data.returns.Decompress(), compressed_range_data.returns.Decompress(),
compressed_range_data.misses.Decompress(), compressed_range_data.misses.Decompress(),
compressed_range_data.reflectivities}; compressed_range_data.reflectivities};

View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_SENSOR_LASER_H_ #ifndef CARTOGRAPHER_SENSOR_RANGE_DATA_H_
#define CARTOGRAPHER_SENSOR_LASER_H_ #define CARTOGRAPHER_SENSOR_RANGE_DATA_H_
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/sensor/compressed_point_cloud.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 // 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 // was detected, and were inserted at a configured distance. It is assumed that
// between the 'origin' and 'misses' is free space. // between the 'origin' and 'misses' is free space.
struct LaserFan { struct RangeData {
Eigen::Vector3f origin; Eigen::Vector3f origin;
PointCloud returns; PointCloud returns;
PointCloud misses; PointCloud misses;
@ -49,20 +49,20 @@ PointCloud ToPointCloud(const proto::LaserScan& proto);
PointCloudWithIntensities ToPointCloudWithIntensities( PointCloudWithIntensities ToPointCloudWithIntensities(
const proto::LaserScan& proto); const proto::LaserScan& proto);
// Converts 'laser_fan' to a proto::LaserFan. // Converts 'range_data' to a proto::RangeData.
proto::LaserFan ToProto(const LaserFan& laser_fan); proto::RangeData ToProto(const RangeData& range_data);
// Converts 'proto' to a LaserFan. // Converts 'proto' to a RangeData.
LaserFan FromProto(const proto::LaserFan& proto); RangeData FromProto(const proto::RangeData& proto);
LaserFan TransformLaserFan(const LaserFan& laser_fan, RangeData TransformRangeData(const RangeData& range_data,
const transform::Rigid3f& transform); const transform::Rigid3f& transform);
// Crops 'laser_fan' according to the region defined by 'min_z' and 'max_z'. // Crops 'range_data' according to the region defined by 'min_z' and 'max_z'.
LaserFan CropLaserFan(const LaserFan& laser_fan, float min_z, float 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 // Like RangeData but with compressed point clouds. The point order changes
// when converting from LaserFan. // when converting from RangeData.
struct CompressedRangeData { struct CompressedRangeData {
Eigen::Vector3f origin; Eigen::Vector3f origin;
CompressedPointCloud returns; CompressedPointCloud returns;
@ -72,11 +72,11 @@ struct CompressedRangeData {
std::vector<uint8> reflectivities; std::vector<uint8> 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 sensor
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_SENSOR_LASER_H_ #endif // CARTOGRAPHER_SENSOR_RANGE_DATA_H_

View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/sensor/laser.h" #include "cartographer/sensor/range_data.h"
#include <utility> #include <utility>
#include <vector> #include <vector>
@ -81,14 +81,14 @@ MATCHER_P(PairApproximatelyEquals, expected,
arg.second == expected.second; arg.second == expected.second;
} }
TEST(LaserTest, Compression) { TEST(RangeDataTest, Compression) {
const LaserFan laser_fan = { const RangeData range_data = {
Eigen::Vector3f(1, 1, 1), Eigen::Vector3f(1, 1, 1),
{Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6), {Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6),
Eigen::Vector3f(0, 1, 2)}, Eigen::Vector3f(0, 1, 2)},
{Eigen::Vector3f(7, 8, 9)}, {Eigen::Vector3f(7, 8, 9)},
{1, 2, 3}}; {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_TRUE(actual.origin.isApprox(Eigen::Vector3f(1, 1, 1), 1e-6));
EXPECT_EQ(3, actual.returns.size()); EXPECT_EQ(3, actual.returns.size());
EXPECT_EQ(1, actual.misses.size()); EXPECT_EQ(1, actual.misses.size());

View File

@ -59,9 +59,9 @@ TRAJECTORY_BUILDER_2D = {
submaps = { submaps = {
resolution = 0.05, resolution = 0.05,
half_length = 200., half_length = 200.,
num_laser_fans = 90, num_range_data = 90,
output_debug_images = false, output_debug_images = false,
laser_fan_inserter = { range_data_inserter = {
insert_free_space = true, insert_free_space = true,
hit_probability = 0.55, hit_probability = 0.55,
miss_probability = 0.49, miss_probability = 0.49,

View File

@ -55,8 +55,8 @@ TRAJECTORY_BUILDER_3D = {
high_resolution = 0.10, high_resolution = 0.10,
high_resolution_max_range = 20., high_resolution_max_range = 20.,
low_resolution = 0.45, low_resolution = 0.45,
num_laser_fans = 160, num_range_data = 160,
laser_fan_inserter = { range_data_inserter = {
hit_probability = 0.55, hit_probability = 0.55,
miss_probability = 0.49, miss_probability = 0.49,
num_free_space_voxels = 2, num_free_space_voxels = 2,

View File

@ -164,22 +164,6 @@ cartographer.common.proto.CeresSolverOptions ceres_solver_options
Not yet documented. 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 cartographer.mapping_2d.proto.LocalTrajectoryBuilderOptions
=========================================================== ===========================================================
@ -200,7 +184,7 @@ float laser_missing_echo_ray_length
empty space. empty space.
float laser_voxel_filter_size 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. cropping.
bool use_online_correlative_scan_matching bool use_online_correlative_scan_matching
@ -237,6 +221,22 @@ bool use_imu_data
True if IMU data should be expected and used. 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 cartographer.mapping_2d.proto.SubmapsOptions
============================================ ============================================
@ -246,7 +246,7 @@ double resolution
double half_length double half_length
Half the width/height of each submap, its "radius". 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 before adding a new submap. Each submap will get twice the
number of scans inserted: First for initialization without being matched number of scans inserted: First for initialization without being matched
against, then while being matched. against, then while being matched.
@ -254,7 +254,7 @@ int32 num_laser_fans
bool output_debug_images bool output_debug_images
If enabled, submap%d.png images are written for debugging. 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. Not yet documented.
@ -334,22 +334,6 @@ double odometer_rotational_variance
Not yet documented. 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 cartographer.mapping_3d.proto.LocalTrajectoryBuilderOptions
=========================================================== ===========================================================
@ -392,6 +376,22 @@ double odometry_rotation_weight
Not yet documented. 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 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 Resolution of the 'low_resolution' version of the map in meters used for
local SLAM only. 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 before adding a new submap. Each submap will get twice the
number of scans inserted: First for initialization without being matched number of scans inserted: First for initialization without being matched
against, then while 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. Not yet documented.