optimization.h
4.58 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
#ifndef OPTIMIZATION_H
#define OPTIMIZATION_H
/*!
\defgroup optimization Optimization
\brief Global optimization for autocalibration
\bug Problem with the function non-homogenious !!!
*/
///@{
///
#include <math.h>
#include "matcell.h"
#include "transformations.h"
#include "utils.h"
#include <iostream>
#include <iomanip>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <nlopt.hpp>
#ifndef PI
#define PI 3.141592653589793238462643383279502884L
#endif
using namespace std;
class Optimization
{
public:
Optimization(){}
~Optimization();
static double wrap(const std::vector<double> &x, std::vector<double> &grad, void *data) {
return (*reinterpret_cast<Optimization*>(data))(x, grad);
}
double deg2rad(double angDeg);
void setMeasurementMatrix(cv::Mat inW);
void init();
bool isInit() const { return ! W.empty(); }
void copyVectorToMatElements(const std::vector<double> &vec, const cv::Mat &idx, cv::Mat &mat);
int _objFuncMethod = OBJFUNC_Rectification;
enum ObjFuncMethod{
OBJFUNC_DistanceSquared,
OBJFUNC_Distance,
OBJFUNC_PseudoInverse,
OBJFUNC_Rectification
};
double _minf;
double getMinf() const{ return _minf;}
int _maxTimeStep1 = 15;
int _maxTimeStep2 = 60;
void setMaxTimeStep1(int maxTime){ _maxTimeStep1 = maxTime; }
void setMaxTimeStep2(int maxTime){ _maxTimeStep2 = maxTime; }
void run();
double operator() (const std::vector<double> &x, std::vector<double> &grad);
double testFunction (const std::vector<double> &x);
double distanceSquared (const std::vector<double> &x);
double distancePseudoInverse (const std::vector<double> &x);
double distanceWminusMMW (const std::vector<double> &x);
int getParametersNumber();
void getInitialConditionsAndBounds(std::vector<double> & _x0, std::vector<double> & _lb, std::vector<double> & _ub);
void setObjectiveFunction(int objFuncMethod) { _objFuncMethod = objFuncMethod; _iterCnt = 0;}
bool isFeaturerScaled = true;
void useFeatureScaling(bool b) { isFeaturerScaled = b; }
double _bestObjValue = HUGE_VAL;
int _iterCnt = 0;
int nbCams, nbPts;
cv::Mat W;
cv::Mat Win;
cv::Mat paramConst; ///< matrix defining if optimization parameter is constant (=0). Size = number of images \f$\times\f$ number of parameters
/** matrix defining optimization bounds. Size = number of images \f$\times\f$ number of parameters.
* lower bounds = -paramBounds;
* upper bounds = +paramBounds;
*/
cv::Mat paramBounds;
/** Offset for all parameters. Size = number of images \f$\times\f$ number of parameters.
* Actual value is calculated as offset + initial
*/
cv::Mat paramOffset;
cv::Mat paramInitial; ///< Initial values of parameters. Size = number of images \f$\times\f$ number of parameters
cv::Mat paramResult; ///< Resulting values of parameters. Offset + optimization results
cv::Mat paramVarIdx; ///< indices of varying parameters in the table of parameters paramConst, paramOffset etc.
int nbParams;
///< indices of each parameter
enum{
K = 0, ///< scale factor
ALPHA, ///< fx/fy, misscaling of axis
S, ///< skew
RT1, ///< \f$ \theta_1 \f$
RR, ///< \f$ \rho \f$
RT2, ///< \f$ \theta_2 \f$
PARAMS_PER_CAM ///< number of parameters for one camera
};
std::vector<cv::Mat> getCameraMatrices() const;
cv::Mat getCalibrationMatrix() const;
cv::Mat getRotationAnglesDeg() const;
cv::Mat getPoints3D(const std::vector<double> &x) { return getPoints3DfromParams(x);}
private:
std::vector<cv::Mat> tm;
void computeWmean(const cv::Mat & Win, cv::Mat & Wmean, std::vector<cv::Mat> & tm);
MatCell getMfromParams(const std::vector<double> &x);
cv::Mat getPoints3DfromParams(const std::vector<double> &x);
cv::Mat getCalibrationMatrixFromParamTable(const cv::Mat ¶mTable) const;
};
#endif // OPTIMIZATIONPROBLEM_H
/*nlopt methods
* nlopt::opt opt(nlopt::GN_CRS2_LM, optim->getParametersNumber()); // 1
//nlopt::opt opt(nlopt::GN_MLSL, paramsNumber); // 2
//nlopt::opt opt(nlopt::GN_MLSL_LDS, paramsNumber); // 2
//nlopt::opt opt(nlopt::GN_ESCH, paramsNumber); // 3
//nlopt::opt opt(nlopt::GN_DIRECT_L, paramsNumber); // 4
//nlopt::opt opt(nlopt::GN_DIRECT, paramsNumber); // 4
//nlopt::opt opt(nlopt::GN_ORIG_DIRECT, paramsNumber); // 99
//nlopt::opt opt(nlopt::GN_ISRES, paramsNumber); // 99
*/