root / rgbdslam / gicp / test_gicp.cpp @ 9240aaa3
History | View | Annotate | Download (6.71 KB)
1 | 9240aaa3 | Alex | /*************************************************************
|
---|---|---|---|
2 | Generalized-ICP Copyright (c) 2009 Aleksandr Segal.
|
||
3 | All rights reserved.
|
||
4 | |||
5 | Redistribution and use in source and binary forms, with
|
||
6 | or without modification, are permitted provided that the
|
||
7 | following conditions are met:
|
||
8 | |||
9 | * Redistributions of source code must retain the above
|
||
10 | copyright notice, this list of conditions and the
|
||
11 | following disclaimer.
|
||
12 | * Redistributions in binary form must reproduce the above
|
||
13 | copyright notice, this list of conditions and the
|
||
14 | following disclaimer in the documentation and/or other
|
||
15 | materials provided with the distribution.
|
||
16 | * The names of the contributors may not be used to endorse
|
||
17 | or promote products derived from this software
|
||
18 | without specific prior written permission.
|
||
19 | |||
20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
|
||
21 | CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
|
||
22 | WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||
23 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
|
||
24 | PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||
25 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
|
||
26 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||
27 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
||
28 | PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
||
29 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||
30 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||
31 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
|
||
32 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||
33 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
|
||
34 | DAMAGE.
|
||
35 | *************************************************************/
|
||
36 | |||
37 | |||
38 | |||
39 | #include <iostream> |
||
40 | #include <fstream> |
||
41 | #include <string> |
||
42 | #include <sstream> |
||
43 | |||
44 | // program options
|
||
45 | #include <boost/program_options.hpp> |
||
46 | #include <boost/filesystem/path.hpp> |
||
47 | #include <boost/filesystem/fstream.hpp> |
||
48 | #include <boost/tokenizer.hpp> |
||
49 | |||
50 | #include "gicp.h" |
||
51 | |||
52 | using namespace std; |
||
53 | using namespace dgc::gicp; |
||
54 | namespace po = boost::program_options;
|
||
55 | |||
56 | |||
57 | static string filename1; |
||
58 | static string filename2; |
||
59 | static string filename_t_base; |
||
60 | static bool debug = true; |
||
61 | static double gicp_epsilon = 1e-3; |
||
62 | static double max_distance = 0.1; // in m |
||
63 | |||
64 | |||
65 | void print_usage(const char* program_name, po::options_description const& desc) { |
||
66 | cout << program_name << " [options] scan1 scan2 [t_base]" << endl;
|
||
67 | cout << desc << endl; |
||
68 | } |
||
69 | |||
70 | |||
71 | bool parse_options(int argc, char** argv) { |
||
72 | bool error = false; |
||
73 | po::options_description desc("Allowed options");
|
||
74 | desc.add_options() |
||
75 | ("help", "print help message") |
||
76 | ("scan1", po::value<string>(), "scan1 filename") |
||
77 | ("scan2", po::value<string>(), "scan2 filename") |
||
78 | ("t_base", po::value<string>(), "base transform filename") |
||
79 | ("debug", po::value<bool>(), "enable debug mode") |
||
80 | ("epsilon", po::value<double>(), "G-ICP epsilon constant") |
||
81 | ("d_max", po::value<double>(), "maximum distance for matching points"); |
||
82 | |||
83 | po::positional_options_description p; |
||
84 | p.add("scan1", 1); |
||
85 | p.add("scan2", 1); |
||
86 | p.add("t_base", 1); |
||
87 | |||
88 | po::variables_map vm; |
||
89 | try {
|
||
90 | po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm); |
||
91 | po::notify(vm); |
||
92 | } |
||
93 | catch(const std::exception &e) { |
||
94 | cout << "Error: " << e.what() << endl;
|
||
95 | print_usage(argv[0], desc);
|
||
96 | error = true;
|
||
97 | return error;
|
||
98 | } |
||
99 | |||
100 | if(vm.count("help")) { |
||
101 | print_usage(argv[0], desc);
|
||
102 | error = true;
|
||
103 | return error;
|
||
104 | } |
||
105 | |||
106 | if(!vm.count("scan1")) { |
||
107 | cout << "Error: scan1 filename not specified!" << endl;
|
||
108 | error = true;
|
||
109 | } |
||
110 | else {
|
||
111 | filename1 = vm["scan1"].as<string>(); |
||
112 | } |
||
113 | |||
114 | if(!vm.count("scan2")) { |
||
115 | cout << "Error: scan2 filename not specified!" << endl;
|
||
116 | error = true;
|
||
117 | } |
||
118 | else {
|
||
119 | filename2 = vm["scan2"].as<string>(); |
||
120 | } |
||
121 | |||
122 | if(error) {
|
||
123 | print_usage(argv[0], desc);
|
||
124 | } |
||
125 | |||
126 | |||
127 | if(vm.count("debug")) { |
||
128 | debug = vm["debug"].as<bool>(); |
||
129 | } |
||
130 | |||
131 | if(vm.count("epsilon")) { |
||
132 | gicp_epsilon = vm["epsilon"].as<double>(); |
||
133 | } |
||
134 | |||
135 | if(vm.count("d_max")) { |
||
136 | max_distance = vm["d_max"].as<double>(); |
||
137 | } |
||
138 | |||
139 | if(vm.count("t_base")) { |
||
140 | filename_t_base = vm["t_base"].as<string>(); |
||
141 | } |
||
142 | |||
143 | return error;
|
||
144 | } |
||
145 | |||
146 | bool load_points(GICPPointSet *set, const char* filename) { |
||
147 | bool error = false; |
||
148 | |||
149 | ifstream in(filename); |
||
150 | if(!in) {
|
||
151 | cout << "Could not open '" << filename << "'." << endl; |
||
152 | return true; |
||
153 | } |
||
154 | string line;
|
||
155 | GICPPoint pt; |
||
156 | pt.range = -1;
|
||
157 | for(int k = 0; k < 3; k++) { |
||
158 | for(int l = 0; l < 3; l++) { |
||
159 | pt.C[k][l] = (k == l)?1:0; |
||
160 | } |
||
161 | } |
||
162 | while(getline(in, line)) {
|
||
163 | istringstream sin(line); |
||
164 | sin >> pt.x >> pt.y >> pt.z; |
||
165 | set->AppendPoint(pt); |
||
166 | } |
||
167 | in.close(); |
||
168 | |||
169 | return false; |
||
170 | }; |
||
171 | |||
172 | int main(int argc, char** argv) { |
||
173 | // cout << "Test program for the gicp library." << endl;
|
||
174 | |||
175 | bool error = parse_options(argc, argv);
|
||
176 | |||
177 | if(error) {
|
||
178 | return 1; |
||
179 | } |
||
180 | |||
181 | GICPPointSet p1, p2; |
||
182 | dgc_transform_t t_base, t0, t1; |
||
183 | |||
184 | // set up the transformations
|
||
185 | dgc_transform_identity(t_base); |
||
186 | dgc_transform_identity(t0); |
||
187 | dgc_transform_identity(t1); |
||
188 | |||
189 | // read base transform from file if one is specified
|
||
190 | if(!filename_t_base.empty()) {
|
||
191 | int status = dgc_transform_read(t_base, filename_t_base.c_str());
|
||
192 | if(status != 0) { |
||
193 | return 1; |
||
194 | } |
||
195 | } |
||
196 | |||
197 | // read points clouds
|
||
198 | cout << "Setting up pointclouds..." << endl;
|
||
199 | error = load_points(&p1, filename1.c_str()); |
||
200 | if(error) {
|
||
201 | return 1; |
||
202 | } |
||
203 | cout << "Loaded " << p1.Size() << " points into GICPPointSet 1." << endl; |
||
204 | error = load_points(&p2, filename2.c_str()); |
||
205 | if(error) {
|
||
206 | return 1; |
||
207 | } |
||
208 | cout << "Loaded " << p2.Size() << " points into GICPPointSet 2." << endl; |
||
209 | |||
210 | // build kdtrees and normal matrices
|
||
211 | cout << "Building KDTree and computing surface normals/matrices..." << endl;
|
||
212 | cout << "gicp_epsilon: " << gicp_epsilon << endl;
|
||
213 | cout << "max dist: " << max_distance << endl;
|
||
214 | |||
215 | p1.SetGICPEpsilon(gicp_epsilon); |
||
216 | p2.SetGICPEpsilon(gicp_epsilon); |
||
217 | p1.BuildKDTree(); |
||
218 | p1.ComputeMatrices(); |
||
219 | p2.BuildKDTree(); |
||
220 | p2.ComputeMatrices(); |
||
221 | |||
222 | if(debug) {
|
||
223 | // save data for debug/visualizations
|
||
224 | p1.SavePoints("pts1.dat");
|
||
225 | p1.SaveMatrices("mats1.dat");
|
||
226 | p2.SavePoints("pts2.dat");
|
||
227 | p2.SaveMatrices("mats2.dat");
|
||
228 | } |
||
229 | |||
230 | // align the point clouds
|
||
231 | cout << "Aligning point cloud..." << endl;
|
||
232 | dgc_transform_copy(t1, t0); |
||
233 | p2.SetDebug(debug); |
||
234 | p2.SetMaxIterationInner(8);
|
||
235 | p2.SetMaxIteration(200);
|
||
236 | int iterations = p2.AlignScan(&p1, t_base, t1, max_distance);
|
||
237 | |||
238 | // print the result
|
||
239 | cout << "Converged: " << endl;
|
||
240 | dgc_transform_print(t_base, "t_base");
|
||
241 | dgc_transform_print(t0, "t0");
|
||
242 | dgc_transform_print(t1, "t1");
|
||
243 | |||
244 | cout << "Converged in " << iterations << " iterations." << endl; |
||
245 | |||
246 | |||
247 | if(debug) {
|
||
248 | ofstream fout("iterations.txt");
|
||
249 | if(!fout) {
|
||
250 | return 0; |
||
251 | } |
||
252 | fout << "Converged in " << iterations << " iterations." << endl; |
||
253 | fout.close(); |
||
254 | } |
||
255 | |||
256 | return 0; |
||
257 | } |