diff --git a/cartographer/io/CMakeLists.txt b/cartographer/io/CMakeLists.txt index 73e9e4c..89839b3 100644 --- a/cartographer/io/CMakeLists.txt +++ b/cartographer/io/CMakeLists.txt @@ -4,6 +4,16 @@ google_library(io_cairo_types 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 HDRS null_points_processor.h @@ -13,6 +23,8 @@ google_library(io_null_points_processor google_library(io_points_batch USES_EIGEN + SRCS + points_batch.cc HDRS points_batch.h DEPENDS diff --git a/cartographer/io/min_max_range_filtering_points_processor.cc b/cartographer/io/min_max_range_filtering_points_processor.cc new file mode 100644 index 0000000..d535c4f --- /dev/null +++ b/cartographer/io/min_max_range_filtering_points_processor.cc @@ -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 batch) { + std::vector 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 diff --git a/cartographer/io/min_max_range_filtering_points_processor.h b/cartographer/io/min_max_range_filtering_points_processor.h new file mode 100644 index 0000000..07d5b5b --- /dev/null +++ b/cartographer/io/min_max_range_filtering_points_processor.h @@ -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 + +#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 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_ diff --git a/cartographer/io/null_points_processor.h b/cartographer/io/null_points_processor.h index 03cc3d9..30a0090 100644 --- a/cartographer/io/null_points_processor.h +++ b/cartographer/io/null_points_processor.h @@ -28,8 +28,8 @@ class NullPointsProcessor : public PointsProcessor { NullPointsProcessor() {} ~NullPointsProcessor() override {} - void Process(const PointsBatch& points_batch) override {} - void Flush() override {} + void Process(std::unique_ptr points_batch) override {} + FlushResult Flush() override { return FlushResult::kFinished; } }; } // namespace io diff --git a/cartographer/io/points_batch.cc b/cartographer/io/points_batch.cc new file mode 100644 index 0000000..4776958 --- /dev/null +++ b/cartographer/io/points_batch.cc @@ -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 to_remove, PointsBatch* batch) { + std::sort(to_remove.begin(), to_remove.end(), std::greater()); + 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 diff --git a/cartographer/io/points_batch.h b/cartographer/io/points_batch.h index a1d0459..89b0429 100644 --- a/cartographer/io/points_batch.h +++ b/cartographer/io/points_batch.h @@ -26,6 +26,9 @@ namespace cartographer { namespace io { +// A point's color. +using Color = std::array; + // A number of points, captured around the same 'time' and by a // sensor at the same 'origin'. struct PointsBatch { @@ -49,10 +52,12 @@ struct PointsBatch { int trajectory_index; std::vector points; - std::vector normals; - std::vector> colors; + std::vector colors; }; +// Removes the indices in 'to_remove' from 'batch'. +void RemovePoints(std::vector to_remove, PointsBatch* batch); + } // namespace io } // namespace cartographer diff --git a/cartographer/io/points_processor.h b/cartographer/io/points_processor.h index 618fe43..06b7a78 100644 --- a/cartographer/io/points_processor.h +++ b/cartographer/io/points_processor.h @@ -17,24 +17,34 @@ #ifndef CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ #define CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ +#include + #include "cartographer/io/points_batch.h" namespace cartographer { namespace io { // 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 -// send through the pipeline. +// next processor in the pipeline. class PointsProcessor { public: + enum class FlushResult { + kRestartStream, + kFinished, + }; + PointsProcessor() {} virtual ~PointsProcessor() {} PointsProcessor(const PointsProcessor&) = delete; PointsProcessor& operator=(const PointsProcessor&) = delete; - virtual void Process(const PointsBatch& points_batch) = 0; - virtual void Flush() = 0; + // Receive a 'points_batch', process it and pass it on. + virtual void Process(std::unique_ptr 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 diff --git a/cartographer/io/xray_points_processor.cc b/cartographer/io/xray_points_processor.cc index b3814fc..8cda39b 100644 --- a/cartographer/io/xray_points_processor.cc +++ b/cartographer/io/xray_points_processor.cc @@ -83,17 +83,25 @@ XRayPointsProcessor::XRayPointsProcessor(const double voxel_size, transform_(transform), voxels_(voxel_size, Eigen::Vector3f::Zero()) {} -void XRayPointsProcessor::Process(const PointsBatch& batch) { - for (const auto& point : batch.points) { +void XRayPointsProcessor::Process(std::unique_ptr batch) { + for (const auto& point : batch->points) { const Eigen::Vector3f camera_point = transform_ * point; *voxels_.mutable_value(voxels_.GetCellIndex(camera_point)) = true; } - next_->Process(batch); + next_->Process(std::move(batch)); } -void XRayPointsProcessor::Flush() { +PointsProcessor::FlushResult XRayPointsProcessor::Flush() { 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() { diff --git a/cartographer/io/xray_points_processor.h b/cartographer/io/xray_points_processor.h index 200c03b..a3cea96 100644 --- a/cartographer/io/xray_points_processor.h +++ b/cartographer/io/xray_points_processor.h @@ -35,8 +35,8 @@ class XRayPointsProcessor : public PointsProcessor { ~XRayPointsProcessor() override {} - void Process(const PointsBatch& batch) override; - void Flush() override; + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; private: using Voxels = mapping_3d::HybridGridBase; diff --git a/cartographer/sensor/CMakeLists.txt b/cartographer/sensor/CMakeLists.txt index 2337931..6a863d4 100644 --- a/cartographer/sensor/CMakeLists.txt +++ b/cartographer/sensor/CMakeLists.txt @@ -60,6 +60,7 @@ google_library(sensor_data HDRS data.h DEPENDS + common_time kalman_filter_pose_tracker sensor_laser transform_rigid_transform