Debian Codegraph @

plugins/core/Standard/qRANSAC_SD/extern/RANSAC_SD/Plane.cpp

raw
1 #include "Plane.h"
2 //#include "pca.h"
3 #include <MiscLib/Performance.h>
4 #include "LevMarFitting.h"
5 #include <GfxTL/VectorXD.h>
6 #include <GfxTL/IndexedIterator.h>
7 #include <GfxTL/Plane.h>
8 #include <GfxTL/Mean.h>
9
10 Plane::Plane(Vec3f p1, Vec3f p2, Vec3f p3)
11 {
12 m_normal= (p2-p1).cross(p3-p2);
13 m_normal.normalize();
14 m_pos = p1;
15 m_dist = m_pos.dot(m_normal);
16 }
17
18 Plane::Plane(Vec3f p1, Vec3f normal)
19 {
20 m_normal = normal;
21 m_pos = p1;
22 m_dist = m_pos.dot(m_normal);
23 }
24
25 Plane::~Plane()
26 {
27 }
28
29 bool Plane::Init(Vec3f p1, Vec3f p2, Vec3f p3)
30 {
31 m_normal= (p2-p1).cross(p3-p2);
32 if(m_normal.sqrLength() < 1E-6f)
33 return false;
34 m_normal.normalize();
35 m_pos = p1;
36 m_dist = m_pos.dot(m_normal);
37 return true;
38 }
39
40 bool Plane::Init(const MiscLib::Vector< Vec3f > &samples)
41 {
42 if(samples.size() < 6)
43 return false;
44 return Init(samples[0], samples[1], samples[2]);
45 }
46
47 bool Plane::InitAverage(const MiscLib::Vector< Vec3f > &samples)
48 {
49 if(samples.size() < 1)
50 return false;
51 m_normal = Vec3f(0, 0, 0);
52 m_pos = Vec3f(0, 0, 0);
53 size_t c = samples.size() / 2;
54 MiscLib::Vector< GfxTL::Vector3Df > normals(c);
55 for(size_t i = 0; i < c; ++i)
56 normals[i] = GfxTL::Vector3Df(samples[i + c]);
57 GfxTL::Vector3Df meanNormal;
58 GfxTL::MeanOfNormals(normals.begin(), normals.end(), &meanNormal);
59 m_normal = Vec3f(meanNormal.Data());
60 GfxTL::Vector3Df mean;
61 GfxTL::Mean(samples.begin(), samples.begin() + c, &mean);
62 m_pos = Vec3f(mean.Data());
63 m_dist = m_pos.dot(m_normal);
64 return true;
65 }
66
67 bool Plane::Init(bool binary, std::istream *i)
68 {
69 if(binary)
70 {
71 i->read((char *)&m_normal, sizeof(m_normal));
72 i->read((char *)&m_dist, sizeof(m_dist));
73 i->read((char *)&m_pos, sizeof(m_pos));
74 }
75 else
76 {
77 for(size_t j = 0; j < 3; ++j)
78 (*i) >> m_normal[j];
79 (*i) >> m_dist;
80 for(size_t j = 0; j < 3; ++j)
81 (*i) >> m_pos[j];
82 }
83 return true;
84 }
85
86 void Plane::Init(float* array){
87 for(int i=0;i<3;i++){
88 m_normal[i] = array[i];
89 m_pos[i] = array[i+4];
90 }
91 m_dist=array[3];
92 }
93
94 void Plane::Init(FILE *i)
95 {
96 size_t readrtn; //unused return warning suppresion
97 readrtn = fread(&m_normal, sizeof(m_normal), 1, i);
98 readrtn = fread(&m_dist, sizeof(m_dist), 1, i);
99 readrtn = fread(&m_pos, sizeof(m_pos), 1, i);
100 }
101
102 bool Plane::equals (Plane other)
103 {
104 return ( (other.getNormal().dot (getNormal() ) > 0.90) &&
105 (getDistance (other.getPosition()) < 0.2) );
106 }
107
108 template< class WeightT >
109 class LevMarPlane
110 : public WeightT
111 {
112 public:
113 enum { NumParams = 4 };
114 typedef float ScalarType;
115
116 LevMarPlane()
117 {}
118 // parametrization:
119 // params[0] - params[2] = normal vector
120 // params[3] = dist to origin
121
122 template< class IteratorT >
123 ScalarType Chi(const ScalarType *params, IteratorT begin, IteratorT end,
124 ScalarType *values, ScalarType *temp) const
125 {
126 ScalarType chi = 0;
127 int size = end - begin;
128 #ifdef DOPARALLEL
129 #pragma omp parallel for schedule(static) reduction(+:chi)
130 #endif
131 for(int idx = 0; idx < size; ++idx)
132 {
133 temp[idx] = params[0] * begin[idx][0] + params[1] * begin[idx][1]
134 + params[2] * begin[idx][2] - params[3];
135 chi += (values[idx] = WeightT::Weigh(temp[idx]))
136 * values[idx];
137 }
138 return chi;
139 }
140
141 template< class IteratorT >
142 void Derivatives(const ScalarType *params, IteratorT begin, IteratorT end,
143 const ScalarType *values, const ScalarType *temp, ScalarType *matrix) const
144 {
145 int size = end - begin;
146 #ifdef DOPARALLEL
147 #pragma omp parallel for schedule(static)
148 #endif
149 for(int idx = 0; idx < size; ++idx)
150 {
151 matrix[idx * NumParams + 0] = begin[idx][0];
152 matrix[idx * NumParams + 1] = begin[idx][1];
153 matrix[idx * NumParams + 2] = begin[idx][2];
154 matrix[idx * NumParams + 3] = -1;
155 WeightT::template DerivWeigh< NumParams >(temp[idx],
156 matrix + idx * NumParams);
157 }
158 }
159
160 void Normalize(ScalarType *params) const
161 {
162 ScalarType len = std::sqrt(params[0] * params[0]
163 + params[1] * params[1] + params[2] * params[2]);
164 params[0] /= len;
165 params[1] /= len;
166 params[2] /= len;
167 }
168 };
169
170 bool Plane::LeastSquaresFit(const PointCloud &pc,
171 MiscLib::Vector< size_t >::const_iterator begin,
172 MiscLib::Vector< size_t >::const_iterator end)
173 {
174 LeastSquaresFit(GfxTL::IndexIterate(begin, pc.begin()),
175 GfxTL::IndexIterate(end, pc.begin()));
176 return true;
177 }
178
179 bool Plane::Interpolate(const MiscLib::Vector< Plane > &planes,
180 const MiscLib::Vector< float > &weights, Plane *ip)
181 {
182 Vec3f normal(0, 0, 0);
183 Vec3f position(0, 0, 0);
184 for(size_t i = 0; i < planes.size(); ++i)
185 {
186 normal += weights[i] * planes[i].getNormal();
187 position += weights[i] * planes[i].getPosition();
188 }
189 normal.normalize();
190 *ip = Plane(position, normal);
191 return true;
192 }
193
194 void Plane::Serialize(bool binary, std::ostream *o) const
195 {
196 if(binary)
197 {
198 o->write((const char *)&m_normal, sizeof(m_normal));
199 o->write((const char *)&m_dist, sizeof(m_dist));
200 o->write((const char *)&m_pos, sizeof(m_pos));
201 }
202 else
203 #ifdef DIRK_FORMAT
204 {
205 (*o) << std::endl
206 << "n " << m_normal[0] << " " << m_normal[1] << " " << m_normal[2] << std::endl
207 << "d " << m_dist << std::endl
208 << "p ";
209 for(size_t i = 0; i < 3; ++i)
210 (*o) << m_pos[i] << " ";
211 (*o) << std::endl;
212 }
213 #else
214 {
215 (*o) << m_normal[0] << " " << m_normal[1] << " " << m_normal[2] << " "
216 << m_dist << " ";
217 for(size_t i = 0; i < 3; ++i)
218 (*o) << m_pos[i] << " ";
219 }
220 #endif
221 }
222
223 size_t Plane::SerializedSize()
224 {
225 return sizeof(Vec3f)
226 + sizeof(float)
227 + sizeof(Vec3f);
228 }
229
230 size_t Plane::SerializedFloatSize()
231 {
232 return 7;
233 }
234
235 void Plane::Serialize(float* array) const
236 {
237 for(int i=0;i<3;i++){
238 array[i] = m_normal[i];
239 array[i+4] = m_pos[i];
240 }
241 array[3] = m_dist;
242 }
243
244
245 void Plane::Serialize(FILE *o) const
246 {
247 fwrite(&m_normal, sizeof(m_normal), 1, o);
248 fwrite(&m_dist, sizeof(m_dist), 1, o);
249 fwrite(&m_pos, sizeof(m_pos), 1, o);
250 }
251
252 void Plane::Transform(float scale, const Vec3f &translate)
253 {
254 m_pos *= scale;
255 m_pos += translate;
256 m_dist = m_pos.dot(m_normal);
257 }
258
259 float Plane::Intersect(const Vec3f &p, const Vec3f &r) const
260 {
261 return -SignedDistance(p) / (m_normal.dot(r));
262 }