align_sequence.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 
45 
46 using namespace std;
47 using namespace PointMatcherSupport;
48 
49 void validateArgs(int argc, char *argv[]);
50 
57 int main(int argc, char *argv[])
58 {
59  validateArgs(argc, argv);
60 
61  typedef PointMatcher<float> PM;
64  typedef PM::DataPoints DP;
65 
66  string outputFileName(argv[0]);
67 
68  // Rigid transformation
69  std::shared_ptr<PM::Transformation> rigidTrans;
70  rigidTrans = PM::get().REG(Transformation).create("RigidTransformation");
71 
72  // Create filters manually to clean the global map
73  std::shared_ptr<PM::DataPointsFilter> densityFilter =
74  PM::get().DataPointsFilterRegistrar.create(
75  "SurfaceNormalDataPointsFilter",
76  {
77  {"knn", "10"},
78  {"epsilon", "5"},
79  {"keepNormals", "0"},
80  {"keepDensities", "1"}
81  }
82  );
83 
84  std::shared_ptr<PM::DataPointsFilter> maxDensitySubsample =
85  PM::get().DataPointsFilterRegistrar.create(
86  "MaxDensityDataPointsFilter",
87  {{"maxDensity", toParam(30)}}
88  );
89  // Main algorithm definition
90  PM::ICP icp;
91 
92  // load YAML config
93  ifstream ifs(argv[1]);
94  validateFile(argv[1]);
95  icp.loadFromYaml(ifs);
96 
97  PMIO::FileInfoVector list(argv[2]);
98 
99  PM::DataPoints mapPointCloud, newCloud;
100  TP T_to_map_from_new = TP::Identity(4,4); // assumes 3D
101 
102  for(unsigned i=0; i < list.size(); i++)
103  {
104  cout << "---------------------\nLoading: " << list[i].readingFileName << endl;
105 
106  // It is assume that the point cloud is express in sensor frame
107  newCloud = DP::load(list[i].readingFileName);
108 
109  if(mapPointCloud.getNbPoints() == 0)
110  {
111  mapPointCloud = newCloud;
112  continue;
113  }
114 
115  // call ICP
116  try
117  {
118  // We use the last transformation as a prior
119  // this assumes that the point clouds were recorded in
120  // sequence.
121  const TP prior = T_to_map_from_new;
122 
123  T_to_map_from_new = icp(newCloud, mapPointCloud, prior);
124  }
125  catch (PM::ConvergenceError& error)
126  {
127  cout << "ERROR PM::ICP failed to converge: " << endl;
128  cout << " " << error.what() << endl;
129  continue;
130  }
131 
132  // This is not necessary in this example, but could be
133  // useful if the same matrix is composed in the loop.
134  T_to_map_from_new = rigidTrans->correctParameters(T_to_map_from_new);
135 
136  // Move the new point cloud in the map reference
137  newCloud = rigidTrans->compute(newCloud, T_to_map_from_new);
138 
139  // Merge point clouds to map
140  mapPointCloud.concatenate(newCloud);
141 
142  // Clean the map
143  mapPointCloud = densityFilter->filter(mapPointCloud);
144  mapPointCloud = maxDensitySubsample->filter(mapPointCloud);
145 
146  // Save the map at each iteration
147  stringstream outputFileNameIter;
148  outputFileNameIter << outputFileName << "_" << i << ".vtk";
149 
150  cout << "outputFileName: " << outputFileNameIter.str() << endl;
151  mapPointCloud.save(outputFileNameIter.str());
152  }
153 
154  return 0;
155 }
156 
157 void validateArgs(int argc, char *argv[])
158 {
159  if (!(argc == 3))
160  {
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;
166 
167  abort();
168  }
169 }
170 
171 
validateArgs
void validateArgs(int argc, char *argv[])
Definition: align_sequence.cpp:157
PointMatcherIO
IO Functions and classes that are dependant on scalar type are defined in this templatized class.
Definition: IO.h:43
PointMatcherSupport::toParam
std::string toParam(const S &value)
Return the a string value using lexical_cast.
Definition: Parametrizable.h:92
PointMatcherSupport::validateFile
void validateFile(const std::string &fileName)
Throw a runtime_error exception if fileName cannot be opened.
Definition: pointmatcher/IO.cpp:355
PointMatcher< float >
main
int main(int argc, char *argv[])
Definition: align_sequence.cpp:57
PMIO
PointMatcherIO< float > PMIO
Definition: eval_solution.cpp:62
PointMatcherIO::FileInfoVector
A vector of file info, to be used in batch processing.
Definition: IO.h:245
std
TP
PM::TransformationParameters TP
Definition: eval_solution.cpp:63
PointMatcher< float >::get
static const PointMatcher & get()
Return instances.
Definition: Registry.cpp:141
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
PointMatcher< float >::TransformationParameters
Matrix TransformationParameters
A matrix holding the parameters a transformation.
Definition: PointMatcher.h:182


mp2p_icp
Author(s):
autogenerated on Wed Oct 23 2024 02:45:38