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