root / rgbdslam / gicp / gicp.cpp @ 9240aaa3
History | View | Annotate | Download (12 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 | #include "gicp.h" |
||
38 | #include "optimize.h" |
||
39 | #include <gsl/gsl_linalg.h> |
||
40 | #include <gsl/gsl_blas.h> |
||
41 | #include <fstream> |
||
42 | |||
43 | #include <iostream> //TODO: remove |
||
44 | #include <sstream> |
||
45 | #include <pthread.h> |
||
46 | |||
47 | |||
48 | using namespace std;//TODO: remove |
||
49 | |||
50 | namespace dgc {
|
||
51 | namespace gicp {
|
||
52 | |||
53 | GICPPointSet::GICPPointSet() |
||
54 | { |
||
55 | kdtree_points_ = NULL;
|
||
56 | kdtree_ = NULL;
|
||
57 | max_iteration_ = 200; // default value |
||
58 | max_iteration_inner_ = 20; // default value for inner loop |
||
59 | epsilon_ = 1e-4; // correspondes to ~1 mm (tolerence for convergence of GICP outer loop) |
||
60 | epsilon_rot_ = 2e-4; |
||
61 | // epsilon_ = 5e-4; // correspondes to ~1 mm (tolerence for convergence of GICP outer loop)
|
||
62 | // epsilon_rot_ = 2e-3;
|
||
63 | gicp_epsilon_ = .0004; // epsilon constant for gicp paper; this is NOT the convergence tolerence |
||
64 | debug_ = false;
|
||
65 | solve_rotation_ = true;
|
||
66 | matrices_done_ = false;
|
||
67 | kdtree_done_ = false;
|
||
68 | pthread_mutex_init(&mutex_, NULL);
|
||
69 | } |
||
70 | |||
71 | GICPPointSet::~GICPPointSet() |
||
72 | { |
||
73 | if (kdtree_ != NULL) |
||
74 | delete kdtree_;
|
||
75 | if (kdtree_points_ != NULL) |
||
76 | annDeallocPts(kdtree_points_); |
||
77 | } |
||
78 | |||
79 | void GICPPointSet::Clear(void) { |
||
80 | pthread_mutex_lock(&mutex_); |
||
81 | matrices_done_ = false;
|
||
82 | kdtree_done_ = false;
|
||
83 | if (kdtree_ != NULL) { |
||
84 | delete kdtree_;
|
||
85 | kdtree_ = NULL;
|
||
86 | } |
||
87 | if (kdtree_points_ != NULL) { |
||
88 | annDeallocPts(kdtree_points_); |
||
89 | kdtree_points_ = NULL;
|
||
90 | } |
||
91 | point_.clear(); |
||
92 | pthread_mutex_unlock(&mutex_); |
||
93 | |||
94 | } |
||
95 | |||
96 | void GICPPointSet::BuildKDTree(void) |
||
97 | { |
||
98 | pthread_mutex_lock(&mutex_); |
||
99 | if(kdtree_done_) {
|
||
100 | return;
|
||
101 | } |
||
102 | kdtree_done_ = true;
|
||
103 | pthread_mutex_unlock(&mutex_); |
||
104 | |||
105 | int i, n = NumPoints();
|
||
106 | |||
107 | if(n == 0) { |
||
108 | return;
|
||
109 | } |
||
110 | |||
111 | kdtree_points_ = annAllocPts(n, 3);
|
||
112 | for(i = 0; i < n; i++) { |
||
113 | kdtree_points_[i][0] = point_[i].x;
|
||
114 | kdtree_points_[i][1] = point_[i].y;
|
||
115 | kdtree_points_[i][2] = point_[i].z;
|
||
116 | } |
||
117 | kdtree_ = new ANNkd_tree(kdtree_points_, n, 3, 10); |
||
118 | } |
||
119 | |||
120 | void GICPPointSet::ComputeMatrices() {
|
||
121 | pthread_mutex_lock(&mutex_); |
||
122 | if(kdtree_ == NULL) { |
||
123 | return;
|
||
124 | } |
||
125 | if(matrices_done_) {
|
||
126 | return;
|
||
127 | } |
||
128 | matrices_done_ = true;
|
||
129 | pthread_mutex_unlock(&mutex_); |
||
130 | |||
131 | int N = NumPoints();
|
||
132 | int K = 20; // number of closest points to use for local covariance estimate |
||
133 | double mean[3]; |
||
134 | |||
135 | ANNpoint query_point = annAllocPt(3);
|
||
136 | |||
137 | ANNdist *nn_dist_sq = new ANNdist[K];
|
||
138 | if(nn_dist_sq == NULL) { |
||
139 | //TODO: handle this
|
||
140 | } |
||
141 | ANNidx *nn_indecies = new ANNidx[K];
|
||
142 | if(nn_indecies == NULL) { |
||
143 | //TODO: handle this
|
||
144 | } |
||
145 | gsl_vector *work = gsl_vector_alloc(3);
|
||
146 | if(work == NULL) { |
||
147 | //TODO: handle
|
||
148 | } |
||
149 | gsl_vector *gsl_singulars = gsl_vector_alloc(3);
|
||
150 | if(gsl_singulars == NULL) { |
||
151 | //TODO: handle
|
||
152 | } |
||
153 | gsl_matrix *gsl_v_mat = gsl_matrix_alloc(3, 3); |
||
154 | if(gsl_v_mat == NULL) { |
||
155 | //TODO: handle
|
||
156 | } |
||
157 | |||
158 | for(int i = 0; i < N; i++) { |
||
159 | query_point[0] = point_[i].x;
|
||
160 | query_point[1] = point_[i].y;
|
||
161 | query_point[2] = point_[i].z;
|
||
162 | |||
163 | gicp_mat_t &cov = point_[i].C; |
||
164 | // zero out the cov and mean
|
||
165 | for(int k = 0; k < 3; k++) { |
||
166 | mean[k] = 0.;
|
||
167 | for(int l = 0; l < 3; l++) { |
||
168 | cov[k][l] = 0.;
|
||
169 | } |
||
170 | } |
||
171 | |||
172 | kdtree_->annkSearch(query_point, K, nn_indecies, nn_dist_sq, 0.0); |
||
173 | |||
174 | // find the covariance matrix
|
||
175 | for(int j = 0; j < K; j++) { |
||
176 | GICPPoint &pt = point_[nn_indecies[j]]; |
||
177 | |||
178 | mean[0] += pt.x;
|
||
179 | mean[1] += pt.y;
|
||
180 | mean[2] += pt.z;
|
||
181 | |||
182 | cov[0][0] += pt.x*pt.x; |
||
183 | |||
184 | cov[1][0] += pt.y*pt.x; |
||
185 | cov[1][1] += pt.y*pt.y; |
||
186 | |||
187 | cov[2][0] += pt.z*pt.x; |
||
188 | cov[2][1] += pt.z*pt.y; |
||
189 | cov[2][2] += pt.z*pt.z; |
||
190 | } |
||
191 | |||
192 | mean[0] /= (double)K; |
||
193 | mean[1] /= (double)K; |
||
194 | mean[2] /= (double)K; |
||
195 | // get the actual covariance
|
||
196 | for(int k = 0; k < 3; k++) { |
||
197 | for(int l = 0; l <= k; l++) { |
||
198 | cov[k][l] /= (double)K;
|
||
199 | cov[k][l] -= mean[k]*mean[l]; |
||
200 | cov[l][k] = cov[k][l]; |
||
201 | } |
||
202 | } |
||
203 | |||
204 | // compute the SVD
|
||
205 | gsl_matrix_view gsl_cov = gsl_matrix_view_array(&cov[0][0], 3, 3); |
||
206 | gsl_linalg_SV_decomp(&gsl_cov.matrix, gsl_v_mat, gsl_singulars, work); |
||
207 | |||
208 | // zero out the cov matrix, since we know U = V since C is symmetric
|
||
209 | for(int k = 0; k < 3; k++) { |
||
210 | for(int l = 0; l < 3; l++) { |
||
211 | cov[k][l] = 0;
|
||
212 | } |
||
213 | } |
||
214 | |||
215 | // reconstitute the covariance matrix with modified singular values using the column vectors in V.
|
||
216 | for(int k = 0; k < 3; k++) { |
||
217 | gsl_vector_view col = gsl_matrix_column(gsl_v_mat, k); |
||
218 | |||
219 | double v = 1.; // biggest 2 singular values replaced by 1 |
||
220 | if(k == 2) { // smallest singular value replaced by gicp_epsilon |
||
221 | v = gicp_epsilon_; |
||
222 | } |
||
223 | |||
224 | gsl_blas_dger(v, &col.vector, &col.vector, &gsl_cov.matrix); |
||
225 | } |
||
226 | } |
||
227 | |||
228 | if(nn_dist_sq != NULL) { |
||
229 | delete [] nn_dist_sq;
|
||
230 | } |
||
231 | if(nn_indecies != NULL) { |
||
232 | delete [] nn_indecies;
|
||
233 | } |
||
234 | if(work != NULL) { |
||
235 | gsl_vector_free(work); |
||
236 | } |
||
237 | if(gsl_v_mat != NULL) { |
||
238 | gsl_matrix_free(gsl_v_mat); |
||
239 | } |
||
240 | if(gsl_singulars != NULL) { |
||
241 | gsl_vector_free(gsl_singulars); |
||
242 | } |
||
243 | query_point; |
||
244 | } |
||
245 | |||
246 | int GICPPointSet::AlignScan(GICPPointSet *scan, dgc_transform_t base_t, dgc_transform_t t, double max_match_dist, bool save_error_plot) |
||
247 | { |
||
248 | double max_d_sq = pow(max_match_dist, 2); |
||
249 | int num_matches = 0; |
||
250 | int n = scan->NumPoints();
|
||
251 | double delta = 0.; |
||
252 | dgc_transform_t t_last; |
||
253 | ofstream fout_corresp; |
||
254 | ANNdist nn_dist_sq; |
||
255 | ANNidx *nn_indecies = new ANNidx[n];
|
||
256 | ANNpoint query_point = annAllocPt(3);
|
||
257 | |||
258 | if(nn_indecies == NULL) { |
||
259 | //TODO: fail here
|
||
260 | } |
||
261 | |||
262 | gicp_mat_t *mahalanobis = new gicp_mat_t[n];
|
||
263 | if(mahalanobis == NULL) { |
||
264 | //TODO: fail here
|
||
265 | } |
||
266 | gsl_matrix *gsl_R = gsl_matrix_alloc(3, 3); |
||
267 | if(gsl_R == NULL) { |
||
268 | //TODO: fail here
|
||
269 | } |
||
270 | gsl_matrix *gsl_temp = gsl_matrix_alloc(3, 3); |
||
271 | if(gsl_temp == NULL) { |
||
272 | //TODO: fail here
|
||
273 | } |
||
274 | |||
275 | bool converged = false; |
||
276 | int iteration = 0; |
||
277 | bool opt_status = false; |
||
278 | |||
279 | |||
280 | /* set up the optimization parameters */
|
||
281 | GICPOptData opt_data; |
||
282 | opt_data.nn_indecies = nn_indecies; |
||
283 | opt_data.p1 = scan; |
||
284 | opt_data.p2 = this;
|
||
285 | opt_data.M = mahalanobis; |
||
286 | opt_data.solve_rotation = solve_rotation_; |
||
287 | dgc_transform_copy(opt_data.base_t, base_t); |
||
288 | |||
289 | GICPOptimizer opt; |
||
290 | opt.SetDebug(debug_); |
||
291 | opt.SetMaxIterations(max_iteration_inner_); |
||
292 | /* set up the mahalanobis matricies */
|
||
293 | /* these are identity for now to ease debugging */
|
||
294 | for(int i = 0; i < n; i++) { |
||
295 | for(int k = 0; k < 3; k++) { |
||
296 | for(int l = 0; l < 3; l++) { |
||
297 | mahalanobis[i][k][l] = (k == l)?1:0.; |
||
298 | } |
||
299 | } |
||
300 | } |
||
301 | |||
302 | if(debug_) {
|
||
303 | dgc_transform_write(base_t, "t_base.tfm");
|
||
304 | dgc_transform_write(t, "t_0.tfm");
|
||
305 | } |
||
306 | |||
307 | while(!converged) {
|
||
308 | dgc_transform_t transform_R; |
||
309 | dgc_transform_copy(transform_R, base_t); |
||
310 | dgc_transform_left_multiply(transform_R, t); |
||
311 | // copy the rotation component of the current total transformation (including base), into a gsl matrix
|
||
312 | for(int i = 0; i < 3; i++) { |
||
313 | for(int j = 0; j < 3; j++) { |
||
314 | gsl_matrix_set(gsl_R, i, j, transform_R[i][j]); |
||
315 | } |
||
316 | } |
||
317 | if(debug_) {
|
||
318 | fout_corresp.open("correspondence.txt");
|
||
319 | } |
||
320 | /* find correpondences */
|
||
321 | num_matches = 0;
|
||
322 | for (int i = 0; i < n; i++) { |
||
323 | query_point[0] = scan->point_[i].x;
|
||
324 | query_point[1] = scan->point_[i].y;
|
||
325 | query_point[2] = scan->point_[i].z;
|
||
326 | |||
327 | dgc_transform_point(&query_point[0], &query_point[1], |
||
328 | &query_point[2], base_t);
|
||
329 | dgc_transform_point(&query_point[0], &query_point[1], |
||
330 | &query_point[2], t);
|
||
331 | |||
332 | kdtree_->annkSearch(query_point, 1, &nn_indecies[i], &nn_dist_sq, 0.0); |
||
333 | |||
334 | if (nn_dist_sq < max_d_sq) {
|
||
335 | if(debug_) {
|
||
336 | fout_corresp << i << "\t" << nn_indecies[i] << endl;
|
||
337 | } |
||
338 | |||
339 | // set up the updated mahalanobis matrix here
|
||
340 | gsl_matrix_view C1 = gsl_matrix_view_array(&scan->point_[i].C[0][0], 3, 3); |
||
341 | gsl_matrix_view C2 = gsl_matrix_view_array(&point_[nn_indecies[i]].C[0][0], 3, 3); |
||
342 | gsl_matrix_view M = gsl_matrix_view_array(&mahalanobis[i][0][0], 3, 3); |
||
343 | gsl_matrix_set_zero(&M.matrix); |
||
344 | gsl_matrix_set_zero(gsl_temp); |
||
345 | |||
346 | // M = R*C1 // using M as a temp variable here
|
||
347 | gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1., gsl_R, &C1.matrix, 1., &M.matrix); |
||
348 | |||
349 | // temp = M*R' // move the temp value to 'temp' here
|
||
350 | gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1., &M.matrix, gsl_R, 0., gsl_temp); |
||
351 | |||
352 | // temp += C2
|
||
353 | gsl_matrix_add(gsl_temp, &C2.matrix); |
||
354 | // at this point temp = C2 + R*C1*R'
|
||
355 | |||
356 | // now invert temp to get the mahalanobis distance metric for gicp
|
||
357 | // M = temp^-1
|
||
358 | gsl_matrix_set_identity(&M.matrix); |
||
359 | gsl_linalg_cholesky_decomp(gsl_temp); |
||
360 | for(int k = 0; k < 3; k++) { |
||
361 | gsl_linalg_cholesky_svx(gsl_temp, &gsl_matrix_row(&M.matrix, k).vector); |
||
362 | } |
||
363 | num_matches++; |
||
364 | } |
||
365 | else {
|
||
366 | nn_indecies[i] = -1; // no match |
||
367 | } |
||
368 | } |
||
369 | |||
370 | if(debug_) { // save the current M matrices to file for debugging |
||
371 | ofstream out("mahalanobis.txt");
|
||
372 | if(out) {
|
||
373 | for(int i = 0; i < n; i++) { |
||
374 | for(int k = 0; k < 3; k++) { |
||
375 | for(int l = 0; l < 3; l++) { |
||
376 | out << mahalanobis[i][k][l] << "\t";
|
||
377 | } |
||
378 | } |
||
379 | out << endl; |
||
380 | } |
||
381 | } |
||
382 | out.close(); |
||
383 | } |
||
384 | |||
385 | if(debug_) {
|
||
386 | fout_corresp.close(); |
||
387 | } |
||
388 | opt_data.num_matches = num_matches; |
||
389 | |||
390 | /* optimize transformation using the current assignment and Mahalanobis metrics*/
|
||
391 | dgc_transform_copy(t_last, t); |
||
392 | opt_status = opt.Optimize(t, opt_data); |
||
393 | |||
394 | if(debug_) {
|
||
395 | cout << "Optimizer converged in " << opt.Iterations() << " iterations." << endl; |
||
396 | cout << "Status: " << opt.Status() << endl;
|
||
397 | |||
398 | std::ostringstream filename; |
||
399 | filename << "t_" << iteration+1 << ".tfm"; |
||
400 | dgc_transform_write(t, filename.str().c_str()); |
||
401 | } |
||
402 | |||
403 | /* compute the delta from this iteration */
|
||
404 | delta = 0.;
|
||
405 | for(int k = 0; k < 4; k++) { |
||
406 | for(int l = 0; l < 4; l++) { |
||
407 | double ratio = 1; |
||
408 | if(k < 3 && l < 3) { // rotation part of the transform |
||
409 | ratio = 1./epsilon_rot_;
|
||
410 | } |
||
411 | else {
|
||
412 | ratio = 1./epsilon_;
|
||
413 | } |
||
414 | double c_delta = ratio*fabs(t_last[k][l] - t[k][l]);
|
||
415 | |||
416 | if(c_delta > delta) {
|
||
417 | delta = c_delta; |
||
418 | } |
||
419 | } |
||
420 | } |
||
421 | if(debug_) {
|
||
422 | cout << "delta = " << delta << endl;
|
||
423 | } |
||
424 | |||
425 | /* check convergence */
|
||
426 | iteration++; |
||
427 | if(iteration >= max_iteration_ || delta < 1) { |
||
428 | converged = true;
|
||
429 | } |
||
430 | } |
||
431 | if(debug_) {
|
||
432 | cout << "Converged in " << iteration << " iterations." << endl; |
||
433 | if(save_error_plot) {
|
||
434 | opt.PlotError(t, opt_data, "error_func");
|
||
435 | } |
||
436 | } |
||
437 | if(nn_indecies != NULL) { |
||
438 | delete [] nn_indecies;
|
||
439 | } |
||
440 | if(mahalanobis != NULL) { |
||
441 | delete [] mahalanobis;
|
||
442 | } |
||
443 | if(gsl_R != NULL) { |
||
444 | gsl_matrix_free(gsl_R); |
||
445 | } |
||
446 | if(gsl_temp != NULL) { |
||
447 | gsl_matrix_free(gsl_temp); |
||
448 | } |
||
449 | annDeallocPt(query_point); |
||
450 | |||
451 | return iteration;
|
||
452 | } |
||
453 | void GICPPointSet::SavePoints(const char *filename) { |
||
454 | ofstream out(filename); |
||
455 | |||
456 | if(out) {
|
||
457 | int n = NumPoints();
|
||
458 | for(int i = 0; i < n; i++) { |
||
459 | out << point_[i].x << "\t" << point_[i].y << "\t" << point_[i].z << endl; |
||
460 | } |
||
461 | } |
||
462 | out.close(); |
||
463 | } |
||
464 | |||
465 | void GICPPointSet::SaveMatrices(const char *filename) { |
||
466 | ofstream out(filename); |
||
467 | |||
468 | if(out) {
|
||
469 | int n = NumPoints();
|
||
470 | for(int i = 0; i < n; i++) { |
||
471 | for(int k = 0; k < 3; k++) { |
||
472 | for(int l = 0; l < 3; l++) { |
||
473 | out << point_[i].C[k][l] << "\t";
|
||
474 | } |
||
475 | } |
||
476 | out << endl; |
||
477 | } |
||
478 | } |
||
479 | out.close(); |
||
480 | } |
||
481 | } |
||
482 | } |