Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/sensor/internal/voxel_filter.h"
00018
00019 #include <cmath>
00020
00021 #include "cartographer/common/math.h"
00022
00023 namespace cartographer {
00024 namespace sensor {
00025
00026 namespace {
00027
00028 PointCloud FilterByMaxRange(const PointCloud& point_cloud,
00029 const float max_range) {
00030 PointCloud result;
00031 for (const RangefinderPoint& point : point_cloud) {
00032 if (point.position.norm() <= max_range) {
00033 result.push_back(point);
00034 }
00035 }
00036 return result;
00037 }
00038
00039 PointCloud AdaptivelyVoxelFiltered(
00040 const proto::AdaptiveVoxelFilterOptions& options,
00041 const PointCloud& point_cloud) {
00042 if (point_cloud.size() <= options.min_num_points()) {
00043
00044 return point_cloud;
00045 }
00046 PointCloud result = VoxelFilter(options.max_length()).Filter(point_cloud);
00047 if (result.size() >= options.min_num_points()) {
00048
00049 return result;
00050 }
00051
00052
00053
00054 for (float high_length = options.max_length();
00055 high_length > 1e-2f * options.max_length(); high_length /= 2.f) {
00056 float low_length = high_length / 2.f;
00057 result = VoxelFilter(low_length).Filter(point_cloud);
00058 if (result.size() >= options.min_num_points()) {
00059
00060
00061
00062 while ((high_length - low_length) / low_length > 1e-1f) {
00063 const float mid_length = (low_length + high_length) / 2.f;
00064 const PointCloud candidate =
00065 VoxelFilter(mid_length).Filter(point_cloud);
00066 if (candidate.size() >= options.min_num_points()) {
00067 low_length = mid_length;
00068 result = candidate;
00069 } else {
00070 high_length = mid_length;
00071 }
00072 }
00073 return result;
00074 }
00075 }
00076 return result;
00077 }
00078
00079 }
00080
00081 PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) {
00082 PointCloud results;
00083 for (const RangefinderPoint& point : point_cloud) {
00084 auto it_inserted =
00085 voxel_set_.insert(IndexToKey(GetCellIndex(point.position)));
00086 if (it_inserted.second) {
00087 results.push_back(point);
00088 }
00089 }
00090 return results;
00091 }
00092
00093 TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) {
00094 TimedPointCloud results;
00095 for (const TimedRangefinderPoint& point : timed_point_cloud) {
00096 auto it_inserted =
00097 voxel_set_.insert(IndexToKey(GetCellIndex(point.position)));
00098 if (it_inserted.second) {
00099 results.push_back(point);
00100 }
00101 }
00102 return results;
00103 }
00104
00105 std::vector<TimedPointCloudOriginData::RangeMeasurement> VoxelFilter::Filter(
00106 const std::vector<TimedPointCloudOriginData::RangeMeasurement>&
00107 range_measurements) {
00108 std::vector<TimedPointCloudOriginData::RangeMeasurement> results;
00109 for (const auto& range_measurement : range_measurements) {
00110 auto it_inserted = voxel_set_.insert(
00111 IndexToKey(GetCellIndex(range_measurement.point_time.position)));
00112 if (it_inserted.second) {
00113 results.push_back(range_measurement);
00114 }
00115 }
00116 return results;
00117 }
00118
00119 VoxelFilter::KeyType VoxelFilter::IndexToKey(const Eigen::Array3i& index) {
00120 KeyType k_0(static_cast<uint32>(index[0]));
00121 KeyType k_1(static_cast<uint32>(index[1]));
00122 KeyType k_2(static_cast<uint32>(index[2]));
00123 return (k_0 << 2 * 32) | (k_1 << 1 * 32) | k_2;
00124 }
00125
00126 Eigen::Array3i VoxelFilter::GetCellIndex(const Eigen::Vector3f& point) const {
00127 Eigen::Array3f index = point.array() / resolution_;
00128 return Eigen::Array3i(common::RoundToInt(index.x()),
00129 common::RoundToInt(index.y()),
00130 common::RoundToInt(index.z()));
00131 }
00132
00133 proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
00134 common::LuaParameterDictionary* const parameter_dictionary) {
00135 proto::AdaptiveVoxelFilterOptions options;
00136 options.set_max_length(parameter_dictionary->GetDouble("max_length"));
00137 options.set_min_num_points(
00138 parameter_dictionary->GetNonNegativeInt("min_num_points"));
00139 options.set_max_range(parameter_dictionary->GetDouble("max_range"));
00140 return options;
00141 }
00142
00143 AdaptiveVoxelFilter::AdaptiveVoxelFilter(
00144 const proto::AdaptiveVoxelFilterOptions& options)
00145 : options_(options) {}
00146
00147 PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud) const {
00148 return AdaptivelyVoxelFiltered(
00149 options_, FilterByMaxRange(point_cloud, options_.max_range()));
00150 }
00151
00152 }
00153 }