Qisi Wang / StereoTriagulation

For doing stereo triangulation of 2 camera obtaining corresponding feature points

Dependents:   AiRDrummer_Visual

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?

UserRevisionLine numberNew 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 }