Qisi Wang / StereoTriagulation

For doing stereo triangulation of 2 camera obtaining corresponding feature points

Dependents:   AiRDrummer_Visual

StereoCamera.h@0:765907e35737, 2015-03-12 (annotated)

Committer:
CheeseW
Date:
Thu Mar 12 08:14:47 2015 +0000
Revision:
0:765907e35737
Finished version; Work well

Who changed what in which revision?

UserRevisionLine numberNew contents of line
CheeseW 0:765907e35737 1 #ifndef AIR_DRUMMER_STEREOCAMERA
CheeseW 0:765907e35737 2 #define AIR_DRUMMER_STEREOCAMERA
CheeseW 0:765907e35737 3
CheeseW 0:765907e35737 4 template <class T>
CheeseW 0:765907e35737 5 struct Vec2D {
CheeseW 0:765907e35737 6 T x;
CheeseW 0:765907e35737 7 T y;
CheeseW 0:765907e35737 8 Vec2D(T _x, T _y) {
CheeseW 0:765907e35737 9 x = _x;
CheeseW 0:765907e35737 10 y = _y;
CheeseW 0:765907e35737 11 }
CheeseW 0:765907e35737 12
CheeseW 0:765907e35737 13 T dot(const Vec2D<T> &other) const;
CheeseW 0:765907e35737 14 };
CheeseW 0:765907e35737 15
CheeseW 0:765907e35737 16 template <class T>
CheeseW 0:765907e35737 17 struct Vec3D {
CheeseW 0:765907e35737 18 T x;
CheeseW 0:765907e35737 19 T y;
CheeseW 0:765907e35737 20 T z;
CheeseW 0:765907e35737 21 Vec3D(T _x, T _y, T _z) {
CheeseW 0:765907e35737 22 x = _x;
CheeseW 0:765907e35737 23 y = _y;
CheeseW 0:765907e35737 24 z = _z;
CheeseW 0:765907e35737 25 }
CheeseW 0:765907e35737 26 T dot(const Vec3D<T> &other) const;
CheeseW 0:765907e35737 27 };
CheeseW 0:765907e35737 28
CheeseW 0:765907e35737 29 class Camera
CheeseW 0:765907e35737 30 {
CheeseW 0:765907e35737 31 public:
CheeseW 0:765907e35737 32 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)
CheeseW 0:765907e35737 33 : fc(_fc)
CheeseW 0:765907e35737 34 , cc(_cc)
CheeseW 0:765907e35737 35 , k1(_kc1)
CheeseW 0:765907e35737 36 , k2(_kc2)
CheeseW 0:765907e35737 37 , p1(_kc3)
CheeseW 0:765907e35737 38 , p2(_kc4)
CheeseW 0:765907e35737 39 , k3(_kc5)
CheeseW 0:765907e35737 40 , alpha(_alpha)
CheeseW 0:765907e35737 41 {}
CheeseW 0:765907e35737 42
CheeseW 0:765907e35737 43 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)
CheeseW 0:765907e35737 44 : fc(_fcX, _fcY)
CheeseW 0:765907e35737 45 , cc(_ccX, _ccY)
CheeseW 0:765907e35737 46 , k1(_kc1)
CheeseW 0:765907e35737 47 , k2(_kc2)
CheeseW 0:765907e35737 48 , p1(_kc3)
CheeseW 0:765907e35737 49 , p2(_kc4)
CheeseW 0:765907e35737 50 , k3(_kc5)
CheeseW 0:765907e35737 51 , alpha(_alpha)
CheeseW 0:765907e35737 52 {}
CheeseW 0:765907e35737 53
CheeseW 0:765907e35737 54 Vec2D<double> undistortion(const Vec2D<double> &p) const;
CheeseW 0:765907e35737 55 Vec2D<double> normalization(const Vec2D<int> &p) const;
CheeseW 0:765907e35737 56
CheeseW 0:765907e35737 57 private:
CheeseW 0:765907e35737 58 // intrisic parameters
CheeseW 0:765907e35737 59 Vec2D<double> fc; // camera focal length
CheeseW 0:765907e35737 60 Vec2D<double> cc; // principal pont coordinates
CheeseW 0:765907e35737 61 double k1; // distortion coefficients
CheeseW 0:765907e35737 62 double k2;
CheeseW 0:765907e35737 63 double p1;
CheeseW 0:765907e35737 64 double p2;
CheeseW 0:765907e35737 65 double k3;
CheeseW 0:765907e35737 66 double alpha; //skew coefficient
CheeseW 0:765907e35737 67
CheeseW 0:765907e35737 68 };
CheeseW 0:765907e35737 69
CheeseW 0:765907e35737 70 class StereoCamera
CheeseW 0:765907e35737 71 {
CheeseW 0:765907e35737 72 public:
CheeseW 0:765907e35737 73 StereoCamera(Camera _leftCam, Camera _rightCam, double* _R, Vec3D<double> _Tvec)
CheeseW 0:765907e35737 74 : leftCam(_leftCam)
CheeseW 0:765907e35737 75 , rightCam(_rightCam)
CheeseW 0:765907e35737 76 , Tvec(_Tvec) {
CheeseW 0:765907e35737 77 for (int i =0; i < 9; i++) {
CheeseW 0:765907e35737 78 R[i] = _R[i];
CheeseW 0:765907e35737 79 }
CheeseW 0:765907e35737 80 }
CheeseW 0:765907e35737 81 Vec3D<double> triangulation(const Vec2D<int> &pLeft, const Vec2D<int> &pRight, int LR = 1) const;
CheeseW 0:765907e35737 82
CheeseW 0:765907e35737 83 private:
CheeseW 0:765907e35737 84 Camera leftCam;
CheeseW 0:765907e35737 85 Camera rightCam;
CheeseW 0:765907e35737 86 // extrinsic parameters: XR = R * XL + Tvec
CheeseW 0:765907e35737 87 double R[9]; // rotation matrix
CheeseW 0:765907e35737 88 Vec3D<double> Tvec; // translation Vec
CheeseW 0:765907e35737 89 };
CheeseW 0:765907e35737 90
CheeseW 0:765907e35737 91 #endif //AIR_DRUMMER_STEREOCAMERA