diff --git a/cartographer/io/CMakeLists.txt b/cartographer/io/CMakeLists.txt index efd3f8b..7689bde 100644 --- a/cartographer/io/CMakeLists.txt +++ b/cartographer/io/CMakeLists.txt @@ -49,6 +49,19 @@ google_library(io_null_points_processor io_points_processor ) +google_library(io_outlier_removing_points_processor + USES_GLOG + SRCS + outlier_removing_points_processor.cc + HDRS + outlier_removing_points_processor.h + DEPENDS + common_lua_parameter_dictionary + common_make_unique + io_points_processor + mapping_3d_hybrid_grid +) + google_library(io_pcd_writing_points_processor USES_GLOG SRCS @@ -104,6 +117,7 @@ google_library(io_points_processor_pipeline_builder io_fixed_ratio_sampling_points_processor io_min_max_range_filtering_points_processor io_null_points_processor + io_outlier_removing_points_processor io_pcd_writing_points_processor io_ply_writing_points_processor io_points_processor diff --git a/cartographer/io/outlier_removing_points_processor.cc b/cartographer/io/outlier_removing_points_processor.cc new file mode 100644 index 0000000..8a038e0 --- /dev/null +++ b/cartographer/io/outlier_removing_points_processor.cc @@ -0,0 +1,119 @@ +/* + * 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/outlier_removing_points_processor.h" + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/make_unique.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +std::unique_ptr +OutlierRemovingPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return common::make_unique( + dictionary->GetDouble("voxel_size"), next); +} + +OutlierRemovingPointsProcessor::OutlierRemovingPointsProcessor( + const double voxel_size, PointsProcessor* next) + : voxel_size_(voxel_size), + next_(next), + state_(State::kPhase1), + voxels_(voxel_size_, Eigen::Vector3f::Zero()) { + LOG(INFO) << "Marking hits..."; +} + +void OutlierRemovingPointsProcessor::Process( + std::unique_ptr points) { + switch (state_) { + case State::kPhase1: + ProcessInPhaseOne(*points); + break; + + case State::kPhase2: + ProcessInPhaseTwo(*points); + break; + + case State::kPhase3: + ProcessInPhaseThree(std::move(points)); + break; + } +} + +PointsProcessor::FlushResult OutlierRemovingPointsProcessor::Flush() { + switch (state_) { + case State::kPhase1: + LOG(INFO) << "Counting rays..."; + state_ = State::kPhase2; + return FlushResult::kRestartStream; + + case State::kPhase2: + LOG(INFO) << "Filtering outliers..."; + state_ = State::kPhase3; + return FlushResult::kRestartStream; + + case State::kPhase3: + CHECK(next_->Flush() == FlushResult::kFinished) + << "Voxel filtering and outlier removal must be configured to occur " + "after any stages that require multiple passes."; + return FlushResult::kFinished; + } + LOG(FATAL); +} + +void OutlierRemovingPointsProcessor::ProcessInPhaseOne( + const PointsBatch& batch) { + for (size_t i = 0; i < batch.points.size(); ++i) { + ++voxels_.mutable_value(voxels_.GetCellIndex(batch.points[i]))->hits; + } +} + +void OutlierRemovingPointsProcessor::ProcessInPhaseTwo( + const PointsBatch& batch) { + // TODO(whess): This samples every 'voxel_size' distance and could be improved + // by better ray casting, and also by marking the hits of the current laser + // fan to be excluded. + for (size_t i = 0; i < batch.points.size(); ++i) { + const Eigen::Vector3f delta = batch.points[i] - batch.origin; + const float length = delta.norm(); + for (float x = 0; x < length; x += voxel_size_) { + const auto index = + voxels_.GetCellIndex(batch.origin + (x / length) * delta); + if (voxels_.value(index).hits > 0) { + ++voxels_.mutable_value(index)->rays; + } + } + } +} + +void OutlierRemovingPointsProcessor::ProcessInPhaseThree( + std::unique_ptr batch) { + constexpr double kMissPerHitLimit = 3; + std::vector to_remove; + for (size_t i = 0; i < batch->points.size(); ++i) { + const auto voxel = voxels_.value(voxels_.GetCellIndex(batch->points[i])); + to_remove.push_back(!(voxel.rays < kMissPerHitLimit * voxel.hits)); + } + RemovePoints(to_remove, batch.get()); + next_->Process(std::move(batch)); +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/outlier_removing_points_processor.h b/cartographer/io/outlier_removing_points_processor.h new file mode 100644 index 0000000..5f233e6 --- /dev/null +++ b/cartographer/io/outlier_removing_points_processor.h @@ -0,0 +1,87 @@ +/* + * 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_OUTLIER_REMOVING_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/mapping_3d/hybrid_grid.h" + +namespace cartographer { +namespace io { + +// Voxel filters the data and only passes on points that we believe are on +// non-moving objects. +class OutlierRemovingPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "voxel_filter_and_remove_moving_objects"; + + OutlierRemovingPointsProcessor(double voxel_size, PointsProcessor* next); + + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~OutlierRemovingPointsProcessor() override {} + + OutlierRemovingPointsProcessor(const OutlierRemovingPointsProcessor&) = + delete; + OutlierRemovingPointsProcessor& operator=( + const OutlierRemovingPointsProcessor&) = delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + // To reduce memory consumption by not having to keep all rays in memory, we + // filter outliers in three phases each going over all data: First we compute + // all voxels containing any hits, then we compute the rays passing through + // each of these voxels, and finally we output all hits in voxels that are + // considered obstructed. + struct VoxelData { + int hits = 0; + int rays = 0; + }; + enum class State { + kPhase1, + kPhase2, + kPhase3, + }; + + // First phase counts the number of hits per voxel. + void ProcessInPhaseOne(const PointsBatch& batch); + + // Second phase counts how many rays pass through each voxel. This is only + // done for voxels that contain hits. This is to reduce memory consumption by + // not adding data to free voxels. + void ProcessInPhaseTwo(const PointsBatch& batch); + + // Third phase produces the output containing all inliers. We consider each + // hit an inlier if it is inside a voxel that has a sufficiently high + // hit-to-ray ratio. + void ProcessInPhaseThree(std::unique_ptr batch); + + const double voxel_size_; + PointsProcessor* const next_; + State state_; + mapping_3d::HybridGridBase voxels_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_ diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index 31dc620..726cc73 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -21,6 +21,7 @@ #include "cartographer/io/fixed_ratio_sampling_points_processor.h" #include "cartographer/io/min_max_range_filtering_points_processor.h" #include "cartographer/io/null_points_processor.h" +#include "cartographer/io/outlier_removing_points_processor.h" #include "cartographer/io/pcd_writing_points_processor.h" #include "cartographer/io/ply_writing_points_processor.h" #include "cartographer/io/xray_points_processor.h" @@ -33,6 +34,7 @@ PointsProcessorPipelineBuilder::PointsProcessorPipelineBuilder() { RegisterNonStatic(); RegisterNonStatic(); RegisterNonStatic(); + RegisterNonStatic(); RegisterNonStatic(); RegisterNonStatic(); RegisterNonStatic();