root / rgbdslam / gicp / test_gicp.cpp @ 9240aaa3
History | View | Annotate | Download (6.71 KB)
1 |
/*************************************************************
|
---|---|
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 |
} |