PointsProcessor can now ask for another pass over the data. (#69)

* PointsProcessor can now ask for another pass.
* Added MinMaxFilteringPointsProcessor to have a user for RemovePoints.
master
Holger Rapp 2016-10-19 13:11:13 +02:00 committed by GitHub
parent a7663f6c15
commit 4a9116a58e
10 changed files with 182 additions and 15 deletions

View File

@ -4,6 +4,16 @@ google_library(io_cairo_types
cairo_types.h cairo_types.h
) )
google_library(io_min_max_range_filtering_points_processor
SRCS
min_max_range_filtering_points_processor.cc
HDRS
min_max_range_filtering_points_processor.h
DEPENDS
io_points_batch
io_points_processor
)
google_library(io_null_points_processor google_library(io_null_points_processor
HDRS HDRS
null_points_processor.h null_points_processor.h
@ -13,6 +23,8 @@ google_library(io_null_points_processor
google_library(io_points_batch google_library(io_points_batch
USES_EIGEN USES_EIGEN
SRCS
points_batch.cc
HDRS HDRS
points_batch.h points_batch.h
DEPENDS DEPENDS

View File

@ -0,0 +1,46 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/io/min_max_range_filtering_points_processor.h"
#include "cartographer/io/points_batch.h"
namespace cartographer {
namespace io {
MinMaxRangeFiteringPointsProcessor::MinMaxRangeFiteringPointsProcessor(
const double min_range, const double max_range, PointsProcessor* next)
: min_range_(min_range), max_range_(max_range), next_(next) {}
void MinMaxRangeFiteringPointsProcessor::Process(
std::unique_ptr<PointsBatch> batch) {
std::vector<int> to_remove;
for (size_t i = 0; i < batch->points.size(); ++i) {
const float range = (batch->points[i] - batch->origin).norm();
if (!(min_range_ <= range && range <= max_range_)) {
to_remove.push_back(i);
}
}
RemovePoints(to_remove, batch.get());
next_->Process(std::move(batch));
}
PointsProcessor::FlushResult MinMaxRangeFiteringPointsProcessor::Flush() {
return next_->Flush();
}
} // namespace io
} // namespace cartographer

View File

@ -0,0 +1,52 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_
#define CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_
#include <memory>
#include "cartographer/io/points_processor.h"
namespace cartographer {
namespace io {
// Filters all points that are farther away from their 'origin' as 'max_range'
// or closer than 'min_range'.
class MinMaxRangeFiteringPointsProcessor : public PointsProcessor {
public:
MinMaxRangeFiteringPointsProcessor(double min_range, double max_range,
PointsProcessor* next);
~MinMaxRangeFiteringPointsProcessor() override {}
MinMaxRangeFiteringPointsProcessor(
const MinMaxRangeFiteringPointsProcessor&) = delete;
MinMaxRangeFiteringPointsProcessor& operator=(
const MinMaxRangeFiteringPointsProcessor&) = delete;
void Process(std::unique_ptr<PointsBatch> batch) override;
FlushResult Flush() override;
private:
const double min_range_;
const double max_range_;
PointsProcessor* const next_;
};
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_

View File

@ -28,8 +28,8 @@ class NullPointsProcessor : public PointsProcessor {
NullPointsProcessor() {} NullPointsProcessor() {}
~NullPointsProcessor() override {} ~NullPointsProcessor() override {}
void Process(const PointsBatch& points_batch) override {} void Process(std::unique_ptr<PointsBatch> points_batch) override {}
void Flush() override {} FlushResult Flush() override { return FlushResult::kFinished; }
}; };
} // namespace io } // namespace io

View File

@ -0,0 +1,33 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/io/points_batch.h"
namespace cartographer {
namespace io {
void RemovePoints(std::vector<int> to_remove, PointsBatch* batch) {
std::sort(to_remove.begin(), to_remove.end(), std::greater<int>());
for (const int index : to_remove) {
batch->points.erase(batch->points.begin() + index);
if (!batch->colors.empty()) {
batch->colors.erase(batch->colors.begin() + index);
}
}
}
} // namespace io
} // namespace cartographer

View File

@ -26,6 +26,9 @@
namespace cartographer { namespace cartographer {
namespace io { namespace io {
// A point's color.
using Color = std::array<uint8_t, 3>;
// A number of points, captured around the same 'time' and by a // A number of points, captured around the same 'time' and by a
// sensor at the same 'origin'. // sensor at the same 'origin'.
struct PointsBatch { struct PointsBatch {
@ -49,10 +52,12 @@ struct PointsBatch {
int trajectory_index; int trajectory_index;
std::vector<Eigen::Vector3f> points; std::vector<Eigen::Vector3f> points;
std::vector<Eigen::Vector3f> normals; std::vector<Color> colors;
std::vector<Eigen::Matrix<uint8_t, 3, 1>> colors;
}; };
// Removes the indices in 'to_remove' from 'batch'.
void RemovePoints(std::vector<int> to_remove, PointsBatch* batch);
} // namespace io } // namespace io
} // namespace cartographer } // namespace cartographer

View File

@ -17,24 +17,34 @@
#ifndef CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ #ifndef CARTOGRAPHER_IO_POINTS_PROCESSOR_H_
#define CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ #define CARTOGRAPHER_IO_POINTS_PROCESSOR_H_
#include <memory>
#include "cartographer/io/points_batch.h" #include "cartographer/io/points_batch.h"
namespace cartographer { namespace cartographer {
namespace io { namespace io {
// A processor in a pipeline. It processes a 'points_batch' and hands it to the // A processor in a pipeline. It processes a 'points_batch' and hands it to the
// next processor in the pipeline. Once 'flush' is called no more data will be // next processor in the pipeline.
// send through the pipeline.
class PointsProcessor { class PointsProcessor {
public: public:
enum class FlushResult {
kRestartStream,
kFinished,
};
PointsProcessor() {} PointsProcessor() {}
virtual ~PointsProcessor() {} virtual ~PointsProcessor() {}
PointsProcessor(const PointsProcessor&) = delete; PointsProcessor(const PointsProcessor&) = delete;
PointsProcessor& operator=(const PointsProcessor&) = delete; PointsProcessor& operator=(const PointsProcessor&) = delete;
virtual void Process(const PointsBatch& points_batch) = 0; // Receive a 'points_batch', process it and pass it on.
virtual void Flush() = 0; virtual void Process(std::unique_ptr<PointsBatch> points_batch) = 0;
// Some implementations will perform expensive computations and others that do
// multiple passes over the data might ask for restarting the stream.
virtual FlushResult Flush() = 0;
}; };
} // namespace io } // namespace io

View File

@ -83,17 +83,25 @@ XRayPointsProcessor::XRayPointsProcessor(const double voxel_size,
transform_(transform), transform_(transform),
voxels_(voxel_size, Eigen::Vector3f::Zero()) {} voxels_(voxel_size, Eigen::Vector3f::Zero()) {}
void XRayPointsProcessor::Process(const PointsBatch& batch) { void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
for (const auto& point : batch.points) { for (const auto& point : batch->points) {
const Eigen::Vector3f camera_point = transform_ * point; const Eigen::Vector3f camera_point = transform_ * point;
*voxels_.mutable_value(voxels_.GetCellIndex(camera_point)) = true; *voxels_.mutable_value(voxels_.GetCellIndex(camera_point)) = true;
} }
next_->Process(batch); next_->Process(std::move(batch));
} }
void XRayPointsProcessor::Flush() { PointsProcessor::FlushResult XRayPointsProcessor::Flush() {
WriteImage(); WriteImage();
return next_->Flush(); switch (next_->Flush()) {
case FlushResult::kRestartStream:
LOG(FATAL) << "X-Ray generation must be configured to occur after any "
"stages that require multiple passes";
case FlushResult::kFinished:
return FlushResult::kFinished;
}
LOG(FATAL);
} }
void XRayPointsProcessor::WriteImage() { void XRayPointsProcessor::WriteImage() {

View File

@ -35,8 +35,8 @@ class XRayPointsProcessor : public PointsProcessor {
~XRayPointsProcessor() override {} ~XRayPointsProcessor() override {}
void Process(const PointsBatch& batch) override; void Process(std::unique_ptr<PointsBatch> batch) override;
void Flush() override; FlushResult Flush() override;
private: private:
using Voxels = mapping_3d::HybridGridBase<bool>; using Voxels = mapping_3d::HybridGridBase<bool>;

View File

@ -60,6 +60,7 @@ google_library(sensor_data
HDRS HDRS
data.h data.h
DEPENDS DEPENDS
common_time
kalman_filter_pose_tracker kalman_filter_pose_tracker
sensor_laser sensor_laser
transform_rigid_transform transform_rigid_transform