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;
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 // std::shared_ptr<PM::Inspector> vtkInspect =
144 // PM::get().InspectorRegistrar.create(name, params);
145 // params.clear();
146 
147  // Prepare transformation
148  std::shared_ptr<PM::Transformation> rigidTrans =
149  PM::get().TransformationRegistrar.create("RigidTransformation");
150 
151  // Build ICP solution
152  icp.readingDataPointsFilters.push_back(minDist_read);
153  icp.readingDataPointsFilters.push_back(rand_read);
154 
155  icp.referenceDataPointsFilters.push_back(minDist_ref);
156  icp.referenceDataPointsFilters.push_back(rand_ref);
157 
158  icp.matcher = kdtree;
159 
160  icp.outlierFilters.push_back(trim);
161 
162  icp.errorMinimizer = pointToPoint;
163 
164  icp.transformationCheckers.push_back(maxIter);
165  icp.transformationCheckers.push_back(diff);
166 
167  // toggle to write vtk files per iteration
168  icp.inspector = nullInspect;
169  //icp.inspector = vtkInspect;
170 
171  icp.transformations.push_back(rigidTrans);
172 
173  // Compute the transformation to express data in ref
175 
176  // Transform data to express it in ref
177  DP data_out(data);
178  icp.transformations.apply(data_out, T);
179 
180  // Safe files to see the results
181  ref.save("test_ref.vtk");
182  data.save("test_data_in.vtk");
183  data_out.save("test_data_out.vtk");
184  cout << "Final transformation:" << endl << T << endl;
185 
186  return 0;
187 }
188 
189 void validateArgs(int argc, char *argv[], bool& isCSV )
190 {
191  if (argc != 3)
192  {
193  cerr << "Wrong number of arguments, usage " << argv[0] << " reference.csv reading.csv" << endl;
194  cerr << "Will create 3 vtk files for inspection: ./test_ref.vtk, ./test_data_in.vtk and ./test_data_out.vtk" << endl;
195  cerr << endl << "2D Example:" << endl;
196  cerr << " " << argv[0] << " ../../examples/data/2D_twoBoxes.csv ../../examples/data/2D_oneBox.csv" << endl;
197  cerr << endl << "3D Example:" << endl;
198  cerr << " " << argv[0] << " ../../examples/data/car_cloud400.csv ../../examples/data/car_cloud401.csv" << endl;
199  exit(1);
200  }
201 }
build_map.T
T
Definition: build_map.py:34
DataPoints
PM::DataPoints DataPoints
Definition: pypoint_matcher_helper.h:16
icp_customized.maxIter
maxIter
Definition: icp_customized.py:87
icp_customized.name
string name
Definition: icp_customized.py:45
icp_customized.rand_ref
rand_ref
Definition: icp_customized.py:63
main
int main(int argc, char *argv[])
Definition: icp_customized.cpp:54
icp_customized.trim
trim
Definition: icp_customized.py:76
PointMatcher< float >
PointMatcherSupport::setLogger
void setLogger(std::shared_ptr< Logger > newLogger)
Set a new logger, protected by a mutex.
Definition: Logger.cpp:98
align_sequence.icp
icp
Definition: align_sequence.py:40
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
testing::internal::string
::std::string string
Definition: gtest.h:1979
align_sequence.params
params
Definition: align_sequence.py:13
icp.data
data
Definition: icp.py:50
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
icp.data_out
data_out
Definition: icp.py:93
ICP
PM::ICP ICP
Definition: pypoint_matcher_helper.h:33
icp.ref
ref
Definition: icp.py:49
icp_customized.kdtree
kdtree
Definition: icp_customized.py:70
icp_customized.minDist_ref
minDist_ref
Definition: icp_customized.py:58
icp_customized.minDist_read
minDist_read
Definition: icp_customized.py:47
std
icp_customized.pointToPoint
pointToPoint
Definition: icp_customized.py:81
icp_customized.diff
diff
Definition: icp_customized.py:94
icp
Definition: icp.py:1
PointMatcher< float >::get
static const PointMatcher & get()
Return instances.
Definition: Registry.cpp:146
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
icp_customized.rand_read
rand_read
Definition: icp_customized.py:52
validateArgs
void validateArgs(int argc, char *argv[], bool &isCSV)
Definition: icp_customized.cpp:189
PointMatcherSupport::Parametrizable::Parameters
std::map< std::string, Parameter > Parameters
Parameters stored as a map of string->string.
Definition: Parametrizable.h:199
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
icp_customized.nullInspect
nullInspect
Definition: icp_customized.py:100


libpointmatcher
Author(s):
autogenerated on Mon Jul 1 2024 02:22:43