00001
00002 #include <PointCloudUtils.hh>
00003 #include <fstream>
00004
00005
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
00048
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
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
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
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
00160
00161
00162
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
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
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
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
00248
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
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
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
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
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
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
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