00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042 #include <pcl/surface/poisson/factor.h>
00043
00044 namespace pcl
00045 {
00046 namespace poisson
00047 {
00048
00050
00052 template <int Degree> template <int Degree2> StartingPolynomial<Degree+Degree2>
00053 StartingPolynomial<Degree>::operator * (const StartingPolynomial<Degree2> &p) const
00054 {
00055 StartingPolynomial<Degree+Degree2> sp;
00056 if (start>p.start)
00057 {
00058 sp.start=start;
00059 }
00060 else
00061 {
00062 sp.start=p.start;
00063 }
00064 sp.p=this->p*p.p;
00065 return sp;
00066 }
00067 template <int Degree> StartingPolynomial<Degree>
00068 StartingPolynomial<Degree>::scale (const double &s) const
00069 {
00070 StartingPolynomial q;
00071 q.start=start*s;
00072 q.p=p.scale (s);
00073 return q;
00074 }
00075 template <int Degree> StartingPolynomial<Degree>
00076 StartingPolynomial<Degree>::shift (const double &s) const
00077 {
00078 StartingPolynomial q;
00079 q.start=start+s;
00080 q.p=p.shift (s);
00081 return q;
00082 }
00083
00084
00085 template <int Degree> int
00086 StartingPolynomial<Degree>::operator < (const StartingPolynomial<Degree> &sp) const
00087 {
00088 if (start<sp.start)
00089 {
00090 return 1;
00091 }
00092 else
00093 {
00094 return 0;
00095 }
00096 }
00097 template <int Degree> int
00098 StartingPolynomial<Degree>::Compare (const void *v1,const void *v2)
00099 {
00100 double d = (reinterpret_cast<const StartingPolynomial*>(v1))->start-(reinterpret_cast<const StartingPolynomial*>(v2))->start;
00101 if (d<0)
00102 return (-1);
00103 else if (d>0)
00104 return (1);
00105 else
00106 return (0);
00107 }
00108
00110
00112 template <int Degree>
00113 PPolynomial<Degree>::PPolynomial (void) : polyCount (0), polys (NULL)
00114 {
00115 }
00116
00117 template <int Degree>
00118 PPolynomial<Degree>::PPolynomial (const PPolynomial<Degree> &p) : polyCount (0), polys (NULL)
00119 {
00120 set (p.polyCount);
00121 memcpy (polys,p.polys,sizeof(StartingPolynomial<Degree> )*p.polyCount);
00122 }
00123
00124 template <int Degree>
00125 PPolynomial<Degree>::~PPolynomial (void)
00126 {
00127 if (polyCount)
00128 {
00129 free (polys);
00130 }
00131 polyCount=0;
00132 polys=NULL;
00133 }
00134 template <int Degree> void
00135 PPolynomial<Degree>::set (StartingPolynomial<Degree> *sps,const int &count)
00136 {
00137 int i,c=0;
00138 set (count);
00139 qsort (sps,count,sizeof(StartingPolynomial<Degree> ),StartingPolynomial<Degree>::Compare);
00140 for (i=0; i<count; i++)
00141 {
00142 if (!c || sps[i].start!=polys[c-1].start)
00143 {
00144 polys[c++]=sps[i];
00145 }
00146 else
00147 {
00148 polys[c-1].p+=sps[i].p;
00149 }
00150 }
00151 reset (c);
00152 }
00153 template <int Degree> int
00154 PPolynomial<Degree>::size (void) const {return int(sizeof(StartingPolynomial<Degree> )*polyCount); }
00155
00156 template <int Degree> void
00157 PPolynomial<Degree>::set (const size_t &size)
00158 {
00159 if (polyCount)
00160 {
00161 free (polys);
00162 }
00163 polyCount=0;
00164 polys=NULL;
00165 polyCount=size;
00166 if (size)
00167 {
00168 polys = reinterpret_cast<StartingPolynomial<Degree>*> (malloc (sizeof (StartingPolynomial<Degree>) * size));
00169 memset (polys, 0, sizeof (StartingPolynomial<Degree>) * size);
00170 }
00171 }
00172
00173 template <int Degree> void
00174 PPolynomial<Degree>::reset (const size_t &newSize)
00175 {
00176 polyCount = newSize;
00177 polys = reinterpret_cast<StartingPolynomial<Degree>*> (realloc (polys, sizeof (StartingPolynomial<Degree>) * newSize));
00178 }
00179
00180 template <int Degree> PPolynomial<Degree>&
00181 PPolynomial<Degree>::operator = (const PPolynomial<Degree> &p)
00182 {
00183 set (p.polyCount);
00184 memcpy (polys,p.polys,sizeof(StartingPolynomial<Degree> )*p.polyCount);
00185 return *this;
00186 }
00187
00188 template <int Degree> template <int Degree2> PPolynomial<Degree>&
00189 PPolynomial<Degree>::operator = (const PPolynomial<Degree2> &p)
00190 {
00191 set (p.polyCount);
00192 for (int i=0; i<int(polyCount); i++)
00193 {
00194 polys[i].start=p.polys[i].start;
00195 polys[i].p=p.polys[i].p;
00196 }
00197 return *this;
00198 }
00199
00200 template <int Degree> double
00201 PPolynomial<Degree>::operator () (const double &t) const
00202 {
00203 double v=0;
00204 for (int i=0; i<int(polyCount) && t> polys[i].start; i++)
00205 {
00206 v+=polys[i].p (t);
00207 }
00208 return v;
00209 }
00210
00211 template <int Degree> double
00212 PPolynomial<Degree>::integral (const double &tMin,const double &tMax) const
00213 {
00214 int m=1;
00215 double start,end,s,v=0;
00216 start=tMin;
00217 end=tMax;
00218 if (tMin>tMax)
00219 {
00220 m=- 1;
00221 start=tMax;
00222 end=tMin;
00223 }
00224 for (int i=0; i<int(polyCount) && polys[i].start<end; i++)
00225 {
00226 if (start<polys[i].start)
00227 {
00228 s=polys[i].start;
00229 }
00230 else
00231 {
00232 s=start;
00233 }
00234 v+=polys[i].p.integral (s,end);
00235 }
00236 return v*m;
00237 }
00238 template <int Degree> double
00239 PPolynomial<Degree>::Integral (void) const {return integral (polys[0].start,polys[polyCount-1].start); }
00240 template <int Degree> PPolynomial<Degree>
00241 PPolynomial<Degree>::operator + (const PPolynomial<Degree> &p) const
00242 {
00243 PPolynomial q;
00244 int i,j;
00245 size_t idx=0;
00246 q.set (polyCount+p.polyCount);
00247 i=j=- 1;
00248
00249 while (idx<q.polyCount)
00250 {
00251 if (j>=int(p.polyCount)-1)
00252 {
00253 q.polys[idx]= polys[++i];
00254 }
00255 else if (i>=int(polyCount)-1)
00256 {
00257 q.polys[idx]=p.polys[++j];
00258 }
00259 else if (polys[i+1].start<p.polys[j+1].start)
00260 {
00261 q.polys[idx]= polys[++i];
00262 }
00263 else
00264 {
00265 q.polys[idx]=p.polys[++j];
00266 }
00267
00268
00269 idx++;
00270 }
00271 return q;
00272 }
00273 template <int Degree> PPolynomial<Degree>
00274 PPolynomial<Degree>::operator - (const PPolynomial<Degree> &p) const
00275 {
00276 PPolynomial q;
00277 int i,j;
00278 size_t idx=0;
00279 q.set (polyCount+p.polyCount);
00280 i=j=- 1;
00281
00282 while (idx<q.polyCount)
00283 {
00284 if (j>=int(p.polyCount)-1)
00285 {
00286 q.polys[idx]= polys[++i];
00287 }
00288 else if (i>=int(polyCount)-1)
00289 {
00290 q.polys[idx].start=p.polys[++j].start; q.polys[idx].p=p.polys[j].p*(- 1.0);
00291 }
00292 else if (polys[i+1].start<p.polys[j+1].start)
00293 {
00294 q.polys[idx]= polys[++i];
00295 }
00296 else
00297 {
00298 q.polys[idx].start=p.polys[++j].start; q.polys[idx].p=p.polys[j].p*(- 1.0);
00299 }
00300
00301
00302 idx++;
00303 }
00304 return q;
00305 }
00306 template <int Degree> PPolynomial<Degree>&
00307 PPolynomial<Degree>::addScaled (const PPolynomial<Degree> &p,const double &scale)
00308 {
00309 int i,j;
00310 StartingPolynomial<Degree> *oldPolys=polys;
00311 size_t idx=0,cnt=0,oldPolyCount=polyCount;
00312 polyCount=0;
00313 polys=NULL;
00314 set (oldPolyCount+p.polyCount);
00315 i=j=- 1;
00316 while (cnt<polyCount)
00317 {
00318 if (j>=int(p.polyCount)-1)
00319 {
00320 polys[idx]=oldPolys[++i];
00321 }
00322 else if (i>=int(oldPolyCount)-1)
00323 {
00324 polys[idx].start= p.polys[++j].start; polys[idx].p=p.polys[j].p*scale;
00325 }
00326 else if (oldPolys[i+1].start<p.polys[j+1].start)
00327 {
00328 polys[idx]=oldPolys[++i];
00329 }
00330 else
00331 {
00332 polys[idx].start= p.polys[++j].start; polys[idx].p=p.polys[j].p*scale;
00333 }
00334 if (idx && polys[idx].start==polys[idx-1].start)
00335 {
00336 polys[idx-1].p+=polys[idx].p;
00337 }
00338 else
00339 {
00340 idx++;
00341 }
00342 cnt++;
00343 }
00344 free (oldPolys);
00345 reset (idx);
00346 return *this;
00347 }
00348 template <int Degree> template <int Degree2> PPolynomial<Degree+Degree2>
00349 PPolynomial<Degree>::operator * (const PPolynomial<Degree2> &p) const
00350 {
00351 PPolynomial<Degree+Degree2> q;
00352 StartingPolynomial<Degree+Degree2> *sp;
00353 int i,j,spCount=int(polyCount*p.polyCount);
00354
00355 sp = reinterpret_cast<StartingPolynomial<Degree+Degree2>*> (malloc (sizeof (StartingPolynomial<Degree+Degree2>) * spCount));
00356 for (i = 0; i < int (polyCount); i++)
00357 {
00358 for (j = 0; j < int (p.polyCount); j++)
00359 {
00360 sp[i*p.polyCount+j] = polys[i] * p.polys[j];
00361 }
00362 }
00363 q.set (sp,spCount);
00364 free (sp);
00365 return (q);
00366 }
00367
00368 template <int Degree> template <int Degree2> PPolynomial<Degree+Degree2>
00369 PPolynomial<Degree>::operator * (const Polynomial<Degree2> &p) const
00370 {
00371 PPolynomial<Degree+Degree2> q;
00372 q.set (polyCount);
00373 for (int i=0; i<int(polyCount); i++)
00374 {
00375 q.polys[i].start=polys[i].start;
00376 q.polys[i].p=polys[i].p*p;
00377 }
00378 return q;
00379 }
00380 template <int Degree> PPolynomial<Degree>
00381 PPolynomial<Degree>::scale (const double &s) const
00382 {
00383 PPolynomial q;
00384 q.set (polyCount);
00385 for (size_t i=0; i<polyCount; i++)
00386 {
00387 q.polys[i]=polys[i].scale (s);
00388 }
00389 return q;
00390 }
00391 template <int Degree> PPolynomial<Degree>
00392 PPolynomial<Degree>::shift (const double &s) const
00393 {
00394 PPolynomial q;
00395 q.set (polyCount);
00396 for (size_t i=0; i<polyCount; i++)
00397 {
00398 q.polys[i]=polys[i].shift (s);
00399 }
00400 return q;
00401 }
00402 template <int Degree> PPolynomial<Degree-1>
00403 PPolynomial<Degree>::derivative (void) const
00404 {
00405 PPolynomial<Degree-1> q;
00406 q.set (polyCount);
00407 for (size_t i=0; i<polyCount; i++)
00408 {
00409 q.polys[i].start=polys[i].start;
00410 q.polys[i].p=polys[i].p.derivative ();
00411 }
00412 return q;
00413 }
00414 template <int Degree> PPolynomial<Degree+1>
00415 PPolynomial<Degree>::integral (void) const
00416 {
00417 int i;
00418 PPolynomial<Degree+1> q;
00419 q.set (polyCount);
00420 for (i=0; i<int(polyCount); i++)
00421 {
00422 q.polys[i].start=polys[i].start;
00423 q.polys[i].p=polys[i].p.integral ();
00424 q.polys[i].p-=q.polys[i].p (q.polys[i].start);
00425 }
00426 return q;
00427 }
00428 template <int Degree> PPolynomial<Degree>&
00429 PPolynomial<Degree>::operator += (const double &s){polys[0].p+=s; }
00430 template <int Degree> PPolynomial<Degree>&
00431 PPolynomial<Degree>::operator -= (const double &s){polys[0].p-=s; }
00432 template <int Degree> PPolynomial<Degree>&
00433 PPolynomial<Degree>::operator *= (const double &s)
00434 {
00435 for (int i=0; i<int(polyCount); i++)
00436 {
00437 polys[i].p*=s;
00438 }
00439 return *this;
00440 }
00441 template <int Degree> PPolynomial<Degree>&
00442 PPolynomial<Degree>::operator /= (const double &s)
00443 {
00444 for (size_t i=0; i<polyCount; i++)
00445 {
00446 polys[i].p/=s;
00447 }
00448 return *this;
00449 }
00450 template <int Degree> PPolynomial<Degree>
00451 PPolynomial<Degree>::operator + (const double &s) const
00452 {
00453 PPolynomial q=*this;
00454 q+=s;
00455 return q;
00456 }
00457 template <int Degree> PPolynomial<Degree>
00458 PPolynomial<Degree>::operator - (const double &s) const
00459 {
00460 PPolynomial q=*this;
00461 q-=s;
00462 return q;
00463 }
00464 template <int Degree> PPolynomial<Degree>
00465 PPolynomial<Degree>::operator * (const double &s) const
00466 {
00467 PPolynomial q=*this;
00468 q*=s;
00469 return q;
00470 }
00471 template <int Degree> PPolynomial<Degree>
00472 PPolynomial<Degree>::operator / (const double &s) const
00473 {
00474 PPolynomial q=*this;
00475 q/=s;
00476 return q;
00477 }
00478
00479 template <int Degree> void
00480 PPolynomial<Degree>::printnl (void) const
00481 {
00482 Polynomial<Degree> p;
00483
00484 if (!polyCount)
00485 {
00486 Polynomial<Degree> p;
00487 printf ("[-Infinity,Infinity]\n");
00488 }
00489 else
00490 {
00491 for (size_t i=0; i<polyCount; i++)
00492 {
00493 printf ("[");
00494 if (polys[i].start== DBL_MAX)
00495 {
00496 printf ("Infinity,");
00497 }
00498 else if (polys[i].start==- DBL_MAX)
00499 {
00500 printf ("-Infinity,");
00501 }
00502 else
00503 {
00504 printf ("%f,",polys[i].start);
00505 }
00506 if (i+1==polyCount)
00507 {
00508 printf ("Infinity]\t");
00509 }
00510 else if (polys[i+1].start== DBL_MAX)
00511 {
00512 printf ("Infinity]\t");
00513 }
00514 else if (polys[i+1].start==- DBL_MAX)
00515 {
00516 printf ("-Infinity]\t");
00517 }
00518 else
00519 {
00520 printf ("%f]\t",polys[i+1].start);
00521 }
00522 p=p+polys[i].p;
00523 p.printnl ();
00524 }
00525 }
00526 printf ("\n");
00527 }
00528
00530 template <int Degree> PPolynomial<Degree>
00531 PPolynomial<Degree>::ConstantFunction (const double &radius)
00532 {
00533 if (Degree < 0)
00534 throw "Could not set degree %d polynomial as constant\n";
00535
00536 PPolynomial q;
00537 q.set (2);
00538
00539 q.polys[0].start=- radius;
00540 q.polys[1].start= radius;
00541
00542 q.polys[0].p.coefficients[0]= 1.0;
00543 q.polys[1].p.coefficients[0]=- 1.0;
00544 return (q);
00545 }
00546
00548 template <> PPolynomial<0>
00549 PPolynomial<0>::GaussianApproximation (const double &width)
00550 {
00551 return (ConstantFunction (width));
00552 }
00553
00555 template <int Degree> PPolynomial<Degree>
00556 PPolynomial<Degree>::GaussianApproximation (const double &width)
00557 {
00558 return (PPolynomial<Degree-1>::GaussianApproximation ().MovingAverage (width));
00559 }
00560
00562 template <int Degree> PPolynomial<Degree+1>
00563 PPolynomial<Degree>::MovingAverage (const double &radius)
00564 {
00565 PPolynomial<Degree+1> A;
00566 Polynomial<Degree+1> p;
00567 StartingPolynomial<Degree+1> *sps;
00568
00569 sps = reinterpret_cast<StartingPolynomial<Degree+1>*> (malloc (sizeof (StartingPolynomial<Degree+1>) * polyCount * 2));
00570
00571 for (int i=0; i<int(polyCount); i++)
00572 {
00573 sps[2*i].start=polys[i].start-radius;
00574 sps[2*i+1].start=polys[i].start+radius;
00575 p=polys[i].p.integral ()-polys[i].p.integral () (polys[i].start);
00576 sps[2*i].p=p.shift (- radius);
00577 sps[2*i+1].p=p.shift (radius)* - 1;
00578 }
00579 A.set (sps,int(polyCount*2));
00580 free (sps);
00581 return A*1.0/(2*radius);
00582 }
00583
00584 template <int Degree> void
00585 PPolynomial<Degree>::getSolutions (const double &c,std::vector<double> &roots,const double &EPS,const double &min,
00586 const double &max) const
00587 {
00588 Polynomial<Degree> p;
00589 std::vector<double> tempRoots;
00590
00591 p.setZero ();
00592 for (size_t i=0; i<polyCount; i++)
00593 {
00594 p+=polys[i].p;
00595 if (polys[i].start>max)
00596 {
00597 break;
00598 }
00599 if (i<polyCount-1 && polys[i+1].start<min)
00600 {
00601 continue;
00602 }
00603 p.getSolutions (c,tempRoots,EPS);
00604 for (size_t j=0; j<tempRoots.size (); j++)
00605 {
00606 if (tempRoots[j]>polys[i].start && (i+1==polyCount || tempRoots[j]<=polys[i+1].start))
00607 {
00608 if (tempRoots[j]>min && tempRoots[j]<max)
00609 {
00610 roots.push_back (tempRoots[j]);
00611 }
00612 }
00613 }
00614 }
00615 }
00616
00617 template <int Degree> void
00618 PPolynomial<Degree>::write (FILE *fp,const int &samples,const double &min,const double &max) const
00619 {
00620 fwrite (&samples,sizeof(int),1,fp);
00621 for (int i=0; i<samples; i++)
00622 {
00623 double x=min+i*(max-min)/(samples-1);
00624 float v=(*this)(x);
00625 fwrite (&v,sizeof(float),1,fp);
00626 }
00627 }
00628
00629
00630 }
00631 }
00632