Diff: StereoCamera.h

Revision:

0:765907e35737

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StereoCamera.h	Thu Mar 12 08:14:47 2015 +0000
@@ -0,0 +1,91 @@
+#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
\ No newline at end of file