Common Lines (find common lines between projections)

Collaboration diagram for Common Lines (find common lines between projections):

Classes
class	CommonLine
	Commonline. More...
class	ProgCommonLine
	CommonLine Parameters. More...
class	CommonLineInfo

Macros
#define	POW2(x)   (x*x)

Functions
void	randomQuaternions (int k, DMatrix &qArray)
void	saveMatrix (const char *fn, DMatrix &array)
void	quaternionToMatrix (const DVector &q, DMatrix &rotMatrix)
void	quaternionCommonLines (const DMatrix &quaternions, CommonLineInfo &clInfo)
void	commonlineMatrixCheat (const DMatrix &quaternions, size_t nRays, DMatrix &clMatrix, DMatrix &clCorr)
void	anglesRotationMatrix (size_t nRays, int i, int j, DMatrix &U)
int	tripletRotationMatrix (const DMatrix &clMatrix, size_t nRays, int k1, int k2, int k3, DMatrix &R)
void	computeSyncMatrix (const DMatrix &clMatrix, size_t nRays, DMatrix &sMatrix)
void	rotationsFromSyncMatrix (const DMatrix &sMatrix)

Variables
constexpr signed int	SMALL_TRIANGLE = -101

Detailed Description

Macro Definition Documentation

POW2

#define POW2

(

x

)

(x*x)

Definition at line 143 of file common_lines.h.

Function Documentation

anglesRotationMatrix()

void anglesRotationMatrix	(	size_t	nRays,
		int	i,
		int	j,
		DMatrix &	U
	)

commonlineMatrixCheat()

void commonlineMatrixCheat	(	const DMatrix &	quaternions,
		size_t	nRays,
		DMatrix &	clMatrix,
		DMatrix &	clCorr
	)

Definition at line 769 of file common_lines.cpp.

{
    int n = quaternions.Xdim();

    clMatrix.initConstant(n, n, 0);
    clCorr.initZeros(n, n);

    CommonLineInfo clInfo(nRays);

    for (int i = 0; i < n - 1; ++i)
        for (int j = i + 1; j < n; ++j)
        {
            clInfo.setImages(i, j);
            quaternionCommonLines(quaternions, clInfo);
            dMij(clMatrix, i, j) = clInfo.getIndex(0);
            dMij(clMatrix, j, i) = clInfo.getIndex(1);
            dMij(clCorr, i, j) = 1.0e-8;
        }

}//function commonlineMatrixCheat

computeSyncMatrix()

void computeSyncMatrix	(	const DMatrix &	clMatrix,
		size_t	nRays,
		DMatrix &	sMatrix
	)

Definition at line 900 of file common_lines.cpp.

{
    int K = clMatrix.Xdim();
    DMatrix J, R, S(3,3);
    J.initIdentity(3);
    dMij(J, 2, 2) = -1;
    int kk;
    int result = 0;

    sMatrix.initIdentity(2*K);

    for (int k1 = 0; k1 < K - 1; ++k1)
    {
        for (int k2 = k1 + 1; k2 < K; ++k2)
        {
            kk = 0; //% Each triplet (k1,k2,k3) defines some rotation matrix from
            //                % image k1 to image k2. kk counts the number of such
            //                % rotations, that is, the number of thrid images k3 that give
            //                % rise to a rotation from image k1 to image k2. Not all
            //                % images k3 give rise to such a rotation, since, for example
            //                % the resuling triangle can be too small.

            //For now average all rotation matrixes, maybe later on
            // we will need to store all of them for a better estimation
            // than just average
            S.initZeros();

            for (int k3 = 0; k3 < K; ++k3)
                if (k3 != k1 && k3 != k2)
                {
                    result = tripletRotationMatrix(clMatrix, nRays, k1, k2, k3, R);
                    if (result >= 0)
                    {
                      S += R;
                      ++kk;
                    }
                    std::cerr << "DEBUG_JM: triplet: " << formatString("%d %d, %d", k1, k2, k3) << std::endl;
                    std::cerr << "DEBUG_JM: R: " << R << std::endl;
                }
            if (kk > 0)
              S /= kk;
            putRotationMatrix(S, k1, k2, sMatrix);
            putRotationMatrix(S.transpose(), k2, k1, sMatrix);
        }
    }
}//function computeSyncMatrix

quaternionCommonLines()

void quaternionCommonLines	(	const DMatrix &	quaternions,
		CommonLineInfo &	clInfo
	)

Definition at line 645 of file common_lines.cpp.

{
    DVector qa1(4);
    DVector qa2(4); //quaternions k1 and k2
    DMatrix  R1, R2; //rotation matrixes

    for (int i = 0; i < 4; ++i)
    {
        qa1(i) = quaternions(i, clInfo.getImage(0));
        qa2(i) = quaternions(i, clInfo.getImage(1));
    }
    quaternionToMatrix(qa1, R1);
    quaternionToMatrix(qa2, R2);
    //Should be the inversve, but transpose is fine here
    //   R1.selfInverse();
    //   R2.selfInverse();
    R1 = R1.transpose();
    R2 = R2.transpose();

    // % Rotated coordinate system is the columns of the rotation matrix.
    // % We use explicit multiplication to show which column corresponds to x,
    // % which to y, and which to z. See commonline_euler for the difference.
    // X1=R1*([1 0 0].');
    // Y1=R1*([0 1 0].');
    // Z1=R1*([0 0 1].');
    //
    // X2=R2*([1 0 0].');
    // Y2=R2*([0 1 0].');
    // Z2=R2*([0 0 1].');
    //
    // Z3=[Z1(2)*Z2(3)-Z1(3)*Z2(2);...
    //     Z1(3)*Z2(1)-Z1(1)*Z2(3);...
    //     Z1(1)*Z2(2)-Z1(2)*Z2(1)];
    DVector X1, Y1, Z1, X2, Y2, Z2;
    DVector &Z3 = clInfo.vector;
    R1.getCol(0, X1);
    R1.getCol(1, Y1);
    R1.getCol(2, Z1);

    R2.getCol(0, X2);
    R2.getCol(1, Y2);
    R2.getCol(2, Z2);

    XX(Z3) = YY(Z1) * ZZ(Z2) - ZZ(Z1) * YY(Z2);
    YY(Z3) = ZZ(Z1) * XX(Z2) - XX(Z1) * ZZ(Z2);
    ZZ(Z3) = XX(Z1) * YY(Z2) - YY(Z1) * XX(Z2);

    double normZ3 = Z3.module();

    if (normZ3 < 1.0e-8)
        REPORT_ERROR(ERR_NUMERICAL, "GCAR:normTooSmall','Images have same orientation");

    Z3 /= normZ3;
    DVector Z3_t(Z3);
    Z3_t.selfTranspose();

    //   % Compute coordinates of the common-line in each local coordinate system.
    //   XY1=[X1 Y1];
    //   XY2=[X2 Y2];
    //   c1=(Z3.')*XY1;
    //   c2=(Z3.')*XY2;
    DMatrix XY1(3, 2), XY2(3, 2);
    XY1.setCol(0, X1);
    XY1.setCol(1, Y1);
    XY2.setCol(0, X2);
    XY2.setCol(1, Y2);
    DVector c1 = Z3_t * XY1;
    DVector c2 = Z3_t * XY2;
    // % Verify that the common-line is indeed common to both planes. The
    // % following warning should never happen! Just to make sure nothing went
    // % terribly wrong.
    // ev1=XY1*c1(:)-Z3;
    // ev2=XY2*c2(:)-Z3;
    c1.selfTranspose();
    c2.selfTranspose();
    DVector ev1 = XY1 * c1 - Z3;
    DVector ev2 = XY2 * c2 - Z3;

    double normEv1 = ev1.module() / normZ3;
    double normEv2 = ev2.module() / normZ3;

    if (normEv1 > 1.0e-12 || normEv2 > 1.0e-12)
        REPORT_ERROR(ERR_NUMERICAL,
                     formatString("GCAR:largeErrors: Common line is not common. Error1 = %f, Error2 = %f", normEv1, normEv2));
    //   % Compute angle of the common line at each projection's coordinate system
    //   theta1=atan2(c1(2),c1(1));
    //   theta2=atan2(c2(2),c2(1));
    //
    //   PI=4*atan(1.0);
    //   theta1=theta1+PI; % Shift from [-pi,pi] to [0,2*pi].
    //   theta2=theta2+PI;
    //std::cerr << "DEBUG_JM: c1: " << c1 <<std::endl;
    //std::cerr << "DEBUG_JM: c2: " << c2 << std::endl;
    double theta1 = atan2(YY(c1), XX(c1)) + PI;
    double theta2 = atan2(YY(c2), XX(c2)) + PI;
    //
    clInfo.setIndex(0, theta1);
    clInfo.setIndex(1, theta2);
}//function quaternionCommonLines

quaternionToMatrix()

void quaternionToMatrix	(	const DVector &	q,
		DMatrix &	rotMatrix
	)

Definition at line 601 of file common_lines.cpp.

{
    //   function rot_matrix = q_to_rot(q)
    //   %
    //   % Convert a quaternion into a rotation matrix.
    //   %
#define q0 dMi(q, 0)
#define q1 dMi(q, 1)
#define q2 dMi(q, 2)
#define q3 dMi(q, 3)

    rotMatrix.resize(3, 3);

    rotMatrix(0, 0) = POW2(q0) + POW2(q1) - POW2(q2) - POW2(q3);
    rotMatrix(0, 1) = 2 * q1 * q2 - 2 * q0 * q3;
    rotMatrix(0, 2) = 2 * q0 * q2 + 2 * q1 * q3;

    rotMatrix(1, 0) = 2 * q1 * q2 + 2 * q0 * q3;
    rotMatrix(1, 1) = POW2(q0) - POW2(q1) + POW2(q2) - POW2(q3);
    rotMatrix(1, 2) = -2 * q0 * q1 + 2 * q2 * q3;

    rotMatrix(2, 0) = -2 * q0 * q2 + 2 * q1 * q3;
    rotMatrix(2, 1) = 2 * q0 * q1 + 2 * q2 * q3;
    rotMatrix(2, 2) = POW2(q0) - POW2(q1) - POW2(q2) + POW2(q3);

    //saveMatrix("rot_matrix.txt", rotMatrix);
}

randomQuaternions()

void randomQuaternions	(	int	k,
		DMatrix &	qArray
	)

Definition at line 546 of file common_lines.cpp.

{
    //  function q=qrand(K)
    //  %
    //  % q=qrand(K)
    //  %
    //  % Generate K random uniformly distributed quaternions.
    //  % Each quaternions is a four-elements column vector. Returns a matrix of
    //  % size 4xK.
    //  %
    //  % The 3-sphere S^3 in R^4 is a double cover of the rotation group SO(3),
    //  % SO(3) = RP^3.
    //  % We identify unit norm quaternions a^2+b^2+c^2+d^2=1 with group elements.
    //  % The antipodal points (-a,-b,-c,-d) and (a,b,c,d) are identified as the
    //  % same group elements, so we take a>=0.
    //  %
    quaternions.initGaussian(4, k);
    //quaternions.resize(4, k);
    //quaternions.initRandom(0., 1.);
    saveMatrix("random_numbers.txt", quaternions);

    DVector q(4);

    for (int j = 0; j < k; ++j)
    {
        //Get the j-column that is the quaternion
        quaternions.getCol(j, q);
        q.selfNormalize();
        if (XX(q) < 0)
            q *= -1;
        quaternions.setCol(j, q);
    }
    saveMatrix("random_quaternions.txt", quaternions);
}

rotationsFromSyncMatrix()

void rotationsFromSyncMatrix

(

const DMatrix &

sMatrix

)

Definition at line 947 of file common_lines.cpp.

{
    int K = sMatrix.Xdim() / 2;
    int Kx3 = 3 * K;

    DMatrix U, V, V1(3, K), V2(3, K);
    DVector W;
    IVector indexes;
    sMatrix.svd(U, W, V);
    indexes.resizeNoCopy(W);
    indexes.enumerate();

    //std::cerr << "DEBUG_JM: W: " << W << std::endl;

#define SORTED_INDEX(i) dMi(indexes, i)
#define SORTED_ELEM(M, i) dMi(M, SORTED_INDEX(i))

    //Order by insertion sort
    int iAux;
    FOR_ALL_ELEMENTS_IN_MATRIX1D(W)
        for (int j = i; j > 0 && SORTED_ELEM(W, j) > SORTED_ELEM(W, j-1); --j)
          VEC_SWAP(indexes, j, j-1, iAux);

//    std::cerr << "DEBUG_JM: VEC_XSIZE(indexes): " << VEC_XSIZE(indexes) << std::endl;
//    std::cerr << "DEBUG_JM: 10 bigger eigenvalues:" <<std::endl;
//    std::cerr << "DEBUG_JM: indexes: " << indexes << std::endl;
//    for (int i = 0; i < VEC_XSIZE(indexes); ++i)
//      std::cerr << SORTED_ELEM(W, i) << std::endl;

    for (int i = 0; i < K; ++i)
    {
      iAux = 2 * i;
      for (int j = 0; j < 3; ++j)
      {
        dMij(V1, j, i) = dMij(V, iAux, SORTED_INDEX(j));
        dMij(V2, j, i) = dMij(V, iAux+1, SORTED_INDEX(j));
      }
    }

    std::cerr << "DEBUG_JM: V1: " << V1 << std::endl;
    std::cerr << "DEBUG_JM: V2: " << V2 << std::endl;

    //% 3*K equations in 9 variables (3 x 3 matrix entries).
    DMatrix equations(Kx3, 9);
    int index;

    for (int k = 0; k < K; ++k)
      for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
        {
          index = 3 * i + j;
          iAux = 3 * k;
          dMij(equations, iAux, index) = dMij(V1, i, k) * dMij(V1, j, k);
          dMij(equations, iAux+1, index) = dMij(V2, i, k) * dMij(V2, j, k);
          dMij(equations, iAux+2, index) = dMij(V1, i, k) * dMij(V2, j, k);
        }
//    std::cerr << "DEBUG_JM: equations: " << equations << std::endl;
    PseudoInverseHelper pih;

    DMatrix &trunc_equations = pih.A;
    trunc_equations.resizeNoCopy(Kx3, 6);
    //Use vector W to select columns from equations
    int cols[6] = {0, 1, 2, 4, 5, 8};
    for (int i = 0; i < 6; ++i)
    {
      equations.getCol(cols[i], W);
      trunc_equations.setCol(i, W);
    }

    DVector &b = pih.b;
    b.initConstant(Kx3, 1.);
    for (int i = 2; i < Kx3; i+=3)
      dMi(b, i) = 0.;

    //% Find the least squares approximation
    DVector ATA_vec;
    DMatrix ATA(3, 3);

    //C
    solveLinearSystem(pih, ATA_vec);

//    std::cerr << "DEBUG_JM: trunc_equations: " << trunc_equations << std::endl;
//    saveMatrix("truncated.txt", trunc_equations);
//    std::cerr << "DEBUG_JM: b: " << b << std::endl;
//    std::cerr << "DEBUG_JM: ATA_vec: " << ATA_vec << std::endl;

    dMij(ATA, 0, 0) = dMi(ATA_vec, 0);
    dMij(ATA, 0, 1) = dMi(ATA_vec, 1);
    dMij(ATA, 0, 2) = dMi(ATA_vec, 2);
    dMij(ATA, 1, 0) = dMi(ATA_vec, 1);
    dMij(ATA, 1, 1) = dMi(ATA_vec, 3);
    dMij(ATA, 1, 2) = dMi(ATA_vec, 4);
    dMij(ATA, 2, 0) = dMi(ATA_vec, 2);
    dMij(ATA, 2, 1) = dMi(ATA_vec, 4);
    dMij(ATA, 2, 2) = dMi(ATA_vec, 5);

    std::cerr << "DEBUG_JM: ATA: " << ATA << std::endl;

    DMatrix A, R1, R2;
    cholesky(ATA, A);
    A = A.transpose();

    std::cerr << "DEBUG_JM: A: " << A << std::endl;

    R1 = A * V1;
    R2 = A * V2;
    //DMatrix R3(R1);
    DVector v1, v2, v3(3);
    std::vector<DMatrix> rotations(K);

    MetaDataVec MD("images.xmd");
    auto idIt(MD.ids().begin());
    for (int i = 0; i < K; ++i)
    {
      DMatrix &R = rotations[i];
      R.resizeNoCopy(3, 3);
      R1.getCol(i, v1);
      R2.getCol(i, v2);
      vectorProduct(v1, v2, v3);
      //R3.setCol(i, v3);
      R.setCol(0, v1);
      R.setCol(1, v2);
      R.setCol(2, v3);
      //% Enforce R to be a rotation (in case the error is large)
      R.svd(U, W, V);
      R = U * V.transpose();

      double rot, tilt, psi;
      //
      Euler_matrix2angles(R.transpose(), rot, tilt, psi);
      MD.setValue(MDL_ANGLE_ROT,rot,*idIt);
      MD.setValue(MDL_ANGLE_TILT,tilt,*idIt);
      MD.setValue(MDL_ANGLE_PSI,psi,*idIt);
      ++idIt;

      std::cerr << "DEBUG_JM: R" << i << " : " << R << std::endl;
    }
    MD.write("imagesReconstruction.xmd");
}//function rotationsFromSyncMatrix

saveMatrix()

void saveMatrix	(	const char *	fn,
		DMatrix &	array
	)

Definition at line 581 of file common_lines.cpp.

{
    std::fstream fs;
    fs.open(fn, std::fstream::trunc | std::fstream::out);
    fs.precision(16);

    for (size_t j = 0; j < MAT_YSIZE(matrix); ++j)
    {
        for (size_t i = 0; i < MAT_XSIZE(matrix); ++i)
        {
            fs << std::scientific << dMij(matrix, j, i) << "\t";
            //std::cerr << dAij(matrix, j, i) << " ";
        }
        fs << std::endl;
        //std::cerr << std::endl;
    }
    fs.close();
    //std::cerr << "DEBUG_JM: leaving saveMatrix" <<std::endl;
}

tripletRotationMatrix()

int tripletRotationMatrix	(	const DMatrix &	clMatrix,
		size_t	nRays,
		int	k1,
		int	k2,
		int	k3,
		DMatrix &	R
	)

Negative output means error -101 Triangle too small

Definition at line 835 of file common_lines.cpp.

{
    DMatrix G(3, 3), Q;
    // % Find Q12, Q13, Q23
    double factor = TWOPI / nRays;
    double a = cos(factor * (cl(3,2)-cl(3,1)));
    double b = cos(factor * (cl(2,3)-cl(2,1)));
    double c = cos(factor * (cl(1,3)-cl(1,2)));

    if (1 + 2 * a * b * c - (POW2(a) + POW2(b) + POW2(c))<1.0e-5)
        return SMALL_TRIANGLE;

    G.initIdentity();
    // Compute G matrix according to (4.2)
    dMij(G, 0, 1) = a;
    dMij(G, 0, 2) = b;
    dMij(G, 1, 0) = a;
    dMij(G, 1, 2) = c;
    dMij(G, 2, 0) = b;
    dMij(G, 2, 1) = c;

    cholesky(G, Q);

    //Our cholesky gives us the lower triangular matrix
    //so we need to transpose to have the same as in Yoel code
    //that's why we take now rows instead of columns
    DVector Q12, Q13, Q23;
    Q.getRow(0, Q23);
    Q.getRow(1, Q13);
    Q.getRow(2, Q12);
    Q12.setCol();
    Q13.setCol();
    Q23.setCol();

    DMatrix R1, R2;
    anglesRotationMatrix( nRays, (int)cl(1, 2), (int)cl(1, 3), Q12, Q13, R1);
    anglesRotationMatrix( nRays, (int)cl(2, 1), (int)cl(2, 3), Q12, Q23, R2);
    // Compute rotation matrix according to (4.6)
    R = R1.transpose() * R2;

    return 0;
}//function tripleRotationMatrix

Variable Documentation

SMALL_TRIANGLE

constexpr signed int SMALL_TRIANGLE = -101

Definition at line 204 of file common_lines.h.