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

Add XRayPointsProcessor::bounding_box(). (#306) 

Changes XRayPointsProcessor to use one bounding box for all floors.
Makes the bounding box accessible via a public member function.
master
Wolfgang Hess 2017-05-24 14:46:17 +02:00 committed by GitHub
parent 3d0d1e46b5
commit 4bb02c7240
2 changed files with 14 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
cartographer/io/xray_points_processor.cc
View File

@ -167,31 +167,22 @@ std::unique_ptr<XRayPointsProcessor> XRayPointsProcessor::FromDictionary(
void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation, void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation,
FileWriter* const file_writer) { FileWriter* const file_writer) {
Eigen::Array3i min(std::numeric_limits<int>::max(), if (bounding_box_.isEmpty()) {
std::numeric_limits<int>::max(), LOG(WARNING) << "Not writing output: bounding box is empty.";
std::numeric_limits<int>::max()); return;
Eigen::Array3i max(std::numeric_limits<int>::min(),
std::numeric_limits<int>::min(),
std::numeric_limits<int>::min());
// Find the maximum and minimum cells.
for (mapping_3d::HybridGridBase<bool>::Iterator it(aggregation.voxels);
!it.Done(); it.Next()) {
const Eigen::Array3i idx = it.GetCellIndex();
min = min.min(idx);
max = max.max(idx);
} }
// Returns the (x, y) pixel of the given 'index'. // Returns the (x, y) pixel of the given 'index'.
const auto voxel_index_to_pixel = [&max, &min](const Eigen::Array3i& index) { const auto voxel_index_to_pixel = [this](const Eigen::Array3i& index) {
// We flip the y axis, since matrices rows are counted from the top. // We flip the y axis, since matrices rows are counted from the top.
return Eigen::Array2i(max[1] - index[1], max[2] - index[2]); return Eigen::Array2i(bounding_box_.max()[1] - index[1],
bounding_box_.max()[2] - index[2]);
}; };
// Hybrid grid uses X: forward, Y: left, Z: up. // Hybrid grid uses X: forward, Y: left, Z: up.
// For the screen we are using. X: right, Y: up // For the screen we are using. X: right, Y: up
const int xsize = max[1] - min[1] + 1; const int xsize = bounding_box_.sizes()[1] + 1;
const int ysize = max[2] - min[2] + 1; const int ysize = bounding_box_.sizes()[2] + 1;
PixelDataMatrix image = PixelDataMatrix(ysize, xsize); PixelDataMatrix image = PixelDataMatrix(ysize, xsize);
for (mapping_3d::HybridGridBase<bool>::Iterator it(aggregation.voxels); for (mapping_3d::HybridGridBase<bool>::Iterator it(aggregation.voxels);
!it.Done(); it.Next()) { !it.Done(); it.Next()) {
@ -217,6 +208,7 @@ void XRayPointsProcessor::Insert(const PointsBatch& batch,
const Eigen::Array3i cell_index = const Eigen::Array3i cell_index =
aggregation->voxels.GetCellIndex(camera_point); aggregation->voxels.GetCellIndex(camera_point);
*aggregation->voxels.mutable_value(cell_index) = true; *aggregation->voxels.mutable_value(cell_index) = true;
bounding_box_.extend(cell_index.matrix());
ColumnData& column_data = ColumnData& column_data =
aggregation->column_data[std::make_pair(cell_index[1], cell_index[2])]; aggregation->column_data[std::make_pair(cell_index[1], cell_index[2])];
const auto& color = const auto& color =

5
cartographer/io/xray_points_processor.h
View File

@ -52,6 +52,8 @@ class XRayPointsProcessor : public PointsProcessor {
void Process(std::unique_ptr<PointsBatch> batch) override; void Process(std::unique_ptr<PointsBatch> batch) override;
FlushResult Flush() override; FlushResult Flush() override;
Eigen::AlignedBox3i bounding_box() const { return bounding_box_; }
private: private:
struct ColumnData { struct ColumnData {
double sum_r = 0.; double sum_r = 0.;
@ -81,6 +83,9 @@ class XRayPointsProcessor : public PointsProcessor {
// Only has one entry if we do not separate into floors. // Only has one entry if we do not separate into floors.
std::vector<Aggregation> aggregations_; std::vector<Aggregation> aggregations_;
// Bounding box containing all cells with data in all 'aggregations_'.
Eigen::AlignedBox3i bounding_box_;
}; };
} // namespace io } // namespace io