ndt_map.cpp
Go to the documentation of this file.
00001 #include <Eigen/Eigen>
00002 #include <pcl/point_cloud.h>
00003 #include <pcl/features/feature.h>
00004 #include <pcl/common/common.h>
00005 
00006 #include <string>
00007 #include <climits>
00008 
00009 //#include <ndt_map/oc_tree.h>
00010 #include <ndt_map/ndt_map.h>
00011 #include <ndt_map/lazy_grid.h>
00012 #include <ndt_map/cell_vector.h>
00013 
00014 #include <cstring>
00015 #include <cstdio>
00016 
00017 namespace lslgeneric
00018 {
00028 void NDTMap::loadPointCloud(const pcl::PointCloud<pcl::PointXYZ> &pc, double range_limit)
00029 {
00030     if(index_ != NULL)
00031     {
00032         //std::cout<<"CLONE INDEX\n";
00033         SpatialIndex *si = index_->clone();
00034         //cout<<"allocating index\n";
00035         if(!isFirstLoad_)
00036         {
00037             //std::cout<<"deleting old index\n";
00038             delete index_;
00039         }
00040         isFirstLoad_ = false;
00041         index_ = si;
00042     }
00043     else
00044     {
00045         //NULL index in constructor, abort!
00046         //ERR("constructor must specify a non-NULL spatial index\n");
00047         return;
00048     }
00049     
00050     LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
00051     if(lz == NULL)
00052     {
00053         fprintf(stderr,"Unfortunately This works only with Lazygrid!\n");
00054         exit(1);
00055     }
00056 
00057     if(index_ == NULL)
00058     {
00059         //ERR("Problem creating index, unimplemented method\n");
00060         return;
00061     }
00062 
00063     pcl::PointCloud<pcl::PointXYZ>::const_iterator it = pc.points.begin();
00064     if(guess_size_) 
00065     {
00066         double maxDist = 0;//, distCeil = 200;
00067 
00068         Eigen::Vector3d centroid(0,0,0);
00069         int npts = 0;
00070         while(it!=pc.points.end())
00071         {
00072             Eigen::Vector3d d;
00073             if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
00074             {
00075                 it++;
00076                 continue;
00077             }
00078             d << it->x, it->y, it->z;
00079             if(range_limit>0)
00080             {
00081                 if(d.norm()>range_limit)
00082                 {
00083                     it++;
00084                     continue;
00085                 }
00086             }
00087             centroid += d;
00088             it++;
00089             npts++;
00090         }
00091 
00092         centroid /= (double)npts;
00093         double maxz=-1000, minz=10000;
00094         //Eigen::Vector4f centroid(0,0,0,0);
00095         //pcl::compute3DCentroid(pc,centroid);
00096 
00097         //compute distance to furthest point
00098         it = pc.points.begin();
00099         while(it!=pc.points.end())
00100         {
00101             Eigen::Vector3d d;
00102             if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
00103             {
00104                 it++;
00105                 continue;
00106             }
00107             if(range_limit>0)
00108             {
00109                 d << it->x, it->y, it->z;
00110                 if(d.norm()>range_limit)
00111                 {
00112                     it++;
00113                     continue;
00114                 }
00115             }
00116             d << centroid(0)-it->x, centroid(1)-it->y, centroid(2)-it->z;
00117             double dist = d.norm();
00118             maxDist = (dist > maxDist) ? dist : maxDist;
00119             maxz = ((centroid(2)-it->z) > maxz) ? (centroid(2)-it->z) : maxz;
00120             minz = ((centroid(2)-it->z) < minz) ? (centroid(2)-it->z) : minz;
00121             it++;
00122         }
00123         // cout<<"Points = " <<pc.points.size()<<" maxDist = "<<maxDist<<endl;
00124         NDTCell *ptCell = new NDTCell();
00125         index_->setCellType(ptCell);
00126         delete ptCell;
00127         index_->setCenter(centroid(0),centroid(1),centroid(2));
00128 
00129         if(map_sizex >0 && map_sizey >0 && map_sizez >0)
00130         {
00131             index_->setSize(map_sizex,map_sizey,map_sizez);
00132         }
00133         else
00134         {
00135             index_->setSize(4*maxDist,4*maxDist,3*(maxz-minz));
00136         }
00137     }
00138     else
00139     {
00140         //set sizes
00141         NDTCell *ptCell = new NDTCell();
00142         index_->setCellType(ptCell);
00143         delete ptCell;
00144         index_->setCenter(centerx,centery,centerz);
00145         if(map_sizex >0 && map_sizey >0 && map_sizez >0)
00146         {
00147             index_->setSize(map_sizex,map_sizey,map_sizez);
00148         }
00149     }
00150 
00151     //    ROS_INFO("centroid is %f,%f,%f", centroid(0),centroid(1),centroid(2));
00152     //    ROS_INFO("maxDist is %lf", maxDist);
00153 
00154     it = pc.points.begin();
00155     while(it!=pc.points.end())
00156     {
00157         Eigen::Vector3d d;
00158         if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
00159         {
00160             it++;
00161             continue;
00162         }
00163         if(range_limit>0)
00164         {
00165             d << it->x, it->y, it->z;
00166             if(d.norm()>range_limit)
00167             {
00168                 it++;
00169                 continue;
00170             }
00171         }
00172         index_->addPoint(*it);
00173         NDTCell *ptCell;
00174         lz->getNDTCellAt(*it,ptCell);
00175 #ifdef REFACTORED
00176         if(ptCell!=NULL) {
00177         //    std::cerr<<"adding to update set\n";
00178             update_set.insert(ptCell);
00179         }
00180 #endif
00181         it++;
00182     }
00183 
00184     isFirstLoad_ = false;
00185 }
00186 
00187 
00193 void NDTMap::loadPointCloudCentroid(const pcl::PointCloud<pcl::PointXYZ> &pc, const Eigen::Vector3d &origin,
00194         const Eigen::Vector3d &old_centroid,const Eigen::Vector3d &map_size, double range_limit){
00195     
00196     if(index_ != NULL){
00197         SpatialIndex *si = index_->clone();
00198         if(!isFirstLoad_) delete index_;
00199         
00200         isFirstLoad_ = false;
00201         index_ = si;
00202     }
00203     else{
00204         return;
00205     }
00206 
00207     if(index_ == NULL) return; 
00208 
00209     NDTCell *ptCell = new NDTCell();
00210     index_->setCellType(ptCell);
00211     delete ptCell;
00212 
00213     LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
00214     if(lz == NULL)
00215     {
00216         fprintf(stderr,"Unfortunately This works only with Lazygrid!\n");
00217         exit(1);
00218     }
00219 
00220     Eigen::Vector3d diff = origin - old_centroid;  //-origin;
00221     double cx=0, cy=0, cz=0;
00222     lz->getCellSize(cx, cy, cz);
00223 
00225     Eigen::Vector3d centroid;
00226     centroid(0) = old_centroid(0) + floor(diff(0) / cx) * cx;
00227     centroid(1) = old_centroid(1) + floor(diff(1) / cy) * cy;
00228     centroid(2) = old_centroid(2) + floor(diff(2) / cz) * cz;
00229 
00230     index_->setCenter(centroid(0),centroid(1),centroid(2));
00231     //index_->setCenter(origin(0),origin(1),origin(2));
00232     index_->setSize(map_size(0),map_size(1),map_size(2));
00233     //lz->initializeAll();
00234 
00235     //fprintf(stderr,"centroid is %lf,%lf,%lf (origin: %lf %lf %lf) (map_size %lf %lf %lf) N=%d", centroid(0),centroid(1),centroid(2), origin(0),origin(1),origin(2), map_size(0), map_size(1), map_size(2),pc.size());
00236     //ROS_INFO("centroid is %f,%f,%f", centroid(0),centroid(1),centroid(2));
00237     //ROS_INFO("maxDist is %lf", maxDist);
00238 
00239     pcl::PointCloud<pcl::PointXYZ>::const_iterator it = pc.points.begin();
00240 
00241     while(it!=pc.points.end())
00242     {
00243         Eigen::Vector3d d;
00244         if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
00245         {
00246             it++;
00247             continue;
00248         }
00249         
00250         if(range_limit>0)
00251         {
00252             d << it->x, it->y, it->z;
00253             d = d-origin;
00254             if(d.norm()>range_limit)
00255             {
00256                 it++;
00257                 continue;
00258             }
00259         }
00260         
00261         //fprintf(stderr,"HEP!");
00262         index_->addPoint(*it);
00263         NDTCell *ptCell=NULL;
00264         lz->getNDTCellAt(*it,ptCell);
00265 #ifdef REFACTORED
00266         if(ptCell!=NULL) update_set.insert(ptCell);
00267 #endif
00268         it++;
00269     }
00270 
00271     isFirstLoad_ = false;
00272 }
00273 
00274 
00275 
00279 void NDTMap::addPointCloudSimple(const pcl::PointCloud<pcl::PointXYZ> &pc,double maxz)
00280 {
00281     if(isFirstLoad_)
00282     {
00283         loadPointCloud( pc);
00284         return;
00285     }
00286 
00287     pcl::PointCloud<pcl::PointXYZ>::const_iterator it = pc.points.begin();
00288     it = pc.points.begin();
00289     LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
00290     if(lz == NULL)
00291     {
00292         fprintf(stderr,"Unfortunately This works only with Lazygrid!\n");
00293         exit(1);
00294     }
00295 
00296     while(it!=pc.points.end())
00297     {
00298         if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
00299         {
00300             it++;
00301             continue;
00302         }
00303         if(it->z>maxz)
00304         {
00305             it++;
00306             continue;
00307         }
00308         index_->addPoint(*it);
00309         NDTCell *ptCell;
00310         lz->getNDTCellAt(*it,ptCell);
00311 #ifdef REFACTORED
00312         if(ptCell!=NULL) update_set.insert(ptCell);
00313 #endif
00314         it++;
00315     }
00316 
00317 }
00318 
00322 void NDTMap::addDistributionToCell(const Eigen::Matrix3d &ucov, const Eigen::Vector3d &umean, unsigned int numpointsindistribution, 
00323         float r, float g,float b,  unsigned int maxnumpoints, float max_occupancy)
00324 {
00325     pcl::PointXYZ pt;
00326     pt.x = umean[0];
00327     pt.y = umean[1];
00328     pt.z = umean[2];
00329     LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
00330     if(lz==NULL)
00331     {
00332         fprintf(stderr,"NOT LAZY GRID!!!\n");
00333         exit(1);
00334     }
00335     NDTCell *ptCell = NULL; 
00336     lz->getNDTCellAt(pt,ptCell);
00337     if(ptCell != NULL)
00338     {
00339         //std::cout<<"BEFORE\n";
00340         //std::cout<<"had G "<<ptCell->hasGaussian_<<" occ "<<ptCell->getOccupancy()<<std::endl;
00341 //      std::cout<<umean.transpose()<<" "<<numpointsindistribution <<std::endl;
00342 //      std::cout<<ucov<<std::endl;
00343 //      std::cout<<ptCell->getMean().transpose()<<std::endl;
00344 //      std::cout<<ptCell->getCov()<<std::endl;
00345         ptCell->updateSampleVariance(ucov, umean, numpointsindistribution, true, max_occupancy,maxnumpoints);
00346         ptCell->setRGB(r,g,b);
00347 //      std::cout<<"AFTER\n";
00348 //      std::cout<<ptCell->getMean().transpose()<<std::endl;
00349 //      std::cout<<ptCell->getCov()<<std::endl;
00350     }
00351 }
00352 
00354 bool NDTMap::getCellAtPoint(const pcl::PointXYZ &refPoint, NDTCell *&cell)
00355 {
00356     LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
00357     if(lz==NULL)
00358     {
00359         fprintf(stderr,"NOT LAZY GRID!!!\n");
00360         exit(1);
00361     }
00362     lz->getNDTCellAt(refPoint,cell);
00363     return (cell != NULL);
00364 }
00365 
00369 void NDTMap::addPointCloud(const Eigen::Vector3d &origin, const pcl::PointCloud<pcl::PointXYZ> &pc, double classifierTh, double maxz, 
00370         double sensor_noise, double occupancy_limit)
00371 {
00372     if(isFirstLoad_)
00373     {
00374                         loadPointCloud( pc);
00375                         return;
00376     }
00377     if(index_ == NULL)
00378     {
00379                         //ERR("Problem creating index, unimplemented method\n");
00380                         return;
00381     }
00382     pcl::PointCloud<pcl::PointXYZ>::const_iterator it = pc.points.begin();
00383 
00384     LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
00385     if(lz==NULL)
00386     {
00387                         fprintf(stderr,"NOT LAZY GRID!!!\n");
00388                         exit(1);
00389     }
00390     pcl::PointXYZ po, pt;
00391     po.x = origin(0); po.y = origin(1); po.z = origin(2);
00392     NDTCell* ptCell = NULL; 
00393 
00394 #ifdef REFACTORED
00395     std::vector< NDTCell*> cells; 
00396     bool updatePositive = true;
00397     double max_range = 200.;
00398 
00399     while(it!=pc.points.end())
00400     {
00401                         if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
00402                         {
00403                                         it++;
00404                                         continue;
00405                         }
00406 
00407                         Eigen::Vector3d diff;
00408                         diff << it->x-origin(0), it->y-origin(1), it->z-origin(2);
00409                         double l = diff.norm();
00410 
00411                         if(l>max_range)
00412                         {
00413                                         fprintf(stderr,"Very long distance (%lf) :( \n",l);
00414                                         it++;
00415                                         continue;
00416                         }
00417 
00418                         cells.clear();
00419                         if(!lz->traceLine(origin,*it,diff,maxz,cells)) {
00420                                         it++;
00421                                         continue;
00422                         }
00423 
00424                         for(unsigned int i=0; i<cells.size(); i++)
00425                         {
00426                                         ptCell = cells[i];
00427                                         if(ptCell != NULL)
00428                                         {
00429                                 double l2target = 0;
00430                                 if(ptCell->hasGaussian_)
00431                                 {
00432                                                 Eigen::Vector3d out, pend,vpt;
00433                                                 pend << it->x,it->y,it->z;
00434                                                 double lik = ptCell->computeMaximumLikelihoodAlongLine(po, *it, out);
00435                                                 l2target = (out-pend).norm();
00436 
00437                                                 double dist = (origin-out).norm();
00438                                                 if(dist > l) continue; 
00439 
00440                                                 l2target = (out-pend).norm(); 
00441 
00442                                                 double sigma_dist = 0.5 * (dist/30.0); 
00443                                                 double snoise = sigma_dist + sensor_noise;
00444                                                 double thr =exp(-0.5*(l2target*l2target)/(snoise*snoise)); 
00445                                                 lik *= (1.0-thr);
00446                                                 if(lik<0.3) continue;
00447                                                 lik = 0.1*lik+0.5; 
00448                                                 double logoddlik = log( (1.0-lik)/(lik) );
00449                                                 //ptCell->updateEmpty(logoddlik,l2target);
00450                                                 //fprintf(stderr,"[E=%.2lf] ", logoddlik);
00451                                                 ptCell->updateOccupancy(logoddlik, occupancy_limit);
00452                                                 if(ptCell->getOccupancy()<=0) ptCell->hasGaussian_ = false; 
00453                                 }
00454                                 else
00455                                 {
00456                                          // ptCell->updateEmpty(-0.2,l2target); ///The cell does not have gaussian, so we mark that we saw it empty...
00457                                          ptCell->updateOccupancy(-0.2, occupancy_limit);
00458                                          if(ptCell->getOccupancy()<=0) ptCell->hasGaussian_ = false; 
00459                                 }
00460                                         }
00461                                         //update_set.insert(ptCell);
00462                         }
00463         if(updatePositive) {
00464             ptCell = dynamic_cast<NDTCell*>(index_->addPoint(*it));
00465             if(ptCell!=NULL) {
00466                                 update_set.insert(ptCell);
00467             }
00468         }
00469         it++;
00470     }
00471     isFirstLoad_ = false;
00472 
00473 #else
00474     double centerX,centerY,centerZ;
00475     lz->getCenter(centerX, centerY, centerZ);
00476     double cellSizeX,cellSizeY,cellSizeZ;
00477     lz->getCellSize(cellSizeX, cellSizeY, cellSizeZ);
00478     int sizeX,sizeY,sizeZ;
00479     lz->getGridSize(sizeX, sizeY, sizeZ);
00480 
00481     NDTCell ****dataArray = lz->getDataArrayPtr();
00482 
00483     double min1 = std::min(cellSizeX,cellSizeY);
00484     double min2 = std::min(cellSizeZ,cellSizeY);
00485 
00486     double resolution = std::min(min1,min2); 
00487 
00488     while(it!=pc.points.end())
00489     {
00490         if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
00491         {
00492             it++;
00493             continue;
00494         }
00495         Eigen::Vector3d diff;
00496         diff << it->x-origin(0), it->y-origin(1), it->z-origin(2);
00497 
00498         double l = diff.norm();
00499         int N = l / (resolution);
00500 
00501         if(l>200)
00502         {
00503             fprintf(stderr,"Very long distance (%lf) :( \n",l);
00504             it++;
00505             continue;
00506         }
00507         if(resolution<0.01)
00508         {
00509             fprintf(stderr,"Resolution very very small (%lf) :( \n",resolution);
00510             it++;
00511             continue;
00512         }
00513 
00514 
00515         if(N <= 0)
00516         {
00517             fprintf(stderr,"N=%d (r=%lf l=%lf) :( ",N,resolution,l);
00518             it++;
00519             continue;
00520         }
00521 
00522         diff = diff/(float)N;
00523 
00524         bool updatePositive = true;
00525         if(it->z>maxz)
00526         {
00527             it++;
00528             continue;
00529         }
00530 
00531         int idxo=0, idyo=0,idzo=0;
00532         for(int i=0; i<N-1; i++)
00533         {
00534             pt.x = origin(0) + ((float)(i+1)) *diff(0);
00535             pt.y = origin(1) + ((float)(i+1)) *diff(1);
00536             pt.z = origin(2) + ((float)(i+1)) *diff(2);
00537             int idx,idy,idz;
00538 
00539             idx = (int)(((pt.x - centerX)/cellSizeX+0.5) + sizeX/2);
00540             idy = (int)(((pt.y - centerY)/cellSizeY+0.5) + sizeY/2);
00541             idz = (int)(((pt.z - centerZ)/cellSizeZ+0.5) + sizeZ/2);
00542 
00543 
00546             if(idx == idxo && idy==idyo && idz ==idzo)
00547             {
00548                 continue;
00549             }
00550             else
00551             {
00552                 idxo = idx;
00553                 idyo=idy,idzo=idz;
00554             }
00556             if(idx < sizeX && idy < sizeY && idz < sizeZ && idx >=0 && idy >=0 && idz >=0)
00557             {
00558                 //fprintf(stderr,"(in)");
00559                 ptCell = (dataArray[idx][idy][idz]);
00560             }
00561             else
00562             {
00563                 //fprintf(stderr,"(out)");
00564                 continue;
00565             }
00566 
00567             if(ptCell != NULL)
00568             {
00569                 double l2target = 0;
00570                 if(ptCell->hasGaussian_)
00571                 {
00572                     Eigen::Vector3d out, pend,vpt;
00573                     pend << it->x,it->y,it->z;
00574                     double lik = ptCell->computeMaximumLikelihoodAlongLine(po, pt, out);
00575                     l2target = (out-pend).norm();
00576 
00577                     double dist = (origin-out).norm();
00578                     if(dist > l) continue; 
00579 
00580                     l2target = (out-pend).norm(); 
00581 
00582                     double sigma_dist = 0.5 * (dist/30.0); 
00583                     double snoise = sigma_dist + sensor_noise;
00584                     double thr =exp(-0.5*(l2target*l2target)/(snoise*snoise)); 
00585                     lik *= (1.0-thr);
00586                     lik = 0.2*lik+0.5; 
00587                     double logoddlik = log( (1.0-lik)/(lik) );
00588                     ptCell->updateEmpty(logoddlik,l2target);
00589                 }
00590                 else
00591                 {
00592                     ptCell->updateEmpty(-0.2,l2target); 
00593                 }
00594             }
00595             else
00596             {
00597                 index_->addPoint(pt); 
00598             }
00599         }
00600 
00601         if(updatePositive) index_->addPoint(*it);
00602         it++;
00603     }
00604     isFirstLoad_ = false;
00605 #endif
00606 }
00607 
00611 
00626 void  NDTMap::addPointCloudMeanUpdate(const Eigen::Vector3d &origin, 
00627         const pcl::PointCloud<pcl::PointXYZ> &pc, 
00628         const Eigen::Vector3d &localmapsize,
00629         unsigned int maxnumpoints, float occupancy_limit,double maxz, double sensor_noise){
00630 
00631     if(isFirstLoad_){
00632         //fprintf(stderr,"First load... ");
00633         loadPointCloud(pc);
00634         computeNDTCells();
00635         //fprintf(stderr,"DONE!");
00636         return;
00637     }
00638     if(index_ == NULL){
00639         return;
00640     }
00641     LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
00642     if(lz==NULL){
00643         fprintf(stderr,"NOT LAZY GRID!!!\n");
00644         exit(1);
00645     }
00647     double centerX,centerY,centerZ;
00648     lz->getCenter(centerX, centerY, centerZ);
00649     double cellSizeX,cellSizeY,cellSizeZ;
00650     lz->getCellSize(cellSizeX, cellSizeY, cellSizeZ);
00651     int sizeX,sizeY,sizeZ;
00652     lz->getGridSize(sizeX, sizeY, sizeZ);
00653 
00654     Eigen::Vector3d old_centroid;
00655     old_centroid(0) = centerX;
00656     old_centroid(1) = centerY;
00657     old_centroid(2) = centerZ;
00658 
00659     double min1 = std::min(cellSizeX,cellSizeY);
00660     double min2 = std::min(cellSizeZ,cellSizeY);
00661     double resolution = std::min(min1,min2); 
00662                 //fprintf(stderr,"1:");
00664     lslgeneric::NDTMap ndlocal(new lslgeneric::LazyGrid(resolution));
00665     ndlocal.loadPointCloudCentroid(pc,origin, old_centroid, localmapsize, 70.0); 
00666     
00668     //ndlocal.computeNDTCells(CELL_UPDATE_MODE_STUDENT_T);
00669     
00671     ndlocal.computeNDTCells();
00672                 
00674     std::vector<lslgeneric::NDTCell*> ndts;
00675     ndts = ndlocal.getAllCells();
00676                 //fprintf(stderr,"2(%u):",ndts.size());
00677     NDTCell *ptCell=NULL;
00678 
00679     pcl::PointXYZ pt;
00680     pcl::PointXYZ po;
00681     po.x = origin(0); po.y = origin(1); po.z = origin(2);
00682     //fprintf(stderr,"NUM = %d\n",ndts.size());
00683     int num_high = 0;
00684     for(unsigned int it=0;it<ndts.size();it++)
00685     {
00686         if(ndts[it] == NULL){
00687             fprintf(stderr,"NDTMap::addPointCloudMeanUpdate::GOT NULL FROM MAP -- SHOULD NEVER HAPPEN!!!\n");
00688             continue;
00689         }
00690 
00691         if(!ndts[it]->hasGaussian_){
00692             fprintf(stderr,"NDTMap::addPointCloudMeanUpdate::NO GAUSSIAN!!!! -- SHOULD NEVER HAPPEN!!!\n");
00693             continue;
00694         }
00695 
00696         int numpoints = ndts[it]->getN();
00697 
00698         if(numpoints<=0){
00699             fprintf(stderr,"addPointCloudMeanUpdate::Number of points in distribution<=0!!");
00700             continue;
00701         }
00702         Eigen::Vector3d diff;
00703         Eigen::Vector3d m = ndts[it]->getMean();
00704         diff = m-origin;
00705 
00706         double l = diff.norm();
00707         int NN = l / (resolution);
00708 
00709         if(l>200){
00710             fprintf(stderr,"addPointCloudMeanUpdate::Very long distance (%lf) :( \n",l);
00711             continue;
00712         }
00713         if(resolution<0.01){
00714             fprintf(stderr,"addPointCloudMeanUpdate::Resolution very very small (%lf) :( \n",resolution);
00715             continue;
00716         }
00717         if(NN < 0){ 
00718             fprintf(stderr,"addPointCloudMeanUpdate::N=%d (r=%lf l=%lf) :( ",NN,resolution,l);
00719             continue;
00720         }
00721 
00722         bool updatePositive = true;
00723         if(m(2)>maxz){
00724             num_high++;
00725             updatePositive = false; 
00726         }
00727         //std::cout<<"c: "<<ndts[it]->getCov()<<std::endl;
00728 
00729         diff = diff/(float)NN;
00730 
00731         //fprintf(stderr,"3(%d):",NN);
00732         int idxo=0, idyo=0,idzo=0;
00733         for(int i=0; i<NN-2; i++)  
00734         {
00735             pt.x = origin(0) + ((float)(i+1)) *diff(0);
00736             pt.y = origin(1) + ((float)(i+1)) *diff(1);
00737             pt.z = origin(2) + ((float)(i+1)) *diff(2);
00738             int idx,idy,idz;
00739 
00740             idx = (int)(((pt.x - centerX)/cellSizeX+0.5) + sizeX/2.0);
00741             idy = (int)(((pt.y - centerY)/cellSizeY+0.5) + sizeY/2.0);
00742             idz = (int)(((pt.z - centerZ)/cellSizeZ+0.5) + sizeZ/2.0);
00743 
00744 
00747             if(idx == idxo && idy==idyo && idz ==idzo){
00748                 continue;
00749             }else{
00750                 idxo = idx;idyo=idy;idzo=idz;
00751             }
00752             ptCell = NULL;
00754             lz->getNDTCellAt(idx,idy,idz,ptCell);
00755             /*
00756                if(ptCell != NULL){
00757                if(!ptCell->hasGaussian_){
00758                ptCell->updateOccupancy(-0.85*numpoints,occupancy_limit);
00759                }else{
00760                ptCell->updateOccupancy(-0.2*numpoints,occupancy_limit);
00761                ptCell = lz->getClosestNDTCell(pt);
00762                }
00763                }*/
00764 
00765             if(ptCell != NULL){
00766                 double l2target = 0;
00767                 if(ptCell->hasGaussian_)
00768                 {
00769                     Eigen::Vector3d out, pend,vpt;
00770                     pend = m;
00771 
00772                     double lik = ptCell->computeMaximumLikelihoodAlongLine(po, pt, out);
00773                     l2target = (out-pend).norm();
00774 
00775                     double dist = (origin-out).norm();
00776                     if(dist > l) continue; 
00777 
00778                     l2target = (out-pend).norm(); 
00779 
00780                     double sigma_dist = 0.5 * (dist/30.0); 
00781                     //double sigma_dist = 0;
00782                     double snoise = sigma_dist + sensor_noise;
00783                     double thr =exp(-0.5*(l2target*l2target)/(snoise*snoise)); 
00784                     lik *= (1.0-thr);
00785                     //lik = 0.4*lik+0.5+0.05; ///Evidence value for empty - alpha * p(x);
00786                     lik = 0.2*lik+0.5; 
00787                     double logoddlik = log( (1.0-lik)/(lik) );
00788 
00789                     //fprintf(stderr,"l=%lf ",numpoints*logoddlik);
00790                     ptCell->updateOccupancy(numpoints*logoddlik,occupancy_limit);
00791                     if(ptCell->getOccupancy()<0) ptCell->hasGaussian_ = false;
00792                 }
00793                 else
00794                 {
00795                     ptCell->updateOccupancy(-0.85*numpoints,occupancy_limit); 
00796                     //ptCell->updateEmpty(-0.2*numpoints,l2target); ///The cell does not have gaussian, so we mark that we saw it empty...
00797                 }
00798             }else{
00799                 ptCell = dynamic_cast<NDTCell*>(index_->addPoint(pt)); 
00800             }
00801         }
00802         if(updatePositive){
00803             Eigen::Matrix3d ucov = ndts[it]->getCov();
00804             float r,g,b;
00805             ndts[it]->getRGB(r,g,b);
00806             addDistributionToCell(ucov, m, numpoints, r, g,b,maxnumpoints,occupancy_limit); 
00807         }
00808     }
00809     //fprintf(stderr,"| Gaussians=%d Invalid=%d |", ndts.size(), num_high);
00810     for(unsigned int i=0;i<ndts.size();i++) delete ndts[i];
00811     isFirstLoad_ = false;
00812 }
00813 
00814 
00818 
00823 bool NDTMap::addMeasurement(const Eigen::Vector3d &origin, pcl::PointXYZ endpoint, double classifierTh, double maxz, double sensor_noise)
00824 {
00825 
00826     if(index_ == NULL)
00827     {
00828         return false;
00829     }
00830 
00831     bool retval = false;
00832 
00833     LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
00834     double centerX,centerY,centerZ;
00835     lz->getCenter(centerX, centerY, centerZ);
00836     double cellSizeX,cellSizeY,cellSizeZ;
00837     lz->getCellSize(cellSizeX, cellSizeY, cellSizeZ);
00838     int sizeX,sizeY,sizeZ;
00839     lz->getGridSize(sizeX, sizeY, sizeZ);
00840 
00841     //Cell<PointT> ****dataArray = lz->getDataArrayPtr();
00842 
00843     double min1 = std::min(cellSizeX,cellSizeY);
00844     double min2 = std::min(cellSizeZ,cellSizeY);
00845 
00846     double resolution = std::min(min1,min2); 
00847 
00848     if(lz==NULL)
00849     {
00850         fprintf(stderr,"NOT LAZY GRID!!!\n");
00851         exit(1);
00852     }
00853     NDTCell *ptCell=NULL;
00854 
00855     pcl::PointXYZ pt;
00856     pcl::PointXYZ po;
00857     po.x = origin(0),origin(1),origin(2);
00858 
00859     Eigen::Vector3d diff;
00860     diff << endpoint.x-origin(0), endpoint.y-origin(1), endpoint.z-origin(2);
00861 
00862 
00863     double l = diff.norm();
00864     if(l>200)
00865     {
00866         fprintf(stderr,"Very long distance (%lf) :( \n",l);
00867         return false;
00868     }
00869     if(resolution<0.01)
00870     {
00871         fprintf(stderr,"Resolution very very small (%lf) :( \n",resolution);
00872         return false;
00873     }
00874 
00875     int NN = l / (resolution);
00876     if(NN <= 0) return false;
00877     diff = diff/(float)NN;
00878     //fprintf(stderr," (N=%d) ",NN);
00879     bool updatePositive = true;
00880     if(endpoint.z>maxz)
00881     {
00882         return false;
00883     }
00884 
00885     int idxo=0, idyo=0,idzo=0;
00886     for(int i=0; i<NN-2; i++)
00887     {
00888         pt.x = origin(0) + ((float)(i+1)) *diff(0);
00889         pt.y = origin(1) + ((float)(i+1)) *diff(1);
00890         pt.z = origin(2) + ((float)(i+1)) *diff(2);
00891         int idx,idy,idz;
00892 
00893         idx = (int)(((pt.x - centerX)/cellSizeX+0.5) + sizeX/2);
00894         idy = (int)(((pt.y - centerY)/cellSizeY+0.5) + sizeY/2);
00895         idz = (int)(((pt.z - centerZ)/cellSizeZ+0.5) + sizeZ/2);
00896 
00897 
00900         if(idx == idxo && idy==idyo && idz ==idzo)
00901         {
00902             continue;
00903         }
00904         else
00905         {
00906             idxo = idx;
00907             idyo=idy,idzo=idz;
00908         }
00910         //if(idx < sizeX && idy < sizeY && idz < sizeZ && idx >=0 && idy >=0 && idz >=0)
00911         //{
00912         //    ptCell = dynamic_cast<NDTCell<PointT> *>  (lz->getCellAt(idx,idy,idz)); //dataArray[idx][idy][idz]);
00913         lz->getNDTCellAt(idx,idy,idz,ptCell);
00914         //}
00915         //else
00916         //{
00917         //    continue;
00918         //}
00919 
00920         if(ptCell != NULL)
00921         {
00922             double l2target = 0;
00923 
00924             if(ptCell->hasGaussian_)
00925             {
00926                 Eigen::Vector3d out, pend,vpt;
00927 
00928                 pend << endpoint.x,endpoint.y,endpoint.z; 
00929 
00930                 double lik = ptCell->computeMaximumLikelihoodAlongLine(po, pt, out);
00931                 double dist = (origin-out).norm();
00932                 if(dist > l) continue; 
00933 
00934                 l2target = (out-pend).norm(); 
00935                 //double thr =exp(-0.5*(l2target*l2target)/(sensor_noise*sensor_noise)); ///This is the probability of max lik point being endpoint
00936                 //ptCell->updateEmpty(lik*(1-thr),l2target);
00937 
00938                 double sigma_dist = 0.5 * (dist/30.0); 
00939                 double snoise = sigma_dist + sensor_noise;
00940                 double thr =exp(-0.5*(l2target*l2target)/(snoise*snoise)); 
00941                 lik *= (1.0-thr);
00942                 lik = 0.2*lik+0.5; 
00943                 double logoddlik = log( (1.0-lik)/(lik) );
00944                 ptCell->updateEmpty(logoddlik,l2target);
00945 
00946 
00947                 /*
00948                 if(lik>thr){
00949                         retval = true;
00950                         ptCell->updateEmpty(lik,l2target);
00951                 }*/
00952             }
00953             else
00954             {
00955                 ptCell->updateEmpty(-0.1,l2target); 
00956             }
00957         }
00958         else
00959         {
00960             index_->addPoint(pt); 
00961         }
00962     }
00963 
00964     if(updatePositive) index_->addPoint(endpoint);
00965 
00966     isFirstLoad_ = false;
00967 
00968     return retval;
00969 }
00976 
00977 double NDTMap::getDepth(Eigen::Vector3d origin, Eigen::Vector3d dir, double maxDepth){
00978         Eigen::Vector3d ray_endpos=origin + dir * maxDepth;
00979         std::vector< NDTCell*> cells; 
00980         
00981         Eigen::Vector3d diff = ray_endpos - origin;
00982         pcl::PointXYZ endP;
00983         endP.x = ray_endpos(0);
00984         endP.y = ray_endpos(1);
00985         endP.z = ray_endpos(2);
00986         
00987         LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
00988         if(lz==NULL){
00989                 fprintf(stderr,"NOT LAZY GRID!!!\n");
00990                 exit(1);
00991         }
00992         
00993         if(!lz->traceLine(origin, endP,diff,1000.0, cells)){
00994                 return maxDepth+1.0;
00995         }
00996         //fprintf(stderr,"Got trace with %d Cells (%lf)\n",cells.size(),(ray_endpos-origin).norm());
00997         pcl::PointXYZ po;
00998         po.x = origin(0);
00999         po.y = origin(1);
01000         po.z = origin(2);
01001         
01002         Eigen::Vector3d out;
01003         bool hasML = false;
01004         
01005         for(unsigned int i=0;i<cells.size();i++){
01006                  if(cells[i]->hasGaussian_){
01007                         double lik = cells[i]->computeMaximumLikelihoodAlongLine(po, endP, out);
01008                         if(lik>0.1){
01009                                 //fprintf(stderr,"Got ML %lf (%lf)\n",lik,(out-origin).norm());
01010                                 hasML = true;
01011                                 break;
01012                         }
01013                 } 
01014         }
01015         
01016         if(hasML) return (out - origin).norm();
01017         
01018         return (maxDepth+1.0);
01019 }
01020 
01021 
01022 double NDTMap::getDepthSmooth(Eigen::Vector3d origin,
01023                                       Eigen::Vector3d dir,
01024                                       double maxDepth,
01025                                       int n_neigh,
01026                                       double weight,
01027                                       double threshold,
01028                                       Eigen::Vector3d *hit)
01029 {
01030     Eigen::Vector3d ray_endpos=origin + dir * maxDepth;
01031     std::vector< NDTCell*> cells, surr; 
01032 
01033     Eigen::Vector3d diff = ray_endpos - origin;
01034     pcl::PointXYZ endP;
01035     endP.x = ray_endpos(0);
01036     endP.y = ray_endpos(1);
01037     endP.z = ray_endpos(2);
01038 
01039     LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
01040     if(lz==NULL){
01041         fprintf(stderr,"NOT LAZY GRID!!!\n");
01042         exit(1);
01043     }
01044 
01045     if(!lz->traceLine(origin, endP,diff,1000.0, cells)){
01046         return maxDepth+1.0;
01047     }
01048 
01049     pcl::PointXYZ startP, p;
01050     startP.x = origin(0);
01051     startP.y = origin(1);
01052     startP.z = origin(2);
01053 
01054     Eigen::Vector3d out;
01055     bool hasML = false;
01056 
01057     for(unsigned int i = 0; i < cells.size(); i++)
01058     {
01059         if(cells[i]->hasGaussian_)
01060         {
01061             surr = lz->getClosestNDTCells(cells[i]->getCenter(), n_neigh, true);
01062             double like = cells[i]->computeMaximumLikelihoodAlongLine(startP, endP, out);
01063             p.x = out(0);
01064             p.y = out(1);
01065             p.z = out(2);
01066             for (unsigned int k = 1u; k < surr.size(); ++k)
01067             {
01068                 like += weight * surr[k]->getLikelihood(p);
01069             }                   
01070             if(like > threshold)
01071             {
01072                 hasML = true;
01073                 break;
01074             }
01075         } 
01076     }
01077 
01078     if (hasML)
01079     {
01080         if (hit != NULL)
01081         {
01082             *hit = out;
01083         }
01084         return (out - origin).norm();
01085     }
01086 
01087     return (maxDepth+1.0);
01088 }
01089 
01093 
01094 
01095 void NDTMap::loadPointCloud(const pcl::PointCloud<pcl::PointXYZ> &pc, const std::vector<std::vector<size_t> > &indices)
01096 {
01097 
01098     loadPointCloud(pc);
01099     // Specific function related to CellVector
01100     CellVector *cl = dynamic_cast<CellVector*>(index_);
01101     if (cl != NULL)
01102     {
01103         for (size_t i = 0; i < indices.size(); i++)
01104         {
01105             cl->addCellPoints(pc, indices[i]);
01106         }
01107 
01108     }
01109     else
01110     {
01111         //ERR("loading point clouds using indices are currently supported in CellVector index_.");
01112     }
01113 }
01114 
01115 void NDTMap::loadDepthImage(const cv::Mat& depthImage, DepthCamera<pcl::PointXYZ> &cameraParams)
01116 {
01117     pcl::PointCloud<pcl::PointXYZ> pc;
01118     cameraParams.convertDepthImageToPointCloud(depthImage, pc);
01119     this->loadPointCloud(pc);
01120 }
01121 
01122 pcl::PointCloud<pcl::PointXYZ> NDTMap::loadDepthImageFeatures(const cv::Mat& depthImage, std::vector<cv::KeyPoint> &keypoints,
01123         size_t &supportSize, double maxVar, DepthCamera<pcl::PointXYZ> &cameraParams, bool estimateParamsDI, bool nonMean)
01124 {
01125     std::vector<cv::KeyPoint> good_keypoints;
01126     Eigen::Vector3d mean;
01127     pcl::PointXYZ mn;
01128     pcl::PointCloud<pcl::PointXYZ> cloudOut;
01129     CellVector *cl = dynamic_cast<CellVector*>(index_);
01130     if(cl==NULL)
01131     {
01132         std::cerr<<"wrong index type!\n";
01133         return cloudOut;
01134     }
01135     for (size_t i=0; i<keypoints.size(); i++)
01136     {
01137         if(!estimateParamsDI)
01138         {
01139             pcl::PointCloud<pcl::PointXYZ> points;
01140             pcl::PointXYZ center;
01141             cameraParams.computePointsAtIndex(depthImage,keypoints[i],supportSize,points,center);
01142             NDTCell *ndcell = new NDTCell();
01143             typename pcl::PointCloud<pcl::PointXYZ>::iterator it = points.points.begin();
01144             while (it!= points.points.end() )
01145             {
01146                 if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
01147                 {
01148                     it++;
01149                     continue;
01150                 }
01151                 ndcell->addPoint(*it);
01152                 it++;
01153             }
01154             ndcell->computeGaussian();
01155             if(ndcell->hasGaussian_)
01156             {
01157                 Eigen::Vector3d evals = ndcell->getEvals();
01158                 if(sqrt(evals(2)) < maxVar)
01159                 {
01160                     if (nonMean)
01161                     {
01162                         if(std::isnan(center.x) ||std::isnan(center.y) ||std::isnan(center.z))
01163                         {
01164                             continue;
01165                         }
01166                         mn = center;
01167                     }
01168                     else
01169                     {
01170                         mean = ndcell->getMean();
01171                         mn.x = mean(0);
01172                         mn.y = mean(1);
01173                         mn.z = mean(2);
01174                     }
01175                     cloudOut.points.push_back(mn);
01176                     ndcell->setCenter(mn);
01177                     cl->addCell(ndcell);
01178                     good_keypoints.push_back(keypoints[i]);
01179                 }
01180             }
01181         }
01182         else
01183         {
01184             assert(nonMean = false); // Not implemented / used.
01185             Eigen::Vector3d mean;
01186             Eigen::Matrix3d cov;
01187             cameraParams.computeParamsAtIndex(depthImage,keypoints[i],supportSize,mean,cov);
01188             NDTCell *ndcell = new NDTCell();
01189             ndcell->setMean(mean);
01190             ndcell->setCov(cov);
01191 
01192             if(ndcell->hasGaussian_)
01193             {
01194                 Eigen::Vector3d evals = ndcell->getEvals();
01195                 //std::cout<<evals.transpose()<<std::endl;
01196                 if(sqrt(evals(2)) < maxVar)
01197                 {
01198                     mean = ndcell->getMean();
01199                     mn.x = mean(0);
01200                     mn.y = mean(1);
01201                     mn.z = mean(2);
01202                     cloudOut.points.push_back(mn);
01203                     ndcell->setCenter(mn);
01204                     cl->addCell(ndcell);
01205                     good_keypoints.push_back(keypoints[i]);
01206                 }
01207             }
01208 
01209         }
01210     }
01211 
01212     //TODO
01213     keypoints = good_keypoints;
01214     return cloudOut;
01215 }
01216 
01219 void NDTMap::computeNDTCells(int cellupdatemode, unsigned int maxnumpoints, float occupancy_limit, Eigen::Vector3d origin, double sensor_noise)
01220 {
01221     CellVector *cv = dynamic_cast<CellVector*>(index_);
01222 
01223     conflictPoints.clear();
01224 #ifdef REFACTORED
01225     typename std::set<NDTCell*>::iterator it = update_set.begin();
01226     while (it != update_set.end())
01227     {
01228         NDTCell *cell = *it;
01229         if(cell!=NULL)
01230         {
01231             cell->computeGaussian(cellupdatemode,maxnumpoints, occupancy_limit, origin,sensor_noise);
01233             if(cell->points_.size()>0)
01234             {
01235                 for(unsigned int i=0; i<cell->points_.size(); i++) conflictPoints.push_back(cell->points_[i]);
01236                 cell->points_.clear();
01237             }
01238             if (cv!=NULL)
01239             {
01240                 // Set the mean to the cell's centre.
01241                 Eigen::Vector3d mean = cell->getMean();
01242                 pcl::PointXYZ pt;
01243                 pt.x = mean[0];
01244                 pt.y = mean[1];
01245                 pt.z = mean[2];
01246 
01247                 cell->setCenter(pt);
01248             }
01249         }
01250         else
01251         {
01252 
01253         }
01254         it++;
01255     }
01256     update_set.clear();
01257 
01258     CellVector *cl = dynamic_cast<CellVector*>(index_);
01259     if(cl!=NULL)
01260     {
01261         cl->initKDTree();
01262     }
01263 #else
01264 typename SpatialIndex::CellVectorItr it = index_->begin();
01265     //typename std::set<NDTCell<PointT>*>::iterator it = update_set.begin();
01266     while (it != index_->end())
01267     //while (it != update_set.end())
01268     {
01269         NDTCell *cell = (*it);
01270         //NDTCell<PointT> *cell = *it;
01271         if(cell!=NULL)
01272         {
01273             cell->computeGaussian(cellupdatemode,maxnumpoints, occupancy_limit, origin,sensor_noise);
01275             if(cell->points_.size()>0)
01276             {
01277                 for(unsigned int i=0; i<cell->points_.size(); i++) conflictPoints.push_back(cell->points_[i]);
01278                 cell->points_.clear();
01279             }
01280             if (cv!=NULL)
01281             {
01282                 // Set the mean to the cell's centre.
01283                 Eigen::Vector3d mean = cell->getMean();
01284                 pcl::PointXYZ pt;
01285                 pt.x = mean[0];
01286                 pt.y = mean[1];
01287                 pt.z = mean[2];
01288 
01289                 cell->setCenter(pt);
01290             }
01291         }
01292         else
01293         {
01294 
01295         }
01296         it++;
01297     }
01298     //update_set.clear();
01299 
01300     CellVector *cl = dynamic_cast<CellVector*>(index_);
01301     if(cl!=NULL)
01302     {
01303         cl->initKDTree();
01304     }
01305 #endif
01306     
01307     
01308 }
01309 
01310 
01311 
01315 void NDTMap::computeNDTCellsSimple()
01316 {
01317     CellVector *cv = dynamic_cast<CellVector*>(index_);
01318 
01319     conflictPoints.clear();
01320 
01321     typename SpatialIndex::CellVectorItr it = index_->begin();
01322 
01323     while (it != index_->end())
01324     {
01325         NDTCell *cell = (*it);
01326         if(cell!=NULL)
01327         {
01328             cell->computeGaussianSimple();
01329           
01330             if (cv!=NULL)
01331             {
01332                 // Set the mean to the cell's centre.
01333                 Eigen::Vector3d mean = cell->getMean();
01334                 pcl::PointXYZ pt;
01335                 pt.x = mean[0];
01336                 pt.y = mean[1];
01337                 pt.z = mean[2];
01338 
01339                 cell->setCenter(pt);
01340             }
01341         }
01342         else
01343         {
01344 
01345         }
01346         it++;
01347     }
01348 
01349     CellVector *cl = dynamic_cast<CellVector*>(index_);
01350     if(cl!=NULL)
01351     {
01352         cl->initKDTree();
01353     }
01354 }
01355 
01358 int NDTMap::writeToJFF(const char* filename)
01359 {
01360 
01361     if(filename == NULL)
01362     {
01363         //ERR("problem outputing to jff\n");
01364         return -1;
01365     }
01366 
01367     FILE * jffout = fopen(filename, "w+b");
01368 
01369     fwrite(_JFFVERSION_, sizeof(char), strlen(_JFFVERSION_), jffout);
01370 
01371     switch(this->getMyIndexInt())
01372     {
01373     case 1:
01374         writeCellVectorJFF(jffout);
01375         break;
01376     case 2:
01377         //writeOctTreeJFF(jffout);
01378         break;
01379     case 3:
01380         writeLazyGridJFF(jffout);
01381         break;
01382     default:
01383         //ERR("unknown index type\n");
01384         return -1;
01385     }
01386 
01387     fclose(jffout);
01388 
01389     return 0;
01390 }
01391 
01392 
01393 int NDTMap::writeCellVectorJFF(FILE * jffout)
01394 {
01395     int indexType[1] = {1};
01396     fwrite(indexType, sizeof(int), 1, jffout);
01397 
01398     // TODO: add CellVector specific stuff
01399 
01400     typename SpatialIndex::CellVectorItr it = index_->begin();
01401     while (it != index_->end())
01402     {
01403         NDTCell *cell = (*it);
01404         if(cell!=NULL)
01405         {
01406             if(cell->hasGaussian_)
01407             {
01408                 // TODO: add index specific content smartly
01409                 if(cell->writeToJFF(jffout) < 0)
01410                     return -1;
01411             }
01412         }
01413         else
01414         {
01415             // do nothing
01416         }
01417         it++;
01418     }
01419 
01420     return 0;
01421 
01422 }
01423 
01424 
01425 int NDTMap::writeOctTreeJFF(FILE * jffout)
01426 {
01427     int indexType[1] = {2};
01428     fwrite(indexType, sizeof(int), 1, jffout);
01429 
01430     // TODO: add OctTree specific stuff
01431 
01432     typename SpatialIndex::CellVectorItr it = index_->begin();
01433     while (it != index_->end())
01434     {
01435         NDTCell *cell = (*it);
01436         if(cell!=NULL)
01437         {
01438             if(cell->hasGaussian_)
01439             {
01440                 // TODO: add index specific content smartly
01441                 if(cell->writeToJFF(jffout) < 0)
01442                     return -1;
01443             }
01444         }
01445         else
01446         {
01447             // do nothing
01448         }
01449         it++;
01450     }
01451 
01452     return 0;
01453 
01454 }
01455 
01456 int NDTMap::writeLazyGridJFF(FILE * jffout)
01457 {
01458     int indexType[1] = {3};
01459     fwrite(indexType, sizeof(int), 1, jffout);
01460 
01461     // add LazyGrid specific stuff
01462     double sizeXmeters, sizeYmeters, sizeZmeters;
01463     double cellSizeX, cellSizeY, cellSizeZ;
01464     double centerX, centerY, centerZ;
01465     LazyGrid *ind = dynamic_cast<LazyGrid*>(index_);
01466 
01467     ind->getGridSizeInMeters(sizeXmeters, sizeYmeters, sizeZmeters);
01468     ind->getCellSize(cellSizeX, cellSizeY, cellSizeZ);
01469     ind->getCenter(centerX, centerY, centerZ);
01470 
01471     double lazyGridData[9] = { sizeXmeters, sizeYmeters, sizeZmeters,
01472                                cellSizeX,   cellSizeY,   cellSizeZ,
01473                                centerX,     centerY,     centerZ
01474                              };
01475 
01476     fwrite(lazyGridData, sizeof(double), 9, jffout);
01477 
01478     fwrite(ind->getProtoType(), sizeof(NDTCell), 1, jffout);
01479 
01480     // loop through all active cells
01481     typename SpatialIndex::CellVectorItr it = index_->begin();
01482     while (it != index_->end())
01483     {
01484         if((*it)->writeToJFF(jffout) < 0)       return -1;
01485         it++;
01486     }
01487 
01488     return 0;
01489 
01490 }
01491 
01506 int NDTMap::loadFromJFF(const char* filename)
01507 {
01508 
01509     FILE * jffin;
01510 
01511     if(filename == NULL)
01512     {
01513         JFFERR("problem outputing to jff");
01514     }
01515 
01516     jffin = fopen(filename,"r+b");
01517 
01518     char versionBuf[16];
01519     if(fread(&versionBuf, sizeof(char), strlen(_JFFVERSION_), jffin) <= 0)
01520     {
01521         JFFERR("reading version failed");
01522     }
01523     versionBuf[strlen(_JFFVERSION_)] = '\0';
01524 
01525     int indexType;
01526     if(fread(&indexType, sizeof(int), 1, jffin) <= 0)
01527     {
01528         JFFERR("reading version failed");
01529     }
01530 
01531     if(indexType != this->getMyIndexInt())
01532     {
01533         switch(indexType)
01534         {
01535         case 1:
01536             std::cerr << "Map uses CellVector\n";
01537             return -1;
01538             break;
01539         case 2:
01540             std::cerr << "Map uses OctTree\n";
01541             return -2;
01542             break;
01543         case 3:
01544             std::cerr << "Map uses LazyGrid\n";
01545             return -3;
01546             break;
01547         }
01548     }
01549 
01550     switch(indexType)
01551     {
01552     case 1:
01553     {
01554         CellVector* cv = dynamic_cast<CellVector * >(index_);
01555         if(cv->loadFromJFF(jffin) < 0)
01556         {
01557             JFFERR("Error loading CellVector");
01558         }
01559         break;
01560     }
01561 #if 0
01562     case 2:
01563     {
01564         OctTree* tr = dynamic_cast<OctTree*>(index_);
01565         if(tr->loadFromJFF(jffin) < 0)
01566         {
01567             JFFERR("Error loading OctTree");
01568         }
01569         break;
01570     }
01571 #endif
01572     case 3:
01573     {
01574         std::cerr << "Map uses LazyGrid\n";
01575         LazyGrid* gr = dynamic_cast<LazyGrid*>(index_);
01576         if(gr->loadFromJFF(jffin) < 0)
01577         {
01578             JFFERR("Error loading LazyGrid");
01579         }
01580         break;
01581     }
01582     default:
01583         JFFERR("error casting index");
01584     }
01585 
01586     NDTCell *ptCell = new NDTCell();
01587     index_->setCellType(ptCell);
01588     delete ptCell;
01589 
01590     fclose(jffin);
01591 
01592    // std::cout << "map loaded successfully " << versionBuf << std::endl;
01593 
01594     isFirstLoad_ = false;
01595 
01596     return 0;
01597 
01598 }
01599 
01600 
01602 std::string NDTMap::getMyIndexStr() const
01603 {
01604     CellVector* cl = dynamic_cast<CellVector * >(index_);
01605     if(cl!=NULL)
01606     {
01607         return std::string("CellVector");
01608     }
01609 #if 0
01610     OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);
01611     if(tr!=NULL)
01612     {
01613         return std::string("OctTree");
01614     }
01615 #endif
01616     LazyGrid *gr = dynamic_cast<LazyGrid*>(index_);
01617     if(gr!=NULL)
01618     {
01619         return std::string("LazyGrid<PointT>");
01620     }
01621 
01622     return std::string("Unknown index type");
01623 }
01624 
01626 int NDTMap::getMyIndexInt() const
01627 {
01628     CellVector* cl = dynamic_cast<CellVector * >(index_);
01629     if(cl!=NULL)
01630     {
01631         return 1;
01632     }
01633 #if 0
01634     OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);
01635     if(tr!=NULL)
01636     {
01637         return 2;
01638     }
01639 #endif
01640     LazyGrid *gr = dynamic_cast<LazyGrid*>(index_);
01641     if(gr!=NULL)
01642     {
01643         return 3;
01644     }
01645 
01646     return -1;
01647 }
01648 
01649 //computes the *negative log likelihood* of a single observation
01650 double NDTMap::getLikelihoodForPoint(pcl::PointXYZ pt)
01651 {
01652     //assert(false);
01653     double uniform=0.00100;
01654     NDTCell* ndCell = NULL;
01655     
01656 #if 0
01657     OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);
01658 
01659     if(tr==NULL)
01660     {
01661 #endif
01662         LazyGrid *gr = dynamic_cast<LazyGrid*>(index_);
01663         if(gr==NULL)
01664         {
01665             //cout<<"bad index - getLikelihoodForPoint\n";
01666             return uniform;
01667         }
01668         ndCell = gr->getClosestNDTCell(pt);
01669 #if 0
01670     }
01671     else
01672     {
01673         ndCell = tr->getClosestNDTCell(pt);
01674     }
01675 #endif
01676     if(ndCell == NULL) return uniform;
01677 
01678     double prob = ndCell->getLikelihood(pt);
01679     prob = (prob<0) ? 0 : prob; //uniform!! TSV
01680     return prob;
01681 }
01682 
01683 /*
01684 //use trilinear interpolation from available immediate neighbors
01685 template<typename PointT>
01686 double NDTMap<PointT>::getLikelihoodForPointWithInterpolation(PointT pt) {
01687 
01688     //ATM only for grid map
01689     //     tll------tlr
01690     //     /|       /|
01691     //    / |      / |
01692     //  tul------tur |    z
01693     //   | bll----|-blr   ^  y
01694     //   | /      | /     | /
01695     //   |/       |/      |/
01696     //  bul------bur      ---> x
01697     double uniform=0;//0.00100;
01698     Cell* cell = NULL;
01699     NDTCell<PointT>* ndCell = NULL;
01700     double cumProb = 0;
01701     double weight = 0;
01702     int evals = 1;
01703 
01704     LazyGrid<PointT> *gr = dynamic_cast<LazyGrid<PointT>*>(index_);
01705     if(gr==NULL) {
01706         //cout<<"bad index - getLikelihoodForPointWithInterpolation\n";
01707         return uniform;
01708     }
01709     cell = gr->getCellForPoint(pt);
01710     if(cell == NULL) return uniform;
01711 
01712 
01713     //get coordinates of cell
01714     int indXn, indYn, indZn;
01715     PointT centerGrid, sizeCell, centerCell;
01716     int sizeGridX, sizeGridY,sizeGridZ;
01717     centerCell = cell->getCenter();
01718     gr->getCenter(centerGrid.x,centerGrid.y,centerGrid.z);
01719     gr->getGridSize(sizeGridX,sizeGridY,sizeGridZ);
01720     gr->getCellSize(sizeCell.x,sizeCell.y,sizeCell.z);
01721     gr->getIndexForPoint(pt,indXn,indYn,indZn);
01722 
01723     double x,y,z;
01724     x = (pt.x - centerCell.x)/sizeCell.x;
01725     y = (pt.y - centerCell.y)/sizeCell.y;
01726     z = (pt.z - centerCell.z)/sizeCell.z;
01727     if(x <0 ) x = 0;
01728     if(y <0 ) y = 0;
01729     if(z <0 ) z = 0;
01730     if(x >1 ) x = 1;
01731     if(y >1 ) y = 1;
01732     if(z >1 ) z = 1;
01733 
01734     //bul
01735     double prob = 0;
01736     ndCell = dynamic_cast<NDTCell<PointT>*> (cell);
01737     if(ndCell != NULL) {
01738         prob = ndCell->getLikelihood(pt);
01739         prob = (prob<0) ? uniform : prob;
01740         weight = (1 - x + 1 - y + 1 - z)/(3.0);
01741         if(weight < 0) cerr<<weight<<endl;
01742         cumProb += prob*weight;
01743         //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
01744         evals++;
01745     }
01746 
01747     //tul
01748     Cell* c = gr->getCellAt(indXn,indYn,indZn+1);
01749     if(c != NULL) {
01750         ndCell = dynamic_cast<NDTCell<PointT>*> (c);
01751         if(ndCell != NULL) {
01752             prob = ndCell->getLikelihood(pt);
01753             prob = (prob<0) ? uniform : prob;
01754             weight = (1 - x + 1 - y + z)/(3.0);
01755             if(weight < 0) cerr<<weight<<endl;
01756             cumProb += prob*weight;
01757             //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
01758             evals++;
01759         }
01760     }
01761     //tur
01762     c = gr->getCellAt(indXn+1,indYn,indZn+1);
01763     if(c != NULL) {
01764         ndCell = dynamic_cast<NDTCell<PointT>*> (c);
01765         if(ndCell != NULL) {
01766             prob = ndCell->getLikelihood(pt);
01767             prob = (prob<0) ? uniform : prob;
01768             weight = (x + 1-y + z )/(3.0);
01769             if(weight < 0) cerr<<weight<<endl;
01770             cumProb += prob*weight;
01771             //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
01772             evals++;
01773         }
01774     }
01775     //tll
01776     c = gr->getCellAt(indXn,indYn+1,indZn+1);
01777     if(c != NULL) {
01778         ndCell = dynamic_cast<NDTCell<PointT>*> (c);
01779         if(ndCell != NULL) {
01780             prob = ndCell->getLikelihood(pt);
01781             prob = (prob<0) ? uniform : prob;
01782             weight = (1-x + y + z )/(3.0);
01783             if(weight < 0) cerr<<weight<<endl;
01784             cumProb += prob*weight;
01785             //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
01786             evals++;
01787         }
01788     }
01789     //tlr
01790     c = gr->getCellAt(indXn+1,indYn+1,indZn+1);
01791     if(c != NULL) {
01792         ndCell = dynamic_cast<NDTCell<PointT>*> (c);
01793         if(ndCell != NULL) {
01794             prob = ndCell->getLikelihood(pt);
01795             prob = (prob<0) ? uniform : prob;
01796             weight = (x + y + z )/(3.0);
01797             if(weight < 0) cerr<<weight<<endl;
01798             cumProb += prob*weight;
01799             //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
01800             evals++;
01801         }
01802     }
01803     //bur
01804     c = gr->getCellAt(indXn+1,indYn,indZn);
01805     if(c != NULL) {
01806         ndCell = dynamic_cast<NDTCell<PointT>*> (c);
01807         if(ndCell != NULL) {
01808             prob = ndCell->getLikelihood(pt);
01809             prob = (prob<0) ? uniform : prob;
01810             weight = (x + 1-y + 1-z )/(3.0);
01811             if(weight < 0) cerr<<weight<<endl;
01812             cumProb += prob*weight;
01813             //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
01814             evals++;
01815         }
01816     }
01817     //bll
01818     c = gr->getCellAt(indXn,indYn+1,indZn);
01819     if(c != NULL) {
01820         ndCell = dynamic_cast<NDTCell<PointT>*> (c);
01821         if(ndCell != NULL) {
01822             prob = ndCell->getLikelihood(pt);
01823             prob = (prob<0) ? uniform : prob;
01824             weight = (1-x + y + 1-z )/(3.0);
01825             if(weight < 0) cerr<<weight<<endl;
01826             cumProb += prob*weight;
01827             //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
01828             evals++;
01829         }
01830     }
01831     //blr
01832     c = gr->getCellAt(indXn+1,indYn+1,indZn);
01833     if(c != NULL) {
01834         ndCell = dynamic_cast<NDTCell<PointT>*> (c);
01835         if(ndCell != NULL) {
01836             prob = ndCell->getLikelihood(pt);
01837             prob = (prob<0) ? uniform : prob;
01838             weight = (x + y + 1-z )/(3.0);
01839             if(weight < 0) cerr<<weight<<endl;
01840             cumProb += prob*weight;
01841             //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
01842             evals++;
01843         }
01844     }
01845 
01846     //cout<<"== "<<cumProb<<endl;
01847     return cumProb;
01848 }
01849 */
01850 
01851 std::vector<NDTCell*> NDTMap::getInitializedCellsForPoint(const pcl::PointXYZ pt) const
01852 {
01853     std::vector<NDTCell*> cells;
01854     LazyGrid *gr = dynamic_cast<LazyGrid*>(index_);
01855     if(gr==NULL)
01856     {
01857         //cout<<"bad index - getCellsForPoint\n";
01858         return cells;
01859     }
01860     cells = gr->getClosestCells(pt);
01861     return cells;
01862 
01863 }
01864 
01865 std::vector<NDTCell*> NDTMap::getCellsForPoint(const pcl::PointXYZ pt, int n_neigh, bool checkForGaussian) const
01866 {
01867     //assert(false);
01868     std::vector<NDTCell*> cells;
01869     LazyGrid *gr = dynamic_cast<LazyGrid*>(index_);
01870     if(gr==NULL)
01871     {
01872         //cout<<"bad index - getCellsForPoint\n";
01873         return cells;
01874     }
01875     cells = gr->getClosestNDTCells(pt,n_neigh,checkForGaussian);
01876     return cells;
01877 
01878     //OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);
01879     //if(tr==NULL) {
01880     //}
01881 }
01882 
01883 bool NDTMap::getCellForPoint(const pcl::PointXYZ &pt, NDTCell* &out_cell, bool checkForGaussian) const
01884 {
01885 
01886     out_cell = NULL;
01887     CellVector *cl = dynamic_cast<CellVector*>(index_);
01888     if(cl!=NULL)
01889     {
01890         out_cell = cl->getClosestNDTCell(pt);
01891         return true;
01892     }
01893 #if 0
01894     OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);
01895     if(tr!=NULL)
01896     {
01897         out_cell = tr->getClosestNDTCell(pt);
01898         return true;
01899     }
01900 #endif
01901     LazyGrid *gr = dynamic_cast<LazyGrid*>(index_);
01902     if(gr!=NULL)
01903     {
01904         out_cell = gr->getClosestNDTCell(pt,checkForGaussian);
01905         return true;
01906     }
01907     //cout<<"bad index - getCellForPoint\n";
01908     return false;
01909 }
01910 
01911 NDTMap* NDTMap::pseudoTransformNDTMap(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T)
01912 {
01913     NDTMap* map = new NDTMap(new CellVector());
01914     CellVector* idx = dynamic_cast<CellVector*> (map->getMyIndex());
01915     typename SpatialIndex::CellVectorItr it = index_->begin();
01916 
01917     while (it != index_->end())
01918     {
01919         NDTCell *cell = (*it);
01920         if(cell->hasGaussian_)
01921         {
01922             Eigen::Vector3d mean = cell->getMean();
01923             Eigen::Matrix3d cov = cell->getCov();
01924             mean = T*mean;
01926             cov = T.rotation()*cov*T.rotation().transpose();
01927             NDTCell* nd = (NDTCell*)cell->clone();
01928             nd->setMean(mean);
01929             nd->setCov(cov);
01930             idx->addNDTCell(nd);
01931         }
01932         it++;
01933     }
01934     return map;
01935 }
01936 
01937 std::vector<NDTCell*> NDTMap::pseudoTransformNDT(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T)
01938 {
01939 
01940     std::vector<NDTCell*> ret;
01941     typename SpatialIndex::CellVectorItr it = index_->begin();
01942     while (it != index_->end())
01943     {
01944         NDTCell *cell = (*it);
01945         if(cell!=NULL)
01946         {
01947             if(cell->hasGaussian_)
01948             {
01949                 Eigen::Vector3d mean = cell->getMean();
01950                 Eigen::Matrix3d cov = cell->getCov();
01951                 mean = T*mean;
01953                 cov = T.rotation()*cov*T.rotation().transpose();
01954                 NDTCell* nd = (NDTCell*)cell->clone();
01955                 nd->setMean(mean);
01956                 nd->setCov(cov);
01957                 ret.push_back(nd);
01958             }
01959         }
01960         else
01961         {
01962             //ERR("problem casting cell to NDT!\n");
01963         }
01964         it++;
01965     }
01966     return ret;
01967 }
01968 
01969 NDTCell* NDTMap::getCellIdx(unsigned int idx) const
01970 {
01971     CellVector *cl = dynamic_cast<CellVector*>(index_);
01972     if (cl != NULL)
01973     {
01974         return cl->getCellIdx(idx);
01975     }
01976     return NULL;
01977 }
01978 
01979 std::vector<lslgeneric::NDTCell*> NDTMap::getAllCells() const
01980 {
01981 
01982     std::vector<NDTCell*> ret;
01983     typename SpatialIndex::CellVectorItr it = index_->begin();
01984     while (it != index_->end())
01985     {
01986         NDTCell *cell = (*it);
01987         if(cell->hasGaussian_)
01988         {
01989             NDTCell* nd = cell->copy();
01990             ret.push_back(nd);
01991         }
01992         it++;
01993     }
01994     return ret;
01995 }
01996 
01997 std::vector<lslgeneric::NDTCell*> NDTMap::getAllInitializedCells()
01998 {
01999     std::vector<NDTCell*> ret;
02000     typename SpatialIndex::CellVectorItr it = index_->begin();
02001     while (it != index_->end())
02002     {
02003         NDTCell* nd = (*it)->copy();
02004         ret.push_back(nd);
02005         it++;
02006     }
02007     return ret;
02008 }
02009 
02010 int NDTMap::numberOfActiveCells()
02011 {
02012     int ret = 0;
02013     if(index_ == NULL) return ret;
02014     typename SpatialIndex::CellVectorItr it = index_->begin();
02015     while (it != index_->end())
02016     {
02017         if((*it)->hasGaussian_)
02018         {
02019             ret++;
02020         }
02021         it++;
02022     }
02023     return ret;
02024 }
02025 
02026 }


ndt_map
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Wed Aug 26 2015 15:24:40