ndt_histogram.hpp
Go to the documentation of this file.
00001 #include <pointcloud_utils.h>
00002 #include <pcl/registration/transformation_estimation_svd.h>
00003 
00004 namespace lslgeneric
00005 {
00006 
00007 template <typename PointT>
00008 NDTHistogram<PointT>::NDTHistogram()
00009 {
00010     N_LINE_BINS = 1;
00011     N_FLAT_BINS = 40;
00012     N_SPHERE_BINS = 10;
00013 
00014     histogramBinsLine = std::vector<int>(N_LINE_BINS,0);
00015     histogramBinsFlat = std::vector<int>(N_FLAT_BINS,0);
00016     histogramBinsSphere = std::vector<int>(N_SPHERE_BINS,0);
00017 
00018     for(int i=0; i<3; i++)
00019     {
00020         dist_histogramBinsLine[i] = std::vector<int>(N_LINE_BINS,0);
00021         dist_histogramBinsFlat[i] = std::vector<int>(N_FLAT_BINS,0);
00022         dist_histogramBinsSphere[i] = std::vector<int>(N_SPHERE_BINS,0);
00023     }
00024 
00025     D1 = 5;
00026     D2 = 10;
00027 
00028     averageDirections = std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> >(N_FLAT_BINS,Eigen::Vector3d(0,0,0));
00029     computeDirections();
00030 
00031     topThree.reserve(3);
00032     for(int r=0; r<3; r++)
00033     {
00034         topThree[r].setIdentity();
00035         topThreeS[r] = INT_MAX;
00036     }
00037     inited = true;
00038 }
00039 
00040 template <typename PointT>
00041 NDTHistogram<PointT>::NDTHistogram (const NDTHistogram<PointT>& other)
00042 {
00043 
00044     histogramBinsLine =   other.histogramBinsLine;
00045     histogramBinsFlat =   other.histogramBinsFlat;
00046     histogramBinsSphere = other.histogramBinsSphere;
00047 
00048     for(int i=0; i<3; i++)
00049     {
00050         dist_histogramBinsLine[i] =   other.dist_histogramBinsLine[i];
00051         dist_histogramBinsFlat[i] =   other.dist_histogramBinsFlat[i];
00052         dist_histogramBinsSphere[i] = other.dist_histogramBinsSphere[i];
00053     }
00054 
00055     D1 = 5;
00056     D2 = 10;
00057 
00058     averageDirections = other.averageDirections;
00059     directions = other.directions;
00060 
00061     topThree.reserve(3);
00062     for(int r=0; r<3; r++)
00063     {
00064         topThree[r].setIdentity();
00065         topThreeS[r] = INT_MAX;
00066     }
00067     inited = true;
00068 }
00069 
00070 template <typename PointT>
00071 NDTHistogram<PointT>::NDTHistogram (NDTMap<PointT> &map)
00072 {
00073 
00074     N_LINE_BINS = 1;
00075     N_FLAT_BINS = 40;
00076     N_SPHERE_BINS = 10;
00077 
00078     histogramBinsLine = std::vector<int>(N_LINE_BINS,0);
00079     histogramBinsFlat = std::vector<int>(N_FLAT_BINS,0);
00080     histogramBinsSphere = std::vector<int>(N_SPHERE_BINS,0);
00081 
00082     for(int i=0; i<3; i++)
00083     {
00084         dist_histogramBinsLine[i] = std::vector<int>(N_LINE_BINS,0);
00085         dist_histogramBinsFlat[i] = std::vector<int>(N_FLAT_BINS,0);
00086         dist_histogramBinsSphere[i] = std::vector<int>(N_SPHERE_BINS,0);
00087     }
00088 
00089     D1 = 5;
00090     D2 = 10;
00091 
00092     averageDirections = std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> >(N_FLAT_BINS,Eigen::Vector3d(0,0,0));
00093     //populate directions
00094     computeDirections();
00095     constructHistogram(map);
00096 
00097     topThree.reserve(3);
00098     for(int r=0; r<3; r++)
00099     {
00100         topThree[r].setIdentity();
00101         topThreeS[r] = INT_MAX;
00102     }
00103 
00104     inited = true;
00105 }
00106 
00107 template <typename PointT>
00108 void NDTHistogram<PointT>::computeDirections()
00109 {
00110 
00111     double dlong = M_PI*(3-sqrt(5));  /* ~2.39996323 */
00112     double dz    = 2.0/N_FLAT_BINS;
00113     double longitude = 0;
00114     double z    = 1 - dz/2;
00115 
00116     for (int k = 0; k<N_FLAT_BINS; k++)
00117     {
00118         double r    = sqrt(1-z*z);
00119         Eigen::Vector3d v;
00120         v<<cos(longitude)*r, sin(longitude)*r, z;
00121         directions.push_back(v);
00122         z    = z - dz;
00123         longitude = longitude + dlong;
00124     }
00125 }
00126 
00127 template <typename PointT>
00128 void NDTHistogram<PointT>::constructHistogram(NDTMap<PointT> &map)
00129 {
00130 
00131     SpatialIndex<PointT> *si = map.getMyIndex();
00132     if(si==NULL) return;
00133 
00134     double LINEAR_FACTOR = 50;
00135     double FLAT_FACTOR = 50;
00136 
00137     typename std::vector<Cell<PointT>*>::iterator it = si->begin();
00138     while(it!=si->end())
00139     {
00140 
00141         NDTCell<PointT> *ndcell = dynamic_cast<NDTCell<PointT>*>(*it);
00142         if(ndcell == NULL)
00143         {
00144             it++;
00145             continue;
00146         }
00147         if(!ndcell->hasGaussian_)
00148         {
00149             it++;
00150             continue;
00151         }
00152         Eigen::Matrix3d evecs = ndcell->getEvecs();
00153         Eigen::Vector3d evals = ndcell->getEvals();
00154 
00155         int idMin,idMax,idMid;
00156         double minEval = evals.minCoeff(&idMin);
00157         double maxEval = evals.maxCoeff(&idMax);
00158         double midEval = -1;
00159         idMid = -1;
00160         for(int j=0; j<3; j++)
00161         {
00162             if(j!=idMin && j!=idMax)
00163             {
00164                 midEval = evals(j);
00165                 idMid = j;
00166             }
00167         }
00168 
00169         double dist = ndcell->getMean().norm();
00170 
00171         //three cases:
00172         //maxEval >> midEval -> linear
00173         if(maxEval > midEval*LINEAR_FACTOR)
00174         {
00175             incrementLineBin(dist);
00176             it++;
00177             continue;
00178         }
00179 
00180         //maxEval ~ midEval >> minEval -> planar
00181         if(midEval > minEval*FLAT_FACTOR)
00182         {
00183             Eigen::Vector3d normal = evecs.col(idMin);
00184             Eigen::Vector3d mean = ndcell->getMean();
00185             if(normal.dot(mean) < 0)
00186             {
00187 //              std::cout<<"switching normal direction\n";
00188                 normal = -normal;
00189             }
00190             incrementFlatBin(normal,dist);
00191             it++;
00192             continue;
00193         }
00194 
00195         //maxEval ~ midEval ~ minEval -> spherical
00196         incrementSphereBin(dist);
00197 
00198         it++;
00199     }
00200 
00201     for(int i=0; i<averageDirections.size(); i++)
00202     {
00203         averageDirections[i].normalize();
00204     }
00205 
00206 }
00207 
00208 template <typename PointT>
00209 void NDTHistogram<PointT>::incrementLineBin(double d)
00210 {
00211     histogramBinsLine[0] ++;
00212 
00213     if(d<D1) dist_histogramBinsLine[0][0] ++;
00214     else if(d>D2) dist_histogramBinsLine[2][0] ++;
00215     else dist_histogramBinsLine[1][0] ++;
00216 }
00217 
00218 template <typename PointT>
00219 void NDTHistogram<PointT>::incrementFlatBin(Eigen::Vector3d &normal, double d)
00220 {
00221     //std::cout<<"n "<<normal.transpose()<<std::endl;
00222     normal.normalize();
00223     //bins are in 3D. go through directions, find smallest difference
00224     double mindist = INT_MAX;
00225     int idmin = -1;
00226     for(unsigned int i=0; i<directions.size(); i++)
00227     {
00228         double dist = (directions[i]-normal).norm();
00229         if(mindist > dist)
00230         {
00231             mindist = dist;
00232             idmin = i;
00233         }
00234     }
00235     //std::cout<<idmin<<std::endl;
00236     if(idmin >=0 && idmin < histogramBinsFlat.size())
00237     {
00238         histogramBinsFlat[idmin] ++;
00239         averageDirections[idmin] += normal;
00240         if(d<D1) dist_histogramBinsFlat[0][idmin] ++;
00241         else if(d>D2) dist_histogramBinsFlat[2][idmin] ++;
00242         else dist_histogramBinsFlat[1][idmin] ++;
00243     }
00244 }
00245 
00246 template <typename PointT>
00247 void NDTHistogram<PointT>::incrementSphereBin(double d)
00248 {
00249     histogramBinsSphere[0] ++;
00250     if(d<D1)
00251     {
00252         int id = floor(((double)d*N_SPHERE_BINS)/(double)D1);
00253         dist_histogramBinsSphere[0][id] ++;
00254     }
00255     else if(d>D2)
00256     {
00257         dist_histogramBinsSphere[2][0] ++;
00258     }
00259     else
00260     {
00261         int id = floor(((double)(d-D1)*N_SPHERE_BINS)/(double)D2);
00262         dist_histogramBinsSphere[1][id] ++;
00263     }
00264 }
00265 
00266 
00267 template <typename PointT>
00268 pcl::PointCloud<PointT> NDTHistogram<PointT>::getDominantDirections(int nDirections)
00269 {
00270 
00271     pcl::PointCloud<PointT> ret;
00272     std::vector<bool> dominated (directions.size(),false);
00273     double NORM_MIN = 0.2;
00274     int MIN_SUPPORT = 3;
00275 
00276     for(int i=0; i<nDirections; i++)
00277     {
00278         //get the next direction
00279         PointT current;
00280         //find max in histogram, that is not dominated
00281         bool found = false;
00282         int maxBin, idMax;
00283         while (!found)
00284         {
00285             maxBin = -1;
00286             idMax = -1;
00287             for(int j=0; j<histogramBinsFlat.size(); j++)
00288             {
00289                 if(histogramBinsFlat[j] > maxBin && !dominated[j])
00290                 {
00291                     maxBin = histogramBinsFlat[j];
00292                     idMax = j;
00293                 }
00294             }
00295 
00296             found = !dominated[idMax];
00297             //check if any of the already found directions are "duals"
00298             /* for(int j=0; j<ret.points.size() && found; j++) {
00299             Eigen::Vector3d v(ret.points[j].x,ret.points[j].y,ret.points[j].z);
00300             v = v + directions[idMax];
00301             found = found && (v.norm() > NORM_MIN);
00302              }*/
00303             //suppress this max and neighbours --- independent of the value of found, this is necessarry
00304             dominated[idMax] = true;
00305             if(idMax-1 >=0) dominated[idMax-1] = true;
00306             if(idMax+1 <dominated.size()) dominated[idMax+1] = true;
00307             if(maxBin < MIN_SUPPORT) break;
00308         }
00309 
00310         if(maxBin < MIN_SUPPORT) break;
00311         //current.x = directions[idMax](0);
00312         //current.y = directions[idMax](1);
00313         //current.z = directions[idMax](2);
00314         current.x = averageDirections[idMax](0);
00315         current.y = averageDirections[idMax](1);
00316         current.z = averageDirections[idMax](2);
00317         //current.intensity = maxBin;
00318         //std::cout<<directions[idMax].transpose()<<" e "<<maxBin<<std::endl;
00319         //std::cout<<averageDirections[idMax].transpose()<<" e "<<maxBin<<std::endl;
00320         ret.points.push_back(current);
00321     }
00322 
00323     return ret;
00324 }
00325 
00326 template <typename PointT>
00327 void NDTHistogram<PointT>::bestFitToHistogram(NDTHistogram<PointT> &target, Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &T, bool bound_transform)
00328 {
00329 
00330     //find the top N dominant directions
00331     int N = 3;
00332     //store vectors as pcl::points
00333     pcl::PointCloud<PointT> dominantBinsMine, dominantBinsTarget;
00334 
00335 //    std::cout<<"d1 : \n";
00336     dominantBinsMine = this->getDominantDirections(N);
00337 //    std::cout<<"d2 : \n";
00338     dominantBinsTarget = target.getDominantDirections(N);
00339 
00340     /*    double N_THIS=0, N_OTHER=0;
00341         for(int i=0; i<histogramBinsFlat.size(); i++) {
00342         N_THIS += histogramBinsFlat[i];
00343         N_OTHER += target.histogramBinsFlat[i];
00344         }
00345     */    //estimate least-squares fit, assuming correspondence
00346     /*    pcl::registration::TransformationEstimationSVD<PointT,PointT> trEst;
00347         Eigen::Matrix4f TR;
00348         trEst.estimateRigidTransformation(dominantBinsMine, dominantBinsTarget, TR);
00349         T = TR.cast<double>();
00350     */
00351     //check for best fitting combination
00352 
00353     for(int r=0; r<3; r++)
00354     {
00355         topThree[r].setIdentity();
00356         topThreeS[r] = INT_MAX;
00357     }
00358     pcl::PointCloud<PointT> mineNew, mineNew2;
00359     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> localT, localT2;
00360 
00361 //    double best = INT_MAX;
00362     //choose 3 out of N
00363     for(int a1 = 0; a1<dominantBinsMine.points.size(); a1++)
00364     {
00365         //int a1 = 0; {
00366         for(int b1 = a1+1; b1<dominantBinsMine.points.size(); b1++)
00367         {
00368             //if(a1 == b1) continue;
00369             //for(int c1 = 0; c1<dominantBinsMine.points.size(); c1++) {
00370             //if(b1 == c1 || a1 == c1) continue;
00371             //int a2 = 0; {
00372             for(int a2 = 0; a2<dominantBinsTarget.points.size(); a2++)
00373             {
00374                 for(int b2 = 0; b2<dominantBinsTarget.points.size(); b2++)
00375                 {
00376                     if(a2 == b2) continue;
00377                     //for(int c2 = 0; c2<dominantBinsTarget.points.size(); c2++) {
00378                     //    if(b2 == c2 || a2 == c2) continue;
00379                     pcl::PointCloud<PointT> useMine, useTarget;
00380                     useMine.points.push_back(dominantBinsMine[a1]);
00381                     useMine.points.push_back(dominantBinsMine[b1]);
00382                     //useMine.points.push_back(dominantBinsMine[c1]);
00383                     useTarget.points.push_back(dominantBinsTarget[a2]);
00384                     useTarget.points.push_back(dominantBinsTarget[b2]);
00385                     //useTarget.points.push_back(dominantBinsTarget[c2]);
00386 
00387                     closedFormSolution(useMine, useTarget, localT2);
00388                     double good2 = this->getSimilarity(target,localT2);
00389                     //      std::cout<<good2<<std::endl;
00390                     //compute score of fitness "good2"
00391                     /*                      double good2 = 0, scale2 = 0;
00392                                             for(int q = 0; q<averageDirections.size(); q++) {
00393 
00394                                                 Eigen::Vector3d tr = localT2*averageDirections[q];
00395                                                 if( this->histogramBinsFlat[q] == 0) {
00396                                                     tr = directions[q]; //fall back to bin direction
00397                                                 }
00398                                                         //find B = the bin in which tr falls
00399                                                 tr.normalize();
00400                                                 //std::cout<<"TR"<<tr.transpose()<<std::endl;
00401                                                 double mindist = INT_MAX;
00402                                                 int idmin = -1;
00403                                                 for(unsigned int i=0; i<directions.size(); i++) {
00404                                                     double dist = (directions[i]-tr).norm();
00405                                                     if(mindist > dist) {
00406                                                         mindist = dist;
00407                                                         idmin = i;
00408                                                     }
00409                                                 }
00410                                                 //std::cout<<idmin<<std::endl;
00411                                                 if(!(idmin >=0 && idmin < histogramBinsFlat.size())) {
00412                                                     continue;
00413                                                 }
00414 
00415 
00416                                                 //find the averageDirection other->average[B]
00417                                                 Eigen::Vector3d other_tr = target.averageDirections[idmin];
00418                                                 if (target.histogramBinsFlat[idmin] == 0) {
00419                                                     other_tr = target.directions[idmin]; //fall back to bin direction
00420                                                 }
00421 
00422                                                 //compute norm of difference, scale by cardinality
00423                                                 //double factor = fabsf((double)(this->histogramBinsFlat[q] - target.histogramBinsFlat[idmin]+1)/
00424                                                 //                      (double)(this->histogramBinsFlat[q]+target.histogramBinsFlat[idmin]+1));
00425                                                 //double factor = this->histogramBinsFlat[q]+target.histogramBinsFlat[idmin];
00426                                                 //scale2 += factor;
00427                                                 //good2 += factor*(tr-other_tr).norm();
00428                                                 good2 += pow((double)this->histogramBinsFlat[q]/N_THIS - (double)target.histogramBinsFlat[idmin]/N_OTHER,2);
00429 
00430                                             }
00431                                             good2 = sqrt(good2);
00432                                             */
00433 
00434                     /*
00435                     //don't use them for finding but just for verifying
00436                     useMine.points.push_back(dominantBinsMine[c1]);
00437                     useTarget.points.push_back(dominantBinsTarget[c2]);
00438                     //calculate goodness of fit
00439                     //                      mineNew = transformPointCloud(localT,useMine);
00440 
00441                     //now also considering last orientation....
00442                     //                      closedFormSolution(useMine, useTarget, localT2);
00443                     mineNew2 = transformPointCloud(localT2,useMine);
00444 
00445                     //                      double good = 0, scale = 0;
00446                     //                      for(int i=0; i<mineNew.points.size(); i++) {
00447                     //                          double factor = (mineNew.points[i].intensity+useTarget.points[i].intensity);
00448                     //                          good += factor*sqrt( pow(mineNew.points[i].x-useTarget.points[i].x,2) +
00449                     //                                  pow(mineNew.points[i].y-useTarget.points[i].y,2) +
00450                     //                                  pow(mineNew.points[i].z-useTarget.points[i].z,2));
00451                     //                          scale += factor;
00452                     //                      }
00453                     //                      good = good/scale;
00454 
00455 
00456                     double good2 = 0, scale2 = 0;
00457                     for(int i=0; i<mineNew2.points.size(); i++) {
00458                     double factor = (mineNew2.points[i].intensity+useTarget.points[i].intensity);
00459                     good2 += factor*sqrt( pow(mineNew2.points[i].x-useTarget.points[i].x,2) +
00460                         pow(mineNew2.points[i].y-useTarget.points[i].y,2) +
00461                         pow(mineNew2.points[i].z-useTarget.points[i].z,2));
00462                     scale2 += factor;
00463                     }
00464                     good2 = good2/scale2;
00465                     */
00466 
00467 //                          std::cout<<"combo "<<a1<<" "<<b1<<" "<<" -- "
00468 //                              <<a2<<" "<<b2<<" "<<" fit = "<<good2<<std::endl;
00469                     /*if(good < best) {
00470                     std::cout<<"local minimum at combo "<<a1<<" "<<b1<<" "<<c1<<" -- "
00471                         <<a2<<" "<<b2<<" "<<c2<<" fit = "<<good<<std::endl;
00472                     best = good;
00473                     T = localT;
00474                     }*/
00475                     Eigen::Quaternion<double> id, errorQ;
00476                     id.setIdentity();
00477                     errorQ = localT2.rotation();
00478                     double angle = fabsf(acos(id.dot(errorQ))/2);
00479                     if(angle > M_PI/8 && bound_transform)
00480                     {
00481                         //transform is too big!
00482                         continue;
00483                     }
00484                     if(good2 < topThreeS[2])
00485                     {
00486 //                              std::cout<<"local minimum at combo "<<a1<<" "<<b1<<" "<<" -- "
00487 //                                  <<a2<<" "<<b2<<" "<<" fit = "<<good2<<std::endl;
00488                         if(good2 < topThreeS[1])
00489                         {
00490                             if(good2 < topThreeS[0])
00491                             {
00492                                 topThree[2] = topThree[1];
00493                                 topThree[1] = topThree[0];
00494                                 topThree[0] = localT2;
00495                                 topThreeS[2] = topThreeS[1];
00496                                 topThreeS[1] = topThreeS[0];
00497                                 topThreeS[0] = good2;
00498                             }
00499                             else
00500                             {
00501                                 topThree[2] = topThree[1];
00502                                 topThree[1] = localT2;
00503                                 topThreeS[2] = topThreeS[1];
00504                                 topThreeS[1] = good2;
00505                             }
00506                         }
00507                         else
00508                         {
00509                             topThree[2] = localT2;
00510                             topThreeS[2] = good2;
00511                         }
00512                     }
00513 //                      }
00514                 }
00515             }
00516             //    }
00517         }
00518     }
00519 
00520     T = topThree[0];
00521 
00522     if(dominantBinsMine.points.size() < 2 || dominantBinsTarget.points.size() < 2)
00523     {
00524         T.setIdentity();
00525     }
00526 
00527 }
00528 
00529 template <typename PointT>
00530 void NDTHistogram<PointT>::closedFormSolution(pcl::PointCloud<PointT> &src, pcl::PointCloud<PointT> &tgt,
00531         Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &T)
00532 {
00533 
00534     T.setIdentity ();
00535     Eigen::Matrix3d H; // = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
00536     Eigen::MatrixXd P1, P2;         //temporary points for sets 1 and 2
00537     unsigned int itr=0;             //loop counters
00538     size_t size = src.points.size();
00539 
00540     // Assemble the correlation matrix H = source * target'
00541     P1 = Eigen::MatrixXd(size,3);
00542     P2 = Eigen::MatrixXd(size,3);
00543     //compute values needed for the N matrix and for scale
00544     for(itr=0; itr<size; itr++)
00545     {
00546         P1(itr,0) = src.points[itr].x;
00547         P1(itr,1) = src.points[itr].y;
00548         P1(itr,2) = src.points[itr].z;
00549         P2(itr,0) = tgt.points[itr].x;
00550         P2(itr,1) = tgt.points[itr].y;
00551         P2(itr,2) = tgt.points[itr].z;
00552     }
00553     H = P1.transpose()*P2;
00554 
00555     // Compute the Singular Value Decomposition
00556     Eigen::JacobiSVD<Eigen::Matrix3d> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
00557     Eigen::Matrix3d u = svd.matrixU ();
00558     Eigen::Matrix3d v = svd.matrixV ();
00559 
00560     // Compute R = V * U'
00561     if (u.determinant () * v.determinant () < 0)
00562     {
00563         for (int x = 0; x < 3; ++x)
00564             v (x, 2) *= -1;
00565     }
00566 
00567     Eigen::Matrix3d R = v * u.transpose ();
00568     // Return the correct transformation
00569     T = R;
00570 
00571 }
00572 
00573 template <typename PointT>
00574 void NDTHistogram<PointT>::printHistogram(bool bMatlab)
00575 {
00576 
00577     if(bMatlab)
00578     {
00579         //prints in a format suitable for matlab plotting
00580         std::cout<<"L=[ ";
00581         for(unsigned int i=0; i<histogramBinsLine.size(); i++)
00582         {
00583             std::cout<<histogramBinsLine[i]<<" ";
00584         }
00585         std::cout<<"];\n";
00586         std::cout<<"F=[";
00587         for(unsigned int i=0; i<histogramBinsFlat.size(); i++)
00588         {
00589             std::cout<<histogramBinsFlat[i]<<" ";
00590         }
00591         std::cout<<"];\n";
00592         for(unsigned int q=0; q<3; q++)
00593         {
00594             std::cout<<"F"<<q<<" = [";
00595             for(unsigned int i=0; i<dist_histogramBinsFlat[q].size(); i++)
00596             {
00597                 std::cout<<dist_histogramBinsFlat[q][i]<<" ";
00598             }
00599             std::cout<<"];\n";
00600         }
00601 
00602         std::cout<<"];\nS=[";
00603         for(unsigned int i=0; i<histogramBinsSphere.size(); i++)
00604         {
00605             std::cout<<histogramBinsSphere[i]<<" ";
00606         }
00607         std::cout<<"];\n";
00608         for(unsigned int q=0; q<3; q++)
00609         {
00610             std::cout<<"S"<<q<<" = [";
00611             for(unsigned int i=0; i<dist_histogramBinsSphere[q].size(); i++)
00612             {
00613                 std::cout<<dist_histogramBinsSphere[q][i]<<" ";
00614             }
00615             std::cout<<"];\n";
00616         }
00617         /*      std::cout<<"];\nD=[";
00618                 for(unsigned int i=0; i<directions.size(); i++) {
00619                 std::cout<<directions[i].transpose()<<"; ";
00620                 }
00621          */
00622 
00623     }
00624     else
00625     {
00626         std::cout<<"L: ";
00627         for(unsigned int i=0; i<histogramBinsLine.size(); i++)
00628         {
00629             std::cout<<histogramBinsLine[i]<<" ";
00630         }
00631 
00632         std::cout<<"\nF: ";
00633         for(unsigned int i=0; i<histogramBinsFlat.size(); i++)
00634         {
00635             std::cout<<histogramBinsFlat[i]<<" ";
00636         }
00637 
00638         std::cout<<"\nS: ";
00639         for(unsigned int i=0; i<histogramBinsSphere.size(); i++)
00640         {
00641             std::cout<<histogramBinsSphere[i]<<" ";
00642         }
00643         std::cout<<"\n";
00644     }
00645 }
00646 
00647 template <typename PointT>
00648 double NDTHistogram<PointT>::getSimilarity(NDTHistogram<PointT> &other)
00649 {
00650 
00651     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T;
00652 
00653     this->bestFitToHistogram(other,T,false);
00654 
00655     return this->getSimilarity(other,T);
00656 }
00657 
00658 template <typename PointT>
00659 double NDTHistogram<PointT>::getSimilarity(NDTHistogram<PointT> &other,
00660         Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &T)
00661 {
00662 
00663     double score[3];
00664 
00665     double N_THIS[3], N_OTHER[3];
00666     for(unsigned int r = 0; r<3; r++)
00667     {
00668         N_THIS[r] = 0;
00669         N_OTHER[r] = 0;
00670         score[r] = 0;
00671         for(int i=0; i<histogramBinsFlat.size(); i++)
00672         {
00673             N_THIS[r] += dist_histogramBinsFlat[r][i];
00674             N_OTHER[r] += other.dist_histogramBinsFlat[r][i];
00675         }
00676         for(int i=0; i<histogramBinsSphere.size(); i++)
00677         {
00678             N_THIS[r] += dist_histogramBinsSphere[r][i] ;
00679             N_OTHER[r] += other.dist_histogramBinsSphere[r][i] ;
00680         }
00681         N_THIS[r] += dist_histogramBinsLine[r][0];
00682         N_OTHER[r]+= other.dist_histogramBinsLine[r][0];
00683         N_THIS[r] = N_THIS[r]==0 ? INT_MAX : N_THIS[r];
00684         N_OTHER[r] = N_OTHER[r]==0 ? INT_MAX : N_OTHER[r];
00685     }
00686 
00687     for(int q = 0; q<averageDirections.size(); q++)
00688     {
00689 
00690         Eigen::Vector3d tr = T*averageDirections[q];
00691         if( this->histogramBinsFlat[q] == 0)
00692         {
00693             tr = directions[q]; //fall back to bin direction
00694         }
00695 
00696         //find B = the bin in which tr falls
00697         tr.normalize();
00698         double mindist = INT_MAX;
00699         int idmin = -1;
00700         for(unsigned int i=0; i<directions.size(); i++)
00701         {
00702             double dist = (directions[i]-tr).norm();
00703             if(mindist > dist)
00704             {
00705                 mindist = dist;
00706                 idmin = i;
00707             }
00708         }
00709         //std::cout<<idmin<<std::endl;
00710         if(!(idmin >=0 && idmin < histogramBinsFlat.size()))
00711         {
00712             continue;
00713         }
00714 
00715         for(unsigned int r = 0; r<3; r++)
00716         {
00717             score[r] += pow((double)this->dist_histogramBinsFlat[r][q]/N_THIS[r] - (double)other.dist_histogramBinsFlat[r][idmin]/N_OTHER[r],2);
00718         }
00719 
00720     }
00721     for(unsigned int r = 0; r<3; r++)
00722     {
00723         for(int i=0; i<histogramBinsSphere.size(); i++)
00724         {
00725             score[r] += pow( (double)this->dist_histogramBinsSphere[r][i]/N_THIS[r] - (double)other.dist_histogramBinsSphere[r][i]/N_OTHER[r] ,2);
00726         }
00727 
00728         score[r] += pow( (double)this->dist_histogramBinsLine[r][0]/N_THIS[r] - (double)other.dist_histogramBinsLine[r][0]/N_OTHER[r] ,2);
00729         double maxN, minN;
00730         maxN = (N_THIS[r] > N_OTHER[r]) ? N_THIS[r] : N_OTHER[r];
00731         minN = (N_THIS[r] < N_OTHER[r]) ? N_THIS[r] : N_OTHER[r];
00732         minN = (minN < 1) ? 1 : minN;
00733 
00734         score[r] = maxN*sqrt(score[r])/minN;
00735     }
00736 
00737 
00738     return score[0]+score[1];
00739 }
00740 
00741 }


ndt_map
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Jan 6 2014 11:31:57