Optimization.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
3  *
4  * Copyright 2007-2012 VTT Technical Research Centre of Finland
5  *
6  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
7  * <http://www.vtt.fi/multimedia/alvar.html>
8  *
9  * ALVAR is free software; you can redistribute it and/or modify it under the
10  * terms of the GNU Lesser General Public License as published by the Free
11  * Software Foundation; either version 2.1 of the License, or (at your option)
12  * any later version.
13  *
14  * This library is distributed in the hope that it will be useful, but WITHOUT
15  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
16  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
17  * for more details.
18  *
19  * You should have received a copy of the GNU Lesser General Public License
20  * along with ALVAR; if not, see
21  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
22  */
23 
24 #include "ar_track_alvar/Alvar.h"
25 #include "ar_track_alvar/Optimization.h"
26 #include "time.h"
27 #include "highgui.h"
28 
29 #include <iostream>
30 using namespace std;
31 
32 namespace alvar {
33 
34 Optimization::Optimization(int n_params, int n_meas)
35 {
36  estimate_param = 0;
37  J = cvCreateMat(n_meas, n_params, CV_64F); cvZero(J);
38  JtJ = cvCreateMat(n_params, n_params, CV_64F); cvZero(JtJ);
39  tmp = cvCreateMat(n_params, n_meas, CV_64F); cvZero(tmp);
40  W = cvCreateMat(n_meas, n_meas, CV_64F); cvZero(W);
41  diag = cvCreateMat(n_params, n_params, CV_64F); cvZero(diag);
42  err = cvCreateMat(n_meas, 1, CV_64F); cvZero(err);
43  delta = cvCreateMat(n_params, 1, CV_64F); cvZero(delta);
44  x_minus = cvCreateMat(n_params, 1, CV_64F); cvZero(x_minus);
45  x_plus = cvCreateMat(n_params, 1, CV_64F); cvZero(x_plus);
46  x_tmp1 = cvCreateMat(n_meas, 1, CV_64F); cvZero(x_tmp1);
47  x_tmp2 = cvCreateMat(n_meas, 1, CV_64F); cvZero(x_tmp2);
48  tmp_par = cvCreateMat(n_params, 1, CV_64F); cvZero(tmp_par);
49 }
50 
51 Optimization::~Optimization()
52 {
53  cvReleaseMat(&J);
54  cvReleaseMat(&JtJ);
55  cvReleaseMat(&diag);
56  cvReleaseMat(&tmp);
57  cvReleaseMat(&W);
58  cvReleaseMat(&err);
59  cvReleaseMat(&delta);
60  cvReleaseMat(&x_plus);
61  cvReleaseMat(&x_minus);
62  cvReleaseMat(&x_tmp1);
63  cvReleaseMat(&x_tmp2);
64  cvReleaseMat(&tmp_par);
65  estimate_param = 0;
66 }
67 
68 double Optimization::CalcTukeyWeight(double residual, double c)
69 {
70  //const double c = 3; // squared distance in the model tracker
71  double ret=0;
72 
73  if(fabs(residual) <= c)
74  {
75  double tmp = 1.0-((residual/c)*(residual/c));
76  ret = ((c*c)/6.0)*(1.0-tmp*tmp*tmp);
77  }
78  else
79  ret = (c*c)/6.0;
80 
81  if(residual)
82  ret = fabs(sqrt(ret)/residual);
83  else
84  ret = 1.0; // ???
85 
86  return ret;
87 }
88 
89 double Optimization::CalcTukeyWeightSimple(double residual, double c)
90 {
91  //const double c = 3;
92  double ret=0;
93 
94  double x2 = residual*residual;
95  if(x2<c*c) return residual;
96  else return c;
97 }
98 
99 void Optimization::CalcJacobian(CvMat* x, CvMat* J, EstimateCallback Estimate)
100 {
101  const double step = 0.001;
102 
103  cvZero(J);
104  for (int i=0; i<J->cols; i++)
105  {
106  CvMat J_column;
107  cvGetCol(J, &J_column, i);
108 
109  cvZero(delta);
110  cvmSet(delta, i, 0, step);
111  cvAdd(x, delta, x_plus);
112  cvmSet(delta, i, 0, -step);
113  cvAdd(x, delta, x_minus);
114 
115  Estimate(x_plus, x_tmp1, estimate_param);
116  Estimate(x_minus, x_tmp2, estimate_param);
117  cvSub(x_tmp1, x_tmp2, &J_column);
118  cvScale(&J_column, &J_column, 1.0/(2*step));
119  }
120 }
121 
122 
123 double Optimization::Optimize(CvMat* parameters, // Initial values are set
124  CvMat* measurements, // Some observations
125  double stop,
126  int max_iter,
127  EstimateCallback Estimate,
128  void *param,
129  OptimizeMethod method,
130  CvMat* parameters_mask, // Mask indicating non-constant parameters)
131  CvMat* J_mat,
132  CvMat* weights)
133 {
134 
135  int n_params = parameters->rows;
136  int n_meas = measurements->rows;
137  double error_new = 0;
138  double error_old = 0;
139  double n1, n2;
140  int cntr = 0;
141  estimate_param = param;
142  lambda = 0.001;
143 
144  while(true)
145  {
146  if(!J_mat)
147  CalcJacobian(parameters, J, Estimate);
148  else
149  J = J_mat;
150 
151  // Zero the columns for constant parameters
152  // TODO: Make this into a J-sized mask matrix before the iteration loop
153  if(parameters_mask)
154  for (int i=0; i<parameters_mask->rows; i++) {
155  if (cvGet2D(parameters_mask, i, 0).val[0] == 0) {
156  CvRect rect;
157  rect.height = J->rows; rect.width = 1;
158  rect.y = 0; rect.x = i;
159  CvMat foo;
160  cvGetSubRect(J, &foo, rect);
161  cvZero(&foo);
162  }
163  }
164 
165  Estimate(parameters, x_tmp1, estimate_param);
166  cvSub(measurements, x_tmp1, err); // err = residual
167  error_old = cvNorm(err, 0, CV_L2);
168 
169  switch(method)
170  {
171  case (GAUSSNEWTON) :
172 
173  cvMulTransposed(J, JtJ, 1);
174  cvInv(JtJ, JtJ, CV_SVD);
175  cvGEMM(JtJ, J, 1.0, 0, 0, tmp, CV_GEMM_B_T); // inv(JtJ)Jt
176  cvMatMul(tmp, err, delta);
177  cvAdd(delta, parameters, parameters);
178 
179  // Lopetusehto
180  n1 = cvNorm(delta);
181  n2 = cvNorm(parameters);
182 
183  if( ((n1/n2) < stop) ||
184  (cntr >= max_iter) )
185  goto end;
186 
187  break;
188 
189  case (LEVENBERGMARQUARDT) :
190 
191  cvSetIdentity(diag, cvRealScalar(lambda));
192 
193  if(weights)
194  for(int k = 0; k < W->rows; ++k)
195  cvmSet(W, k, k, weights->data.db[k]);
196 
197  // JtWJ
198  if(weights)
199  {
200  cvGEMM(J, W, 1, 0, 0, tmp, CV_GEMM_A_T);
201  cvGEMM(tmp, J, 1, 0, 0, JtJ, 0);
202  }
203  else
204  cvMulTransposed(J, JtJ, 1);
205 
206  // JtJ + lambda*I
207  // or JtWJ + lambda*I if weights are used...
208  cvAdd(JtJ, diag, JtJ);
209  cvInv(JtJ, JtJ, CV_SVD);
210  cvGEMM(JtJ, J, 1.0, 0, 0, tmp, CV_GEMM_B_T);
211 
212  if(weights)
213  cvGEMM(tmp, W, 1, 0, 0, tmp, 0);
214 
215  cvMatMul(tmp, err, delta);
216  cvAdd(delta, parameters, tmp_par);
217 
218  Estimate(tmp_par, x_tmp1, estimate_param);
219  cvSub(measurements, x_tmp1, err);
220 
221  error_new = cvNorm(err, 0, CV_L2);
222 
223  if(error_new < error_old)
224  {
225  cvCopy(tmp_par, parameters);
226  lambda = lambda/10.0;
227  }
228  else
229  {
230  lambda = lambda*10.0;
231  }
232  if(lambda>10) lambda = 10;
233  if(lambda<0.00001) lambda = 0.00001;
234 
235  n1 = cvNorm(delta);
236  n2 = cvNorm(parameters);
237 
238  if( (n1/n2) < stop ||
239  (cntr >= max_iter) )
240  {
241  goto end;
242  }
243 
244  break;
245 
246  case (TUKEY_LM) :
247 
248  cvSetIdentity(diag, cvRealScalar(lambda));
249 
250  // Tukey weights
251  for(int k = 0; k < W->rows; ++k)
252  {
253  if(weights) // If using weight vector
254  if(weights->data.db[k] != -1.0) // If true weight given
255  cvmSet(W, k, k, weights->data.db[k]); // Use given weight
256  else
257  cvmSet(W, k, k, CalcTukeyWeight(err->data.db[k], 3)); // otherwise use Tukey weight
258  else
259  cvmSet(W, k, k, CalcTukeyWeight(err->data.db[k], 3)); // Otherwise use Tukey weight
260  }
261 
262  cvGEMM(J, W, 1, 0, 0, tmp, CV_GEMM_A_T);
263  cvGEMM(tmp, J, 1, 0, 0, JtJ, 0);
264  cvAdd(JtJ, diag, JtJ);
265  cvInv(JtJ, JtJ, CV_SVD);
266  cvGEMM(JtJ, J, 1.0, 0, 0, tmp, CV_GEMM_B_T);
267  cvGEMM(tmp, W, 1, 0, 0, tmp, 0);
268  cvMatMul(tmp, err, delta);
269  cvAdd(delta, parameters, tmp_par);
270 
271  Estimate(tmp_par, x_tmp1, estimate_param);
272  cvSub(measurements, x_tmp1, err);
273 
274  error_new = cvNorm(err, 0, CV_L2);
275 
276  if(error_new < error_old)
277  {
278  cvCopy(tmp_par, parameters);
279  lambda = lambda/10.0;
280  }
281  else
282  {
283  lambda = lambda*10.0;
284  }
285  if(lambda>10) lambda = 10;
286  if(lambda<0.00001) lambda = 0.00001;
287 
288  n1 = cvNorm(delta);
289  n2 = cvNorm(parameters);
290 
291  if( ((n1/n2) < stop) ||
292  (cntr >= max_iter) )
293  {
294  goto end;
295  }
296 
297  break;
298  }
299  ++cntr;
300  }
301 
302 end :
303 
304  return error_old;
305 }
306 
307 } // namespace alvar
alvar
Main ALVAR namespace.
Definition: Alvar.h:174
Estimate
void Estimate(CvMat *state, CvMat *projection, void *param)
Definition: SampleOptimization.cpp:40
alvar::Optimization::OptimizeMethod
OptimizeMethod
Selection between the algorithm used in optimization. Following should be noticed: ...
Definition: Optimization.h:74
Optimization.h
This file implements several optimization algorithms.
step
unsigned int step
Alvar.h
This file defines library export definitions, version numbers and build information.

ar_track_alvar
Author(s): Scott Niekum
autogenerated on Mon Jun 10 2019 12:47:04