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 <float.h> 00030 #include <math.h> 00031 #include <algorithm> 00032 #include "factor.h" 00033 00034 //////////////// 00035 // Polynomial // 00036 //////////////// 00037 00038 namespace pcl 00039 { 00040 namespace poisson 00041 { 00042 00043 00044 template<int Degree> 00045 Polynomial<Degree>::Polynomial(void){memset(coefficients,0,sizeof(double)*(Degree+1));} 00046 template<int Degree> 00047 template<int Degree2> 00048 Polynomial<Degree>::Polynomial(const Polynomial<Degree2>& P){ 00049 memset(coefficients,0,sizeof(double)*(Degree+1)); 00050 for(int i=0;i<=Degree && i<=Degree2;i++){coefficients[i]=P.coefficients[i];} 00051 } 00052 00053 00054 template<int Degree> 00055 template<int Degree2> 00056 Polynomial<Degree>& Polynomial<Degree>::operator = (const Polynomial<Degree2> &p){ 00057 int d=Degree<Degree2?Degree:Degree2; 00058 memset(coefficients,0,sizeof(double)*(Degree+1)); 00059 memcpy(coefficients,p.coefficients,sizeof(double)*(d+1)); 00060 return *this; 00061 } 00062 00063 template<int Degree> 00064 Polynomial<Degree-1> Polynomial<Degree>::derivative(void) const{ 00065 Polynomial<Degree-1> p; 00066 for(int i=0;i<Degree;i++){p.coefficients[i]=coefficients[i+1]*(i+1);} 00067 return p; 00068 } 00069 00070 template<int Degree> 00071 Polynomial<Degree+1> Polynomial<Degree>::integral(void) const{ 00072 Polynomial<Degree+1> p; 00073 p.coefficients[0]=0; 00074 for(int i=0;i<=Degree;i++){p.coefficients[i+1]=coefficients[i]/(i+1);} 00075 return p; 00076 } 00077 template<> double Polynomial< 0 >::operator() ( double t ) const { return coefficients[0]; } 00078 template<> double Polynomial< 1 >::operator() ( double t ) const { return coefficients[0]+coefficients[1]*t; } 00079 template<> double Polynomial< 2 >::operator() ( double t ) const { return coefficients[0]+(coefficients[1]+coefficients[2]*t)*t; } 00080 template<int Degree> 00081 double Polynomial<Degree>::operator() ( double t ) const{ 00082 double v=coefficients[Degree]; 00083 for( int d=Degree-1 ; d>=0 ; d-- ) v = v*t + coefficients[d]; 00084 return v; 00085 } 00086 template<int Degree> 00087 double Polynomial<Degree>::integral( double tMin , double tMax ) const 00088 { 00089 double v=0; 00090 double t1,t2; 00091 t1=tMin; 00092 t2=tMax; 00093 for(int i=0;i<=Degree;i++){ 00094 v+=coefficients[i]*(t2-t1)/(i+1); 00095 if(t1!=-DBL_MAX && t1!=DBL_MAX){t1*=tMin;} 00096 if(t2!=-DBL_MAX && t2!=DBL_MAX){t2*=tMax;} 00097 } 00098 return v; 00099 } 00100 template<int Degree> 00101 int Polynomial<Degree>::operator == (const Polynomial& p) const{ 00102 for(int i=0;i<=Degree;i++){if(coefficients[i]!=p.coefficients[i]){return 0;}} 00103 return 1; 00104 } 00105 template<int Degree> 00106 int Polynomial<Degree>::operator != (const Polynomial& p) const{ 00107 for(int i=0;i<=Degree;i++){if(coefficients[i]==p.coefficients[i]){return 0;}} 00108 return 1; 00109 } 00110 template<int Degree> 00111 int Polynomial<Degree>::isZero(void) const{ 00112 for(int i=0;i<=Degree;i++){if(coefficients[i]!=0){return 0;}} 00113 return 1; 00114 } 00115 template<int Degree> 00116 void Polynomial<Degree>::setZero(void){memset(coefficients,0,sizeof(double)*(Degree+1));} 00117 00118 template<int Degree> 00119 Polynomial<Degree>& Polynomial<Degree>::addScaled(const Polynomial& p,double s){ 00120 for(int i=0;i<=Degree;i++){coefficients[i]+=p.coefficients[i]*s;} 00121 return *this; 00122 } 00123 template<int Degree> 00124 Polynomial<Degree>& Polynomial<Degree>::operator += (const Polynomial<Degree>& p){ 00125 for(int i=0;i<=Degree;i++){coefficients[i]+=p.coefficients[i];} 00126 return *this; 00127 } 00128 template<int Degree> 00129 Polynomial<Degree>& Polynomial<Degree>::operator -= (const Polynomial<Degree>& p){ 00130 for(int i=0;i<=Degree;i++){coefficients[i]-=p.coefficients[i];} 00131 return *this; 00132 } 00133 template<int Degree> 00134 Polynomial<Degree> Polynomial<Degree>::operator + (const Polynomial<Degree>& p) const{ 00135 Polynomial q; 00136 for(int i=0;i<=Degree;i++){q.coefficients[i]=(coefficients[i]+p.coefficients[i]);} 00137 return q; 00138 } 00139 template<int Degree> 00140 Polynomial<Degree> Polynomial<Degree>::operator - (const Polynomial<Degree>& p) const{ 00141 Polynomial q; 00142 for(int i=0;i<=Degree;i++) {q.coefficients[i]=coefficients[i]-p.coefficients[i];} 00143 return q; 00144 } 00145 template<int Degree> 00146 void Polynomial<Degree>::Scale(const Polynomial& p,double w,Polynomial& q){ 00147 for(int i=0;i<=Degree;i++){q.coefficients[i]=p.coefficients[i]*w;} 00148 } 00149 template<int Degree> 00150 void Polynomial<Degree>::AddScaled(const Polynomial& p1,double w1,const Polynomial& p2,double w2,Polynomial& q){ 00151 for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]*w1+p2.coefficients[i]*w2;} 00152 } 00153 template<int Degree> 00154 void Polynomial<Degree>::AddScaled(const Polynomial& p1,double w1,const Polynomial& p2,Polynomial& q){ 00155 for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]*w1+p2.coefficients[i];} 00156 } 00157 template<int Degree> 00158 void Polynomial<Degree>::AddScaled(const Polynomial& p1,const Polynomial& p2,double w2,Polynomial& q){ 00159 for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]+p2.coefficients[i]*w2;} 00160 } 00161 00162 template<int Degree> 00163 void Polynomial<Degree>::Subtract(const Polynomial &p1,const Polynomial& p2,Polynomial& q){ 00164 for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]-p2.coefficients[i];} 00165 } 00166 template<int Degree> 00167 void Polynomial<Degree>::Negate(const Polynomial& in,Polynomial& out){ 00168 out=in; 00169 for(int i=0;i<=Degree;i++){out.coefficients[i]=-out.coefficients[i];} 00170 } 00171 00172 template<int Degree> 00173 Polynomial<Degree> Polynomial<Degree>::operator - (void) const{ 00174 Polynomial q=*this; 00175 for(int i=0;i<=Degree;i++){q.coefficients[i]=-q.coefficients[i];} 00176 return q; 00177 } 00178 template<int Degree> 00179 template<int Degree2> 00180 Polynomial<Degree+Degree2> Polynomial<Degree>::operator * (const Polynomial<Degree2>& p) const{ 00181 Polynomial<Degree+Degree2> q; 00182 for(int i=0;i<=Degree;i++){for(int j=0;j<=Degree2;j++){q.coefficients[i+j]+=coefficients[i]*p.coefficients[j];}} 00183 return q; 00184 } 00185 00186 template<int Degree> 00187 Polynomial<Degree>& Polynomial<Degree>::operator += ( double s ) 00188 { 00189 coefficients[0]+=s; 00190 return *this; 00191 } 00192 template<int Degree> 00193 Polynomial<Degree>& Polynomial<Degree>::operator -= ( double s ) 00194 { 00195 coefficients[0]-=s; 00196 return *this; 00197 } 00198 template<int Degree> 00199 Polynomial<Degree>& Polynomial<Degree>::operator *= ( double s ) 00200 { 00201 for(int i=0;i<=Degree;i++){coefficients[i]*=s;} 00202 return *this; 00203 } 00204 template<int Degree> 00205 Polynomial<Degree>& Polynomial<Degree>::operator /= ( double s ) 00206 { 00207 for(int i=0;i<=Degree;i++){coefficients[i]/=s;} 00208 return *this; 00209 } 00210 template<int Degree> 00211 Polynomial<Degree> Polynomial<Degree>::operator + ( double s ) const 00212 { 00213 Polynomial<Degree> q=*this; 00214 q.coefficients[0]+=s; 00215 return q; 00216 } 00217 template<int Degree> 00218 Polynomial<Degree> Polynomial<Degree>::operator - ( double s ) const 00219 { 00220 Polynomial q=*this; 00221 q.coefficients[0]-=s; 00222 return q; 00223 } 00224 template<int Degree> 00225 Polynomial<Degree> Polynomial<Degree>::operator * ( double s ) const 00226 { 00227 Polynomial q; 00228 for(int i=0;i<=Degree;i++){q.coefficients[i]=coefficients[i]*s;} 00229 return q; 00230 } 00231 template<int Degree> 00232 Polynomial<Degree> Polynomial<Degree>::operator / ( double s ) const 00233 { 00234 Polynomial q; 00235 for( int i=0 ; i<=Degree ; i++ ) q.coefficients[i] = coefficients[i]/s; 00236 return q; 00237 } 00238 template<int Degree> 00239 Polynomial<Degree> Polynomial<Degree>::scale( double s ) const 00240 { 00241 Polynomial q=*this; 00242 double s2=1.0; 00243 for(int i=0;i<=Degree;i++){ 00244 q.coefficients[i]*=s2; 00245 s2/=s; 00246 } 00247 return q; 00248 } 00249 template<int Degree> 00250 Polynomial<Degree> Polynomial<Degree>::shift( double t ) const 00251 { 00252 Polynomial<Degree> q; 00253 for(int i=0;i<=Degree;i++){ 00254 double temp=1; 00255 for(int j=i;j>=0;j--){ 00256 q.coefficients[j]+=coefficients[i]*temp; 00257 temp*=-t*j; 00258 temp/=(i-j+1); 00259 } 00260 } 00261 return q; 00262 } 00263 template<int Degree> 00264 void Polynomial<Degree>::printnl(void) const{ 00265 for(int j=0;j<=Degree;j++){ 00266 printf("%6.4f x^%d ",coefficients[j],j); 00267 if(j<Degree && coefficients[j+1]>=0){printf("+");} 00268 } 00269 printf("\n"); 00270 } 00271 template<int Degree> 00272 void Polynomial<Degree>::getSolutions(double c,std::vector<double>& roots,double EPS) const 00273 { 00274 double r[4][2]; 00275 int rCount=0; 00276 roots.clear(); 00277 switch(Degree){ 00278 case 1: 00279 rCount=Factor(coefficients[1],coefficients[0]-c,r,EPS); 00280 break; 00281 case 2: 00282 rCount=Factor(coefficients[2],coefficients[1],coefficients[0]-c,r,EPS); 00283 break; 00284 case 3: 00285 rCount=Factor(coefficients[3],coefficients[2],coefficients[1],coefficients[0]-c,r,EPS); 00286 break; 00287 // case 4: 00288 // rCount=Factor(coefficients[4],coefficients[3],coefficients[2],coefficients[1],coefficients[0]-c,r,EPS); 00289 // break; 00290 default: 00291 printf("Can't solve polynomial of degree: %d\n",Degree); 00292 } 00293 for(int i=0;i<rCount;i++){ 00294 if(fabs(r[i][1])<=EPS){ 00295 roots.push_back(r[i][0]); 00296 } 00297 } 00298 } 00299 template< > 00300 Polynomial< 0 > Polynomial< 0 >::BSplineComponent( int i ) 00301 { 00302 Polynomial p; 00303 p.coefficients[0] = 1.; 00304 return p; 00305 } 00306 template< int Degree > 00307 Polynomial< Degree > Polynomial< Degree >::BSplineComponent( int i ) 00308 { 00309 Polynomial p; 00310 if( i>0 ) 00311 { 00312 Polynomial< Degree > _p = Polynomial< Degree-1 >::BSplineComponent( i-1 ).integral(); 00313 p -= _p; 00314 p.coefficients[0] += _p(1); 00315 } 00316 if( i<Degree ) 00317 { 00318 Polynomial< Degree > _p = Polynomial< Degree-1 >::BSplineComponent( i ).integral(); 00319 p += _p; 00320 } 00321 return p; 00322 } 00323 00324 } 00325 }