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

Always use the 3D voxel filter. (#61)

master
Wolfgang Hess 2016-10-13 18:56:41 +02:00 committed by GitHub
parent 5aad2d6feb
commit cac501cdb1
5 changed files with 15 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
View File

@ -182,7 +182,7 @@ void ConstraintBuilder::ComputeConstraint(
const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_index);
const sensor::PointCloud filtered_point_cloud =
sensor::ToPointCloud(adaptive_voxel_filter_.Filter(*point_cloud));
adaptive_voxel_filter_.Filter(sensor::ToPointCloud(*point_cloud));
// The 'constraint_transform' (i <- j) is computed from:
// - a 'filtered_point_cloud' in j,

4
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -72,8 +72,8 @@ class SparsePoseGraphTest : public ::testing::Test {
sampling_ratio = 1.,
max_constraint_distance = 6.,
adaptive_voxel_filter = {
max_length = 1e-5,
min_num_points = 1,
max_length = 1e-2,
min_num_points = 1000,
max_range = 50.,
},
min_score = 0.5,

53
cartographer/sensor/voxel_filter.cc
View File

@ -25,11 +25,10 @@ namespace sensor {
namespace {
template <typename PointCloudType>
PointCloudType FilterByMaxRange(const PointCloudType& point_cloud,
const float max_range) {
PointCloudType result;
for (const auto& point : point_cloud) {
PointCloud FilterByMaxRange(const PointCloud& point_cloud,
const float max_range) {
PointCloud result;
for (const Eigen::Vector3f& point : point_cloud) {
if (point.norm() <= max_range) {
result.push_back(point);
}
@ -37,15 +36,14 @@ PointCloudType FilterByMaxRange(const PointCloudType& point_cloud,
return result;
}
template <typename PointCloudType>
PointCloudType AdaptivelyVoxelFiltered(
PointCloud AdaptivelyVoxelFiltered(
const proto::AdaptiveVoxelFilterOptions& options,
const PointCloudType& point_cloud) {
const PointCloud& point_cloud) {
if (point_cloud.size() <= options.min_num_points()) {
// 'point_cloud' is already sparse enough.
return point_cloud;
}
PointCloudType result = VoxelFiltered(point_cloud, options.max_length());
PointCloud result = VoxelFiltered(point_cloud, options.max_length());
if (result.size() >= options.min_num_points()) {
// Filtering with 'max_length' resulted in a sufficiently dense point cloud.
return result;
@ -63,7 +61,7 @@ PointCloudType AdaptivelyVoxelFiltered(
// edge length is at most 10% off.
while ((high_length - low_length) / low_length > 1e-1f) {
const float mid_length = (low_length + high_length) / 2.f;
const PointCloudType candidate = VoxelFiltered(point_cloud, mid_length);
const PointCloud candidate = VoxelFiltered(point_cloud, mid_length);
if (candidate.size() >= options.min_num_points()) {
low_length = mid_length;
result = candidate;
@ -79,37 +77,16 @@ PointCloudType AdaptivelyVoxelFiltered(
} // namespace
PointCloud2D VoxelFiltered(const PointCloud2D& point_cloud, const float size) {
PointCloud VoxelFiltered(const PointCloud& point_cloud, const float size) {
VoxelFilter voxel_filter(size);
voxel_filter.InsertPointCloud(point_cloud);
return voxel_filter.point_cloud();
}
PointCloud VoxelFiltered(const PointCloud& point_cloud, const float size) {
VoxelFilter3D voxel_filter(size);
voxel_filter.InsertPointCloud(point_cloud);
return voxel_filter.point_cloud();
}
VoxelFilter::VoxelFilter(const float size) : size_(size) {}
void VoxelFilter::InsertPointCloud(const PointCloud2D& point_cloud) {
for (const Eigen::Vector2f& point : point_cloud) {
if (voxels_
.emplace(common::RoundToInt64(point.x() / size_),
common::RoundToInt64(point.y() / size_))
.second) {
point_cloud_.push_back(point);
}
}
}
const PointCloud2D& VoxelFilter::point_cloud() const { return point_cloud_; }
VoxelFilter3D::VoxelFilter3D(const float size)
VoxelFilter::VoxelFilter(const float size)
: voxels_(size, Eigen::Vector3f::Zero()) {}
void VoxelFilter3D::InsertPointCloud(const PointCloud& point_cloud) {
void VoxelFilter::InsertPointCloud(const PointCloud& point_cloud) {
for (const Eigen::Vector3f& point : point_cloud) {
auto* const value = voxels_.mutable_value(voxels_.GetCellIndex(point));
if (*value == 0) {
@ -119,7 +96,7 @@ void VoxelFilter3D::InsertPointCloud(const PointCloud& point_cloud) {
}
}
const PointCloud& VoxelFilter3D::point_cloud() const { return point_cloud_; }
const PointCloud& VoxelFilter::point_cloud() const { return point_cloud_; }
proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
@ -135,12 +112,6 @@ AdaptiveVoxelFilter::AdaptiveVoxelFilter(
const proto::AdaptiveVoxelFilterOptions& options)
: options_(options) {}
PointCloud2D AdaptiveVoxelFilter::Filter(
const PointCloud2D& point_cloud) const {
return AdaptivelyVoxelFiltered(
options_, FilterByMaxRange(point_cloud, options_.max_range()));
}
PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud) const {
return AdaptivelyVoxelFiltered(
options_, FilterByMaxRange(point_cloud, options_.max_range()));

38
cartographer/sensor/voxel_filter.h
View File

@ -17,10 +17,6 @@
#ifndef CARTOGRAPHER_SENSOR_VOXEL_FILTER_H_
#define CARTOGRAPHER_SENSOR_VOXEL_FILTER_H_
#include <tuple>
#include <unordered_set>
#include <utility>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/sensor/point_cloud.h"
@ -29,10 +25,6 @@
namespace cartographer {
namespace sensor {
// Returns a voxel filtered copy of 'point_cloud' where 'size' is the length
// a voxel edge.
PointCloud2D VoxelFiltered(const PointCloud2D& point_cloud, float size);
// Returns a voxel filtered copy of 'point_cloud' where 'size' is the length
// a voxel edge.
PointCloud VoxelFiltered(const PointCloud& point_cloud, float size);
@ -48,35 +40,6 @@ class VoxelFilter {
VoxelFilter(const VoxelFilter&) = delete;
VoxelFilter& operator=(const VoxelFilter&) = delete;
// Inserts a point cloud into the voxel filter.
void InsertPointCloud(const PointCloud2D& point_cloud);
// Returns the filtered point cloud representing the occupied voxels.
const PointCloud2D& point_cloud() const;
private:
struct IntegerPairHash {
size_t operator()(const std::pair<int64, int64>& x) const {
const uint64 first = x.first;
const uint64 second = x.second;
return first ^ (first + 0x9e3779b9u + (second << 6) + (second >> 2));
}
};
const float size_;
std::unordered_set<std::pair<int64, int64>, IntegerPairHash> voxels_;
PointCloud2D point_cloud_;
};
// The same as VoxelFilter but for 3D PointClouds.
class VoxelFilter3D {
public:
// 'size' is the length of a voxel edge.
explicit VoxelFilter3D(float size);
VoxelFilter3D(const VoxelFilter3D&) = delete;
VoxelFilter3D& operator=(const VoxelFilter3D&) = delete;
// Inserts a point cloud into the voxel filter.
void InsertPointCloud(const PointCloud& point_cloud);
@ -99,7 +62,6 @@ class AdaptiveVoxelFilter {
AdaptiveVoxelFilter(const AdaptiveVoxelFilter&) = delete;
AdaptiveVoxelFilter& operator=(const AdaptiveVoxelFilter&) = delete;
PointCloud2D Filter(const PointCloud2D& point_cloud) const;
PointCloud Filter(const PointCloud& point_cloud) const;
private:

7
cartographer/sensor/voxel_filter_test.cc
View File

@ -27,13 +27,6 @@ namespace {
using ::testing::ContainerEq;
TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) {
PointCloud2D point_cloud = {
{0.f, 0.f}, {0.1f, -0.1f}, {0.3f, -0.1f}, {0.f, 0.f}};
EXPECT_THAT(VoxelFiltered(point_cloud, 0.3f),
ContainerEq(PointCloud2D{point_cloud[0], point_cloud[2]}));
}
TEST(VoxelFilter3DTest, ReturnsTheFirstPointInEachVoxel) {
PointCloud point_cloud = {{0.f, 0.f, 0.f},
{0.1f, -0.1f, 0.1f},
{0.3f, -0.1f, 0.f},