23 mvImageSize = vImageSize;
34 mvFocal[0] = mvImageSize[0] * mgvvCameraParams[0];
35 mvFocal[1] = mvImageSize[1] * (mgvvCameraParams)[1];
36 mvCenter[0] = mvImageSize[0] * (mgvvCameraParams)[2] - 0.5;
37 mvCenter[1] = mvImageSize[1] * (mgvvCameraParams)[3] - 0.5;
40 mvInvFocal[0] = 1.0 / mvFocal[0];
41 mvInvFocal[1] = 1.0 / mvFocal[1];
44 mdW = (mgvvCameraParams)[4];
47 md2Tan = 2.0 * tan(mdW / 2.0);
48 mdOneOver2Tan = 1.0 / md2Tan;
50 mdDistortionEnabled = 1.0;
56 mdDistortionEnabled = 0.0;
61 v2[0]= max((mgvvCameraParams)[2], 1.0 - (mgvvCameraParams)[2]) / (mgvvCameraParams)[0];
62 v2[1]= max((mgvvCameraParams)[3], 1.0 - (mgvvCameraParams)[3]) / (mgvvCameraParams)[1];
63 mdLargestRadius = invrtrans(sqrt(v2*v2));
66 mdMaxR = 1.5 * mdLargestRadius;
71 Vector<2> v2Center = UnProject(mvImageSize / 2);
73 Vector<2> v2Diff = v2Center - v2RootTwoAway;
74 mdOnePixelDist = sqrt(v2Diff * v2Diff) / sqrt(2.0);
80 vector<Vector<2> > vv2Verts;
81 vv2Verts.push_back(UnProject(
makeVector( -0.5, -0.5)));
82 vv2Verts.push_back(UnProject(
makeVector( mvImageSize[0]-0.5, -0.5)));
83 vv2Verts.push_back(UnProject(
makeVector( mvImageSize[0]-0.5, mvImageSize[1]-0.5)));
84 vv2Verts.push_back(UnProject(
makeVector( -0.5, mvImageSize[1]-0.5)));
87 for(
int i=0; i<4; i++)
88 for(
int j=0; j<2; j++)
90 if(vv2Verts[i][j] < v2Min[j]) v2Min[j] = vv2Verts[i][j];
91 if(vv2Verts[i][j] > v2Max[j]) v2Max[j] = vv2Verts[i][j];
98 mvUFBLinearInvFocal = v2Range;
99 mvUFBLinearFocal[0] = 1.0 / mvUFBLinearInvFocal[0];
100 mvUFBLinearFocal[1] = 1.0 / mvUFBLinearInvFocal[1];
101 mvUFBLinearCenter[0] = -1.0 * v2Min[0] * mvUFBLinearFocal[0];
102 mvUFBLinearCenter[1] = -1.0 * v2Min[1] * mvUFBLinearFocal[1];
111 mdLastR = sqrt(vCam * vCam);
112 mbInvalid = (mdLastR > mdMaxR);
113 mdLastFactor = rtrans_factor(mdLastR);
114 mdLastDistR = mdLastFactor * mdLastR;
115 mvLastDistCam = mdLastFactor * mvLastCam;
117 mvLastIm[0] = mvCenter[0] + mvFocal[0] * mvLastDistCam[0];
118 mvLastIm[1] = mvCenter[1] + mvFocal[1] * mvLastDistCam[1];
128 mvLastDistCam[0] = (mvLastIm[0] - mvCenter[0]) * mvInvFocal[0];
129 mvLastDistCam[1] = (mvLastIm[1] - mvCenter[1]) * mvInvFocal[1];
130 mdLastDistR = sqrt(mvLastDistCam * mvLastDistCam);
131 mdLastR = invrtrans(mdLastDistR);
133 if(mdLastDistR > 0.01)
134 dFactor = mdLastR / mdLastDistR;
137 mdLastFactor = 1.0 / dFactor;
138 mvLastCam = dFactor * mvLastDistCam;
149 double left = mvImplaneTL[0] * near;
150 double right = mvImplaneBR[0] * near;
151 double top = mvImplaneTL[1] * near;
152 double bottom = mvImplaneBR[1] * near;
159 m4[0][0] = (2 * near) / (right - left);
160 m4[1][1] = (2 * near) / (top - bottom);
162 m4[0][2] = (right + left) / (left - right);
163 m4[1][2] = (top + bottom) / (bottom - top);
164 m4[2][2] = (far + near) / (far - near);
167 m4[2][3] = 2*near*far / (near - far);
182 double &x = mvLastCam[0];
183 double &y = mvLastCam[1];
184 double r = mdLastR * mdDistortionEnabled;
194 mdWinv * (k * x) / (r*r*(1 + k*k*r*r)) - x * mdLastFactor / (r*r);
196 mdWinv * (k * y) / (r*r*(1 + k*k*r*r)) - y * mdLastFactor / (r*r);
201 m2Derivs[0][0] = mvFocal[0] * (dFracBydx * x + mdLastFactor);
202 m2Derivs[1][0] = mvFocal[1] * (dFracBydx * y);
203 m2Derivs[0][1] = mvFocal[0] * (dFracBydy * x);
204 m2Derivs[1][1] = mvFocal[1] * (dFracBydy * y + mdLastFactor);
220 if(i == NUMTRACKERCAMPARAMETERS-1 && mdW == 0.0)
224 vNUpdate[i] += 0.001;
225 UpdateParams(vNUpdate);
227 m2NNumDerivs.T()[i] = (v2Out_B - v2Out) / 0.001;
228 mgvvCameraParams = vNNormal;
232 m2NNumDerivs.T()[NUMTRACKERCAMPARAMETERS-1] =
Zeros;
239 (mgvvCameraParams) = (mgvvCameraParams) + vUpdate;
255 mdLastR = sqrt(vCam * vCam);
256 mbInvalid = (mdLastR > mdMaxR);
257 mdLastFactor = rtrans_factor(mdLastR);
258 mdLastDistR = mdLastFactor * mdLastR;
259 mvLastDistCam = mdLastFactor * mvLastCam;
261 mvLastIm[0] = (mgvvCameraParams)[2] + (mgvvCameraParams)[0] * mvLastDistCam[0];
262 mvLastIm[1] = (mgvvCameraParams)[3] + (mgvvCameraParams)[1] * mvLastDistCam[1];
269 mvLastDistCam[0] = (mvLastIm[0] - (mgvvCameraParams)[2]) / (mgvvCameraParams)[0];
270 mvLastDistCam[1] = (mvLastIm[1] - (mgvvCameraParams)[3]) / (mgvvCameraParams)[1];
271 mdLastDistR = sqrt(mvLastDistCam * mvLastDistCam);
272 mdLastR = invrtrans(mdLastDistR);
274 if(mdLastDistR > 0.01)
275 dFactor = mdLastR / mdLastDistR;
278 mdLastFactor = 1.0 / dFactor;
279 mvLastCam = dFactor * mvLastDistCam;
void DisableRadialDistortion()
Vector< 2 > Project(const Vector< 2 > &camframe)
TooN::Vector< 2 > vec(const ImageRef &ir)
Vector< 2 > UnProject(const Vector< 2 > &imframe)
Vector< 2 > UFBUnProject(const Vector< 2 > &camframe)
Matrix< 4 > MakeUFBLinearFrustumMatrix(double near, double far)
static const Vector< NUMTRACKERCAMPARAMETERS > mvDefaultParams
Vector< 1 > makeVector(double x1)
Vector< 2 > UFBProject(const Vector< 2 > &camframe)
void SetImageSize(Vector< 2 > v2ImageSize)
Matrix< 2, NUMTRACKERCAMPARAMETERS > GetCameraParameterDerivs()
ATANCamera(std::string sName)
#define NUMTRACKERCAMPARAMETERS
void UpdateParams(Vector< NUMTRACKERCAMPARAMETERS > vUpdate)
Matrix< 2, 2 > GetProjectionDerivs()
static Operator< Internal::Zero > Zeros