41 #include <boost/format.hpp>
42 #include <boost/filesystem/path.hpp>
43 #include <boost/filesystem/operations.hpp>
57 int main(
int argc,
char *argv[])
64 typedef PM::DataPoints
DP;
66 string outputFileName(argv[0]);
69 std::shared_ptr<PM::Transformation> rigidTrans;
70 rigidTrans =
PM::get().REG(Transformation).create(
"RigidTransformation");
73 std::shared_ptr<PM::DataPointsFilter> densityFilter =
74 PM::get().DataPointsFilterRegistrar.create(
75 "SurfaceNormalDataPointsFilter",
80 {
"keepDensities",
"1"}
84 std::shared_ptr<PM::DataPointsFilter> maxDensitySubsample =
85 PM::get().DataPointsFilterRegistrar.create(
86 "MaxDensityDataPointsFilter",
93 ifstream ifs(argv[1]);
95 icp.loadFromYaml(ifs);
99 PM::DataPoints mapPointCloud, newCloud;
100 TP T_to_map_from_new = TP::Identity(4,4);
102 for(
unsigned i=0; i < list.size(); i++)
104 cout <<
"---------------------\nLoading: " << list[i].readingFileName << endl;
107 newCloud =
DP::load(list[i].readingFileName);
109 if(mapPointCloud.getNbPoints() == 0)
111 mapPointCloud = newCloud;
121 const TP prior = T_to_map_from_new;
123 T_to_map_from_new = icp(newCloud, mapPointCloud, prior);
125 catch (PM::ConvergenceError& error)
127 cout <<
"ERROR PM::ICP failed to converge: " << endl;
128 cout <<
" " << error.what() << endl;
134 T_to_map_from_new = rigidTrans->correctParameters(T_to_map_from_new);
137 newCloud = rigidTrans->compute(newCloud, T_to_map_from_new);
140 mapPointCloud.concatenate(newCloud);
143 mapPointCloud = densityFilter->filter(mapPointCloud);
144 mapPointCloud = maxDensitySubsample->filter(mapPointCloud);
147 stringstream outputFileNameIter;
148 outputFileNameIter << outputFileName <<
"_" << i <<
".vtk";
150 cout <<
"outputFileName: " << outputFileNameIter.str() << endl;
151 mapPointCloud.save(outputFileNameIter.str());
161 cerr <<
"Error in command line, usage " << argv[0] <<
" icpConfiguration.yaml listOfFiles.csv" << endl;
162 cerr << endl <<
"Example:" << endl;
163 cerr << argv[0] <<
" ../examples/data/default.yaml ../examples/data/carCloudList.csv" << endl;
164 cerr << endl <<
" - or - " << endl << endl;
165 cerr << argv[0] <<
" ../examples/data/default.yaml ../examples/data/cloudList.csv" << endl << endl;