root / rgbdslam / gicp / bfgs_funcs.cpp @ 9240aaa3
History | View | Annotate | Download (11 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 | #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 | } |