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

View File

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

View File

@ -35,10 +35,10 @@ struct TrajectoryNode {
common::Time time; common::Time time;
// LaserFan in 'pose' frame. Only used in the 2D case. // 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. // 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. // Trajectory this node belongs to.
// TODO(jmason): The naming here is confusing because 'trajectory' doesn't // TODO(jmason): The naming here is confusing because 'trajectory' doesn't

View File

@ -41,7 +41,7 @@ kalman_filter::PoseTracker* GlobalTrajectoryBuilder::pose_tracker() const {
} }
void GlobalTrajectoryBuilder::AddHorizontalLaserFan( 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 = std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
local_trajectory_builder_.AddHorizontalLaserFan(time, laser_fan); local_trajectory_builder_.AddHorizontalLaserFan(time, laser_fan);
if (insertion_result != nullptr) { if (insertion_result != nullptr) {

View File

@ -41,13 +41,13 @@ class GlobalTrajectoryBuilder
const override; const override;
void AddHorizontalLaserFan(common::Time time, 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, void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override; const Eigen::Vector3d& angular_velocity) override;
void AddOdometerPose( void AddOdometerPose(
common::Time time, const transform::Rigid3d& pose, common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) override; 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."; LOG(FATAL) << "Not implemented.";
}; };

View File

@ -51,7 +51,7 @@ LaserFanInserter::LaserFanInserter(
miss_table_(mapping::ComputeLookupTableToApplyOdds( miss_table_(mapping::ComputeLookupTableToApplyOdds(
mapping::Odds(options.miss_probability()))) {} 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 { ProbabilityGrid* const probability_grid) const {
CHECK_NOTNULL(probability_grid)->StartUpdate(); CHECK_NOTNULL(probability_grid)->StartUpdate();

View File

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

View File

@ -44,7 +44,7 @@ class LaserFanInserterTest : public ::testing::Test {
} }
void InsertPointCloud() { 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(-3.5, 0.5, 0.f);
laser_fan.returns.emplace_back(-2.5, 1.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(-1.5, 2.5, 0.f);

View File

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

View File

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

View File

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

View File

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

View File

@ -147,7 +147,7 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
} // namespace } // 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&)>& hit_visitor,
const std::function<void(const Eigen::Array2i&)>& miss_visitor) { const std::function<void(const Eigen::Array2i&)>& miss_visitor) {
const double superscaled_resolution = limits.resolution() / kSubpixelScale; const double superscaled_resolution = limits.resolution() / kSubpixelScale;

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -114,7 +114,7 @@ Submaps::Submaps(const proto::SubmapsOptions& options)
AddSubmap(Eigen::Vector2f::Zero()); 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()); CHECK_LT(num_laser_fans_, std::numeric_limits<int>::max());
++num_laser_fans_; ++num_laser_fans_;
for (const int index : insertion_indices()) { for (const int index : insertion_indices()) {

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -186,7 +186,7 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
return first * (to - from) + from; 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. // 360 degree rays at 16 angles.
sensor::PointCloud directions_in_laser_frame; sensor::PointCloud directions_in_laser_frame;
for (int r = -8; r != 8; ++r) { for (int r = -8; r != 8; ++r) {

View File

@ -76,7 +76,7 @@ LaserFanInserter::LaserFanInserter(
miss_table_(mapping::ComputeLookupTableToApplyOdds( miss_table_(mapping::ComputeLookupTableToApplyOdds(
mapping::Odds(options_.miss_probability()))) {} 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 { HybridGrid* hybrid_grid) const {
CHECK_NOTNULL(hybrid_grid)->StartUpdate(); CHECK_NOTNULL(hybrid_grid)->StartUpdate();

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -36,7 +36,7 @@ namespace cartographer {
namespace mapping_3d { namespace mapping_3d {
void InsertIntoProbabilityGrid( 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, const float slice_z, const mapping_2d::LaserFanInserter& laser_fan_inserter,
mapping_2d::ProbabilityGrid* result); mapping_2d::ProbabilityGrid* result);
@ -69,7 +69,7 @@ class Submaps : public mapping::Submaps {
mapping::proto::SubmapQuery::Response* response) override; mapping::proto::SubmapQuery::Response* response) override;
// Inserts 'laser_fan' into the Submap collection. // 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. // Returns the 'high_resolution' HybridGrid to be used for matching.
const HybridGrid& high_resolution_matching_grid() const; const HybridGrid& high_resolution_matching_grid() const;

View File

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

View File

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

View File

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

View File

@ -28,7 +28,7 @@ namespace {
using ::testing::Contains; using ::testing::Contains;
using ::testing::PrintToString; using ::testing::PrintToString;
TEST(ProjectorTest, ToLaserFan3D) { TEST(ProjectorTest, ToLaserFan) {
proto::LaserScan laser_scan; proto::LaserScan laser_scan;
for (int i = 0; i < 8; ++i) { for (int i = 0; i < 8; ++i) {
laser_scan.add_range()->add_value(1.f); 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_max(8.f * static_cast<float>(M_PI_4));
laser_scan.set_angle_increment(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[0].isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), 1e-6));
EXPECT_TRUE(fan.returns[1].isApprox( EXPECT_TRUE(fan.returns[1].isApprox(
Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6)); 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)); 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; proto::LaserScan laser_scan;
laser_scan.add_range()->add_value(1.f); laser_scan.add_range()->add_value(1.f);
laser_scan.add_range()->add_value(std::numeric_limits<float>::infinity()); 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_max(3.f * static_cast<float>(M_PI_4));
laser_scan.set_angle_increment(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()); 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[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)); 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) { TEST(LaserTest, Compression) {
LaserFan3D fan = {Eigen::Vector3f(1, 1, 1), LaserFan fan = {Eigen::Vector3f(1, 1, 1),
{Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6), {Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6),
Eigen::Vector3f(0, 1, 2)}, Eigen::Vector3f(0, 1, 2)},
{Eigen::Vector3f(7, 8, 9)}, {Eigen::Vector3f(7, 8, 9)},
{1, 2, 3}}; {1, 2, 3}};
LaserFan3D actual = Decompress(Compress(fan)); LaserFan actual = Decompress(Compress(fan));
EXPECT_TRUE(actual.origin.isApprox(Eigen::Vector3f(1, 1, 1), 1e-6)); EXPECT_TRUE(actual.origin.isApprox(Eigen::Vector3f(1, 1, 1), 1e-6));
EXPECT_EQ(3, actual.returns.size()); EXPECT_EQ(3, actual.returns.size());
EXPECT_EQ(1, actual.misses.size()); EXPECT_EQ(1, actual.misses.size());

View File

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