| 31 | } //anonymous namespace |
| 32 | |
| 33 | std::optional<VertBitSet> pointIterativeSampling( const PointCloud& cloud, int numSamples, const ProgressCallback & cb ) |
| 34 | { |
| 35 | MR_TIMER; |
| 36 | VertBitSet res = cloud.validPoints; |
| 37 | auto toRemove = (int)res.count() - numSamples; |
| 38 | if ( toRemove <= 0 ) |
| 39 | return res; |
| 40 | |
| 41 | const auto sz = cloud.validPoints.size(); |
| 42 | Buffer<VertId, VertId> closestNei( sz ); |
| 43 | Buffer<PointInfo, VertId> info( sz ); |
| 44 | cloud.getAABBTree(); |
| 45 | BitSetParallelFor( cloud.validPoints, [&]( VertId v ) |
| 46 | { |
| 47 | const auto prj = findProjectionOnPoints( cloud.points[v], cloud, FLT_MAX, nullptr, 0, [v]( VertId x ) { return v == x; } ); |
| 48 | closestNei[v] = prj.vId; |
| 49 | info[v].sumDistSq = prj.distSq; |
| 50 | } ); |
| 51 | |
| 52 | if ( !reportProgress( cb, 0.1f ) ) |
| 53 | return {}; |
| 54 | |
| 55 | Vector<VertId, VertId> first( sz ); ///< first[v] contains a pointId having closest point v |
| 56 | Buffer<VertId, VertId> next( sz ); ///< next[v] contains pointId having the same closest point as v's closest point |
| 57 | for ( auto v : cloud.validPoints ) |
| 58 | { |
| 59 | const auto cv = closestNei[v]; |
| 60 | next[v] = first[cv]; |
| 61 | first[cv] = v; |
| 62 | } |
| 63 | |
| 64 | if ( !reportProgress( cb, 0.2f ) ) |
| 65 | return {}; |
| 66 | |
| 67 | using HeapT = Heap<PointInfo, VertId>; |
| 68 | std::vector<HeapT::Element> elms; |
| 69 | elms.reserve( numSamples + toRemove ); |
| 70 | for ( auto vid : cloud.validPoints ) |
| 71 | elms.push_back( { vid, info[vid] } ); |
| 72 | info.clear(); |
| 73 | HeapT heap( std::move( elms ) ); |
| 74 | |
| 75 | if ( !reportProgress( cb, 0.3f ) ) |
| 76 | return {}; |
| 77 | |
| 78 | const auto k = 1.0f / toRemove; |
| 79 | while ( toRemove > 0 ) |
| 80 | { |
| 81 | auto [v, vinfo] = heap.top(); |
| 82 | assert( vinfo.sumDistSq < FLT_MAX ); |
| 83 | --toRemove; |
| 84 | assert( res.test( v ) ); |
| 85 | res.reset( v ); |
| 86 | heap.setSmallerValue( v, PointInfo() ); |
| 87 | |
| 88 | auto cv = closestNei[v]; // TODO: remove cv as well, and select a sample in between v and cv |
| 89 | auto cvinfo = heap.value( cv ); |
| 90 | cvinfo.sumDistSq += vinfo.sumDistSq; |