Adds a voxel filtering and moving objects removing points processor. (#99)

master
Holger Rapp 2016-10-25 16:36:56 +02:00 committed by GitHub
parent 915dbd1894
commit a3157239b7
4 changed files with 222 additions and 0 deletions

View File

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

View File

@ -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>
OutlierRemovingPointsProcessor::FromDictionary(
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
return common::make_unique<OutlierRemovingPointsProcessor>(
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<PointsBatch> 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<PointsBatch> batch) {
constexpr double kMissPerHitLimit = 3;
std::vector<int> 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

View File

@ -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<OutlierRemovingPointsProcessor> FromDictionary(
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
~OutlierRemovingPointsProcessor() override {}
OutlierRemovingPointsProcessor(const OutlierRemovingPointsProcessor&) =
delete;
OutlierRemovingPointsProcessor& operator=(
const OutlierRemovingPointsProcessor&) = delete;
void Process(std::unique_ptr<PointsBatch> 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<PointsBatch> batch);
const double voxel_size_;
PointsProcessor* const next_;
State state_;
mapping_3d::HybridGridBase<VoxelData> voxels_;
};
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_

View File

@ -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<CountingPointsProcessor>();
RegisterNonStatic<FixedRatioSamplingPointsProcessor>();
RegisterNonStatic<MinMaxRangeFiteringPointsProcessor>();
RegisterNonStatic<OutlierRemovingPointsProcessor>();
RegisterNonStatic<PcdWritingPointsProcessor>();
RegisterNonStatic<PlyWritingPointsProcessor>();
RegisterNonStatic<XRayPointsProcessor>();