Debian Codegraph @

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

raw
1 #ifndef LOWSTRETCHSPHEREPARAMETRIZATION_HEADER
2 #define LOWSTRETCHSPHEREPARAMETRIZATION_HEADER
3 #include "Sphere.h"
4 #include "basic.h"
5 #include <GfxTL/VectorXD.h>
6 #include <GfxTL/Frame.h>
7 #include <GfxTL/MathHelper.h>
8 #include <GfxTL/AABox.h>
9 #include <utility>
10 #include <MiscLib/Vector.h>
11
12 class LowStretchSphereParametrization
13 {
14 public:
15 LowStretchSphereParametrization(const Sphere &sphere);
16 void Shape(const Sphere &sphere);
17 const Sphere &Shape() const { return *m_sphere; }
18 inline void Parameters(const Vec3f &p,
19 std::pair< float, float > *param) const;
20 inline bool InSpace(float u, float v, Vec3f *p) const;
21 inline bool InSpace(float u, float v, Vec3f *p, Vec3f *n) const;
22 void WrapBitmap(const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
23 float epsilon, bool *uwrap, bool *vwrap) const { *uwrap = false; *vwrap = false; }
24 void WrapComponents(const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
25 float epsilon, size_t uextent, size_t vextent,
26 MiscLib::Vector< int > *componentImg,
27 MiscLib::Vector< std::pair< int, size_t > > *labels) const;
28 template< class IteratorT >
29 void Optimize(IteratorT begin, IteratorT end, float epsilon);
30 static size_t SerializedSize();
31 void Serialize(std::ostream *o, bool binary) const;
32 void Deserialize(std::istream *i, bool binary);
33
34 private:
35 const Sphere *m_sphere;
36 GfxTL::Frame< 3, float > m_frame;
37 };
38
39 void LowStretchSphereParametrization::Parameters(const Vec3f &p,
40 std::pair< float, float > *param) const
41 {
42 Vec3f s = p - m_sphere->Center();
43 float slength = s.length();
44 GfxTL::Vector3Df l;
45 m_frame.ToLocal(*((GfxTL::Vector3Df *)&s), &l);
46 if(slength > 0) l[2] /= slength;
47 l[2] = GfxTL::Math< float >::Clamp(l[2], -1, 1);
48 float radius = std::sqrt(1.f - l[2] * l[2]) * m_sphere->Radius();
49 param->first = std::acos(l[2]) * m_sphere->Radius();
50 param->second = std::atan2(l[1], l[0]) * radius;
51 }
52
53 bool LowStretchSphereParametrization::InSpace(float u, float v, Vec3f *p) const
54 {
55 float uangle = u / m_sphere->Radius();
56 float cosu = std::cos(uangle);
57 float sinu = std::sin(uangle);
58 float radius = sinu * m_sphere->Radius();
59 float vangle = v / radius;
60 float sinv = std::sin(vangle);
61 float cosv = std::cos(vangle);
62 GfxTL::Vector3Df g;
63 m_frame.ToGlobal(GfxTL::Vector3Df(sinu * cosv, sinu * sinv, cosu), &g);
64 *p = m_sphere->Radius() * Vec3f(g) + m_sphere->Center();
65 return true;
66 }
67
68 bool LowStretchSphereParametrization::InSpace(float u, float v, Vec3f *p,
69 Vec3f *n) const
70 {
71 float uangle = u / m_sphere->Radius();
72 float cosu = std::cos(uangle);
73 float sinu = std::sin(uangle);
74 float radius = sinu * m_sphere->Radius();
75 float vangle = v / radius;
76 float sinv = std::sin(vangle);
77 float cosv = std::cos(vangle);
78 GfxTL::Vector3Df g;
79 m_frame.ToGlobal(GfxTL::Vector3Df(sinu * cosv, sinu * sinv, cosu), &g);
80 *n = Vec3f(g);
81 *p = m_sphere->Radius() * (*n) + m_sphere->Center();
82 return true;
83 }
84
85 template< class IteratorT >
86 void LowStretchSphereParametrization::Optimize(IteratorT begin, IteratorT end,
87 float epsilon)
88 {
89 // tries to find a rotation of the parametrization that does not cut
90 // the points
91
92 // pull the center of the points onto the equator
93 float minUangle = float(M_PI), maxUangle = 0;
94 if(end - begin <= 1)
95 return;
96 for(IteratorT i = begin; i != end; ++i)
97 {
98 Vec3f s = *i - m_sphere->Center();
99 float slength = s.length();
100 float h = m_frame.Height(*((GfxTL::Vector3Df *)&s));
101 if(slength > 0) h /= slength;
102 h = GfxTL::Math< float >::Clamp(h, -1, 1);
103 float uangle = std::acos(h);
104 if(minUangle > uangle)
105 minUangle = uangle;
106 if(maxUangle < uangle)
107 maxUangle = uangle;
108 }
109 float centerUangle = (minUangle + maxUangle) / 2;
110 // centerUangle should be located at 90 degrees
111 GfxTL::Quaternion< float > q;
112 q.RotationRad(-2 * (centerUangle - float(M_PI / 2)), m_frame[0][0], m_frame[0][1], m_frame[0][2]);
113 GfxTL::Vector3Df newNormal;
114 q.Rotate(m_frame[2], &newNormal);
115 m_frame.FromNormal(newNormal);
116
117 // a rotation around the normal is searched
118 // for this find all the angles and then extract the largest gap
119 MiscLib::Vector< float > vangles;
120 for(IteratorT i = begin; i != end; ++i)
121 {
122 Vec3f s = *i - m_sphere->Center();
123 float slength = s.length();
124 GfxTL::Vector3Df l;
125 m_frame.ToLocal(*((GfxTL::Vector3Df *)&s), &l);
126 if(slength > 0) l[2] /= slength;
127 l[2] = GfxTL::Math< float >::Clamp(l[2], -1, 1);
128 float uangle = std::acos(l[2]);
129 if(uangle * m_sphere->Radius() < float(M_PI) * m_sphere->Radius() - 2 * epsilon
130 && uangle * m_sphere->Radius() > float(-M_PI) * m_sphere->Radius() + 2 * epsilon)
131 vangles.push_back(std::atan2(l[1], l[0]));
132 }
133 if (vangles.size() < 2)
134 {
135 return;
136 }
137 std::sort(vangles.begin(), vangles.end());
138 // try to find a large gap
139 float maxGap = vangles.front() + 2 * float(M_PI) - vangles.back();
140 float lower = vangles.back(), upper = vangles.front() + 2 * float(M_PI);
141 for(size_t i = 1; i < vangles.size(); ++i)
142 {
143 float gap = vangles[i] - vangles[i - 1];
144 if(gap > maxGap)
145 {
146 maxGap = gap;
147 lower = vangles[i - 1];
148 upper = vangles[i];
149 }
150 }
151 // reparameterize by rotating the frame on the normal such that x direction
152 // coincides with the gap
153 float rotationAngle = (lower + upper) / 2;
154 m_frame.RotateOnNormal(rotationAngle + float(M_PI));
155 }
156
157 #endif