Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include "pointmatcher/PointMatcher.h"
00037 #include <cassert>
00038 #include <iostream>
00039 #include "boost/filesystem.hpp"
00040
00041 using namespace std;
00042
00043 void validateArgs(int argc, char *argv[], bool& isCSV);
00044
00054 int main(int argc, char *argv[])
00055 {
00056 bool isCSV = true;
00057 validateArgs(argc, argv, isCSV);
00058
00059 typedef PointMatcher<float> PM;
00060 typedef PM::DataPoints DP;
00061
00062
00063 const DP ref(DP::load(argv[1]));
00064 const DP data(DP::load(argv[2]));
00065
00066
00067 PM::ICP icp;
00068 PointMatcherSupport::Parametrizable::Parameters params;
00069 std::string name;
00070
00071
00072 setLogger(PM::get().LoggerRegistrar.create("FileLogger"));
00073
00074
00075 name = "MinDistDataPointsFilter";
00076 params["minDist"] = "1.0";
00077 std::shared_ptr<PM::DataPointsFilter> minDist_read =
00078 PM::get().DataPointsFilterRegistrar.create(name, params);
00079 params.clear();
00080
00081 name = "RandomSamplingDataPointsFilter";
00082 params["prob"] = "0.05";
00083 std::shared_ptr<PM::DataPointsFilter> rand_read =
00084 PM::get().DataPointsFilterRegistrar.create(name, params);
00085 params.clear();
00086
00087
00088 name = "MinDistDataPointsFilter";
00089 params["minDist"] = "1.0";
00090 std::shared_ptr<PM::DataPointsFilter> minDist_ref =
00091 PM::get().DataPointsFilterRegistrar.create(name, params);
00092 params.clear();
00093
00094 name = "RandomSamplingDataPointsFilter";
00095 params["prob"] = "0.05";
00096 std::shared_ptr<PM::DataPointsFilter> rand_ref =
00097 PM::get().DataPointsFilterRegistrar.create(name, params);
00098 params.clear();
00099
00100
00101 name = "KDTreeMatcher";
00102 params["knn"] = "1";
00103 params["epsilon"] = "3.16";
00104 std::shared_ptr<PM::Matcher> kdtree =
00105 PM::get().MatcherRegistrar.create(name, params);
00106 params.clear();
00107
00108
00109 name = "TrimmedDistOutlierFilter";
00110 params["ratio"] = "0.75";
00111 std::shared_ptr<PM::OutlierFilter> trim =
00112 PM::get().OutlierFilterRegistrar.create(name, params);
00113 params.clear();
00114
00115
00116 name = "PointToPointErrorMinimizer";
00117 std::shared_ptr<PM::ErrorMinimizer> pointToPoint =
00118 PM::get().ErrorMinimizerRegistrar.create(name);
00119
00120
00121 name = "CounterTransformationChecker";
00122 params["maxIterationCount"] = "150";
00123 std::shared_ptr<PM::TransformationChecker> maxIter =
00124 PM::get().TransformationCheckerRegistrar.create(name, params);
00125 params.clear();
00126
00127 name = "DifferentialTransformationChecker";
00128 params["minDiffRotErr"] = "0.001";
00129 params["minDiffTransErr"] = "0.01";
00130 params["smoothLength"] = "4";
00131 std::shared_ptr<PM::TransformationChecker> diff =
00132 PM::get().TransformationCheckerRegistrar.create(name, params);
00133 params.clear();
00134
00135
00136 std::shared_ptr<PM::Inspector> nullInspect =
00137 PM::get().InspectorRegistrar.create("NullInspector");
00138
00139
00140
00141
00142
00143
00144
00145
00146 params.clear();
00147
00148
00149 std::shared_ptr<PM::Transformation> rigidTrans =
00150 PM::get().TransformationRegistrar.create("RigidTransformation");
00151
00152
00153 icp.readingDataPointsFilters.push_back(minDist_read);
00154 icp.readingDataPointsFilters.push_back(rand_read);
00155
00156 icp.referenceDataPointsFilters.push_back(minDist_ref);
00157 icp.referenceDataPointsFilters.push_back(rand_ref);
00158
00159 icp.matcher = kdtree;
00160
00161 icp.outlierFilters.push_back(trim);
00162
00163 icp.errorMinimizer = pointToPoint;
00164
00165 icp.transformationCheckers.push_back(maxIter);
00166 icp.transformationCheckers.push_back(diff);
00167
00168
00169 icp.inspector = nullInspect;
00170
00171
00172 icp.transformations.push_back(rigidTrans);
00173
00174
00175 PM::TransformationParameters T = icp(data, ref);
00176
00177
00178 DP data_out(data);
00179 icp.transformations.apply(data_out, T);
00180
00181
00182 ref.save("test_ref.vtk");
00183 data.save("test_data_in.vtk");
00184 data_out.save("test_data_out.vtk");
00185 cout << "Final transformation:" << endl << T << endl;
00186
00187 return 0;
00188 }
00189
00190 void validateArgs(int argc, char *argv[], bool& isCSV )
00191 {
00192 if (argc != 3)
00193 {
00194 cerr << "Wrong number of arguments, usage " << argv[0] << " reference.csv reading.csv" << endl;
00195 cerr << "Will create 3 vtk files for inspection: ./test_ref.vtk, ./test_data_in.vtk and ./test_data_out.vtk" << endl;
00196 cerr << endl << "2D Example:" << endl;
00197 cerr << " " << argv[0] << " ../../examples/data/2D_twoBoxes.csv ../../examples/data/2D_oneBox.csv" << endl;
00198 cerr << endl << "3D Example:" << endl;
00199 cerr << " " << argv[0] << " ../../examples/data/car_cloud400.csv ../../examples/data/car_cloud401.csv" << endl;
00200 exit(1);
00201 }
00202 }