For doing stereo triangulation of 2 camera obtaining corresponding feature points
StereoCamera.cpp@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 | #include "StereoCamera.h" |
CheeseW | 0:765907e35737 | 2 | |
CheeseW | 0:765907e35737 | 3 | template <class T> |
CheeseW | 0:765907e35737 | 4 | T Vec2D<T>::dot(const Vec2D<T> &other) const |
CheeseW | 0:765907e35737 | 5 | { |
CheeseW | 0:765907e35737 | 6 | return this->x*other.x+this->y*other.y; |
CheeseW | 0:765907e35737 | 7 | } |
CheeseW | 0:765907e35737 | 8 | |
CheeseW | 0:765907e35737 | 9 | template <class T> |
CheeseW | 0:765907e35737 | 10 | T Vec3D<T>::dot(const Vec3D<T> &other) const |
CheeseW | 0:765907e35737 | 11 | { |
CheeseW | 0:765907e35737 | 12 | return this->x*other.x+this->y*other.y + this->z*other.z; |
CheeseW | 0:765907e35737 | 13 | } |
CheeseW | 0:765907e35737 | 14 | |
CheeseW | 0:765907e35737 | 15 | Vec2D<double> Camera::undistortion(const Vec2D<double> &p) const |
CheeseW | 0:765907e35737 | 16 | { |
CheeseW | 0:765907e35737 | 17 | Vec2D<double> rp(p); |
CheeseW | 0:765907e35737 | 18 | double r_2,kRadial,dx,dy; |
CheeseW | 0:765907e35737 | 19 | for (int i = 0; i < 20; i++) { |
CheeseW | 0:765907e35737 | 20 | r_2 = rp.dot(rp); |
CheeseW | 0:765907e35737 | 21 | kRadial = 1 + k1 * r_2+k2*r_2*r_2+k3*r_2*r_2*r_2; // radial distortion |
CheeseW | 0:765907e35737 | 22 | dx = 2*p1*rp.x*rp.y+p2*(r_2+2*rp.x*rp.x); |
CheeseW | 0:765907e35737 | 23 | dy = p1*(r_2+2*rp.y*rp.y)+2*p2*rp.x*rp.y; |
CheeseW | 0:765907e35737 | 24 | rp.x = (p.x-dx)/kRadial; |
CheeseW | 0:765907e35737 | 25 | rp.y = (p.y-dy)/kRadial; |
CheeseW | 0:765907e35737 | 26 | } |
CheeseW | 0:765907e35737 | 27 | return rp; |
CheeseW | 0:765907e35737 | 28 | } |
CheeseW | 0:765907e35737 | 29 | Vec2D<double> Camera::normalization(const Vec2D<int> &p) const |
CheeseW | 0:765907e35737 | 30 | { |
CheeseW | 0:765907e35737 | 31 | Vec2D<double> rp(0,0); |
CheeseW | 0:765907e35737 | 32 | // Subtract pincipal point and divide by the focal length |
CheeseW | 0:765907e35737 | 33 | rp.x = (p.x-cc.x)/fc.x; |
CheeseW | 0:765907e35737 | 34 | rp.y = (p.y-cc.y)/fc.y; |
CheeseW | 0:765907e35737 | 35 | |
CheeseW | 0:765907e35737 | 36 | // Undo skew |
CheeseW | 0:765907e35737 | 37 | rp.x = rp.x - alpha*rp.y; |
CheeseW | 0:765907e35737 | 38 | |
CheeseW | 0:765907e35737 | 39 | |
CheeseW | 0:765907e35737 | 40 | // Compensate for lens distortion |
CheeseW | 0:765907e35737 | 41 | if (k1||k2||p1||p2||k3) { |
CheeseW | 0:765907e35737 | 42 | return undistortion(rp); |
CheeseW | 0:765907e35737 | 43 | } else { |
CheeseW | 0:765907e35737 | 44 | return rp; |
CheeseW | 0:765907e35737 | 45 | } |
CheeseW | 0:765907e35737 | 46 | } |
CheeseW | 0:765907e35737 | 47 | |
CheeseW | 0:765907e35737 | 48 | Vec3D<double> StereoCamera::triangulation(const Vec2D<int> &pLeft, const Vec2D<int> &pRight, int LR) const |
CheeseW | 0:765907e35737 | 49 | { |
CheeseW | 0:765907e35737 | 50 | // Normalize hte image projection according ot the intrinsic parameters of the left and right cameras |
CheeseW | 0:765907e35737 | 51 | Vec2D<double> xl2 = leftCam.normalization(pLeft); |
CheeseW | 0:765907e35737 | 52 | Vec2D<double> xr2 = rightCam.normalization(pRight); |
CheeseW | 0:765907e35737 | 53 | |
CheeseW | 0:765907e35737 | 54 | // Extend the normalized projections in homogeneous coordinates |
CheeseW | 0:765907e35737 | 55 | Vec3D<double> xl3(xl2.x,xl2.y,1); |
CheeseW | 0:765907e35737 | 56 | Vec3D<double> xr3(xr2.x,xr2.y,1); |
CheeseW | 0:765907e35737 | 57 | |
CheeseW | 0:765907e35737 | 58 | Vec3D<double> u(R[0]*xl3.x+R[1]*xl3.y+R[2]*xl3.z, |
CheeseW | 0:765907e35737 | 59 | R[3]*xl3.x+R[4]*xl3.y+R[5]*xl3.z, |
CheeseW | 0:765907e35737 | 60 | R[6]*xl3.x+R[7]*xl3.y+R[8]*xl3.z); |
CheeseW | 0:765907e35737 | 61 | |
CheeseW | 0:765907e35737 | 62 | double n_xl3_2 = xl3.dot(xl3); |
CheeseW | 0:765907e35737 | 63 | double n_xr3_2 = xr3.dot(xr3); |
CheeseW | 0:765907e35737 | 64 | |
CheeseW | 0:765907e35737 | 65 | double DD = n_xl3_2 * n_xr3_2 - (u.dot(xr3))*(u.dot(xr3)); |
CheeseW | 0:765907e35737 | 66 | |
CheeseW | 0:765907e35737 | 67 | double dot_uT = u.dot(Tvec); |
CheeseW | 0:765907e35737 | 68 | double dot_xrT = xr3.dot(Tvec); |
CheeseW | 0:765907e35737 | 69 | double dot_xru = xr3.dot(u); |
CheeseW | 0:765907e35737 | 70 | |
CheeseW | 0:765907e35737 | 71 | double NN1 = dot_xru*dot_xrT - n_xr3_2 * dot_uT; |
CheeseW | 0:765907e35737 | 72 | double NN2 = n_xl3_2*dot_xrT - dot_uT*dot_xru; |
CheeseW | 0:765907e35737 | 73 | |
CheeseW | 0:765907e35737 | 74 | double Zl = NN1/DD; |
CheeseW | 0:765907e35737 | 75 | double Zr = NN2/DD; |
CheeseW | 0:765907e35737 | 76 | |
CheeseW | 0:765907e35737 | 77 | double x1 = xl3.x*Zl; |
CheeseW | 0:765907e35737 | 78 | double y1 = xl3.y*Zl; |
CheeseW | 0:765907e35737 | 79 | double z1 = xl3.z*Zl; |
CheeseW | 0:765907e35737 | 80 | |
CheeseW | 0:765907e35737 | 81 | double xTemp = xr3.x*Zr - Tvec.x; |
CheeseW | 0:765907e35737 | 82 | double yTemp = xr3.y*Zr - Tvec.y; |
CheeseW | 0:765907e35737 | 83 | double zTemp = xr3.z*Zr - Tvec.z; |
CheeseW | 0:765907e35737 | 84 | |
CheeseW | 0:765907e35737 | 85 | double x2 = R[0]*xTemp + R[3]*yTemp + R[6]*zTemp; |
CheeseW | 0:765907e35737 | 86 | double y2 = R[1]*xTemp + R[4]*yTemp + R[7]*zTemp; |
CheeseW | 0:765907e35737 | 87 | double z2 = R[2]*xTemp + R[5]*yTemp + R[8]*zTemp; |
CheeseW | 0:765907e35737 | 88 | |
CheeseW | 0:765907e35737 | 89 | xTemp = (x1+x2)/2; |
CheeseW | 0:765907e35737 | 90 | yTemp = (y1+y2)/2; |
CheeseW | 0:765907e35737 | 91 | zTemp = (z1+z2)/2; |
CheeseW | 0:765907e35737 | 92 | |
CheeseW | 0:765907e35737 | 93 | if (LR) { |
CheeseW | 0:765907e35737 | 94 | return Vec3D<double>(xTemp, yTemp, zTemp); |
CheeseW | 0:765907e35737 | 95 | } else { |
CheeseW | 0:765907e35737 | 96 | return Vec3D<double>(R[0]*xTemp+R[1]*yTemp+R[2]*zTemp+Tvec.x |
CheeseW | 0:765907e35737 | 97 | ,R[3]*xTemp+R[4]*yTemp+R[5]*zTemp+Tvec.y |
CheeseW | 0:765907e35737 | 98 | ,R[6]*xTemp+R[7]*yTemp+R[8]*zTemp+Tvec.z); |
CheeseW | 0:765907e35737 | 99 | } |
CheeseW | 0:765907e35737 | 100 | |
CheeseW | 0:765907e35737 | 101 | } |