root / rgbdslam / gicp / gicp.cpp @ 9240aaa3
History  View  Annotate  Download (12 KB)
1 
/*************************************************************


2 
GeneralizedICP 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_ = 1e4; // correspondes to ~1 mm (tolerence for convergence of GICP outer loop) 
60 
epsilon_rot_ = 2e4; 
61 
// epsilon_ = 5e4; // correspondes to ~1 mm (tolerence for convergence of GICP outer loop)

62 
// epsilon_rot_ = 2e3;

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 
} 