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

Rename CompressedLaserFan to CompressedRangeData. (#223) 

CompressedLaserFan is misleading since data can come from a
different type of sensor, e.g. a depth camera.
master
Wolfgang Hess 2017-03-22 13:06:41 +01:00 committed by GitHub
parent c78f29fdac
commit a2abe45542
12 changed files with 72 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/mapping/trajectory_node.h
View File

@ -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'

11
cartographer/mapping_2d/map_limits.h
View File

@ -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<float>());
Decompress(data.range_data_3d), node.pose.cast<float>());
bounding_box.extend(laser_fan.origin.head<2>());
for (const Eigen::Vector3f& hit : laser_fan.returns) {
bounding_box.extend(hit.head<2>());

7
cartographer/mapping_2d/probability_grid.h
View File

@ -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

39
cartographer/mapping_2d/probability_grid_test.cc
View File

@ -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) {

13
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -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, &notification](
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, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
constraints_.insert(constraints_.end(), result.begin(), result.end());
common::MutexLocker locker(&mutex_);
notification = true;
});
locker.Await([&notification]() { return notification; });
}

2
cartographer/mapping_3d/hybrid_grid.h
View File

@ -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"

2
cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc
View File

@ -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<float>())),
0.f, &histogram_);
}

13
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -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, &notification](
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, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
constraints_.insert(constraints_.end(), result.begin(), result.end());
common::MutexLocker locker(&mutex_);
notification = true;
});
locker.Await([&notification]() { return notification; });
}

4
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -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_) {

10
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
View File

@ -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,

14
cartographer/sensor/laser.cc
View File

@ -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<int> 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

14
cartographer/sensor/laser.h
View File

@ -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<uint8> 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