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 <cassert>
00039 #include <iostream>
00040 #include <fstream>
00041 #include <boost/format.hpp>
00042 #include <boost/filesystem/path.hpp>
00043 #include <boost/filesystem/operations.hpp>
00044 #include <boost/lexical_cast.hpp>
00045
00046 using namespace std;
00047 using namespace PointMatcherSupport;
00048
00049 void validateArgs(int argc, char *argv[]);
00050 void setupArgs(int argc, char *argv[], unsigned int& startId, unsigned int& endId, string& extension);
00051 vector<string> loadYamlFile(string listFileName);
00052
00060 int main(int argc, char *argv[])
00061 {
00062 validateArgs(argc, argv);
00063
00064 typedef PointMatcher<float> PM;
00065 typedef PointMatcherIO<float> PMIO;
00066 typedef PM::TransformationParameters TP;
00067 typedef PM::DataPoints DP;
00068
00069
00070 PMIO::FileInfoVector list(argv[1]);
00071 const unsigned totalPointCount = boost::lexical_cast<unsigned>(argv[2]);
00072 string outputFileName(argv[3]);
00073
00074
00075 setLogger(PM::get().LoggerRegistrar.create("FileLogger"));
00076
00077 PM::DataPoints mapCloud;
00078
00079 PM::DataPoints lastCloud, newCloud;
00080 TP T = TP::Identity(4,4);
00081
00082
00083 std::shared_ptr<PM::Transformation> transformation;
00084 transformation = PM::get().REG(Transformation).create("RigidTransformation");
00085
00086
00087 std::shared_ptr<PM::DataPointsFilter> removeScanner =
00088 PM::get().DataPointsFilterRegistrar.create(
00089 "MinDistDataPointsFilter",
00090 {{"minDist", "1.0"}}
00091 );
00092
00093
00094 std::shared_ptr<PM::DataPointsFilter> randSubsample =
00095 PM::get().DataPointsFilterRegistrar.create(
00096 "RandomSamplingDataPointsFilter",
00097 {{"prob", toParam(0.65)}}
00098 );
00099
00100
00101
00102 std::shared_ptr<PM::DataPointsFilter> normalFilter =
00103 PM::get().DataPointsFilterRegistrar.create(
00104 "SurfaceNormalDataPointsFilter",
00105 {
00106 {"knn", toParam(10)},
00107 {"epsilon", toParam(5)},
00108 {"keepNormals",toParam(1)},
00109 {"keepDensities",toParam(0)}
00110 }
00111 );
00112
00113 std::shared_ptr<PM::DataPointsFilter> densityFilter =
00114 PM::get().DataPointsFilterRegistrar.create(
00115 "SurfaceNormalDataPointsFilter",
00116 {
00117 {"knn", "10"},
00118 {"epsilon", "5"},
00119 {"keepDensities","1"},
00120 {"keepNormals","0"}
00121 }
00122 );
00123
00124 std::shared_ptr<PM::DataPointsFilter> observationDirectionFilter =
00125 PM::get().DataPointsFilterRegistrar.create(
00126 "ObservationDirectionDataPointsFilter"
00127 );
00128
00129 std::shared_ptr<PM::DataPointsFilter> orientNormalFilter =
00130 PM::get().DataPointsFilterRegistrar.create(
00131 "OrientNormalsDataPointsFilter",
00132 {{"towardCenter", "1"}}
00133 );
00134
00135 std::shared_ptr<PM::DataPointsFilter> uniformSubsample =
00136 PM::get().DataPointsFilterRegistrar.create(
00137 "MaxDensityDataPointsFilter",
00138 {{"maxDensity", toParam(30)}}
00139 );
00140
00141 std::shared_ptr<PM::DataPointsFilter> shadowFilter =
00142 PM::get().DataPointsFilterRegistrar.create(
00143 "ShadowDataPointsFilter"
00144 );
00145
00146 for(unsigned i=0; i < list.size(); i++)
00147 {
00148 cout << endl << "-----------------------------" << endl;
00149 cout << "Loading " << list[i].readingFileName;
00150 newCloud = DP::load(list[i].readingFileName);
00151
00152 cout << " found " << newCloud.getNbPoints() << " points. " << endl;
00153
00154
00155 if(list[i].groundTruthTransformation.rows() != 0)
00156 T = list[i].groundTruthTransformation;
00157 else
00158 {
00159 cout << "ERROR: the field gTXX (ground truth) is required" << endl;
00160 abort();
00161 }
00162
00163 PM::Parameters params;
00164
00165
00166 newCloud = removeScanner->filter(newCloud);
00167
00168
00169
00170 newCloud = randSubsample->filter(newCloud);
00171
00172
00173 newCloud = normalFilter->filter(newCloud);
00174 newCloud = observationDirectionFilter->filter(newCloud);
00175 newCloud = orientNormalFilter->filter(newCloud);
00176 newCloud = shadowFilter->filter(newCloud);
00177
00178
00179 cout << "Transformation matrix: " << endl << T << endl;
00180 newCloud = transformation->compute(newCloud, T);
00181
00182 if(i==0)
00183 {
00184 mapCloud = newCloud;
00185 }
00186 else
00187 {
00188 mapCloud.concatenate(newCloud);
00189
00190
00191 double probToKeep = totalPointCount/(double)mapCloud.features.cols();
00192 if(probToKeep < 1)
00193 {
00194
00195 mapCloud = densityFilter->filter(mapCloud);
00196 mapCloud = uniformSubsample->filter(mapCloud);
00197
00198 probToKeep = totalPointCount/(double)mapCloud.features.cols();
00199
00200 if(probToKeep < 1)
00201 {
00202 cout << "Randomly keep " << probToKeep*100 << "\% points" << endl;
00203 randSubsample = PM::get().DataPointsFilterRegistrar.create(
00204 "RandomSamplingDataPointsFilter",
00205 {{"prob", toParam(probToKeep)}}
00206 );
00207 mapCloud = randSubsample->filter(mapCloud);
00208 }
00209 }
00210 }
00211
00212 stringstream outputFileNameIter;
00213 outputFileNameIter << boost::filesystem::path(outputFileName).stem().c_str() << "_" << i << ".vtk";
00214
00215 mapCloud.save(outputFileNameIter.str());
00216 }
00217
00218 mapCloud = densityFilter->filter(mapCloud);
00219 mapCloud = uniformSubsample->filter(mapCloud);
00220
00221 mapCloud = densityFilter->filter(mapCloud);
00222
00223 cout << endl ;
00224 cout << "-----------------------------" << endl;
00225 cout << "-----------------------------" << endl;
00226 cout << "Final number of points in the map: " << mapCloud.getNbPoints() << endl;
00227 mapCloud.save(outputFileName);
00228 cout << endl ;
00229
00230 return 0;
00231 }
00232
00233 void validateArgs(int argc, char *argv[])
00234 {
00235 if (!(argc == 4))
00236 {
00237 cerr << endl;
00238 cerr << "Error in command line, usage " << argv[0] << " listOfFiles.csv maxPoint outputFileName.{vtk,csv,ply}" << endl;
00239 cerr << endl;
00240 cerr << " example using file from example/data: " << endl;
00241 cerr << " " << argv[0] << " carCloudList.csv 30000 test.vtk" << endl;
00242 cerr << endl;
00243 cerr << "Note: the file listOfFiles.csv need to include a serialize transformation matrix. For example:" << endl;
00244 cerr << " fileName1.vtk, T00, T01, T02, T03, T10, T11, T12, T13, T20, T21, T22, T23, T30, T31, T32" << endl;
00245 cerr << endl;
00246 cerr << "Where Txy would be a 4x4 transformation matrix:" << endl;
00247 cerr << " [T00 T01 T02 T03] " << endl;
00248 cerr << " [T10 T11 T12 T13] " << endl;
00249 cerr << " [T20 T21 T22 T23] " << endl;
00250 cerr << " [T30 T31 T32 T33] (i.e., 0,0,0,1)" << endl;
00251 cerr << endl;
00252 cerr << "For more data visit:" << endl;
00253 cerr << " http://projects.asl.ethz.ch/datasets/doku.php?id=laserregistration:laserregistration" << endl;
00254 cerr << endl;
00255
00256 abort();
00257 }
00258 }
00259
00260
00261