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

Rename LaserFan3D to LaserFan. (#64)

master
Wolfgang Hess 2016-10-14 12:06:54 +02:00 committed by GitHub
parent 9020f71605
commit 098349f67e
41 changed files with 165 additions and 171 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer/mapping/global_trajectory_builder_interface.h
View File

@ -78,13 +78,13 @@ class GlobalTrajectoryBuilderInterface {
virtual kalman_filter::PoseTracker* pose_tracker() const = 0;
virtual const PoseEstimate& pose_estimate() const = 0;
virtual void AddHorizontalLaserFan(common::Time,
const sensor::LaserFan3D& laser_fan) = 0;
virtual void AddHorizontalLaserFan(common::Time time,
const sensor::LaserFan& laser_fan) = 0;
virtual void AddImuData(common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) = 0;
virtual void AddLaserFan3D(common::Time time,
const sensor::LaserFan3D& laser_fan) = 0;
const sensor::LaserFan& laser_fan) = 0;
virtual void AddOdometerPose(
common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) = 0;

4
cartographer/mapping/trajectory_node.h
View File

@ -35,10 +35,10 @@ struct TrajectoryNode {
common::Time time;
// LaserFan in 'pose' frame. Only used in the 2D case.
sensor::LaserFan3D laser_fan_2d;
sensor::LaserFan laser_fan_2d;
// LaserFan in 'pose' frame. Only used in the 3D case.
sensor::CompressedLaserFan3D laser_fan_3d;
sensor::CompressedLaserFan laser_fan_3d;
// Trajectory this node belongs to.
// TODO(jmason): The naming here is confusing because 'trajectory' doesn't

2
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -41,7 +41,7 @@ kalman_filter::PoseTracker* GlobalTrajectoryBuilder::pose_tracker() const {
}
void GlobalTrajectoryBuilder::AddHorizontalLaserFan(
const common::Time time, const sensor::LaserFan3D& laser_fan) {
const common::Time time, const sensor::LaserFan& laser_fan) {
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
local_trajectory_builder_.AddHorizontalLaserFan(time, laser_fan);
if (insertion_result != nullptr) {

4
cartographer/mapping_2d/global_trajectory_builder.h
View File

@ -41,13 +41,13 @@ class GlobalTrajectoryBuilder
const override;
void AddHorizontalLaserFan(common::Time time,
const sensor::LaserFan3D& laser_fan) override;
const sensor::LaserFan& laser_fan) override;
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override;
void AddOdometerPose(
common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) override;
void AddLaserFan3D(common::Time, const sensor::LaserFan3D&) override {
void AddLaserFan3D(common::Time, const sensor::LaserFan&) override {
LOG(FATAL) << "Not implemented.";
};

2
cartographer/mapping_2d/laser_fan_inserter.cc
View File

@ -51,7 +51,7 @@ LaserFanInserter::LaserFanInserter(
miss_table_(mapping::ComputeLookupTableToApplyOdds(
mapping::Odds(options.miss_probability()))) {}
void LaserFanInserter::Insert(const sensor::LaserFan3D& laser_fan,
void LaserFanInserter::Insert(const sensor::LaserFan& laser_fan,
ProbabilityGrid* const probability_grid) const {
CHECK_NOTNULL(probability_grid)->StartUpdate();

2
cartographer/mapping_2d/laser_fan_inserter.h
View File

@ -42,7 +42,7 @@ class LaserFanInserter {
LaserFanInserter& operator=(const LaserFanInserter&) = delete;
// Inserts 'laser_fan' into 'probability_grid'.
void Insert(const sensor::LaserFan3D& laser_fan,
void Insert(const sensor::LaserFan& laser_fan,
ProbabilityGrid* probability_grid) const;
const std::vector<uint16>& hit_table() const { return hit_table_; }

2
cartographer/mapping_2d/laser_fan_inserter_test.cc
View File

@ -44,7 +44,7 @@ class LaserFanInserterTest : public ::testing::Test {
}
void InsertPointCloud() {
sensor::LaserFan3D laser_fan;
sensor::LaserFan laser_fan;
laser_fan.returns.emplace_back(-3.5, 0.5, 0.f);
laser_fan.returns.emplace_back(-2.5, 1.5, 0.f);
laser_fan.returns.emplace_back(-1.5, 2.5, 0.f);

20
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -78,13 +78,13 @@ kalman_filter::PoseTracker* LocalTrajectoryBuilder::pose_tracker() const {
return pose_tracker_.get();
}
sensor::LaserFan3D LocalTrajectoryBuilder::BuildCroppedLaserFan(
sensor::LaserFan LocalTrajectoryBuilder::BuildCroppedLaserFan(
const transform::Rigid3f& tracking_to_tracking_2d,
const sensor::LaserFan3D& laser_fan) const {
const sensor::LaserFan3D cropped_fan = sensor::CropLaserFan(
sensor::TransformLaserFan3D(laser_fan, tracking_to_tracking_2d),
const sensor::LaserFan& laser_fan) const {
const sensor::LaserFan cropped_fan = sensor::CropLaserFan(
sensor::TransformLaserFan(laser_fan, tracking_to_tracking_2d),
options_.horizontal_laser_min_z(), options_.horizontal_laser_max_z());
return sensor::LaserFan3D{
return sensor::LaserFan{
cropped_fan.origin,
sensor::VoxelFiltered(cropped_fan.returns,
options_.horizontal_laser_voxel_filter_size()),
@ -95,7 +95,7 @@ sensor::LaserFan3D LocalTrajectoryBuilder::BuildCroppedLaserFan(
void LocalTrajectoryBuilder::ScanMatch(
common::Time time, const transform::Rigid3d& pose_prediction,
const transform::Rigid3d& tracking_to_tracking_2d,
const sensor::LaserFan3D& laser_fan_in_tracking_2d,
const sensor::LaserFan& laser_fan_in_tracking_2d,
transform::Rigid3d* pose_observation,
kalman_filter::PoseCovariance* covariance_observation) {
const ProbabilityGrid& probability_grid =
@ -140,7 +140,7 @@ void LocalTrajectoryBuilder::ScanMatch(
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
LocalTrajectoryBuilder::AddHorizontalLaserFan(
const common::Time time, const sensor::LaserFan3D& laser_fan) {
const common::Time time, const sensor::LaserFan& laser_fan) {
// Initialize pose tracker now if we do not ever use an IMU.
if (!options_.use_imu_data()) {
InitializePoseTracker(time);
@ -165,7 +165,7 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
-transform::GetYaw(pose_prediction), Eigen::Vector3d::UnitZ())) *
pose_prediction.rotation());
const sensor::LaserFan3D laser_fan_in_tracking_2d =
const sensor::LaserFan laser_fan_in_tracking_2d =
BuildCroppedLaserFan(tracking_to_tracking_2d.cast<float>(), laser_fan);
if (laser_fan_in_tracking_2d.returns.empty()) {
@ -212,8 +212,8 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
for (int insertion_index : submaps_.insertion_indices()) {
insertion_submaps.push_back(submaps_.Get(insertion_index));
}
submaps_.InsertLaserFan(TransformLaserFan3D(
laser_fan_in_tracking_2d, tracking_2d_to_map.cast<float>()));
submaps_.InsertLaserFan(TransformLaserFan(laser_fan_in_tracking_2d,
tracking_2d_to_map.cast<float>()));
return common::make_unique<InsertionResult>(InsertionResult{
time, &submaps_, matching_submap, insertion_submaps,

10
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -49,7 +49,7 @@ class LocalTrajectoryBuilder {
std::vector<const mapping::Submap*> insertion_submaps;
transform::Rigid3d tracking_to_tracking_2d;
transform::Rigid3d tracking_2d_to_map;
sensor::LaserFan3D laser_fan_in_tracking_2d;
sensor::LaserFan laser_fan_in_tracking_2d;
transform::Rigid2d pose_estimate_2d;
kalman_filter::PoseCovariance covariance_estimate;
};
@ -64,7 +64,7 @@ class LocalTrajectoryBuilder {
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
const;
std::unique_ptr<InsertionResult> AddHorizontalLaserFan(
common::Time, const sensor::LaserFan3D& laser_fan);
common::Time, const sensor::LaserFan& laser_fan);
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void AddOdometerPose(common::Time time, const transform::Rigid3d& pose,
@ -76,15 +76,15 @@ class LocalTrajectoryBuilder {
private:
// Transforms 'laser_scan', crops and voxel filters.
sensor::LaserFan3D BuildCroppedLaserFan(
sensor::LaserFan BuildCroppedLaserFan(
const transform::Rigid3f& tracking_to_tracking_2d,
const sensor::LaserFan3D& laser_fan) const;
const sensor::LaserFan& laser_fan) const;
// Scan match 'laser_fan_in_tracking_2d' and fill in the
// 'pose_observation' and 'covariance_observation' with the result.
void ScanMatch(common::Time time, const transform::Rigid3d& pose_prediction,
const transform::Rigid3d& tracking_to_tracking_2d,
const sensor::LaserFan3D& laser_fan_in_tracking_2d,
const sensor::LaserFan& laser_fan_in_tracking_2d,
transform::Rigid3d* pose_observation,
kalman_filter::PoseCovariance* covariance_observation);

4
cartographer/mapping_2d/map_limits.h
View File

@ -102,14 +102,14 @@ class MapLimits {
for (const auto& node : trajectory_nodes) {
const auto& data = *node.constant_data;
if (!data.laser_fan_3d.returns.empty()) {
const sensor::LaserFan3D laser_fan = sensor::TransformLaserFan3D(
const sensor::LaserFan laser_fan = sensor::TransformLaserFan(
Decompress(data.laser_fan_3d), node.pose.cast<float>());
bounding_box.extend(laser_fan.origin.head<2>());
for (const Eigen::Vector3f& hit : laser_fan.returns) {
bounding_box.extend(hit.head<2>());
}
} else {
const sensor::LaserFan3D laser_fan = sensor::TransformLaserFan3D(
const sensor::LaserFan laser_fan = sensor::TransformLaserFan(
data.laser_fan_2d, node.pose.cast<float>());
bounding_box.extend(laser_fan.origin.head<2>());
for (const Eigen::Vector3f& hit : laser_fan.returns) {

6
cartographer/mapping_2d/map_limits_test.cc
View File

@ -39,12 +39,12 @@ TEST(MapLimitsTest, ConstructAndGet) {
TEST(MapLimitsTest, ComputeMapLimits) {
const mapping::TrajectoryNode::ConstantData constant_data{
common::FromUniversal(52),
sensor::LaserFan3D{
sensor::LaserFan{
Eigen::Vector3f::Zero(),
{Eigen::Vector3f(-30.f, 1.f, 0.f), Eigen::Vector3f(50.f, -10.f, 0.f)},
{}},
Compress(sensor::LaserFan3D{Eigen::Vector3f::Zero(), {}, {}, {}}),
nullptr, transform::Rigid3d::Identity()};
Compress(sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}, {}}), nullptr,
transform::Rigid3d::Identity()};
const mapping::TrajectoryNode trajectory_node{&constant_data,
transform::Rigid3d::Identity()};
constexpr double kResolution = 0.05;

2
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::LaserFan3D& laser_fan, const MapLimits& limits,
void CastRays(const sensor::LaserFan& laser_fan, 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;

2
cartographer/mapping_2d/ray_casting.h
View File

@ -30,7 +30,7 @@ namespace mapping_2d {
// For each ray in 'laser_fan', calls 'hit_visitor' and 'miss_visitor' on the
// appropriate cells. Hits are handled before misses.
void CastRays(const sensor::LaserFan3D& laser_fan, const MapLimits& limits,
void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
const std::function<void(const Eigen::Array2i&)>& hit_visitor,
const std::function<void(const Eigen::Array2i&)>& miss_visitor);

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

@ -150,7 +150,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
probability_grid.StartUpdate();
laser_fan_inserter.Insert(
sensor::LaserFan3D{
sensor::LaserFan{
Eigen::Vector3f(expected_pose.translation().x(),
expected_pose.translation().y(), 0.f),
sensor::TransformPointCloud(
@ -203,7 +203,7 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
probability_grid.StartUpdate();
laser_fan_inserter.Insert(
sensor::LaserFan3D{
sensor::LaserFan{
transform::Embed3D(expected_pose * perturbation).translation(),
sensor::TransformPointCloud(point_cloud,
transform::Embed3D(expected_pose)),

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

@ -58,7 +58,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
point_cloud_.emplace_back(-0.125f, 0.025f, 0.f);
probability_grid_.StartUpdate();
laser_fan_inserter_->Insert(
sensor::LaserFan3D{Eigen::Vector3f::Zero(), point_cloud_, {}},
sensor::LaserFan{Eigen::Vector3f::Zero(), point_cloud_, {}},
&probability_grid_);
{
auto parameter_dictionary = common::MakeDictionary(

6
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -86,7 +86,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
void SparsePoseGraph::AddScan(
common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::LaserFan3D& laser_fan_in_pose, const transform::Rigid2d& pose,
const sensor::LaserFan& laser_fan_in_pose, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance,
const mapping::Submaps* submaps,
const mapping::Submap* const matching_submap,
@ -100,8 +100,8 @@ void SparsePoseGraph::AddScan(
constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{
time, laser_fan_in_pose,
Compress(sensor::LaserFan3D{Eigen::Vector3f::Zero(), {}, {}, {}}),
submaps, transform::Rigid3d(tracking_to_pose)});
Compress(sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}, {}}), submaps,
transform::Rigid3d(tracking_to_pose)});
trajectory_nodes_.push_back(mapping::TrajectoryNode{
&constant_node_data_->back(), optimized_pose,
});

2
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -70,7 +70,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// by scan matching against the 'matching_submap' and the scan was inserted
// into the 'insertion_submaps'.
void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::LaserFan3D& laser_fan_in_pose,
const sensor::LaserFan& laser_fan_in_pose,
const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& pose_covariance,
const mapping::Submaps* submaps,

4
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -157,10 +157,10 @@ class SparsePoseGraphTest : public ::testing::Test {
for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index));
}
const sensor::LaserFan3D laser_fan{
const sensor::LaserFan laser_fan{
Eigen::Vector3f::Zero(), new_point_cloud, {}};
const transform::Rigid2d pose_estimate = noise * current_pose_;
submaps_->InsertLaserFan(TransformLaserFan3D(
submaps_->InsertLaserFan(TransformLaserFan(
laser_fan, transform::Embed3D(pose_estimate.cast<float>())));
sparse_pose_graph_->AddScan(common::FromUniversal(0),
transform::Rigid3d::Identity(), laser_fan,

2
cartographer/mapping_2d/submaps.cc
View File

@ -114,7 +114,7 @@ Submaps::Submaps(const proto::SubmapsOptions& options)
AddSubmap(Eigen::Vector2f::Zero());
}
void Submaps::InsertLaserFan(const sensor::LaserFan3D& laser_fan) {
void Submaps::InsertLaserFan(const sensor::LaserFan& laser_fan) {
CHECK_LT(num_laser_fans_, std::numeric_limits<int>::max());
++num_laser_fans_;
for (const int index : insertion_indices()) {

2
cartographer/mapping_2d/submaps.h
View File

@ -64,7 +64,7 @@ class Submaps : public mapping::Submaps {
mapping::proto::SubmapQuery::Response* response) override;
// Inserts 'laser_fan' into the Submap collection.
void InsertLaserFan(const sensor::LaserFan3D& laser_fan);
void InsertLaserFan(const sensor::LaserFan& laser_fan);
private:
void FinishSubmap(int index);

4
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -49,8 +49,8 @@ void GlobalTrajectoryBuilder::AddImuData(
sparse_pose_graph_->AddImuData(time, linear_acceleration, angular_velocity);
}
void GlobalTrajectoryBuilder::AddLaserFan3D(
const common::Time time, const sensor::LaserFan3D& laser_fan) {
void GlobalTrajectoryBuilder::AddLaserFan3D(const common::Time time,
const sensor::LaserFan& laser_fan) {
auto insertion_result =
local_trajectory_builder_->AddLaserFan3D(time, laser_fan);

4
cartographer/mapping_3d/global_trajectory_builder.h
View File

@ -43,13 +43,13 @@ class GlobalTrajectoryBuilder
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override;
void AddLaserFan3D(common::Time time,
const sensor::LaserFan3D& laser_fan) override;
const sensor::LaserFan& laser_fan) override;
void AddOdometerPose(
common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) override;
const PoseEstimate& pose_estimate() const override;
void AddHorizontalLaserFan(common::Time, const sensor::LaserFan3D&) override {
void AddHorizontalLaserFan(common::Time, const sensor::LaserFan&) override {
LOG(FATAL) << "Not implemented.";
}

27
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -74,8 +74,8 @@ void KalmanLocalTrajectoryBuilder::AddImuData(
}
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::AddLaserFan3D(
const common::Time time, const sensor::LaserFan3D& laser_fan) {
KalmanLocalTrajectoryBuilder::AddLaserFan3D(const common::Time time,
const sensor::LaserFan& laser_fan) {
if (!pose_tracker_) {
LOG(INFO) << "PoseTracker not yet initialized.";
return nullptr;
@ -87,14 +87,13 @@ KalmanLocalTrajectoryBuilder::AddLaserFan3D(
time, &pose_prediction, &unused_covariance_prediction);
if (num_accumulated_ == 0) {
first_pose_prediction_ = pose_prediction.cast<float>();
accumulated_laser_fan_ =
sensor::LaserFan3D{Eigen::Vector3f::Zero(), {}, {}};
accumulated_laser_fan_ = sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}};
}
const transform::Rigid3f tracking_delta =
first_pose_prediction_.inverse() * pose_prediction.cast<float>();
const sensor::LaserFan3D laser_fan_in_first_tracking =
sensor::TransformLaserFan3D(laser_fan, tracking_delta);
const sensor::LaserFan laser_fan_in_first_tracking =
sensor::TransformLaserFan(laser_fan, tracking_delta);
for (const Eigen::Vector3f& laser_return :
laser_fan_in_first_tracking.returns) {
const Eigen::Vector3f delta =
@ -117,17 +116,17 @@ KalmanLocalTrajectoryBuilder::AddLaserFan3D(
if (num_accumulated_ >= options_.scans_per_accumulation()) {
num_accumulated_ = 0;
return AddAccumulatedLaserFan3D(
time, sensor::TransformLaserFan3D(accumulated_laser_fan_,
tracking_delta.inverse()));
return AddAccumulatedLaserFan(
time, sensor::TransformLaserFan(accumulated_laser_fan_,
tracking_delta.inverse()));
}
return nullptr;
}
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan3D(
const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking) {
const sensor::LaserFan3D filtered_laser_fan = {
KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan(
const common::Time time, const sensor::LaserFan& laser_fan_in_tracking) {
const sensor::LaserFan filtered_laser_fan = {
laser_fan_in_tracking.origin,
sensor::VoxelFiltered(laser_fan_in_tracking.returns,
options_.laser_voxel_filter_size()),
@ -215,7 +214,7 @@ void KalmanLocalTrajectoryBuilder::AddTrajectoryNodeIndex(
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking,
const common::Time time, const sensor::LaserFan& laser_fan_in_tracking,
const transform::Rigid3d& pose_observation,
const kalman_filter::PoseCovariance& covariance_estimate) {
if (motion_filter_.IsSimilar(time, pose_observation)) {
@ -227,7 +226,7 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index));
}
submaps_->InsertLaserFan(sensor::TransformLaserFan3D(
submaps_->InsertLaserFan(sensor::TransformLaserFan(
laser_fan_in_tracking, pose_observation.cast<float>()));
return std::unique_ptr<InsertionResult>(new InsertionResult{
time, laser_fan_in_tracking, pose_observation, covariance_estimate,

10
cartographer/mapping_3d/kalman_local_trajectory_builder.h
View File

@ -49,7 +49,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override;
std::unique_ptr<InsertionResult> AddLaserFan3D(
common::Time time, const sensor::LaserFan3D& laser_fan) override;
common::Time time, const sensor::LaserFan& laser_fan) override;
void AddOdometerPose(
common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) override;
@ -60,11 +60,11 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
kalman_filter::PoseTracker* pose_tracker() const override;
private:
std::unique_ptr<InsertionResult> AddAccumulatedLaserFan3D(
common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking);
std::unique_ptr<InsertionResult> AddAccumulatedLaserFan(
common::Time time, const sensor::LaserFan& laser_fan_in_tracking);
std::unique_ptr<InsertionResult> InsertIntoSubmap(
const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking,
const common::Time time, const sensor::LaserFan& laser_fan_in_tracking,
const transform::Rigid3d& pose_observation,
const kalman_filter::PoseCovariance& covariance_estimate);
@ -85,7 +85,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
int num_accumulated_;
transform::Rigid3f first_pose_prediction_;
sensor::LaserFan3D accumulated_laser_fan_;
sensor::LaserFan accumulated_laser_fan_;
};
} // namespace mapping_3d

2
cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc
View File

@ -186,7 +186,7 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
return first * (to - from) + from;
}
sensor::LaserFan3D GenerateLaserFan(const transform::Rigid3d& pose) {
sensor::LaserFan GenerateLaserFan(const transform::Rigid3d& pose) {
// 360 degree rays at 16 angles.
sensor::PointCloud directions_in_laser_frame;
for (int r = -8; r != 8; ++r) {

2
cartographer/mapping_3d/laser_fan_inserter.cc
View File

@ -76,7 +76,7 @@ LaserFanInserter::LaserFanInserter(
miss_table_(mapping::ComputeLookupTableToApplyOdds(
mapping::Odds(options_.miss_probability()))) {}
void LaserFanInserter::Insert(const sensor::LaserFan3D& laser_fan,
void LaserFanInserter::Insert(const sensor::LaserFan& laser_fan,
HybridGrid* hybrid_grid) const {
CHECK_NOTNULL(hybrid_grid)->StartUpdate();

3
cartographer/mapping_3d/laser_fan_inserter.h
View File

@ -36,8 +36,7 @@ class LaserFanInserter {
LaserFanInserter& operator=(const LaserFanInserter&) = delete;
// Inserts 'laser_fan' into 'hybrid_grid'.
void Insert(const sensor::LaserFan3D& laser_fan,
HybridGrid* hybrid_grid) const;
void Insert(const sensor::LaserFan& laser_fan, HybridGrid* hybrid_grid) const;
private:
const proto::LaserFanInserterOptions options_;

2
cartographer/mapping_3d/laser_fan_inserter_test.cc
View File

@ -46,7 +46,7 @@ class LaserFanInserterTest : public ::testing::Test {
{-1.5f, 0.5f, 4.5f},
{-0.5f, 1.5f, 4.5f},
{0.5f, 2.5f, 4.5f}};
laser_fan_inserter_->Insert(sensor::LaserFan3D{origin, laser_returns, {}},
laser_fan_inserter_->Insert(sensor::LaserFan{origin, laser_returns, {}},
&hybrid_grid_);
}

4
cartographer/mapping_3d/local_trajectory_builder_interface.h
View File

@ -35,7 +35,7 @@ class LocalTrajectoryBuilderInterface {
struct InsertionResult {
common::Time time;
sensor::LaserFan3D laser_fan_in_tracking;
sensor::LaserFan laser_fan_in_tracking;
transform::Rigid3d pose_observation;
kalman_filter::PoseCovariance covariance_estimate;
const Submaps* submaps;
@ -54,7 +54,7 @@ class LocalTrajectoryBuilderInterface {
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) = 0;
virtual std::unique_ptr<InsertionResult> AddLaserFan3D(
common::Time time, const sensor::LaserFan3D& laser_fan) = 0;
common::Time time, const sensor::LaserFan& laser_fan) = 0;
virtual void AddOdometerPose(
common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) = 0;

18
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -134,7 +134,7 @@ void OptimizingLocalTrajectoryBuilder::AddOdometerPose(
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::AddLaserFan3D(
const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking) {
const common::Time time, const sensor::LaserFan& laser_fan_in_tracking) {
CHECK_GT(laser_fan_in_tracking.returns.size(), 0);
// TODO(hrapp): Handle misses.
@ -335,7 +335,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
num_accumulated_ = 0;
const transform::Rigid3d optimized_pose = batches_.back().state.ToRigid();
sensor::LaserFan3D accumulated_laser_fan_in_tracking = {
sensor::LaserFan accumulated_laser_fan_in_tracking = {
Eigen::Vector3f::Zero(), {}, {}};
for (const auto& batch : batches_) {
@ -346,15 +346,15 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
}
}
return AddAccumulatedLaserFan3D(time, optimized_pose,
accumulated_laser_fan_in_tracking);
return AddAccumulatedLaserFan(time, optimized_pose,
accumulated_laser_fan_in_tracking);
}
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::AddAccumulatedLaserFan3D(
OptimizingLocalTrajectoryBuilder::AddAccumulatedLaserFan(
const common::Time time, const transform::Rigid3d& optimized_pose,
const sensor::LaserFan3D& laser_fan_in_tracking) {
const sensor::LaserFan3D filtered_laser_fan = {
const sensor::LaserFan& laser_fan_in_tracking) {
const sensor::LaserFan filtered_laser_fan = {
laser_fan_in_tracking.origin,
sensor::VoxelFiltered(laser_fan_in_tracking.returns,
options_.laser_voxel_filter_size()),
@ -393,7 +393,7 @@ void OptimizingLocalTrajectoryBuilder::AddTrajectoryNodeIndex(
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking,
const common::Time time, const sensor::LaserFan& laser_fan_in_tracking,
const transform::Rigid3d& pose_observation,
const kalman_filter::PoseCovariance& covariance_estimate) {
if (motion_filter_.IsSimilar(time, pose_observation)) {
@ -405,7 +405,7 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index));
}
submaps_->InsertLaserFan(sensor::TransformLaserFan3D(
submaps_->InsertLaserFan(sensor::TransformLaserFan(
laser_fan_in_tracking, pose_observation.cast<float>()));
return std::unique_ptr<InsertionResult>(new InsertionResult{
time, laser_fan_in_tracking, pose_observation, covariance_estimate,

8
cartographer/mapping_3d/optimizing_local_trajectory_builder.h
View File

@ -54,7 +54,7 @@ class OptimizingLocalTrajectoryBuilder
const Eigen::Vector3d& angular_velocity) override;
std::unique_ptr<InsertionResult> AddLaserFan3D(
common::Time time,
const sensor::LaserFan3D& laser_fan_in_tracking) override;
const sensor::LaserFan& laser_fan_in_tracking) override;
void AddOdometerPose(
const common::Time time, const transform::Rigid3d& pose,
@ -106,12 +106,12 @@ class OptimizingLocalTrajectoryBuilder
void RemoveObsoleteSensorData();
std::unique_ptr<InsertionResult> AddAccumulatedLaserFan3D(
std::unique_ptr<InsertionResult> AddAccumulatedLaserFan(
common::Time time, const transform::Rigid3d& pose_observation,
const sensor::LaserFan3D& laser_fan_in_tracking);
const sensor::LaserFan& laser_fan_in_tracking);
std::unique_ptr<InsertionResult> InsertIntoSubmap(
const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking,
const common::Time time, const sensor::LaserFan& laser_fan_in_tracking,
const transform::Rigid3d& pose_observation,
const kalman_filter::PoseCovariance& covariance_estimate);

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

@ -89,12 +89,11 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
HybridGrid hybrid_grid(0.05f /* resolution */,
Eigen::Vector3f(0.5f, 1.5f, 2.5f) /* origin */);
hybrid_grid.StartUpdate();
laser_fan_inserter.Insert(
sensor::LaserFan3D{
expected_pose.translation(),
sensor::TransformPointCloud(point_cloud, expected_pose),
{}},
&hybrid_grid);
laser_fan_inserter.Insert(sensor::LaserFan{expected_pose.translation(),
sensor::TransformPointCloud(
point_cloud, expected_pose),
{}},
&hybrid_grid);
FastCorrelativeScanMatcher fast_correlative_scan_matcher(hybrid_grid, {},
options);

4
cartographer/mapping_3d/sparse_pose_graph.cc
View File

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

3
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -70,8 +70,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// against the 'matching_submap' and the scan was inserted into the
// 'insertion_submaps'. The index into the vector of trajectory nodes as
// used with GetTrajectoryNodes() is returned.
int AddScan(common::Time time,
const sensor::LaserFan3D& laser_fan_in_tracking,
int AddScan(common::Time time, const sensor::LaserFan& laser_fan_in_tracking,
const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& pose_covariance,
const Submaps* submaps, const Submap* matching_submap,

21
cartographer/mapping_3d/submaps.cc
View File

@ -39,16 +39,17 @@ struct LaserSegment {
// We compute a slice around the xy-plane. 'transform' is applied to the laser
// rays in global map frame to allow choosing an arbitrary slice.
void GenerateSegmentForSlice(const sensor::LaserFan3D& laser_fan_3d,
void GenerateSegmentForSlice(const sensor::LaserFan& laser_fan,
const transform::Rigid3f& pose,
const transform::Rigid3f& transform,
std::vector<LaserSegment>* segments) {
const sensor::LaserFan3D laser_fan =
sensor::TransformLaserFan3D(laser_fan_3d, transform * pose);
segments->reserve(laser_fan.returns.size());
for (const Eigen::Vector3f& hit : laser_fan.returns) {
const Eigen::Vector2f laser_origin_xy = laser_fan.origin.head<2>();
const float laser_origin_z = laser_fan.origin.z();
const sensor::LaserFan transformed_laser_fan =
sensor::TransformLaserFan(laser_fan, transform * pose);
segments->reserve(transformed_laser_fan.returns.size());
for (const Eigen::Vector3f& hit : transformed_laser_fan.returns) {
const Eigen::Vector2f laser_origin_xy =
transformed_laser_fan.origin.head<2>();
const float laser_origin_z = transformed_laser_fan.origin.z();
const float delta_z = hit.z() - laser_origin_z;
const Eigen::Vector2f delta_xy = hit.head<2>() - laser_origin_xy;
if (laser_origin_z < -kSliceHalfHeight) {
@ -181,12 +182,12 @@ void InsertSegmentsIntoProbabilityGrid(
} // namespace
void InsertIntoProbabilityGrid(
const sensor::LaserFan3D& laser_fan_3d, const transform::Rigid3f& pose,
const sensor::LaserFan& laser_fan, const transform::Rigid3f& pose,
const float slice_z, const mapping_2d::LaserFanInserter& laser_fan_inserter,
mapping_2d::ProbabilityGrid* result) {
std::vector<LaserSegment> segments;
GenerateSegmentForSlice(
laser_fan_3d, pose,
laser_fan, pose,
transform::Rigid3f::Translation(-slice_z * Eigen::Vector3f::UnitZ()),
&segments);
InsertSegmentsIntoProbabilityGrid(segments, laser_fan_inserter.hit_table(),
@ -265,7 +266,7 @@ void Submaps::SubmapToProto(
global_submap_pose.translation().z())));
}
void Submaps::InsertLaserFan(const sensor::LaserFan3D& laser_fan) {
void Submaps::InsertLaserFan(const sensor::LaserFan& laser_fan) {
CHECK_LT(num_laser_fans_, std::numeric_limits<int>::max());
++num_laser_fans_;
for (const int index : insertion_indices()) {

4
cartographer/mapping_3d/submaps.h
View File

@ -36,7 +36,7 @@ namespace cartographer {
namespace mapping_3d {
void InsertIntoProbabilityGrid(
const sensor::LaserFan3D& laser_fan_3d, const transform::Rigid3f& pose,
const sensor::LaserFan& laser_fan, const transform::Rigid3f& pose,
const float slice_z, const mapping_2d::LaserFanInserter& laser_fan_inserter,
mapping_2d::ProbabilityGrid* result);
@ -69,7 +69,7 @@ class Submaps : public mapping::Submaps {
mapping::proto::SubmapQuery::Response* response) override;
// Inserts 'laser_fan' into the Submap collection.
void InsertLaserFan(const sensor::LaserFan3D& laser_fan);
void InsertLaserFan(const sensor::LaserFan& laser_fan);
// Returns the 'high_resolution' HybridGrid to be used for matching.
const HybridGrid& high_resolution_matching_grid() const;

10
cartographer/sensor/data.h
View File

@ -30,7 +30,7 @@ namespace sensor {
// filled in. It is only used for time ordering sensor data before passing it
// on.
struct Data {
enum class Type { kImu, kLaserFan3D, kOdometry };
enum class Type { kImu, kLaserFan, kOdometry };
struct Odometry {
transform::Rigid3d pose;
@ -46,10 +46,8 @@ struct Data {
: type(Type::kImu), frame_id(frame_id), imu(imu) {}
Data(const string& frame_id,
const ::cartographer::sensor::LaserFan3D& laser_fan_3d)
: type(Type::kLaserFan3D),
frame_id(frame_id),
laser_fan_3d(laser_fan_3d) {}
const ::cartographer::sensor::LaserFan& laser_fan)
: type(Type::kLaserFan), frame_id(frame_id), laser_fan(laser_fan) {}
Data(const string& frame_id, const Odometry& odometry)
: type(Type::kOdometry), frame_id(frame_id), odometry(odometry) {}
@ -57,7 +55,7 @@ struct Data {
Type type;
string frame_id;
Imu imu;
sensor::LaserFan3D laser_fan_3d;
sensor::LaserFan laser_fan;
Odometry odometry;
};

54
cartographer/sensor/laser.cc
View File

@ -37,13 +37,13 @@ std::vector<uint8> ReorderReflectivities(
} // namespace
LaserFan3D ToLaserFan3D(const proto::LaserScan& proto, const float min_range,
const float max_range,
const float missing_echo_ray_length) {
LaserFan ToLaserFan(const proto::LaserScan& proto, const float min_range,
const float max_range,
const float missing_echo_ray_length) {
CHECK_GE(min_range, 0.f);
CHECK_GT(proto.angle_increment(), 0.f);
CHECK_GT(proto.angle_max(), proto.angle_min());
LaserFan3D laser_fan = {Eigen::Vector3f::Zero(), {}, {}};
LaserFan laser_fan = {Eigen::Vector3f::Zero(), {}, {}};
float angle = proto.angle_min();
for (const auto& range : proto.range()) {
if (range.value_size() > 0) {
@ -64,8 +64,8 @@ LaserFan3D ToLaserFan3D(const proto::LaserScan& proto, const float min_range,
return laser_fan;
}
proto::LaserFan3D ToProto(const LaserFan3D& laser_fan) {
proto::LaserFan3D proto;
proto::LaserFan ToProto(const LaserFan& laser_fan) {
proto::LaserFan proto;
*proto.mutable_origin() = transform::ToProto(laser_fan.origin);
*proto.mutable_point_cloud() = ToProto(laser_fan.returns);
*proto.mutable_missing_echo_point_cloud() = ToProto(laser_fan.misses);
@ -74,19 +74,19 @@ proto::LaserFan3D ToProto(const LaserFan3D& laser_fan) {
return proto;
}
LaserFan3D FromProto(const proto::LaserFan3D& proto) {
auto laser_fan_3d = LaserFan3D{
LaserFan FromProto(const proto::LaserFan& proto) {
auto laser_fan = LaserFan{
transform::ToEigen(proto.origin()), ToPointCloud(proto.point_cloud()),
ToPointCloud(proto.missing_echo_point_cloud()),
};
std::copy(proto.reflectivity().begin(), proto.reflectivity().end(),
std::back_inserter(laser_fan_3d.reflectivities));
return laser_fan_3d;
std::back_inserter(laser_fan.reflectivities));
return laser_fan;
}
LaserFan3D TransformLaserFan3D(const LaserFan3D& laser_fan,
const transform::Rigid3f& transform) {
return LaserFan3D{
LaserFan TransformLaserFan(const LaserFan& laser_fan,
const transform::Rigid3f& transform) {
return LaserFan{
transform * laser_fan.origin,
TransformPointCloud(laser_fan.returns, transform),
TransformPointCloud(laser_fan.misses, transform),
@ -94,9 +94,9 @@ LaserFan3D TransformLaserFan3D(const LaserFan3D& laser_fan,
};
}
LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan,
const float max_range) {
LaserFan3D result{laser_fan.origin, {}, {}, {}};
LaserFan FilterLaserFanByMaxRange(const LaserFan& laser_fan,
const float max_range) {
LaserFan result{laser_fan.origin, {}, {}, {}};
for (const Eigen::Vector3f& return_ : laser_fan.returns) {
if ((return_ - laser_fan.origin).norm() <= max_range) {
result.returns.push_back(return_);
@ -105,28 +105,28 @@ LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan,
return result;
}
LaserFan3D CropLaserFan(const LaserFan3D& laser_fan, const float min_z,
const float max_z) {
return LaserFan3D{laser_fan.origin, Crop(laser_fan.returns, min_z, max_z),
Crop(laser_fan.misses, min_z, max_z)};
LaserFan CropLaserFan(const LaserFan& laser_fan, const float min_z,
const float max_z) {
return LaserFan{laser_fan.origin, Crop(laser_fan.returns, min_z, max_z),
Crop(laser_fan.misses, min_z, max_z)};
}
CompressedLaserFan3D Compress(const LaserFan3D& laser_fan) {
CompressedLaserFan Compress(const LaserFan& laser_fan) {
std::vector<int> new_to_old;
CompressedPointCloud compressed_returns =
CompressedPointCloud::CompressAndReturnOrder(laser_fan.returns,
&new_to_old);
return CompressedLaserFan3D{
return CompressedLaserFan{
laser_fan.origin, std::move(compressed_returns),
CompressedPointCloud(laser_fan.misses),
ReorderReflectivities(laser_fan.reflectivities, new_to_old)};
}
LaserFan3D Decompress(const CompressedLaserFan3D& compressed_laser_fan) {
return LaserFan3D{compressed_laser_fan.origin,
compressed_laser_fan.returns.Decompress(),
compressed_laser_fan.misses.Decompress(),
compressed_laser_fan.reflectivities};
LaserFan Decompress(const CompressedLaserFan& compressed_laser_fan) {
return LaserFan{compressed_laser_fan.origin,
compressed_laser_fan.returns.Decompress(),
compressed_laser_fan.misses.Decompress(),
compressed_laser_fan.reflectivities};
}
} // namespace sensor

33
cartographer/sensor/laser.h
View File

@ -29,7 +29,7 @@ namespace sensor {
// where laser returns were detected. 'misses' are points in the direction of
// rays for which no return was detected, and were inserted at a configured
// distance. It is assumed that between the 'origin' and 'misses' is free space.
struct LaserFan3D {
struct LaserFan {
Eigen::Vector3f origin;
PointCloud returns;
PointCloud misses;
@ -42,30 +42,29 @@ struct LaserFan3D {
// the range ['min_range', 'max_range']. Beams beyond 'max_range' are inserted
// into the 'misses' point cloud with length 'missing_echo_ray_length'. The
// points in both clouds are stored in scan order.
LaserFan3D ToLaserFan3D(const proto::LaserScan& proto, float min_range,
float max_range, float missing_echo_ray_length);
LaserFan ToLaserFan(const proto::LaserScan& proto, float min_range,
float max_range, float missing_echo_ray_length);
// Converts 3D 'laser_fan' to a proto::LaserFan3D.
proto::LaserFan3D ToProto(const LaserFan3D& laser_fan);
// Converts 3D 'laser_fan' to a proto::LaserFan.
proto::LaserFan ToProto(const LaserFan& laser_fan);
// Converts 'proto' to a LaserFan3D.
LaserFan3D FromProto(const proto::LaserFan3D& proto);
// Converts 'proto' to a LaserFan.
LaserFan FromProto(const proto::LaserFan& proto);
LaserFan3D TransformLaserFan3D(const LaserFan3D& laser_fan,
const transform::Rigid3f& transform);
LaserFan TransformLaserFan(const LaserFan& laser_fan,
const transform::Rigid3f& transform);
// Filter a 'laser_fan', retaining only the returns that have no more than
// 'max_range' distance from the laser origin. Removes misses and reflectivity
// information.
LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan,
float max_range);
LaserFan FilterLaserFanByMaxRange(const LaserFan& laser_fan, float max_range);
// Crops 'laser_fan' according to the region defined by 'min_z' and 'max_z'.
LaserFan3D CropLaserFan(const LaserFan3D& laser_fan, float min_z, float max_z);
LaserFan CropLaserFan(const LaserFan& laser_fan, float min_z, float max_z);
// Like LaserFan3D but with compressed point clouds. The point order changes
// when converting from LaserFan3D.
struct CompressedLaserFan3D {
// Like LaserFan but with compressed point clouds. The point order changes
// when converting from LaserFan.
struct CompressedLaserFan {
Eigen::Vector3f origin;
CompressedPointCloud returns;
CompressedPointCloud misses;
@ -74,9 +73,9 @@ struct CompressedLaserFan3D {
std::vector<uint8> reflectivities;
};
CompressedLaserFan3D Compress(const LaserFan3D& laser_fan);
CompressedLaserFan Compress(const LaserFan& laser_fan);
LaserFan3D Decompress(const CompressedLaserFan3D& compressed_laser_fan);
LaserFan Decompress(const CompressedLaserFan& compressed_laser_fan);
} // namespace sensor
} // namespace cartographer

20
cartographer/sensor/laser_test.cc
View File

@ -28,7 +28,7 @@ namespace {
using ::testing::Contains;
using ::testing::PrintToString;
TEST(ProjectorTest, ToLaserFan3D) {
TEST(ProjectorTest, ToLaserFan) {
proto::LaserScan laser_scan;
for (int i = 0; i < 8; ++i) {
laser_scan.add_range()->add_value(1.f);
@ -37,7 +37,7 @@ TEST(ProjectorTest, ToLaserFan3D) {
laser_scan.set_angle_max(8.f * static_cast<float>(M_PI_4));
laser_scan.set_angle_increment(static_cast<float>(M_PI_4));
const LaserFan3D fan = ToLaserFan3D(laser_scan, 0.f, 10.f, 1.f);
const LaserFan fan = ToLaserFan(laser_scan, 0.f, 10.f, 1.f);
EXPECT_TRUE(fan.returns[0].isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), 1e-6));
EXPECT_TRUE(fan.returns[1].isApprox(
Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6));
@ -53,7 +53,7 @@ TEST(ProjectorTest, ToLaserFan3D) {
Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), 1e-6));
}
TEST(ProjectorTest, ToLaserFan3DWithInfinityAndNaN) {
TEST(ProjectorTest, ToLaserFanWithInfinityAndNaN) {
proto::LaserScan laser_scan;
laser_scan.add_range()->add_value(1.f);
laser_scan.add_range()->add_value(std::numeric_limits<float>::infinity());
@ -64,7 +64,7 @@ TEST(ProjectorTest, ToLaserFan3DWithInfinityAndNaN) {
laser_scan.set_angle_max(3.f * static_cast<float>(M_PI_4));
laser_scan.set_angle_increment(static_cast<float>(M_PI_4));
const LaserFan3D fan = ToLaserFan3D(laser_scan, 2.f, 10.f, 1.f);
const LaserFan fan = ToLaserFan(laser_scan, 2.f, 10.f, 1.f);
ASSERT_EQ(2, fan.returns.size());
EXPECT_TRUE(fan.returns[0].isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), 1e-6));
EXPECT_TRUE(fan.returns[1].isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), 1e-6));
@ -82,12 +82,12 @@ MATCHER_P(PairApproximatelyEquals, expected,
}
TEST(LaserTest, Compression) {
LaserFan3D fan = {Eigen::Vector3f(1, 1, 1),
{Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6),
Eigen::Vector3f(0, 1, 2)},
{Eigen::Vector3f(7, 8, 9)},
{1, 2, 3}};
LaserFan3D actual = Decompress(Compress(fan));
LaserFan fan = {Eigen::Vector3f(1, 1, 1),
{Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6),
Eigen::Vector3f(0, 1, 2)},
{Eigen::Vector3f(7, 8, 9)},
{1, 2, 3}};
LaserFan actual = Decompress(Compress(fan));
EXPECT_TRUE(actual.origin.isApprox(Eigen::Vector3f(1, 1, 1), 1e-6));
EXPECT_EQ(3, actual.returns.size());
EXPECT_EQ(1, actual.misses.size());

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

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