Point Cloud Library (PCL)  1.14.1-dev
ppolynomial.hpp
1 /*
2 Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without modification,
6 are permitted provided that the following conditions are met:
7 
8 Redistributions of source code must retain the above copyright notice, this list of
9 conditions and the following disclaimer. Redistributions in binary form must reproduce
10 the above copyright notice, this list of conditions and the following disclaimer
11 in the documentation and/or other materials provided with the distribution.
12 
13 Neither the name of the Johns Hopkins University nor the names of its contributors
14 may be used to endorse or promote products derived from this software without specific
15 prior written permission.
16 
17 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
18 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
19 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
20 SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
21 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23 BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
26 DAMAGE.
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 
39 namespace 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  }
68 
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  }
98 
99  template<int Degree>
101  if(polyCount){free(polys);}
102  polyCount=0;
103  polys=NULL;
104  }
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;}
114  }
115  reset( c );
116  }
117  template <int Degree>
118  int PPolynomial<Degree>::size(void) const{return int(sizeof(StartingPolynomial<Degree>)*polyCount);}
119 
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 >
392  {
394  }
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)
void printnl(void) const
PPolynomial< Degree-1 > derivative(void) const
PPolynomial< Degree+1 > MovingAverage(double radius)
double Integral(void) const
Polynomial shift(double t) const
Definition: polynomial.hpp:253
void getSolutions(double c, std::vector< double > &roots, double EPS) const
Definition: polynomial.hpp:275
void printnl(void) const
Definition: polynomial.hpp:267
double integral(double tMin, double tMax) const
Definition: polynomial.hpp:90
StartingPolynomial shift(double t) const
Definition: ppolynomial.hpp:62
Polynomial< Degree > p
Definition: ppolynomial.h:46
StartingPolynomial< Degree+Degree2 > operator*(const StartingPolynomial< Degree2 > &p) const
Definition: ppolynomial.hpp:47
StartingPolynomial scale(double s) const
Definition: ppolynomial.hpp:55
int operator<(const StartingPolynomial &sp) const
Definition: ppolynomial.hpp:71
static int Compare(const void *v1, const void *v2)
Definition: ppolynomial.hpp:76