Rename LaserFan3D to LaserFan. (#64)
parent
9020f71605
commit
098349f67e
|
@ -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;
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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.";
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -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();
|
||||||
|
|
||||||
|
|
|
@ -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_; }
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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);
|
||||||
|
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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);
|
||||||
|
|
||||||
|
|
|
@ -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)),
|
||||||
|
|
|
@ -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(
|
||||||
|
|
|
@ -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,
|
||||||
});
|
});
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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()) {
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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);
|
||||||
|
|
||||||
|
|
|
@ -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.";
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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();
|
||||||
|
|
||||||
|
|
|
@ -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_;
|
||||||
|
|
|
@ -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_);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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);
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
|
||||||
|
|
|
@ -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(
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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()) {
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -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};
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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());
|
||||||
|
|
|
@ -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;
|
||||||
|
|
Loading…
Reference in New Issue