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

Filter RangeMeasurement (#995) 

[RFC=0017](https://github.com/googlecartographer/rfcs/blob/master/text/0017-synchronize-points.md)
master
gaschler 2018-03-15 12:31:08 +01:00 committed by Wally B. Feed
parent 067d01a364
commit bd7d7202bf
2 changed files with 21 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
cartographer/sensor/internal/voxel_filter.cc
View File

@ -101,6 +101,21 @@ TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) {
return results; return results;
} }
std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>
VoxelFilter::Filter(
const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&
range_measurements) {
std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> results;
for (const auto& range_measurement : range_measurements) {
auto it_inserted = voxel_set_.insert(
IndexToKey(GetCellIndex(range_measurement.point_time.head<3>())));
if (it_inserted.second) {
results.push_back(range_measurement);
}
}
return results;
}
VoxelFilter::KeyType VoxelFilter::IndexToKey(const Eigen::Array3i& index) { VoxelFilter::KeyType VoxelFilter::IndexToKey(const Eigen::Array3i& index) {
KeyType k_0(static_cast<uint32>(index[0])); KeyType k_0(static_cast<uint32>(index[0]));
KeyType k_1(static_cast<uint32>(index[1])); KeyType k_1(static_cast<uint32>(index[1]));

6
cartographer/sensor/internal/voxel_filter.h
View File

@ -23,6 +23,7 @@
#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/proto/adaptive_voxel_filter_options.pb.h" #include "cartographer/sensor/proto/adaptive_voxel_filter_options.pb.h"
#include "cartographer/sensor/timed_point_cloud_data.h"
namespace cartographer { namespace cartographer {
namespace sensor { namespace sensor {
@ -44,6 +45,11 @@ class VoxelFilter {
// Same for TimedPointCloud. // Same for TimedPointCloud.
TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud); TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud);
// Same for RangeMeasurement.
std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> Filter(
const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&
range_measurements);
private: private:
using KeyType = std::bitset<3 * 32>; using KeyType = std::bitset<3 * 32>;