root / rgbdslam / gicp / optimize.cpp @ 9240aaa3
History | View | Annotate | Download (9.25 KB)
1 | 9240aaa3 | Alex | /*************************************************************
|
---|---|---|---|
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 | } |