void
StdMetricBundleOptimizer::fillJacobians(Matrix<double>& Ak,
Matrix<double>& Bk,
Matrix<double>& Ck,
int i, int j, int k)
{
Vector3d XX;
Matrix3x6d d_dRT;
Matrix3x3d d_dX;
this->poseDerivatives(i, j, XX, d_dRT, d_dX);
double const f = _cams[i].getFocalLength();
double const ar = _cams[i].getAspectRatio();
Matrix2x3d dp_dX;
double const bx = f / (XX[2] * XX[2]);
double const by = ar * bx;
dp_dX[0][0] = bx * XX[2]; dp_dX[0][1] = 0; dp_dX[0][2] = -bx * XX[0];
dp_dX[1][0] = 0; dp_dX[1][1] = by * XX[2]; dp_dX[1][2] = -by * XX[1];
multiply_A_B(dp_dX, d_dRT, Ak);
multiply_A_B(dp_dX, d_dX, Bk);
} // end StdMetricBundleOptimizer::fillJacobians()
void
StereoMetricBundleOptimizer::fillJacobians(Matrix<double>& Ak, Matrix<double>& Bk, Matrix<double>& Ck,
int i, int j, int k)
{
int const l = _correspondingSubCamera[k];
Vector3d XX;
Matrix3x6d d_dRT;
Matrix3x3d d_dX;
::poseDerivatives(_rotations[i], _translations[i], _rigCameras[l], _Xs[j], XX, d_dRT, d_dX);
double const f = _rigCameras[l].getFocalLength();
double const ar = _rigCameras[l].getAspectRatio();
Matrix2x3d dp_dX;
double const bx = f / (XX[2] * XX[2]);
double const by = ar * bx;
dp_dX[0][0] = bx * XX[2]; dp_dX[0][1] = 0; dp_dX[0][2] = -bx * XX[0];
dp_dX[1][0] = 0; dp_dX[1][1] = by * XX[2]; dp_dX[1][2] = -by * XX[1];
multiply_A_B(dp_dX, d_dRT, Ak);
multiply_A_B(dp_dX, d_dX, Bk);
} // end StereoMetricBundleOptimizer::fillJacobians()
void
CommonInternalsMetricBundleOptimizer::fillJacobians(Matrix<double>& Ak,
Matrix<double>& Bk,
Matrix<double>& Ck,
int i, int j, int k)
{
double const focalLength = _K[0][0];
Vector3d XX;
Matrix3x6d dXX_dRT;
Matrix3x3d dXX_dX;
this->poseDerivatives(i, j, XX, dXX_dRT, dXX_dX);
Vector2d xu; // undistorted image point
xu[0] = XX[0] / XX[2];
xu[1] = XX[1] / XX[2];
Vector2d const xd = _distortion(xu); // distorted image point
Matrix2x2d dp_dxd;
dp_dxd[0][0] = focalLength; dp_dxd[0][1] = 0;
dp_dxd[1][0] = 0; dp_dxd[1][1] = _cachedAspectRatio * focalLength;
{
// First, lets do the derivative wrt the structure and motion parameters.
Matrix2x3d dxu_dXX;
dxu_dXX[0][0] = 1.0f / XX[2]; dxu_dXX[0][1] = 0; dxu_dXX[0][2] = -XX[0]/(XX[2]*XX[2]);
dxu_dXX[1][0] = 0; dxu_dXX[1][1] = 1.0f / XX[2]; dxu_dXX[1][2] = -XX[1]/(XX[2]*XX[2]);
Matrix2x2d dxd_dxu = _distortion.derivativeWrtUndistortedPoint(xu);
Matrix2x2d dp_dxu = dp_dxd * dxd_dxu;
Matrix2x3d dp_dXX = dp_dxu * dxu_dXX;
multiply_A_B(dp_dXX, dXX_dRT, Ak);
multiply_A_B(dp_dXX, dXX_dX, Bk);
} // end scope
switch (_mode)
{
case FULL_BUNDLE_FOCAL_AND_RADIAL_K1:
{
// Focal length.
Ck[0][0] = xd[0];
Ck[1][0] = xd[1];
// For radial, k1 only.
Matrix2x2d dxd_dk1k2 = _distortion.derivativeWrtRadialParameters(xu);
Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2;
Ck[0][1] = d_dk1k2[0][0];
Ck[1][1] = d_dk1k2[1][0];
break;
}
case FULL_BUNDLE_FOCAL_AND_RADIAL:
{
// Focal length.
Ck[0][0] = xd[0];
Ck[1][0] = xd[1];
// Radial k1 and k2.
Matrix2x2d dxd_dk1k2 = _distortion.derivativeWrtRadialParameters(xu);
Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2;
copyMatrixSlice(d_dk1k2, 0, 0, 2, 2, Ck, 0, 1);
break;
}
case FULL_BUNDLE_RADIAL_TANGENTIAL:
{
Matrix2x2d dxd_dp1p2 = _distortion.derivativeWrtTangentialParameters(xu);
Matrix2x2d d_dp1p2 = dp_dxd * dxd_dp1p2;
copyMatrixSlice(d_dp1p2, 0, 0, 2, 2, Ck, 0, 5);
// No break here!
}
case FULL_BUNDLE_RADIAL:
{
Matrix2x2d dxd_dk1k2 = _distortion.derivativeWrtRadialParameters(xu);
Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2;
copyMatrixSlice(d_dk1k2, 0, 0, 2, 2, Ck, 0, 3);
// No break here!
}
case FULL_BUNDLE_FOCAL_LENGTH_PP:
{
Ck[0][1] = 1; Ck[0][2] = 0;
Ck[1][1] = 0; Ck[1][2] = 1;
// No break here!
}
case FULL_BUNDLE_FOCAL_LENGTH:
{
Ck[0][0] = xd[0];
Ck[1][0] = xd[1];
}
case FULL_BUNDLE_METRIC:
{
}
} // end switch
} // end CommonInternalsMetricBundleOptimizer::fillJacobians()
void
VaryingInternalsMetricBundleOptimizer::fillJacobians(Matrix<double>& Ak,
Matrix<double>& Bk,
Matrix<double>& Ck,
int i, int j, int k)
{
Vector3d XX;
Matrix3x6d dXX_dRT;
Matrix3x3d dXX_dX;
this->poseDerivatives(i, j, XX, dXX_dRT, dXX_dX);
Vector2d xu; // undistorted image point
xu[0] = XX[0] / XX[2];
xu[1] = XX[1] / XX[2];
Vector2d const xd = _distortions[i](xu); // distorted image point
double const focalLength = _cams[i].getFocalLength();
double const aspectRatio = _cams[i].getAspectRatio();
Matrix2x2d dp_dxd;
dp_dxd[0][0] = focalLength; dp_dxd[0][1] = 0;
dp_dxd[1][0] = 0; dp_dxd[1][1] = aspectRatio * focalLength;
{
// First, lets do the derivative wrt the structure and motion parameters.
Matrix2x3d dxu_dXX;
dxu_dXX[0][0] = 1.0f / XX[2]; dxu_dXX[0][1] = 0; dxu_dXX[0][2] = -XX[0]/(XX[2]*XX[2]);
dxu_dXX[1][0] = 0; dxu_dXX[1][1] = 1.0f / XX[2]; dxu_dXX[1][2] = -XX[1]/(XX[2]*XX[2]);
Matrix2x2d dxd_dxu = _distortions[i].derivativeWrtUndistortedPoint(xu);
Matrix2x2d dp_dxu = dp_dxd * dxd_dxu;
Matrix2x3d dp_dXX = dp_dxu * dxu_dXX;
Matrix2x6d dp_dRT;
multiply_A_B(dp_dXX, dXX_dRT, dp_dRT);
copyMatrixSlice(dp_dRT, 0, 0, 2, 6, Ak, 0, 0);
multiply_A_B(dp_dXX, dXX_dX, Bk);
} // end scope
switch (_mode)
{
case FULL_BUNDLE_RADIAL_TANGENTIAL:
{
Matrix2x2d dxd_dp1p2 = _distortions[i].derivativeWrtTangentialParameters(xu);
Matrix2x2d d_dp1p2 = dp_dxd * dxd_dp1p2;
copyMatrixSlice(d_dp1p2, 0, 0, 2, 2, Ak, 0, 11);
// No break here!
}
case FULL_BUNDLE_RADIAL:
{
Matrix2x2d dxd_dk1k2 = _distortions[i].derivativeWrtRadialParameters(xu);
Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2;
copyMatrixSlice(d_dk1k2, 0, 0, 2, 2, Ak, 0, 9);
// No break here!
}
case FULL_BUNDLE_FOCAL_LENGTH_PP:
{
Ak[0][7] = 1; Ak[0][8] = 0;
Ak[1][7] = 0; Ak[1][8] = 1;
// No break here!
}
case FULL_BUNDLE_FOCAL_LENGTH:
{
Ak[0][6] = xd[0];
Ak[1][6] = xd[1];
}
case FULL_BUNDLE_METRIC:
{
}
} // end switch
} // end VaryingInternalsMetricBundleOptimizer::fillJacobians()
Matrix3x3d
computeIntersectionCovariance(vector<Matrix3x4d> const& projections,
vector<PointMeasurement> const& measurements,
double sigma)
{
Matrix<double> Cp(3, 3, 0.0);
Cp[0][0] = Cp[1][1] = sigma;
Cp[2][2] = 0.0;
int const N = measurements.size();
Matrix<double> A(2*N, 4, 0.0);
InlineMatrix<double, 2, 3> Sp;
makeZeroMatrix(Sp);
InlineMatrix<double, 2, 4> Sp_P;
for (int i = 0; i < N; ++i)
{
Sp[0][1] = -1; Sp[0][2] = measurements[i].pos[1];
Sp[1][0] = 1; Sp[1][2] = -measurements[i].pos[0];
int const view = measurements[i].view;
multiply_A_B(Sp, projections[view], Sp_P);
A[2*i+0][0] = Sp_P[0][0]; A[2*i+0][1] = Sp_P[0][1]; A[2*i+0][2] = Sp_P[0][2]; A[2*i+0][3] = Sp_P[0][3];
A[2*i+1][0] = Sp_P[1][0]; A[2*i+1][1] = Sp_P[1][1]; A[2*i+1][2] = Sp_P[1][2]; A[2*i+1][3] = Sp_P[1][3];
} // end for (i)
SVD<double> svd(A);
Matrix<double> V;
svd.getV(V);
Vector4d X;
X[0] = V[0][3]; X[1] = V[1][3]; X[2] = V[2][3]; X[3] = V[3][3];
Vector3d P;
Matrix<double> S(2, 3, 0.0);
Matrix<double> B(2*N, 3*N, 0.0);
for (int i = 0; i < N; ++i)
{
int const view = measurements[i].view;
multiply_A_v(projections[view], X, P);
P[0] /= P[2]; P[1] /= P[2]; P[2] = 1.0;
S[0][1] = -P[2]; S[0][2] = P[1];
S[1][0] = P[2]; S[1][2] = -P[0];
B[2*i+0][3*i+0] = -S[0][0]; B[2*i+0][3*i+1] = -S[0][1]; B[2*i+0][3*i+2] = S[0][2];
B[2*i+1][3*i+0] = -S[1][0]; B[2*i+1][3*i+1] = -S[1][1]; B[2*i+1][3*i+2] = S[1][2];
} // end for (i)
Matrix<double> C(3*N, 3*N, 0.0);
for (int i = 0; i < N; ++i)
{
C[3*i+0][3*i+0] = Cp[0][0]; C[3*i+0][3*i+1] = Cp[0][1]; C[3*i+0][3*i+2] = Cp[0][2];
C[3*i+1][3*i+0] = Cp[1][0]; C[3*i+1][3*i+1] = Cp[1][1]; C[3*i+1][3*i+2] = Cp[1][2];
C[3*i+2][3*i+0] = Cp[2][0]; C[3*i+2][3*i+1] = Cp[2][1]; C[3*i+2][3*i+2] = Cp[2][2];
} // end for (i)
Matrix<double> B_C(2*N, 3*N);
multiply_A_B(B, C, B_C);
Matrix<double> T(2*N, 2*N);
multiply_A_Bt(B_C, B, T);
Matrix<double> Tinv;
invertMatrix(T, Tinv);
Matrix<double> NN(5, 5), N4(4, 4);
Matrix<double> At_Tinv(4, 2*N);
multiply_At_B(A, Tinv, At_Tinv);
multiply_A_B(At_Tinv, A, N4);
for (int r = 0; r < 4; ++r)
for (int c = 0; c < 4; ++c)
NN[r][c] = N4[r][c];
NN[0][4] = NN[4][0] = X[0];
NN[1][4] = NN[4][1] = X[1];
NN[2][4] = NN[4][2] = X[2];
NN[3][4] = NN[4][3] = X[3];
NN[4][4] = 0.0;
Matrix<double> Ninv(5, 5);
invertMatrix(NN, Ninv);
Matrix4x4d sigma_XX;
for (int r = 0; r < 4; ++r)
for (int c = 0; c < 4; ++c)
sigma_XX[r][c] = Ninv[r][c];
Matrix3x4d Je;
makeZeroMatrix(Je);
Je[0][0] = Je[1][1] = Je[2][2] = 1.0 / X[3];
Je[0][3] = -X[0] / (X[3]*X[3]);
Je[1][3] = -X[1] / (X[3]*X[3]);
Je[2][3] = -X[2] / (X[3]*X[3]);
Matrix3x3d sigma_X = Je * sigma_XX * Je.transposed();
return sigma_X;
}
