ndt_occupancy_map.hpp
Go to the documentation of this file.
00001 #include <Eigen/Eigen>
00002 #include <pcl/point_cloud.h>
00003 #include <pcl/features/feature.h>
00004 
00005 #include <string>
00006 #include <climits>
00007 
00008 #include<ndt_map/oc_tree.h>
00009 #include<ndt_map/lazy_grid.h>
00010 #include<ndt_map/cell_vector.h>
00011 
00012 namespace lslgeneric
00013 {
00014 
00017 template<typename PointT>
00018 void NDTOccupancyMap<PointT>::loadPointCloud(const Eigen::Vector3d &origin, const pcl::PointCloud<PointT> &pc)
00019 {
00020     if(index_ != NULL)
00021     {
00022         //std::cout<<"CLONE INDEX\n";
00023         SpatialIndex<PointT> *si = index_->clone();
00024         //cout<<"allocating index\n";
00025         if(!isFirstLoad_)
00026         {
00027             //std::cout<<"deleting old index\n";
00028             delete index_;
00029         }
00030         isFirstLoad_ = false;
00031         index_ = si;
00032     }
00033     else
00034     {
00035         //NULL index in constructor, abort!
00036         //ERR("constructor must specify a non-NULL spatial index\n");
00037         return;
00038     }
00039 
00040     if(index_ == NULL)
00041     {
00042         //ERR("Problem creating index, unimplemented method\n");
00043         return;
00044     }
00045 
00046     double maxDist = 0;//, distCeil = 200;
00047 
00048     typename pcl::PointCloud<PointT>::const_iterator it = pc.points.begin();
00049     Eigen::Vector3d centroid(0,0,0);
00050     int npts = 0;
00051     while(it!=pc.points.end())
00052     {
00053         Eigen::Vector3d d;
00054         if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
00055         {
00056             it++;
00057             continue;
00058         }
00059         d << it->x, it->y, it->z;
00060         centroid += d;
00061         it++;
00062         npts++;
00063     }
00064 
00065     centroid /= (double)npts;
00066 
00067     //Eigen::Vector4f centroid(0,0,0,0);
00068     //pcl::compute3DCentroid(pc,centroid);
00069 
00070     //compute distance to furthest point
00071     it = pc.points.begin();
00072     double maxz=0, minz=10000;
00073 
00074     while(it!=pc.points.end())
00075     {
00076         Eigen::Vector3d d;
00077         if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
00078         {
00079             it++;
00080             continue;
00081         }
00082         d << centroid(0)-it->x, centroid(1)-it->y, centroid(2)-it->z;
00083         double dist = d.norm();
00084         maxDist = (dist > maxDist) ? dist : maxDist;
00085         maxz = ((centroid(2)-it->z) > maxz) ? (centroid(2)-it->z) : maxz;
00086         minz = ((centroid(2)-it->z) < minz) ? (centroid(2)-it->z) : minz;
00087         it++;
00088     }
00089     fprintf(stderr,"minz=%lf maxz=%lf diff=%lf",minz,maxz,maxz-minz);
00090 
00091     // cout<<"Points = " <<pc.points.size()<<" maxDist = "<<maxDist<<endl;
00092 
00093     NDTCell<PointT> *ptCell = new NDTCell<PointT>();
00094     index_->setCellType(ptCell);
00095     delete ptCell;
00096 
00097     index_->setCenter(centroid(0),centroid(1),centroid(2));
00098     index_->setSize(3*maxDist,3*maxDist,2*(maxz-minz));
00099 
00100     /*
00101                 LazyGrid<PointT> *gr = dynamic_cast<LazyGrid<PointT>*>(index_);
00102                 if(gr!=NULL) {
00103                         fprintf(stderr,"Lazy Grid initialization!!\n");
00104                         gr->initializeAll();
00105                         fprintf(stderr,"Done!");
00106                 }else{
00107                         fprintf(stderr,"ERROR: This implementation is only for the Lazy Grid - Sorry!");
00108                         exit(1);
00109                 }
00110     */
00111 
00112 //    ROS_INFO("centroid is %f,%f,%f", centroid(0),centroid(1),centroid(2));
00113 //    ROS_INFO("maxDist is %lf", maxDist);
00114 
00115     it = pc.points.begin();
00116     while(it!=pc.points.end())
00117     {
00118         if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
00119         {
00120             it++;
00121             continue;
00122         }
00123 
00124         Eigen::Vector3d diff;
00125         diff << it->x-origin(0), it->y-origin(1), it->z-origin(2);
00126         double l = diff.norm();
00127         unsigned int N = l / resolution;
00128 
00129         diff = diff/(float)N;
00130         PointT pt;
00131         for(unsigned int i=0; i<N; i++)
00132         {
00133             pt.x = origin(0) + ((float)(i+1)) *diff(0);
00134             pt.y = origin(1) + ((float)(i+1)) *diff(1);
00135             pt.z = origin(2) + ((float)(i+1)) *diff(2);
00136             //fprintf(stderr,"Requesting point for (%.3f %.3f %.3f) target = (%.3f %.3f %.3f).... ",pt.x,pt.y,pt.z,it->x, it->y, it->z );
00137             NDTCell<PointT> *ptCell = dynamic_cast<NDTCell<PointT> *> (index_->getCellForPoint(pt));
00138             //fprintf(stderr,"Done\n");
00139             if(ptCell != NULL)
00140             {
00141                 //fprintf(stderr,"Updating point for (%.3f %.3f %.3f) target = (%.3f %.3f %.3f).... ",pt.x,pt.y,pt.z,it->x, it->y, it->z );
00142                 ptCell->updateEmpty();
00143             }
00144             else
00145             {
00146                 //fprintf(stderr,"The cell is NULL\n");
00147             }
00148         }
00149         //pt.x = it->x; pt.y = it->y; pt.z = it->z;
00150         //NDTCell<PointT> *ptCell = dynamic_cast<NDTCell<PointT> *> (index_->getCellForPoint(pt));
00151         //ptCell->updateOccupancy(0.5);
00152 
00153 
00154         index_->addPoint(*it);
00155         it++;
00156     }
00157 
00158     isFirstLoad_ = false;
00159 }
00160 
00161 
00166 template<typename PointT>
00167 void NDTOccupancyMap<PointT>::addPointCloud(const Eigen::Vector3d &origin, const pcl::PointCloud<PointT> &pc)
00168 {
00169     if(isFirstLoad_)
00170     {
00171         loadPointCloud(origin, pc);
00172     }
00173 
00174     if(index_ == NULL)
00175     {
00176         //ERR("Problem creating index, unimplemented method\n");
00177         return;
00178     }
00179     typename pcl::PointCloud<PointT>::const_iterator it = pc.points.begin();
00180     it = pc.points.begin();
00181     while(it!=pc.points.end())
00182     {
00183         if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
00184         {
00185             it++;
00186             continue;
00187         }
00188         Eigen::Vector3d diff;
00189         diff << it->x-origin(0), it->y-origin(1), it->z-origin(2);
00190         double l = diff.norm();
00191         unsigned int N = l / (resolution);
00192 
00193         diff = diff/(float)N;
00194         PointT pt;
00195         for(unsigned int i=0; i<N; i++)
00196         {
00197             pt.x = origin(0) + ((float)(i+1)) *diff(0);
00198             pt.y = origin(1) + ((float)(i+1)) *diff(1);
00199             pt.z = origin(2) + ((float)(i+1)) *diff(2);
00200             NDTCell<PointT> *ptCell = dynamic_cast<NDTCell<PointT> *> (index_->getCellForPoint(pt));
00201 
00202             if(ptCell != NULL)
00203             {
00204                 //fprintf(stderr,"Updating point for (%.3f %.3f %.3f) target = (%.3f %.3f %.3f).... ",pt.x,pt.y,pt.z,it->x, it->y, it->z );
00205                 ptCell->updateEmpty();
00206             }
00207             else
00208             {
00209                 //fprintf(stderr,"The cell is NULL\n");
00210             }
00211         }
00212 
00213 
00214         index_->addPoint(*it);
00215         it++;
00216     }
00217     isFirstLoad_ = false;
00218 }
00219 
00220 
00221 
00222 
00223 
00224 template<typename PointT>
00225 void NDTOccupancyMap<PointT>::loadPointCloud(const pcl::PointCloud<PointT> &pc, const std::vector<std::vector<size_t> > &indices)
00226 {
00227 
00228     loadPointCloud(pc);
00229     // Specific function related to CellVector
00230     CellVector<PointT> *cl = dynamic_cast<CellVector<PointT>*>(index_);
00231     if (cl != NULL)
00232     {
00233         for (size_t i = 0; i < indices.size(); i++)
00234         {
00235             cl->addCellPoints(pc, indices[i]);
00236         }
00237 
00238     }
00239     else
00240     {
00241         //ERR("loading point clouds using indices are currently supported in CellVector index_.");
00242     }
00243 }
00244 
00245 template<typename PointT>
00246 void NDTOccupancyMap<PointT>::loadDepthImage(const cv::Mat& depthImage, DepthCamera<PointT> &cameraParams)
00247 {
00248     pcl::PointCloud<PointT> pc;
00249     cameraParams.convertDepthImageToPointCloud(depthImage, pc);
00250     this->loadPointCloud(pc);
00251 }
00252 
00253 template<typename PointT>
00254 pcl::PointCloud<PointT> NDTOccupancyMap<PointT>::loadDepthImageFeatures(const cv::Mat& depthImage, std::vector<cv::KeyPoint> &keypoints,
00255         size_t &supportSize, double maxVar, DepthCamera<PointT> &cameraParams, bool estimateParamsDI, bool nonMean)
00256 {
00257     std::vector<cv::KeyPoint> good_keypoints;
00258     Eigen::Vector3d mean;
00259     PointT mn;
00260     pcl::PointCloud<PointT> cloudOut;
00261     CellVector<PointT> *cl = dynamic_cast<CellVector<PointT>*>(index_);
00262     if(cl==NULL)
00263     {
00264         std::cerr<<"wrong index type!\n";
00265         return cloudOut;
00266     }
00267     for (size_t i=0; i<keypoints.size(); i++)
00268     {
00269         if(!estimateParamsDI)
00270         {
00271             pcl::PointCloud<PointT> points;
00272             PointT center;
00273             cameraParams.computePointsAtIndex(depthImage,keypoints[i],supportSize,points,center);
00274             NDTCell<PointT> *ndcell = new NDTCell<PointT>();
00275             typename pcl::PointCloud<PointT>::iterator it = points.points.begin();
00276             while (it!= points.points.end() )
00277             {
00278                 if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
00279                 {
00280                     it++;
00281                     continue;
00282                 }
00283                 ndcell->addPoint(*it);
00284                 it++;
00285             }
00286             ndcell->computeGaussian();
00287             if(ndcell->hasGaussian_)
00288             {
00289                 Eigen::Vector3d evals = ndcell->getEvals();
00290                 if(sqrt(evals(2)) < maxVar)
00291                 {
00292                     if (nonMean)
00293                     {
00294                         if(std::isnan(center.x) ||std::isnan(center.y) ||std::isnan(center.z))
00295                         {
00296                             continue;
00297                         }
00298                         mn = center;
00299                     }
00300                     else
00301                     {
00302                         mean = ndcell->getMean();
00303                         mn.x = mean(0);
00304                         mn.y = mean(1);
00305                         mn.z = mean(2);
00306                     }
00307                     cloudOut.points.push_back(mn);
00308                     ndcell->setCenter(mn);
00309                     cl->addCell(ndcell);
00310                     good_keypoints.push_back(keypoints[i]);
00311                 }
00312             }
00313         }
00314         else
00315         {
00316             assert(nonMean = false); // Not implemented / used.
00317             Eigen::Vector3d mean;
00318             Eigen::Matrix3d cov;
00319             cameraParams.computeParamsAtIndex(depthImage,keypoints[i],supportSize,mean,cov);
00320             NDTCell<PointT> *ndcell = new NDTCell<PointT>();
00321             ndcell->setMean(mean);
00322             ndcell->setCov(cov);
00323 
00324             if(ndcell->hasGaussian_)
00325             {
00326                 Eigen::Vector3d evals = ndcell->getEvals();
00327                 //std::cout<<evals.transpose()<<std::endl;
00328                 if(sqrt(evals(2)) < maxVar)
00329                 {
00330                     mean = ndcell->getMean();
00331                     mn.x = mean(0);
00332                     mn.y = mean(1);
00333                     mn.z = mean(2);
00334                     cloudOut.points.push_back(mn);
00335                     ndcell->setCenter(mn);
00336                     cl->addCell(ndcell);
00337                     good_keypoints.push_back(keypoints[i]);
00338                 }
00339             }
00340 
00341         }
00342     }
00343 
00344     //TODO
00345     keypoints = good_keypoints;
00346     return cloudOut;
00347 }
00348 
00351 template<typename PointT>
00352 void NDTOccupancyMap<PointT>::computeNDTCells(int cellupdatemode)
00353 {
00354     CellVector<PointT> *cv = dynamic_cast<CellVector<PointT>*>(index_);
00355 
00356     typename SpatialIndex<PointT>::CellVectorItr it = index_->begin();
00357 
00358     while (it != index_->end())
00359     {
00360         NDTCell<PointT> *cell = dynamic_cast<NDTCell<PointT>*> (*it);
00361         //fprintf(stderr,"*");
00362         if(cell!=NULL)
00363         {
00364             //fprintf(stderr,"Something is not null!\n");
00365             cell->computeGaussian(cellupdatemode);
00366             if (cv!=NULL)
00367             {
00368                 // Set the mean to the cell's centre.
00369                 Eigen::Vector3d mean = cell->getMean();
00370                 //cout << "mean : " << mean << std::endl;
00371                 PointT pt;
00372                 pt.x = mean[0];
00373                 pt.y = mean[1];
00374                 pt.z = mean[2];
00375 
00376                 cell->setCenter(pt);
00377             }
00378         }
00379         else
00380         {
00381             //ERR("problem casting cell to NDT!\n");
00382         }
00383         it++;
00384     }
00385 
00386     LazyGrid<PointT> *lz = dynamic_cast<LazyGrid<PointT>*>(index_);
00387     if(lz!=NULL)
00388     {
00389         lz->initKDTree();
00390     }
00391 
00392     CellVector<PointT> *cl = dynamic_cast<CellVector<PointT>*>(index_);
00393     if(cl!=NULL)
00394     {
00395         cl->initKDTree();
00396     }
00397 }
00398 
00401 template<typename PointT>
00402 void NDTOccupancyMap<PointT>::writeToVRML(const char* filename)
00403 {
00404 
00405     if(filename == NULL)
00406     {
00407         //ERR("problem outputing to vrml\n");
00408         return;
00409     }
00410     /*
00411        std::string fn(filename);
00412        fn = "oct_"+fn;
00413 
00414        FILE *fo = fopen(fn.c_str(),"w");
00415        if(fo == NULL) {
00416        ERR("problem outputing to vrml\n");
00417        return;
00418        }
00419        fprintf(fo,"#VRML V2.0 utf8\n");
00420        writeToVRML(fo,true);
00421        fclose(fo);
00422      */
00423 
00424     FILE *fout = fopen(filename,"w");
00425     if(fout == NULL)
00426     {
00427         //ERR("problem outputing to vrml\n");
00428         return;
00429     }
00430     fprintf(fout,"#VRML V2.0 utf8\n");
00431     writeToVRML(fout);
00432     fclose(fout);
00433 }
00434 
00437 template<typename PointT>
00438 void NDTOccupancyMap<PointT>::writeToVRML(FILE* fout)
00439 {
00440     if(fout == NULL)
00441     {
00442         //ERR("problem outputing to vrml\n");
00443         return;
00444     }
00445 
00446     //move the ellipsoid stuff to NDTCell
00447     typename SpatialIndex<PointT>::CellVectorItr it = index_->begin();
00448     while (it != index_->end())
00449     {
00450         NDTCell<PointT> *cell = dynamic_cast<NDTCell<PointT>*> (*it);
00451         //      double xs,ys,zs;
00452         if(cell!=NULL)
00453         {
00454             cell->writeToVRML(fout);
00455         }
00456         else
00457         {
00458             //      ERR("problem casting cell to NDT!\n");
00459         }
00460         it++;
00461     }
00462 
00463 }
00464 template<typename PointT>
00465 void NDTOccupancyMap<PointT>::writeToVRML(FILE* fout, Eigen::Vector3d col)
00466 {
00467     if(fout == NULL)
00468     {
00469         //ERR("problem outputing to vrml\n");
00470         return;
00471     }
00472 
00473     //move the ellipsoid stuff to NDTCell
00474     typename SpatialIndex<PointT>::CellVectorItr it = index_->begin();
00475     while (it != index_->end())
00476     {
00477         NDTCell<PointT> *cell = dynamic_cast<NDTCell<PointT>*> (*it);
00478         if(cell!=NULL)
00479         {
00480             cell->writeToVRML(fout,col);
00481         }
00482         else
00483         {
00484         }
00485         it++;
00486     }
00487 }
00488 
00490 template<typename PointT>
00491 std::string NDTOccupancyMap<PointT>::getMyIndexStr() const
00492 {
00493     CellVector<PointT>* cl = dynamic_cast<CellVector<PointT> * >(index_);
00494     if(cl!=NULL)
00495     {
00496         return std::string("CellVector");
00497     }
00498     OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);
00499     if(tr!=NULL)
00500     {
00501         return std::string("OctTree");
00502     }
00503     LazyGrid<PointT> *gr = dynamic_cast<LazyGrid<PointT>*>(index_);
00504     if(gr!=NULL)
00505     {
00506         return std::string("LazyGrid<PointT>");
00507     }
00508 
00509     return std::string("Unknown index type");
00510 }
00511 
00512 //computes the *negative log likelihood* of a single observation
00513 template<typename PointT>
00514 double NDTOccupancyMap<PointT>::getLikelihoodForPoint(PointT pt)
00515 {
00516     //assert(false);
00517     double uniform=0.00100;
00518     NDTCell<PointT>* ndCell = NULL;
00519     OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);
00520 
00521     if(tr==NULL)
00522     {
00523         LazyGrid<PointT> *gr = dynamic_cast<LazyGrid<PointT>*>(index_);
00524         if(gr==NULL)
00525         {
00526             //cout<<"bad index - getLikelihoodForPoint\n";
00527             return uniform;
00528         }
00529         ndCell = gr->getClosestNDTCell(pt);
00530     }
00531     else
00532     {
00533         ndCell = tr->getClosestNDTCell(pt);
00534     }
00535     if(ndCell == NULL) return uniform;
00536 
00537     double prob = ndCell->getLikelihood(pt);
00538     prob = (prob<0) ? 0 : prob; //uniform!! TSV
00539     return prob;
00540 }
00541 
00542 /*
00543 //use trilinear interpolation from available immediate neighbors
00544 template<typename PointT>
00545 double NDTOccupancyMap<PointT>::getLikelihoodForPointWithInterpolation(PointT pt) {
00546 
00547     //ATM only for grid map
00548     //     tll------tlr
00549     //     /|       /|
00550     //    / |      / |
00551     //  tul------tur |    z
00552     //   | bll----|-blr   ^  y
00553     //   | /      | /     | /
00554     //   |/       |/      |/
00555     //  bul------bur      ---> x
00556     double uniform=0;//0.00100;
00557     Cell* cell = NULL;
00558     NDTCell<PointT>* ndCell = NULL;
00559     double cumProb = 0;
00560     double weight = 0;
00561     int evals = 1;
00562 
00563     LazyGrid<PointT> *gr = dynamic_cast<LazyGrid<PointT>*>(index_);
00564     if(gr==NULL) {
00565         //cout<<"bad index - getLikelihoodForPointWithInterpolation\n";
00566         return uniform;
00567     }
00568     cell = gr->getCellForPoint(pt);
00569     if(cell == NULL) return uniform;
00570 
00571 
00572     //get coordinates of cell
00573     int indXn, indYn, indZn;
00574     PointT centerGrid, sizeCell, centerCell;
00575     int sizeGridX, sizeGridY,sizeGridZ;
00576     centerCell = cell->getCenter();
00577     gr->getCenter(centerGrid.x,centerGrid.y,centerGrid.z);
00578     gr->getGridSize(sizeGridX,sizeGridY,sizeGridZ);
00579     gr->getCellSize(sizeCell.x,sizeCell.y,sizeCell.z);
00580     gr->getIndexForPoint(pt,indXn,indYn,indZn);
00581 
00582     double x,y,z;
00583     x = (pt.x - centerCell.x)/sizeCell.x;
00584     y = (pt.y - centerCell.y)/sizeCell.y;
00585     z = (pt.z - centerCell.z)/sizeCell.z;
00586     if(x <0 ) x = 0;
00587     if(y <0 ) y = 0;
00588     if(z <0 ) z = 0;
00589     if(x >1 ) x = 1;
00590     if(y >1 ) y = 1;
00591     if(z >1 ) z = 1;
00592 
00593     //bul
00594     double prob = 0;
00595     ndCell = dynamic_cast<NDTCell<PointT>*> (cell);
00596     if(ndCell != NULL) {
00597         prob = ndCell->getLikelihood(pt);
00598         prob = (prob<0) ? uniform : prob;
00599         weight = (1 - x + 1 - y + 1 - z)/(3.0);
00600         if(weight < 0) cerr<<weight<<endl;
00601         cumProb += prob*weight;
00602         //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
00603         evals++;
00604     }
00605 
00606     //tul
00607     Cell* c = gr->getCellAt(indXn,indYn,indZn+1);
00608     if(c != NULL) {
00609         ndCell = dynamic_cast<NDTCell<PointT>*> (c);
00610         if(ndCell != NULL) {
00611             prob = ndCell->getLikelihood(pt);
00612             prob = (prob<0) ? uniform : prob;
00613             weight = (1 - x + 1 - y + z)/(3.0);
00614             if(weight < 0) cerr<<weight<<endl;
00615             cumProb += prob*weight;
00616             //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
00617             evals++;
00618         }
00619     }
00620     //tur
00621     c = gr->getCellAt(indXn+1,indYn,indZn+1);
00622     if(c != NULL) {
00623         ndCell = dynamic_cast<NDTCell<PointT>*> (c);
00624         if(ndCell != NULL) {
00625             prob = ndCell->getLikelihood(pt);
00626             prob = (prob<0) ? uniform : prob;
00627             weight = (x + 1-y + z )/(3.0);
00628             if(weight < 0) cerr<<weight<<endl;
00629             cumProb += prob*weight;
00630             //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
00631             evals++;
00632         }
00633     }
00634     //tll
00635     c = gr->getCellAt(indXn,indYn+1,indZn+1);
00636     if(c != NULL) {
00637         ndCell = dynamic_cast<NDTCell<PointT>*> (c);
00638         if(ndCell != NULL) {
00639             prob = ndCell->getLikelihood(pt);
00640             prob = (prob<0) ? uniform : prob;
00641             weight = (1-x + y + z )/(3.0);
00642             if(weight < 0) cerr<<weight<<endl;
00643             cumProb += prob*weight;
00644             //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
00645             evals++;
00646         }
00647     }
00648     //tlr
00649     c = gr->getCellAt(indXn+1,indYn+1,indZn+1);
00650     if(c != NULL) {
00651         ndCell = dynamic_cast<NDTCell<PointT>*> (c);
00652         if(ndCell != NULL) {
00653             prob = ndCell->getLikelihood(pt);
00654             prob = (prob<0) ? uniform : prob;
00655             weight = (x + y + z )/(3.0);
00656             if(weight < 0) cerr<<weight<<endl;
00657             cumProb += prob*weight;
00658             //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
00659             evals++;
00660         }
00661     }
00662     //bur
00663     c = gr->getCellAt(indXn+1,indYn,indZn);
00664     if(c != NULL) {
00665         ndCell = dynamic_cast<NDTCell<PointT>*> (c);
00666         if(ndCell != NULL) {
00667             prob = ndCell->getLikelihood(pt);
00668             prob = (prob<0) ? uniform : prob;
00669             weight = (x + 1-y + 1-z )/(3.0);
00670             if(weight < 0) cerr<<weight<<endl;
00671             cumProb += prob*weight;
00672             //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
00673             evals++;
00674         }
00675     }
00676     //bll
00677     c = gr->getCellAt(indXn,indYn+1,indZn);
00678     if(c != NULL) {
00679         ndCell = dynamic_cast<NDTCell<PointT>*> (c);
00680         if(ndCell != NULL) {
00681             prob = ndCell->getLikelihood(pt);
00682             prob = (prob<0) ? uniform : prob;
00683             weight = (1-x + y + 1-z )/(3.0);
00684             if(weight < 0) cerr<<weight<<endl;
00685             cumProb += prob*weight;
00686             //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
00687             evals++;
00688         }
00689     }
00690     //blr
00691     c = gr->getCellAt(indXn+1,indYn+1,indZn);
00692     if(c != NULL) {
00693         ndCell = dynamic_cast<NDTCell<PointT>*> (c);
00694         if(ndCell != NULL) {
00695             prob = ndCell->getLikelihood(pt);
00696             prob = (prob<0) ? uniform : prob;
00697             weight = (x + y + 1-z )/(3.0);
00698             if(weight < 0) cerr<<weight<<endl;
00699             cumProb += prob*weight;
00700             //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
00701             evals++;
00702         }
00703     }
00704 
00705     //cout<<"== "<<cumProb<<endl;
00706     return cumProb;
00707 }
00708 */
00709 
00710 template<typename PointT>
00711 std::vector<NDTCell<PointT>*> NDTOccupancyMap<PointT>::getCellsForPoint(const PointT pt, double radius)
00712 {
00713     //assert(false);
00714     std::vector<NDTCell<PointT>*> cells;
00715     OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);
00716     if(tr==NULL)
00717     {
00718         LazyGrid<PointT> *gr = dynamic_cast<LazyGrid<PointT>*>(index_);
00719         if(gr==NULL)
00720         {
00721             //cout<<"bad index - getCellsForPoint\n";
00722             return cells;
00723         }
00724         cells = gr->getClosestNDTCells(pt,radius);
00725         return cells;
00726     }
00727     //TODO:implement for ocTree
00728     return cells;
00729 }
00730 
00731 template<typename PointT>
00732 bool NDTOccupancyMap<PointT>::getCellForPoint(const PointT &pt, NDTCell<PointT> *&out_cell)
00733 {
00734 
00735 #if 0
00736     OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index);
00737     if(tr==NULL)
00738     {
00739         LazyGrid<PointT> *gr = dynamic_cast<LazyGrid<PointT>*>(index);
00740         if(gr==NULL)
00741         {
00742             CellVector *cl = dynamic_cast<CellVector*>(index);
00743             if(cl==NULL)
00744             {
00745                 cout<<"bad index - getCellForPoint\n";
00746                 return false;
00747             }
00748             out_cell = cl->getClosestNDTCell(pt);
00749             return true;
00750         }
00751         out_cell = gr->getClosestNDTCell(pt);
00752         return true;
00753     }
00754     out_cell = tr->getClosestNDTCell(pt);
00755     return true;
00756 #endif
00757     CellVector<PointT> *cl = dynamic_cast<CellVector<PointT>*>(index_);
00758     if(cl!=NULL)
00759     {
00760         out_cell = cl->getClosestNDTCell(pt);
00761         return true;
00762     }
00763     OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);
00764     if(tr!=NULL)
00765     {
00766         out_cell = tr->getClosestNDTCell(pt);
00767         return true;
00768     }
00769 
00770     LazyGrid<PointT> *gr = dynamic_cast<LazyGrid<PointT>*>(index_);
00771     if(gr!=NULL)
00772     {
00773         //fprintf(stderr,"Lazy!!\n");
00774         out_cell = gr->getClosestNDTCell(pt);
00775         return true;
00776     }
00777     //cout<<"bad index - getCellForPoint\n";
00778     return false;
00779 
00780     /*
00781     if(c==NULL) {
00782     //we have to find the closest leaf cell
00783     c = tr->getClosestLeafCell(pt);
00784     }
00785     if(c==NULL) {
00786     cout<<"null cell\n";
00787     return false;
00788     }
00789     NDTCell* ndCell = dynamic_cast<NDTCell*>(c);
00790     if(ndCell == NULL) {
00791     cout<<"not ndt cell\n";
00792     return false;
00793     }
00794     if(!ndCell->hasGaussian) {
00795     Eigen::Vector3d dim;
00796     ndCell->getDimensions(dim(0),dim(1),dim(2));
00797     typename SpatialIndex<PointT>::CellVectorItr neigh;
00798     index->getNeighbors(pt,20*dim.norm(),neigh);
00799     double minDist = INT_MAX;
00800     cout<<"neighs "<<neigh.size()<<endl;
00801     for(int i=0; i<neigh.size(); i++) {
00802         NDTCell *n = dynamic_cast<NDTCell*>(neigh[i]);
00803         if(n==NULL) continue;
00804         if(n->hasGaussian) {
00805         double d = lslgeneric::geomDist(n->getCenter(),pt);
00806         if(d<minDist) {
00807             ndCell = n;
00808         }
00809         }
00810     }
00811     if(!ndCell->hasGaussian) {
00812         cout<<"no gaussian\n";
00813         return false;
00814     }
00815     }
00816     out_cell = ndCell;
00817     return true;
00818     */
00819 }
00820 
00821 template<typename PointT>
00822 void NDTOccupancyMap<PointT>::debugToVRML(const char* fname, pcl::PointCloud<PointT> &pc)
00823 {
00824 
00825     FILE* fout = fopen(fname, "w");
00826 
00827     fprintf(fout,"#VRML V2.0 utf8\n");
00828     this->writeToVRML(fout);
00829     lslgeneric::writeToVRML(fout,pc,Eigen::Vector3d(1,0,0));
00830 
00831     fprintf(fout,"Shape {\n\tgeometry IndexedLineSet {\n\tcoord Coordinate {\n\t point [\n\t");
00832 
00833     int n_lines = 0;
00834     PointT centerCell;
00835     for(size_t i=0; i<pc.points.size(); i++)
00836     {
00837         NDTCell<PointT>* link;
00838         if(this->getCellForPoint(pc.points[i], link))
00839         {
00840             if(link == NULL) continue;
00841             centerCell = link->getCenter();
00842             if(link->hasGaussian_)
00843             {
00844                 centerCell.x = link->getMean()(0);
00845                 centerCell.y = link->getMean()(1);
00846                 centerCell.z = link->getMean()(2);
00847             }
00848             fprintf(fout,"%lf %lf %lf\n\t%lf %lf %lf\n\t",
00849                     pc.points[i].x, pc.points[i].y,pc.points[i].z,
00850                     centerCell.x, centerCell.y, centerCell.z);
00851             n_lines++;
00852         }
00853     }
00854 
00855     fprintf(fout, "]\n\t}\n\tcoordIndex [\n\t");
00856     for(int i = 0; i<n_lines; i++)
00857     {
00858         fprintf(fout,"%d, %d, -1\n\t",2*i, 2*i+1);
00859     }
00860     fprintf(fout, "]\n}\n}\n");
00861 
00862 
00863     fclose(fout);
00864 }
00865 
00866 template<typename PointT>
00867 std::vector<NDTCell<PointT>*> NDTOccupancyMap<PointT>::pseudoTransformNDT(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T)
00868 {
00869 
00870     std::vector<NDTCell<PointT>*> ret;
00871     typename SpatialIndex<PointT>::CellVectorItr it = index_->begin();
00872     while (it != index_->end())
00873     {
00874         NDTCell<PointT> *cell = dynamic_cast<NDTCell<PointT>*> (*it);
00875         if(cell!=NULL)
00876         {
00877             if(cell->hasGaussian_)
00878             {
00879                 Eigen::Vector3d mean = cell->getMean();
00880                 Eigen::Matrix3d cov = cell->getCov();
00881                 mean = T*mean;
00882                 cov = T.rotation().transpose()*cov*T.rotation();
00883                 NDTCell<PointT>* nd = (NDTCell<PointT>*)cell->clone();
00884                 nd->setMean(mean);
00885                 nd->setCov(cov);
00886                 ret.push_back(nd);
00887             }
00888         }
00889         else
00890         {
00891             //ERR("problem casting cell to NDT!\n");
00892         }
00893         it++;
00894     }
00895     return ret;
00896 }
00897 
00898 template<typename PointT>
00899 NDTCell<PointT>*
00900 NDTOccupancyMap<PointT>::getCellIdx(unsigned int idx)
00901 {
00902     CellVector<PointT> *cl = dynamic_cast<CellVector<PointT>*>(index_);
00903     if (cl != NULL)
00904     {
00905         return cl->getCellIdx(idx);
00906     }
00907     return NULL;
00908 }
00909 
00910 
00914 template<typename PointT>
00915 std::vector<lslgeneric::NDTCell<PointT>*>  NDTOccupancyMap<PointT>::getDynamicCells(unsigned int Timescale, float threshold)
00916 {
00917     std::vector<NDTCell<PointT>*> ret;
00918     typename SpatialIndex<PointT>::CellVectorItr it = index_->begin();
00919     while (it != index_->end())
00920     {
00921         NDTCell<PointT> *cell = dynamic_cast<NDTCell<PointT>*> (*it);
00922         if(cell!=NULL)
00923         {
00924 
00925             if(cell->getDynamicLikelihood(Timescale)>threshold)
00926             {
00927                 NDTCell<PointT>* nd = (NDTCell<PointT>*)cell->copy();
00928                 ret.push_back(nd);
00929             }
00930         }
00931         else
00932         {
00933         }
00934         it++;
00935     }
00936     return ret;
00937 }
00938 
00939 
00940 template<typename PointT>
00941 //std::vector<lslgeneric::NDTCell<pcl::PointXYZ>*> NDTOccupancyMap<PointT>::getAllCells(){
00942 std::vector<lslgeneric::NDTCell<PointT>*> NDTOccupancyMap<PointT>::getAllCells()
00943 {
00944 
00945     std::vector<NDTCell<PointT>*> ret;
00946     typename SpatialIndex<PointT>::CellVectorItr it = index_->begin();
00947     while (it != index_->end())
00948     {
00949         NDTCell<PointT> *cell = dynamic_cast<NDTCell<PointT>*> (*it);
00950         if(cell!=NULL)
00951         {
00952             if(cell->hasGaussian_)
00953             {
00954                 NDTCell<PointT>* nd = (NDTCell<PointT>*)cell->copy();
00955                 ret.push_back(nd);
00956             }
00957         }
00958         else
00959         {
00960         }
00961         it++;
00962     }
00963     return ret;
00964 }
00965 
00966 template<typename PointT>
00967 int NDTOccupancyMap<PointT>::numberOfActiveCells()
00968 {
00969     int ret = 0;
00970     if(index_ == NULL) return ret;
00971     typename SpatialIndex<PointT>::CellVectorItr it = index_->begin();
00972     while (it != index_->end())
00973     {
00974         NDTCell<PointT> *cell = dynamic_cast<NDTCell<PointT>*> (*it);
00975         if(cell!=NULL)
00976         {
00977             if(cell->hasGaussian_)
00978             {
00979                 ret++;
00980             }
00981         }
00982         it++;
00983     }
00984     return ret;
00985 }
00986 
00987 }


ndt_map
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Oct 6 2014 03:18:54