batchTestRangerAccuracy.cc
Go to the documentation of this file.
00001 //general
00002 #include <PointCloudUtils.hh>
00003 #include <fstream>
00004 
00005 //ndt
00006 #include <NDTMap.hh>
00007 #include <OctTree.hh>
00008 #include <NDTMatcherF2F.hh>
00009 
00010 using namespace std;
00011 
00012 pcl::PointCloud<pcl::PointXYZ> generateNegatives(pcl::PointCloud<pcl::PointXYZ> &cloud)
00013 {
00014 
00015     pcl::PointCloud<pcl::PointXYZ> negCloud;
00016     double min_offset = 0.1;
00017     double max_offset = 2;
00018     pcl::PointXYZ origin, neg;
00019 
00020     origin.x = origin.y = origin.z = 0;
00021     for(int i =0; i<cloud.points.size(); i++)
00022     {
00023         double rand_offset = min_offset + (max_offset-min_offset)*
00024                              (double)rand()/(double)RAND_MAX;
00025 
00026 
00027         neg.x = (cloud.points[i].x-origin.x);
00028         neg.y = (cloud.points[i].y-origin.y);
00029         neg.z = (cloud.points[i].z-origin.z);
00030         double len = sqrt(neg.x*neg.x + neg.y*neg.y + neg.z*neg.z);
00031         double factor = (len - rand_offset) / len;
00032         factor = (factor < 0) ? 0 : factor;
00033         neg.x = factor*(cloud.points[i].x-origin.x) + origin.x;
00034         neg.y = factor*(cloud.points[i].y-origin.y) + origin.y;
00035         neg.z = factor*(cloud.points[i].z-origin.z) + origin.z;
00036         negCloud.points.push_back(neg);
00037     }
00038     return negCloud;
00039 }
00040 
00041 int main(int argc, char **argv)
00042 {
00043 
00044 
00045     char filename[200];
00046     FILE *fout;
00047     //read in config file
00048     //read in config file name
00049     if(argc != 3)
00050     {
00051         cout<<"usage: ./representationTester logDirectory N_CLOUDS\n";
00052         return -1;
00053     }
00054 
00055     bool doMatch = true;
00056     bool doGroundTruth = false;
00057 
00058     pcl::PointCloud<pcl::PointXYZ> cloudLaser, negLaser;
00059     pcl::PointCloud<pcl::PointXYZ> cloudKinect, negKinect;
00060     pcl::PointCloud<pcl::PointXYZ> cloudSR, negSR;
00061     pcl::PointCloud<pcl::PointXYZ> cloudFotonic, negFotonic;
00062     pcl::PointCloud<pcl::PointXYZ> gt, negGt;
00063 
00064     string dirName = argv[1];
00065     string outputName;
00066 
00067     int N_CLOUDS = atoi(argv[2]);
00068     double LTHRESH = 0.0005;
00069 
00070     int tpKinect[N_CLOUDS], fpKinect[N_CLOUDS], tnKinect[N_CLOUDS], fnKinect[N_CLOUDS];
00071     int tpSR[N_CLOUDS], fpSR[N_CLOUDS], tnSR[N_CLOUDS], fnSR[N_CLOUDS];
00072     int tpFotonic[N_CLOUDS], fpFotonic[N_CLOUDS], tnFotonic[N_CLOUDS], fnFotonic[N_CLOUDS];
00073     int tpLaser[N_CLOUDS], fpLaser[N_CLOUDS], tnLaser[N_CLOUDS], fnLaser[N_CLOUDS];
00074 
00075     outputName = dirName+"results.m";
00076     ofstream foutResults((outputName).c_str());
00077 
00078     lslgeneric::OctTree::BIG_CELL_SIZE = 6.4;
00079     lslgeneric::OctTree::SMALL_CELL_SIZE = 0.4;
00080     lslgeneric::OctTree prototype;
00081     int tp=0,fp=0,tn=0,fn=0;
00082 
00083     for(int i=0; i<N_CLOUDS; i++)
00084     {
00085 
00086         char cloudname[200];
00087         snprintf(cloudname,199,"%s/laser/pc%04d.wrl",dirName.c_str(),i);
00088         cloudLaser = lslgeneric::readVRML(cloudname);
00089 
00090         snprintf(cloudname,199,"%s/kinect/pc%04d.wrl",dirName.c_str(),i);
00091         cloudKinect = lslgeneric::readVRML(cloudname);
00092 
00093         snprintf(cloudname,199,"%s/sr/pc%04d.wrl",dirName.c_str(),i);
00094         cloudSR = lslgeneric::readVRML(cloudname);
00095 
00096         snprintf(cloudname,199,"%s/fotonic/pc%04d.wrl",dirName.c_str(),i);
00097         cloudFotonic = lslgeneric::readVRML(cloudname);
00098 
00099 
00100         lslgeneric::NDTMap ndt(&prototype);
00101 
00102         if(doGroundTruth)
00103         {
00104             gt = cloudLaser;
00105             snprintf(cloudname,199,"%s/gt.wrl",dirName.c_str());
00106             cloudLaser= lslgeneric::readVRML(cloudname);
00107             ndt.loadPointCloud(cloudLaser);
00108         }
00109 
00110         if(doMatch)
00111         {
00112             Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T;
00113             //lslgeneric::NDTMatcherF2F matcher;
00114             double __res[] = {0.2, 0.4, 1, 2};
00115             std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
00116             lslgeneric::NDTMatcherF2F matcher(false, false, false, resolutions);
00117 
00118             pcl::PointCloud<pcl::PointXYZ> tmp;
00119             if(!doGroundTruth)
00120             {
00121                 for(int q=0; q<cloudLaser.points.size(); q++)
00122                 {
00123                     pcl::PointXYZ pt = cloudLaser.points[q];
00124                     double angle_x = atan2(pt.x,pt.y);
00125                     double angle_y = atan2(pt.x,pt.z);
00126                     double angle_z = atan2(pt.y,pt.z);
00127                     //cout<<angle_x<<" "<<angle_y<<" "<<angle_z<<endl;
00128                     double dist = sqrt(pt.x*pt.x + pt.y*pt.y + pt.z*pt.z);
00129                     if(angle_y > -0.33 && dist<3.5) tmp.points.push_back(pt);
00130                 }
00131                 cloudLaser = tmp;
00132                 T = Eigen::Translation<double,3>(-0.15,0.17,0.44)*
00133                     Eigen::AngleAxis<double>(0.06,Eigen::Vector3d::UnitX()) *
00134                     Eigen::AngleAxis<double>(0.1,Eigen::Vector3d::UnitY()) *
00135                     Eigen::AngleAxis<double>(-0.15,Eigen::Vector3d::UnitZ()) ;
00136                 cout<<"Laser: "<<T.translation().transpose()<<" "<<T.rotation().eulerAngles(0,1,2).transpose()<<endl;
00137                 lslgeneric::transformPointCloudInPlace(T,cloudLaser);
00138                 ndt.loadPointCloud(cloudLaser);
00139             }
00140 
00141 
00142             tmp.points.clear();
00143             for(int q=0; q<cloudKinect.points.size(); q++)
00144             {
00145                 pcl::PointXYZ pt = cloudKinect.points[q];
00146                 double angle_x = atan2(pt.x,pt.y);
00147                 double angle_y = atan2(pt.x,pt.z);
00148                 double angle_z = atan2(pt.y,pt.z);
00149                 double dist = sqrt(pt.x*pt.x + pt.y*pt.y + pt.z*pt.z);
00150                 //cout<<angle_x<<" "<<angle_y<<" "<<angle_z<<endl;
00151                 if(angle_y > -0.33 && angle_y < 0.33 && angle_z > -0.25 && dist<3.5) tmp.points.push_back(pt);
00152             }
00153             cloudKinect = tmp;
00154 
00155             T = Eigen::Translation<double,3>(-0.121,0.2278,0.514)*
00156                 Eigen::AngleAxis<double>(0.066,Eigen::Vector3d::UnitX()) *
00157                 Eigen::AngleAxis<double>(0.1098,Eigen::Vector3d::UnitY()) *
00158                 Eigen::AngleAxis<double>(-0.0707,Eigen::Vector3d::UnitZ()) ;
00159             /*T = Eigen::Translation<double,3>(-0.07,0.058,0.44)*
00160             Eigen::AngleAxis<double>(0.07,Eigen::Vector3d::UnitX()) *
00161             Eigen::AngleAxis<double>(0.09,Eigen::Vector3d::UnitY()) *
00162             Eigen::AngleAxis<double>(-0.08,Eigen::Vector3d::UnitZ()) ;*/
00163             lslgeneric::transformPointCloudInPlace(T,cloudKinect);
00164             matcher.match(cloudLaser, cloudKinect, T);
00165             cout<<"Kinect: "<<T.translation().transpose()<<" "<<T.rotation().eulerAngles(0,1,2).transpose()<<endl;
00166             lslgeneric::transformPointCloudInPlace(T,cloudKinect);
00167             snprintf(cloudname,199,"%s/ndt/rk%04d.wrl",dirName.c_str(),i);
00168             fout = fopen(cloudname,"w");
00169             fprintf(fout,"#VRML V2.0 utf8\n");
00170             lslgeneric::writeToVRML(fout,cloudLaser,Eigen::Vector3d(1,1,1));
00171             lslgeneric::writeToVRML(fout,cloudKinect,Eigen::Vector3d(1,0,0));
00172             fclose(fout);
00173 
00174             tmp.points.clear();
00175             for(int q=0; q<cloudSR.points.size(); q++)
00176             {
00177                 pcl::PointXYZ pt = cloudSR.points[q];
00178                 double angle_x = atan2(pt.x,pt.y);
00179                 double angle_y = atan2(pt.x,pt.z);
00180                 double angle_z = atan2(pt.y,pt.z);
00181                 double dist = sqrt(pt.x*pt.x + pt.y*pt.y + pt.z*pt.z);
00182                 //cout<<angle_x<<" "<<angle_y<<" "<<angle_z<<endl;
00183                 if(angle_y > -0.33 && angle_y < 0.33 && angle_z > -0.25 && dist<3.5) tmp.points.push_back(pt);
00184             }
00185             cloudSR = tmp;
00186             T = Eigen::Translation<double,3>(0.052,0.036,0.42)*
00187                 Eigen::AngleAxis<double>(0.067,Eigen::Vector3d::UnitX()) *
00188                 Eigen::AngleAxis<double>(0.061,Eigen::Vector3d::UnitY()) *
00189                 Eigen::AngleAxis<double>(-0.0859,Eigen::Vector3d::UnitZ()) ;
00190             lslgeneric::transformPointCloudInPlace(T,cloudSR);
00191             matcher.match(cloudLaser, cloudSR, T);
00192             cout<<"SR: "<<T.translation().transpose()<<" "<<T.rotation().eulerAngles(0,1,2).transpose()<<endl;
00193             lslgeneric::transformPointCloudInPlace(T,cloudSR);
00194             snprintf(cloudname,199,"%s/ndt/rs%04d.wrl",dirName.c_str(),i);
00195             fout = fopen(cloudname,"w");
00196             fprintf(fout,"#VRML V2.0 utf8\n");
00197             lslgeneric::writeToVRML(fout,cloudLaser,Eigen::Vector3d(1,1,1));
00198             lslgeneric::writeToVRML(fout,cloudSR,Eigen::Vector3d(1,0,0));
00199             fclose(fout);
00200 
00201             tmp.points.clear();
00202             for(int q=0; q<cloudFotonic.points.size(); q++)
00203             {
00204                 pcl::PointXYZ pt = cloudFotonic.points[q];
00205                 double angle_x = atan2(pt.x,pt.y);
00206                 double angle_y = atan2(pt.x,pt.z);
00207                 double angle_z = atan2(pt.y,pt.z);
00208                 //cout<<angle_x<<" "<<angle_y<<" "<<angle_z<<endl;
00209                 double dist = sqrt(pt.x*pt.x + pt.y*pt.y + pt.z*pt.z);
00210                 if(angle_y > -0.33 && angle_y < 0.33 && angle_z > -0.25 && dist<3.5) tmp.points.push_back(pt);
00211             }
00212             cloudFotonic = tmp;
00213             T = Eigen::Translation<double,3>(-0.0988,0.045,0.452)*
00214                 Eigen::AngleAxis<double>(0.0952,Eigen::Vector3d::UnitX()) *
00215                 Eigen::AngleAxis<double>(0.0718,Eigen::Vector3d::UnitY()) *
00216                 Eigen::AngleAxis<double>(-0.0758,Eigen::Vector3d::UnitZ()) ;
00217             lslgeneric::transformPointCloudInPlace(T,cloudFotonic);
00218             matcher.match(cloudLaser, cloudFotonic, T);
00219             cout<<"Fotonic: "<<T.translation().transpose()<<" "<<T.rotation().eulerAngles(0,1,2).transpose()<<endl;
00220             lslgeneric::transformPointCloudInPlace(T,cloudFotonic);
00221             snprintf(cloudname,199,"%s/ndt/rf%04d.wrl",dirName.c_str(),i);
00222             fout = fopen(cloudname,"w");
00223             fprintf(fout,"#VRML V2.0 utf8\n");
00224             lslgeneric::writeToVRML(fout,cloudLaser,Eigen::Vector3d(1,1,1));
00225             lslgeneric::writeToVRML(fout,cloudFotonic,Eigen::Vector3d(1,0,0));
00226             fclose(fout);
00227 
00228 
00229             if(doGroundTruth)
00230             {
00231                 pcl::PointCloud<pcl::PointXYZ> gt2;
00232                 for(int q=0; q<gt.points.size(); q++)
00233                 {
00234                     pcl::PointXYZ pt = gt.points[q];
00235                     double angle_x = atan2(pt.x,pt.y);
00236                     double angle_y = atan2(pt.x,pt.z);
00237                     double angle_z = atan2(pt.y,pt.z);
00238                     //cout<<angle_x<<" "<<angle_y<<" "<<angle_z<<endl;
00239                     double dist = sqrt(pt.x*pt.x + pt.y*pt.y + pt.z*pt.z);
00240                     if(angle_y > -0.33 && dist <3.5) gt2.points.push_back(pt);
00241                 }
00242                 gt = gt2;
00243                 T = Eigen::Translation<double,3>(-0.15,0.17,0.44)*
00244                     Eigen::AngleAxis<double>(0.06,Eigen::Vector3d::UnitX()) *
00245                     Eigen::AngleAxis<double>(0.1,Eigen::Vector3d::UnitY()) *
00246                     Eigen::AngleAxis<double>(-0.15,Eigen::Vector3d::UnitZ()) ;
00247                 //lslgeneric::transformPointCloudInPlace(T,gt);
00248                 //matcher.match(cloudLaser, gt, T);
00249                 cout<<"Laser: "<<T.translation().transpose()<<" "<<T.rotation().eulerAngles(0,1,2).transpose()<<endl;
00250                 lslgeneric::transformPointCloudInPlace(T,gt);
00251                 snprintf(cloudname,199,"%s/ndt/rl%04d.wrl",dirName.c_str(),i);
00252                 fout = fopen(cloudname,"w");
00253                 fprintf(fout,"#VRML V2.0 utf8\n");
00254                 lslgeneric::writeToVRML(fout,cloudLaser,Eigen::Vector3d(1,1,1));
00255                 lslgeneric::writeToVRML(fout,gt,Eigen::Vector3d(1,0,0));
00256                 fclose(fout);
00257                 cloudLaser = gt;
00258             }
00259         }
00260 
00261         ndt.computeNDTCells();
00262         snprintf(filename,199,"%s/ndt/ndtTree%d.wrl",dirName.c_str(),i);
00263         ndt.writeToVRML(filename);
00264 
00265         negLaser = generateNegatives(cloudLaser);
00266         snprintf(cloudname,199,"%s/laser/negpc%04d.wrl",dirName.c_str(),i);
00267         lslgeneric::writeToVRML(cloudname,negLaser);
00268         negKinect = generateNegatives(cloudKinect);
00269         snprintf(cloudname,199,"%s/kinect/negpc%04d.wrl",dirName.c_str(),i);
00270         lslgeneric::writeToVRML(cloudname,negKinect);
00271         negSR = generateNegatives(cloudSR);
00272         snprintf(cloudname,199,"%s/sr/negpc%04d.wrl",dirName.c_str(),i);
00273         lslgeneric::writeToVRML(cloudname,negSR);
00274         negFotonic = generateNegatives(cloudFotonic);
00275         snprintf(cloudname,199,"%s/fotonic/negpc%04d.wrl",dirName.c_str(),i);
00276         lslgeneric::writeToVRML(cloudname,negFotonic);
00277 
00278         /*      tp=0;fp=0;tn=0;fn=0;
00279                 lslgeneric::NDTMap ndtLaser(&prototype);
00280                 if(doGroundTruth) {
00281                     ndtLaser.loadPointCloud(gt);
00282                 } else {
00283                     ndtLaser.loadPointCloud(cloudLaser);
00284                 }
00285                 ndtLaser.computeNDTCells();
00286                 for(int j=0; j<cloudLaser.points.size(); j++) {
00287                     double prob = ndtLaser.getLikelihoodForPoint(cloudLaser.points[j]);
00288                     double nprob = ndtLaser.getLikelihoodForPoint(negLaser.points[j]);
00289                     (prob > LTHRESH) ? tp++ : fp++;
00290                     (nprob > LTHRESH) ? fn++ : tn++;
00291                 }
00292                 cout<<"Laser at "<<i<<" : "<<tp<<" "<<fp<<" "<<tn<<" "<<fn<<endl;
00293                 tpLaser[i] = tp; fpLaser[i] = fp; tnLaser[i] = tn; fnLaser[i] = fn;
00294 
00295                 tp=0;fp=0;tn=0;fn=0;
00296                 lslgeneric::NDTMap ndtKinect(&prototype);
00297                 ndtKinect.loadPointCloud(cloudKinect);
00298                 ndtKinect.computeNDTCells();
00299                 for(int j=0; j<cloudLaser.points.size(); j++) {
00300                     double prob = ndtKinect.getLikelihoodForPoint(cloudLaser.points[j]);
00301                     double nprob = ndtKinect.getLikelihoodForPoint(negLaser.points[j]);
00302                     (prob > LTHRESH) ? tp++ : fp++;
00303                     (nprob > LTHRESH) ? fn++ : tn++;
00304                 }
00305                 cout<<"Kinect at "<<i<<" : "<<tp<<" "<<fp<<" "<<tn<<" "<<fn<<endl;
00306                 tpKinect[i] = tp; fpKinect[i] = fp; tnKinect[i] = tn; fnKinect[i] = fn;
00307 
00308                 tp=0;fp=0;tn=0;fn=0;
00309                 lslgeneric::NDTMap ndtSR(&prototype);
00310                 ndtSR.loadPointCloud(cloudSR);
00311                 ndtSR.computeNDTCells();
00312                 for(int j=0; j<cloudLaser.points.size(); j++) {
00313                     double prob = ndtSR.getLikelihoodForPoint(cloudLaser.points[j]);
00314                     double nprob = ndtSR.getLikelihoodForPoint(negLaser.points[j]);
00315                     (prob > LTHRESH) ? tp++ : fp++;
00316                     (nprob > LTHRESH) ? fn++ : tn++;
00317                 }
00318                 cout<<"SR at "<<i<<" : "<<tp<<" "<<fp<<" "<<tn<<" "<<fn<<endl;
00319                 tpSR[i] = tp; fpSR[i] = fp; tnSR[i] = tn; fnSR[i] = fn;
00320                 tp=0;fp=0;tn=0;fn=0;
00321 
00322                 lslgeneric::NDTMap ndtFotonic(&prototype);
00323                 ndtFotonic.loadPointCloud(cloudFotonic);
00324                 ndtFotonic.computeNDTCells();
00325                 for(int j=0; j<cloudLaser.points.size(); j++) {
00326                     double prob = ndtFotonic.getLikelihoodForPoint(cloudLaser.points[j]);
00327                     double nprob = ndtFotonic.getLikelihoodForPoint(negLaser.points[j]);
00328                     (prob > LTHRESH) ? tp++ : fp++;
00329                     (nprob > LTHRESH) ? fn++ : tn++;
00330                 }
00331                 cout<<"Fotonic at "<<i<<" : "<<tp<<" "<<fp<<" "<<tn<<" "<<fn<<endl;
00332                 tpFotonic[i] = tp; fpFotonic[i] = fp; tnFotonic[i] = tn; fnFotonic[i] = fn;
00333         */
00334         tp=0;
00335         fp=0;
00336         tn=0;
00337         fn=0;
00338         for(int j=0; j<cloudLaser.points.size(); j++)
00339         {
00340             double prob = ndt.getLikelihoodForPoint(cloudLaser.points[j]);
00341             double nprob = ndt.getLikelihoodForPoint(negLaser.points[j]);
00342             (prob > LTHRESH) ? tp++ : fp++;
00343             (nprob > LTHRESH) ? fn++ : tn++;
00344         }
00345         cout<<"Laser at "<<i<<" : "<<tp<<" "<<fp<<" "<<tn<<" "<<fn<<endl;
00346         tpLaser[i] = tp;
00347         fpLaser[i] = fp;
00348         tnLaser[i] = tn;
00349         fnLaser[i] = fn;
00350 
00351 
00352         tp=0;
00353         fp=0;
00354         tn=0;
00355         fn=0;
00356         for(int j=0; j<cloudKinect.points.size(); j++)
00357         {
00358             double prob = ndt.getLikelihoodForPoint(cloudKinect.points[j]);
00359             double nprob = ndt.getLikelihoodForPoint(negKinect.points[j]);
00360             (prob > LTHRESH) ? tp++ : fp++;
00361             (nprob > LTHRESH) ? fn++ : tn++;
00362         }
00363         cout<<"Kinect at "<<i<<" : "<<tp<<" "<<fp<<" "<<tn<<" "<<fn<<endl;
00364         tpKinect[i] = tp;
00365         fpKinect[i] = fp;
00366         tnKinect[i] = tn;
00367         fnKinect[i] = fn;
00368 
00369 
00370         tp=0;
00371         fp=0;
00372         tn=0;
00373         fn=0;
00374         for(int j=0; j<cloudSR.points.size(); j++)
00375         {
00376             double prob = ndt.getLikelihoodForPoint(cloudSR.points[j]);
00377             double nprob = ndt.getLikelihoodForPoint(negSR.points[j]);
00378             (prob > LTHRESH) ? tp++ : fp++;
00379             (nprob > LTHRESH) ? fn++ : tn++;
00380         }
00381         cout<<"SR at "<<i<<" : "<<tp<<" "<<fp<<" "<<tn<<" "<<fn<<endl;
00382         tpSR[i] = tp;
00383         fpSR[i] = fp;
00384         tnSR[i] = tn;
00385         fnSR[i] = fn;
00386 
00387         tp=0;
00388         fp=0;
00389         tn=0;
00390         fn=0;
00391         for(int j=0; j<cloudFotonic.points.size(); j++)
00392         {
00393             double prob = ndt.getLikelihoodForPoint(cloudFotonic.points[j]);
00394             double nprob = ndt.getLikelihoodForPoint(negFotonic.points[j]);
00395             (prob > LTHRESH) ? tp++ : fp++;
00396             (nprob > LTHRESH) ? fn++ : tn++;
00397         }
00398         cout<<"Fotonic at "<<i<<" : "<<tp<<" "<<fp<<" "<<tn<<" "<<fn<<endl;
00399         tpFotonic[i] = tp;
00400         fpFotonic[i] = fp;
00401         tnFotonic[i] = tn;
00402         fnFotonic[i] = fn;
00403 
00404     }
00405 
00406     cout<<"Done, now writing results for "<<N_CLOUDS<<" clouds at "<<outputName<<"...\n";
00407     //laser
00408     foutResults<<"tpLaser = [";
00409     for(int i=0; i<N_CLOUDS; i++)
00410     {
00411         foutResults<<(double)tpLaser[i]/(double)(tpLaser[i]+fnLaser[i])<<" ";
00412     }
00413     foutResults<<"];\nfpLaser = [";
00414     for(int i=0; i<N_CLOUDS; i++)
00415     {
00416         foutResults<<(double)fpLaser[i]/(double)(tnLaser[i]+fpLaser[i])<<" ";
00417     }
00418     foutResults<<"];\ntnLaser = [";
00419     for(int i=0; i<N_CLOUDS; i++)
00420     {
00421         foutResults<<(double)tnLaser[i]/(double)(tpLaser[i]+fnLaser[i])<<" ";
00422     }
00423     foutResults<<"];\nfnLaser = [";
00424     for(int i=0; i<N_CLOUDS; i++)
00425     {
00426         foutResults<<(double)fnLaser[i]/(double)(tnLaser[i]+fpLaser[i])<<" ";
00427     }
00428     foutResults<<"];\n";
00429 
00430     //kinect
00431     foutResults<<"tpKinect = [";
00432     for(int i=0; i<N_CLOUDS; i++)
00433     {
00434         foutResults<<(double)tpKinect[i]/(double)(tpKinect[i]+fnKinect[i])<<" ";
00435     }
00436     foutResults<<"];\nfpKinect = [";
00437     for(int i=0; i<N_CLOUDS; i++)
00438     {
00439         foutResults<<(double)fpKinect[i]/(double)(tnKinect[i]+fpKinect[i])<<" ";
00440     }
00441     foutResults<<"];\ntnKinect = [";
00442     for(int i=0; i<N_CLOUDS; i++)
00443     {
00444         foutResults<<(double)tnKinect[i]/(double)(tpKinect[i]+fnKinect[i])<<" ";
00445     }
00446     foutResults<<"];\nfnKinect = [";
00447     for(int i=0; i<N_CLOUDS; i++)
00448     {
00449         foutResults<<(double)fnKinect[i]/(double)(tnKinect[i]+fpKinect[i])<<" ";
00450     }
00451     foutResults<<"];\n";
00452 
00453     //sr
00454     foutResults<<"tpSR = [";
00455     for(int i=0; i<N_CLOUDS; i++)
00456     {
00457         foutResults<<(double)tpSR[i]/(double)(tpSR[i]+fnSR[i])<<" ";
00458     }
00459     foutResults<<"];\nfpSR = [";
00460     for(int i=0; i<N_CLOUDS; i++)
00461     {
00462         foutResults<<(double)fpSR[i]/(double)(tnSR[i]+fpSR[i])<<" ";
00463     }
00464     foutResults<<"];\ntnSR = [";
00465     for(int i=0; i<N_CLOUDS; i++)
00466     {
00467         foutResults<<(double)tnSR[i]/(double)(tnSR[i]+fpSR[i])<<" ";
00468     }
00469     foutResults<<"];\nfnSR = [";
00470     for(int i=0; i<N_CLOUDS; i++)
00471     {
00472         foutResults<<(double)fnSR[i]/(double)(tpSR[i]+fnSR[i])<<" ";
00473     }
00474     foutResults<<"];\n";
00475 
00476     //fotonic
00477     foutResults<<"tpFotonic = [";
00478     for(int i=0; i<N_CLOUDS; i++)
00479     {
00480         foutResults<<(double)tpFotonic[i]/(double)(tpFotonic[i]+fnFotonic[i])<<" ";
00481     }
00482     foutResults<<"];\nfpFotonic = [";
00483     for(int i=0; i<N_CLOUDS; i++)
00484     {
00485         foutResults<<(double)fpFotonic[i]/(double)(tnFotonic[i]+fpFotonic[i])<<" ";
00486     }
00487     foutResults<<"];\ntnFotonic = [";
00488     for(int i=0; i<N_CLOUDS; i++)
00489     {
00490         foutResults<<(double)tnFotonic[i]/(double)(tnFotonic[i]+fpFotonic[i])<<" ";
00491     }
00492     foutResults<<"];\nfnFotonic = [";
00493     for(int i=0; i<N_CLOUDS; i++)
00494     {
00495         foutResults<<(double)fnFotonic[i]/(double)(tpFotonic[i]+fnFotonic[i])<<" ";
00496     }
00497     foutResults<<"];\n";
00498 
00499     return 0;
00500 
00501 }
00502 
00503 


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