root / rgbdslam / gicp / optimize.cpp @ 9240aaa3
History  View  Annotate  Download (9.25 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  
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 = (maxmin)/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 = 1e2; 
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\tfvalue\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 
} 