| 17 | } |
| 18 | |
| 19 | void Plane::filter(const VectorType &points, const FeatureModelParams &fmParams) |
| 20 | { |
| 21 | int numFeatures = 0; |
| 22 | PointT origin = computeCentroid(points); |
| 23 | boost::multi_array<VectorType, 2> scgf( |
| 24 | boost::extents[fmParams.groundRadiiBins][fmParams.groundThetaBins]); |
| 25 | // Add points into the corresponding bin |
| 26 | for (const PointT &p : points) |
| 27 | { |
| 28 | // Sort into the multiarray grid |
| 29 | Scalar pointRadius = euclideanDist2D(origin, p); |
| 30 | if (pointRadius < fmParams.maxGroundLidarDist && |
| 31 | pointRadius > fmParams.minGroundLidarDist) |
| 32 | { |
| 33 | Scalar pointTheta = atan2(p.y - origin.y, p.x - origin.x); |
| 34 | int pointRadiiBin = |
| 35 | floor(pointRadius / (fmParams.maxGroundLidarDist / |
| 36 | (Scalar)fmParams.groundRadiiBins)); |
| 37 | int pointThetaBin = |
| 38 | floor((PIDEF + pointTheta) / |
| 39 | (2 * PIDEF / (Scalar)fmParams.groundThetaBins)); |
| 40 | // Edge case when pointTheta = 180 etc |
| 41 | pointRadiiBin = |
| 42 | std::max(0, std::min(pointRadiiBin, fmParams.groundRadiiBins - 1)); |
| 43 | pointThetaBin = |
| 44 | std::max(0, std::min(pointThetaBin, fmParams.groundThetaBins - 1)); |
| 45 | scgf[pointRadiiBin][pointThetaBin].push_back(p); |
| 46 | } |
| 47 | } |
| 48 | // Retain the bottom k% of points in each cell |
| 49 | for (int i = 0; i < scgf.shape()[0]; i++) |
| 50 | { |
| 51 | for (int j = 0; j < scgf.shape()[1]; j++) |
| 52 | { |
| 53 | if (scgf[i][j].size() > 0) |
| 54 | { |
| 55 | float retainNum = (1 / fmParams.groundRetainThresh); |
| 56 | if (retainNum < scgf[i][j].size()) |
| 57 | { |
| 58 | // Bottom 10% |
| 59 | int bottomIdx = |
| 60 | int(scgf[i][j].size() / retainNum); |
| 61 | |
| 62 | // Sort by z value |
| 63 | std::sort( |
| 64 | scgf[i][j].begin(), scgf[i][j].end(), |
| 65 | [](const PointT &p1, const PointT &p2) |
| 66 | { return p1.z < p2.z; }); |
| 67 | // Erase all points after the 10% |
| 68 | scgf[i][j].erase(scgf[i][j].begin() + bottomIdx, scgf[i][j].end()); |
| 69 | numFeatures = |
| 70 | numFeatures + bottomIdx + 1; // Update the number of features |
| 71 | } |
| 72 | |
| 73 | // adding to list of features |
| 74 | for (int k = 0; k < scgf[i][j].size(); k++) |
| 75 | { |
| 76 | features.push_back(scgf[i][j][k]); |