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 computeConsistentRotations_LSQ(int const nIterations, int const nViews, std::vector<Matrix3x3d> const& relativeRotations, std::vector<std::pair<int, int> > const& viewPairs, std::vector<Matrix3x3d>& rotations) { double const gamma = 1.0; int const nRelPoses = relativeRotations.size(); rotations.resize(nViews); Matrix3x3d zero3x3d; makeZeroMatrix(zero3x3d); Matrix4x4d zeroQuat; makeZeroMatrix(zeroQuat); zeroQuat[0][0] = 1; vector<double> denomQ(nViews, 1.0); // from the psd constraint for (int k = 0; k < nRelPoses; ++k) { int const i = viewPairs[k].first; int const j = viewPairs[k].second; denomQ[i] += 1; denomQ[j] += 1; } for (int i = 0; i < nViews; ++i) denomQ[i] = 1.0 / denomQ[i]; vector<Matrix4x4d> Q(nViews, zeroQuat); vector<Matrix4x4d> Q1(nViews, zeroQuat); vector<Matrix4x4d> ZQ1(nViews, zeroQuat); vector<Matrix4x4d> Q2i(nRelPoses, zeroQuat); vector<Matrix4x4d> Q2j(nRelPoses, zeroQuat); vector<Matrix4x4d> ZQ2i(nRelPoses, zeroQuat); vector<Matrix4x4d> ZQ2j(nRelPoses, zeroQuat); vector<Matrix3x3d> A(nRelPoses, zero3x3d); vector<Matrix3x3d> A1(nRelPoses, zero3x3d); vector<Matrix3x3d> A2(nRelPoses, zero3x3d); vector<Matrix3x3d> ZA1(nRelPoses, zero3x3d); vector<Matrix3x3d> ZA2(nRelPoses, zero3x3d); for (int iter = 0; iter < nIterations; ++iter) { // Convex hull of rotation matrices for (int i = 0; i < nViews; ++i) { Matrix4x4d q = Q[i] + ZQ1[i]; projectConvHull_SO3(q); Q1[i] = q; addMatricesIP(Q[i] - q, ZQ1[i]); } // end for (i) // Squared Frobenius term 0.5*sum_k |A_k|_F^2 for (int k = 0; k < nRelPoses; ++k) { Matrix3x3d a = A[k] + ZA1[k]; scaleMatrixIP(1.0 / (1.0 + gamma), a); A1[k] = a; addMatricesIP(A[k] - a, ZA1[k]); } // end for (k) // Enforce linear consistency for (int k = 0; k < nRelPoses; ++k) { int const i = viewPairs[k].first; int const j = viewPairs[k].second; Matrix4x4d qi = Q[i] + ZQ2i[k]; Matrix4x4d qj = Q[j] + ZQ2j[k]; Matrix3x3d a = A[k] + ZA2[k]; proxConsistency(relativeRotations[k], qi, qj, a); Q2i[k] = qi; Q2j[k] = qj; A2[k] = a; addMatricesIP(Q[i] - qi, ZQ2i[k]); addMatricesIP(Q[j] - qj, ZQ2j[k]); addMatricesIP(A[k] - a, ZA2[k]); } // end for (k) // Averaging of the solutions for (int i = 0; i < nViews; ++i) Q[i] = Q1[i] - ZQ1[i]; for (int k = 0; k < nRelPoses; ++k) A[k] = A1[k] - ZA1[k]; for (int k = 0; k < nRelPoses; ++k) { int const i = viewPairs[k].first; int const j = viewPairs[k].second; addMatricesIP(Q2i[k] - ZQ2i[k], Q[i]); addMatricesIP(Q2j[k] - ZQ2j[k], Q[j]); addMatricesIP(A2[k] - ZA2[k], A[k]); } // end for (k) for (int i = 0; i < nViews; ++i) scaleMatrixIP(denomQ[i], Q[i]); for (int k = 0; k < nRelPoses; ++k) scaleMatrixIP(0.5, A[k]); if ((iter % 500) == 0) { double E = 0; for (int k = 0; k < nRelPoses; ++k) E += sqrMatrixNormFrobenius(A[k]); cout << "iter: " << iter << " E = " << E << endl; } } // end for (iter) rotations.resize(nViews); for (int i = 0; i < nViews; ++i) rotations[i] = getRotationFromQuat(Q[i]); } // end computeConsistentRotations_LSQ()
void computeConsistentRotations_Linf(double const sigma, int const nIterations, int const nViews, std::vector<Matrix3x3d> const& relativeRotations, std::vector<std::pair<int, int> > const& viewPairs, std::vector<Matrix3x3d>& rotations, std::vector<double>& zs) { double const gamma = 1.0; int const nRelPoses = relativeRotations.size(); rotations.resize(nViews); Matrix3x3d zero3x3d; makeZeroMatrix(zero3x3d); Matrix4x4d zeroQuat; makeZeroMatrix(zeroQuat); zeroQuat[0][0] = 1; double const denomT = 1.0 / (1.0 + nRelPoses); vector<double> denomQ(nViews, 1.0); // from the psd constraint for (int k = 0; k < nRelPoses; ++k) { int const i = viewPairs[k].first; int const j = viewPairs[k].second; denomQ[i] += 1; denomQ[j] += 1; } for (int i = 0; i < nViews; ++i) denomQ[i] = 1.0 / denomQ[i]; double T = 0.0; vector<double> T1(nRelPoses); vector<double> ZT1(nRelPoses, 0.0); double T2; double ZT2 = 0; vector<Matrix4x4d> Q(nViews, zeroQuat); vector<Matrix4x4d> Q1(nViews, zeroQuat); vector<Matrix4x4d> ZQ1(nViews, zeroQuat); vector<Matrix4x4d> Q2i(nRelPoses, zeroQuat); vector<Matrix4x4d> Q2j(nRelPoses, zeroQuat); vector<Matrix4x4d> ZQ2i(nRelPoses, zeroQuat); vector<Matrix4x4d> ZQ2j(nRelPoses, zeroQuat); vector<Matrix3x3d> A(nRelPoses, zero3x3d); vector<Matrix3x3d> A1(nRelPoses, zero3x3d); vector<Matrix3x3d> A2(nRelPoses, zero3x3d); vector<Matrix3x3d> ZA1(nRelPoses, zero3x3d); vector<Matrix3x3d> ZA2(nRelPoses, zero3x3d); for (int iter = 0; iter < nIterations; ++iter) { // Convex hull of rotation matrices for (int i = 0; i < nViews; ++i) { Matrix4x4d q = Q[i] + ZQ1[i]; if (i > 0) projectConvHull_SO3(q); else { makeZeroMatrix(q); q[0][0] = 1; } Q1[i] = q; addMatricesIP(Q[i] - q, ZQ1[i]); } // end for (i) // Shrinkage of T (we want to minimize T) T2 = std::max(0.0, T + ZT2 - gamma); ZT2 += T - T2; // Cone constraint for (int k = 0; k < nRelPoses; ++k) { double t = T + ZT1[k]; Matrix3x3d a = A[k] + ZA1[k]; proxDataResidual_Frobenius(sigma, t, a); T1[k] = t; ZT1[k] += T - t; A1[k] = a; addMatricesIP(A[k] - a, ZA1[k]); } // end for (k) // Enforce linear consistency for (int k = 0; k < nRelPoses; ++k) { int const i = viewPairs[k].first; int const j = viewPairs[k].second; Matrix4x4d qi = Q[i] + ZQ2i[k]; Matrix4x4d qj = Q[j] + ZQ2j[k]; Matrix3x3d a = A[k] + ZA2[k]; proxConsistency(relativeRotations[k], qi, qj, a); Q2i[k] = qi; Q2j[k] = qj; A2[k] = a; addMatricesIP(Q[i] - qi, ZQ2i[k]); addMatricesIP(Q[j] - qj, ZQ2j[k]); addMatricesIP(A[k] - a, ZA2[k]); } // end for (k) // Averaging of the solutions for (int i = 0; i < nViews; ++i) Q[i] = Q1[i] - ZQ1[i]; T = T2 - ZT2; for (int k = 0; k < nRelPoses; ++k) T += T1[k] - ZT1[k]; T *= denomT; T = std::max(0.0, T); for (int k = 0; k < nRelPoses; ++k) A[k] = A1[k] - ZA1[k]; for (int k = 0; k < nRelPoses; ++k) { int const i = viewPairs[k].first; int const j = viewPairs[k].second; addMatricesIP(Q2i[k] - ZQ2i[k], Q[i]); addMatricesIP(Q2j[k] - ZQ2j[k], Q[j]); addMatricesIP(A2[k] - ZA2[k], A[k]); } // end for (k) for (int i = 0; i < nViews; ++i) scaleMatrixIP(denomQ[i], Q[i]); for (int k = 0; k < nRelPoses; ++k) scaleMatrixIP(0.5, A[k]); if ((iter % 500) == 0) { cout << "iter: " << iter << " t = " << T << endl; cout << "T1 = "; displayVector(T1); cout << "ZT1 = "; displayVector(ZT1); cout << "T2 = " << T2 << " ZT2 = " << ZT2 << endl; Matrix<double> ZZ(4, 4); for (int i = 0; i < nViews; ++i) { copyMatrix(Q[i], ZZ); SVD<double> svd(ZZ); cout << "Q = "; displayMatrix(ZZ); cout << "SV = "; displayVector(svd.getSingularValues()); //Matrix3x3d R = getRotationFromQuat(Q[i]); //cout << "R = "; displayMatrix(R); } // end for (i) } } // end for (iter) rotations.resize(nViews); for (int i = 0; i < nViews; ++i) rotations[i] = getRotationFromQuat(Q[i]); zs = ZT1; } // end computeConsistentRotations_Linf()
void computeConsistentRotations_L1(double const sigma, int const nIterations, int const nViews, std::vector<Matrix3x3d> const& relativeRotations, std::vector<std::pair<int, int> > const& viewPairs, std::vector<Matrix3x3d>& rotations) { double const gamma = 1.0; int const nRelPoses = relativeRotations.size(); rotations.resize(nViews); Matrix3x3d zero3x3d; makeZeroMatrix(zero3x3d); Matrix4x4d zeroQuat; makeZeroMatrix(zeroQuat); zeroQuat[0][0] = 1; vector<double> denomQ(nViews, 1.0); // from the psd constraint for (int k = 0; k < nRelPoses; ++k) { int const i = viewPairs[k].first; int const j = viewPairs[k].second; denomQ[i] += 1; denomQ[j] += 1; } for (int i = 0; i < nViews; ++i) denomQ[i] = 1.0 / denomQ[i]; vector<double> T(nRelPoses, 0.0); vector<double> T1(nRelPoses); vector<double> ZT1(nRelPoses, 0.0); vector<double> T2(nRelPoses); vector<double> ZT2(nRelPoses, 0.0); vector<Matrix4x4d> Q(nViews, zeroQuat); vector<Matrix4x4d> Q1(nViews, zeroQuat); vector<Matrix4x4d> ZQ1(nViews, zeroQuat); vector<Matrix4x4d> Q2i(nRelPoses, zeroQuat); vector<Matrix4x4d> Q2j(nRelPoses, zeroQuat); vector<Matrix4x4d> ZQ2i(nRelPoses, zeroQuat); vector<Matrix4x4d> ZQ2j(nRelPoses, zeroQuat); vector<Matrix3x3d> A(nRelPoses, zero3x3d); vector<Matrix3x3d> A1(nRelPoses, zero3x3d); vector<Matrix3x3d> A2(nRelPoses, zero3x3d); vector<Matrix3x3d> ZA1(nRelPoses, zero3x3d); vector<Matrix3x3d> ZA2(nRelPoses, zero3x3d); for (int iter = 0; iter < nIterations; ++iter) { // Convex hull of rotation matrices for (int i = 0; i < nViews; ++i) { Matrix4x4d q = Q[i] + ZQ1[i]; projectConvHull_SO3(q); Q1[i] = q; addMatricesIP(Q[i] - q, ZQ1[i]); } // end for (i) // Shrinkage of T (we want to minimize T) for (int k = 0; k < nRelPoses; ++k) { T2[k] = std::max(0.0, T[k] + ZT2[k] - gamma); ZT2[k] += T[k] - T2[k]; } // end for (k) // Cone constraint for (int k = 0; k < nRelPoses; ++k) { double t = T1[k] + ZT1[k]; Matrix3x3d a = A[k] + ZA1[k]; proxDataResidual_Frobenius(sigma, t, a); T1[k] = t; ZT1[k] += T[k] - t; A1[k] = a; addMatricesIP(A[k] - a, ZA1[k]); } // end for (k) // Enforce linear consistency for (int k = 0; k < nRelPoses; ++k) { int const i = viewPairs[k].first; int const j = viewPairs[k].second; Matrix4x4d qi = Q[i] + ZQ2i[k]; Matrix4x4d qj = Q[j] + ZQ2j[k]; Matrix3x3d a = A[k] + ZA2[k]; proxConsistency(relativeRotations[k], qi, qj, a); Q2i[k] = qi; Q2j[k] = qj; A2[k] = a; addMatricesIP(Q[i] - qi, ZQ2i[k]); addMatricesIP(Q[j] - qj, ZQ2j[k]); addMatricesIP(A[k] - a, ZA2[k]); } // end for (k) // Averaging of the solutions for (int i = 0; i < nViews; ++i) Q[i] = Q1[i] - ZQ1[i]; for (int k = 0; k < nRelPoses; ++k) T[k] = std::max(0.0, 0.5 * (T1[k] - ZT1[k] + T2[k] - ZT2[k])); for (int k = 0; k < nRelPoses; ++k) A[k] = A1[k] - ZA1[k]; for (int k = 0; k < nRelPoses; ++k) { int const i = viewPairs[k].first; int const j = viewPairs[k].second; addMatricesIP(Q2i[k] - ZQ2i[k], Q[i]); addMatricesIP(Q2j[k] - ZQ2j[k], Q[j]); addMatricesIP(A2[k] - ZA2[k], A[k]); } // end for (k) for (int i = 0; i < nViews; ++i) scaleMatrixIP(denomQ[i], Q[i]); for (int k = 0; k < nRelPoses; ++k) scaleMatrixIP(0.5, A[k]); if ((iter % 500) == 0) { cout << "iter: " << iter << endl; cout << " T = "; displayVector(T); } } // end for (iter) rotations.resize(nViews); for (int i = 0; i < nViews; ++i) rotations[i] = getRotationFromQuat(Q[i]); } // end computeConsistentRotations_L1()
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()