File content as of revision 0:765907e35737:

#include "StereoCamera.h"

template <class T>
T Vec2D<T>::dot(const Vec2D<T> &other) const
{
    return this->x*other.x+this->y*other.y;
}

template <class T>
T Vec3D<T>::dot(const Vec3D<T> &other) const
{
    return this->x*other.x+this->y*other.y + this->z*other.z;
}

Vec2D<double> Camera::undistortion(const Vec2D<double> &p) const
{
    Vec2D<double> rp(p);
    double r_2,kRadial,dx,dy;
    for (int i = 0; i < 20; i++) {
        r_2 = rp.dot(rp);
        kRadial = 1 + k1 * r_2+k2*r_2*r_2+k3*r_2*r_2*r_2; // radial distortion
        dx = 2*p1*rp.x*rp.y+p2*(r_2+2*rp.x*rp.x);
        dy = p1*(r_2+2*rp.y*rp.y)+2*p2*rp.x*rp.y;
        rp.x = (p.x-dx)/kRadial;
        rp.y = (p.y-dy)/kRadial;
    }
    return rp;
}
Vec2D<double> Camera::normalization(const Vec2D<int> &p) const
{
    Vec2D<double> rp(0,0);
    // Subtract pincipal point and divide by the focal length
    rp.x = (p.x-cc.x)/fc.x;
    rp.y = (p.y-cc.y)/fc.y;

    // Undo skew
    rp.x = rp.x - alpha*rp.y;


    // Compensate for lens distortion
    if (k1||k2||p1||p2||k3) {
        return undistortion(rp);
    } else {
        return rp;
    }
}

Vec3D<double> StereoCamera::triangulation(const Vec2D<int> &pLeft, const Vec2D<int> &pRight, int LR) const
{
    // Normalize hte image projection according ot the intrinsic parameters of the left and right cameras
    Vec2D<double> xl2 = leftCam.normalization(pLeft);
    Vec2D<double> xr2 = rightCam.normalization(pRight);

    // Extend the normalized projections in homogeneous coordinates
    Vec3D<double> xl3(xl2.x,xl2.y,1);
    Vec3D<double> xr3(xr2.x,xr2.y,1);

    Vec3D<double> u(R[0]*xl3.x+R[1]*xl3.y+R[2]*xl3.z,
                    R[3]*xl3.x+R[4]*xl3.y+R[5]*xl3.z,
                    R[6]*xl3.x+R[7]*xl3.y+R[8]*xl3.z);

    double n_xl3_2 = xl3.dot(xl3);
    double n_xr3_2 = xr3.dot(xr3);

    double DD = n_xl3_2 * n_xr3_2 - (u.dot(xr3))*(u.dot(xr3));

    double dot_uT = u.dot(Tvec);
    double dot_xrT = xr3.dot(Tvec);
    double dot_xru = xr3.dot(u);

    double NN1 = dot_xru*dot_xrT - n_xr3_2 * dot_uT;
    double NN2 = n_xl3_2*dot_xrT - dot_uT*dot_xru;

    double Zl = NN1/DD;
    double Zr = NN2/DD;

    double x1 = xl3.x*Zl;
    double y1 = xl3.y*Zl;
    double z1 = xl3.z*Zl;

    double xTemp = xr3.x*Zr - Tvec.x;
    double yTemp = xr3.y*Zr - Tvec.y;
    double zTemp = xr3.z*Zr - Tvec.z;

    double x2 = R[0]*xTemp + R[3]*yTemp + R[6]*zTemp;
    double y2 = R[1]*xTemp + R[4]*yTemp + R[7]*zTemp;
    double z2 = R[2]*xTemp + R[5]*yTemp + R[8]*zTemp;

    xTemp = (x1+x2)/2;
    yTemp = (y1+y2)/2;
    zTemp = (z1+z2)/2;

    if (LR) {
        return Vec3D<double>(xTemp, yTemp, zTemp);
    } else {
        return Vec3D<double>(R[0]*xTemp+R[1]*yTemp+R[2]*zTemp+Tvec.x
                             ,R[3]*xTemp+R[4]*yTemp+R[5]*zTemp+Tvec.y
                             ,R[6]*xTemp+R[7]*yTemp+R[8]*zTemp+Tvec.z);
    }

}