29 const float max_range) {
31 for (
const Eigen::Vector3f& point : point_cloud) {
32 if (point.norm() <= max_range) {
33 result.push_back(point);
40 const proto::AdaptiveVoxelFilterOptions& options,
42 if (point_cloud.size() <= options.min_num_points()) {
46 PointCloud result = VoxelFilter(options.max_length()).Filter(point_cloud);
47 if (result.size() >= options.min_num_points()) {
54 for (
float high_length = options.max_length();
55 high_length > 1e-2f * options.max_length(); high_length /= 2.f) {
56 float low_length = high_length / 2.f;
57 result = VoxelFilter(low_length).Filter(point_cloud);
58 if (result.size() >= options.min_num_points()) {
62 while ((high_length - low_length) / low_length > 1e-1f) {
63 const float mid_length = (low_length + high_length) / 2.f;
65 VoxelFilter(mid_length).Filter(point_cloud);
66 if (candidate.size() >= options.min_num_points()) {
67 low_length = mid_length;
70 high_length = mid_length;
83 for (
const Eigen::Vector3f& point : point_cloud) {
85 if (it_inserted.second) {
86 results.push_back(point);
94 for (
const Eigen::Vector4f& point : timed_point_cloud) {
97 if (it_inserted.second) {
98 results.push_back(point);
104 std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>
106 const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&
107 range_measurements) {
108 std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> results;
109 for (
const auto& range_measurement : range_measurements) {
112 if (it_inserted.second) {
113 results.push_back(range_measurement);
120 KeyType k_0(static_cast<uint32>(index[0]));
121 KeyType k_1(static_cast<uint32>(index[1]));
122 KeyType k_2(static_cast<uint32>(index[2]));
123 return (k_0 << 2 * 32) | (k_1 << 1 * 32) | k_2;
127 Eigen::Array3f index = point.array() /
resolution_;
135 proto::AdaptiveVoxelFilterOptions options;
136 options.set_max_length(parameter_dictionary->
GetDouble(
"max_length"));
137 options.set_min_num_points(
139 options.set_max_range(parameter_dictionary->
GetDouble(
"max_range"));
144 const proto::AdaptiveVoxelFilterOptions& options)
148 return AdaptivelyVoxelFiltered(
PointCloud Filter(const PointCloud &point_cloud) const
int RoundToInt(const float x)
AdaptiveVoxelFilter(const proto::AdaptiveVoxelFilterOptions &options)
int GetNonNegativeInt(const std::string &key)
double GetDouble(const std::string &key)
proto::ProbabilityGridRangeDataInserterOptions2D options_
static KeyType IndexToKey(const Eigen::Array3i &index)
Eigen::Array3i GetCellIndex(const Eigen::Vector3f &point) const
std::vector< Eigen::Vector3f > PointCloud
proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(common::LuaParameterDictionary *const parameter_dictionary)
std::vector< Eigen::Vector4f > TimedPointCloud
std::bitset< 3 *32 > KeyType
const proto::AdaptiveVoxelFilterOptions options_
std::unordered_set< KeyType > voxel_set_
PointCloud Filter(const PointCloud &point_cloud)