eval_solution.cpp
Go to the documentation of this file.
00001 // kate: replace-tabs off; indent-width 4; indent-mode normal
00002 // vim: ts=4:sw=4:noexpandtab
00003 /*
00004 
00005 Copyright (c) 2010--2012,
00006 François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
00007 You can contact the authors at <f dot pomerleau at gmail dot com> and
00008 <stephane at magnenat dot net>
00009 
00010 All rights reserved.
00011 
00012 Redistribution and use in source and binary forms, with or without
00013 modification, are permitted provided that the following conditions are met:
00014     * Redistributions of source code must retain the above copyright
00015       notice, this list of conditions and the following disclaimer.
00016     * Redistributions in binary form must reproduce the above copyright
00017       notice, this list of conditions and the following disclaimer in the
00018       documentation and/or other materials provided with the distribution.
00019     * Neither the name of the <organization> nor the
00020       names of its contributors may be used to endorse or promote products
00021       derived from this software without specific prior written permission.
00022 
00023 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00024 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00025 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00026 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
00027 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00028 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00030 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00031 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00032 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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         // Default values
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         // Option parsing
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         // Evaluation configuration
00153         Config config;
00154 
00155         if (vm.count("help")) {
00156                 cout << desc << endl;
00157                 return 1;
00158         }
00159 
00160         // Check for config
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(); // ncurse screen
00204 
00205         // Starting evaluation
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();                       /* End curses mode                */
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();                       /* End curses mode                */
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                         // Ensure that all required columns are there
00232                         validateFileInfo(eval_list[0]);
00233                         move(0,0);
00234                         clrtoeol();
00235                         mvprintw(0, 0, " <<< Evaluating %s >>>", it->second.name.c_str());
00236                         //cout << endl << "<<< Evaluating " << it->second.name << " >>>" << endl;
00237                 
00238                         // Spawn threads
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                         // List of thread
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                                 // Start evaluation for every line
00260                                 if(j == nbCore-1)
00261                                 {
00262                                         // last core receive the reste
00263                                         a_threads[j] = boost::thread(&EvaluationModule::evaluateSolution, &v_evalModules[j], name.str(), yaml_config, j, it_start, eval_list.end());
00264                                         //evalCore.evaluateSolution(yaml_config, j, it_start, eval_list.end());
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                                         //evalCore.evaluateSolution(yaml_config, j, it_start, it_start + nbPerThread);
00270                                         it_start += nbPerThread;
00271                                 }
00272                                 cout << endl;
00273                         }
00274 
00275                         // Wait for the results
00276                         for (int k=0; k<nbCore; ++k)
00277                         {
00278                                 if(a_threads[k].joinable())
00279                                         a_threads[k].join();
00280                         }
00281 
00282                         // Write the results to a file
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                         // dump header
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                         // dump results
00309                         // for all threads
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();/* End curses mode               */
00323                                         cerr << "Cannot find tmp file named "<< tmp_file_name << endl;
00324                                 }
00325                         }
00326 
00327                         fout.close();
00328                 }
00329         }
00330         endwin();                       /* End curses mode                */
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                 //if(answer == "" || answer == "n" ||answer == "N" )
00416                 //      return 1;
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         // Ensure that validation and protocol folders are there
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                         // Dowload validation
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                         // Dowload protocol
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                         // Download data set
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         //ncurse output
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();                      /* Print it on to the real screen */
00577         m_display.unlock();
00578 
00579         //cout << "\r  Core " << coreId << ": " << i+1 << "/" << total << "     last dur: " <<  sec << " sec, avr: " << average_time << " sec, eta: " << h << "h" << m % 60 << "m" << eta % 60 << "s             ";     
00580 }
00581 
00582 
00583 
00584 
00585 
00586 // ----------------------------------------------------
00587 // EvaluationModule
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                 // Load point clouds
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                 // Build ICP based on config file
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                 // Apply ICP
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                 // Output eta to console
00657                 displayLoadingBar(coreId, current_line, count, nbFailures, t_singleTest.elapsed(), t_eval_list.elapsed());
00658                 current_line ++;
00659         }
00660 
00661         fout.close();
00662 }


libpointmatcher
Author(s):
autogenerated on Mon Sep 14 2015 02:59:04