For doing stereo triangulation of 2 camera obtaining corresponding feature points
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?
User | Revision | Line number | New 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 |