Non-linear optimization routines. There are three methods implemented that include Gauss-Newton, Levenberg-Marquardt and Tukey m-estimator. More...
#include <Optimization.h>
Public Types | |
| typedef void(* | EstimateCallback )(CvMat *state, CvMat *projection, void *param) |
| Pointer to the function that projects the state of the system to the measurements. | |
| enum | OptimizeMethod { GAUSSNEWTON, LEVENBERGMARQUARDT, TUKEY_LM } |
| Selection between the algorithm used in optimization. Following should be noticed: More... | |
Public Member Functions | |
| void | CalcJacobian (CvMat *x, CvMat *J, EstimateCallback Estimate) |
| Numerically differentiates and calculates the Jacobian around x. | |
| CvMat * | GetErr () |
| Returns the current residual vector. | |
| Optimization (int n_params, int n_meas) | |
| Constructor. | |
| double | Optimize (CvMat *parameters, CvMat *measurements, double stop, int max_iter, EstimateCallback Estimate, void *param=0, OptimizeMethod method=LEVENBERGMARQUARDT, CvMat *parameters_mask=0, CvMat *J_mat=0, CvMat *weights=0) |
| Runs the optimization loop with selected parameters. | |
| ~Optimization () | |
Private Member Functions | |
| double | CalcTukeyWeight (double residual, double c) |
| double | CalcTukeyWeightSimple (double residual, double c) |
Private Attributes | |
| CvMat * | delta |
| CvMat * | diag |
| CvMat * | err |
| void * | estimate_param |
| CvMat * | J |
| CvMat * | JtJ |
| double | lambda |
| CvMat * | tmp |
| CvMat * | tmp_par |
| CvMat * | W |
| CvMat * | x_minus |
| CvMat * | x_plus |
| CvMat * | x_tmp1 |
| CvMat * | x_tmp2 |
Non-linear optimization routines. There are three methods implemented that include Gauss-Newton, Levenberg-Marquardt and Tukey m-estimator.
Definition at line 44 of file Optimization.h.
| typedef void(* alvar::Optimization::EstimateCallback)(CvMat *state, CvMat *projection, void *param) |
Pointer to the function that projects the state of the system to the measurements.
| state | System parameters, e.g. camera parameterization in optical tracking. |
| projection | The system state projection is stored here. E.g image measurements in optical tracking. |
| param | Additional parameters to the function. E.g. some constant parameters that are not optimized. |
Definition at line 101 of file Optimization.h.
Selection between the algorithm used in optimization. Following should be noticed:
Definition at line 74 of file Optimization.h.
| alvar::Optimization::Optimization | ( | int | n_params, |
| int | n_meas | ||
| ) |
Constructor.
| n_params | Number of parameters to be optimized. |
| n_meas | Number of measurements that are observed. |
Definition at line 34 of file Optimization.cpp.
Definition at line 51 of file Optimization.cpp.
| void alvar::Optimization::CalcJacobian | ( | CvMat * | x, |
| CvMat * | J, | ||
| EstimateCallback | Estimate | ||
| ) |
Numerically differentiates and calculates the Jacobian around x.
| x | The set of parameters around which the Jacobian is evaluated. |
| J | Resulting Jacobian matrix is stored here. |
| Estimate | The function to be differentiated. |
Definition at line 99 of file Optimization.cpp.
| double alvar::Optimization::CalcTukeyWeight | ( | double | residual, |
| double | c | ||
| ) | [private] |
Definition at line 68 of file Optimization.cpp.
| double alvar::Optimization::CalcTukeyWeightSimple | ( | double | residual, |
| double | c | ||
| ) | [private] |
Definition at line 89 of file Optimization.cpp.
| CvMat* alvar::Optimization::GetErr | ( | ) | [inline] |
Returns the current residual vector.
Definition at line 93 of file Optimization.h.
| double alvar::Optimization::Optimize | ( | CvMat * | parameters, |
| CvMat * | measurements, | ||
| double | stop, | ||
| int | max_iter, | ||
| EstimateCallback | Estimate, | ||
| void * | param = 0, |
||
| OptimizeMethod | method = LEVENBERGMARQUARDT, |
||
| CvMat * | parameters_mask = 0, |
||
| CvMat * | J_mat = 0, |
||
| CvMat * | weights = 0 |
||
| ) |
Runs the optimization loop with selected parameters.
| parameters | Vector of parameters to be optimized. Initial values should be set. |
| measurements | Vector of measurements that are observed. |
| stop | Optimization loop ends as the stop limit is reached. Criteria is calculated as |
| max_iter | Maximum number of iteration loops that are evaluated if stop is not reached. |
| Estimate | Pointer to the function that maps the state to the measurements. See EstimateCallback. |
| method | One of the three possible optimization methods. |
| parameters_mask | Vector that defines the parameters that are optimized. If vector element is 0, corresponding parameter is not altered. |
| J_mat | Jacobian matrix. If not given, numerical differentation is used. |
| weights | Weight vector that can be submitted to give different weights to different measurements. Currently works only with OptimizeMethod::TUKEY_LM. |
Definition at line 123 of file Optimization.cpp.
CvMat* alvar::Optimization::delta [private] |
Definition at line 56 of file Optimization.h.
CvMat* alvar::Optimization::diag [private] |
Definition at line 53 of file Optimization.h.
CvMat* alvar::Optimization::err [private] |
Definition at line 55 of file Optimization.h.
void* alvar::Optimization::estimate_param [private] |
Definition at line 49 of file Optimization.h.
CvMat* alvar::Optimization::J [private] |
Definition at line 50 of file Optimization.h.
CvMat* alvar::Optimization::JtJ [private] |
Definition at line 51 of file Optimization.h.
double alvar::Optimization::lambda [private] |
Definition at line 66 of file Optimization.h.
CvMat* alvar::Optimization::tmp [private] |
Definition at line 54 of file Optimization.h.
CvMat* alvar::Optimization::tmp_par [private] |
Definition at line 61 of file Optimization.h.
CvMat* alvar::Optimization::W [private] |
Definition at line 52 of file Optimization.h.
CvMat* alvar::Optimization::x_minus [private] |
Definition at line 58 of file Optimization.h.
CvMat* alvar::Optimization::x_plus [private] |
Definition at line 57 of file Optimization.h.
CvMat* alvar::Optimization::x_tmp1 [private] |
Definition at line 59 of file Optimization.h.
CvMat* alvar::Optimization::x_tmp2 [private] |
Definition at line 60 of file Optimization.h.