Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
ppolynomial.hpp
1/*
2Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho
3All rights reserved.
4
5Redistribution and use in source and binary forms, with or without modification,
6are permitted provided that the following conditions are met:
7
8Redistributions of source code must retain the above copyright notice, this list of
9conditions and the following disclaimer. Redistributions in binary form must reproduce
10the above copyright notice, this list of conditions and the following disclaimer
11in the documentation and/or other materials provided with the distribution.
12
13Neither the name of the Johns Hopkins University nor the names of its contributors
14may be used to endorse or promote products derived from this software without specific
15prior written permission.
16
17THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
18EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
19OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
20SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
21INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
26DAMAGE.
27*/
28
29#include "factor.h"
30
31#include <cstdio>
32#include <cstdlib> // for malloc, needed by gcc-5
33#include <cstring>
34
35////////////////////////
36// StartingPolynomial //
37////////////////////////
38
39namespace pcl
40{
41 namespace poisson
42 {
43
44
45 template<int Degree>
46 template<int Degree2>
49 if(start>p.start){sp.start=start;}
50 else{sp.start=p.start;}
51 sp.p=this->p*p.p;
52 return sp;
53 }
54 template<int Degree>
57 q.start=start*s;
58 q.p=p.scale(s);
59 return q;
60 }
61 template<int Degree>
64 q.start=start+s;
65 q.p=p.shift(s);
66 return q;
67 }
69
70 template<int Degree>
72 if(start<sp.start){return 1;}
73 else{return 0;}
74 }
75 template<int Degree>
76 int StartingPolynomial<Degree>::Compare(const void* v1,const void* v2){
77 double d=((StartingPolynomial*)(v1))->start-((StartingPolynomial*)(v2))->start;
78 if (d<0) {return -1;}
79 else if (d>0) {return 1;}
80 else {return 0;}
81 }
82
83 /////////////////
84 // PPolynomial //
85 /////////////////
86 template<int Degree>
88 polyCount=0;
89 polys=NULL;
90 }
91 template<int Degree>
93 polyCount=0;
94 polys=NULL;
95 set(p.polyCount);
96 memcpy(polys,p.polys,sizeof(StartingPolynomial<Degree>)*p.polyCount);
97 }
99 template<int Degree>
101 if(polyCount){free(polys);}
102 polyCount=0;
103 polys=NULL;
105 template<int Degree>
107 int i,c=0;
108 set(count);
110 for( i=0 ; i<count ; i++ )
111 {
112 if( !c || sps[i].start!=polys[c-1].start ) polys[c++] = sps[i];
113 else{polys[c-1].p+=sps[i].p;}
115 reset( c );
117 template <int Degree>
118 int PPolynomial<Degree>::size(void) const{return int(sizeof(StartingPolynomial<Degree>)*polyCount);}
120 template<int Degree>
121 void PPolynomial<Degree>::set( std::size_t size )
122 {
123 if(polyCount){free(polys);}
124 polyCount=0;
125 polys=NULL;
126 polyCount=size;
127 if(size){
128 polys=(StartingPolynomial<Degree>*)malloc(sizeof(StartingPolynomial<Degree>)*size);
129 memset(polys,0,sizeof(StartingPolynomial<Degree>)*size);
130 }
131 }
132 template<int Degree>
133 void PPolynomial<Degree>::reset( std::size_t newSize )
134 {
135 polyCount=newSize;
136 polys=(StartingPolynomial<Degree>*)realloc(polys,sizeof(StartingPolynomial<Degree>)*newSize);
137 }
138
139 template<int Degree>
141 set(p.polyCount);
142 memcpy(polys,p.polys,sizeof(StartingPolynomial<Degree>)*p.polyCount);
143 return *this;
144 }
145
146 template<int Degree>
147 template<int Degree2>
149 set(p.polyCount);
150 for(int i=0;i<int(polyCount);i++){
151 polys[i].start=p.polys[i].start;
152 polys[i].p=p.polys[i].p;
153 }
154 return *this;
155 }
156
157 template<int Degree>
158 double PPolynomial<Degree>::operator ()( double t ) const
159 {
160 double v=0;
161 for( int i=0 ; i<int(polyCount) && t>polys[i].start ; i++ ) v+=polys[i].p(t);
162 return v;
163 }
164
165 template<int Degree>
166 double PPolynomial<Degree>::integral( double tMin , double tMax ) const
167 {
168 int m=1;
169 double start,end,s,v=0;
170 start=tMin;
171 end=tMax;
172 if(tMin>tMax){
173 m=-1;
174 start=tMax;
175 end=tMin;
176 }
177 for(int i=0;i<int(polyCount) && polys[i].start<end;i++){
178 if(start<polys[i].start){s=polys[i].start;}
179 else{s=start;}
180 v+=polys[i].p.integral(s,end);
181 }
182 return v*m;
183 }
184 template<int Degree>
185 double PPolynomial<Degree>::Integral(void) const{return integral(polys[0].start,polys[polyCount-1].start);}
186 template<int Degree>
188 PPolynomial q;
189 int i,j;
190 std::size_t idx=0;
191 q.set(polyCount+p.polyCount);
192 i=j=-1;
193
194 while(idx<q.polyCount){
195 if (j>=int(p.polyCount)-1) {q.polys[idx]= polys[++i];}
196 else if (i>=int( polyCount)-1) {q.polys[idx]=p.polys[++j];}
197 else if(polys[i+1].start<p.polys[j+1].start){q.polys[idx]= polys[++i];}
198 else {q.polys[idx]=p.polys[++j];}
199 idx++;
200 }
201 return q;
202 }
203 template<int Degree>
205 PPolynomial q;
206 int i,j;
207 std::size_t idx=0;
208 q.set(polyCount+p.polyCount);
209 i=j=-1;
210
211 while(idx<q.polyCount){
212 if (j>=int(p.polyCount)-1) {q.polys[idx]= polys[++i];}
213 else if (i>=int( polyCount)-1) {q.polys[idx].start=p.polys[++j].start;q.polys[idx].p=p.polys[j].p*(-1.0);}
214 else if(polys[i+1].start<p.polys[j+1].start){q.polys[idx]= polys[++i];}
215 else {q.polys[idx].start=p.polys[++j].start;q.polys[idx].p=p.polys[j].p*(-1.0);}
216 idx++;
217 }
218 return q;
219 }
220 template<int Degree>
222 int i,j;
223 StartingPolynomial<Degree>* oldPolys=polys;
224 std::size_t idx=0,cnt=0,oldPolyCount=polyCount;
225 polyCount=0;
226 polys=NULL;
227 set(oldPolyCount+p.polyCount);
228 i=j=-1;
229 while(cnt<polyCount){
230 if (j>=int( p.polyCount)-1) {polys[idx]=oldPolys[++i];}
231 else if (i>=int(oldPolyCount)-1) {polys[idx].start= p.polys[++j].start;polys[idx].p=p.polys[j].p*scale;}
232 else if (oldPolys[i+1].start<p.polys[j+1].start){polys[idx]=oldPolys[++i];}
233 else {polys[idx].start= p.polys[++j].start;polys[idx].p=p.polys[j].p*scale;}
234 if(idx && polys[idx].start==polys[idx-1].start) {polys[idx-1].p+=polys[idx].p;}
235 else{idx++;}
236 cnt++;
237 }
238 free(oldPolys);
239 reset(idx);
240 return *this;
241 }
242 template<int Degree>
243 template<int Degree2>
247 int i,j,spCount=int(polyCount*p.polyCount);
248
250 for(i=0;i<int(polyCount);i++){
251 for(j=0;j<int(p.polyCount);j++){
252 sp[i*p.polyCount+j]=polys[i]*p.polys[j];
253 }
254 }
255 q.set(sp,spCount);
256 free(sp);
257 return q;
258 }
259 template<int Degree>
260 template<int Degree2>
263 q.set(polyCount);
264 for(int i=0;i<int(polyCount);i++){
265 q.polys[i].start=polys[i].start;
266 q.polys[i].p=polys[i].p*p;
267 }
268 return q;
269 }
270 template<int Degree>
272 {
273 PPolynomial q;
274 q.set(polyCount);
275 for(std::size_t i=0;i<polyCount;i++){q.polys[i]=polys[i].scale(s);}
276 return q;
277 }
278 template<int Degree>
280 {
281 PPolynomial q;
282 q.set(polyCount);
283 for(std::size_t i=0;i<polyCount;i++){q.polys[i]=polys[i].shift(s);}
284 return q;
285 }
286 template<int Degree>
288 PPolynomial<Degree-1> q;
289 q.set(polyCount);
290 for(std::size_t i=0;i<polyCount;i++){
291 q.polys[i].start=polys[i].start;
292 q.polys[i].p=polys[i].p.derivative();
293 }
294 return q;
295 }
296 template<int Degree>
298 int i;
300 q.set(polyCount);
301 for(i=0;i<int(polyCount);i++){
302 q.polys[i].start=polys[i].start;
303 q.polys[i].p=polys[i].p.integral();
304 q.polys[i].p-=q.polys[i].p(q.polys[i].start);
305 }
306 return q;
307 }
308 template<int Degree>
310 template<int Degree>
312 template<int Degree>
314 {
315 for(int i=0;i<int(polyCount);i++){polys[i].p*=s;}
316 return *this;
317 }
318 template<int Degree>
320 {
321 for(std::size_t i=0;i<polyCount;i++){polys[i].p/=s;}
322 return *this;
323 }
324 template<int Degree>
326 {
327 PPolynomial q=*this;
328 q+=s;
329 return q;
330 }
331 template<int Degree>
333 {
334 PPolynomial q=*this;
335 q-=s;
336 return q;
337 }
338 template<int Degree>
340 {
341 PPolynomial q=*this;
342 q*=s;
343 return q;
344 }
345 template<int Degree>
347 {
348 PPolynomial q=*this;
349 q/=s;
350 return q;
351 }
352
353 template<int Degree>
356
357 if(!polyCount){
359 printf("[-Infinity,Infinity]\n");
360 }
361 else{
362 for(std::size_t i=0;i<polyCount;i++){
363 printf("[");
364 if (polys[i ].start== DBL_MAX){printf("Infinity,");}
365 else if (polys[i ].start==-DBL_MAX){printf("-Infinity,");}
366 else {printf("%f,",polys[i].start);}
367 if(i+1==polyCount) {printf("Infinity]\t");}
368 else if (polys[i+1].start== DBL_MAX){printf("Infinity]\t");}
369 else if (polys[i+1].start==-DBL_MAX){printf("-Infinity]\t");}
370 else {printf("%f]\t",polys[i+1].start);}
371 p=p+polys[i].p;
372 p.printnl();
373 }
374 }
375 printf("\n");
376 }
377 template< > inline
379 {
380 PPolynomial q;
381 q.set(2);
382
383 q.polys[0].start=-radius;
384 q.polys[1].start= radius;
385
386 q.polys[0].p.coefficients[0]= 1.0;
387 q.polys[1].p.coefficients[0]=-1.0;
388 return q;
389 }
390 template< int Degree >
395 template<int Degree>
397 {
401
402 sps=(StartingPolynomial<Degree+1>*)malloc(sizeof(StartingPolynomial<Degree+1>)*polyCount*2);
403
404 for(int i=0;i<int(polyCount);i++){
405 sps[2*i ].start=polys[i].start-radius;
406 sps[2*i+1].start=polys[i].start+radius;
407 p=polys[i].p.integral()-polys[i].p.integral()(polys[i].start);
408 sps[2*i ].p=p.shift(-radius);
409 sps[2*i+1].p=p.shift( radius)*-1;
410 }
411 A.set(sps,int(polyCount*2));
412 free(sps);
413 return A*1.0/(2*radius);
414 }
415 template<int Degree>
416 void PPolynomial<Degree>::getSolutions(double c,std::vector<double>& roots,double EPS,double min,double max) const{
418 std::vector<double> tempRoots;
419
420 p.setZero();
421 for(std::size_t i=0;i<polyCount;i++){
422 p+=polys[i].p;
423 if(polys[i].start>max){break;}
424 if(i<polyCount-1 && polys[i+1].start<min){continue;}
425 p.getSolutions(c,tempRoots,EPS);
426 for(std::size_t j=0;j<tempRoots.size();j++){
427 if(tempRoots[j]>polys[i].start && (i+1==polyCount || tempRoots[j]<=polys[i+1].start)){
428 if(tempRoots[j]>min && tempRoots[j]<max){roots.push_back(tempRoots[j]);}
429 }
430 }
431 }
432 }
433
434 template<int Degree>
435 void PPolynomial<Degree>::write(FILE* fp,int samples,double min,double max) const{
436 fwrite(&samples,sizeof(int),1,fp);
437 for(int i=0;i<samples;i++){
438 double x=min+i*(max-min)/(samples-1);
439 float v=(*this)(x);
440 fwrite(&v,sizeof(float),1,fp);
441 }
442 }
443
444 }
445}
PPolynomial operator-(const PPolynomial &p) const
PPolynomial operator/(double s) const
StartingPolynomial< Degree > * polys
Definition ppolynomial.h:62
PPolynomial & operator-=(double s)
PPolynomial shift(double t) const
void getSolutions(double c, std::vector< double > &roots, double EPS, double min=-DBL_MAX, double max=DBL_MAX) const
PPolynomial & operator=(const PPolynomial &p)
double operator()(double t) const
PPolynomial & operator+=(double s)
PPolynomial & operator/=(double s)
PPolynomial & operator*=(double s)
void write(FILE *fp, int samples, double min, double max) const
void reset(std::size_t newSize)
PPolynomial operator+(const PPolynomial &p) const
PPolynomial scale(double s) const
PPolynomial< Degree+1 > integral(void) const
static PPolynomial BSpline(double radius=0.5)
PPolynomial< Degree+Degree2 > operator*(const Polynomial< Degree2 > &p) const
void set(std::size_t size)
PPolynomial & addScaled(const PPolynomial &poly, double scale)
PPolynomial< Degree-1 > derivative(void) const
PPolynomial< Degree+1 > MovingAverage(double radius)
double Integral(void) const
Polynomial shift(double t) const
void getSolutions(double c, std::vector< double > &roots, double EPS) const
void printnl(void) const
double integral(double tMin, double tMax) const
StartingPolynomial shift(double t) const
StartingPolynomial< Degree+Degree2 > operator*(const StartingPolynomial< Degree2 > &p) const
StartingPolynomial scale(double s) const
int operator<(const StartingPolynomial &sp) const
static int Compare(const void *v1, const void *v2)