libNumerics/computeH.cpp
// LSE for rotation: solve linear system under quadratic constraint
LSE for rotation: solve linear system under quadratic constraint
LSE for rotation : solve linear system under quadratic constraint
bool ComputeH::compute_rotation(vector<double>& B) const
{
    if(Ann[15] <= 0) 
        return false;
    B(0) = Ann[15] * Bn[0] - Ann[2] * Bn[2] - Ann[3] * Bn[3];
    B(1) = Ann[15] * Bn[1] - Ann[3] * Bn[2] + Ann[2] * Bn[3];
    double root = sqrt(B(0)*B(0) + B(1)*B(1));
    if(root < minEigenValue)
        return false;
    double lambda1 = (Ann[2]*Ann[2] + Ann[3]*Ann[3] + root) / Ann[15];
    B(0) /= root;
    B(1) /= root;
    B(2) = (-Ann[2]*Bn[0] - Ann[3]*Bn[1] + lambda1 * Bn[2]) / root;
    B(3) = (-Ann[3]*Bn[0] + Ann[2]*Bn[1] + lambda1 * Bn[3]) / root;
    float v1 = q_error(B);
    vector<double> C(4);
    double lambda2 = (Ann[2]*Ann[2] + Ann[3]*Ann[3] - root) / Ann[15];
    C(0) = -B(0);
    C(1) = -B(1);
    C(2) = -(-Ann[2]*Bn[0] - Ann[3]*Bn[1] + lambda2 * Bn[2]) / root;
    C(3) = -(-Ann[3]*Bn[0] + Ann[2]*Bn[1] + lambda2 * Bn[3]) / root;
    if(v1 > q_error(C)) 
        B = C;
    return true;
}
bool ComputeH :: compute_rotation ( vector < double > & B ) const { if ( Ann [ 15 ] <= 0 ) return false ; B ( 0 ) = Ann [ 15 ] * Bn [ 0 ] - Ann [ 2 ] * Bn [ 2 ] - Ann [ 3 ] * Bn [ 3 ] ; B ( 1 ) = Ann [ 15 ] * Bn [ 1 ] - Ann [ 3 ] * Bn [ 2 ] + Ann [ 2 ] * Bn [ 3 ] ; double root = sqrt ( B ( 0 ) * B ( 0 ) + B ( 1 ) * B ( 1 ) ) ; if ( root < minEigenValue ) return false ; double lambda1 = ( Ann [ 2 ] * Ann [ 2 ] + Ann [ 3 ] * Ann [ 3 ] + root ) / Ann [ 15 ] ; B ( 0 ) /= root ; B ( 1 ) /= root ; B ( 2 ) = ( - Ann [ 2 ] * Bn [ 0 ] - Ann [ 3 ] * Bn [ 1 ] + lambda1 * Bn [ 2 ] ) / root ; B ( 3 ) = ( - Ann [ 3 ] * Bn [ 0 ] + Ann [ 2 ] * Bn [ 1 ] + lambda1 * Bn [ 3 ] ) / root ; float v1 = q_error ( B ) ; vector < double > C ( 4 ) ; double lambda2 = ( Ann [ 2 ] * Ann [ 2 ] + Ann [ 3 ] * Ann [ 3 ] - root ) / Ann [ 15 ] ; C ( 0 ) = - B ( 0 ) ; C ( 1 ) = - B ( 1 ) ; C ( 2 ) = - ( - Ann [ 2 ] * Bn [ 0 ] - Ann [ 3 ] * Bn [ 1 ] + lambda2 * Bn [ 2 ] ) / root ; C ( 3 ) = - ( - Ann [ 3 ] * Bn [ 0 ] + Ann [ 2 ] * Bn [ 1 ] + lambda2 * Bn [ 3 ] ) / root ; if ( v1 > q_error ( C ) ) B = C ; return true ; }
LSE for rotation: solve linear system under quadratic constraint
LSE for rotation : solve linear system under quadratic constraint
// No point added or absurd value // Test first solution // Test second solution // Keep second solution