12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/global_trajectory_builder_interface.h
View File

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

5
cartographer/mapping/map_builder.cc
View File

@ -27,7 +27,7 @@
#include "cartographer/mapping_2d/global_trajectory_builder.h"
#include "cartographer/mapping_3d/global_trajectory_builder.h"
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/sensor/voxel_filter.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
@ -141,7 +141,8 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
" submaps in this trajectory.";
}
response->set_submap_version(submaps->Get(submap_index)->end_laser_fan_index);
response->set_submap_version(
submaps->Get(submap_index)->end_range_data_index);
const std::vector<transform::Rigid3d> submap_transforms =
sparse_pose_graph_->GetSubmapTransforms(*submaps);
CHECK_EQ(submap_transforms.size(), submaps->size());

18
cartographer/mapping/submaps.h
View File

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

2
cartographer/mapping/trajectory_builder.h
View File

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

4
cartographer/mapping/trajectory_connectivity_test.cc
View File

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

6
cartographer/mapping/trajectory_node.h
View File

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

4
cartographer/mapping_2d/CMakeLists.txt
View File

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

6
cartographer/mapping_2d/global_trajectory_builder.cc
View File

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

52
cartographer/mapping_2d/local_trajectory_builder.cc
View File

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

14
cartographer/mapping_2d/local_trajectory_builder.h
View File

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

20
cartographer/mapping_2d/map_limits.h
View File

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

4
cartographer/mapping_2d/map_limits_test.cc
View File

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

2
cartographer/mapping_2d/proto/local_trajectory_builder_options.proto
View File

@ -33,7 +33,7 @@ message LocalTrajectoryBuilderOptions {
// empty space.
optional float laser_missing_echo_ray_length = 16;
// Voxel filter that gets applied to the horizontal laser immediately after
// Voxel filter that gets applied to the range data immediately after
// cropping.
optional float laser_voxel_filter_size = 3;

3
cartographer/mapping_2d/proto/probability_grid.proto
View File

@ -20,7 +20,8 @@ package cartographer.mapping_2d.proto;
message ProbabilityGrid {
optional MapLimits limits = 1;
// These values are actually int16s, but protos don't have a native int16 type.
// These values are actually int16s, but protos don't have a native int16
// type.
repeated int32 cells = 2;
repeated int32 update_indices = 3;
optional int32 max_x = 4;

2
cartographer/mapping_2d/proto/laser_fan_inserter_options.proto → cartographer/mapping_2d/proto/range_data_inserter_options.proto
View File

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

6
cartographer/mapping_2d/proto/submaps_options.proto
View File

@ -14,7 +14,7 @@
syntax = "proto2";
import "cartographer/mapping_2d/proto/laser_fan_inserter_options.proto";
import "cartographer/mapping_2d/proto/range_data_inserter_options.proto";
package cartographer.mapping_2d.proto;
@ -28,10 +28,10 @@ message SubmapsOptions {
// Number of scans before adding a new submap. Each submap will get twice the
// number of scans inserted: First for initialization without being matched
// against, then while being matched.
optional int32 num_laser_fans = 3;
optional int32 num_range_data = 3;
// If enabled, submap%d.png images are written for debugging.
optional bool output_debug_images = 4;
optional LaserFanInserterOptions laser_fan_inserter_options = 5;
optional RangeDataInserterOptions range_data_inserter_options = 5;
}

14
cartographer/mapping_2d/laser_fan_inserter.cc → cartographer/mapping_2d/range_data_inserter.cc
View File

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

26
cartographer/mapping_2d/laser_fan_inserter.h → cartographer/mapping_2d/range_data_inserter.h
View File

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

34
cartographer/mapping_2d/laser_fan_inserter_test.cc → cartographer/mapping_2d/range_data_inserter_test.cc
View File

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

18
cartographer/mapping_2d/ray_casting.cc
View File

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

6
cartographer/mapping_2d/ray_casting.h
View File

@ -21,16 +21,16 @@
#include "cartographer/mapping_2d/map_limits.h"
#include "cartographer/mapping_2d/xy_index.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace mapping_2d {
// For each ray in 'laser_fan', calls 'hit_visitor' and 'miss_visitor' on the
// For each ray in 'range_data', calls 'hit_visitor' and 'miss_visitor' on the
// appropriate cells. Hits are handled before misses.
void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
void CastRays(const sensor::RangeData& range_data, const MapLimits& limits,
const std::function<void(const Eigen::Array2i&)>& hit_visitor,
const std::function<void(const Eigen::Array2i&)>& miss_visitor);

19
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

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

12
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc
View File

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

10
cartographer/mapping_2d/sparse_pose_graph.cc
View File

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

4
cartographer/mapping_2d/sparse_pose_graph.h
View File

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

14
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

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

37
cartographer/mapping_2d/submaps.cc
View File

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

20
cartographer/mapping_2d/submaps.h
View File

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

22
cartographer/mapping_2d/submaps_test.cc
View File

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

4
cartographer/mapping_3d/CMakeLists.txt
View File

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

2
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -53,7 +53,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
}
const int trajectory_node_index = sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->laser_fan_in_tracking,
insertion_result->time, insertion_result->range_data_in_tracking,
insertion_result->pose_observation, insertion_result->covariance_estimate,
insertion_result->submaps, insertion_result->matching_submap,
insertion_result->insertion_submaps);

57
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

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

10
cartographer/mapping_3d/kalman_local_trajectory_builder.h
View File

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

12
cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc
View File

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

4
cartographer/mapping_3d/local_trajectory_builder_interface.h
View File

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

50
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

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

8
cartographer/mapping_3d/optimizing_local_trajectory_builder.h
View File

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

2
cartographer/mapping_3d/proto/laser_fan_inserter_options.proto → cartographer/mapping_3d/proto/range_data_inserter_options.proto
View File

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

6
cartographer/mapping_3d/proto/submaps_options.proto
View File

@ -14,7 +14,7 @@
syntax = "proto2";
import "cartographer/mapping_3d/proto/laser_fan_inserter_options.proto";
import "cartographer/mapping_3d/proto/range_data_inserter_options.proto";
package cartographer.mapping_3d.proto;
@ -34,7 +34,7 @@ message SubmapsOptions {
// Number of scans before adding a new submap. Each submap will get twice the
// number of scans inserted: First for initialization without being matched
// against, then while being matched.
optional int32 num_laser_fans = 2;
optional int32 num_range_data = 2;
optional LaserFanInserterOptions laser_fan_inserter_options = 3;
optional RangeDataInserterOptions range_data_inserter_options = 3;
}

22
cartographer/mapping_3d/laser_fan_inserter.cc → cartographer/mapping_3d/range_data_inserter.cc
View File

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

27
cartographer/mapping_3d/laser_fan_inserter.h → cartographer/mapping_3d/range_data_inserter.h
View File

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

24
cartographer/mapping_3d/laser_fan_inserter_test.cc → cartographer/mapping_3d/range_data_inserter_test.cc
View File

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

16
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

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

6
cartographer/mapping_3d/sparse_pose_graph.cc
View File

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

5
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -66,12 +66,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
SparsePoseGraph(const SparsePoseGraph&) = delete;
SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
// Adds a new 'laser_fan_in_tracking' observation at 'time', and a 'pose'
// Adds a new 'range_data_in_tracking' observation at 'time', and a 'pose'
// that will later be optimized. The 'pose' was determined by scan matching
// against the 'matching_submap' and the scan was inserted into the
// 'insertion_submaps'. The index into the vector of trajectory nodes as
// used with GetTrajectoryNodes() is returned.
int AddScan(common::Time time, const sensor::LaserFan& laser_fan_in_tracking,
int AddScan(common::Time time,
const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& pose_covariance,
const Submaps* submaps, const Submap* matching_submap,

182
cartographer/mapping_3d/submaps.cc
View File

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

27
cartographer/mapping_3d/submaps.h
View File

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

4
cartographer/sensor/CMakeLists.txt
View File

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

2
cartographer/sensor/data.h
View File

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

4
cartographer/sensor/proto/sensor.proto
View File

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

54
cartographer/sensor/laser.cc → cartographer/sensor/range_data.cc
View File

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

30
cartographer/sensor/laser.h → cartographer/sensor/range_data.h
View File

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

8
cartographer/sensor/laser_test.cc → cartographer/sensor/range_data_test.cc
View File

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

4
configuration_files/trajectory_builder_2d.lua
View File

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

4
configuration_files/trajectory_builder_3d.lua
View File

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

74
docs/source/configuration.rst
View File

@ -164,22 +164,6 @@ cartographer.common.proto.CeresSolverOptions ceres_solver_options
Not yet documented.
cartographer.mapping_2d.proto.LaserFanInserterOptions
=====================================================
double hit_probability
Probability change for a hit (this will be converted to odds and therefore
must be greater than 0.5).
double miss_probability
Probability change for a miss (this will be converted to odds and therefore
must be less than 0.5).
bool insert_free_space
If 'false', free space will not change the probabilities in the occupancy
grid.
cartographer.mapping_2d.proto.LocalTrajectoryBuilderOptions
===========================================================
@ -200,7 +184,7 @@ float laser_missing_echo_ray_length
empty space.
float laser_voxel_filter_size
Voxel filter that gets applied to the horizontal laser immediately after
Voxel filter that gets applied to the range data immediately after
cropping.
bool use_online_correlative_scan_matching
@ -237,6 +221,22 @@ bool use_imu_data
True if IMU data should be expected and used.
cartographer.mapping_2d.proto.RangeDataInserterOptions
======================================================
double hit_probability
Probability change for a hit (this will be converted to odds and therefore
must be greater than 0.5).
double miss_probability
Probability change for a miss (this will be converted to odds and therefore
must be less than 0.5).
bool insert_free_space
If 'false', free space will not change the probabilities in the occupancy
grid.
cartographer.mapping_2d.proto.SubmapsOptions
============================================
@ -246,7 +246,7 @@ double resolution
double half_length
Half the width/height of each submap, its "radius".
int32 num_laser_fans
int32 num_range_data
Number of scans before adding a new submap. Each submap will get twice the
number of scans inserted: First for initialization without being matched
against, then while being matched.
@ -254,7 +254,7 @@ int32 num_laser_fans
bool output_debug_images
If enabled, submap%d.png images are written for debugging.
cartographer.mapping_2d.proto.LaserFanInserterOptions laser_fan_inserter_options
cartographer.mapping_2d.proto.RangeDataInserterOptions range_data_inserter_options
Not yet documented.
@ -334,22 +334,6 @@ double odometer_rotational_variance
Not yet documented.
cartographer.mapping_3d.proto.LaserFanInserterOptions
=====================================================
double hit_probability
Probability change for a hit (this will be converted to odds and therefore
must be greater than 0.5).
double miss_probability
Probability change for a miss (this will be converted to odds and therefore
must be less than 0.5).
int32 num_free_space_voxels
Up to how many free space voxels are updated for scan matching.
0 disables free space.
cartographer.mapping_3d.proto.LocalTrajectoryBuilderOptions
===========================================================
@ -392,6 +376,22 @@ double odometry_rotation_weight
Not yet documented.
cartographer.mapping_3d.proto.RangeDataInserterOptions
======================================================
double hit_probability
Probability change for a hit (this will be converted to odds and therefore
must be greater than 0.5).
double miss_probability
Probability change for a miss (this will be converted to odds and therefore
must be less than 0.5).
int32 num_free_space_voxels
Up to how many free space voxels are updated for scan matching.
0 disables free space.
cartographer.mapping_3d.proto.SubmapsOptions
============================================
@ -407,12 +407,12 @@ double low_resolution
Resolution of the 'low_resolution' version of the map in meters used for
local SLAM only.
int32 num_laser_fans
int32 num_range_data
Number of scans before adding a new submap. Each submap will get twice the
number of scans inserted: First for initialization without being matched
against, then while being matched.
cartographer.mapping_3d.proto.LaserFanInserterOptions laser_fan_inserter_options
cartographer.mapping_3d.proto.RangeDataInserterOptions range_data_inserter_options
Not yet documented.