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/Bibliography.h"
00038
00039 #include "boost/filesystem.hpp"
00040
00041 #include <cassert>
00042 #include <fstream>
00043 #include <iostream>
00044 #include <algorithm>
00045
00046 using namespace std;
00047 using namespace PointMatcherSupport;
00048
00049 typedef PointMatcher<float> PM;
00050 typedef PM::DataPoints DP;
00051 typedef PM::Parameters Parameters;
00052
00053 void listModules();
00054 int validateArgs(const int argc, const char *argv[],
00055 bool& isTransfoSaved,
00056 string& configFile,
00057 string& outputBaseFile,
00058 string& initTranslation, string& initRotation);
00059 PM::TransformationParameters parseTranslation(string& translation,
00060 const int cloudDimension);
00061 PM::TransformationParameters parseRotation(string& rotation,
00062 const int cloudDimension);
00063 void usage(const char *argv[]);
00064
00072 int main(int argc, const char *argv[])
00073 {
00074 bool isTransfoSaved = false;
00075 string configFile;
00076 string outputBaseFile("test");
00077 string initTranslation("0,0,0");
00078 string initRotation("1,0,0;0,1,0;0,0,1");
00079 const int ret = validateArgs(argc, argv, isTransfoSaved, configFile,
00080 outputBaseFile, initTranslation, initRotation);
00081 if (ret != 0)
00082 {
00083 return ret;
00084 }
00085 const char *refFile(argv[argc-2]);
00086 const char *dataFile(argv[argc-1]);
00087
00088
00089 const DP ref(DP::load(refFile));
00090 const DP data(DP::load(dataFile));
00091
00092
00093 PM::ICP icp;
00094
00095 if (configFile.empty())
00096 {
00097
00098 icp.setDefault();
00099 }
00100 else
00101 {
00102
00103 ifstream ifs(configFile.c_str());
00104 if (!ifs.good())
00105 {
00106 cerr << "Cannot open config file " << configFile << ", usage:"; usage(argv); exit(1);
00107 }
00108 icp.loadFromYaml(ifs);
00109 }
00110
00111 int cloudDimension = ref.getEuclideanDim();
00112
00113 if (!(cloudDimension == 2 || cloudDimension == 3))
00114 {
00115 cerr << "Invalid input point clouds dimension" << endl;
00116 exit(1);
00117 }
00118
00119 PM::TransformationParameters translation =
00120 parseTranslation(initTranslation, cloudDimension);
00121 PM::TransformationParameters rotation =
00122 parseRotation(initRotation, cloudDimension);
00123 PM::TransformationParameters initTransfo = translation*rotation;
00124
00125 std::shared_ptr<PM::Transformation> rigidTrans;
00126 rigidTrans = PM::get().REG(Transformation).create("RigidTransformation");
00127
00128 if (!rigidTrans->checkParameters(initTransfo)) {
00129 cerr << endl
00130 << "Initial transformation is not rigid, identiy will be used"
00131 << endl;
00132 initTransfo = PM::TransformationParameters::Identity(
00133 cloudDimension+1,cloudDimension+1);
00134 }
00135
00136 const DP initializedData = rigidTrans->compute(data, initTransfo);
00137
00138
00139 PM::TransformationParameters T = icp(initializedData, ref);
00140 float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
00141 cout << "match ratio: " << matchRatio << endl;
00142
00143
00144
00145
00146 DP data_out(initializedData);
00147 icp.transformations.apply(data_out, T);
00148
00149 cout << endl << "------------------" << endl;
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162 PM::Matches matches;
00163
00164 Parametrizable::Parameters params;
00165 params["knn"] = toParam(1);
00166 params["epsilon"] = toParam(0);
00167
00168 std::shared_ptr<PM::Matcher> matcherHausdorff = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
00169
00170
00171 matcherHausdorff->init(ref);
00172 matches = matcherHausdorff->findClosests(data_out);
00173 float maxDist1 = matches.getDistsQuantile(1.0);
00174 float maxDistRobust1 = matches.getDistsQuantile(0.85);
00175
00176
00177 matcherHausdorff->init(data_out);
00178 matches = matcherHausdorff->findClosests(ref);
00179 float maxDist2 = matches.getDistsQuantile(1.0);
00180 float maxDistRobust2 = matches.getDistsQuantile(0.85);
00181
00182 float haussdorffDist = std::max(maxDist1, maxDist2);
00183 float haussdorffQuantileDist = std::max(maxDistRobust1, maxDistRobust2);
00184
00185 cout << "Haussdorff distance: " << std::sqrt(haussdorffDist) << " m" << endl;
00186 cout << "Haussdorff quantile distance: " << std::sqrt(haussdorffQuantileDist) << " m" << endl;
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198 icp.matcher->init(ref);
00199
00200
00201 matches = icp.matcher->findClosests(data_out);
00202
00203
00204 const PM::OutlierWeights outlierWeights = icp.outlierFilters.compute(data_out, ref, matches);
00205
00206
00207 const PM::ErrorMinimizer::ErrorElements matchedPoints( data_out, ref, outlierWeights, matches);
00208
00209
00210 const int dim = matchedPoints.reading.getEuclideanDim();
00211 const int nbMatchedPoints = matchedPoints.reading.getNbPoints();
00212 const PM::Matrix matchedRead = matchedPoints.reading.features.topRows(dim);
00213 const PM::Matrix matchedRef = matchedPoints.reference.features.topRows(dim);
00214
00215
00216 const PM::Matrix dist = (matchedRead - matchedRef).colwise().norm();
00217 const float meanDist = dist.sum()/nbMatchedPoints;
00218 cout << "Robust mean distance: " << meanDist << " m" << endl;
00219
00220
00221
00222 cout << "------------------" << endl << endl;
00223
00224
00225
00226 ref.save(outputBaseFile + "_ref.vtk");
00227 data.save(outputBaseFile + "_data_in.vtk");
00228 data_out.save(outputBaseFile + "_data_out.vtk");
00229 if(isTransfoSaved) {
00230 ofstream transfoFile;
00231 string initFileName = outputBaseFile + "_init_transfo.txt";
00232 string icpFileName = outputBaseFile + "_icp_transfo.txt";
00233 string completeFileName = outputBaseFile + "_complete_transfo.txt";
00234
00235 transfoFile.open(initFileName.c_str());
00236 if(transfoFile.is_open()) {
00237 transfoFile << initTransfo << endl;
00238 transfoFile.close();
00239 } else {
00240 cout << "Unable to write the initial transformation file\n" << endl;
00241 }
00242
00243 transfoFile.open(icpFileName.c_str());
00244 if(transfoFile.is_open()) {
00245 transfoFile << T << endl;
00246 transfoFile.close();
00247 } else {
00248 cout << "Unable to write the ICP transformation file\n" << endl;
00249 }
00250
00251 transfoFile.open(completeFileName.c_str());
00252 if(transfoFile.is_open()) {
00253 transfoFile << T*initTransfo << endl;
00254 transfoFile.close();
00255 } else {
00256 cout << "Unable to write the complete transformation file\n" << endl;
00257 }
00258 }
00259 else {
00260 cout << "ICP transformation:" << endl << T << endl;
00261 }
00262
00263 return 0;
00264 }
00265
00266
00267 template<typename R>
00268 void dumpRegistrar(const PM& pm, const R& registrar, const std::string& name,
00269 CurrentBibliography& bib)
00270 {
00271 cout << "* " << name << " *\n" << endl;
00272 for (BOOST_AUTO(it, registrar.begin()); it != registrar.end(); ++it)
00273 {
00274 cout << it->first << endl;
00275 cout << getAndReplaceBibEntries(it->second->description(), bib) << endl;
00276 cout << it->second->availableParameters() << endl;
00277 }
00278 cout << endl;
00279 }
00280
00281 #define DUMP_REGISTRAR_CONTENT(pm, name, bib) \
00282 dumpRegistrar(pm, pm.REG(name), # name, bib);
00283
00284 void listModules()
00285 {
00286 CurrentBibliography bib;
00287
00288 DUMP_REGISTRAR_CONTENT(PM::get(), Transformation, bib)
00289 DUMP_REGISTRAR_CONTENT(PM::get(), DataPointsFilter, bib)
00290 DUMP_REGISTRAR_CONTENT(PM::get(), Matcher, bib)
00291 DUMP_REGISTRAR_CONTENT(PM::get(), OutlierFilter, bib)
00292 DUMP_REGISTRAR_CONTENT(PM::get(), ErrorMinimizer, bib)
00293 DUMP_REGISTRAR_CONTENT(PM::get(), TransformationChecker, bib)
00294 DUMP_REGISTRAR_CONTENT(PM::get(), Inspector, bib)
00295 DUMP_REGISTRAR_CONTENT(PM::get(), Logger, bib)
00296
00297 cout << "* Bibliography *" << endl << endl;
00298 bib.dump(cout);
00299 }
00300
00301
00302 int validateArgs(const int argc, const char *argv[],
00303 bool& isTransfoSaved,
00304 string& configFile,
00305 string& outputBaseFile,
00306 string& initTranslation, string& initRotation)
00307 {
00308 if (argc == 1)
00309 {
00310 cerr << "Not enough arguments, usage:";
00311 usage(argv);
00312 return 1;
00313 }
00314 else if (argc == 2)
00315 {
00316 if (string(argv[1]) == "-l")
00317 {
00318 listModules();
00319 return -1;
00320 }
00321 else
00322 {
00323 cerr << "Wrong option, usage:";
00324 usage(argv);
00325 return 2;
00326 }
00327 }
00328
00329 const int endOpt(argc - 2);
00330 for (int i = 1; i < endOpt; i += 2)
00331 {
00332 const string opt(argv[i]);
00333 if (i + 1 > endOpt)
00334 {
00335 cerr << "Missing value for option " << opt << ", usage:"; usage(argv); exit(1);
00336 }
00337 if (opt == "--isTransfoSaved") {
00338 if (strcmp(argv[i+1], "1") == 0 || strcmp(argv[i+1], "true") == 0) {
00339 isTransfoSaved = true;
00340 }
00341 else if (strcmp(argv[i+1], "0") == 0
00342 || strcmp(argv[i+1],"false") == 0) {
00343 isTransfoSaved = false;
00344 }
00345 else {
00346 cerr << "Invalid value for parameter isTransfoSaved." << endl
00347 << "Value must be true or false or 1 or 0." << endl
00348 << "Default value will be used." << endl;
00349 }
00350 }
00351 else if (opt == "--config") {
00352 configFile = argv[i+1];
00353 }
00354 else if (opt == "--output") {
00355 outputBaseFile = argv[i+1];
00356 }
00357 else if (opt == "--initTranslation") {
00358 initTranslation = argv[i+1];
00359 }
00360 else if (opt == "--initRotation") {
00361 initRotation = argv[i+1];
00362 }
00363 else
00364 {
00365 cerr << "Unknown option " << opt << ", usage:"; usage(argv); exit(1);
00366 }
00367 }
00368 return 0;
00369 }
00370
00371 PM::TransformationParameters parseTranslation(string& translation,
00372 const int cloudDimension) {
00373 PM::TransformationParameters parsedTranslation;
00374 parsedTranslation = PM::TransformationParameters::Identity(
00375 cloudDimension+1,cloudDimension+1);
00376
00377 translation.erase(std::remove(translation.begin(), translation.end(), '['),
00378 translation.end());
00379 translation.erase(std::remove(translation.begin(), translation.end(), ']'),
00380 translation.end());
00381 std::replace( translation.begin(), translation.end(), ',', ' ');
00382 std::replace( translation.begin(), translation.end(), ';', ' ');
00383
00384 float translationValues[3] = {0};
00385 stringstream translationStringStream(translation);
00386 for( int i = 0; i < cloudDimension; i++) {
00387 if(!(translationStringStream >> translationValues[i])) {
00388 cerr << "An error occured while trying to parse the initial "
00389 << "translation." << endl
00390 << "No initial translation will be used" << endl;
00391 return parsedTranslation;
00392 }
00393 }
00394 float extraOutput = 0;
00395 if((translationStringStream >> extraOutput)) {
00396 cerr << "Wrong initial translation size" << endl
00397 << "No initial translation will be used" << endl;
00398 return parsedTranslation;
00399 }
00400
00401 for( int i = 0; i < cloudDimension; i++) {
00402 parsedTranslation(i,cloudDimension) = translationValues[i];
00403 }
00404
00405 return parsedTranslation;
00406 }
00407
00408 PM::TransformationParameters parseRotation(string &rotation,
00409 const int cloudDimension){
00410 PM::TransformationParameters parsedRotation;
00411 parsedRotation = PM::TransformationParameters::Identity(
00412 cloudDimension+1,cloudDimension+1);
00413
00414 rotation.erase(std::remove(rotation.begin(), rotation.end(), '['),
00415 rotation.end());
00416 rotation.erase(std::remove(rotation.begin(), rotation.end(), ']'),
00417 rotation.end());
00418 std::replace( rotation.begin(), rotation.end(), ',', ' ');
00419 std::replace( rotation.begin(), rotation.end(), ';', ' ');
00420
00421 float rotationMatrix[9] = {0};
00422 stringstream rotationStringStream(rotation);
00423 for( int i = 0; i < cloudDimension*cloudDimension; i++) {
00424 if(!(rotationStringStream >> rotationMatrix[i])) {
00425 cerr << "An error occured while trying to parse the initial "
00426 << "rotation." << endl
00427 << "No initial rotation will be used" << endl;
00428 return parsedRotation;
00429 }
00430 }
00431 float extraOutput = 0;
00432 if((rotationStringStream >> extraOutput)) {
00433 cerr << "Wrong initial rotation size" << endl
00434 << "No initial rotation will be used" << endl;
00435 return parsedRotation;
00436 }
00437
00438 for( int i = 0; i < cloudDimension*cloudDimension; i++) {
00439 parsedRotation(i/cloudDimension,i%cloudDimension) = rotationMatrix[i];
00440 }
00441
00442 return parsedRotation;
00443 }
00444
00445
00446 void usage(const char *argv[])
00447 {
00448
00449 cerr << endl << endl;
00450 cerr << "* To list modules:" << endl;
00451 cerr << " " << argv[0] << " -l" << endl;
00452 cerr << endl;
00453 cerr << "* To run ICP:" << endl;
00454 cerr << " " << argv[0] << " [OPTIONS] reference.csv reading.csv" << endl;
00455 cerr << endl;
00456 cerr << "OPTIONS can be a combination of:" << endl;
00457 cerr << "--config YAML_CONFIG_FILE Load the config from a YAML file (default: default parameters)" << endl;
00458 cerr << "--output BASEFILENAME Name of output files (default: test)" << endl;
00459 cerr << "--initTranslation [x,y,z] Add an initial 3D translation before applying ICP (default: 0,0,0)" << endl;
00460 cerr << "--initTranslation [x,y] Add an initial 2D translation before applying ICP (default: 0,0)" << endl;
00461 cerr << "--initRotation [r00,r01,r02,r10,r11,r12,r20,r21,r22]" << endl;
00462 cerr << " Add an initial 3D rotation before applying ICP (default: 1,0,0,0,1,0,0,0,1)" << endl;
00463 cerr << "--initRotation [r00,r01,r10,r11]" << endl;
00464 cerr << " Add an initial 2D rotation before applying ICP (default: 1,0,0,1)" << endl;
00465 cerr << "--isTransfoSaved BOOL Save transformation matrix in three different files:" << endl;
00466 cerr << " - BASEFILENAME_inti_transfo.txt" << endl;
00467 cerr << " - BASEFILENAME_icp_transfo.txt" << endl;
00468 cerr << " - BASEFILENAME_complete_transfo.txt" << endl;
00469 cerr << " (default: false)" << endl;
00470 cerr << endl;
00471 cerr << "Running this program with a VTKFileInspector as Inspector will create three" << endl;
00472 cerr << "vtk ouptput files: ./test_ref.vtk, ./test_data_in.vtk and ./test_data_out.vtk" << endl;
00473 cerr << endl << "2D Example:" << endl;
00474 cerr << " " << argv[0] << " ../examples/data/2D_twoBoxes.csv ../examples/data/2D_oneBox.csv" << endl;
00475 cerr << endl << "3D Example:" << endl;
00476 cerr << " " << argv[0] << " ../examples/data/car_cloud400.csv ../examples/data/car_cloud401.csv" << endl;
00477 cerr << endl;
00478 }