00001 #include <boost/math/distributions/chi_squared.hpp>
00002 #include <Eigen/Eigen>
00003
00004
00005
00006
00007
00008 namespace lslgeneric
00009 {
00010
00011 template <typename PointT>
00012 void AdaptiveOctTree<PointT>::setParameters(bool _useDornikHansen,
00013 bool _useFlatness,
00014 double _RSS_THRESHOLD,
00015 double _DH_SIGNIFICANCE_LVL,
00016 double _MIN_CELL_SIZE,
00017 double _FLAT_FACTOR,
00018 double _BIG_CELL_SIZE,
00019 double _SMALL_CELL_SIZE
00020 )
00021 {
00022
00023 this->BIG_CELL_SIZE = _BIG_CELL_SIZE;
00024 this->SMALL_CELL_SIZE = _SMALL_CELL_SIZE;
00025
00026 useDornikHansen = _useDornikHansen;
00027 useFlatness = _useFlatness;
00028 RSS_THRESHOLD = _RSS_THRESHOLD;
00029 DH_SIGNIFICANCE_LVL = _DH_SIGNIFICANCE_LVL;
00030 MIN_CELL_SIZE = _MIN_CELL_SIZE;
00031 FLAT_FACTOR = _FLAT_FACTOR;
00032
00033 parametersSet = true;
00034 }
00037 template <typename PointT>
00038 AdaptiveOctTree<PointT>::AdaptiveOctTree() : OctTree<PointT>()
00039 {
00040 if(!parametersSet)
00041 {
00042 setParameters();
00043 }
00044 }
00045
00048 template <typename PointT>
00049 AdaptiveOctTree<PointT>::AdaptiveOctTree(pcl::PointXYZ center, double xsize, double ysize,
00050 double zsize, NDTCell<PointT>* type, OctTree<PointT> *_parent, unsigned int _depth) :
00051 OctTree<PointT>(center,xsize,ysize,zsize,type,_parent,_depth)
00052 {
00053 if(!parametersSet)
00054 {
00055 setParameters();
00056 }
00057 }
00058
00061 template <typename PointT>
00062 AdaptiveOctTree<PointT>::~AdaptiveOctTree()
00063 {
00064 }
00065
00069 template <typename PointT>
00070 void AdaptiveOctTree<PointT>::computeTreeLeafs()
00071 {
00072 if(this->isLeaf())
00073 {
00074 myTreeLeafs.push_back(this);
00075 return;
00076 }
00077
00078 myTreeLeafs.clear();
00079 std::vector<OctTree<PointT>*> next;
00080 next.push_back(this);
00081
00082 while(next.size()>0)
00083 {
00084 OctTree<PointT> *cur = next.front();
00085 if(cur!=NULL)
00086 {
00087 if(cur->isLeaf())
00088 {
00089 myTreeLeafs.push_back(cur);
00090 }
00091 else
00092 {
00093 for(int i=0; i<8; i++)
00094 {
00095 OctTree<PointT>* tmp = cur->getChild(i);
00096 if(tmp!=NULL)
00097 {
00098 next.push_back(tmp);
00099 }
00100 }
00101 }
00102 }
00103 next.erase(next.begin());
00104 }
00105 }
00106
00110 template <typename PointT>
00111 void AdaptiveOctTree<PointT>::postProcessPoints()
00112 {
00113
00114
00115 computeTreeLeafs();
00116
00117
00118 for(unsigned int i=0; i<myTreeLeafs.size(); i++)
00119 {
00120 NDTCell<PointT> * nd = dynamic_cast<NDTCell<PointT>*>((myTreeLeafs[i])->myCell_);
00121 if(nd == NULL) continue;
00122 if(useDornikHansen)
00123 {
00124 double significance = computeDornikHansen(nd);
00125 if(significance < 0)
00126 {
00127
00128
00129 continue;
00130 }
00131 if(significance < DH_SIGNIFICANCE_LVL)
00132 {
00133
00134 std::vector<OctTree<PointT>*> newLeafs = splitTree(myTreeLeafs[i]);
00135 myTreeLeafs.insert(myTreeLeafs.end(),newLeafs.begin(),newLeafs.end());
00136 }
00137 }
00138 else if(useFlatness)
00139 {
00140 nd->computeGaussian();
00141 if(!nd->hasGaussian_)
00142 {
00143 continue;
00144 }
00145
00146 Eigen::Vector3d evals = nd->getEvals();
00147 int idMin, idMax;
00148 double minEval = evals.minCoeff(&idMin);
00149 double maxEval = evals.maxCoeff(&idMax);
00150 int idMiddle = -1;
00151 for(int j=0; j<3; j++)
00152 {
00153 if(j!=idMin && j!=idMax)
00154 {
00155 idMiddle = j;
00156 }
00157 }
00158 if(idMiddle < 0) continue;
00159
00160 if(minEval*FLAT_FACTOR > evals(idMiddle))
00161 {
00162 std::vector<OctTree<PointT>*> newLeafs = splitTree(myTreeLeafs[i]);
00163 myTreeLeafs.insert(myTreeLeafs.end(),newLeafs.begin(),newLeafs.end());
00164 }
00165
00166
00167 }
00168 else
00169 {
00170 double rss = computeResidualSquare(nd);
00171 if(rss > RSS_THRESHOLD)
00172 {
00173
00174 std::vector<OctTree<PointT>*> newLeafs = splitTree(myTreeLeafs[i]);
00175 myTreeLeafs.insert(myTreeLeafs.end(),newLeafs.begin(),newLeafs.end());
00176 }
00177 }
00178 }
00179
00180 this->leafsCached_ = false;
00181 }
00182
00186 template <typename PointT>
00187 double AdaptiveOctTree<PointT>::computeDornikHansen(NDTCell<PointT> *cell)
00188 {
00189 double pval = 1;
00190 double Ep = 0;
00191
00192 if(cell->points_.size() <= 7) return -1;
00193 cell->computeGaussian();
00194
00195
00196 boost::math::chi_squared dist(6);
00197
00198 Eigen::Vector3d mean = cell->getMean();
00199 Eigen::Matrix3d C = cell->getCov();
00200
00201 Eigen::MatrixXd Xhat(cell->points_.size(),3);
00202
00203 for(unsigned int i=0; i< cell->points_.size(); i++)
00204 {
00205 Xhat(i,0) = cell->points_[i].x - mean(0);
00206 Xhat(i,1) = cell->points_[i].y - mean(1);
00207 Xhat(i,2) = cell->points_[i].z - mean(2);
00208 }
00209
00210
00211 Eigen::Matrix3d V;
00212 Eigen::Matrix3d H;
00213 Eigen::Vector3d lambda;
00214 Eigen::Matrix3d L;
00215 Eigen::Matrix3d R1;
00216
00217 Eigen::MatrixXd R;
00218
00219 V(0,0) = 1/sqrt(C(0,0));
00220 V(1,1) = 1/sqrt(C(1,1));
00221 V(2,2) = 1/sqrt(C(2,2));
00222
00223 C = V*C*V;
00224 Eigen::EigenSolver<Eigen::Matrix3d> eig(C);
00225 H = eig.eigenvectors().real();
00226 lambda = eig.eigenvalues().real();
00227
00228
00229 if(lambda.minCoeff() <= 0) return -1;
00230
00231 L(0,0) = 1/sqrt(lambda(0));
00232 L(1,1) = 1/sqrt(lambda(1));
00233 L(2,2) = 1/sqrt(lambda(2));
00234
00235
00236 R1 = H*L*H.transpose()*V;
00237 R = R1*Xhat.transpose();
00238
00239
00240
00241 Ep = 0.0;
00242 double n = R.cols();
00243 for(unsigned int dim = 0; dim < 3; dim ++)
00244 {
00245 double m2 = 0, m3 = 0, m4 =0;
00246 double b1 = 0, b2;
00247 double beta, omega2, gamma, y;
00248 double gamma2, a,c, k, alpha, chi;
00249 double z1,z2;
00250
00251 for(unsigned int i=0; i<R.cols(); i++)
00252 {
00253 m2 += pow(R(dim,i),2);
00254 m3 += pow(R(dim,i),3);
00255 m4 += pow(R(dim,i),4);
00256 }
00257 m2 /= R.cols();
00258 m3 /= R.cols();
00259 m4 /= R.cols();
00260
00261 b1 = m3/(pow(m2,1.5));
00262 b2 = m4/(pow(m2,2));
00263
00264
00265
00266
00267 beta = 3*(n*n + 27*n -70)*(n+1)*(n+3)/((n-2)*(n+5)*(n+7)*(n+9));
00268 omega2 = -1+sqrt(2*(beta-1));
00269 gamma = 1/sqrt(log(sqrt(omega2)));
00270 y = b1*sqrt((omega2-1)*(n+1)*(n+3)/(12*(n-2)));
00271 z1 = gamma*log(y+sqrt(y*y+1));
00272
00273 gamma2 = (n-3)*(n+1)*(n*n+15*n-4);
00274 a = (n-2)*(n+5)*(n+7)*(n*n+27*n-70)/(6*gamma2);
00275 c = (n-7)*(n+5)*(n+7)*(n*n+2*n-5)/(6*gamma2);
00276 k = (n+5)*(n+7)*(pow(n,3)+37*n*n+11*n-313)/(12*gamma2);
00277 alpha = a + b1*b1*c;
00278 chi = (b2-1-b1*b1)*2*k;
00279 z2 = (pow((chi/(2*alpha)),(1./3.))-1+(1./(9*alpha)))*sqrt(9*alpha);
00280
00281
00282
00283
00284 Ep += z1*z1 + z2*z2;
00285 }
00286
00287
00288 pval = 1-boost::math::cdf(dist,Ep);
00289
00290
00291
00292 if(pval>DH_SIGNIFICANCE_LVL)
00293 {
00294 double dx,dy,dz;
00295 cell->getDimensions(dx,dy,dz);
00296 std::cout<<"final split was at ("<<dx<<","<<dy<<","<<dz<<"); pval is "<<pval<<std::endl;
00297 }
00298 return 0;
00299
00300 }
00304 template <typename PointT>
00305 double AdaptiveOctTree<PointT>::computeResidualSquare(NDTCell<PointT> *cell)
00306 {
00307 double rss = 0;
00308 double meanResidual = 0;
00309 double residualVar = 0;
00310 if(cell->points_.size() <= 3) return 0;
00311
00312 cell->computeGaussian();
00313
00314 Eigen::Vector3d cur, curProd;
00315 Eigen::Matrix3d cov = cell->getCov();
00316 Eigen::Vector3d mean = cell->getMean();
00317
00318
00319 for(unsigned int i=0; i< cell->points_.size(); i++)
00320 {
00321
00322 cur(0) = cell->points_[i].x - mean(0);
00323 cur(1) = cell->points_[i].y - mean(1);
00324 cur(2) = cell->points_[i].z - mean(2);
00325
00326
00327
00328 rss += cur.dot(cur);
00329 meanResidual += cur.norm()/cell->points_.size();
00330 }
00331
00332 for(unsigned int i=0; i< cell->points_.size(); i++)
00333 {
00334 cur(0) = cell->points_[i].x - mean(0);
00335 cur(1) = cell->points_[i].y - mean(1);
00336 cur(2) = cell->points_[i].z - mean(2);
00337
00338 residualVar += pow(cur.norm()-meanResidual,2)/(cell->points_.size()-1);
00339 }
00340 double bic = rss/residualVar + log(cell->points_.size());
00341
00342 return bic;
00343 }
00344
00349 template <typename PointT>
00350 std::vector<OctTree<PointT>*> AdaptiveOctTree<PointT>::splitTree(OctTree<PointT> *octLeaf)
00351 {
00352 std::vector<OctTree<PointT>*> newLeafs;
00353
00354 if(octLeaf->isLeaf())
00355 {
00356 double xs,ys,zs;
00357 octLeaf->myCell_->getDimensions(xs,ys,zs);
00358
00359 double cellSize = (xs+ys+zs)/3.;
00360
00361 if(octLeaf->depth_ > this->MAX_DEPTH || cellSize <= this->MIN_CELL_SIZE )
00362 {
00363
00364 return newLeafs;
00365 }
00366
00367 pcl::PointXYZ myCenter = octLeaf->myCell_->getCenter();
00368
00369
00370 for(unsigned int it=0; it<8; it++)
00371 {
00372
00373 pcl::PointXYZ newCenter;
00374
00375
00376 newCenter.x = (myCenter.x + pow(-1.,it/4)*xs/4.);
00377 newCenter.y = (myCenter.y + pow(-1.,it/2)*ys/4.);
00378 newCenter.z = (myCenter.z + pow(-1.,(it+1)/2)*zs/4.);
00379
00380 octLeaf->children_[it] = new OctTree<PointT>(newCenter,xs/2,ys/2,
00381 zs/2, octLeaf->myCell_, this, this->depth_+1);
00382 newLeafs.push_back(octLeaf->children_[it]);
00383 }
00384
00385 for(unsigned int jt=0; jt<octLeaf->myCell_->points_.size(); jt++)
00386 {
00387 size_t ind = octLeaf->getIndexForPoint(octLeaf->myCell_->points_[jt]);
00388 octLeaf->children_[ind]->addPoint(octLeaf->myCell_->points_[jt]);
00389 }
00390 octLeaf->leaf_=false;
00391 octLeaf->myCell_->points_.clear();
00392 }
00393
00394 return newLeafs;
00395 }
00396
00401 template <typename PointT>
00402 SpatialIndex<PointT>* AdaptiveOctTree<PointT>::clone()
00403 {
00404 if(this->myCell_ == NULL)
00405 {
00406 return new AdaptiveOctTree();
00407 }
00408 double sx,sy,sz;
00409 this->myCell_->getDimensions(sx,sy,sz);
00410 AdaptiveOctTree<PointT> *tr = new AdaptiveOctTree<PointT>(this->myCell_->getCenter(),sx,sy,sz,this->myCell_);
00411 return tr;
00412 }
00413
00414 }