void
MetricBundleOptimizerBase::poseDerivatives(int i, int j, Vector3d& XX,
Matrix3x6d& d_dRT, Matrix3x3d& d_dX) const
{
XX = _cams[i].transformPointIntoCameraSpace(_Xs[j]);
// See Frank Dellaerts bundle adjustment tutorial.
// d(dR * R0 * X + t)/d omega = -[R0 * X]_x
Matrix3x3d J;
makeCrossProductMatrix(XX - _cams[i].getTranslation(), J);
scaleMatrixIP(-1.0, J);
// Now the transformation from world coords into camera space is xx = Rx + T
// Hence the derivative of x wrt. T is just the identity matrix.
makeIdentityMatrix(d_dRT);
copyMatrixSlice(J, 0, 0, 3, 3, d_dRT, 0, 3);
// The derivative of Rx+T wrt x is just R.
copyMatrix(_cams[i].getRotation(), d_dX);
} // end MetricBundleOptimizerBase::poseDerivatives()
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()
void
computeConsistentRotations(int const nViews,
std::vector<Matrix3x3d> const& relativeRotations,
std::vector<std::pair<int, int> > const& viewPairs,
std::vector<Matrix3x3d>& rotations, int method)
{
#if !defined(V3DLIB_ENABLE_ARPACK)
if (method == V3D_CONSISTENT_ROTATION_METHOD_SPARSE_EIG)
method = V3D_CONSISTENT_ROTATION_METHOD_EIG_ATA;
#endif
int const nRelPoses = relativeRotations.size();
rotations.resize(nViews);
switch (method)
{
case V3D_CONSISTENT_ROTATION_METHOD_SVD:
{
Matrix<double> A(3*nRelPoses, 3*nViews, 0.0);
Matrix3x3d I;
makeIdentityMatrix(I);
scaleMatrixIP(-1.0, I);
for (int i = 0; i < nRelPoses; ++i)
{
int const view1 = viewPairs[i].first;
int const view2 = viewPairs[i].second;
Matrix3x3d const& Rrel = relativeRotations[i];
copyMatrixSlice(Rrel, 0, 0, 3, 3, A, 3*i, 3*view1);
copyMatrixSlice(I, 0, 0, 3, 3, A, 3*i, 3*view2);
} // end for (i)
SVD<double> svd(A);
int const startColumn = A.num_cols()-3; // last columns of right sing. vec for SVD
Matrix<double> const& V = svd.getV();
for (int i = 0; i < nViews; ++i)
{
copyMatrixSlice(V, 3*i, startColumn, 3, 3, rotations[i], 0, 0);
enforceRotationMatrix(rotations[i]);
}
break;
}
case V3D_CONSISTENT_ROTATION_METHOD_SVD_ATA:
case V3D_CONSISTENT_ROTATION_METHOD_EIG_ATA:
case V3D_CONSISTENT_ROTATION_METHOD_SPARSE_EIG:
{
vector<pair<int, int> > nzA;
vector<double> valsA;
nzA.reserve(12*nRelPoses);
valsA.reserve(12*nRelPoses);
for (int i = 0; i < nRelPoses; ++i)
{
int const view1 = viewPairs[i].first;
int const view2 = viewPairs[i].second;
Matrix3x3d const& Rrel = relativeRotations[i];
nzA.push_back(make_pair(3*i+0, 3*view1+0)); valsA.push_back(Rrel[0][0]);
nzA.push_back(make_pair(3*i+0, 3*view1+1)); valsA.push_back(Rrel[0][1]);
nzA.push_back(make_pair(3*i+0, 3*view1+2)); valsA.push_back(Rrel[0][2]);
nzA.push_back(make_pair(3*i+1, 3*view1+0)); valsA.push_back(Rrel[1][0]);
nzA.push_back(make_pair(3*i+1, 3*view1+1)); valsA.push_back(Rrel[1][1]);
nzA.push_back(make_pair(3*i+1, 3*view1+2)); valsA.push_back(Rrel[1][2]);
nzA.push_back(make_pair(3*i+2, 3*view1+0)); valsA.push_back(Rrel[2][0]);
nzA.push_back(make_pair(3*i+2, 3*view1+1)); valsA.push_back(Rrel[2][1]);
nzA.push_back(make_pair(3*i+2, 3*view1+2)); valsA.push_back(Rrel[2][2]);
nzA.push_back(make_pair(3*i+0, 3*view2+0)); valsA.push_back(-1.0);
nzA.push_back(make_pair(3*i+1, 3*view2+1)); valsA.push_back(-1.0);
nzA.push_back(make_pair(3*i+2, 3*view2+2)); valsA.push_back(-1.0);
} // end for (i)
CCS_Matrix<double> A(3*nRelPoses, 3*nViews, nzA, valsA);
if (method == V3D_CONSISTENT_ROTATION_METHOD_SPARSE_EIG)
{
#if defined(V3DLIB_ENABLE_ARPACK)
Vector<double> sigma;
Matrix<double> V;
SparseSymmetricEigConfig cfg;
cfg.maxArnoldiIterations = 100000;
computeSparseSVD(A, V3D_ARPACK_SMALLEST_MAGNITUDE_EIGENVALUES, 3, sigma, V, cfg);
//computeSparseSVD(A, V3D_ARPACK_SMALLEST_EIGENVALUES, 3, sigma, V, cfg);
for (int i = 0; i < nViews; ++i)
{
copyMatrixSlice(V, 3*i, 0, 3, 1, rotations[i], 0, 2);
copyMatrixSlice(V, 3*i, 1, 3, 1, rotations[i], 0, 1);
copyMatrixSlice(V, 3*i, 2, 3, 1, rotations[i], 0, 0);
}
#endif
}
else
{
Matrix<double> AtA(3*nViews, 3*nViews);
multiply_At_A_SparseDense(A, AtA);
if (method == V3D_CONSISTENT_ROTATION_METHOD_SVD_ATA)
{
SVD<double> svd(AtA);
int const startColumn = A.num_cols()-3; // last columns of right sing. vec for SVD
Matrix<double> const& V = svd.getV();
for (int i = 0; i < nViews; ++i)
copyMatrixSlice(V, 3*i, startColumn, 3, 3, rotations[i], 0, 0);
}
else
{
Eigenvalue<double> svd(AtA);
int const startColumn = 0; // first columns of eigenvector matrix
Matrix<double> const& V = svd.getV();
for (int i = 0; i < nViews; ++i)
copyMatrixSlice(V, 3*i, startColumn, 3, 3, rotations[i], 0, 0);
} // end if
} // end if
break;
}
default:
throwV3DErrorHere("Unknown method argument for computeConsistentRotations().");
} // end switch
for (int i = 0; i < nViews; ++i)
enforceRotationMatrix(rotations[i]);
// Remove gauge freedom by setting R[0] = I.
Matrix3x3d const R0t = rotations[0].transposed();
for (int i = 0; i < nViews; ++i)
rotations[i] = rotations[i] * R0t;
// Note: it seems, that either all Rs have det(R)=1 or all have det(R)=-1.
// Since we remove the gauge freedem by multiplying all rotations with R_0^t,
// we always end up with det(R)=1 and any code to enforce a positive determinant
// is not necessary.
} // end computeConsistentRotations()