build_map.cpp
Go to the documentation of this file.
1 // kate: replace-tabs off; indent-width 4; indent-mode normal
2 // vim: ts=4:sw=4:noexpandtab
3 /*
4 
5 Copyright (c) 2010--2012,
6 François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
7 You can contact the authors at <f dot pomerleau at gmail dot com> and
8 <stephane at magnenat dot net>
9 
10 All rights reserved.
11 
12 Redistribution and use in source and binary forms, with or without
13 modification, are permitted provided that the following conditions are met:
14  * Redistributions of source code must retain the above copyright
15  notice, this list of conditions and the following disclaimer.
16  * Redistributions in binary form must reproduce the above copyright
17  notice, this list of conditions and the following disclaimer in the
18  documentation and/or other materials provided with the distribution.
19  * Neither the name of the <organization> nor the
20  names of its contributors may be used to endorse or promote products
21  derived from this software without specific prior written permission.
22 
23 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
24 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
25 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
27 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
28 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
30 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
31 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
32 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 
34 */
35 
37 #include "pointmatcher/IO.h"
38 #include <cassert>
39 #include <iostream>
40 #include <fstream>
41 #include <boost/format.hpp>
42 #include <boost/filesystem/path.hpp>
43 #include <boost/filesystem/operations.hpp>
44 #include <boost/lexical_cast.hpp>
45 
46 using namespace std;
47 using namespace PointMatcherSupport;
48 
49 void validateArgs(int argc, char *argv[]);
50 void setupArgs(int argc, char *argv[], unsigned int& startId, unsigned int& endId, string& extension);
51 vector<string> loadYamlFile(string listFileName);
52 
60 int main(int argc, char *argv[])
61 {
62  validateArgs(argc, argv);
63 
64  typedef PointMatcher<float> PM;
67  typedef PM::DataPoints DP;
68 
69  // Process arguments
70  PMIO::FileInfoVector list(argv[1]);
71  const unsigned totalPointCount = boost::lexical_cast<unsigned>(argv[2]);
72  string outputFileName(argv[3]);
73 
74 
75  setLogger(PM::get().LoggerRegistrar.create("FileLogger"));
76 
77  PM::DataPoints mapCloud;
78 
79  PM::DataPoints lastCloud, newCloud;
80  TP T = TP::Identity(4,4);
81 
82  // Define transformation chain
83  std::shared_ptr<PM::Transformation> transformation;
84  transformation = PM::get().REG(Transformation).create("RigidTransformation");
85 
86  // This filter will remove a sphere of 1 m radius. Easy way to remove the sensor self-scanning.
87  std::shared_ptr<PM::DataPointsFilter> removeScanner =
88  PM::get().DataPointsFilterRegistrar.create(
89  "MinDistDataPointsFilter",
90  {{"minDist", "1.0"}}
91  );
92 
93  // This filter will randomly remove 35% of the points.
94  std::shared_ptr<PM::DataPointsFilter> randSubsample =
95  PM::get().DataPointsFilterRegistrar.create(
96  "RandomSamplingDataPointsFilter",
97  {{"prob", toParam(0.65)}}
98  );
99 
100  // For a complete description of filter, see
101  // https://github.com/norlab-ulaval/libpointmatcher/blob/master/doc/Datafilters.md
102  std::shared_ptr<PM::DataPointsFilter> normalFilter =
103  PM::get().DataPointsFilterRegistrar.create(
104  "SurfaceNormalDataPointsFilter",
105  {
106  {"knn", toParam(10)},
107  {"epsilon", toParam(5)},
108  {"keepNormals",toParam(1)},
109  {"keepDensities",toParam(0)}
110  }
111  );
112 
113  std::shared_ptr<PM::DataPointsFilter> densityFilter =
114  PM::get().DataPointsFilterRegistrar.create(
115  "SurfaceNormalDataPointsFilter",
116  {
117  {"knn", "10"},
118  {"epsilon", "5"},
119  {"keepDensities","1"},
120  {"keepNormals","0"}
121  }
122  );
123 
124  std::shared_ptr<PM::DataPointsFilter> observationDirectionFilter =
125  PM::get().DataPointsFilterRegistrar.create(
126  "ObservationDirectionDataPointsFilter"
127  );
128 
129  std::shared_ptr<PM::DataPointsFilter> orientNormalFilter =
130  PM::get().DataPointsFilterRegistrar.create(
131  "OrientNormalsDataPointsFilter",
132  {{"towardCenter", "1"}}
133  );
134 
135  std::shared_ptr<PM::DataPointsFilter> uniformSubsample =
136  PM::get().DataPointsFilterRegistrar.create(
137  "MaxDensityDataPointsFilter",
138  {{"maxDensity", toParam(30)}}
139  );
140 
141  std::shared_ptr<PM::DataPointsFilter> shadowFilter =
142  PM::get().DataPointsFilterRegistrar.create(
143  "ShadowDataPointsFilter"
144  );
145 
146  for(unsigned i=0; i < list.size(); i++)
147  {
148  cout << endl << "-----------------------------" << endl;
149  cout << "Loading " << list[i].readingFileName;
150  newCloud = DP::load(list[i].readingFileName);
151 
152  cout << " found " << newCloud.getNbPoints() << " points. " << endl;
153 
154 
155  if(list[i].groundTruthTransformation.rows() != 0)
156  T = list[i].groundTruthTransformation;
157  else
158  {
159  cout << "ERROR: the field gTXX (ground truth) is required" << endl;
160  abort();
161  }
162 
164 
165  // Remove the scanner
166  newCloud = removeScanner->filter(newCloud);
167 
168 
169  // Accelerate the process and dissolve lines
170  newCloud = randSubsample->filter(newCloud);
171 
172  // Build filter to remove shadow points and down-sample
173  newCloud = normalFilter->filter(newCloud);
174  newCloud = observationDirectionFilter->filter(newCloud);
175  newCloud = orientNormalFilter->filter(newCloud);
176  newCloud = shadowFilter->filter(newCloud);
177 
178  // Transforme pointCloud
179  cout << "Transformation matrix: " << endl << T << endl;
180  newCloud = transformation->compute(newCloud, T);
181 
182  if(i==0)
183  {
184  mapCloud = newCloud;
185  }
186  else
187  {
188  mapCloud.concatenate(newCloud);
189 
190  // Control point cloud size
191  double probToKeep = totalPointCount/(double)mapCloud.features.cols();
192  if(probToKeep < 1)
193  {
194 
195  mapCloud = densityFilter->filter(mapCloud);
196  mapCloud = uniformSubsample->filter(mapCloud);
197 
198  probToKeep = totalPointCount/(double)mapCloud.features.cols();
199 
200  if(probToKeep < 1)
201  {
202  cout << "Randomly keep " << probToKeep*100 << "\% points" << endl;
203  randSubsample = PM::get().DataPointsFilterRegistrar.create(
204  "RandomSamplingDataPointsFilter",
205  {{"prob", toParam(probToKeep)}}
206  );
207  mapCloud = randSubsample->filter(mapCloud);
208  }
209  }
210  }
211 
212  stringstream outputFileNameIter;
213  outputFileNameIter << boost::filesystem::path(outputFileName).stem().c_str() << "_" << i << ".vtk";
214 
215  mapCloud.save(outputFileNameIter.str());
216  }
217 
218  mapCloud = densityFilter->filter(mapCloud);
219  mapCloud = uniformSubsample->filter(mapCloud);
220 
221  mapCloud = densityFilter->filter(mapCloud);
222 
223  cout << endl ;
224  cout << "-----------------------------" << endl;
225  cout << "-----------------------------" << endl;
226  cout << "Final number of points in the map: " << mapCloud.getNbPoints() << endl;
227  mapCloud.save(outputFileName);
228  cout << endl ;
229 
230  return 0;
231 }
232 
233 void validateArgs(int argc, char *argv[])
234 {
235  if (!(argc == 4))
236  {
237  cerr << endl;
238  cerr << "Error in command line, usage " << argv[0] << " listOfFiles.csv maxPoint outputFileName.{vtk,csv,ply}" << endl;
239  cerr << endl;
240  cerr << " example using file from example/data: " << endl;
241  cerr << " " << argv[0] << " carCloudList.csv 30000 test.vtk" << endl;
242  cerr << endl;
243  cerr << "Note: the file listOfFiles.csv need to include a serialize transformation matrix. For example:" << endl;
244  cerr << " fileName1.vtk, T00, T01, T02, T03, T10, T11, T12, T13, T20, T21, T22, T23, T30, T31, T32" << endl;
245  cerr << endl;
246  cerr << "Where Txy would be a 4x4 transformation matrix:" << endl;
247  cerr << " [T00 T01 T02 T03] " << endl;
248  cerr << " [T10 T11 T12 T13] " << endl;
249  cerr << " [T20 T21 T22 T23] " << endl;
250  cerr << " [T30 T31 T32 T33] (i.e., 0,0,0,1)" << endl;
251  cerr << endl;
252  cerr << "For more data visit:" << endl;
253  cerr << " http://projects.asl.ethz.ch/datasets/doku.php?id=laserregistration:laserregistration" << endl;
254  cerr << endl;
255 
256  abort();
257  }
258 }
259 
260 
261 
PointMatcherIO
IO Functions and classes that are dependant on scalar type are defined in this templatized class.
Definition: IO.h:43
PointMatcher< float >::Parameters
Parametrizable::Parameters Parameters
alias
Definition: PointMatcher.h:186
PointMatcherSupport::toParam
std::string toParam(const S &value)
Return the string value using lexical_cast.
Definition: Parametrizable.h:123
build_map.T
T
Definition: build_map.py:34
DataPoints
PM::DataPoints DataPoints
Definition: pypoint_matcher_helper.h:16
PointMatcher< float >
PointMatcherSupport::setLogger
void setLogger(std::shared_ptr< Logger > newLogger)
Set a new logger, protected by a mutex.
Definition: Logger.cpp:98
PointMatcher::Transformation
A function that transforms points and their descriptors given a transformation matrix.
Definition: PointMatcher.h:404
loadYamlFile
vector< string > loadYamlFile(string listFileName)
align_sequence.params
params
Definition: align_sequence.py:13
PMIO
PointMatcherIO< float > PMIO
Definition: eval_solution.cpp:62
PointMatcherSupport::Registrar::create
std::shared_ptr< Interface > create(const std::string &name, const Parametrizable::Parameters &params=Parametrizable::Parameters()) const
Create an instance.
Definition: Registrar.h:161
PointMatcherIO::FileInfoVector
A vector of file info, to be used in batch processing.
Definition: IO.h:245
validateArgs
void validateArgs(int argc, char *argv[])
Definition: build_map.cpp:233
setupArgs
void setupArgs(int argc, char *argv[], unsigned int &startId, unsigned int &endId, string &extension)
main
int main(int argc, char *argv[])
Definition: build_map.cpp:60
std
TP
PM::TransformationParameters TP
Definition: eval_solution.cpp:63
build_map.transformation
transformation
Definition: build_map.py:37
PointMatcher< float >::get
static const PointMatcher & get()
Return instances.
Definition: Registry.cpp:146
PointMatcherSupport
Functions and classes that are not dependant on scalar type are defined in this namespace.
Definition: Bibliography.cpp:45
DP
PM::DataPoints DP
Definition: convert.cpp:45
PM
PointMatcher< float > PM
Definition: eval_solution.cpp:61
PointMatcher::DataPoints::load
static DataPoints load(const std::string &fileName)
Load a point cloud from a file, determine format from extension.
Definition: pointmatcher/IO.cpp:375
PointMatcher.h
public interface
IO.h
PointMatcherSupport::Registrar
A factor for subclasses of Interface.
Definition: Registrar.h:59
PointMatcher< float >::TransformationParameters
Matrix TransformationParameters
A matrix holding the parameters a transformation.
Definition: PointMatcher.h:182


libpointmatcher
Author(s):
autogenerated on Mon Sep 16 2024 02:24:07