Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (11 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 "optimize.h"
38

    
39
namespace dgc {
40
  namespace gicp {
41
    // GICP cost function
42
    double GICPOptimizer::f(const gsl_vector *x, void *params) {
43
      GICPOptData *opt_data = (GICPOptData *)params;
44
      double pt1[3];
45
      double pt2[3]; 
46
      double res[3]; // residual
47
      double temp[3];
48
      gsl_vector_view gsl_pt1 = gsl_vector_view_array(pt1, 3);
49
      gsl_vector_view gsl_pt2 = gsl_vector_view_array(pt2, 3);
50
      gsl_vector_view gsl_res = gsl_vector_view_array(res, 3);
51
      gsl_vector_view gsl_temp = gsl_vector_view_array(temp, 3);
52
      gsl_matrix_view gsl_M;
53
      dgc_transform_t t;
54

    
55
      // initialize the temp variable; if it happens to be NaN at start, bad things will happen in blas routines below
56
      temp[0] = 0;
57
      temp[1] = 0;
58
      temp[2] = 0;
59
      
60

    
61
      // take the base transformation
62
      dgc_transform_copy(t, opt_data->base_t); 
63
      // apply the current state
64
      apply_state(t, x);
65
            
66
      double f = 0;
67
      double temp_double = 0;
68
      int N = opt_data->p1->Size();
69
      for(int i = 0; i < N; i++) {
70
        int j = opt_data->nn_indecies[i];        
71
        if(j != -1) {
72
          // get point 1
73
          pt1[0] = (*opt_data->p1)[i].x;
74
          pt1[1] = (*opt_data->p1)[i].y;
75
          pt1[2] = (*opt_data->p1)[i].z;
76
          
77
          // get point 2
78
          pt2[0] = (*opt_data->p2)[j].x;
79
          pt2[1] = (*opt_data->p2)[j].y;
80
          pt2[2] = (*opt_data->p2)[j].z;
81
          
82
          //get M-matrix
83
          gsl_M = gsl_matrix_view_array(&opt_data->M[i][0][0], 3, 3);
84
          
85
          
86
          //transform point 1
87
          dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], t);
88
          res[0] = pt1[0] - pt2[0];
89
          res[1] = pt1[1] - pt2[1];
90
          res[2] = pt1[2] - pt2[2];
91

    
92
          //cout << "res: (" << res[0] << ", " <<res[1] <<", " << res[2] << ")" << endl;
93
          
94
          // temp := M*res
95
          gsl_blas_dsymv(CblasLower, 1., &gsl_M.matrix, &gsl_res.vector, 0., &gsl_temp.vector);
96
          // temp_double := res'*temp = temp'*M*temp
97
          gsl_blas_ddot(&gsl_res.vector, &gsl_temp.vector, &temp_double);
98
          // increment total error
99
          
100
          f += temp_double/(double)opt_data->num_matches;          
101
          //cout << "temp: " << temp_double << endl;
102
          //cout << "f: " << f << "\t (" << opt_data->num_matches << ")" << endl;
103
          //print_gsl_matrix(&gsl_M.matrix, "M");
104
        }
105
      }
106
      
107
      return f;
108
    }
109
    void GICPOptimizer::df(const gsl_vector *x, void *params, gsl_vector *g) {
110
      GICPOptData *opt_data = (GICPOptData *)params;
111
      double pt1[3];
112
      double pt2[3]; 
113
      double res[3]; // residual
114
      double temp[3]; // temp local vector
115
      double temp_mat[9]; // temp matrix used for accumulating the rotation gradient
116
      gsl_vector_view gsl_pt1 = gsl_vector_view_array(pt1, 3);
117
      gsl_vector_view gsl_pt2 = gsl_vector_view_array(pt2, 3);
118
      gsl_vector_view gsl_res = gsl_vector_view_array(res, 3);
119
      gsl_vector_view gsl_temp = gsl_vector_view_array(temp, 3);
120
      gsl_vector_view gsl_gradient_t = gsl_vector_subvector(g, 0, 3); // translation comp. of gradient
121
      gsl_matrix_view gsl_temp_mat_r = gsl_matrix_view_array(temp_mat, 3, 3);
122
      gsl_matrix_view gsl_M;
123
      dgc_transform_t t;
124
      double temp_double;
125
      
126
      // take the base transformation
127
      dgc_transform_copy(t, opt_data->base_t); 
128
      // apply the current state
129
      apply_state(t, x);
130
      
131
      // zero all accumulator variables
132
      gsl_vector_set_zero(g);
133
      gsl_vector_set_zero(&gsl_temp.vector);
134
      gsl_matrix_set_zero(&gsl_temp_mat_r.matrix);
135
            
136
      for(int i = 0; i < opt_data->p1->Size(); i++) {
137
        int j = opt_data->nn_indecies[i];        
138
        if(j != -1) {
139
          // get point 1
140
          pt1[0] = (*opt_data->p1)[i].x;
141
          pt1[1] = (*opt_data->p1)[i].y;
142
          pt1[2] = (*opt_data->p1)[i].z;
143
          
144
          // get point 2
145
          pt2[0] = (*opt_data->p2)[j].x;
146
          pt2[1] = (*opt_data->p2)[j].y;
147
          pt2[2] = (*opt_data->p2)[j].z;
148
          
149
          //get M-matrix
150
          gsl_M = gsl_matrix_view_array(&opt_data->M[i][0][0], 3, 3);          
151

    
152
          //transform point 1
153
          dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], t);
154
          res[0] = pt1[0] - pt2[0];
155
          res[1] = pt1[1] - pt2[1];
156
          res[2] = pt1[2] - pt2[2];
157
          
158
          // temp := M*res
159
          gsl_blas_dsymv(CblasLower, 1., &gsl_M.matrix, &gsl_res.vector, 0., &gsl_temp.vector);
160
          // temp_double := res'*temp = temp'*M*res
161
          gsl_blas_ddot(&gsl_res.vector, &gsl_temp.vector, &temp_double);
162

    
163
          // increment total translation gradient:
164
          // gsl_gradient_t += 2*M*res/num_matches
165
          gsl_blas_dsymv(CblasLower, 2./(double)opt_data->num_matches, &gsl_M.matrix, &gsl_res.vector, 1., &gsl_gradient_t.vector);          
166
          
167
          if(opt_data->solve_rotation) {
168
            // compute rotation gradient here
169
            // get back the original untransformed point to compute the rotation gradient
170
            pt1[0] = (*opt_data->p1)[i].x;
171
            pt1[1] = (*opt_data->p1)[i].y;
172
            pt1[2] = (*opt_data->p1)[i].z;
173
            dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], opt_data->base_t);
174
            gsl_blas_dger(2./(double)opt_data->num_matches, &gsl_pt1.vector, &gsl_temp.vector, &gsl_temp_mat_r.matrix);
175
          }
176
        }
177
      }
178
      // the above loop sets up the gradient with respect to the translation, and the matrix derivative w.r.t. the rotation matrix
179
      // this code sets up the matrix derivatives dR/dPhi, dR/dPsi, dR/dTheta. i.e. the derivatives of the whole rotation matrix with respect to the euler angles
180
      // note that this code assumes the XYZ order of euler angles, with the Z rotation corresponding to bearing. This means the Z angle is negative of what it would be
181
      // in the regular XYZ euler-angle convention.
182
      
183
      // now use the d/dR matrix to compute the derivative with respect to euler angles and put it directly into g[3], g[4], g[5];
184
      if(opt_data->solve_rotation) {
185
        compute_dr(x, &gsl_temp_mat_r.matrix, g);
186
      }
187
    }
188
    
189
    void GICPOptimizer::fdf(const gsl_vector *x, void *params, double * f, gsl_vector *g) {
190
      GICPOptData *opt_data = (GICPOptData *)params;
191
      double pt1[3];
192
      double pt2[3]; 
193
      double res[3]; // residual
194
      double temp[3]; // temp local vector
195
      double temp_mat[9]; // temp matrix used for accumulating the rotation gradient
196
      gsl_vector_view gsl_pt1 = gsl_vector_view_array(pt1, 3);
197
      gsl_vector_view gsl_pt2 = gsl_vector_view_array(pt2, 3);
198
      gsl_vector_view gsl_res = gsl_vector_view_array(res, 3);
199
      gsl_vector_view gsl_temp = gsl_vector_view_array(temp, 3);
200
      gsl_vector_view gsl_gradient_t = gsl_vector_subvector(g, 0, 3); // translation comp. of gradient
201
      gsl_vector_view gsl_gradient_r = gsl_vector_subvector(g, 3, 3); // rotation comp. of gradient
202
      gsl_matrix_view gsl_temp_mat_r = gsl_matrix_view_array(temp_mat, 3, 3);
203
      gsl_matrix_view gsl_M;
204
      dgc_transform_t t;
205
      double temp_double;
206

    
207
      // take the base transformation
208
      dgc_transform_copy(t, opt_data->base_t); 
209
      // apply the current state      
210
      apply_state(t, x);
211
            
212
      // zero all accumulator variables
213
      *f = 0;
214
      gsl_vector_set_zero(g);
215
      gsl_vector_set_zero(&gsl_temp.vector);
216
      gsl_matrix_set_zero(&gsl_temp_mat_r.matrix);
217
      
218
      for(int i = 0; i < opt_data->p1->Size(); i++) {
219
        int j = opt_data->nn_indecies[i];        
220
        if(j != -1) {
221
          // get point 1
222
          pt1[0] = (*opt_data->p1)[i].x;
223
          pt1[1] = (*opt_data->p1)[i].y;
224
          pt1[2] = (*opt_data->p1)[i].z;
225
          
226
          // get point 2
227
          pt2[0] = (*opt_data->p2)[j].x;
228
          pt2[1] = (*opt_data->p2)[j].y;
229
          pt2[2] = (*opt_data->p2)[j].z;
230
          
231
          //cout << "accessing " << i << " of " << opt_data->p1->Size() << ", " << opt_data->p2->Size() << endl;
232
          //get M-matrix
233
          gsl_M = gsl_matrix_view_array(&opt_data->M[i][0][0], 3, 3);          
234
          
235
          //transform point 1
236
          dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], t);
237
          res[0] = pt1[0] - pt2[0];
238
          res[1] = pt1[1] - pt2[1];
239
          res[2] = pt1[2] - pt2[2];
240
          
241
          // compute the transformed residual
242
          // temp := M*res
243
          //print_gsl_matrix(&gsl_M.matrix, "gsl_m");
244
          gsl_blas_dsymv(CblasLower, 1., &gsl_M.matrix, &gsl_res.vector, 0., &gsl_temp.vector);
245
          
246
          // compute M-norm of the residual
247
          // temp_double := res'*temp = temp'*M*res
248
          gsl_blas_ddot(&gsl_res.vector, &gsl_temp.vector, &temp_double);
249
          
250
          // accumulate total error: f += res'*M*res
251
          *f += temp_double/(double)opt_data->num_matches;
252
          
253
          // accumulate translation gradient:
254
          // gsl_gradient_t += 2*M*res
255
          gsl_blas_dsymv(CblasLower, 2./(double)opt_data->num_matches, &gsl_M.matrix, &gsl_res.vector, 1., &gsl_gradient_t.vector);          
256
          
257
          if(opt_data->solve_rotation) {
258
            // accumulate the rotation gradient matrix
259
            // get back the original untransformed point to compute the rotation gradient
260
            pt1[0] = (*opt_data->p1)[i].x;
261
            pt1[1] = (*opt_data->p1)[i].y;
262
            pt1[2] = (*opt_data->p1)[i].z;
263
            dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], opt_data->base_t);
264
            // gsl_temp_mat_r += 2*(gsl_temp).(gsl_pt1)' [ = (2*M*residual).(gsl_pt1)' ]          
265
            gsl_blas_dger(2./(double)opt_data->num_matches, &gsl_pt1.vector, &gsl_temp.vector, &gsl_temp_mat_r.matrix); 
266
          }
267
        }
268
      }      
269
      // the above loop sets up the gradient with respect to the translation, and the matrix derivative w.r.t. the rotation matrix
270
      // this code sets up the matrix derivatives dR/dPhi, dR/dPsi, dR/dTheta. i.e. the derivatives of the whole rotation matrix with respect to the euler angles
271
      // note that this code assumes the XYZ order of euler angles, with the Z rotation corresponding to bearing. This means the Z angle is negative of what it would be
272
      // in the regular XYZ euler-angle convention.
273
      if(opt_data->solve_rotation) {
274
        // now use the d/dR matrix to compute the derivative with respect to euler angles and put it directly into g[3], g[4], g[5];
275
        compute_dr(x, &gsl_temp_mat_r.matrix, g);
276
      }
277
    }
278
  }
279
}