Debian Codegraph @

plugins/core/Standard/qRANSAC_SD/extern/RANSAC_SD/BasePrimitiveShape.h

raw
1 #ifndef BASEPRIMITIVESHAPE_HEADER
2 #define BASEPRIMITIVESHAPE_HEADER
3 #include "PrimitiveShape.h"
4 #include "ScoreComputer.h"
5 #include <MiscLib/Random.h>
6 #include <GfxTL/MathHelper.h>
7
8 #ifndef DLL_LINKAGE
9 #define DLL_LINKAGE
10 #endif
11
12 class DLL_LINKAGE BasePrimitiveShape
13 : public PrimitiveShape
14 {
15 protected:
16 template< class ShapeT >
17 unsigned int ConfidenceTests(unsigned int numTests, float epsilon,
18 float normalThresh, float rms, const PointCloud &pc,
19 const MiscLib::Vector< size_t > &indices) const
20 {
21 unsigned int numFailures = 0;
22 // estimate shapes
23 const unsigned int numSamples = ShapeT::RequiredSamples;
24 if(numSamples >= indices.size())
25 return numTests;
26 MiscLib::Vector< ShapeT > shapes;
27 for(unsigned int i = 0; i < numTests; ++i)
28 {
29 MiscLib::Vector< size_t > sampleIndices;
30 for(unsigned int j = 0; j < numSamples; ++j)
31 {
32 size_t idx;
33 do
34 {
35 idx = indices[MiscLib::rn_rand() % indices.size()];
36 }
37 while(std::find(sampleIndices.begin(), sampleIndices.end(), idx)
38 != sampleIndices.end());
39 sampleIndices.push_back(idx);
40 }
41 MiscLib::Vector< Vec3f > samples(numSamples << 1);
42 for(unsigned int j = 0; j < numSamples; ++j)
43 {
44 samples[j] = pc[sampleIndices[j]].pos;
45 samples[j + numSamples] = pc[sampleIndices[j]].normal;
46 }
47 ShapeT shape;
48 if(shape.Init(samples))
49 shapes.push_back(shape);
50 else
51 ++numFailures;
52 }
53
54 MiscLib::Vector< size_t > scores(shapes.size(), 0);
55 //MiscLib::Vector< float > sse(shapes.size(), 0.f);
56 Vec3f n;
57 for(size_t i = 0; i < indices.size(); ++i)
58 {
59 for(size_t j = 0; j < shapes.size(); ++j)
60 {
61 //float d = shapes[j].Distance(pc[indices[i]].pos);
62 //sse[j] += d * d;
63 float d = shapes[j].DistanceAndNormal(pc[indices[i]].pos, &n);
64 if(d > epsilon)
65 continue;
66 float dn = n.dot(pc[indices[i]].normal);
67 if(fabs(dn) > normalThresh)
68 ++scores[j];
69 }
70 }
71
72 for(size_t i = 0; i < /*sse.size()*/scores.size(); ++i)
73 {
74 if(scores[i] < .9f * indices.size())
75 //if(fabs(std::sqrt(sse[i] / indices.size()) - rms) > 1e-2)
76 ++numFailures;
77 }
78
79 return numFailures;
80 }
81 };
82
83 #endif