Revision 0:765907e35737, committed 2015-03-12

Comitter:

CheeseW

Date:

Thu Mar 12 08:14:47 2015 +0000

Commit message:

Finished version; Work well

Changed in this revision

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

--- /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