Point Cloud Library (PCL)  1.9.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 ////////////////////////
32 // StartingPolynomial //
33 ////////////////////////
34 
35 namespace pcl
36 {
37  namespace poisson
38  {
39 
40 
41  template<int Degree>
42  template<int Degree2>
45  if(start>p.start){sp.start=start;}
46  else{sp.start=p.start;}
47  sp.p=this->p*p.p;
48  return sp;
49  }
50  template<int Degree>
53  q.start=start*s;
54  q.p=p.scale(s);
55  return q;
56  }
57  template<int Degree>
60  q.start=start+s;
61  q.p=p.shift(s);
62  return q;
63  }
64 
65 
66  template<int Degree>
68  if(start<sp.start){return 1;}
69  else{return 0;}
70  }
71  template<int Degree>
72  int StartingPolynomial<Degree>::Compare(const void* v1,const void* v2){
73  double d=((StartingPolynomial*)(v1))->start-((StartingPolynomial*)(v2))->start;
74  if (d<0) {return -1;}
75  else if (d>0) {return 1;}
76  else {return 0;}
77  }
78 
79  /////////////////
80  // PPolynomial //
81  /////////////////
82  template<int Degree>
84  polyCount=0;
85  polys=NULL;
86  }
87  template<int Degree>
89  polyCount=0;
90  polys=NULL;
91  set(p.polyCount);
92  memcpy(polys,p.polys,sizeof(StartingPolynomial<Degree>)*p.polyCount);
93  }
94 
95  template<int Degree>
97  if(polyCount){free(polys);}
98  polyCount=0;
99  polys=NULL;
100  }
101  template<int Degree>
103  int i,c=0;
104  set(count);
106  for( i=0 ; i<count ; i++ )
107  {
108  if( !c || sps[i].start!=polys[c-1].start ) polys[c++] = sps[i];
109  else{polys[c-1].p+=sps[i].p;}
110  }
111  reset( c );
112  }
113  template <int Degree>
114  int PPolynomial<Degree>::size(void) const{return int(sizeof(StartingPolynomial<Degree>)*polyCount);}
115 
116  template<int Degree>
117  void PPolynomial<Degree>::set( std::size_t size )
118  {
119  if(polyCount){free(polys);}
120  polyCount=0;
121  polys=NULL;
122  polyCount=size;
123  if(size){
124  polys=(StartingPolynomial<Degree>*)malloc(sizeof(StartingPolynomial<Degree>)*size);
125  memset(polys,0,sizeof(StartingPolynomial<Degree>)*size);
126  }
127  }
128  template<int Degree>
129  void PPolynomial<Degree>::reset( std::size_t newSize )
130  {
131  polyCount=newSize;
132  polys=(StartingPolynomial<Degree>*)realloc(polys,sizeof(StartingPolynomial<Degree>)*newSize);
133  }
134 
135  template<int Degree>
137  set(p.polyCount);
138  memcpy(polys,p.polys,sizeof(StartingPolynomial<Degree>)*p.polyCount);
139  return *this;
140  }
141 
142  template<int Degree>
143  template<int Degree2>
145  set(p.polyCount);
146  for(int i=0;i<int(polyCount);i++){
147  polys[i].start=p.polys[i].start;
148  polys[i].p=p.polys[i].p;
149  }
150  return *this;
151  }
152 
153  template<int Degree>
154  double PPolynomial<Degree>::operator ()( double t ) const
155  {
156  double v=0;
157  for( int i=0 ; i<int(polyCount) && t>polys[i].start ; i++ ) v+=polys[i].p(t);
158  return v;
159  }
160 
161  template<int Degree>
162  double PPolynomial<Degree>::integral( double tMin , double tMax ) const
163  {
164  int m=1;
165  double start,end,s,v=0;
166  start=tMin;
167  end=tMax;
168  if(tMin>tMax){
169  m=-1;
170  start=tMax;
171  end=tMin;
172  }
173  for(int i=0;i<int(polyCount) && polys[i].start<end;i++){
174  if(start<polys[i].start){s=polys[i].start;}
175  else{s=start;}
176  v+=polys[i].p.integral(s,end);
177  }
178  return v*m;
179  }
180  template<int Degree>
181  double PPolynomial<Degree>::Integral(void) const{return integral(polys[0].start,polys[polyCount-1].start);}
182  template<int Degree>
184  PPolynomial q;
185  int i,j;
186  std::size_t idx=0;
187  q.set(polyCount+p.polyCount);
188  i=j=-1;
189 
190  while(idx<q.polyCount){
191  if (j>=int(p.polyCount)-1) {q.polys[idx]= polys[++i];}
192  else if (i>=int( polyCount)-1) {q.polys[idx]=p.polys[++j];}
193  else if(polys[i+1].start<p.polys[j+1].start){q.polys[idx]= polys[++i];}
194  else {q.polys[idx]=p.polys[++j];}
195  idx++;
196  }
197  return q;
198  }
199  template<int Degree>
201  PPolynomial q;
202  int i,j;
203  std::size_t idx=0;
204  q.set(polyCount+p.polyCount);
205  i=j=-1;
206 
207  while(idx<q.polyCount){
208  if (j>=int(p.polyCount)-1) {q.polys[idx]= polys[++i];}
209  else if (i>=int( polyCount)-1) {q.polys[idx].start=p.polys[++j].start;q.polys[idx].p=p.polys[j].p*(-1.0);}
210  else if(polys[i+1].start<p.polys[j+1].start){q.polys[idx]= polys[++i];}
211  else {q.polys[idx].start=p.polys[++j].start;q.polys[idx].p=p.polys[j].p*(-1.0);}
212  idx++;
213  }
214  return q;
215  }
216  template<int Degree>
218  int i,j;
219  StartingPolynomial<Degree>* oldPolys=polys;
220  std::size_t idx=0,cnt=0,oldPolyCount=polyCount;
221  polyCount=0;
222  polys=NULL;
223  set(oldPolyCount+p.polyCount);
224  i=j=-1;
225  while(cnt<polyCount){
226  if (j>=int( p.polyCount)-1) {polys[idx]=oldPolys[++i];}
227  else if (i>=int(oldPolyCount)-1) {polys[idx].start= p.polys[++j].start;polys[idx].p=p.polys[j].p*scale;}
228  else if (oldPolys[i+1].start<p.polys[j+1].start){polys[idx]=oldPolys[++i];}
229  else {polys[idx].start= p.polys[++j].start;polys[idx].p=p.polys[j].p*scale;}
230  if(idx && polys[idx].start==polys[idx-1].start) {polys[idx-1].p+=polys[idx].p;}
231  else{idx++;}
232  cnt++;
233  }
234  free(oldPolys);
235  reset(idx);
236  return *this;
237  }
238  template<int Degree>
239  template<int Degree2>
243  int i,j,spCount=int(polyCount*p.polyCount);
244 
246  for(i=0;i<int(polyCount);i++){
247  for(j=0;j<int(p.polyCount);j++){
248  sp[i*p.polyCount+j]=polys[i]*p.polys[j];
249  }
250  }
251  q.set(sp,spCount);
252  free(sp);
253  return q;
254  }
255  template<int Degree>
256  template<int Degree2>
259  q.set(polyCount);
260  for(int i=0;i<int(polyCount);i++){
261  q.polys[i].start=polys[i].start;
262  q.polys[i].p=polys[i].p*p;
263  }
264  return q;
265  }
266  template<int Degree>
268  {
269  PPolynomial q;
270  q.set(polyCount);
271  for(std::size_t i=0;i<polyCount;i++){q.polys[i]=polys[i].scale(s);}
272  return q;
273  }
274  template<int Degree>
276  {
277  PPolynomial q;
278  q.set(polyCount);
279  for(std::size_t i=0;i<polyCount;i++){q.polys[i]=polys[i].shift(s);}
280  return q;
281  }
282  template<int Degree>
284  PPolynomial<Degree-1> q;
285  q.set(polyCount);
286  for(std::size_t i=0;i<polyCount;i++){
287  q.polys[i].start=polys[i].start;
288  q.polys[i].p=polys[i].p.derivative();
289  }
290  return q;
291  }
292  template<int Degree>
294  int i;
296  q.set(polyCount);
297  for(i=0;i<int(polyCount);i++){
298  q.polys[i].start=polys[i].start;
299  q.polys[i].p=polys[i].p.integral();
300  q.polys[i].p-=q.polys[i].p(q.polys[i].start);
301  }
302  return q;
303  }
304  template<int Degree>
306  template<int Degree>
308  template<int Degree>
310  {
311  for(int i=0;i<int(polyCount);i++){polys[i].p*=s;}
312  return *this;
313  }
314  template<int Degree>
316  {
317  for(std::size_t i=0;i<polyCount;i++){polys[i].p/=s;}
318  return *this;
319  }
320  template<int Degree>
322  {
323  PPolynomial q=*this;
324  q+=s;
325  return q;
326  }
327  template<int Degree>
329  {
330  PPolynomial q=*this;
331  q-=s;
332  return q;
333  }
334  template<int Degree>
336  {
337  PPolynomial q=*this;
338  q*=s;
339  return q;
340  }
341  template<int Degree>
343  {
344  PPolynomial q=*this;
345  q/=s;
346  return q;
347  }
348 
349  template<int Degree>
352 
353  if(!polyCount){
355  printf("[-Infinity,Infinity]\n");
356  }
357  else{
358  for(std::size_t i=0;i<polyCount;i++){
359  printf("[");
360  if (polys[i ].start== DBL_MAX){printf("Infinity,");}
361  else if (polys[i ].start==-DBL_MAX){printf("-Infinity,");}
362  else {printf("%f,",polys[i].start);}
363  if(i+1==polyCount) {printf("Infinity]\t");}
364  else if (polys[i+1].start== DBL_MAX){printf("Infinity]\t");}
365  else if (polys[i+1].start==-DBL_MAX){printf("-Infinity]\t");}
366  else {printf("%f]\t",polys[i+1].start);}
367  p=p+polys[i].p;
368  p.printnl();
369  }
370  }
371  printf("\n");
372  }
373  template< >
375  {
376  PPolynomial q;
377  q.set(2);
378 
379  q.polys[0].start=-radius;
380  q.polys[1].start= radius;
381 
382  q.polys[0].p.coefficients[0]= 1.0;
383  q.polys[1].p.coefficients[0]=-1.0;
384  return q;
385  }
386  template< int Degree >
388  {
390  }
391  template<int Degree>
393  {
397 
398  sps=(StartingPolynomial<Degree+1>*)malloc(sizeof(StartingPolynomial<Degree+1>)*polyCount*2);
399 
400  for(int i=0;i<int(polyCount);i++){
401  sps[2*i ].start=polys[i].start-radius;
402  sps[2*i+1].start=polys[i].start+radius;
403  p=polys[i].p.integral()-polys[i].p.integral()(polys[i].start);
404  sps[2*i ].p=p.shift(-radius);
405  sps[2*i+1].p=p.shift( radius)*-1;
406  }
407  A.set(sps,int(polyCount*2));
408  free(sps);
409  return A*1.0/(2*radius);
410  }
411  template<int Degree>
412  void PPolynomial<Degree>::getSolutions(double c,std::vector<double>& roots,double EPS,double min,double max) const{
414  std::vector<double> tempRoots;
415 
416  p.setZero();
417  for(std::size_t i=0;i<polyCount;i++){
418  p+=polys[i].p;
419  if(polys[i].start>max){break;}
420  if(i<polyCount-1 && polys[i+1].start<min){continue;}
421  p.getSolutions(c,tempRoots,EPS);
422  for(std::size_t j=0;j<tempRoots.size();j++){
423  if(tempRoots[j]>polys[i].start && (i+1==polyCount || tempRoots[j]<=polys[i+1].start)){
424  if(tempRoots[j]>min && tempRoots[j]<max){roots.push_back(tempRoots[j]);}
425  }
426  }
427  }
428  }
429 
430  template<int Degree>
431  void PPolynomial<Degree>::write(FILE* fp,int samples,double min,double max) const{
432  fwrite(&samples,sizeof(int),1,fp);
433  for(int i=0;i<samples;i++){
434  double x=min+i*(max-min)/(samples-1);
435  float v=(*this)(x);
436  fwrite(&v,sizeof(float),1,fp);
437  }
438  }
439 
440  }
441 }
PPolynomial operator-(const PPolynomial &p) const
PPolynomial< Degree-1 > derivative(void) const
PPolynomial & operator*=(double s)
static PPolynomial BSpline(double radius=0.5)
StartingPolynomial shift(double t) const
Definition: ppolynomial.hpp:58
double Integral(void) const
double integral(double tMin, double tMax) const
Definition: polynomial.hpp:87
void printnl(void) const
Definition: polynomial.hpp:264
Polynomial< Degree > p
Definition: ppolynomial.h:46
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
void getSolutions(double c, std::vector< double > &roots, double EPS) const
Definition: polynomial.hpp:272
void set(std::size_t size)
PPolynomial & operator-=(double s)
PPolynomial< Degree+1 > MovingAverage(double radius)
PPolynomial & operator=(const PPolynomial &p)
void getSolutions(double c, std::vector< double > &roots, double EPS, double min=-DBL_MAX, double max=DBL_MAX) const
PPolynomial & operator+=(double s)
PPolynomial & operator/=(double s)
StartingPolynomial< Degree+Degree2 > operator*(const StartingPolynomial< Degree2 > &p) const
Definition: ppolynomial.hpp:43
PPolynomial operator/(double s) const
StartingPolynomial scale(double s) const
Definition: ppolynomial.hpp:51
PPolynomial< Degree+Degree2 > operator*(const Polynomial< Degree2 > &p) const
PPolynomial< Degree+1 > integral(void) const
PPolynomial & addScaled(const PPolynomial &poly, double scale)
void write(FILE *fp, int samples, double min, double max) const
double operator()(double t) const
StartingPolynomial< Degree > * polys
Definition: ppolynomial.h:62
void reset(std::size_t newSize)
PPolynomial scale(double s) const
void printnl(void) const
PPolynomial shift(double t) const
PPolynomial operator+(const PPolynomial &p) const
int operator<(const StartingPolynomial &sp) const
Definition: ppolynomial.hpp:67
Polynomial shift(double t) const
Definition: polynomial.hpp:250
static int Compare(const void *v1, const void *v2)
Definition: ppolynomial.hpp:72