00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include "pointmatcher/PointMatcher.h"
00037 #include "pointmatcher/IO.h"
00038 #include "pointmatcher/Timer.h"
00039 #include <cassert>
00040 #include <iostream>
00041 #include <fstream>
00042 #include <map>
00043 #include <time.h>
00044
00045 #include <ncurses.h>
00046
00047 #include <boost/program_options.hpp>
00048 #include <boost/filesystem.hpp>
00049 #include <boost/date_time/posix_time/posix_time.hpp>
00050 #include <boost/date_time/posix_time/posix_time_io.hpp>
00051 #include <boost/thread/thread.hpp>
00052 #include <boost/thread/mutex.hpp>
00053
00054
00055 using namespace std;
00056 using namespace PointMatcherSupport;
00057 namespace po = boost::program_options;
00058 namespace fs = boost::filesystem;
00059 namespace pt = boost::posix_time;
00060
00061 typedef PointMatcher<float> PM;
00062 typedef PointMatcherIO<float> PMIO;
00063 typedef PM::TransformationParameters TP;
00064
00065 struct DataSetInfo
00066 {
00067 string name;
00068 bool downloaded;
00069 string path;
00070 DataSetInfo(){};
00071 DataSetInfo(string name, bool downloaded):
00072 name(name),
00073 downloaded(downloaded)
00074 {}
00075 };
00076
00077 struct Config
00078 {
00079
00080 string path_config;
00081 string path_download;
00082 string path_result;
00083 string path_server_validation;
00084 string path_server_protocols;
00085 map<string, DataSetInfo> dataSetStatus;
00086
00087 Config()
00088 {
00089 path_config = string(getenv("HOME")) + "/.lpm/eval_solution.conf";
00090 path_download = string(getenv("HOME")) + "/.lpm/download/";
00091 path_result = "./";
00092 path_server_validation = "robotics.ethz.ch/~asl-datasets/evaluations/validation";
00093 path_server_protocols = "robotics.ethz.ch/~asl-datasets/evaluations/protocols";
00094 DataSetInfo info;
00095 info = DataSetInfo("apartment", false);
00096 info.path = "robotics.ethz.ch/~asl-datasets/apartment_03-Dec-2011-18_13_33/csv_local/local_frame.zip";
00097 dataSetStatus[info.name] = info;
00098 info = DataSetInfo("eth", false);
00099 info.path = "http://robotics.ethz.ch/~asl-datasets/ETH_hauptgebaude_23-Aug-2011-18_43_49/csv_local/local_frame.zip";
00100 dataSetStatus[info.name] = info;
00101 info = DataSetInfo("plain", false);
00102 info.path = "http://robotics.ethz.ch/~asl-datasets/plain_01-Sep-2011-16_39_18/csv_local/local_frame.zip";
00103 dataSetStatus[info.name] = info;
00104 info = DataSetInfo("stairs", false);
00105 info.path = "http://robotics.ethz.ch/~asl-datasets/stairs_26-Aug-2011-14_26_14/csv_local/local_frame.zip";
00106 dataSetStatus[info.name] = info;
00107 info = DataSetInfo("gazebo", false);
00108 info.path = "http://robotics.ethz.ch/~asl-datasets/gazebo_winter_18-Jan-2012-16_10_04/csv_local/local_frame.zip";
00109 dataSetStatus[info.name] = info;
00110 info = DataSetInfo("wood", false);
00111 info.path = "http://robotics.ethz.ch/~asl-datasets/wood_summer_25-Aug-2011-13_00_30/csv_local/local_frame.zip";
00112 dataSetStatus[info.name] = info;
00113 }
00114
00115 };
00116
00117 class EvaluationModule
00118 {
00119 public:
00120 EvaluationModule();
00121 int coreId;
00122 string tmp_file_name;
00123 double result_time;
00124 void evaluateSolution(const string &tmp_file_name, const string &yaml_config, const int &coreId, PMIO::FileInfoVector::const_iterator it_eval, PMIO::FileInfoVector::const_iterator it_end);
00125
00126 };
00127
00128 po::options_description setupOptions(const string & name);
00129 string outputStatus(map<string, DataSetInfo> dataSetStatus);
00130 string enterValidPath(string message);
00131 void setConfig(Config& config);
00132 void saveConfig(Config& config);
00133 void loadConfig(Config& config);
00134 void downloadDataSets(Config& config, po::variables_map &vm);
00135 void validateFileInfo(const PMIO::FileInfo &fileInfo);
00136 void displayLoadingBar(const int &coreId, const int &i, const int &total, const int &nbFailures, const double sec, const double total_time);
00137
00138 int main(int argc, char *argv[])
00139 {
00140 srand ( time(NULL) );
00141
00142
00143 po::options_description desc = setupOptions(argv[0]);
00144 po::positional_options_description p;
00145 p.add("icp-config", -1);
00146
00147 po::variables_map vm;
00148 po::store(po::command_line_parser(argc, argv).
00149 options(desc).positional(p).run(), vm);
00150 po::notify(vm);
00151
00152
00153 Config config;
00154
00155 if (vm.count("help")) {
00156 cout << desc << endl;
00157 return 1;
00158 }
00159
00160
00161 fs::path c_path(config.path_config);
00162 if(fs::exists(c_path) == false)
00163 {
00164 fs::create_directories(c_path.parent_path());
00165 cout << ">> no configuration found. Using default values." << endl;
00166
00167 }
00168 else
00169 {
00170 loadConfig(config);
00171 }
00172
00173 if (vm.count("config"))
00174 {
00175 setConfig(config);
00176 saveConfig(config);
00177 return 0;
00178 }
00179
00180 if (vm.count("download"))
00181 {
00182 downloadDataSets(config, vm);
00183 saveConfig(config);
00184 return 0;
00185 }
00186
00187 if (vm.count("icp-config") == false)
00188 {
00189 cerr << "You must provide a YAMl file to evaluate it." << endl;
00190 return 1;
00191 }
00192
00193 const string yaml_config = vm["icp-config"].as<string>();
00194 {
00195 ifstream cfgIfs(yaml_config.c_str());
00196 if (!cfgIfs.good())
00197 {
00198 cerr << "Cannot open YAML file name: it must exist and be readable." << endl;
00199 return 2;
00200 }
00201 }
00202
00203 initscr();
00204
00205
00206 for(BOOST_AUTO(it, config.dataSetStatus.begin()); it != config.dataSetStatus.end(); ++it)
00207 {
00208 if(vm.count("all") || vm.count(it->second.name))
00209 {
00210 if(it->second.downloaded == false)
00211 {
00212 endwin();
00213 cerr << ">> Please download data set first." << endl
00214 << ">> You can use the option --download -A to download them all." << endl;
00215 return 1;
00216 }
00217
00218 const string protocol_name = config.path_download + "protocols/" + it->second.name + "_protocol.csv";
00219 const string data_directory = config.path_download + it->second.name + "/";
00220
00221 if(!fs::exists(fs::path(protocol_name)))
00222 {
00223 endwin();
00224 cerr << ">> Missing protocol file: " << protocol_name << endl
00225 << ">> You can use the option --download to download it back." << endl;
00226 return 1;
00227 }
00228
00229 const PMIO::FileInfoVector eval_list(protocol_name, data_directory, "");
00230
00231
00232 validateFileInfo(eval_list[0]);
00233 move(0,0);
00234 clrtoeol();
00235 mvprintw(0, 0, " <<< Evaluating %s >>>", it->second.name.c_str());
00236
00237
00238
00239 const int maxNbCore = 16;
00240 int nbCore = 1;
00241 if(vm["threads"].as<int>() > 1 || vm["threads"].as<int>() < maxNbCore)
00242 {
00243 nbCore = vm["threads"].as<int>();
00244 }
00245
00246
00247 boost::thread a_threads[maxNbCore];
00248 std::vector<EvaluationModule> v_evalModules;
00249
00250 const int nbPerThread = eval_list.size()/nbCore;
00251 PMIO::FileInfoVector::const_iterator it_start = eval_list.begin();
00252 for (int j=0; j<nbCore; ++j)
00253 {
00254 v_evalModules.push_back(EvaluationModule());
00255 v_evalModules[j].coreId = j;
00256 stringstream name;
00257 name << ".tmp_core" << j << "_" << rand() << ".csv";
00258 v_evalModules[j].tmp_file_name = name.str();
00259
00260 if(j == nbCore-1)
00261 {
00262
00263 a_threads[j] = boost::thread(&EvaluationModule::evaluateSolution, &v_evalModules[j], name.str(), yaml_config, j, it_start, eval_list.end());
00264
00265 }
00266 else
00267 {
00268 a_threads[j] = boost::thread(&EvaluationModule::evaluateSolution, &v_evalModules[j], name.str(), yaml_config, j, it_start, it_start + nbPerThread);
00269
00270 it_start += nbPerThread;
00271 }
00272 cout << endl;
00273 }
00274
00275
00276 for (int k=0; k<nbCore; ++k)
00277 {
00278 if(a_threads[k].joinable())
00279 a_threads[k].join();
00280 }
00281
00282
00283 stringstream ss_path_time;
00284 pt::time_facet *facet = new pt::time_facet("%d-%b-%Y-%H_%M_%S");
00285 ss_path_time.imbue(locale(ss_path_time.getloc(), facet));
00286
00287 ss_path_time << config.path_result << it->second.name << "_";
00288 ss_path_time << pt::second_clock::local_time() << ".csv";
00289
00290 move(nbCore+3,0);
00291 clrtoeol();
00292 mvprintw(nbCore+3, 0, "Last result written to: %s", ss_path_time.str().c_str());
00293 std::ofstream fout(ss_path_time.str().c_str());
00294 if (!fout.good())
00295 {
00296 cerr << "Warning, cannot open result file " << ss_path_time << ", results were not saved" << endl;
00297 continue;
00298 }
00299
00300
00301 fout << "time";
00302 for(int r=0; r<4;++r)
00303 for(int c=0; c<4;++c)
00304 fout << ", T" << r << c;
00305
00306 fout << "\n";
00307
00308
00309
00310 for(unsigned i=0; i<v_evalModules.size(); ++i)
00311 {
00312 const string tmp_file_name = v_evalModules[i].tmp_file_name;
00313 ifstream tmp_file(tmp_file_name.c_str());
00314 if(tmp_file.is_open())
00315 {
00316 fout << tmp_file.rdbuf();
00317 tmp_file.close();
00318 fs::remove(fs::path(tmp_file_name));
00319 }
00320 else
00321 {
00322 endwin();
00323 cerr << "Cannot find tmp file named "<< tmp_file_name << endl;
00324 }
00325 }
00326
00327 fout.close();
00328 }
00329 }
00330 endwin();
00331
00332 return 0;
00333 }
00334
00335
00336 po::options_description setupOptions(const string & name)
00337 {
00338 po::options_description desc("Allowed options");
00339 desc.add_options()
00340 ("help,h", "Print this message")
00341 ("icp-config", po::value<string>(), "YAML configuration file")
00342 ("config,C", "Interactive configuration")
00343 ("download,D", "Download selected data sets from the web")
00344 ("evaluate,E", "Evaluate a solution over selected data sets")
00345 ("threads,j", po::value<int>()->default_value(1), "Number of threads to use. Max 16.")
00346 ("apartment,a", "Apply action only on the data set Apartment")
00347 ("eth,e", "Apply action only on the data set ETH Hauptgebaude")
00348 ("plain,p", "Apply action only on the data set Mountain Plain")
00349 ("stairs,s", "Apply action only on the data set Stairs")
00350 ("gazebo,g", "Apply action only on the data set Gazebo Winter")
00351 ("wood,w", "Apply action only on the data set Wood Summer")
00352 ("all,A", "Apply action for all data sets")
00353 ;
00354
00355 return desc;
00356 }
00357
00358 string outputStatus(map<string, DataSetInfo> dataSetStatus)
00359 {
00360 stringstream ss;
00361
00362 for(BOOST_AUTO(it, dataSetStatus.begin()); it != dataSetStatus.end(); ++it)
00363 {
00364 string paddedName = it->second.name + ":";
00365 while (paddedName.length() < 12) paddedName += " ";
00366 ss << "\t" << paddedName;
00367 if(it->second.downloaded)
00368 {
00369 ss << "downloaded.";
00370 }
00371 else
00372 {
00373 ss << "not on your system.";
00374 }
00375
00376 ss << endl;
00377 }
00378
00379 return ss.str();
00380 }
00381
00382
00383 string enterValidPath(string message)
00384 {
00385 bool validPath = false;
00386 string path_name;
00387 while(!validPath)
00388 {
00389 cout << message;
00390 getline(cin, path_name);
00391 validPath = fs::is_directory(fs::path(path_name));
00392 if(validPath == false)
00393 cout << ">> Not a valid path" << endl;
00394 }
00395
00396 return path_name;
00397 }
00398
00399
00400 void setConfig(Config& config)
00401 {
00402 string answer = "0";
00403 cout << "Current configuration:" << endl
00404 << "\t Download path: " << config.path_download << endl
00405 << "\t Result path: " << config.path_result << endl;
00406
00407 cout << "Data set status:" << endl;
00408 cout << outputStatus(config.dataSetStatus);
00409
00410 while(!(answer == "" || answer == "y" ||answer == "Y" || answer == "n" || answer == "N"))
00411 {
00412 cout << endl << "Do you want to change something? [y/N]: ";
00413 getline(cin, answer);
00414
00415
00416
00417 if(answer == "y" ||answer == "Y" )
00418 {
00419 config.path_download = enterValidPath("Enter data set path or where they will be downloaded: ");
00420 cout << endl;
00421 config.path_result = enterValidPath("Enter the result path (where the result of the test will be saved): ");
00422
00423 }
00424 }
00425 }
00426
00427 void saveConfig(Config& config)
00428 {
00429 YAML::Emitter emitter;
00430
00431 emitter << YAML::BeginMap;
00432 emitter << YAML::Key << "path_download";
00433 emitter << YAML::Value << config.path_download;
00434 emitter << YAML::Key << "path_result";
00435 emitter << YAML::Value << config.path_result;
00436
00437 for(BOOST_AUTO(it, config.dataSetStatus.begin()); it != config.dataSetStatus.end(); ++it)
00438 {
00439 emitter << YAML::Key << it->second.name;
00440 emitter << YAML::Value;
00441 emitter << YAML::BeginMap;
00442 emitter << YAML::Key << "downloaded";
00443 emitter << YAML::Value << it->second.downloaded;
00444 emitter << YAML::EndMap;
00445 }
00446 emitter << YAML::EndMap;
00447
00448 std::ofstream fout(config.path_config.c_str());
00449 if (!fout.good())
00450 {
00451 cerr << "Warning, cannot open config file " << config.path_config << ", content was not saved" << endl;
00452 return;
00453 }
00454 fout << emitter.c_str();
00455 fout.close();
00456 }
00457
00458 void loadConfig(Config& config)
00459 {
00460 ifstream f_config(config.path_config.c_str());
00461 if (!f_config.good())
00462 {
00463 cerr << "Warning, cannot open config file " << config.path_config << ", content was not loaded" << endl;
00464 return;
00465 }
00466 YAML::Parser parser(f_config);
00467
00468 YAML::Node doc;
00469 while(parser.GetNextDocument(doc))
00470 {
00471 doc["path_download"] >> config.path_download;
00472 doc["path_result"] >> config.path_result;
00473 for(BOOST_AUTO(it, config.dataSetStatus.begin()); it != config.dataSetStatus.end(); ++it)
00474 {
00475 string dataSetName = it->second.name;
00476 const YAML::Node &node = doc[dataSetName];
00477 node["downloaded"] >> it->second.downloaded;
00478 }
00479 }
00480 }
00481
00482 void downloadDataSets(Config& config, po::variables_map &vm)
00483 {
00484
00485 fs::path extra_path(config.path_download+"/validation/");
00486 if(!fs::is_directory(extra_path))
00487 fs::create_directories(extra_path);
00488 extra_path = fs::path(config.path_download+"/protocols/");
00489 if(!fs::is_directory(extra_path))
00490 fs::create_directories(extra_path);
00491
00492 for(BOOST_AUTO(it, config.dataSetStatus.begin()); it != config.dataSetStatus.end(); ++it)
00493 {
00494 if(vm.count("all") || vm.count(it->second.name))
00495 {
00496 cout << ">> Fetching files for: " << it->second.name << "..." << endl << endl;
00497 fs::path d_path(config.path_download+it->second.name);
00498 if(!fs::is_directory(d_path))
00499 fs::create_directories(d_path);
00500
00501 string cmd;
00502 int sysRes;
00503 #define CHECK_RES if (sysRes != 0) { cerr << "Warning, system command \"" << cmd << "\" failed with result code " << sysRes << endl; }
00504
00505
00506 cout << ">> Downloading validation file ..." << endl;
00507 cmd = "wget -P " + config.path_download + "/validation/ " + config.path_server_validation + "/" + it->second.name + "_validation.csv";
00508 sysRes = system(cmd.c_str());
00509 CHECK_RES
00510
00511
00512 cout << ">> Downloading protocol file ..." << endl;
00513 cmd = "wget -P " + config.path_download + "/protocols/ " + config.path_server_protocols + "/" + it->second.name + "_protocol.csv";
00514 sysRes = system(cmd.c_str());
00515 CHECK_RES
00516
00517
00518 cout << ">> Downloading data set files ..." << endl;
00519 cmd = "wget -P " + d_path.string() + " " + it->second.path;
00520 sysRes = system(cmd.c_str());
00521 CHECK_RES
00522
00523 cout << ">> Unzipping dataset..." << endl;
00524 cmd = "unzip -q " + d_path.string() + "/local_frame.zip -d " + d_path.string() + "/";
00525 sysRes = system(cmd.c_str());
00526 CHECK_RES
00527
00528 cmd = "rm " + d_path.string() + "/local_frame.zip";
00529 sysRes = system(cmd.c_str());
00530 CHECK_RES
00531
00532 it->second.downloaded = true;
00533 }
00534 }
00535 }
00536
00537 void validateFileInfo(const PMIO::FileInfo &fileInfo)
00538 {
00539 if(fileInfo.initialTransformation.rows() == 0)
00540 {
00541 cout << "Missing columns representing initial transformation \"iTxy\"" << endl;
00542 abort();
00543 }
00544
00545 if(fileInfo.readingFileName == "")
00546 {
00547 cout << "Missing column named \"reading\"" << endl;
00548 abort();
00549 }
00550
00551 if(fileInfo.referenceFileName == "")
00552 {
00553 cout << "Missing column named \"reference\"" << endl;
00554 abort();
00555 }
00556
00557 }
00558
00559 boost::mutex m_display;
00560 void displayLoadingBar(const int &coreId, const int &i, const int &total, const int &nbFailures, const double sec, const double total_time)
00561 {
00562 const double average_time = total_time/double(i+1);
00563 int time = average_time*double(total-i);
00564 const int h=time/3600;
00565 time=time%3600;
00566 const int m=time/60;
00567 time=time%60;
00568 const int s=time;
00569
00570
00571 m_display.lock();
00572
00573 move(coreId+1,0);
00574 clrtoeol();
00575 mvprintw(coreId+1, 10, " Core %2d: %5d/%5d failed: %2d last dur: %2.3f sec, avr: %2.3f sec, eta: %3dh%02dm%02d",coreId,i+1, total, nbFailures, sec, average_time, h, m, s);
00576 refresh();
00577 m_display.unlock();
00578
00579
00580 }
00581
00582
00583
00584
00585
00586
00587
00588
00589 EvaluationModule::EvaluationModule():
00590 coreId(0)
00591 {
00592 }
00593
00594 void EvaluationModule::evaluateSolution(const string &tmp_file_name, const string &yaml_config, const int &coreId, PMIO::FileInfoVector::const_iterator it_eval, PMIO::FileInfoVector::const_iterator it_end)
00595 {
00596 PM::DataPoints refCloud, readCloud;
00597 string last_read_name = "";
00598 string last_ref_name = "";
00599 const int count = std::distance(it_eval, it_end);
00600 int current_line = 0;
00601 timer t_eval_list;
00602
00603 std::ofstream fout(tmp_file_name.c_str());
00604 if (!fout.good())
00605 {
00606 cerr << "Warning, cannot open temporary result file " << tmp_file_name << ", evaluation was skipped" << endl;
00607 return;
00608 }
00609
00610 for( ; it_eval < it_end; ++it_eval)
00611 {
00612 timer t_singleTest;
00613
00614
00615 if(last_read_name != it_eval->readingFileName)
00616 {
00617 readCloud = PM::DataPoints::load(it_eval->readingFileName);
00618 last_read_name = it_eval->readingFileName;
00619 }
00620
00621 if(last_ref_name != it_eval->referenceFileName)
00622 {
00623 refCloud = PM::DataPoints::load(it_eval->referenceFileName);
00624 last_ref_name = it_eval->referenceFileName;
00625 }
00626
00627
00628 PM::ICP icp;
00629 ifstream ifs(yaml_config.c_str());
00630 icp.loadFromYaml(ifs);
00631
00632 const TP Tinit = it_eval->initialTransformation;
00633
00634 timer t_icp;
00635
00636 int nbFailures = 0;
00637 TP Tresult = TP::Identity(4,4);
00638
00639 try
00640 {
00641 Tresult = icp(readCloud, refCloud, Tinit);
00642 }
00643 catch (PM::ConvergenceError error)
00644 {
00645 nbFailures ++;
00646 }
00647
00648 fout << t_icp.elapsed();
00649
00650 for(int r=0; r<4;++r)
00651 for(int c=0; c<4;++c)
00652 fout << ", " << Tresult(r,c);
00653
00654 fout << "\n";
00655
00656
00657 displayLoadingBar(coreId, current_line, count, nbFailures, t_singleTest.elapsed(), t_eval_list.elapsed());
00658 current_line ++;
00659 }
00660
00661 fout.close();
00662 }