Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho 00003 All rights reserved. 00004 00005 Redistribution and use in source and binary forms, with or without modification, 00006 are permitted provided that the following conditions are met: 00007 00008 Redistributions of source code must retain the above copyright notice, this list of 00009 conditions and the following disclaimer. Redistributions in binary form must reproduce 00010 the above copyright notice, this list of conditions and the following disclaimer 00011 in the documentation and/or other materials provided with the distribution. 00012 00013 Neither the name of the Johns Hopkins University nor the names of its contributors 00014 may be used to endorse or promote products derived from this software without specific 00015 prior written permission. 00016 00017 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 00018 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES 00019 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 00020 SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00021 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 00022 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 00023 BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00024 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00025 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 00026 DAMAGE. 00027 */ 00028 00029 #include "factor.h" 00030 00031 //////////////////////// 00032 // StartingPolynomial // 00033 //////////////////////// 00034 00035 namespace pcl 00036 { 00037 namespace poisson 00038 { 00039 00040 00041 template<int Degree> 00042 template<int Degree2> 00043 StartingPolynomial<Degree+Degree2> StartingPolynomial<Degree>::operator * (const StartingPolynomial<Degree2>& p) const{ 00044 StartingPolynomial<Degree+Degree2> sp; 00045 if(start>p.start){sp.start=start;} 00046 else{sp.start=p.start;} 00047 sp.p=this->p*p.p; 00048 return sp; 00049 } 00050 template<int Degree> 00051 StartingPolynomial<Degree> StartingPolynomial<Degree>::scale(double s) const{ 00052 StartingPolynomial q; 00053 q.start=start*s; 00054 q.p=p.scale(s); 00055 return q; 00056 } 00057 template<int Degree> 00058 StartingPolynomial<Degree> StartingPolynomial<Degree>::shift(double s) const{ 00059 StartingPolynomial q; 00060 q.start=start+s; 00061 q.p=p.shift(s); 00062 return q; 00063 } 00064 00065 00066 template<int Degree> 00067 int StartingPolynomial<Degree>::operator < (const StartingPolynomial<Degree>& sp) const{ 00068 if(start<sp.start){return 1;} 00069 else{return 0;} 00070 } 00071 template<int Degree> 00072 int StartingPolynomial<Degree>::Compare(const void* v1,const void* v2){ 00073 double d=((StartingPolynomial*)(v1))->start-((StartingPolynomial*)(v2))->start; 00074 if (d<0) {return -1;} 00075 else if (d>0) {return 1;} 00076 else {return 0;} 00077 } 00078 00079 ///////////////// 00080 // PPolynomial // 00081 ///////////////// 00082 template<int Degree> 00083 PPolynomial<Degree>::PPolynomial(void){ 00084 polyCount=0; 00085 polys=NULL; 00086 } 00087 template<int Degree> 00088 PPolynomial<Degree>::PPolynomial(const PPolynomial<Degree>& p){ 00089 polyCount=0; 00090 polys=NULL; 00091 set(p.polyCount); 00092 memcpy(polys,p.polys,sizeof(StartingPolynomial<Degree>)*p.polyCount); 00093 } 00094 00095 template<int Degree> 00096 PPolynomial<Degree>::~PPolynomial(void){ 00097 if(polyCount){free(polys);} 00098 polyCount=0; 00099 polys=NULL; 00100 } 00101 template<int Degree> 00102 void PPolynomial<Degree>::set(StartingPolynomial<Degree>* sps,int count){ 00103 int i,c=0; 00104 set(count); 00105 qsort(sps,count,sizeof(StartingPolynomial<Degree>),StartingPolynomial<Degree>::Compare); 00106 for( i=0 ; i<count ; i++ ) 00107 { 00108 if( !c || sps[i].start!=polys[c-1].start ) polys[c++] = sps[i]; 00109 else{polys[c-1].p+=sps[i].p;} 00110 } 00111 reset( c ); 00112 } 00113 template <int Degree> 00114 int PPolynomial<Degree>::size(void) const{return int(sizeof(StartingPolynomial<Degree>)*polyCount);} 00115 00116 template<int Degree> 00117 void PPolynomial<Degree>::set( size_t size ) 00118 { 00119 if(polyCount){free(polys);} 00120 polyCount=0; 00121 polys=NULL; 00122 polyCount=size; 00123 if(size){ 00124 polys=(StartingPolynomial<Degree>*)malloc(sizeof(StartingPolynomial<Degree>)*size); 00125 memset(polys,0,sizeof(StartingPolynomial<Degree>)*size); 00126 } 00127 } 00128 template<int Degree> 00129 void PPolynomial<Degree>::reset( size_t newSize ) 00130 { 00131 polyCount=newSize; 00132 polys=(StartingPolynomial<Degree>*)realloc(polys,sizeof(StartingPolynomial<Degree>)*newSize); 00133 } 00134 00135 template<int Degree> 00136 PPolynomial<Degree>& PPolynomial<Degree>::operator = (const PPolynomial<Degree>& p){ 00137 set(p.polyCount); 00138 memcpy(polys,p.polys,sizeof(StartingPolynomial<Degree>)*p.polyCount); 00139 return *this; 00140 } 00141 00142 template<int Degree> 00143 template<int Degree2> 00144 PPolynomial<Degree>& PPolynomial<Degree>::operator = (const PPolynomial<Degree2>& p){ 00145 set(p.polyCount); 00146 for(int i=0;i<int(polyCount);i++){ 00147 polys[i].start=p.polys[i].start; 00148 polys[i].p=p.polys[i].p; 00149 } 00150 return *this; 00151 } 00152 00153 template<int Degree> 00154 double PPolynomial<Degree>::operator ()( double t ) const 00155 { 00156 double v=0; 00157 for( int i=0 ; i<int(polyCount) && t>polys[i].start ; i++ ) v+=polys[i].p(t); 00158 return v; 00159 } 00160 00161 template<int Degree> 00162 double PPolynomial<Degree>::integral( double tMin , double tMax ) const 00163 { 00164 int m=1; 00165 double start,end,s,v=0; 00166 start=tMin; 00167 end=tMax; 00168 if(tMin>tMax){ 00169 m=-1; 00170 start=tMax; 00171 end=tMin; 00172 } 00173 for(int i=0;i<int(polyCount) && polys[i].start<end;i++){ 00174 if(start<polys[i].start){s=polys[i].start;} 00175 else{s=start;} 00176 v+=polys[i].p.integral(s,end); 00177 } 00178 return v*m; 00179 } 00180 template<int Degree> 00181 double PPolynomial<Degree>::Integral(void) const{return integral(polys[0].start,polys[polyCount-1].start);} 00182 template<int Degree> 00183 PPolynomial<Degree> PPolynomial<Degree>::operator + (const PPolynomial<Degree>& p) const{ 00184 PPolynomial q; 00185 int i,j; 00186 size_t idx=0; 00187 q.set(polyCount+p.polyCount); 00188 i=j=-1; 00189 00190 while(idx<q.polyCount){ 00191 if (j>=int(p.polyCount)-1) {q.polys[idx]= polys[++i];} 00192 else if (i>=int( polyCount)-1) {q.polys[idx]=p.polys[++j];} 00193 else if(polys[i+1].start<p.polys[j+1].start){q.polys[idx]= polys[++i];} 00194 else {q.polys[idx]=p.polys[++j];} 00195 idx++; 00196 } 00197 return q; 00198 } 00199 template<int Degree> 00200 PPolynomial<Degree> PPolynomial<Degree>::operator - (const PPolynomial<Degree>& p) const{ 00201 PPolynomial q; 00202 int i,j; 00203 size_t idx=0; 00204 q.set(polyCount+p.polyCount); 00205 i=j=-1; 00206 00207 while(idx<q.polyCount){ 00208 if (j>=int(p.polyCount)-1) {q.polys[idx]= polys[++i];} 00209 else if (i>=int( polyCount)-1) {q.polys[idx].start=p.polys[++j].start;q.polys[idx].p=p.polys[j].p*(-1.0);} 00210 else if(polys[i+1].start<p.polys[j+1].start){q.polys[idx]= polys[++i];} 00211 else {q.polys[idx].start=p.polys[++j].start;q.polys[idx].p=p.polys[j].p*(-1.0);} 00212 idx++; 00213 } 00214 return q; 00215 } 00216 template<int Degree> 00217 PPolynomial<Degree>& PPolynomial<Degree>::addScaled(const PPolynomial<Degree>& p,double scale){ 00218 int i,j; 00219 StartingPolynomial<Degree>* oldPolys=polys; 00220 size_t idx=0,cnt=0,oldPolyCount=polyCount; 00221 polyCount=0; 00222 polys=NULL; 00223 set(oldPolyCount+p.polyCount); 00224 i=j=-1; 00225 while(cnt<polyCount){ 00226 if (j>=int( p.polyCount)-1) {polys[idx]=oldPolys[++i];} 00227 else if (i>=int(oldPolyCount)-1) {polys[idx].start= p.polys[++j].start;polys[idx].p=p.polys[j].p*scale;} 00228 else if (oldPolys[i+1].start<p.polys[j+1].start){polys[idx]=oldPolys[++i];} 00229 else {polys[idx].start= p.polys[++j].start;polys[idx].p=p.polys[j].p*scale;} 00230 if(idx && polys[idx].start==polys[idx-1].start) {polys[idx-1].p+=polys[idx].p;} 00231 else{idx++;} 00232 cnt++; 00233 } 00234 free(oldPolys); 00235 reset(idx); 00236 return *this; 00237 } 00238 template<int Degree> 00239 template<int Degree2> 00240 PPolynomial<Degree+Degree2> PPolynomial<Degree>::operator * (const PPolynomial<Degree2>& p) const{ 00241 PPolynomial<Degree+Degree2> q; 00242 StartingPolynomial<Degree+Degree2> *sp; 00243 int i,j,spCount=int(polyCount*p.polyCount); 00244 00245 sp=(StartingPolynomial<Degree+Degree2>*)malloc(sizeof(StartingPolynomial<Degree+Degree2>)*spCount); 00246 for(i=0;i<int(polyCount);i++){ 00247 for(j=0;j<int(p.polyCount);j++){ 00248 sp[i*p.polyCount+j]=polys[i]*p.polys[j]; 00249 } 00250 } 00251 q.set(sp,spCount); 00252 free(sp); 00253 return q; 00254 } 00255 template<int Degree> 00256 template<int Degree2> 00257 PPolynomial<Degree+Degree2> PPolynomial<Degree>::operator * (const Polynomial<Degree2>& p) const{ 00258 PPolynomial<Degree+Degree2> q; 00259 q.set(polyCount); 00260 for(int i=0;i<int(polyCount);i++){ 00261 q.polys[i].start=polys[i].start; 00262 q.polys[i].p=polys[i].p*p; 00263 } 00264 return q; 00265 } 00266 template<int Degree> 00267 PPolynomial<Degree> PPolynomial<Degree>::scale( double s ) const 00268 { 00269 PPolynomial q; 00270 q.set(polyCount); 00271 for(size_t i=0;i<polyCount;i++){q.polys[i]=polys[i].scale(s);} 00272 return q; 00273 } 00274 template<int Degree> 00275 PPolynomial<Degree> PPolynomial<Degree>::shift( double s ) const 00276 { 00277 PPolynomial q; 00278 q.set(polyCount); 00279 for(size_t i=0;i<polyCount;i++){q.polys[i]=polys[i].shift(s);} 00280 return q; 00281 } 00282 template<int Degree> 00283 PPolynomial<Degree-1> PPolynomial<Degree>::derivative(void) const{ 00284 PPolynomial<Degree-1> q; 00285 q.set(polyCount); 00286 for(size_t i=0;i<polyCount;i++){ 00287 q.polys[i].start=polys[i].start; 00288 q.polys[i].p=polys[i].p.derivative(); 00289 } 00290 return q; 00291 } 00292 template<int Degree> 00293 PPolynomial<Degree+1> PPolynomial<Degree>::integral(void) const{ 00294 int i; 00295 PPolynomial<Degree+1> q; 00296 q.set(polyCount); 00297 for(i=0;i<int(polyCount);i++){ 00298 q.polys[i].start=polys[i].start; 00299 q.polys[i].p=polys[i].p.integral(); 00300 q.polys[i].p-=q.polys[i].p(q.polys[i].start); 00301 } 00302 return q; 00303 } 00304 template<int Degree> 00305 PPolynomial<Degree>& PPolynomial<Degree>::operator += ( double s ) {polys[0].p+=s;} 00306 template<int Degree> 00307 PPolynomial<Degree>& PPolynomial<Degree>::operator -= ( double s ) {polys[0].p-=s;} 00308 template<int Degree> 00309 PPolynomial<Degree>& PPolynomial<Degree>::operator *= ( double s ) 00310 { 00311 for(int i=0;i<int(polyCount);i++){polys[i].p*=s;} 00312 return *this; 00313 } 00314 template<int Degree> 00315 PPolynomial<Degree>& PPolynomial<Degree>::operator /= ( double s ) 00316 { 00317 for(size_t i=0;i<polyCount;i++){polys[i].p/=s;} 00318 return *this; 00319 } 00320 template<int Degree> 00321 PPolynomial<Degree> PPolynomial<Degree>::operator + ( double s ) const 00322 { 00323 PPolynomial q=*this; 00324 q+=s; 00325 return q; 00326 } 00327 template<int Degree> 00328 PPolynomial<Degree> PPolynomial<Degree>::operator - ( double s ) const 00329 { 00330 PPolynomial q=*this; 00331 q-=s; 00332 return q; 00333 } 00334 template<int Degree> 00335 PPolynomial<Degree> PPolynomial<Degree>::operator * ( double s ) const 00336 { 00337 PPolynomial q=*this; 00338 q*=s; 00339 return q; 00340 } 00341 template<int Degree> 00342 PPolynomial<Degree> PPolynomial<Degree>::operator / ( double s ) const 00343 { 00344 PPolynomial q=*this; 00345 q/=s; 00346 return q; 00347 } 00348 00349 template<int Degree> 00350 void PPolynomial<Degree>::printnl(void) const{ 00351 Polynomial<Degree> p; 00352 00353 if(!polyCount){ 00354 Polynomial<Degree> p; 00355 printf("[-Infinity,Infinity]\n"); 00356 } 00357 else{ 00358 for(size_t i=0;i<polyCount;i++){ 00359 printf("["); 00360 if (polys[i ].start== DBL_MAX){printf("Infinity,");} 00361 else if (polys[i ].start==-DBL_MAX){printf("-Infinity,");} 00362 else {printf("%f,",polys[i].start);} 00363 if(i+1==polyCount) {printf("Infinity]\t");} 00364 else if (polys[i+1].start== DBL_MAX){printf("Infinity]\t");} 00365 else if (polys[i+1].start==-DBL_MAX){printf("-Infinity]\t");} 00366 else {printf("%f]\t",polys[i+1].start);} 00367 p=p+polys[i].p; 00368 p.printnl(); 00369 } 00370 } 00371 printf("\n"); 00372 } 00373 template< > 00374 PPolynomial< 0 > PPolynomial< 0 >::BSpline( double radius ) 00375 { 00376 PPolynomial q; 00377 q.set(2); 00378 00379 q.polys[0].start=-radius; 00380 q.polys[1].start= radius; 00381 00382 q.polys[0].p.coefficients[0]= 1.0; 00383 q.polys[1].p.coefficients[0]=-1.0; 00384 return q; 00385 } 00386 template< int Degree > 00387 PPolynomial< Degree > PPolynomial<Degree>::BSpline( double radius ) 00388 { 00389 return PPolynomial< Degree-1 >::BSpline().MovingAverage( radius ); 00390 } 00391 template<int Degree> 00392 PPolynomial<Degree+1> PPolynomial<Degree>::MovingAverage( double radius ) 00393 { 00394 PPolynomial<Degree+1> A; 00395 Polynomial<Degree+1> p; 00396 StartingPolynomial<Degree+1>* sps; 00397 00398 sps=(StartingPolynomial<Degree+1>*)malloc(sizeof(StartingPolynomial<Degree+1>)*polyCount*2); 00399 00400 for(int i=0;i<int(polyCount);i++){ 00401 sps[2*i ].start=polys[i].start-radius; 00402 sps[2*i+1].start=polys[i].start+radius; 00403 p=polys[i].p.integral()-polys[i].p.integral()(polys[i].start); 00404 sps[2*i ].p=p.shift(-radius); 00405 sps[2*i+1].p=p.shift( radius)*-1; 00406 } 00407 A.set(sps,int(polyCount*2)); 00408 free(sps); 00409 return A*1.0/(2*radius); 00410 } 00411 template<int Degree> 00412 void PPolynomial<Degree>::getSolutions(double c,std::vector<double>& roots,double EPS,double min,double max) const{ 00413 Polynomial<Degree> p; 00414 std::vector<double> tempRoots; 00415 00416 p.setZero(); 00417 for(size_t i=0;i<polyCount;i++){ 00418 p+=polys[i].p; 00419 if(polys[i].start>max){break;} 00420 if(i<polyCount-1 && polys[i+1].start<min){continue;} 00421 p.getSolutions(c,tempRoots,EPS); 00422 for(size_t j=0;j<tempRoots.size();j++){ 00423 if(tempRoots[j]>polys[i].start && (i+1==polyCount || tempRoots[j]<=polys[i+1].start)){ 00424 if(tempRoots[j]>min && tempRoots[j]<max){roots.push_back(tempRoots[j]);} 00425 } 00426 } 00427 } 00428 } 00429 00430 template<int Degree> 00431 void PPolynomial<Degree>::write(FILE* fp,int samples,double min,double max) const{ 00432 fwrite(&samples,sizeof(int),1,fp); 00433 for(int i=0;i<samples;i++){ 00434 double x=min+i*(max-min)/(samples-1); 00435 float v=(*this)(x); 00436 fwrite(&v,sizeof(float),1,fp); 00437 } 00438 } 00439 00440 } 00441 }