From 7ca606897e0fb8099c01ea24b118977b126c0f77 Mon Sep 17 00:00:00 2001 From: "xiang.gao" Date: Sun, 28 Aug 2016 17:20:27 +0800 Subject: [PATCH] ch7/triangulation --- ch6/g2o_curve_fitting/main.cpp | 1 - ch7/CMakeLists.txt | 10 +- ch7/feature_extration.cpp | 12 +- ch7/pose_estimation_2d2d.cpp | 177 ++++++++++++++++++++------- ch7/triangulation.cpp | 214 +++++++++++++++++++++++++++++++++ 5 files changed, 363 insertions(+), 51 deletions(-) create mode 100644 ch7/triangulation.cpp diff --git a/ch6/g2o_curve_fitting/main.cpp b/ch6/g2o_curve_fitting/main.cpp index f67fcc7ad..946a912db 100644 --- a/ch6/g2o_curve_fitting/main.cpp +++ b/ch6/g2o_curve_fitting/main.cpp @@ -17,7 +17,6 @@ class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW -public: virtual void setToOriginImpl() // 重置 { _estimate << 0,0,0; diff --git a/ch7/CMakeLists.txt b/ch7/CMakeLists.txt index ee2f4e0a2..7f4d44128 100644 --- a/ch7/CMakeLists.txt +++ b/ch7/CMakeLists.txt @@ -1,8 +1,16 @@ cmake_minimum_required( VERSION 2.8 ) project( vo1 ) +set( CMAKE_CXX_FLAGS "-std=c++11" ) + find_package( OpenCV ) include_directories( ${OpenCV_INCLUDE_DIRS} ) add_executable( feature_extraction feature_extration.cpp ) -target_link_libraries( feature_extraction ${OpenCV_LIBS} ) \ No newline at end of file +target_link_libraries( feature_extraction ${OpenCV_LIBS} ) + +add_executable( pose_estimation_2d2d pose_estimation_2d2d.cpp ) +target_link_libraries( pose_estimation_2d2d ${OpenCV_LIBS} ) + +add_executable( triangulation triangulation.cpp ) +target_link_libraries( triangulation ${OpenCV_LIBS} ) \ No newline at end of file diff --git a/ch7/feature_extration.cpp b/ch7/feature_extration.cpp index 53c04ae2d..e6a573f6b 100644 --- a/ch7/feature_extration.cpp +++ b/ch7/feature_extration.cpp @@ -1,14 +1,18 @@ -#include #include -#include "opencv2/core/core.hpp" -#include "opencv2/features2d/features2d.hpp" -#include "opencv2/highgui/highgui.hpp" +#include +#include +#include using namespace std; using namespace cv; int main ( int argc, char** argv ) { + if ( argc != 3 ) + { + cout<<"usage: feature_extraction img1 img2"< +#include +#include +#include +#include +using namespace std; +using namespace cv; +void find_feature_matches( const Mat& img_1, const Mat& img_2, + std::vector& keypoints_1, + std::vector& keypoints_2, + std::vector< DMatch >& matches ); -void pose_estimation_2d2d( std::vector keypoints_1, - std::vector keypoints_2, - std::vector< DMatch > matches){ - - // 相机内参,TUM Freiburg2 - Mat K = (Mat_(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1); - - - //-- 把匹配点转换为vector的形式 - int point_count = max(keypoints_1.size(),keypoints_2.size()); - - vector points1(point_count); - vector points2(point_count); - - for( int i = 0; i < (int)matches.size(); i++ ){ - points1[i] = keypoints_1[matches[i].queryIdx].pt; - points2[i] = keypoints_2[matches[i].trainIdx].pt; - } - - - //-- 计算基础矩阵 - Mat fundamental_matrix; - fundamental_matrix = findFundamentalMat(points1, points2, CV_FM_8POINT, 3, 0.99); - cout<<"fundamental_matrix is "<< fundamental_matrix< keypoints_1, + std::vector keypoints_2, + std::vector< DMatch > matches, + Mat& R, Mat& t ); + +int main( int argc, char** argv ) +{ + if ( argc != 3 ) + { + cout<<"usage: feature_extraction img1 img2"< keypoints_1, keypoints_2; + vector matches; + find_feature_matches( img_1, img_2, keypoints_1, keypoints_2, matches ); + cout<<"一共找到了"<(3,3) << + 0, -t.at(2,0), t.at(1,0), + t.at(2,0), 0, -t.at(0,0), + -t.at(1.0), t.at(0,0), 0); + + cout<<"t^R="<& keypoints_1, + std::vector& keypoints_2, + std::vector< DMatch >& matches ) +{ + //-- 初始化 + Mat descriptors_1, descriptors_2; + Ptr orb = ORB::create ( 500, 1.2f, 8, 31, 0, 2, ORB::HARRIS_SCORE,31,20 ); + + //-- 第一步:检测 Oriented FAST 角点位置 + orb->detect ( img_1,keypoints_1 ); + orb->detect ( img_2,keypoints_2 ); + + //-- 第二步:根据角点位置计算 BRIEF 描述子 + orb->compute ( img_1, keypoints_1, descriptors_1 ); + orb->compute ( img_2, keypoints_2, descriptors_2 ); + + //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离 + vector match; + BFMatcher matcher ( NORM_HAMMING ); + matcher.match ( descriptors_1, descriptors_2, match ); + + //-- 第四步:匹配点对筛选 + double min_dist=10000, max_dist=0; + + //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离 + for ( int i = 0; i < descriptors_1.rows; i++ ) + { + double dist = match[i].distance; + if ( dist < min_dist ) min_dist = dist; + if ( dist > max_dist ) max_dist = dist; + } + + printf ( "-- Max dist : %f \n", max_dist ); + printf ( "-- Min dist : %f \n", min_dist ); + + //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限. + for ( int i = 0; i < descriptors_1.rows; i++ ) + { + if ( match[i].distance <= max ( 2*min_dist, 30.0 ) ) + { + matches.push_back ( match[i] ); + } + } +} + +void pose_estimation_2d2d ( std::vector keypoints_1, + std::vector keypoints_2, + std::vector< DMatch > matches, + Mat& R, Mat& t ) +{ + // 相机内参,TUM Freiburg2 + Mat K = ( Mat_ ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); + + //-- 把匹配点转换为vector的形式 + vector points1; + vector points2; + + for ( int i = 0; i < ( int ) matches.size(); i++ ) + { + points1.push_back( keypoints_1[matches[i].queryIdx].pt ); + points2.push_back( keypoints_2[matches[i].trainIdx].pt ); + } + + //-- 计算基础矩阵 + Mat fundamental_matrix; + fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT ); + cout<<"fundamental_matrix is "< +#include +#include +#include +#include + +using namespace std; +using namespace cv; + +void find_feature_matches ( + const Mat& img_1, const Mat& img_2, + std::vector& keypoints_1, + std::vector& keypoints_2, + std::vector< DMatch >& matches ); + +void pose_estimation_2d2d ( + const std::vector& keypoints_1, + const std::vector& keypoints_2, + const std::vector< DMatch >& matches, + Mat& R, Mat& t ); + +void triangulation ( + const vector& keypoint_1, + const vector& keypoint_2, + const std::vector< DMatch >& matches, + const Mat& R, const Mat& t, + vector& points +); + +// 像素坐标转相机归一化坐标 +Point2d pixel2cam( const Point2d& p, const Mat& K ); + +int main ( int argc, char** argv ) +{ + if ( argc != 3 ) + { + cout<<"usage: feature_extraction img1 img2"< keypoints_1, keypoints_2; + vector matches; + find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches ); + cout<<"一共找到了"< points; + triangulation( keypoints_1, keypoints_2, matches, R, t, points ); + + //-- 验证三角化点与特征点的重投影关系 + Mat K = ( Mat_ ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); + for ( int i=0; i& keypoints_1, + const std::vector& keypoints_2, + const std::vector< DMatch >& matches, + Mat& R, Mat& t ) +{ + // 相机内参,TUM Freiburg2 + Mat K = ( Mat_ ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); + + //-- 把匹配点转换为vector的形式 + vector points1; + vector points2; + + for ( int i = 0; i < ( int ) matches.size(); i++ ) + { + points1.push_back ( keypoints_1[matches[i].queryIdx].pt ); + points2.push_back ( keypoints_2[matches[i].trainIdx].pt ); + } + + //-- 计算基础矩阵 + Mat fundamental_matrix; + fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT ); + cout<<"fundamental_matrix is "<& keypoint_1, + const vector< KeyPoint >& keypoint_2, + const std::vector< DMatch >& matches, + const Mat& R, const Mat& t, + vector< Point3d >& points ) +{ + Mat T1 = (Mat_ (3,4) << + 1,0,0,0, + 0,1,0,0, + 0,0,1,0); + Mat T2 = (Mat_ (3,4) << + R.at(0,0), R.at(0,1), R.at(0,2), t.at(0,0), + R.at(1,0), R.at(1,1), R.at(1,2), t.at(1,0), + R.at(2,0), R.at(2,1), R.at(2,2), t.at(2,0) + ); + + Mat K = ( Mat_ ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); + vector pts_1, pts_2; + for ( DMatch m:matches ) + { + // 将像素坐标转换至相机坐标 + pts_1.push_back ( pixel2cam( keypoint_1[m.queryIdx].pt, K) ); + pts_2.push_back ( pixel2cam( keypoint_2[m.trainIdx].pt, K) ); + } + + Mat pts_4d; + cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d ); + + // 转换成非齐次坐标 + for ( int i=0; i(3,i); + Point3d p ( + pts_4d.at(0,i) / d4, + pts_4d.at(1,i) / d4, + pts_4d.at(2,i) / d4 + ); + if ( p.z < 0 ) p*=-1; + points.push_back( p ); + } +} + +Point2d pixel2cam ( const Point2d& p, const Mat& K ) +{ + return Point2d + ( + ( p.x - K.at(0,2) ) / K.at(0,0), + ( p.y - K.at(1,2) ) / K.at(1,1) + ); +} +