IntrinsicsExtrinsicsContainer.cpp

Go to the documentation of this file.

#include "IntrinsicsExtrinsicsContainer.hpp"
#include "Rotation.hpp"

GslMatrix IntrinsicsExtrinsicsContainer::getExtrinsicsMatrix(unsigned int imgIdx) const
{
        GslMatrix result(3,4);
        result.setToIdentity();

        // Get a rotation, in axis/angle form
        GslVector axis;
        Rotation r;
        r.setScaledAxis(getRotation(imgIdx));

        // Convert that rotation into matrix form
        GslMatrix m3x3(3,3);
        r.getMatrix(m3x3);

        // Copy that matrix into the result we'll return
        for (int row = 0; row < 3; row++)
                for (int col = 0; col < 3; col++)
                        result(row,col) = m3x3(row,col);

        // Get our translation
        GslVector translation = getTranslation(imgIdx);

        // Put the translation into the result also
        result(0,3) = translation(0);
        result(1,3) = translation(1);
        result(2,3) = translation(2);

        return result;
}



GslMatrix IntrinsicsExtrinsicsContainer::getIntrinsicsMatrix() const
{
        GslMatrix result(3,3);
        result.setToZero();
        result(0,0) = alpha();
        result(0,1) = skewness();
        result(0,2) = u0();
        result(1,1) = beta();
        result(1,2) = v0();
        result(2,2) = 1.;

        return result;
}



void IntrinsicsExtrinsicsContainer::setRotation (unsigned int index, const GslVector &v) 
{ 
        unsigned int startIndex = 7 + 6 * index;
        this->at(startIndex++) = v(0);
        this->at(startIndex++) = v(1);
        this->at(startIndex++) = v(2);
}



void IntrinsicsExtrinsicsContainer::setTranslation(unsigned int imgIdx, const GslVector &v)
{ 
        unsigned int startIndex = 7 + (imgIdx * 6) + 3;
        this->at(startIndex++) = v(0);
        this->at(startIndex++) = v(1);
        this->at(startIndex++) = v(2);
}



GslVector IntrinsicsExtrinsicsContainer::getRotation (unsigned int imgIdx) const
{
        unsigned int startIndex = 7 + (imgIdx * 6);
        if (startIndex >= this->getNumRows())
                throw Error("IntrinsicsExtrinsicsContainer::getRotation(): invalid param %d is >= our number of items %d.", startIndex, getNumRows());

        GslVector result(3);
        result(0) = this->at(startIndex++);
        result(1) = this->at(startIndex++);
        result(2) = this->at(startIndex++);

        return result;
}



GslVector IntrinsicsExtrinsicsContainer::getTranslation(unsigned int imgIdx) const
{
        unsigned int startIndex = 7 + (imgIdx * 6) + 3;
        if (startIndex >= this->getNumRows())
                throw Error("IntrinsicsExtrinsicsContainer::getTranslation(): invalid param %d is >= our number of items %d.", startIndex, getNumRows());

        GslVector result(3);
        result(0) = this->at(startIndex++);
        result(1) = this->at(startIndex++);
        result(2) = this->at(startIndex++);

        return result;
}



GslMatrix IntrinsicsExtrinsicsContainer::getTransformation(unsigned int imgIdx) const
{
        GslMatrix result = getIntrinsicsMatrix() * getExtrinsicsMatrix(imgIdx);
        double homogenous = result(result.getNumRows()-1, result.getNumCols()-1);
        for (unsigned int row = 0; row < result.getNumRows(); row++)
        {
                for (unsigned int col = 0; col < result.getNumCols(); col++)
                {
                        result(row,col) /= homogenous;
                }
        }
        return result;
}

---