├── README.md
├── Results
    ├── kitti00.png
    ├── kitti01.png
    ├── kitti02.png
    ├── kitti03.png
    ├── kitti04.png
    └── kitti05.png
└── src
    ├── Frame.cpp
    ├── Frame.hpp
    ├── MathOperations.h
    ├── camera.cpp
    ├── camera.hpp
    └── main.cpp


/README.md:
--------------------------------------------------------------------------------
 1 | # SimpleStereoVO
 2 | A very simple stereo visual odometer implemented in OpenCV.<br>
 3 | 
 4 | ## Some results: 
 5 | * For 'VO+IMU', I simply use the orientation provided by the IMU and the translation results from VO. <br>
 6 | * Video: http://v.youku.com/v_show/id_XMTgzMzczODI4MA==.html
 7 | ![image](https://github.com/meyiao/SimpleStereoVO/blob/master/Results/kitti00.png)<br>
 8 | ![image](https://github.com/meyiao/SimpleStereoVO/blob/master/Results/kitti01.png)<br>
 9 | ![image](https://github.com/meyiao/SimpleStereoVO/blob/master/Results/kitti02.png)<br>
10 | ![image](https://github.com/meyiao/SimpleStereoVO/blob/master/Results/kitti03.png)<br>
11 | ![image](https://github.com/meyiao/SimpleStereoVO/blob/master/Results/kitti04.png)<br>
12 | ![image](https://github.com/meyiao/SimpleStereoVO/blob/master/Results/kitti05.png)<br>
13 | 
14 | 
15 | ## Usage
16 | Specifying your dataset parameters (marked as !!!CHANGE!!!) in main.cpp<br>
17 | Note that the stereo images must be rectified.<br>
18 | 
19 | ## Dataset
20 | It runs properly on the KITTI benchmark dataset : http://www.cvlibs.net/datasets/kitti/eval_odometry.php <br>
21 | 
22 | 
23 | 


--------------------------------------------------------------------------------
/Results/kitti00.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/meyiao/SimpleStereoVO/af410243f7c44b7073fcd81116832ea4adeeab88/Results/kitti00.png


--------------------------------------------------------------------------------
/Results/kitti01.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/meyiao/SimpleStereoVO/af410243f7c44b7073fcd81116832ea4adeeab88/Results/kitti01.png


--------------------------------------------------------------------------------
/Results/kitti02.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/meyiao/SimpleStereoVO/af410243f7c44b7073fcd81116832ea4adeeab88/Results/kitti02.png


--------------------------------------------------------------------------------
/Results/kitti03.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/meyiao/SimpleStereoVO/af410243f7c44b7073fcd81116832ea4adeeab88/Results/kitti03.png


--------------------------------------------------------------------------------
/Results/kitti04.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/meyiao/SimpleStereoVO/af410243f7c44b7073fcd81116832ea4adeeab88/Results/kitti04.png


--------------------------------------------------------------------------------
/Results/kitti05.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/meyiao/SimpleStereoVO/af410243f7c44b7073fcd81116832ea4adeeab88/Results/kitti05.png


--------------------------------------------------------------------------------
/src/Frame.cpp:
--------------------------------------------------------------------------------
  1 | //
  2 | //  Frame.cpp
  3 | //  dabino
  4 | //
  5 | //  Created by 谭智丹 on 16/3/8.
  6 | //  Copyright © 2016年 谭智丹. All rights reserved.
  7 | //
  8 | 
  9 | #include "Frame.hpp"
 10 | 
 11 | Frame::Frame(Mat &img, int bs, Point2f ppt, float focalt, float baselinet)
 12 | {
 13 |     
 14 |     im_width = img.cols;
 15 |     im_height = img.rows;
 16 |     block_size = bs;
 17 |     pp = ppt;
 18 |     focal = focalt;
 19 |     baseline = baselinet;
 20 |     
 21 | }
 22 | 
 23 | 
 24 | // Assign corners into grids.
 25 | // We devide an image into small square grids. Then we assign the FAST corners into
 26 | // these grids. Each element of 'subidx' is a vector, which contains all the indices for
 27 | // corners that belong to this grid.
 28 | void Frame::GetFeaturesIntoGrid(vector<Point2f> &vPoints, vector<vector<int> > &subidx)
 29 | {
 30 |     subidx.clear();
 31 |     int n_xSub = im_width/block_size;
 32 |     int n_ySub = im_height/block_size;
 33 |     subidx.resize(n_xSub*n_ySub);
 34 |     
 35 |     for (int i=0; i<vPoints.size(); i++) {
 36 |         int xi = MIN(vPoints[i].x / block_size, n_xSub-1);
 37 |         int yi = MIN(vPoints[i].y / block_size, n_ySub-1);
 38 |         
 39 |         int numeli = xi*n_ySub + yi;
 40 |         subidx[numeli].push_back(i);
 41 |     }
 42 |     
 43 | }
 44 | 
 45 | 
 46 | // Sort corners contained in the same grid by their scores.
 47 | void Frame::SortSubIdx(vector<cv::KeyPoint> &vKeyPoints, vector<vector<int> > &subidx)
 48 | {
 49 |     for (size_t i=0; i<subidx.size(); i++) {
 50 |         vector<float> sub_response;
 51 |         vector<int> dsIdx;
 52 |         if (subidx[i].size()>1) {
 53 |             for (size_t j=0; j<subidx[i].size(); j++) {
 54 |                 sub_response.push_back(vKeyPoints[subidx[i][j]].response);
 55 |             }
 56 |             sortIdx(sub_response, dsIdx, CV_SORT_DESCENDING);
 57 |             for (size_t j=0; j<dsIdx.size(); j++) {
 58 |                 dsIdx[j] = subidx[i][dsIdx[j]];
 59 |             }
 60 |             for (size_t j=0; j<dsIdx.size(); j++) {
 61 |                 subidx[i][j] = dsIdx[j];
 62 |             }
 63 |         }
 64 |     }
 65 | }
 66 | 
 67 | 
 68 | // For each grid, only keep the top N FAST corners with highest scores.
 69 | void Frame::SelectTopN(vector<Point2f> &iPoints, vector<Point2f> &oPoints, vector<vector<int> > &subidx, int N)
 70 | {
 71 |     for (size_t i=0; i<subidx.size(); i++) {
 72 |         if (!subidx[i].empty()) {
 73 |             
 74 |             size_t numPick = MIN(subidx[i].size(), N);
 75 |             
 76 |             for (size_t j=0; j<numPick; j++) {
 77 |                 oPoints.push_back(iPoints[subidx[i][j]]);
 78 |             }
 79 |         }
 80 |     }
 81 | }
 82 | 
 83 | 
 84 | // Calculate stereo disparities.
 85 | // For stereo matching, I use the KLT optical-flow tracker for simplicity, it perfoms quite satisfaying.
 86 | // But it's rather slow, and not so robust in challenging illumilation environment.
 87 | // Instead you can use a similarity score matching method (SAD/SSD/NCC/ZNCC).
 88 | // Or you can choose to make the KLT tracker one-dimensional (along the stero epipolar line).
 89 | void Frame::FindDisparity(vector<Point2f> &pts1, vector<float>& disp ,cv::Mat &img1, cv::Mat &img2, float ythresh)
 90 | {
 91 |     vector<float> err;
 92 |     vector<uchar> status;
 93 |     vector<Point2f> pts2;
 94 |     calcOpticalFlowPyrLK(img1, img2, pts1, pts2, status, err, Size(31,31));
 95 |     
 96 |     size_t k=0;
 97 |     for (size_t i=0; i<pts1.size(); i++) {
 98 |         if (status[i]) {
 99 |             if (abs(pts1[i].y-pts2[i].y)<ythresh) {
100 |                 float diffx = (pts1[i].x - pts2[i].x);
101 |                 if ((diffx>0.5) &&(diffx<150)) {
102 |                     pts1[k] = pts1[i];
103 |                     disp.push_back(diffx);
104 |                     k++;
105 |                 }
106 |             }
107 |         }
108 |     }
109 |     
110 |     pts1.resize(k);
111 |     disp.resize(k);
112 |     
113 | }
114 | 
115 | 
116 | /*
117 | void Frame::st2rt(vector<float> &stat, cv::Mat &R, cv::Mat &T)
118 | {
119 |     float rx=stat[0], ry=stat[1], rz=stat[2];
120 |     R = (Mat_<float>(3,3)<<1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f);
121 |     T = (Mat_<float>(3,1)<<stat[3], stat[4], stat[5]);
122 |     
123 |     float r = sqrt(rx*rx + ry*ry + rz*rz);
124 |     if (r > 0.00001) {
125 |         rx /= r; ry /= r; rz /= r;
126 |         
127 |         // compute some quantity to avoid repetition
128 |         float ct = cos(r);
129 |         float st = sin(r);
130 |         float ict = 1.0f - ct;
131 |         float rxy_ict = rx * ry * ict;
132 |         float rxz_ict = rx * rz * ict;
133 |         float ryz_ict = ry * rz * ict;
134 |         float rx_st = rx * st;
135 |         float ry_st = ry * st;
136 |         float rz_st = rz * st;
137 |         
138 |         // calculate the elements of R
139 |         float r11, r12, r13, r21, r22, r23, r31, r32, r33;
140 |         r11 = ct + rx * rx * ict;
141 |         r12 = rxy_ict + rz_st;
142 |         r13 = rxz_ict - ry_st;
143 |         r21 = rxy_ict - rz_st;
144 |         r22 = ct + ry * ry * ict;
145 |         r23 = ryz_ict + rx_st;
146 |         r31 = rxz_ict + ry_st;
147 |         r32 = ryz_ict - rx_st;
148 |         r33 = ct + rz * rz * ict;
149 |         
150 |         R = (Mat_<float>(3,3)<<r11, r12, r13, r21, r22, r23, r31, r32, r33);
151 |         
152 |     }
153 |     
154 | }*/


--------------------------------------------------------------------------------
/src/Frame.hpp:
--------------------------------------------------------------------------------
 1 | //  Frame.hpp
 2 | //
 3 | //  Created by 谭谭谭 on 16/3/8.
 4 | //  Copyright © 2016年 谭谭谭. All rights reserved.
 5 | //
 6 | 
 7 | #ifndef Frame_hpp
 8 | #define Frame_hpp
 9 | 
10 | #include <stdio.h>
11 | #include <vector>
12 | 
13 | #include <opencv2/opencv.hpp>
14 | 
15 | using namespace cv;
16 | using namespace std;
17 | 
18 | class Frame
19 | {
20 | public:
21 |     Frame(Mat& img, int bs, Point2f ppt, float focalt, float baselinet);
22 |     
23 |     // Assign detected FAST corners into grids
24 |     void GetFeaturesIntoGrid(vector<Point2f>& vPoints, vector< vector<int> >& subidx);
25 |     
26 |     // For all FAST corners belonging to the same grid, we sort them according to their
27 |     // FAST-response scores.
28 |     void SortSubIdx(vector<KeyPoint>& vKeyPoints, vector<vector<int> >& subidx);
29 |     
30 |     // For each grid, only keep the N FAST corners with highest scores.
31 |     void SelectTopN(vector<Point2f>& iPoints, vector<Point2f>& oPoints, vector<vector<int>> & subidx, int N);
32 |     
33 |     // Calculate stereo disparities.
34 |     void FindDisparity(vector<Point2f>& pts1, vector<float>& disp, Mat& img1, Mat& img2, float ythresh);
35 |     
36 |     // Convert a state vector to rotaion matrix R, and translation vector T.
37 |     // void st2rt(vector<float>& stat, Mat& R, Mat& T);
38 |     
39 |     
40 | private:
41 |     int im_width;       // image width
42 |     int im_height;      // image height
43 |     int block_size;     // grid size
44 |     Point2f pp;         // principle point
45 |     float focal;        // focal length
46 |     float baseline;     // stereo base line
47 | };
48 | 
49 | #endif /* Frame_hpp */


--------------------------------------------------------------------------------
/src/MathOperations.h:
--------------------------------------------------------------------------------
  1 | //
  2 | //  MathOperations.h
  3 | //  dabino
  4 | //
  5 | //  Created by 谭智丹 on 16/4/27.
  6 | //  Copyright © 2016年 谭智丹. All rights reserved.
  7 | //
  8 | 
  9 | #ifndef MathOperations_h
 10 | #define MathOperations_h
 11 | 
 12 | #include<opencv2/opencv.hpp>
 13 | #include "camera.hpp"
 14 | 
 15 | using namespace cv;
 16 | using namespace std;
 17 | 
 18 | /* rvec2mat : convert a rvec into a rotation matrix R (R_Sn) */
 19 | void rvec2mat(Mat& rvec, Mat& R)
 20 | {
 21 |     // rotation angle
 22 |     double rx = rvec.at<double>(0,0);
 23 |     double ry = rvec.at<double>(1,0);
 24 |     double rz = rvec.at<double>(2,0);
 25 |     double theta = sqrt(rx*rx + ry*ry + rz*rz);
 26 |     
 27 |     // set rotation matrix to identity
 28 |     R = (Mat_<double>(3,3)<<1, 0, 0, 0, 1, 0, 0, 0, 1);
 29 |     
 30 |     // calculate rotaion matrix
 31 |     if (theta > 0.0001) {
 32 |         rx /= theta;
 33 |         ry /= theta;
 34 |         rz /= theta;
 35 |         
 36 |         // compute some quantity to avoid repetition
 37 |         double ct = cos(theta);
 38 |         double st = sin(theta);
 39 |         double ict = 1 - ct;
 40 |         double rxy_ict = rx * ry * ict;
 41 |         double rxz_ict = rx * rz * ict;
 42 |         double ryz_ict = ry * rz * ict;
 43 |         double rx_st = rx * st;
 44 |         double ry_st = ry * st;
 45 |         double rz_st = rz * st;
 46 |         
 47 |         // calculate the elements of R
 48 |         double R00 = ct + rx * rx * ict;
 49 |         double R01 = rxy_ict + rz_st;
 50 |         double R02 = rxz_ict - ry_st;
 51 |         double R10 = rxy_ict - rz_st;
 52 |         double R11 = ct + ry * ry * ict;
 53 |         double R12 = ryz_ict + rx_st;
 54 |         double R20 = rxz_ict + ry_st;
 55 |         double R21 = ryz_ict - rx_st;
 56 |         double R22 = ct + rz * rz * ict;
 57 |         
 58 |         R = (Mat_<double>(3,3)<< R00, R10, R20, R01, R11, R21, R02, R12, R22);
 59 |         
 60 |     }
 61 |     
 62 | }
 63 | 
 64 | 
 65 | /* Transform3D : Using R&T to transform a 3D point seen in one camera frame into another camera frame
 66 |  R : R_Cc0
 67 |  T : c0->c, described in the c0 frame
 68 |  */
 69 | void Transform3D(vector<Point3f>& oldPts3d, vector<Point3f>& newPts3d, Mat& R, Mat& T)
 70 | {
 71 |     // we need to convert R and T to float type
 72 |     Mat fR, fT;
 73 |     R.convertTo(fR, CV_32F);
 74 |     T.convertTo(fT, CV_32F);
 75 |     
 76 |     for (size_t i=0; i<oldPts3d.size(); i++) {
 77 |         Mat oldx(oldPts3d[i]);
 78 |         Mat x;
 79 |         x = fR*oldx + fT;
 80 |         
 81 |         newPts3d.push_back(Point3f(x.at<float>(0,0),x.at<float>(1,0),x.at<float>(2,0)));
 82 |     }
 83 | }
 84 | 
 85 | 
 86 | void projectObj2img(vector<Point3f>& pts3d, vector<Point2f>& pts2d, camera::camera cam, vector<bool>& isInImg)
 87 | {
 88 |     float fx = cam.fx, fy = cam.fy, cx = cam.cx, cy = cam.cy;
 89 |     float minu = 3, maxu = cam.imwidth -3;
 90 |     float minv = 3, maxv = cam.imheight-3;
 91 |     
 92 |     for (size_t i=0; i<pts3d.size(); i++) {
 93 |         if (pts3d[i].z == 0) {
 94 |             isInImg.push_back(false);
 95 |             pts2d.push_back(Point2f(pts3d[i].x, pts3d[i].y));
 96 |         }
 97 |         else {
 98 |             float invz = 1/pts3d[i].z;
 99 |             float u = fx*invz*pts3d[i].x + cx;
100 |             float v = fy*invz*pts3d[i].y + cy;
101 |             pts2d.push_back(Point2f(u, v));
102 |             
103 |             if (u<minu || u>maxu || v<minv || v>maxv )
104 |                 isInImg.push_back(false);
105 |             else
106 |                 isInImg.push_back(true);
107 |             
108 |         }
109 |     }
110 |     
111 | }
112 | 
113 | #endif /* MathOperations_h */
114 | 


--------------------------------------------------------------------------------
/src/camera.cpp:
--------------------------------------------------------------------------------
 1 | //
 2 | //  camera.cpp
 3 | //  dabino
 4 | //
 5 | //  Created by 谭智丹 on 16/4/27.
 6 | //  Copyright © 2016年 谭智丹. All rights reserved.
 7 | //
 8 | 
 9 | #include "camera.hpp"
10 | 
11 | /* we can use
12 |  camera cam(.., .., ......);
13 |  to set the camera intrinsics
14 |  */
15 | 
16 | camera::camera(int width, int height, float focalx, float focaly, float ppx, float ppy)
17 | {
18 |     imwidth = width;
19 |     imheight = height;
20 |     fx = focalx;
21 |     fy = focaly;
22 |     cx = ppx;
23 |     cy = ppy;
24 |     camMatrix = (Mat_<float>(3,3)<<focalx, 0.0f, ppx, 0.0f, focaly, ppy, 0.0f, 0.0f, 1.0f);
25 | }
26 | 


--------------------------------------------------------------------------------
/src/camera.hpp:
--------------------------------------------------------------------------------
 1 | //
 2 | //  camera.hpp
 3 | //  dabino
 4 | //
 5 | //  Created by 谭智丹 on 16/4/27.
 6 | //  Copyright © 2016年 谭智丹. All rights reserved.
 7 | //
 8 | 
 9 | #ifndef camera_hpp
10 | #define camera_hpp
11 | 
12 | #include <stdio.h>
13 | 
14 | #include <opencv2/opencv.hpp>
15 | 
16 | using namespace cv;
17 | 
18 | class camera
19 | {
20 | public:
21 |     camera(int width, int height, float focalx, float focaly, float ppx, float ppy);
22 |     float imwidth;      // image width
23 |     float imheight;     // image height
24 |     float fx;           // focal length x
25 |     float fy;           // focal length y
26 |     float cx;           // principal point's x coordinate
27 |     float cy;           // principal point's y coordinate
28 |     Mat   camMatrix;    // camera projection matrix
29 | };
30 | 
31 | 
32 | #endif /* camera_hpp */
33 | 


--------------------------------------------------------------------------------
/src/main.cpp:
--------------------------------------------------------------------------------
  1 | //
  2 | //  main.cpp
  3 | //  dabino
  4 | //
  5 | //  Created by 谭智丹 on 16/3/8.
  6 | //  Copyright © 2016年 谭智丹. All rights reserved.
  7 | //
  8 | 
  9 | #include <opencv2/core/core.hpp>
 10 | #include <opencv2/highgui/highgui.hpp>
 11 | #include <opencv2/imgproc/imgproc.hpp>
 12 | #include <opencv2/video/tracking.hpp>
 13 | #include <opencv2/features2d/features2d.hpp>
 14 | #include <vector>
 15 | #include <iostream>
 16 | #include <fstream>
 17 | 
 18 | #include "Frame.hpp"
 19 | #include "camera.hpp"
 20 | #include "MathOperations.h"
 21 | 
 22 | using namespace cv;
 23 | using namespace std;
 24 | 
 25 | //------------Dataset Parameters-----------------------//!!!!!!!!!!!!!CHANGE!!!!!!!!!!!!!!!!!!!
 26 | /*----------Kitti Sequence 00, 01, 02--------*/
 27 | const float focal = 718.856;
 28 | const Point2f pp = Point2f(607.1928, 185.2157);
 29 | 
 30 | /*----------Kitti Sequence 03--------*/
 31 | //const float focal = 721.5377;
 32 | //const Point2f pp = Point2f(609.5593, 172.854);
 33 | 
 34 | /*----------Kitti Sequence 04, 05, 06--------*/
 35 | //const float focal = 707.0912;
 36 | //const Point2f pp = Point2f(601.8873, 183.1104);
 37 | 
 38 | const float baseline = 0.53715;
 39 | 
 40 | const int total_frames = 4541;    // 00
 41 | //const int total_frames = 1101;      // 01
 42 | //const int total_frames = 4661;      // 02
 43 | //const int total_frames = 801;       // 03
 44 | //const int total_frames = 271;      // 04
 45 | //const int total_frames = 2761; // 05
 46 | //const int total_frames = 1101;   // 06
 47 |  
 48 | const Mat cam_mat = (Mat_<float>(3,3)<<focal, 0.0f, pp.x, 0.0f, focal, pp.y, 0.0f, 0.0f, 1.0f);
 49 | const float y_threshold = 1.0f;
 50 | 
 51 | //------------------------------------------------------------//
 52 | 
 53 | 
 54 | int main(int argc, const char * argv[]) {
 55 |     
 56 |     // The dataset file location
 57 |     string head_name = "/Users/tanzhidan/Documents/data/kitti/sequences/00/"; //!!!!!!!CHANGE!!!!!!!!!!!!!!!!!!
 58 |     
 59 |     // Char array used to store the image name
 60 |     char tail_name[20];
 61 |     
 62 |     // Read the first frame
 63 |     Mat fisrt_frame = imread(head_name + "image_0/000000.png", 0);//!!!!!!!!!!MAY NEED CHANGE!!!!!!!!!!!!!!!!!
 64 |     Frame frame(fisrt_frame, 30, pp, focal, baseline);
 65 |     int start_frame = 0;
 66 |     
 67 |     // Open a txt file to store the results
 68 |     ofstream fout("/Users/tanzhidan/Documents/kitti_0.txt"); // !!!!!!!!!!!CHANGE!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 69 |     if (!fout) {
 70 |         cout<<"File not opened!"<<endl;
 71 |         return 1;
 72 |     }
 73 | 
 74 |     // Create a window for display
 75 |     namedWindow("window", 0);
 76 |     
 77 |     // Create a camera object
 78 |     camera cam(fisrt_frame.cols, fisrt_frame.rows, focal, focal, pp.x, pp.y);
 79 |     
 80 |     // Containers for previous frame's information
 81 |     Mat old_img_l;              // image
 82 |     vector<Point2f> old_pts;    // corners' locations
 83 |     vector<float> old_disp;     // disparities
 84 |     
 85 |     // Termination criteria used when executing sub-pixel refinement
 86 |     TermCriteria crit = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01);
 87 |     
 88 |     // Pose state vector
 89 |     Mat oldRvec = (Mat_<double>(3,1)<<0, 0, 0); // rotation
 90 |     Mat oldTvec = (Mat_<double>(3,1)<<0, 0, 0); // translation
 91 |     
 92 |     
 93 |     // Here we go!
 94 |     for (int numFrame=start_frame; numFrame<total_frames; numFrame++) {
 95 |         
 96 |         clock_t ts = clock();
 97 |         
 98 |         // First of all, get the file name
 99 |         sprintf(tail_name, "%06d.png", numFrame);
100 |         cout << endl;
101 |         cout <<"# Frame:         \t"<<numFrame<<endl;
102 |         
103 |         // Load the stereo image pair
104 |         Mat img_l = imread(head_name+"image_0/"+tail_name, 0);
105 |         Mat img_r = imread(head_name+"image_1/"+tail_name, 0);
106 |         if (img_l.empty()) {
107 |             cout<<"Error: can not load the images!"<<endl;
108 |             return -1;
109 |         }
110 |         
111 |         // Detect fast corners
112 |         vector<KeyPoint> keyPts;
113 |         FAST(img_l, keyPts, 20, true, 2); // threshold is 20, do non-maximun suppression, 16-9 method
114 |         vector<Point2f> pts;
115 |         KeyPoint::convert(keyPts, pts);
116 |         cout<<"# detected:      \t"<<pts.size()<<endl;
117 |         
118 |         // Assign features into grids
119 |         vector<vector<int>> subidx;
120 |         frame.GetFeaturesIntoGrid(pts, subidx);
121 |         
122 |         // Sort the subidx
123 |         frame.SortSubIdx(keyPts, subidx);
124 |         int occupied=0;
125 |         for (size_t i=0; i<subidx.size(); i++) {
126 |             if (!subidx[i].empty()) {
127 |                 occupied++;
128 |             }
129 |         }
130 |         
131 |         // Pick up top points in each grid
132 |         vector<Point2f> topPts;
133 |         int want_in_each = MIN(1000 / occupied, 7);
134 |         
135 |         frame.SelectTopN(pts, topPts, subidx, want_in_each);
136 |         cout << "Selected:\t" << topPts.size() << endl;
137 |         
138 |         // Sub-pixel refinement (very optional!)
139 |         cornerSubPix(img_l, topPts, Size(5,5), Size(-1,-1), crit);
140 |         
141 |         // Stereo matching
142 |         vector<float> disp;
143 |         frame.FindDisparity(topPts, disp, img_l, img_r, y_threshold);
144 |         
145 |         
146 |         ///////
147 |         if (numFrame > start_frame) {
148 |             
149 |             //---------Predict points' locations in current frame------//
150 |             vector<Point3f> prevPts3d, prediPts3d;
151 |             vector<Point2f> prediPts;
152 |             vector<bool> canBeSeen;
153 |             
154 |             // 3D points described in last camera coordinates
155 |             for (size_t i=0; i<old_pts.size(); i++) {
156 |                 float b_by_d = baseline / old_disp[i];
157 |                 float Z = focal * b_by_d;
158 |                 float X = (old_pts[i].x-pp.x) * b_by_d;
159 |                 float Y = (old_pts[i].y-pp.y) * b_by_d;
160 |                 
161 |                 prevPts3d.push_back(Point3f(X,Y,Z));
162 |             }
163 |             
164 |             // Tranfrom to current-camera-coordinates.
165 |             // To predict current camera pose(relative to previous camera),
166 |             // we assume that it's just the same as (the relative pose between last_camera & last_last_camera).
167 |             Mat oldR;
168 |             rvec2mat(oldRvec, oldR);
169 |             Transform3D(prevPts3d, prediPts3d, oldR, oldTvec);
170 |             
171 |             // Project these points to image plane.
172 |             projectObj2img(prediPts3d, prediPts, cam, canBeSeen);
173 |             
174 |             
175 |             
176 |             
177 |             //-------------Tracker----------------------------------//
178 |             Mat err;
179 |             vector<uchar> f_status;
180 |             vector<Point2f> new_pts;
181 |             
182 |             // Optical flow tracker
183 |             new_pts = prediPts;
184 |             calcOpticalFlowPyrLK(old_img_l, img_l, old_pts, new_pts, f_status, err, Size(7,7));
185 |             
186 |             // Check: If the distance between the tracked and the predicted locations exceeds 15,
187 |             // it may be a wrong association, we just discard it.
188 |             size_t tracked = 0;
189 |             if (numFrame > start_frame+1) {
190 |                 for (size_t i=0; i<old_pts.size(); i++) {
191 |                     if (f_status[i]) {
192 |                         if (sqrt(pow(prediPts[i].x-new_pts[i].x, 2.0)+pow(prediPts[i].y-new_pts[i].y, 2))<15) {
193 |                             old_pts[tracked] = old_pts[i];
194 |                             new_pts[tracked] = new_pts[i];
195 |                             old_disp[tracked] = old_disp[i];
196 |                             tracked++;
197 |                         }
198 |                     }
199 |                 }
200 |                 old_pts.resize(tracked);
201 |                 new_pts.resize(tracked);
202 |                 old_disp.resize(tracked);
203 |                 cout<<"# tracked:       \t"<<tracked<<endl;
204 |             }
205 |             
206 |             
207 |             // Display
208 |             Mat img1_c;
209 |             cvtColor(old_img_l, img1_c, CV_GRAY2BGR);
210 |             for (size_t i=0; i<old_pts.size(); i++) {
211 |                 circle(img1_c, old_pts[i], 4, Scalar(0,0,255), -1);
212 |                 line(img1_c, old_pts[i], new_pts[i], Scalar(100,200,100), 2);
213 |             }
214 |             resize(img1_c, img1_c, Size(img1_c.cols/1.5, img1_c.rows));
215 |             imshow("window", img1_c);
216 |             
217 |             
218 |             
219 |             //---------------Compute the relative camera pose--------------//
220 |             // 1. Calculate the 3D points
221 |             vector<Point3f> pts3d;
222 |             for (size_t i=0; i<old_pts.size(); i++) {
223 |                 float b_by_d = baseline / old_disp[i];
224 |                 float Z = focal * b_by_d;
225 |                 float X = (old_pts[i].x-pp.x) * b_by_d;
226 |                 float Y = (old_pts[i].y-pp.y) * b_by_d;
227 |                 pts3d.push_back(Point3f(X,Y,Z));
228 |             }
229 |             
230 |             // 2. Ransac p3p
231 |             Mat rvec, tvec;
232 |             vector<int> inliers;
233 |             solvePnPRansac(pts3d, new_pts, cam_mat, Mat(), rvec, tvec, false, 500, 2.0f, 0.999, inliers, SOLVEPNP_ITERATIVE);
234 |             cout<<"# inliers:       \t"<<inliers.size()<<endl;
235 |             
236 |             // 3. Reject outliers, PnP
237 |             size_t k=0;
238 |             for (size_t i=0; i<inliers.size(); i++) {
239 |                 pts3d[k] = pts3d[inliers[i]];
240 |                 new_pts[k] = new_pts[inliers[i]];
241 |                 k++;
242 |             }
243 |             pts3d.resize(k);
244 |             new_pts.resize(k);
245 |           
246 |             solvePnP(pts3d, new_pts, cam_mat, Mat(), rvec, tvec);
247 |             
248 |             
249 |             
250 |             
251 |             //--------------------------------------------------------------//
252 |             // Check the result, in case it's too bad.
253 |             // We assumes that :
254 |             // 1. The car mainly moves foward
255 |             // 2. Speed < 35 m/s
256 |             // 3. The difference between previous&current translation < 1 m
257 |             if (numFrame > start_frame+2) {
258 |                 double tx = tvec.at<double>(0,0);
259 |                 double ty = tvec.at<double>(1,0);
260 |                 double tz = tvec.at<double>(2,0);
261 |                 double oldtx = oldTvec.at<double>(0,0);
262 |                 double oldty = oldTvec.at<double>(1,0);
263 |                 double oldtz = oldTvec.at<double>(2,0);
264 |                 double difft = sqrt(pow(tx-oldtx, 2) + pow(ty-oldty, 2) + pow(tz-oldtz, 2));
265 |                 if (fabs(tx)>0.5 || fabs(ty)>0.5 || fabs(tz)>3.5 || difft > 1) {
266 |                     rvec = oldRvec;
267 |                     tvec = oldTvec;
268 |                 }
269 |             }
270 |             
271 |             
272 |             // Save the result
273 |             fout<<numFrame<<"\t";
274 |             fout<<rvec.at<double>(0,0)<<"  \t";
275 |             fout<<rvec.at<double>(1,0)<<"  \t";
276 |             fout<<rvec.at<double>(2,0)<<"  \t";
277 |             fout<<tvec.at<double>(0,0)<<"  \t";
278 |             fout<<tvec.at<double>(1,0)<<"  \t";
279 |             fout<<tvec.at<double>(2,0)<<"  \t";
280 |             fout<<new_pts.size()<<endl;
281 |             
282 |             
283 |             // Update oldRvec and oldTvec
284 |             oldRvec = rvec;
285 |             oldTvec = tvec;
286 |             
287 | 
288 |         }
289 |         
290 |         // Update
291 |         old_img_l = img_l.clone();
292 |         old_pts = topPts;
293 |         old_disp = disp;
294 |         
295 |         clock_t te = clock();
296 |         cout << "time\t" << (double)(te-ts)/CLOCKS_PER_SEC << "\n";
297 |     
298 | 
299 |     }
300 |     
301 |     fout.close();
302 |     
303 |     return 0;
304 |     
305 | }
306 | 


--------------------------------------------------------------------------------