libNumerics/computeH.cpp
// Add corresponding lines of equation ux + by + x = 0
Add corresponding lines of equation ux + by + x = 0
Add corresponding lines of equation ux + by + x = 0
void ComputeH::add(float x, float y, float z, float X, float Y, float Z,
                    float w)
{
    float s = 1.0f / (float)sqrt(x*x + y*y);
    x *= s;
    y *= s;
    z *= s;
    s = 1.0f / (float)sqrt(X*X + Y*Y);
    X *= s;
    Y *= s;
    Z *= s;
    if(_type <= Similarity) { 
        add_4parameters(x, y, z, X, Y, Z, w);
        return;
    }
    double x2 = x*x, y2 = y*y, z2 = z*z, xy = x*y, xz = x*z, yz = y*z;
    double X2 = X*X, Y2 = Y*Y, Z2 = Z*Z, XY = X*Y, XZ = X*Z, YZ = Y*Z;
    double *A = Ann, *B = Bn;
    *A++ += w* (y2+z2) * X2; 
    *A++ -= w* xy * X2;
    *A++ += w* (y2+z2) * XY;
    *A++ -= w* xy * XY;
    *A++ -= w* xz * X2;
    *A++ -= w* xz * XY;
    if(_type == Projective) {
        *A++ += w* (y2+z2) * XZ;
        *A++ -= w* xy * XZ;
    }
    *B++ += w* xz * XZ;
    A++; 
    *A++ += w* (x2+z2) * X2;
    *A++ -= w* xy * XY;
    *A++ += w* (x2+z2) * XY;
    *A++ -= w* yz * X2;
    *A++ -= w* yz * XY;
    if(_type == Projective) {
        *A++ -= w* xy * XZ;
        *A++ += w* (x2+z2) * XZ;
    }
    *B++ -= w* yz * XZ;
    A += 2; 
    *A++ += w* (y2+z2) * Y2;
    *A++ -= w* xy * Y2;
    *A++ -= w* xz * XY;
    *A++ -= w* xz * Y2;
    if(_type == Projective) {
        *A++ += w* (y2+z2) * YZ;
        *A++ -= w* xy * YZ;
    }
    *B++ += w* xz * YZ;
    A += 3; 
    *A++ += w* (x2+z2) * Y2;
    *A++ -= w* yz * XY;
    *A++ -= w* yz * Y2;
    if(_type == Projective) {
        *A++ -= w* xy * YZ;
        *A++ += w* (x2+z2) * YZ;
    }
    *B++ += w* yz * YZ;
    A += 4; 
    *A++ += w* X2; 
    *A++ += w* XY; 
    if(_type == Projective) {
        *A++ -= w* xz * XZ;
        *A++ -= w* yz * XZ;
    }
    *B++ -= w* XZ; 
    A += 5; 
    *A++ += w* Y2; 
    *B++ -= w* YZ; 
    if(_type == Projective) {
        *A++ -= w* xz * YZ;
        *A++ -= w* yz * YZ;
        A += 6; 
        *A++ += w* (y2+z2) * Y2;
        *A++ -= w* xy * Z2;
        *B++ += w* xz * Z2;
        A += 7; 
        *A++ += w* (x2+z2) * Z2;
        *B++ += w* yz * Z2;
    }
    b += w* Z2;  
}
void ComputeH :: add ( float x , float y , float z , float X , float Y , float Z , float w ) { float s = 1.0f / ( float ) sqrt ( x * x + y * y ) ; x *= s ; y *= s ; z *= s ; s = 1.0f / ( float ) sqrt ( X * X + Y * Y ) ; X *= s ; Y *= s ; Z *= s ; if ( _type <= Similarity ) { add_4parameters ( x , y , z , X , Y , Z , w ) ; return ; } double x2 = x * x , y2 = y * y , z2 = z * z , xy = x * y , xz = x * z , yz = y * z ; double X2 = X * X , Y2 = Y * Y , Z2 = Z * Z , XY = X * Y , XZ = X * Z , YZ = Y * Z ; double * A = Ann , * B = Bn ; * A ++ += w * ( y2 + z2 ) * X2 ; * A ++ -= w * xy * X2 ; * A ++ += w * ( y2 + z2 ) * XY ; * A ++ -= w * xy * XY ; * A ++ -= w * xz * X2 ; * A ++ -= w * xz * XY ; if ( _type == Projective ) { * A ++ += w * ( y2 + z2 ) * XZ ; * A ++ -= w * xy * XZ ; } * B ++ += w * xz * XZ ; A ++ ; * A ++ += w * ( x2 + z2 ) * X2 ; * A ++ -= w * xy * XY ; * A ++ += w * ( x2 + z2 ) * XY ; * A ++ -= w * yz * X2 ; * A ++ -= w * yz * XY ; if ( _type == Projective ) { * A ++ -= w * xy * XZ ; * A ++ += w * ( x2 + z2 ) * XZ ; } * B ++ -= w * yz * XZ ; A += 2 ; * A ++ += w * ( y2 + z2 ) * Y2 ; * A ++ -= w * xy * Y2 ; * A ++ -= w * xz * XY ; * A ++ -= w * xz * Y2 ; if ( _type == Projective ) { * A ++ += w * ( y2 + z2 ) * YZ ; * A ++ -= w * xy * YZ ; } * B ++ += w * xz * YZ ; A += 3 ; * A ++ += w * ( x2 + z2 ) * Y2 ; * A ++ -= w * yz * XY ; * A ++ -= w * yz * Y2 ; if ( _type == Projective ) { * A ++ -= w * xy * YZ ; * A ++ += w * ( x2 + z2 ) * YZ ; } * B ++ += w * yz * YZ ; A += 4 ; * A ++ += w * X2 ; * A ++ += w * XY ; if ( _type == Projective ) { * A ++ -= w * xz * XZ ; * A ++ -= w * yz * XZ ; } * B ++ -= w * XZ ; A += 5 ; * A ++ += w * Y2 ; * B ++ -= w * YZ ; if ( _type == Projective ) { * A ++ -= w * xz * YZ ; * A ++ -= w * yz * YZ ; A += 6 ; * A ++ += w * ( y2 + z2 ) * Y2 ; * A ++ -= w * xy * Z2 ; * B ++ += w * xz * Z2 ; A += 7 ; * A ++ += w * ( x2 + z2 ) * Z2 ; * B ++ += w * yz * Z2 ; } b += w * Z2 ; }
Add corresponding lines of equation ux + by + x = 0
Add corresponding lines of equation ux + by + x = 0
// Separate for readability // Equation 1 // Equation 2 // Equation 3 // Equation 4 // Equation 5 // *(x2+y2=1) // *(x2+y2=1) // *(x2+y2=1) // Equation 6 // *(x2+y2=1) // *(x2+y2=1) // Equation 7 // Equation 8 // *(x2+y2=1) 