For doing stereo triangulation of 2 camera obtaining corresponding feature points
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
StereoCamera.cpp | Show annotated file Show diff for this revision Revisions of this file |
StereoCamera.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 000000000000 -r 765907e35737 StereoCamera.cpp --- /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
diff -r 000000000000 -r 765907e35737 StereoCamera.h --- /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