| 71 | } |
| 72 | |
| 73 | void OptimalNeighborhood::filter(PointView& view) |
| 74 | { |
| 75 | // Build the 3D KD-tree. |
| 76 | const KD3Index& index = view.build3dIndex(); |
| 77 | |
| 78 | for (PointRef p : view) |
| 79 | { |
| 80 | // find the max k-nearest neighbors |
| 81 | PointIdList id3(m_kMax); |
| 82 | std::vector<double> dists(m_kMax); |
| 83 | index.knnSearch(p, m_kMax, &id3, &dists); |
| 84 | |
| 85 | double minentropy = (std::numeric_limits<double>::max)(); |
| 86 | point_count_t kopt(0); |
| 87 | double ropt(0.0); |
| 88 | double mx(0.0); |
| 89 | double my(0.0); |
| 90 | double mz(0.0); |
| 91 | Matrix3d B = Matrix3d::Zero(3, 3); |
| 92 | |
| 93 | // precompute covariance matrix up to k-1 neighbors |
| 94 | for (point_count_t k = 0; k < m_kMin - 1; ++k) |
| 95 | { |
| 96 | PointRef q = view.point(id3[k]); |
| 97 | |
| 98 | double dx = q.getFieldAs<double>(Id::X) - mx; |
| 99 | double dy = q.getFieldAs<double>(Id::Y) - my; |
| 100 | double dz = q.getFieldAs<double>(Id::Z) - mz; |
| 101 | double n = double(k + 1); |
| 102 | mx += dx / n; |
| 103 | my += dy / n; |
| 104 | mz += dz / n; |
| 105 | double s = (n - 1) / n; |
| 106 | B(0, 0) = B(0, 0) + s * dx * dx; |
| 107 | B(1, 1) = B(1, 1) + s * dy * dy; |
| 108 | B(2, 2) = B(2, 2) + s * dz * dz; |
| 109 | B(1, 0) = B(0, 1) = B(0, 1) + s * dx * dy; |
| 110 | B(2, 0) = B(0, 2) = B(0, 2) + s * dx * dz; |
| 111 | B(1, 2) = B(2, 1) = B(2, 1) + s * dy * dz; |
| 112 | } |
| 113 | |
| 114 | // update covariance for all k in the range [kMin, kMax], compute |
| 115 | // eigenentropy and update optimal values |
| 116 | for (point_count_t k = m_kMin - 1; k < m_kMax; ++k) |
| 117 | { |
| 118 | PointRef q = view.point(id3[k]); |
| 119 | |
| 120 | double dx = q.getFieldAs<double>(Id::X) - mx; |
| 121 | double dy = q.getFieldAs<double>(Id::Y) - my; |
| 122 | double dz = q.getFieldAs<double>(Id::Z) - mz; |
| 123 | double n = double(k + 1); |
| 124 | mx += dx / n; |
| 125 | my += dy / n; |
| 126 | mz += dz / n; |
| 127 | double s = (n - 1) / n; |
| 128 | B(0, 0) = B(0, 0) + s * dx * dx; |
| 129 | B(1, 1) = B(1, 1) + s * dy * dy; |
| 130 | B(2, 2) = B(2, 2) + s * dz * dz; |