├── README.md ├── mono_vo_2d-2d ├── .gitignore ├── CMakeLists.txt ├── result │ ├── 00.png │ ├── 01.png │ ├── 02.png │ ├── 03.png │ ├── 04.png │ ├── 05.png │ ├── 06.png │ ├── 07.png │ ├── 08.png │ ├── 09.png │ └── 10.png └── src │ ├── vo.cpp │ └── vo.h ├── stereo_vo_2d-3d ├── CMakeLists.txt ├── result │ ├── 0.png │ ├── 5.png │ ├── 6.png │ ├── 7.png │ └── 8.png └── src │ ├── main.cpp │ ├── vo.cpp │ └── vo.h └── wudao_final.pdf /README.md: -------------------------------------------------------------------------------- 1 | # visual odometry 2 | 3 | This is an OpenCV 3.4 implementation on [KITTI's Visual Odometry Dataset (greyscale)](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) 4 | 5 | ## Method 6 | 2D-2D Monocular VO and 2D-3D Stereo VO 7 | Implemenation details in [report](wudao_final.pdf) 8 | 9 | ## Compile 10 | In 2 directory: 11 | ```bash 12 | mkdir build 13 | cd build 14 | cmake .. 15 | make 16 | ``` 17 | 18 | ## Run 19 | After compilation, in the build directly 20 | Mono 21 | ```bash 22 | ./vo [sequence ID] [dataset directory] 23 | ``` 24 | Stereo 25 | ```bash 26 | ./vo [sequence ID] [max frame] [optimize frame] [dataset directory] 27 | ``` 28 | 29 | ## Performance 30 | ![set0](./mono_vo_2d-2d/result/00.png) 31 | ![set1](./stereo_vo_2d-3d/result/0.png) 32 | -------------------------------------------------------------------------------- /mono_vo_2d-2d/.gitignore: -------------------------------------------------------------------------------- 1 | /build -------------------------------------------------------------------------------- /mono_vo_2d-2d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project( vo ) 3 | 4 | find_package( OpenCV 3.0 REQUIRED ) 5 | include_directories( ${OpenCV_INCLUDE_DIRS} ) 6 | 7 | file(GLOB vo 8 | "src/*.h" 9 | "src/*.cpp" 10 | ) 11 | add_executable( vo ${vo} ) 12 | target_link_libraries( vo ${OpenCV_LIBS} ) 13 | -------------------------------------------------------------------------------- /mono_vo_2d-2d/result/00.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/mono_vo_2d-2d/result/00.png -------------------------------------------------------------------------------- /mono_vo_2d-2d/result/01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/mono_vo_2d-2d/result/01.png -------------------------------------------------------------------------------- /mono_vo_2d-2d/result/02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/mono_vo_2d-2d/result/02.png -------------------------------------------------------------------------------- /mono_vo_2d-2d/result/03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/mono_vo_2d-2d/result/03.png -------------------------------------------------------------------------------- /mono_vo_2d-2d/result/04.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/mono_vo_2d-2d/result/04.png -------------------------------------------------------------------------------- /mono_vo_2d-2d/result/05.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/mono_vo_2d-2d/result/05.png -------------------------------------------------------------------------------- /mono_vo_2d-2d/result/06.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/mono_vo_2d-2d/result/06.png -------------------------------------------------------------------------------- /mono_vo_2d-2d/result/07.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/mono_vo_2d-2d/result/07.png -------------------------------------------------------------------------------- /mono_vo_2d-2d/result/08.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/mono_vo_2d-2d/result/08.png -------------------------------------------------------------------------------- /mono_vo_2d-2d/result/09.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/mono_vo_2d-2d/result/09.png -------------------------------------------------------------------------------- /mono_vo_2d-2d/result/10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/mono_vo_2d-2d/result/10.png -------------------------------------------------------------------------------- /mono_vo_2d-2d/src/vo.cpp: -------------------------------------------------------------------------------- 1 | #include "vo.h" 2 | 3 | #define MIN_NUM_FEAT 2000 4 | 5 | // global 6 | int SEQ; // sequence id 7 | char* DIR; // dataset directory 8 | 9 | 10 | void getCalib(double* focal, Point2f& pp){ 11 | char filename[100]; 12 | sprintf(filename, "%s/sequences/%02d/calib.txt", DIR, SEQ); 13 | ifstream myfile(filename); 14 | 15 | if (myfile.is_open()){ 16 | string line; string camera = "P0: "; 17 | while (getline(myfile, line)){ // find right calib line 18 | if (line.compare(0, camera.length(), camera)==0) break; 19 | } 20 | 21 | double entry; 22 | istringstream ss(line.substr(4)); // remove camera header 23 | for (int i=0; i<12;i++){ 24 | ss >> entry; 25 | if (i==0) *focal=entry; 26 | if (i==2) pp.x=entry; 27 | if (i==6) pp.y=entry; 28 | } 29 | } else { 30 | cerr << "unable to open file"; 31 | exit(2); 32 | } 33 | } 34 | 35 | 36 | void getTruth(vector& scales, vector< vector >& truth){ 37 | char filename[100]; 38 | sprintf(filename, "%s/poses/%02d.txt", DIR, SEQ); 39 | ifstream myfile(filename); 40 | 41 | double x, y, z; 42 | double x_prev=0, y_prev=0, z_prev=0; 43 | double scale; 44 | vector pose; 45 | 46 | string line; double entry; 47 | if (myfile.is_open()){ 48 | while(getline(myfile, line)){ 49 | istringstream ss(line); 50 | for (int i=0; i<12;i++){ 51 | ss >> entry; 52 | if (i==3) x=entry; 53 | if (i==7) y=entry; 54 | if (i==11) z=entry; 55 | } 56 | scale = sqrt((x-x_prev)*(x-x_prev) + (y-y_prev)*(y-y_prev) + (z-z_prev)*(z-z_prev)); 57 | scales.push_back(scale); 58 | 59 | x_prev=x; y_prev=y; z_prev=z; 60 | 61 | pose.clear(); 62 | pose.push_back(x); pose.push_back(y); pose.push_back(z); 63 | truth.push_back(pose); 64 | } 65 | myfile.close(); 66 | } else { 67 | cerr << "unable to open file"; 68 | exit(3); 69 | } 70 | } 71 | 72 | 73 | int main( int argc, char** argv ) { 74 | // parse command 75 | if (argc != 3){ 76 | cerr << "Syntax: " << argv[0] << " [seq id] [dataset dir]\n"; 77 | exit(1); 78 | } 79 | SEQ = atoi(argv[1]); 80 | DIR = argv[2]; 81 | 82 | // load calibration 83 | double focal; Point2f pp; getCalib(&focal, pp); 84 | // load scale 85 | vector scales; vector< vector > truth; getTruth(scales, truth); 86 | 87 | // init 88 | Mat prevImage, currImage; 89 | vector prevFeatures, currFeatures; vector status; 90 | double scale; 91 | char filename[100]; 92 | 93 | //read the first two frames from the dataset 94 | sprintf(filename, "%s/sequences/%02d/image_0/%06d.png", DIR, SEQ, 0); 95 | prevImage = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); 96 | sprintf(filename, "%s/sequences/%02d/image_0/%06d.png", DIR, SEQ, 1); 97 | currImage = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); 98 | 99 | if ( !prevImage.data || !currImage.data) { 100 | cerr<< "ERR: reading images " << endl; return -1; 101 | } 102 | 103 | // feature detection and tracking 104 | featureDetection(prevImage, prevFeatures); 105 | featureTracking(prevImage, currImage, prevFeatures, currFeatures, status); 106 | 107 | // recovering the pose and the essential matrix 108 | Mat E, R, t, mask; 109 | E = findEssentialMat(currFeatures, prevFeatures, focal, pp, RANSAC, 0.999, 1.0, mask); 110 | recoverPose(E, currFeatures, prevFeatures, R, t, focal, pp, mask); 111 | 112 | // rotation and tranlation vectors in camera frame (not world frame!!!) 113 | Mat R_f = R.clone(); 114 | Mat t_f = t.clone(); 115 | 116 | // update for iteration 117 | prevImage = currImage.clone(); 118 | prevFeatures = currFeatures; 119 | 120 | char text[100]; 121 | Point textOrg(50, 950); 122 | 123 | clock_t begin = clock(); 124 | 125 | namedWindow( "Monocular", WINDOW_AUTOSIZE );// Create a window for display. 126 | namedWindow( "Trajectory", WINDOW_AUTOSIZE );// Create a window for display. 127 | 128 | Mat traj = Mat::zeros(1000, 1000, CV_8UC3); 129 | 130 | for(int numFrame=2; ; numFrame++) { 131 | sprintf(filename, "%s/sequences/%02d/image_0/%06d.png", DIR, SEQ, numFrame); 132 | currImage = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); 133 | if (!currImage.data) break; // no frame now 134 | 135 | featureTracking(prevImage, currImage, prevFeatures, currFeatures, status); 136 | 137 | // epipolar geometry 138 | E = findEssentialMat(currFeatures, prevFeatures, focal, pp, RANSAC, 0.999, 1.0, mask); 139 | recoverPose(E, currFeatures, prevFeatures, R, t, focal, pp, mask); 140 | 141 | scale = scales[numFrame]; 142 | if (scale>0.15 && t.at(2)>t.at(0) && t.at(2)>t.at(1)) { 143 | t_f = t_f + scale*(R_f*t); 144 | R_f = R_f*R; 145 | } // else the frame is abandon 146 | 147 | // new feature detection is triggered if features drop below a threshold 148 | if (prevFeatures.size() < MIN_NUM_FEAT) { 149 | featureDetection(prevImage, prevFeatures); 150 | featureTracking(prevImage,currImage,prevFeatures,currFeatures, status); 151 | } 152 | 153 | // update for iteration 154 | prevImage = currImage.clone(); 155 | prevFeatures = currFeatures; 156 | 157 | // plot 158 | int x_true = truth[numFrame][0]+500; 159 | int z_true = -truth[numFrame][2]+500; 160 | circle(traj, Point(x_true, z_true) ,1, CV_RGB(0,255,0), 2); 161 | int x = int(t_f.at(0)) + 500; 162 | int z = -int(t_f.at(2)) + 500; 163 | circle(traj, Point(x, z) ,1, CV_RGB(255,0,0), 2); 164 | 165 | // update text 166 | rectangle( traj, Point(50, 930), Point(950, 960), CV_RGB(0,0,0), CV_FILLED); 167 | sprintf(text, "x = %02fm y = %02fm z = %02fm frame: %d features: %lu time: %02fs", 168 | t_f.at(0), t_f.at(1), t_f.at(2), numFrame, currFeatures.size(), double(clock()-begin)/CLOCKS_PER_SEC); 169 | putText(traj, text, textOrg, FONT_HERSHEY_PLAIN, 1, Scalar::all(255), 1, 8); 170 | 171 | imshow( "Monocular", currImage); 172 | imshow( "Trajectory", traj); 173 | waitKey(1); 174 | 175 | } 176 | 177 | // save result 178 | putText(traj, "Feature-based Monocular VO", Point(250, 900), FONT_HERSHEY_PLAIN, 2, Scalar::all(255), 2, 8); 179 | sprintf(filename, "../result/%02d.png", SEQ); 180 | imwrite(filename, traj); 181 | 182 | return 0; 183 | } -------------------------------------------------------------------------------- /mono_vo_2d-2d/src/vo.h: -------------------------------------------------------------------------------- 1 | #include "opencv2/video/tracking.hpp" 2 | #include "opencv2/imgproc/imgproc.hpp" 3 | #include "opencv2/highgui/highgui.hpp" 4 | #include "opencv2/features2d/features2d.hpp" 5 | #include "opencv2/calib3d/calib3d.hpp" 6 | 7 | 8 | #include 9 | #include 10 | #include // for copy 11 | #include // for ostream_iterator 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace cv; 19 | using namespace std; 20 | 21 | void featureTracking(Mat img_1, Mat img_2, vector& points1, vector& points2, vector& status) { 22 | 23 | //this function automatically gets rid of points for which tracking fails 24 | 25 | vector err; 26 | Size winSize=Size(21,21); 27 | TermCriteria termcrit=TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01); // max count + epsilon 28 | 29 | calcOpticalFlowPyrLK(img_1, img_2, points1, points2, status, err, winSize, 3, termcrit, 0, 0.001); 30 | 31 | //getting rid of points for which the KLT tracking failed or those who have gone outside the frame 32 | int indexCorrection = 0; 33 | for( int i=0; i& points1) { //uses FAST as of now, modify parameters as necessary 50 | vector keypoints_1; 51 | int fast_threshold = 20; 52 | bool nonmaxSuppression = true; 53 | FAST(img_1, keypoints_1, fast_threshold, nonmaxSuppression); 54 | KeyPoint::convert(keypoints_1, points1, vector()); 55 | // cerr << "feature detected: " << points1.size() << endl; 56 | } 57 | -------------------------------------------------------------------------------- /stereo_vo_2d-3d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED( VERSION 2.8 ) 2 | 3 | PROJECT( vo ) 4 | SET( CMAKE_CXX_FLAGS "-std=c++11") 5 | 6 | find_package( OpenCV REQUIRED ) 7 | include_directories( ${OpenCV_INCLUDE_DIRS}) 8 | 9 | file(GLOB vo 10 | "src/*.h" 11 | "src/*.cpp" 12 | ) 13 | add_executable( vo ${vo} ) 14 | target_link_libraries( vo 15 | ${OpenCV_LIBS} 16 | ) 17 | -------------------------------------------------------------------------------- /stereo_vo_2d-3d/result/0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/stereo_vo_2d-3d/result/0.png -------------------------------------------------------------------------------- /stereo_vo_2d-3d/result/5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/stereo_vo_2d-3d/result/5.png -------------------------------------------------------------------------------- /stereo_vo_2d-3d/result/6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/stereo_vo_2d-3d/result/6.png -------------------------------------------------------------------------------- /stereo_vo_2d-3d/result/7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/stereo_vo_2d-3d/result/7.png -------------------------------------------------------------------------------- /stereo_vo_2d-3d/result/8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/stereo_vo_2d-3d/result/8.png -------------------------------------------------------------------------------- /stereo_vo_2d-3d/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "vo.h" 2 | using namespace std; 3 | 4 | // global 5 | int SEQ; // sequence id 6 | char* DIR; // dataset directory 7 | 8 | 9 | void getCalib(vector& calib){ 10 | char filename[100]; 11 | sprintf(filename, "%s/sequences/%02d/calib.txt", DIR, SEQ); 12 | ifstream myfile(filename); 13 | 14 | if (myfile.is_open()){ 15 | string line; string camera = "P0: "; 16 | while (getline(myfile, line)){ // find right calib line 17 | if (line.compare(0, camera.length(), camera)==0) break; 18 | } 19 | 20 | float entry; 21 | istringstream ss(line.substr(4)); // remove camera header 22 | for (int i=0; i<12;i++){ 23 | ss >> entry; 24 | if (i==0) calib.push_back(entry); 25 | if (i==2) calib.push_back(entry); 26 | if (i==6) calib.push_back(entry); 27 | } 28 | } else { 29 | cerr << "unable to open file"; 30 | exit(2); 31 | } 32 | } 33 | 34 | 35 | void getTruth(vector< vector >& truth){ 36 | char filename[100]; 37 | sprintf(filename, "%s/poses/%02d.txt", DIR, SEQ); 38 | ifstream myfile(filename); 39 | 40 | float x, y, z; 41 | float x_prev=0, y_prev=0, z_prev=0; 42 | vector pose; 43 | 44 | string line; float entry; 45 | if (myfile.is_open()){ 46 | while(getline(myfile, line)){ 47 | istringstream ss(line); 48 | for (int i=0; i<12;i++){ 49 | ss >> entry; 50 | if (i==3) x=entry; 51 | if (i==7) y=entry; 52 | if (i==11) z=entry; 53 | } 54 | pose.clear(); 55 | pose.push_back(x); pose.push_back(y); pose.push_back(z); 56 | truth.push_back(pose); 57 | 58 | x_prev=x; y_prev=y; z_prev=z; 59 | } 60 | myfile.close(); 61 | } else { 62 | cerr << "unable to open file"; 63 | exit(3); 64 | } 65 | } 66 | 67 | 68 | int main( int argc, char** argv ) { 69 | // parse command 70 | if (argc != 5){ 71 | cerr << "Syntax: " << argv[0] << " [seq id] [max frame] [opt frame] [dataset dir]\n"; 72 | exit(1); 73 | } 74 | SEQ = atoi(argv[1]); 75 | int max_frame = atoi(argv[2]); 76 | int opt_frame = atoi(argv[3]); 77 | DIR = argv[4]; 78 | 79 | // load calibration 80 | vector calib; getCalib(calib); 81 | // load truth 82 | vector< vector > truth; getTruth(truth); 83 | 84 | // init 85 | char filename[100]; sprintf(filename, "%s/sequences/%02d", DIR, SEQ); 86 | string left_dir = string(filename) + "/image_0/%06d.png"; 87 | string right_dir = string(filename) + "/image_1/%06d.png"; 88 | 89 | cv::Mat K = (cv::Mat_(3, 3) << calib[0], 0, calib[1], 0, calib[0], calib[2], 0, 0, 1); 90 | float offset = 0.53716; // left right camera x offset, calculated from calib.txt 91 | 92 | VO vo(K, offset, left_dir, right_dir, max_frame, opt_frame); 93 | vo.run(truth); 94 | 95 | return 0; 96 | } -------------------------------------------------------------------------------- /stereo_vo_2d-3d/src/vo.cpp: -------------------------------------------------------------------------------- 1 | #include "vo.h" 2 | 3 | void VO::run(const vector>& truth){ 4 | int i = 0; 5 | vector pose_optimized; 6 | 7 | // init 8 | Mat left_img = load_image(left_dir, i); 9 | Mat right_img = load_image(right_dir, i); 10 | if ( !left_img.data || !right_img.data) { cerr<< "Error reading images\n"; return;} 11 | 12 | vector feature_points; 13 | vector landmark_set; 14 | // extract_feature(left_img, right_img, landmark_history, feature_points); 15 | extract_feature(left_img, right_img, landmark_set, feature_points); 16 | 17 | // Frame frame; 18 | // frame.feature2d = feature_points; 19 | // for (int j = 0; j < landmark_history.size(); j++) frame.landmark_index.push_back(j); 20 | // frame.rvec = Mat::zeros(3,1,CV_32F); 21 | // frame.tvec = Mat::zeros(3,1,CV_32F); 22 | // frame_history.push_back(frame); 23 | 24 | // init plot 25 | char text[100]; 26 | Point text1(50, 950); Point text2(50, 970); 27 | clock_t begin = clock(); 28 | Mat traj = Mat::zeros(1000, 1000, CV_8UC3); 29 | 30 | Mat prev_img = left_img; 31 | Mat curr_img; 32 | for(i = 1; i landmark_set; 38 | // feature_points = frame_history.back().feature2d; 39 | // for (auto index : frame_history.back().landmark_index) { 40 | // landmark_set.push_back(landmark_history[index]); 41 | // } 42 | 43 | // tracking 44 | vector tracked = track_feature(prev_img, curr_img, feature_points, landmark_set); 45 | if (landmark_set.size()<5) continue; 46 | 47 | // PnP 48 | Mat dist_coeffs = Mat::zeros(4,1,CV_64F); 49 | vector inliers; 50 | Mat tvec, rvec; 51 | solvePnPRansac(landmark_set, feature_points, K, dist_coeffs, rvec, tvec, false, 100, 8.0, 0.99, inliers); 52 | cout << "inliers: " << inliers.size() << "\tlandmarks: "<< landmark_set.size() << endl; 53 | if (inliers.size()<5) continue; 54 | // solvePnP(landmark_set, feature_points, K, dist_coeffs, rvec, tvec, false); 55 | cout << tvec << endl; 56 | 57 | // relative pose, note epipolar is reversed 58 | Mat R; Rodrigues(rvec, R); 59 | R = R.t(); 60 | Mat T = -R*tvec; 61 | 62 | cout <<"estm: " << T.t() << endl; 63 | cout <<"true: " <<"["<(0), T.at(1), T.at(2)) ); 73 | 74 | // plot 75 | Point pose_true = Point(int(truth[i][0])+500, -int(truth[i][2])+500); 76 | circle(traj, pose_true ,1, CV_RGB(0,255,0), 2); 77 | Point pose_est = Point(int(T.at(0)) + 500, -int(T.at(2)) + 500); 78 | circle(traj, pose_est ,1, CV_RGB(255,0,0), 2); 79 | // update text 80 | rectangle( traj, Point(0, 930), Point(1000, 980), CV_RGB(0,0,0), CV_FILLED); 81 | sprintf(text, "x = %02fm y = %02fm z = %02fm", T.at(0), T.at(1), T.at(2)); 82 | putText(traj, text, text1, FONT_HERSHEY_PLAIN, 1, Scalar::all(255), 1, 8); 83 | sprintf(text, "frame: %d features: %lu landmarks: %lu time: %02fs", 84 | i, feature_points.size(), landmark_history.size(), double(clock()-begin)/CLOCKS_PER_SEC); 85 | putText(traj, text, text2, FONT_HERSHEY_PLAIN, 1, Scalar::all(255), 1, 8); 86 | 87 | imshow( "Trajectory", traj); 88 | waitKey(1); 89 | } 90 | // save result 91 | putText(traj, "Stereo VO with bundle adjustment", Point(250, 900), FONT_HERSHEY_PLAIN, 2, Scalar::all(255), 2, 8); 92 | imwrite("../result/vo.png", traj); 93 | } 94 | 95 | Mat VO::load_image(const string& dir, int seq){ 96 | char filename[100]; 97 | sprintf(filename, dir.c_str(), seq); 98 | return imread(filename, CV_LOAD_IMAGE_GRAYSCALE); 99 | } 100 | 101 | void VO::extract_feature(const Mat& img1, const Mat& img2, vector& landmarks, vector& feature_points){ 102 | Ptr orb = ORB::create(300); // matching need tuning 103 | vector keypoints1, keypoints2; 104 | Mat descriptors1, descriptors2; 105 | 106 | // compute feature and descriptor 107 | orb->detect(img1, keypoints1); 108 | orb->detect(img2, keypoints2); 109 | orb->compute(img1, keypoints1, descriptors1); 110 | orb->compute(img2, keypoints2, descriptors2); 111 | 112 | // convert for flann 113 | descriptors1.convertTo(descriptors1, CV_32F); 114 | descriptors2.convertTo(descriptors2, CV_32F); 115 | // matching 116 | FlannBasedMatcher matcher; 117 | vector> matches; 118 | matcher.knnMatch(descriptors1, descriptors2, matches, 2); 119 | 120 | vector match_points1; 121 | vector match_points2; 122 | // vector bestMatches; 123 | 124 | for (int i = 0; i < matches.size(); i++) { 125 | 126 | const DMatch& bestMatch = matches[i][0]; 127 | const DMatch& betterMatch = matches[i][1]; 128 | 129 | if (bestMatch.distance < 0.7*betterMatch.distance) { // ratio test 130 | match_points1.push_back(keypoints1[bestMatch.queryIdx].pt); 131 | match_points2.push_back(keypoints2[bestMatch.trainIdx].pt); 132 | // bestMatches.push_back(bestMatch); 133 | } 134 | } 135 | 136 | // project to 3d 137 | Mat candidates; 138 | triangulatePoints(P_left, P_right, match_points1, match_points2, candidates); 139 | 140 | for (int i = 0; i(0,i)/candidates.at(3,i); 143 | p.y = candidates.at(1,i)/candidates.at(3,i); 144 | p.z = candidates.at(2,i)/candidates.at(3,i); 145 | // if (p.z<10 || p.z>50) continue;// remove too close or too far 146 | landmarks.push_back(p); 147 | feature_points.push_back(match_points1[i]); 148 | } 149 | // // drawing the results 150 | // namedWindow("matches", 1); 151 | // Mat img_matches; 152 | // drawMatches(img1, keypoints1, img2, keypoints2, bestMatches, img_matches); 153 | // imshow("matches", img_matches); 154 | // waitKey(0); 155 | } 156 | 157 | vector VO::track_feature(const Mat& prev_img, const Mat& curr_img, vector& features, vector& landmarks){ 158 | vector nextPts; 159 | vector status; 160 | vector err; 161 | vector tracked; 162 | 163 | calcOpticalFlowPyrLK(prev_img, curr_img, features, nextPts, status, err); 164 | 165 | vector old_landmarks = landmarks; 166 | features.clear(); 167 | landmarks.clear(); 168 | 169 | for (int i = 0; i VO::update_frame(int i, const Mat& R, const Mat& T, const vector& features, 180 | // const vector& tracked, const vector& inliers, const Mat& rvec, const Mat& tvec){ 181 | // vector new_2d; vector new_3d; 182 | // create_new_features(i, R, T, new_2d, new_3d); 183 | 184 | // const vector& prev_index = frame_history.back().landmark_index; 185 | // vector new_index= remove_duplicate(features, new_2d, inliers, 5); 186 | // cout << "new: " << new_index.size() << ": "<< new_2d.size() < next_index; vector next_features; 189 | 190 | // // tracked and inlier feature 191 | // for (auto index : inliers) { 192 | // next_features.push_back(features[index]); 193 | // next_index.push_back(prev_index[tracked[index]]); 194 | // } 195 | // // new feature 196 | // int start = landmark_history.size(); 197 | // for (auto index : new_index){ 198 | // landmark_history.push_back(new_3d[index]); 199 | // next_features.push_back(new_2d[index]); 200 | // next_index.push_back(start++); 201 | // } 202 | 203 | // Frame frame; 204 | // frame.feature2d = next_features; 205 | // frame.landmark_index = next_index; 206 | // frame.rvec = rvec; 207 | // frame.tvec = tvec; 208 | // frame_history.push_back(frame); 209 | 210 | // return next_features; 211 | // } 212 | 213 | void VO::create_new_features(int i, const Mat& R, const Mat& T, vector& features, vector& landmarks){ 214 | if (features.size()!=0){ 215 | features.clear(); 216 | landmarks.clear(); 217 | } 218 | Mat left_img = load_image(left_dir, i); 219 | Mat right_img = load_image(right_dir, i); 220 | 221 | vector new_features; vector new_landmarks; 222 | extract_feature(left_img, right_img, new_landmarks, new_features); 223 | 224 | // cout << R << "\n" << T << endl; 225 | 226 | for (int j = 0; j< new_landmarks.size(); j++){ 227 | const Point3f& pc = new_landmarks[j]; // in camera frame 228 | 229 | // cout << "camera frame:" << pc << endl; 230 | if (pc.z<=0) continue; 231 | Point3f pw; // in world frame 232 | pw.x = R.at(0, 0)*pc.x + R.at(0, 1)*pc.y + R.at(0, 2)*pc.z + T.at(0, 3); 233 | pw.y = R.at(1, 0)*pc.x + R.at(1, 1)*pc.y + R.at(1, 2)*pc.z + T.at(1, 3); 234 | pw.z = R.at(2, 0)*pc.x + R.at(2, 1)*pc.y + R.at(2, 2)*pc.z + T.at(2, 3); 235 | // cout << "world frame:" << pw << endl; 236 | 237 | landmarks.push_back(pw); 238 | features.push_back(new_features[j]); 239 | } 240 | } 241 | 242 | vector VO::remove_duplicate(const vector& old_features, const vector& new_features, 243 | const vector& mask, int radius){ 244 | vector new_index; 245 | for (int i=0; i 10 | #include 11 | #include 12 | using namespace std; 13 | using namespace cv; 14 | 15 | // struct Frame{ 16 | // vector feature2d; 17 | // vector landmark_index; 18 | // Mat rvec; 19 | // Mat tvec; 20 | // }; 21 | 22 | 23 | class VO { 24 | public: 25 | VO(){} 26 | ~VO(){} 27 | VO(const Mat _K, float offset, const string& _left_dir, const string& _right_dir, int _max_frame, int _opt_frame){ 28 | K = _K; left_dir = _left_dir; right_dir=_right_dir; max_frame = _max_frame; opt_frame = _opt_frame; 29 | 30 | Mat M_left = Mat::zeros(3,4, CV_64F); M_left.at(0,0) =1; M_left.at(1,1) =1;M_left.at(2,2) =1; 31 | Mat M_right = Mat::zeros(3,4, CV_64F); M_right.at(0,0) =1; M_right.at(1,1) =1;M_right.at(2,2) =1; 32 | M_right.at(0,3) = -offset; // camera to world 33 | 34 | P_left = K*M_left; 35 | P_right = K*M_right; 36 | } 37 | 38 | void run(const vector>& truth); 39 | 40 | private: 41 | Mat K; 42 | Mat P_left; 43 | Mat P_right; 44 | string left_dir; 45 | string right_dir; 46 | int max_frame; 47 | int opt_frame; 48 | 49 | vector landmark_history; 50 | vector pose_history; 51 | // vector frame_history; 52 | 53 | Mat load_image(const string& dir, int seq); 54 | void extract_feature(const Mat& img1, const Mat& img2, vector& landmark_history, vector& feature_points); 55 | vector track_feature(const Mat& prev_img, const Mat& curr_img, vector& features, vector& landmarks); 56 | // vector update_frame(int i, const Mat& R, const Mat& T, const vector& features, 57 | // const vector& tracked, const vector& inliers, const Mat& rvec, const Mat& tvec); 58 | void create_new_features(int i, const Mat& R, const Mat& T, vector& features, vector& landmarks); 59 | vector remove_duplicate(const vector& old_features, const vector& new_features, const vector& mask, int radius); 60 | 61 | 62 | }; 63 | 64 | 65 | #endif -------------------------------------------------------------------------------- /wudao_final.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MaxLing/visual_odometry/9b987574f58e660f792aa917acebab4d67759050/wudao_final.pdf --------------------------------------------------------------------------------