|
| 1 | +#include "opencv_all.hpp" |
| 2 | + |
| 3 | +int main(void) |
| 4 | +{ |
| 5 | + bool use_5pt = false; |
| 6 | + double camera_focal = 718.8560; |
| 7 | + cv::Point2d camera_center(607.1928, 185.2157); |
| 8 | + |
| 9 | + // Open a file to write camera trajectory |
| 10 | + FILE* camera_traj = NULL; |
| 11 | + camera_traj = fopen("camera_trajectory.csv", "wt"); |
| 12 | + if (camera_traj == NULL) return -1; |
| 13 | + |
| 14 | + // Open an video and get the initial image |
| 15 | + cv::VideoCapture video; |
| 16 | + if (!video.open("data/KITTI_00_L/%06d.png")) return -1; |
| 17 | + |
| 18 | + cv::Mat gray_prev; |
| 19 | + video >> gray_prev; |
| 20 | + if (gray_prev.empty()) |
| 21 | + { |
| 22 | + video.release(); |
| 23 | + return -1; |
| 24 | + } |
| 25 | + if (gray_prev.channels() > 1) cv::cvtColor(gray_prev, gray_prev, CV_RGB2GRAY); |
| 26 | + |
| 27 | + // Run and show video stabilization |
| 28 | + cv::Mat camera_pose = cv::Mat::eye(4, 4, CV_64F); |
| 29 | + while (true) |
| 30 | + { |
| 31 | + // Grab an image from the video |
| 32 | + cv::Mat image, gray; |
| 33 | + video >> image; |
| 34 | + if (image.empty()) break; |
| 35 | + if (image.channels() > 1) cv::cvtColor(image, gray, CV_RGB2GRAY); |
| 36 | + else gray = image.clone(); |
| 37 | + |
| 38 | + // Extract optical flow |
| 39 | + std::vector<cv::Point2f> point_prev, point; |
| 40 | + cv::goodFeaturesToTrack(gray_prev, point_prev, 2000, 0.01, 10); |
| 41 | + std::vector<uchar> m_status; |
| 42 | + cv::Mat err; |
| 43 | + cv::calcOpticalFlowPyrLK(gray_prev, gray, point_prev, point, m_status, err); |
| 44 | + gray_prev = gray; |
| 45 | + |
| 46 | + // Calculate relative pose |
| 47 | + cv::Mat E, inlier_mask; |
| 48 | + if (use_5pt) |
| 49 | + { |
| 50 | + E = cv::findEssentialMat(point_prev, point, camera_focal, camera_center, cv::RANSAC, 0.99, 1, inlier_mask); |
| 51 | + } |
| 52 | + else |
| 53 | + { |
| 54 | + cv::Mat F = cv::findFundamentalMat(point_prev, point, cv::FM_RANSAC, 1, 0.99, inlier_mask); |
| 55 | + cv::Mat K = (cv::Mat_<double>(3, 3) << camera_focal, 0, camera_center.x, 0, camera_focal, camera_center.y, 0, 0, 1); |
| 56 | + E = K.t() * F * K; |
| 57 | + } |
| 58 | + cv::Mat R, t; |
| 59 | + int inlier_num = cv::recoverPose(E, point_prev, point, R, t, camera_focal, camera_center, inlier_mask); |
| 60 | + |
| 61 | + // Accumulate pose |
| 62 | + cv::Mat T = cv::Mat::eye(4, 4, R.type()); |
| 63 | + T(cv::Range(0, 3), cv::Range(0, 3)) = R * 1.0; |
| 64 | + T(cv::Range(0, 3), cv::Range(3, 4)) = t * 1.0; |
| 65 | + camera_pose = camera_pose * T.inv(); |
| 66 | + |
| 67 | + // Show the image and write camera pose |
| 68 | + if (image.channels() < 3) cv::cvtColor(image, image, CV_GRAY2RGB); |
| 69 | + for (size_t i = 0; i < point_prev.size(); i++) |
| 70 | + { |
| 71 | + if (inlier_mask.at<uchar>(i) > 0) cv::line(image, point_prev[i], point[i], cv::Scalar(0, 0, 255)); |
| 72 | + else cv::line(image, point_prev[i], point[i], cv::Scalar(0, 255, 0)); |
| 73 | + } |
| 74 | + cv::imshow("3DVT Tutorial: Monocular Visual Odometry", image); |
| 75 | + fprintf(camera_traj, "%.6f, %.6f, %.6f\n", camera_pose.at<double>(0, 3), camera_pose.at<double>(1, 3), camera_pose.at<double>(2, 3)); |
| 76 | + if (cv::waitKey(1) == 27) break; // "ESC" key |
| 77 | + } |
| 78 | + |
| 79 | + video.release(); |
| 80 | + fclose(camera_traj); |
| 81 | + return 0; |
| 82 | +} |
0 commit comments