ImageCalibrator.cpp

#include "ImageCalibrator.hpp"
#include "GslVector.hpp"
#include "GslMatrix.hpp"


void ImageCalibrator::printHomography()
{
    homography.print("ImageCalibrator homography");        
}



void ImageCalibrator::printPoints()
{
    for (unsigned int i = 0; i < points.size(); i++)
        printf("x=%.3lf, y=%.3lf, u=%.3lf, v=%.3lf\n",
        points[i].x, points[i].y, points[i].u, points[i].v);
}



void ImageCalibrator::recalculateHomography()
{
/*
 * For each point, plug its 3d and image coordinates into a matrix
 * which will help us solve for the homogenous matrix we need.
 */

    GslMatrix table (points.size()*2, 9);

    GslVector vec(9);

    for (unsigned int pointIndex = 0; pointIndex < points.size(); pointIndex++)
    {
        PointCorrespondence p = points[pointIndex];

        vec(0) = p.x;
        vec(1) = p.y;
        vec(2) = 1;

        vec(3) = 0;
        vec(4) = 0;
        vec(5) = 0;

        vec(6) = p.x * -p.u;
        vec(7) = p.y * -p.u;
        vec(8) = -p.u;

        table.setRow (pointIndex*2, vec);

        vec(0) = 0;
        vec(1) = 0;
        vec(2) = 0;
        
        vec(3) = p.x;
        vec(4) = p.y;
        vec(5) = 1;

        vec(6) = p.x * -p.v;
        vec(7) = p.y * -p.v;
        vec(8) = -p.v;

        table.setRow (1+pointIndex*2, vec);
    }
    
    GslVector nullspaceVector = getNullspaceVector(table);

/*
 * Now we use a singular value decomposition (SVD) to get the nullspace
 * vector for our matrix. This nullspace vector contains the values of our
 * homogenous matrix.
 */
    for (int rows = 0; rows < 3; rows++)
        for (int cols = 0; cols < 3; cols++)
            homography(rows,cols) = nullspaceVector(cols+rows*3);
}



GslVector getNullspaceVector (GslMatrix matrix)
{
    int matrixRows = matrix.getNumRows();
    int matrixCols = matrix.getNumCols();

    GslMatrix U = matrix;
    GslMatrix V(matrixCols, matrixCols);
    GslVector S(matrixCols);

    matrix.SingularValueDecomposition(U, S, V);

        // Vector in the "V" matrix with the lowest eigenvalue
        // is our nullspace vector
    GslVector result(matrixCols);
    for (int i = 0; i < matrixCols; i++)
        result(i) = V(i, matrixCols-1);

    return result;    
}

---