Statistics
| Branch: | Revision:

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
}