diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index 599fb18..4a6b9e8 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -38,8 +38,8 @@ struct TrajectoryNode { // LaserFan in 'pose' frame. Only used in the 2D case. sensor::LaserFan laser_fan_2d; - // LaserFan in 'pose' frame. Only used in the 3D case. - sensor::CompressedLaserFan laser_fan_3d; + // Range data in 'pose' frame. Only used in the 3D case. + sensor::CompressedRangeData range_data_3d; // Trajectory this node belongs to. // TODO(macmason): The naming here is confusing because 'trajectory' diff --git a/cartographer/mapping_2d/map_limits.h b/cartographer/mapping_2d/map_limits.h index c263c8e..b46db3a 100644 --- a/cartographer/mapping_2d/map_limits.h +++ b/cartographer/mapping_2d/map_limits.h @@ -23,9 +23,9 @@ #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/math.h" +#include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping_2d/proto/map_limits.pb.h" #include "cartographer/mapping_2d/xy_index.h" -#include "cartographer/mapping/trajectory_node.h" #include "cartographer/sensor/laser.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" @@ -79,8 +79,9 @@ class MapLimits { // Returns true of the ProbabilityGrid contains 'xy_index'. bool Contains(const Eigen::Array2i& xy_index) const { return (Eigen::Array2i(0, 0) <= xy_index).all() && - (xy_index < Eigen::Array2i(cell_limits_.num_x_cells, - cell_limits_.num_y_cells)).all(); + (xy_index < + Eigen::Array2i(cell_limits_.num_x_cells, cell_limits_.num_y_cells)) + .all(); } // Computes MapLimits that contain the origin, and all laser rays (both @@ -106,9 +107,9 @@ class MapLimits { Eigen::AlignedBox2f bounding_box(Eigen::Vector2f::Zero()); for (const auto& node : trajectory_nodes) { const auto& data = *node.constant_data; - if (!data.laser_fan_3d.returns.empty()) { + if (!data.range_data_3d.returns.empty()) { const sensor::LaserFan laser_fan = sensor::TransformLaserFan( - Decompress(data.laser_fan_3d), node.pose.cast()); + Decompress(data.range_data_3d), node.pose.cast()); bounding_box.extend(laser_fan.origin.head<2>()); for (const Eigen::Vector3f& hit : laser_fan.returns) { bounding_box.extend(hit.head<2>()); diff --git a/cartographer/mapping_2d/probability_grid.h b/cartographer/mapping_2d/probability_grid.h index ffb73e8..2d7e4cc 100644 --- a/cartographer/mapping_2d/probability_grid.h +++ b/cartographer/mapping_2d/probability_grid.h @@ -27,10 +27,10 @@ #include "cartographer/common/math.h" #include "cartographer/common/port.h" +#include "cartographer/mapping/probability_values.h" #include "cartographer/mapping_2d/map_limits.h" #include "cartographer/mapping_2d/proto/probability_grid.pb.h" #include "cartographer/mapping_2d/xy_index.h" -#include "cartographer/mapping/probability_values.h" #include "glog/logging.h" namespace cartographer { @@ -123,9 +123,8 @@ class ProbabilityGrid { // Returns true if the probability at the specified index is known. bool IsKnown(const Eigen::Array2i& xy_index) const { - return limits_.Contains(xy_index) && - cells_[GetIndexOfCell(xy_index)] != - mapping::kUnknownProbabilityValue; + return limits_.Contains(xy_index) && cells_[GetIndexOfCell(xy_index)] != + mapping::kUnknownProbabilityValue; } // Fills in 'offset' and 'limits' to define a subregion of that contains all diff --git a/cartographer/mapping_2d/probability_grid_test.cc b/cartographer/mapping_2d/probability_grid_test.cc index cc1591c..7c674fd 100644 --- a/cartographer/mapping_2d/probability_grid_test.cc +++ b/cartographer/mapping_2d/probability_grid_test.cc @@ -146,26 +146,35 @@ TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) { const CellLimits& cell_limits = limits.cell_limits(); ASSERT_EQ(14, cell_limits.num_x_cells); ASSERT_EQ(8, cell_limits.num_y_cells); - EXPECT_TRUE((Eigen::Array2i(0, 0) == - limits.GetXYIndexOfCellContainingPoint(7, 13)).all()); - EXPECT_TRUE((Eigen::Array2i(13, 0) == - limits.GetXYIndexOfCellContainingPoint(7, -13)).all()); - EXPECT_TRUE((Eigen::Array2i(0, 7) == - limits.GetXYIndexOfCellContainingPoint(-7, 13)).all()); - EXPECT_TRUE((Eigen::Array2i(13, 7) == - limits.GetXYIndexOfCellContainingPoint(-7, -13)).all()); + EXPECT_TRUE( + (Eigen::Array2i(0, 0) == limits.GetXYIndexOfCellContainingPoint(7, 13)) + .all()); + EXPECT_TRUE( + (Eigen::Array2i(13, 0) == limits.GetXYIndexOfCellContainingPoint(7, -13)) + .all()); + EXPECT_TRUE( + (Eigen::Array2i(0, 7) == limits.GetXYIndexOfCellContainingPoint(-7, 13)) + .all()); + EXPECT_TRUE( + (Eigen::Array2i(13, 7) == limits.GetXYIndexOfCellContainingPoint(-7, -13)) + .all()); // Check around the origin. - EXPECT_TRUE((Eigen::Array2i(6, 3) == - limits.GetXYIndexOfCellContainingPoint(0.5, 0.5)).all()); - EXPECT_TRUE((Eigen::Array2i(6, 3) == - limits.GetXYIndexOfCellContainingPoint(1.5, 1.5)).all()); + EXPECT_TRUE( + (Eigen::Array2i(6, 3) == limits.GetXYIndexOfCellContainingPoint(0.5, 0.5)) + .all()); + EXPECT_TRUE( + (Eigen::Array2i(6, 3) == limits.GetXYIndexOfCellContainingPoint(1.5, 1.5)) + .all()); EXPECT_TRUE((Eigen::Array2i(7, 3) == - limits.GetXYIndexOfCellContainingPoint(0.5, -0.5)).all()); + limits.GetXYIndexOfCellContainingPoint(0.5, -0.5)) + .all()); EXPECT_TRUE((Eigen::Array2i(6, 4) == - limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5)).all()); + limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5)) + .all()); EXPECT_TRUE((Eigen::Array2i(7, 4) == - limits.GetXYIndexOfCellContainingPoint(-0.5, -0.5)).all()); + limits.GetXYIndexOfCellContainingPoint(-0.5, -0.5)) + .all()); } TEST(ProbabilityGridTest, CorrectCropping) { diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 56473b0..c9a9750 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -311,12 +311,13 @@ void SparsePoseGraph::WaitForAllComputations() { std::cout << "\r\x1b[K" << progress_info.str() << std::flush; } std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; - constraint_builder_.WhenDone([this, ¬ification]( - const sparse_pose_graph::ConstraintBuilder::Result& result) { - constraints_.insert(constraints_.end(), result.begin(), result.end()); - common::MutexLocker locker(&mutex_); - notification = true; - }); + constraint_builder_.WhenDone( + [this, ¬ification]( + const sparse_pose_graph::ConstraintBuilder::Result& result) { + constraints_.insert(constraints_.end(), result.begin(), result.end()); + common::MutexLocker locker(&mutex_); + notification = true; + }); locker.Await([¬ification]() { return notification; }); } diff --git a/cartographer/mapping_3d/hybrid_grid.h b/cartographer/mapping_3d/hybrid_grid.h index e2aef39..ca1e186 100644 --- a/cartographer/mapping_3d/hybrid_grid.h +++ b/cartographer/mapping_3d/hybrid_grid.h @@ -27,8 +27,8 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/common/port.h" -#include "cartographer/mapping_3d/proto/hybrid_grid.pb.h" #include "cartographer/mapping/probability_values.h" +#include "cartographer/mapping_3d/proto/hybrid_grid.pb.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" diff --git a/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc index 5bbfd36..3149f3c 100644 --- a/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc @@ -151,7 +151,7 @@ RotationalScanMatcher::RotationalScanMatcher( for (const mapping::TrajectoryNode& node : nodes) { AddValuesToHistogram( GetValuesForHistogram(sensor::TransformPointCloud( - node.constant_data->laser_fan_3d.returns.Decompress(), + node.constant_data->range_data_3d.returns.Decompress(), node.pose.cast())), 0.f, &histogram_); } diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 9270008..9cb35a2 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -302,12 +302,13 @@ void SparsePoseGraph::WaitForAllComputations() { std::cout << "\r\x1b[K" << progress_info.str() << std::flush; } std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; - constraint_builder_.WhenDone([this, ¬ification]( - const sparse_pose_graph::ConstraintBuilder::Result& result) { - constraints_.insert(constraints_.end(), result.begin(), result.end()); - common::MutexLocker locker(&mutex_); - notification = true; - }); + constraint_builder_.WhenDone( + [this, ¬ification]( + const sparse_pose_graph::ConstraintBuilder::Result& result) { + constraints_.insert(constraints_.end(), result.begin(), result.end()); + common::MutexLocker locker(&mutex_); + notification = true; + }); locker.Await([¬ification]() { return notification; }); } diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 7b3d0c0..1637f80 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -94,7 +94,7 @@ void ConstraintBuilder::MaybeAddConstraint( ++pending_computations_[current_computation_]; const int current_computation = current_computation_; const auto* const point_cloud = - &trajectory_nodes[scan_index].constant_data->laser_fan_3d.returns; + &trajectory_nodes[scan_index].constant_data->range_data_3d.returns; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_index, submap_nodes, &submap->high_resolution_hybrid_grid, [=]() EXCLUDES(mutex_) { @@ -124,7 +124,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( ++pending_computations_[current_computation_]; const int current_computation = current_computation_; const auto* const point_cloud = - &trajectory_nodes[scan_index].constant_data->laser_fan_3d.returns; + &trajectory_nodes[scan_index].constant_data->range_data_3d.returns; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_index, submap_nodes, &submap->high_resolution_hybrid_grid, [=]() EXCLUDES(mutex_) { diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index 0f513a7..80d046c 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h @@ -68,10 +68,10 @@ class ConstraintBuilder { ConstraintBuilder& operator=(const ConstraintBuilder&) = delete; // Schedules exploring a new constraint between 'submap' identified by - // 'submap_index', and the 'laser_fan_3d.returns' in 'trajectory_nodes' for + // 'submap_index', and the 'range_data_3d.returns' in 'trajectory_nodes' for // 'scan_index'. The 'initial_relative_pose' is relative to the 'submap'. // - // The pointees of 'submap' and 'laser_fan_3d.returns' must stay valid until + // The pointees of 'submap' and 'range_data_3d.returns' must stay valid until // all computations are finished. void MaybeAddConstraint( int submap_index, const Submap* submap, int scan_index, @@ -79,15 +79,15 @@ class ConstraintBuilder { const transform::Rigid3d& initial_relative_pose); // Schedules exploring a new constraint between 'submap' identified by - // 'submap_index' and the 'laser_fan_3d.returns' in 'trajectory_nodes' for + // 'submap_index' and the 'range_data_3d.returns' in 'trajectory_nodes' for // 'scan_index'. This performs full-submap matching. // // The scan at 'scan_index' should be from trajectory 'scan_trajectory', and // the 'submap' should be from 'submap_trajectory'. The // 'trajectory_connectivity' is updated if the full-submap match succeeds. // - // The pointees of 'submap' and 'point_cloud' must stay valid until all - // computations are finished. + // The pointees of 'submap' and 'range_data_3d.returns' must stay valid until + // all computations are finished. void MaybeAddGlobalConstraint( int submap_index, const Submap* submap, int scan_index, const mapping::Submaps* scan_trajectory, diff --git a/cartographer/sensor/laser.cc b/cartographer/sensor/laser.cc index e453ef6..94b6e23 100644 --- a/cartographer/sensor/laser.cc +++ b/cartographer/sensor/laser.cc @@ -108,22 +108,22 @@ LaserFan CropLaserFan(const LaserFan& laser_fan, const float min_z, Crop(laser_fan.misses, min_z, max_z)}; } -CompressedLaserFan Compress(const LaserFan& laser_fan) { +CompressedRangeData Compress(const LaserFan& laser_fan) { std::vector new_to_old; CompressedPointCloud compressed_returns = CompressedPointCloud::CompressAndReturnOrder(laser_fan.returns, &new_to_old); - return CompressedLaserFan{ + return CompressedRangeData{ laser_fan.origin, std::move(compressed_returns), CompressedPointCloud(laser_fan.misses), ReorderReflectivities(laser_fan.reflectivities, new_to_old)}; } -LaserFan Decompress(const CompressedLaserFan& compressed_laser_fan) { - return LaserFan{compressed_laser_fan.origin, - compressed_laser_fan.returns.Decompress(), - compressed_laser_fan.misses.Decompress(), - compressed_laser_fan.reflectivities}; +LaserFan Decompress(const CompressedRangeData& compressed_range_data) { + return LaserFan{compressed_range_data.origin, + compressed_range_data.returns.Decompress(), + compressed_range_data.misses.Decompress(), + compressed_range_data.reflectivities}; } } // namespace sensor diff --git a/cartographer/sensor/laser.h b/cartographer/sensor/laser.h index 882ffe9..b0fcfb7 100644 --- a/cartographer/sensor/laser.h +++ b/cartographer/sensor/laser.h @@ -25,10 +25,10 @@ namespace cartographer { namespace sensor { -// A 3D variant of LaserFan. Rays begin at 'origin'. 'returns' are the points -// where laser returns were detected. 'misses' are points in the direction of -// rays for which no return was detected, and were inserted at a configured -// distance. It is assumed that between the 'origin' and 'misses' is free space. +// Rays begin at 'origin'. 'returns' are the points where laser returns were +// detected. 'misses' are points in the direction of rays for which no return +// was detected, and were inserted at a configured distance. It is assumed that +// between the 'origin' and 'misses' is free space. struct LaserFan { Eigen::Vector3f origin; PointCloud returns; @@ -63,7 +63,7 @@ LaserFan CropLaserFan(const LaserFan& laser_fan, float min_z, float max_z); // Like LaserFan but with compressed point clouds. The point order changes // when converting from LaserFan. -struct CompressedLaserFan { +struct CompressedRangeData { Eigen::Vector3f origin; CompressedPointCloud returns; CompressedPointCloud misses; @@ -72,9 +72,9 @@ struct CompressedLaserFan { std::vector reflectivities; }; -CompressedLaserFan Compress(const LaserFan& laser_fan); +CompressedRangeData Compress(const LaserFan& laser_fan); -LaserFan Decompress(const CompressedLaserFan& compressed_laser_fan); +LaserFan Decompress(const CompressedRangeData& compressed_range_Data); } // namespace sensor } // namespace cartographer