icp_customized.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 <cassert>
38 #include <iostream>
39 #include "boost/filesystem.hpp"
40 
41 using namespace std;
42 
43 void validateArgs(int argc, char *argv[], bool& isCSV);
44 
54 int main(int argc, char *argv[])
55 {
56  bool isCSV = true;
57  validateArgs(argc, argv, isCSV);
58 
59  typedef PointMatcher<float> PM;
60  typedef PM::DataPoints DP;
61 
62  // Load point clouds
63  const DP ref(DP::load(argv[1]));
64  const DP data(DP::load(argv[2]));
65 
66  // Create the default ICP algorithm
67  PM::ICP icp;
69  std::string name;
70 
71  // Uncomment for console outputs
72  setLogger(PM::get().LoggerRegistrar.create("FileLogger"));
73 
74  // Prepare reading filters
75  name = "MinDistDataPointsFilter";
76  params["minDist"] = "1.0";
77  std::shared_ptr<PM::DataPointsFilter> minDist_read =
78  PM::get().DataPointsFilterRegistrar.create(name, params);
79  params.clear();
80 
81  name = "RandomSamplingDataPointsFilter";
82  params["prob"] = "0.05";
83  std::shared_ptr<PM::DataPointsFilter> rand_read =
84  PM::get().DataPointsFilterRegistrar.create(name, params);
85  params.clear();
86 
87  // Prepare reference filters
88  name = "MinDistDataPointsFilter";
89  params["minDist"] = "1.0";
90  std::shared_ptr<PM::DataPointsFilter> minDist_ref =
91  PM::get().DataPointsFilterRegistrar.create(name, params);
92  params.clear();
93 
94  name = "RandomSamplingDataPointsFilter";
95  params["prob"] = "0.05";
96  std::shared_ptr<PM::DataPointsFilter> rand_ref =
97  PM::get().DataPointsFilterRegistrar.create(name, params);
98  params.clear();
99 
100  // Prepare matching function
101  name = "KDTreeMatcher";
102  params["knn"] = "1";
103  params["epsilon"] = "3.16";
104  std::shared_ptr<PM::Matcher> kdtree =
105  PM::get().MatcherRegistrar.create(name, params);
106  params.clear();
107 
108  // Prepare outlier filters
109  name = "TrimmedDistOutlierFilter";
110  params["ratio"] = "0.75";
111  std::shared_ptr<PM::OutlierFilter> trim =
112  PM::get().OutlierFilterRegistrar.create(name, params);
113  params.clear();
114 
115  // Prepare error minimization
116  name = "PointToPointErrorMinimizer";
117  std::shared_ptr<PM::ErrorMinimizer> pointToPoint =
118  PM::get().ErrorMinimizerRegistrar.create(name);
119 
120  // Prepare transformation checker filters
121  name = "CounterTransformationChecker";
122  params["maxIterationCount"] = "150";
123  std::shared_ptr<PM::TransformationChecker> maxIter =
124  PM::get().TransformationCheckerRegistrar.create(name, params);
125  params.clear();
126 
127  name = "DifferentialTransformationChecker";
128  params["minDiffRotErr"] = "0.001";
129  params["minDiffTransErr"] = "0.01";
130  params["smoothLength"] = "4";
131  std::shared_ptr<PM::TransformationChecker> diff =
132  PM::get().TransformationCheckerRegistrar.create(name, params);
133  params.clear();
134 
135  // Prepare inspector
136  std::shared_ptr<PM::Inspector> nullInspect =
137  PM::get().InspectorRegistrar.create("NullInspector");
138 
139  //name = "VTKFileInspector";
140  // params["dumpDataLinks"] = "1";
141  // params["dumpReading"] = "1";
142  // params["dumpReference"] = "1";
143 
144  //PM::Inspector* vtkInspect =
145  // PM::get().InspectorRegistrar.create(name, params);
146  params.clear();
147 
148  // Prepare transformation
149  std::shared_ptr<PM::Transformation> rigidTrans =
150  PM::get().TransformationRegistrar.create("RigidTransformation");
151 
152  // Build ICP solution
153  icp.readingDataPointsFilters.push_back(minDist_read);
154  icp.readingDataPointsFilters.push_back(rand_read);
155 
156  icp.referenceDataPointsFilters.push_back(minDist_ref);
157  icp.referenceDataPointsFilters.push_back(rand_ref);
158 
159  icp.matcher = kdtree;
160 
161  icp.outlierFilters.push_back(trim);
162 
163  icp.errorMinimizer = pointToPoint;
164 
165  icp.transformationCheckers.push_back(maxIter);
166  icp.transformationCheckers.push_back(diff);
167 
168  // toggle to write vtk files per iteration
169  icp.inspector = nullInspect;
170  //icp.inspector = vtkInspect;
171 
172  icp.transformations.push_back(rigidTrans);
173 
174  // Compute the transformation to express data in ref
175  PM::TransformationParameters T = icp(data, ref);
176 
177  // Transform data to express it in ref
178  DP data_out(data);
179  icp.transformations.apply(data_out, T);
180 
181  // Safe files to see the results
182  ref.save("test_ref.vtk");
183  data.save("test_data_in.vtk");
184  data_out.save("test_data_out.vtk");
185  cout << "Final transformation:" << endl << T << endl;
186 
187  return 0;
188 }
189 
190 void validateArgs(int argc, char *argv[], bool& isCSV )
191 {
192  if (argc != 3)
193  {
194  cerr << "Wrong number of arguments, usage " << argv[0] << " reference.csv reading.csv" << endl;
195  cerr << "Will create 3 vtk files for inspection: ./test_ref.vtk, ./test_data_in.vtk and ./test_data_out.vtk" << endl;
196  cerr << endl << "2D Example:" << endl;
197  cerr << " " << argv[0] << " ../../examples/data/2D_twoBoxes.csv ../../examples/data/2D_oneBox.csv" << endl;
198  cerr << endl << "3D Example:" << endl;
199  cerr << " " << argv[0] << " ../../examples/data/car_cloud400.csv ../../examples/data/car_cloud401.csv" << endl;
200  exit(1);
201  }
202 }
main
int main(int argc, char *argv[])
Definition: icp_customized.cpp:54
PointMatcher< float >
PointMatcherSupport::setLogger
void setLogger(std::shared_ptr< Logger > newLogger)
Set a new logger, protected by a mutex.
Definition: Logger.cpp:98
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
testing::internal::string
::std::string string
Definition: gtest.h:1979
std
PointMatcher< float >::get
static const PointMatcher & get()
Return instances.
Definition: Registry.cpp:141
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
validateArgs
void validateArgs(int argc, char *argv[], bool &isCSV)
Definition: icp_customized.cpp:190
PointMatcher::DataPoints::save
void save(const std::string &fileName, bool binary=false) const
Save a point cloud to a file, determine format from extension.
Definition: pointmatcher/IO.cpp:809
PointMatcherSupport::Parametrizable::Parameters
std::map< std::string, Parameter > Parameters
Parameters stored as a map of string->string.
Definition: Parametrizable.h:156
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:40