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