ch7/triangulation

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;

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} )
+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} )

ch7/feature_extration.cpp

@@ -1,14 +1,18 @@
-#include <stdio.h>
 #include <iostream>
-#include "opencv2/core/core.hpp"
-#include "opencv2/features2d/features2d.hpp"
-#include "opencv2/highgui/highgui.hpp"
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/highgui/highgui.hpp>
 
 using namespace std;
 using namespace cv;
 
 int main ( int argc, char** argv )
 {
+    if ( argc != 3 )
+    {
+        cout<<"usage: feature_extraction img1 img2"<<endl;
+        return 1;
+    }
     //-- 读取图像
     Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
     Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

ch7/pose_estimation_2d2d.cpp

@@ -1,47 +1,134 @@
+#include <iostream>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+using namespace std; 
+using namespace cv;
 
+void find_feature_matches( const Mat& img_1, const Mat& img_2, 
+                            std::vector<KeyPoint>& keypoints_1,
+                            std::vector<KeyPoint>& keypoints_2,
+                            std::vector< DMatch >& matches );
 
-void pose_estimation_2d2d(  std::vector<KeyPoint> keypoints_1,
-			    std::vector<KeyPoint> keypoints_2,
-			    std::vector< DMatch > matches){
-
-  // 相机内参,TUM Freiburg2		
-  Mat K = (Mat_<double>(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
-
-
-  //-- 把匹配点转换为vector<Point2f>的形式
-  int point_count = max(keypoints_1.size(),keypoints_2.size());
-
-  vector<Point2f> points1(point_count);
-  vector<Point2f> 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<<endl;
-
-
-  //-- 计算本质矩阵 
-  Point2d principal_point(325.1, 249.7);						//相机主点, TUM dataset标定值
-  int focal_length = 521;								//相机焦距, TUM dataset标定值
-  Mat essential_matrix, R, t;
-  essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point, RANSAC);	
-  cout<<"essential_matrix is "<< essential_matrix<<endl;
-
-
-  //-- 计算单应矩阵
-  Mat homography_matrix;
-  homography_matrix = findHomography(points1, points2, RANSAC, 3, noArray(), 2000, 0.99);
-  cout<<"homography_matrix is "<< homography_matrix<<endl;
-
-
-  //-- 从本质矩阵中恢复旋转和平移信息.
-  recoverPose(essential_matrix, points2, points1, R, t, focal_length, principal_point);				
-  cout<<"R is "<<R<<endl;
-  cout<<"t is "<<t<<endl;
-}
+void pose_estimation_2d2d ( std::vector<KeyPoint> keypoints_1,
+                            std::vector<KeyPoint> 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"<<endl;
+        return 1;
+    }
+    //-- 读取图像
+    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
+    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );
+
+    vector<KeyPoint> keypoints_1, keypoints_2;
+    vector<DMatch> matches;
+    find_feature_matches( img_1, img_2, keypoints_1, keypoints_2, matches );
+    cout<<"一共找到了"<<matches.size()<<"组匹配点"<<endl;
+
+    //-- 估计两张图像间运动
+    Mat R,t;
+    pose_estimation_2d2d( keypoints_1, keypoints_2, matches, R, t );
+
+    //-- 验证E=t^R*scale
+    Mat t_x = (Mat_<double>(3,3) << 
+        0,                      -t.at<double>(2,0),     t.at<double>(1,0), 
+        t.at<double>(2,0),      0,                      -t.at<double>(0,0),
+        -t.at<double>(1.0),     t.at<double>(0,0),      0);
+
+    cout<<"t^R="<<endl<<t_x*R<<endl;
+    return 0;
+}
+
+void find_feature_matches( const Mat& img_1, const Mat& img_2, 
+                            std::vector<KeyPoint>& keypoints_1,
+                            std::vector<KeyPoint>& keypoints_2,
+                            std::vector< DMatch >& matches )
+{
+    //-- 初始化
+    Mat descriptors_1, descriptors_2;
+    Ptr<ORB> 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<DMatch> 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<KeyPoint> keypoints_1,
+                            std::vector<KeyPoint> keypoints_2,
+                            std::vector< DMatch > matches, 
+                            Mat& R, Mat& t )
+{
+    // 相机内参,TUM Freiburg2
+    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
+
+    //-- 把匹配点转换为vector<Point2f>的形式
+    vector<Point2f> points1;
+    vector<Point2f> 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 "<<endl<< fundamental_matrix<<endl;
+
+    //-- 计算本质矩阵
+    Point2d principal_point ( 325.1, 249.7 );				//相机主点, TUM dataset标定值
+    int focal_length = 521;						//相机焦距, TUM dataset标定值
+    Mat essential_matrix;
+    essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point, RANSAC );
+    cout<<"essential_matrix is "<<endl<< essential_matrix<<endl;
+
+    //-- 计算单应矩阵
+    Mat homography_matrix;
+    homography_matrix = findHomography ( points1, points2, RANSAC, 3, noArray(), 2000, 0.99 );
+    cout<<"homography_matrix is "<<endl<<homography_matrix<<endl;
+
+    //-- 从本质矩阵中恢复旋转和平移信息.
+    recoverPose ( essential_matrix, points2, points1, R, t, focal_length, principal_point );
+    cout<<"R is "<<endl<<R<<endl;
+    cout<<"t is "<<endl<<t<<endl;
+}