| 96 | } |
| 97 | |
| 98 | Indices OutlierFilter::processStatistical(PointViewPtr inView) |
| 99 | { |
| 100 | KD3Index index(*inView); |
| 101 | index.build(); |
| 102 | |
| 103 | point_count_t np = inView->size(); |
| 104 | |
| 105 | PointIdList inliers, outliers; |
| 106 | |
| 107 | std::vector<double> distances(np, 0.0); |
| 108 | |
| 109 | // we increase the count by one because the query point itself will |
| 110 | // be included with a distance of 0 |
| 111 | point_count_t count = m_meanK + 1; |
| 112 | PointIdList indices(count); |
| 113 | std::vector<double> sqr_dists(count); |
| 114 | for (PointId i = 0; i < np; ++i) |
| 115 | { |
| 116 | |
| 117 | index.knnSearch(i, count, &indices, &sqr_dists); |
| 118 | |
| 119 | for (size_t j = 1; j < count; ++j) |
| 120 | { |
| 121 | double delta = std::sqrt(sqr_dists[j]) - distances[i]; |
| 122 | distances[i] += (delta / j); |
| 123 | } |
| 124 | indices.clear(); indices.resize(count); |
| 125 | sqr_dists.clear(); sqr_dists.resize(count); |
| 126 | } |
| 127 | |
| 128 | size_t n(0); |
| 129 | double M1(0.0); |
| 130 | double M2(0.0); |
| 131 | for (auto const& d : distances) |
| 132 | { |
| 133 | size_t n1(n); |
| 134 | n++; |
| 135 | double delta = d - M1; |
| 136 | double delta_n = delta / n; |
| 137 | M1 += delta_n; |
| 138 | M2 += delta * delta_n * n1; |
| 139 | } |
| 140 | double mean = M1; |
| 141 | double variance = M2 / (n - 1.0); |
| 142 | double stdev = std::sqrt(variance); |
| 143 | |
| 144 | double threshold = mean + m_multiplier * stdev; |
| 145 | |
| 146 | for (PointId i = 0; i < np; ++i) |
| 147 | { |
| 148 | if (distances[i] < threshold) |
| 149 | inliers.push_back(i); |
| 150 | else |
| 151 | outliers.push_back(i); |
| 152 | } |
| 153 | |
| 154 | return Indices{inliers, outliers}; |
| 155 | } |