examples/icp.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 
38 
39 #include "boost/filesystem.hpp"
40 
41 #include <cassert>
42 #include <fstream>
43 #include <iostream>
44 #include <algorithm>
45 
46 using namespace std;
47 
52 
53 void listModules();
54 int validateArgs(const int argc, const char *argv[],
55  bool& isVerbose,
56  bool& isTransfoSaved,
57  string& configFile,
58  string& outputBaseFile,
59  string& initTranslation, string& initRotation);
61  const int cloudDimension);
63  const int cloudDimension);
64 // Helper functions
65 void usage(const char *argv[]);
66 
74 int main(int argc, const char *argv[])
75 {
76  bool isTransfoSaved = false;
77  bool isVerbose = false;
78  string configFile;
79  string outputBaseFile("test");
80  string initTranslation("0,0,0");
81  string initRotation("1,0,0;0,1,0;0,0,1");
82  const int ret = validateArgs(argc, argv, isVerbose, isTransfoSaved, configFile,
83  outputBaseFile, initTranslation, initRotation);
84  if (ret != 0)
85  {
86  return ret;
87  }
88  const char *refFile(argv[argc-2]);
89  const char *dataFile(argv[argc-1]);
90 
91  // Load point clouds
92  const DP ref(DP::load(refFile));
93  const DP data(DP::load(dataFile));
94 
95  // Create the default ICP algorithm
96  PM::ICP icp;
97 
98 
99  if (configFile.empty())
100  {
101  // See the implementation of setDefault() to create a custom ICP algorithm
102  icp.setDefault();
103  }
104  else
105  {
106  // load YAML config
107  ifstream ifs(configFile.c_str());
108  if (!ifs.good())
109  {
110  cerr << "Cannot open config file " << configFile << ", usage:"; usage(argv); exit(1);
111  }
112  icp.loadFromYaml(ifs);
113  }
114 
115 
116  int cloudDimension = ref.getEuclideanDim();
117 
118  if (!(cloudDimension == 2 || cloudDimension == 3))
119  {
120  cerr << "Invalid input point clouds dimension" << endl;
121  exit(1);
122  }
123 
124 
125 
127  parseTranslation(initTranslation, cloudDimension);
129  parseRotation(initRotation, cloudDimension);
130  PM::TransformationParameters initTransfo = translation*rotation;
131 
132  std::shared_ptr<PM::Transformation> rigidTrans;
133  rigidTrans = PM::get().REG(Transformation).create("RigidTransformation");
134 
135  if (!rigidTrans->checkParameters(initTransfo)) {
136  cerr << endl
137  << "Initial transformation is not rigid, identiy will be used"
138  << endl;
139  initTransfo = PM::TransformationParameters::Identity(
140  cloudDimension+1,cloudDimension+1);
141  }
142 
143  const DP initializedData = rigidTrans->compute(data, initTransfo);
144 
145  // Compute the transformation to express data in ref
146  PM::TransformationParameters T = icp(initializedData, ref);
147  if(isVerbose)
148  cout << "match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl;
149 
150  // Transform data to express it in ref
151  DP data_out(initializedData);
152  icp.transformations.apply(data_out, T);
153 
154  // Safe files to see the results
155  ref.save(outputBaseFile + "_ref.vtk");
156  data.save(outputBaseFile + "_data_in.vtk");
157  data_out.save(outputBaseFile + "_data_out.vtk");
158  if(isTransfoSaved) {
159  ofstream transfoFile;
160  string initFileName = outputBaseFile + "_init_transfo.txt";
161  string icpFileName = outputBaseFile + "_icp_transfo.txt";
162  string completeFileName = outputBaseFile + "_complete_transfo.txt";
163 
164  transfoFile.open(initFileName.c_str());
165  if(transfoFile.is_open()) {
166  transfoFile << initTransfo << endl;
167  transfoFile.close();
168  } else {
169  cerr << "Unable to write the initial transformation file\n" << endl;
170  }
171 
172  transfoFile.open(icpFileName.c_str());
173  if(transfoFile.is_open()) {
174  transfoFile << T << endl;
175  transfoFile.close();
176  } else {
177  cerr << "Unable to write the ICP transformation file\n" << endl;
178  }
179 
180  transfoFile.open(completeFileName.c_str());
181  if(transfoFile.is_open()) {
182  transfoFile << T*initTransfo << endl;
183  transfoFile.close();
184  } else {
185  cerr << "Unable to write the complete transformation file\n" << endl;
186  }
187  }
188  else
189  {
190  if(isVerbose)
191  cout << "ICP transformation:" << endl << T << endl;
192  }
193 
194  return 0;
195 }
196 
197 // The following code allows to dump all existing modules
198 template<typename R>
199 void dumpRegistrar(const PM& pm, const R& registrar, const std::string& name,
200  CurrentBibliography& bib)
201 {
202  cout << "* " << name << " *\n" << endl;
203  for (BOOST_AUTO(it, registrar.begin()); it != registrar.end(); ++it)
204  {
205  cout << it->first << endl;
206  cout << getAndReplaceBibEntries(it->second->description(), bib) << endl;
207  cout << it->second->availableParameters() << endl;
208  }
209  cout << endl;
210 }
211 
212 #define DUMP_REGISTRAR_CONTENT(pm, name, bib) \
213  dumpRegistrar(pm, pm.REG(name), # name, bib);
214 
216 {
218 
227 
228  cout << "* Bibliography *" << endl << endl;
229  bib.dump(cout);
230 }
231 
232 // Make sure that the command arguments make sense
233 int validateArgs(const int argc, const char *argv[],
234  bool& isVerbose,
235  bool& isTransfoSaved,
236  string& configFile,
237  string& outputBaseFile,
238  string& initTranslation, string& initRotation)
239 {
240  if (argc == 1)
241  {
242  cerr << "Not enough arguments, usage:";
243  usage(argv);
244  return 1;
245  }
246  else if (argc == 2)
247  {
248  if (string(argv[1]) == "-l")
249  {
250  listModules();
251  return -1; // we use -1 to say that we wish to quit but in a normal way
252  }
253  else
254  {
255  cerr << "Wrong option, usage:";
256  usage(argv);
257  return 2;
258  }
259  }
260 
261  const int endOpt(argc - 2);
262  for (int i = 1; i < endOpt; i += 2)
263  {
264  const string opt(argv[i]);
265  if (opt == "--verbose" || opt == "-v") {
266  isVerbose = true;
267  i --;
268  continue;
269  }
270  if (i + 1 > endOpt)
271  {
272  cerr << "Missing value for option " << opt << ", usage:"; usage(argv); exit(1);
273  }
274  if (opt == "--isTransfoSaved") {
275  if (strcmp(argv[i+1], "1") == 0 || strcmp(argv[i+1], "true") == 0) {
276  isTransfoSaved = true;
277  }
278  else if (strcmp(argv[i+1], "0") == 0
279  || strcmp(argv[i+1],"false") == 0) {
280  isTransfoSaved = false;
281  }
282  else {
283  cerr << "Invalid value for parameter isTransfoSaved." << endl
284  << "Value must be true or false or 1 or 0." << endl
285  << "Default value will be used." << endl;
286  }
287  }
288  else if (opt == "--config") {
289  configFile = argv[i+1];
290  }
291  else if (opt == "--output") {
292  outputBaseFile = argv[i+1];
293  }
294  else if (opt == "--initTranslation") {
295  initTranslation = argv[i+1];
296  }
297  else if (opt == "--initRotation") {
298  initRotation = argv[i+1];
299  }
300  else
301  {
302  cerr << "Unknown option " << opt << ", usage:"; usage(argv); exit(1);
303  }
304  }
305  return 0;
306 }
307 
309  const int cloudDimension) {
310  PM::TransformationParameters parsedTranslation;
311  parsedTranslation = PM::TransformationParameters::Identity(
312  cloudDimension+1,cloudDimension+1);
313 
314  translation.erase(std::remove(translation.begin(), translation.end(), '['),
315  translation.end());
316  translation.erase(std::remove(translation.begin(), translation.end(), ']'),
317  translation.end());
318  std::replace( translation.begin(), translation.end(), ',', ' ');
319  std::replace( translation.begin(), translation.end(), ';', ' ');
320 
321  float translationValues[3] = {0};
322  stringstream translationStringStream(translation);
323  for( int i = 0; i < cloudDimension; i++) {
324  if(!(translationStringStream >> translationValues[i])) {
325  cerr << "An error occured while trying to parse the initial "
326  << "translation." << endl
327  << "No initial translation will be used" << endl;
328  return parsedTranslation;
329  }
330  }
331  float extraOutput = 0;
332  if((translationStringStream >> extraOutput)) {
333  cerr << "Wrong initial translation size" << endl
334  << "No initial translation will be used" << endl;
335  return parsedTranslation;
336  }
337 
338  for( int i = 0; i < cloudDimension; i++) {
339  parsedTranslation(i,cloudDimension) = translationValues[i];
340  }
341 
342  return parsedTranslation;
343 }
344 
346  const int cloudDimension){
347  PM::TransformationParameters parsedRotation;
348  parsedRotation = PM::TransformationParameters::Identity(
349  cloudDimension+1,cloudDimension+1);
350 
351  rotation.erase(std::remove(rotation.begin(), rotation.end(), '['),
352  rotation.end());
353  rotation.erase(std::remove(rotation.begin(), rotation.end(), ']'),
354  rotation.end());
355  std::replace( rotation.begin(), rotation.end(), ',', ' ');
356  std::replace( rotation.begin(), rotation.end(), ';', ' ');
357 
358  float rotationMatrix[9] = {0};
359  stringstream rotationStringStream(rotation);
360  for( int i = 0; i < cloudDimension*cloudDimension; i++) {
361  if(!(rotationStringStream >> rotationMatrix[i])) {
362  cerr << "An error occured while trying to parse the initial "
363  << "rotation." << endl
364  << "No initial rotation will be used" << endl;
365  return parsedRotation;
366  }
367  }
368  float extraOutput = 0;
369  if((rotationStringStream >> extraOutput)) {
370  cerr << "Wrong initial rotation size" << endl
371  << "No initial rotation will be used" << endl;
372  return parsedRotation;
373  }
374 
375  for( int i = 0; i < cloudDimension*cloudDimension; i++) {
376  parsedRotation(i/cloudDimension,i%cloudDimension) = rotationMatrix[i];
377  }
378 
379  return parsedRotation;
380 }
381 
382 // Dump command-line help
383 void usage(const char *argv[])
384 {
385  //TODO: add new options --isTransfoSaved, --initTranslation, --initRotation
386  cerr << endl << endl;
387  cerr << "* To list modules:" << endl;
388  cerr << " " << argv[0] << " -l" << endl;
389  cerr << endl;
390  cerr << "* To run ICP:" << endl;
391  cerr << " " << argv[0] << " [OPTIONS] reference.csv reading.csv" << endl;
392  cerr << endl;
393  cerr << "OPTIONS can be a combination of:" << endl;
394  cerr << "-v,--verbose Be more verbose (info logging to stdout)" << endl;
395  cerr << "--config YAML_CONFIG_FILE Load the config from a YAML file (default: default parameters)" << endl;
396  cerr << "--output BASEFILENAME Name of output files (default: test)" << endl;
397  cerr << "--initTranslation [x,y,z] Add an initial 3D translation before applying ICP (default: 0,0,0)" << endl;
398  cerr << "--initTranslation [x,y] Add an initial 2D translation before applying ICP (default: 0,0)" << endl;
399  cerr << "--initRotation [r00,r01,r02,r10,r11,r12,r20,r21,r22]" << endl;
400  cerr << " Add an initial 3D rotation before applying ICP (default: 1,0,0,0,1,0,0,0,1)" << endl;
401  cerr << "--initRotation [r00,r01,r10,r11]" << endl;
402  cerr << " Add an initial 2D rotation before applying ICP (default: 1,0,0,1)" << endl;
403  cerr << "--isTransfoSaved BOOL Save transformation matrix in three different files:" << endl;
404  cerr << " - BASEFILENAME_inti_transfo.txt" << endl;
405  cerr << " - BASEFILENAME_icp_transfo.txt" << endl;
406  cerr << " - BASEFILENAME_complete_transfo.txt" << endl;
407  cerr << " (default: false)" << endl;
408  cerr << endl;
409  cerr << "Running this program with a VTKFileInspector as Inspector will create three" << endl;
410  cerr << "vtk ouptput files: ./test_ref.vtk, ./test_data_in.vtk and ./test_data_out.vtk" << endl;
411  cerr << endl << "2D Example:" << endl;
412  cerr << " " << argv[0] << " ../examples/data/2D_twoBoxes.csv ../examples/data/2D_oneBox.csv" << endl;
413  cerr << endl << "3D Example:" << endl;
414  cerr << " " << argv[0] << " ../examples/data/car_cloud400.csv ../examples/data/car_cloud401.csv" << endl;
415  cerr << endl;
416 }
417 
418 
419 
PM::TransformationParameters parseTranslation(string &translation, const int cloudDimension)
void listModules()
public interface
int main(int argc, const char *argv[])
The logger interface, used to output warnings and informations.
Definition: PointMatcher.h:104
std::string getAndReplaceBibEntries(const std::string &text, CurrentBibliography &curBib)
PM::TransformationParameters parseRotation(string &rotation, const int cloudDimension)
void usage(const char *argv[])
data_out
Definition: icp.py:93
PM::DataPoints DataPoints
::std::string string
Definition: gtest.h:1979
rotation
Definition: icp.py:74
data
Definition: icp.py:50
PM::Inspector Inspector
PointMatcher< float > PM
PointMatcherSupport::CurrentBibliography CurrentBibliography
An error minimizer will compute a transformation matrix such as to minimize the error between the rea...
Definition: PointMatcher.h:530
A function that transforms points and their descriptors given a transformation matrix.
Definition: PointMatcher.h:404
Parametrizable::Parameters Parameters
alias
Definition: PointMatcher.h:186
ref
Definition: icp.py:49
static const PointMatcher & get()
Return instances.
Definition: Registry.cpp:145
void dump(std::ostream &os) const
translation
Definition: icp.py:73
PM::TransformationChecker TransformationChecker
PM::Parameters Parameters
int validateArgs(const int argc, const char *argv[], bool &isVerbose, bool &isTransfoSaved, string &configFile, string &outputBaseFile, string &initTranslation, string &initRotation)
void save(const std::string &fileName, bool binary=false) const
Save a point cloud to a file, determine format from extension.
PM::DataPoints DP
PM::ICP ICP
#define DUMP_REGISTRAR_CONTENT(pm, name, bib)
static DataPoints load(const std::string &fileName)
Load a point cloud from a file, determine format from extension.
PM::DataPointsFilter DataPointsFilter
unsigned getEuclideanDim() const
Return the dimension of the point cloud.
Matrix TransformationParameters
A matrix holding the parameters a transformation.
Definition: PointMatcher.h:182
PM::OutlierFilter OutlierFilter
void dumpRegistrar(const PM &pm, const R &registrar, const std::string &name, CurrentBibliography &bib)
PM::Matcher Matcher


libpointmatcher
Author(s):
autogenerated on Sat May 27 2023 02:38:02