Statistics
| Branch: | Revision:

root / rgbdslam / gicp / gicp.cpp @ 9240aaa3

History | View | Annotate | Download (12 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
#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
}