rotations.cpp

Go to the documentation of this file.

/* rotations.cpp :  Mass spectrum candidate filtering
 * Copyright (C) 2020  Daniel Haley
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "atomprobe/algorithm/rotations.h"

#include "../helper/helpFuncs.h"
#include "atomprobe/helper/maths/misc.h"

#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>

#ifdef DEBUG
#include <random>
#include "atomprobe/helper/maths/quat.h"
#endif

namespace AtomProbe
{

//TODO: Move to general maths file  
//Compute determinant of gsl_matrix. A is replaced by LU decomposition
// if inPlace = true
double gsl_matrix_det(gsl_matrix *A, bool inPlace=false) 
{
    /*
      inPlace = 1 => A is replaced with the LU decomposed copy.
      inPlace = 0 => A is retained, and a copy is used for LU.
    */

    double det;
    int signum;
    gsl_permutation *p = gsl_permutation_alloc(A->size1);
    gsl_matrix *tmpA;

    if (inPlace)
        tmpA = A;
    else {
        tmpA = gsl_matrix_alloc(A->size1, A->size2);
        gsl_matrix_memcpy(tmpA, A);
    }


    gsl_linalg_LU_decomp(tmpA, p, &signum);
    det = gsl_linalg_LU_det(tmpA, signum);
    gsl_permutation_free(p);
    if (! inPlace)
        gsl_matrix_free(tmpA);


    return det;
}

void computeRotationMatrix(const Point3D &ur1, const Point3D &ur2,
    const Point3D &r1, const Point3D &r2, std::vector<std:: vector<float> > &m)
{
        gsl_matrix *mm = gsl_matrix_alloc(3,3);
        computeRotationMatrix(ur1,ur2,r1,r2,mm);
        m.clear();
        m.resize(3);
        for(auto ui=0;ui<3;ui++)
        {
                m[ui].resize(3);
                for(auto uj=0;uj<3;uj++)
                        m[ui][uj] =gsl_matrix_get(mm,ui,uj);
        }

};

void computeRotationMatrix(const Point3D &ur1, const Point3D &ur2,
    const Point3D &r1, const Point3D &r2, gsl_matrix *m)
{
    //TRIAD algorithm, for determining rotation matrix from two
    // linearly independant vector pairs.
    // This is a specific case of "Wahba's Problem", where no noise is present
    
    //vectors should be pre-normalised
    ASSERT(TOL_EQ(ur1.sqrMag(),1) && TOL_EQ(ur2.sqrMag(),1));
    ASSERT(TOL_EQ(r1.sqrMag(),1) && TOL_EQ(r2.sqrMag(),1));

    //unrotated and rotated vectors should be linearly independant
    ASSERT(ur1.crossProd(ur2).sqrMag() -1 < 0.001);
    ASSERT(r1.crossProd(r2).sqrMag() -1 < 0.001);


    ASSERT(m->size1== 3);
    ASSERT(m->size2== 3);

    //r1 x r2 and ur1 x ur2
    Point3D rCross,urCross;
    rCross = r1.crossProd(r2);
    urCross = ur1.crossProd(ur2);

    //reorthogonalise
    Point3D doubleCross1,doubleCross2;
    doubleCross1 = r1.crossProd(rCross);
    doubleCross2 = ur1.crossProd(urCross);

    gsl_matrix *a,*b;
    a = gsl_matrix_alloc(3,3);
    b = gsl_matrix_alloc(3,3);

    //Build the two matrices used in the triad
    for(unsigned int ui=0;ui<3;ui++)
    {
        //build A matrix, row by row
        gsl_matrix_set(a,ui,0,r1[ui]);
        gsl_matrix_set(a,ui,1,rCross[ui]);
        gsl_matrix_set(a,ui,2,doubleCross1[ui]);

        //build (B^T) matrix, col by col
        gsl_matrix_set(b,0,ui,ur1[ui]);
        gsl_matrix_set(b,1,ui,urCross[ui]);
        gsl_matrix_set(b,2,ui,doubleCross2[ui]);

    }

    //The two matrices should be unit determinant
    ASSERT(TOL_EQ(fabs(gsl_determinant(a)),1));
    ASSERT(TOL_EQ(fabs(gsl_determinant(b)),1));

    //Compute m = a*b; where m is the rotation matrix
    gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
                1.0, a, b,
                0.0, m);


    gsl_matrix_free(a);
    gsl_matrix_free(b);

}


unsigned int computeRotationMatrixWahba(const std::vector<Point3D> &unrotated,const std::vector<Point3D> &rotated, 
         const std::vector<float> &weights,gsl_matrix* &R)
{

    //Matched vector sizes
    ASSERT(rotated.size() == unrotated.size()); 
    //constrained problem
    ASSERT(rotated.size() >=2); 

    if(rotated.size() < 2)
        return 1;

    if(R == nullptr)
        R=gsl_matrix_alloc(3,3);
    else
    {
        ASSERT(R->size1 == 3 && R->size2 ==3);
    }

    //See "attitude determination using vector observations and the singular value decomposition"
    // Markley, L. Journal of the Astronautical Sciences, Nov 1987
    //We need to find a matrix , B, which is of the form
    //  B = sum[ k_i *(a_i w_i v_i') ]
    // where v_i is the pre-rotation vectors, and w_i are the post-rotation vectors
    // k_i are the weights for the measurement (e.g. inverse error)

    gsl_matrix *B = gsl_matrix_alloc(3,3);
    gsl_matrix_set_all(B,0);
    gsl_vector *v1,*v2;
    v1= gsl_vector_alloc(3);
    v2= gsl_vector_alloc(3);

    for(auto ui=0u; ui<unrotated.size();ui++)
    {
        ASSERT(TOL_EQ(unrotated[ui].sqrMag(),1)); 
        ASSERT(TOL_EQ(rotated[ui].sqrMag(),1)); 
        for(auto uj=0u;uj<3;uj++)
        {
            v1->data[uj] = unrotated[ui][uj];
            v2->data[uj] = rotated[ui][uj];
        }

        //Sum outer products
        gsl_blas_dger(weights[ui],v1, v2, B);   
    }

    gsl_vector_free(v1);
    gsl_vector_free(v2);

    //OK. so now we have the B matrix, decompose 
    // to obtain U and V 
    
    //Allocate output area for SVD
    gsl_matrix *V  = gsl_matrix_alloc(B->size2,B->size2);
    gsl_vector *S  = gsl_vector_alloc(B->size2);
    gsl_vector *work = gsl_vector_alloc(B->size2);

    //On output, B is replaced by "U"
    gsl_linalg_SV_decomp (B, V, S, work);

    gsl_vector_free(work);

    //FIXME: Should check for singularity 
    //We don't actually use the sv's  themselves
    gsl_vector_free(S);
    
    // M = diag([1 1 det(U) det(V)])
    gsl_matrix *M=gsl_matrix_alloc(3,3);
    gsl_matrix_set_all(M,0);

    //Fill M
    gsl_matrix_set(M,0,0,1);
    gsl_matrix_set(M,1,1,1);
    gsl_matrix_set(M,2,2,gsl_determinant(B)*gsl_determinant(V));
    
    //Now compute R = U M V'
    //T= U*M, recall B is reassigned as U
    gsl_matrix *T=gsl_matrix_alloc(3,3);
    gsl_matrix_mult(B,M,T);

    //R=T*V'
    gsl_matrix_transpose(V);
    gsl_matrix_mult(T,V,R,false);


    //Compared to other routines in this library, the transformation here
    // is actually the inverse rotation, R', so transpose to get R
    gsl_matrix_transpose(R);

    gsl_matrix_free(V);
    gsl_matrix_free(B);
    gsl_matrix_free(T);
    gsl_matrix_free(M);

    return 0;

}

//Return the rotation matrix in axis-angle (pt is axis, theta is angle)
gsl_matrix *getRotationMatrix(const Point3D &pt, float theta)
{
    Point3D norm =pt;
    norm.normalise();

    gsl_matrix *R = gsl_matrix_alloc(3,3);
    
    double versin = 1.0-cos(theta);
    double ct = cos(theta);
    double st= sin(theta);

    //set diagonal
    for(auto ui=0;ui<3;ui++)
        gsl_matrix_set(R,ui,ui,ct+norm[ui]*norm[ui]*versin); 

    //top row
    gsl_matrix_set(R,0,1,norm[0]*norm[1]*versin - norm[2]*st); //1
    gsl_matrix_set(R,0,2,norm[0]*norm[2]*versin + norm[1]*st); //2

    //middle row
    gsl_matrix_set(R,1,0,norm[1]*norm[0]*versin+norm[2]*st); //0
    gsl_matrix_set(R,1,2,norm[1]*norm[2]*versin-norm[0]*st); //2

    //bottom row
    gsl_matrix_set(R,2,0,norm[2]*norm[0]*versin-norm[1]*st); //0
    gsl_matrix_set(R,2,1,norm[2]*norm[1]*versin+norm[0]*st); //1

    //Determinant should be 1
    ASSERT(TOL_EQ(gsl_determinant(R),1));

    return R;
}


#ifdef DEBUG
bool testRotationAlgorithms();
bool testTRIADRotation();
bool testWahba();



bool testRotationAlgorithms()
{
    if(!testTRIADRotation())
        return false;
    if(!testWahba())
        return false;

    return true;    
}

bool testTRIADRotation()
{

    Point3D pZ = Point3D(0,0,1);
    Point3D pY = Point3D(0,1,0);
    
    Point3D prY = Point3D(0,1,0);
    Point3D prZ = Point3D(1,0,1);
    prY.normalise();
    prZ.normalise();

    gsl_matrix *m;
    m = gsl_matrix_alloc(3,3);  
    
    computeRotationMatrix(pY,pZ,prY,prZ,m);
    TEST(TOL_EQ(fabs(gsl_determinant(m)),1.0),"Unit determinant");

    //If we transform the original vectors to their new orientaitons
    // we should get the new vectors back out
    pZ.transform3x3(m);
    pY.transform3x3(m);

    TEST(pZ.sqrDist(prZ) < 0.01,"Rotate test");
    TEST(pY.sqrDist(prY) < 0.01,"Rotate test");

    gsl_matrix_free(m);

    return true;
}

bool testWahba()
{
    const unsigned int NPTS=10;

    std::random_device rd;  //Will be used to obtain a seed for the random number engine
    std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
    std::uniform_real_distribution<> distrib(0, 1);

    std::vector<Point3D> pts,rotatedPts;
    pts.resize(NPTS);
    for(auto ui=0;ui<NPTS;ui++)
    {
        pts[ui] = Point3D(distrib(gen),
                distrib(gen),
                distrib(gen));

        pts[ui].normalise();
    }

    //Perform null rotation
    rotatedPts = pts;

    std::vector<float> weights;
    weights.resize(pts.size(),1);

    gsl_matrix *R=nullptr;
    computeRotationMatrixWahba(pts,rotatedPts,weights,R);

    //Check for identity
    for(auto ui=0;ui<3;ui++)
    {
        for(auto uj=0;uj<3;uj++)
        {
            if(ui == uj)
            {
                TEST(TOL_EQ(gsl_matrix_get(R,ui,uj),1.0),"Identity matrix");
            }
            else
            {
                TEST(TOL_EQ(gsl_matrix_get(R,ui,uj),0), "Identity matrix (1)");
            }
        }
    }

    gsl_matrix_free(R);

    //Compute a rotation matrix
    Point3D axis=Point3D(1,2,3);
    axis.normalise();
    float angle=0.51;
    R=getRotationMatrix(axis,angle);



    //Rotate each point by the same axis-angle
    for(auto &p : rotatedPts)
        quat_rot(p,axis,angle);

    //Retrieve rotation matrix
    gsl_matrix *R_computed=nullptr;
    computeRotationMatrixWahba(pts,rotatedPts,weights,R_computed);
    
    for(auto ui=0u; ui<9;ui++)
    {
        ASSERT( TOL_EQ(R_computed->data[ui],R->data[ui]));
    }

    gsl_matrix_free(R);
    gsl_matrix_free(R_computed);
    return true;

}

#endif

}