//computes X'X where
MatrixXd XtX(const MatrixXd& xx) {
const int n(xx.cols());
MatrixXd AtA(MatrixXd(n, n).setZero().
selfadjointView<Lower>().rankUpdate(xx.adjoint()));
return (AtA);
}
void solveLinear(Double_t eps = 1.e-12)
{
cout << "Perform the fit y = c0 + c1 * x in four different ways" << endl;
const Int_t nrVar = 2;
const Int_t nrPnts = 4;
Double_t ax[] = {0.0,1.0,2.0,3.0};
Double_t ay[] = {1.4,1.5,3.7,4.1};
Double_t ae[] = {0.5,0.2,1.0,0.5};
// Make the vectors 'Use" the data : they are not copied, the vector data
// pointer is just set appropriately
TVectorD x; x.Use(nrPnts,ax);
TVectorD y; y.Use(nrPnts,ay);
TVectorD e; e.Use(nrPnts,ae);
TMatrixD A(nrPnts,nrVar);
TMatrixDColumn(A,0) = 1.0;
TMatrixDColumn(A,1) = x;
cout << " - 1. solve through Normal Equations" << endl;
const TVectorD c_norm = NormalEqn(A,y,e);
cout << " - 2. solve through SVD" << endl;
// numerically preferred method
// first bring the weights in place
TMatrixD Aw = A;
TVectorD yw = y;
for (Int_t irow = 0; irow < A.GetNrows(); irow++) {
TMatrixDRow(Aw,irow) *= 1/e(irow);
yw(irow) /= e(irow);
}
TDecompSVD svd(Aw);
Bool_t ok;
const TVectorD c_svd = svd.Solve(yw,ok);
cout << " - 3. solve with pseudo inverse" << endl;
const TMatrixD pseudo1 = svd.Invert();
TVectorD c_pseudo1 = yw;
c_pseudo1 *= pseudo1;
cout << " - 4. solve with pseudo inverse, calculated brute force" << endl;
TMatrixDSym AtA(TMatrixDSym::kAtA,Aw);
const TMatrixD pseudo2 = AtA.Invert() * Aw.T();
TVectorD c_pseudo2 = yw;
c_pseudo2 *= pseudo2;
cout << " - 5. Minuit through TGraph" << endl;
TGraphErrors *gr = new TGraphErrors(nrPnts,ax,ay,0,ae);
TF1 *f1 = new TF1("f1","pol1",0,5);
gr->Fit("f1","Q");
TVectorD c_graph(nrVar);
c_graph(0) = f1->GetParameter(0);
c_graph(1) = f1->GetParameter(1);
// Check that all 4 answers are identical within a certain
// tolerance . The 1e-12 is somewhat arbitrary . It turns out that
// the TGraph fit is different by a few times 1e-13.
Bool_t same = kTRUE;
same &= VerifyVectorIdentity(c_norm,c_svd,0,eps);
same &= VerifyVectorIdentity(c_norm,c_pseudo1,0,eps);
same &= VerifyVectorIdentity(c_norm,c_pseudo2,0,eps);
same &= VerifyVectorIdentity(c_norm,c_graph,0,eps);
if (same)
cout << " All solutions are the same within tolerance of " << eps << endl;
else
cout << " Some solutions differ more than the allowed tolerance of " << eps << endl;
}
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()
