File content as of revision 0:765907e35737:

#ifndef AIR_DRUMMER_STEREOCAMERA
#define AIR_DRUMMER_STEREOCAMERA

template <class T>
struct Vec2D {
    T x;
    T y;
    Vec2D(T _x, T _y) {
        x = _x;
        y = _y;
    }

    T dot(const Vec2D<T> &other) const;
};

template <class T>
struct Vec3D {
    T x;
    T y;
    T z;
    Vec3D(T _x, T _y, T _z) {
        x = _x;
        y = _y;
        z = _z;
    }
    T dot(const Vec3D<T> &other) const;
};

class Camera
{
public:
    Camera(Vec2D<double> _fc, Vec2D<double> _cc = Vec2D<double>(0,0), double _kc1 = 0, double _kc2 = 0, double _kc3 = 0, double _kc4 = 0, double _kc5 = 0, double _alpha = 0)
        : fc(_fc)
        , cc(_cc)
        , k1(_kc1)
        , k2(_kc2)
        , p1(_kc3)
        , p2(_kc4)
        , k3(_kc5)
        , alpha(_alpha)
    {}

    Camera(double _fcX, double _fcY, double _ccX = 0, double _ccY = 0, double _kc1 = 0, double _kc2 = 0, double _kc3 = 0, double _kc4 = 0, double _kc5 = 0, double _alpha = 0)
        : fc(_fcX, _fcY)
        , cc(_ccX, _ccY)
        , k1(_kc1)
        , k2(_kc2)
        , p1(_kc3)
        , p2(_kc4)
        , k3(_kc5)
        , alpha(_alpha)
    {}

    Vec2D<double> undistortion(const Vec2D<double> &p) const;
    Vec2D<double> normalization(const Vec2D<int> &p) const;

private:
    // intrisic parameters
    Vec2D<double> fc;   // camera focal length
    Vec2D<double> cc;   // principal pont coordinates
    double k1;         // distortion coefficients
    double k2;
    double p1;
    double p2;
    double k3;
    double alpha;       //skew coefficient

};

class StereoCamera
{
public:
    StereoCamera(Camera _leftCam, Camera _rightCam, double* _R, Vec3D<double> _Tvec)
        : leftCam(_leftCam)
        , rightCam(_rightCam)
        , Tvec(_Tvec) {
        for (int i =0; i < 9; i++) {
            R[i] = _R[i];
        }
    }
    Vec3D<double> triangulation(const Vec2D<int> &pLeft, const Vec2D<int> &pRight, int LR = 1) const;

private:
    Camera leftCam;
    Camera rightCam;
    // extrinsic parameters: XR = R * XL + Tvec
    double R[9];    // rotation matrix
    Vec3D<double> Tvec;     // translation Vec
};

#endif //AIR_DRUMMER_STEREOCAMERA