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 |
} |