Statistics
| Branch: | Revision:

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

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

    
41
#include <iostream>
42
#include <assert.h>
43
#include <fstream>
44
#include <sstream>
45

    
46
using namespace std;
47

    
48
namespace dgc {
49
  namespace gicp {
50
    
51
    GICPOptimizer::GICPOptimizer() :
52
      T_min(gsl_multimin_fdfminimizer_vector_bfgs2)
53
    {
54
      max_iter = 20;
55
      iter = -1;
56
      status = GSL_CONTINUE;
57
      
58
      gsl_minimizer = gsl_multimin_fdfminimizer_alloc(T_min, N);
59
      
60
      if(gsl_minimizer == NULL) {
61
        //TODO: handle
62
      }
63
      x = gsl_vector_alloc(N);
64
      if(x == NULL) {
65
        //TODO: handle
66
      }
67
      debug = false;
68
    }
69

    
70
    GICPOptimizer::~GICPOptimizer() {
71
      if(gsl_minimizer != NULL) {
72
        gsl_multimin_fdfminimizer_free(gsl_minimizer);
73
      }
74
      if(x != NULL) {
75
        gsl_vector_free(x);
76
      }
77
    }
78

    
79
    // this method outputs data files containing the value of the error function on a 2d grid.
80
    // a seperate file and grid is constructed for each pair of coordinates
81
    // this is used to verify convergence of the numerical minimization
82
    void GICPOptimizer::PlotError(dgc_transform_t t, GICPOptData &opt_data, const char* filename) {
83
      double resolution = .005;
84
      double min = -.1;
85
      double max = .1;
86
      
87
      // initialize the starting point
88
      double tx, ty, tz, rx, ry, rz;
89
      dgc_transform_get_translation(t, &tx, &ty, &tz);
90
      dgc_transform_get_rotation(t, &rx, &ry, &rz);
91
      gsl_vector_set(x, 0, tx);
92
      gsl_vector_set(x, 1, ty);
93
      gsl_vector_set(x, 2, tz);
94
      gsl_vector_set(x, 3, rx);
95
      gsl_vector_set(x, 4, ry);
96
      gsl_vector_set(x, 5, rz);
97
      
98
      dgc_transform_t temp;
99

    
100
      int N = (max-min)/resolution;
101

    
102
      for(int n1 = 0; n1 < 6; n1++) {
103
        for(int n2 = 0; n2 < n1; n2++) {
104
          // set up the filename for this pair of coordinates
105
          ostringstream full_filename;
106
          full_filename << filename << "_" << n1 << "vs" << n2 << ".dat";
107
          // open the file
108
          ofstream fout(full_filename.str().c_str());
109
          if(!fout) {
110
            cout << "Could not open file for writing." << endl;
111
            return;
112
          }
113
          
114
          // loop through pairs of values for these two coordinates while keeping others fixed
115
          double val1_0 = gsl_vector_get(x, n1);
116
          for(int k1 = 0; k1 < N; k1++) {    
117
            gsl_vector_set(x, n1, val1_0 + min + resolution*k1);
118
            
119
            double val2_0 = gsl_vector_get(x, n2);
120
            for(int k2 = 0; k2 < N; k2++) {
121
              gsl_vector_set(x, n2, val2_0 + min + resolution*k2);
122
              
123
              dgc_transform_copy(temp, opt_data.base_t);
124
              apply_state(temp, x);
125
          
126
              double error = f(x, &opt_data);
127
              fout << error << "\t";                              
128
            }
129
            gsl_vector_set(x, n2, val2_0); // restore to old value
130
            
131
            fout << endl;
132
          }
133
          gsl_vector_set(x, n1, val1_0); // restore to old value
134

    
135
          // close the file for this pair of coordinates
136
          fout.close();
137
        }
138
      }
139
    }
140

    
141
    bool GICPOptimizer::Optimize(dgc_transform_t t, GICPOptData & opt_data) {
142
      double line_search_tol = .01;
143
      double gradient_tol = 1e-2;
144
      double step_size = 1.;
145
                  
146
      // set up the gsl function_fdf struct
147
      gsl_multimin_function_fdf func;
148
      func.f = f;
149
      func.df = df;
150
      func.fdf = fdf;
151
      func.n = N;
152
      func.params = &opt_data;
153
      
154
      // initialize the starting point
155
      double tx, ty, tz, rx, ry, rz;
156
      dgc_transform_get_translation(t, &tx, &ty, &tz);
157
      dgc_transform_get_rotation(t, &rx, &ry, &rz);
158
      gsl_vector_set(x, 0, tx);
159
      gsl_vector_set(x, 1, ty);
160
      gsl_vector_set(x, 2, tz);
161
      gsl_vector_set(x, 3, rx);
162
      gsl_vector_set(x, 4, ry);
163
      gsl_vector_set(x, 5, rz);
164
      
165
      // initialize the minimizer
166
      gsl_multimin_fdfminimizer_set(gsl_minimizer, &func, x, step_size, line_search_tol);
167
      
168
      //iterate the minimization algorithm using gsl primatives
169
      status = GSL_CONTINUE;
170
      iter = 0;
171
      if(debug) {
172
        cout << "iter\t\tf-value\t\tstatus" << endl;
173
        cout << iter << "\t\t" << gsl_minimizer->f << "\t\t" << Status() << endl;
174
      }
175
      while(status == GSL_CONTINUE && iter < max_iter) {
176
        iter++;
177
        status = gsl_multimin_fdfminimizer_iterate(gsl_minimizer);
178
        if(debug) {
179
          cout << iter << "\t\t" << gsl_minimizer->f << "\t\t" << Status() <<endl;
180
        }
181
        if(status) {
182
          break;
183
        }
184
        status = gsl_multimin_test_gradient (gsl_minimizer->gradient, gradient_tol);
185
      }
186
      
187
      if(status == GSL_SUCCESS || iter == max_iter) {
188
        //set t to the converged solution
189
        
190
        dgc_transform_identity(t);
191
        // apply the current state to the base
192
        apply_state(t, gsl_minimizer->x);
193
        //dgc_transform_print(t, "converged to:");        
194
        return true;
195
      }
196
      else {
197
        // the algorithm failed to converge
198
        return false;
199
      }
200
    }
201
    
202

    
203
    double GICPOptimizer::mat_inner_prod(gsl_matrix const* mat1, gsl_matrix const* mat2) {
204
      double r = 0.;
205
      int n = mat1->size1;
206
      
207
      for(int i = 0; i < n; i++) {
208
        for(int j = 0; j < n; j++) { // tr(mat1^t.mat2)
209
          r += gsl_matrix_get(mat1, j, i)*gsl_matrix_get(mat2, i, j);
210
        }
211
      }
212
      
213
      return r;
214
    }
215
    
216
    void GICPOptimizer::apply_state(dgc_transform_t t, gsl_vector const* x) {
217
      double tx, ty, tz, rx, ry, rz;
218
      tx = gsl_vector_get(x, 0);
219
      ty = gsl_vector_get(x, 1);
220
      tz = gsl_vector_get(x, 2);
221
      rx = gsl_vector_get(x, 3);
222
      ry = gsl_vector_get(x, 4);
223
      rz = gsl_vector_get(x, 5);
224

    
225
      dgc_transform_rotate_x(t, rx);
226
      dgc_transform_rotate_y(t, ry);
227
      dgc_transform_rotate_z(t, rz);      
228
      dgc_transform_translate(t, tx, ty, tz);
229
    }
230
    
231
    void GICPOptimizer::compute_dr(gsl_vector const* x, gsl_matrix const* gsl_temp_mat_r, gsl_vector *g) {
232
      double dR_dPhi[3][3];
233
      double dR_dTheta[3][3];
234
      double dR_dPsi[3][3];
235
      gsl_matrix_view gsl_d_rx = gsl_matrix_view_array(&dR_dPhi[0][0],3, 3);
236
      gsl_matrix_view gsl_d_ry = gsl_matrix_view_array(&dR_dTheta[0][0],3, 3);
237
      gsl_matrix_view gsl_d_rz = gsl_matrix_view_array(&dR_dPsi[0][0],3, 3);
238

    
239
      double phi = gsl_vector_get(x ,3);
240
      double theta = gsl_vector_get(x ,4);
241
      double psi = gsl_vector_get(x ,5);  
242
      
243
      double cphi = cos(phi), sphi = sin(phi);
244
      double ctheta = cos(theta), stheta = sin(theta);
245
      double cpsi = cos(psi), spsi = sin(psi);
246
      
247
      dR_dPhi[0][0] = 0.;
248
      dR_dPhi[1][0] = 0.;
249
      dR_dPhi[2][0] = 0.;
250
      
251
      dR_dPhi[0][1] = sphi*spsi + cphi*cpsi*stheta;
252
      dR_dPhi[1][1] = -cpsi*sphi + cphi*spsi*stheta;
253
      dR_dPhi[2][1] = cphi*ctheta;
254
      
255
      dR_dPhi[0][2] = cphi*spsi - cpsi*sphi*stheta;
256
      dR_dPhi[1][2] = -cphi*cpsi - sphi*spsi*stheta;
257
      dR_dPhi[2][2] = -ctheta*sphi;
258
      
259
      dR_dTheta[0][0] = -cpsi*stheta;
260
      dR_dTheta[1][0] = -spsi*stheta;
261
      dR_dTheta[2][0] = -ctheta;
262
      
263
      dR_dTheta[0][1] = cpsi*ctheta*sphi;
264
      dR_dTheta[1][1] = ctheta*sphi*spsi;
265
      dR_dTheta[2][1] = -sphi*stheta;
266
        
267
      dR_dTheta[0][2] = cphi*cpsi*ctheta;
268
      dR_dTheta[1][2] = cphi*ctheta*spsi;
269
      dR_dTheta[2][2] = -cphi*stheta;
270
      
271
      dR_dPsi[0][0] = -ctheta*spsi;
272
      dR_dPsi[1][0] = cpsi*ctheta;
273
      dR_dPsi[2][0] = 0.;
274
      
275
      dR_dPsi[0][1] = -cphi*cpsi - sphi*spsi*stheta;
276
      dR_dPsi[1][1] = -cphi*spsi + cpsi*sphi*stheta;
277
      dR_dPsi[2][1] = 0.;
278
      
279
      dR_dPsi[0][2] = cpsi*sphi - cphi*spsi*stheta;
280
      dR_dPsi[1][2] = sphi*spsi + cphi*cpsi*stheta;
281
      dR_dPsi[2][2] = 0.;
282
      
283
      // set d/d_rx = tr(dR_dPhi'*gsl_temp_mat_r) [= <dR_dPhi, gsl_temp_mat_r>]
284
      gsl_vector_set(g, 3, mat_inner_prod(&gsl_d_rx.matrix, gsl_temp_mat_r));
285
      // set d/d_ry = tr(dR_dTheta'*gsl_temp_mat_r) = [<dR_dTheta, gsl_temp_mat_r>]
286
      gsl_vector_set(g, 4, mat_inner_prod(&gsl_d_ry.matrix, gsl_temp_mat_r));
287
      // set d/d_rz = tr(dR_dPsi'*gsl_temp_mat_r) = [<dR_dPsi, gsl_temp_mat_r>]
288
      gsl_vector_set(g, 5, mat_inner_prod(&gsl_d_rz.matrix, gsl_temp_mat_r));      
289

    
290
    }
291

    
292
  }
293
}