視覺SLAM學習【3】-----視覺SLAM通過三角測量和PnP法估計特徵點的空間位置目錄
嵌入式開發學習也慢慢的進入尾聲了，從之前的ros學習，到gazebo再到機械臂的學習，到現在的slam視覺學習，層次逐漸上升，難度逐漸加大，本次博客，林君學長將打大家理解，如何通過三角測量和PnP法估計特徵點的空間位置，並且自己創建項目工程用make方式編譯，不用g++編譯，一起來看如下步驟吧！
一、G2O的安裝
g2o的核裏帶有各種各樣的求解器，而它的頂點、邊的類型則多種多樣。通過自定義頂點和邊，事實上，只要一個優化問題能夠表達成圖，那麼就可以用g2o去求解它。常見的，比如bundle adjustment，ICP，數據擬合，都可以用g2o來做;
本次博客的內容，我們需要用到G20,所以我們首先需要進度G2o的安裝，一起看步驟吧！
1、g2o的下載
1）、方法一，通過如下鏈接在GitHub上面下載：
https://github.com/RainerKuemmerle/g2o
2）、方法二，林君學長已經將g2o的源代碼(未編譯)包上傳到CSDN我的資源模塊，小夥伴們可以去下載，鏈接如下所示：
https://download.csdn.net/download/qq_42451251/12413010
2、文件上傳ubuntu
1）、將下載好的g2o的源代碼資源包上傳至ubuntu的任意位置解壓並重新命名爲g2o，下面以林君學長的路徑爲例
3、安裝依賴庫
sudo apt-get install libeigen3-dev libsuitesparse-dev libqt4-dev qt4-qmake libqglviewer-dev
4、g2o的編譯
1）、進入g2o文件夾，創建build文件夾，存放編譯產生的文件
cd ~/lenovo/g2o
mkdir build
cd build
2）、編譯
cmake ..
make -j8
以上的編譯，林君學長是用的8線程編譯的，如果小夥伴的電腦不支持8線程的話，可以改爲make-j4和make
如上則爲成功編譯！
3）、安裝
sudo make install
通過以上，我們的g2o就安裝好了，編譯的時候有點慢，我們耐心等待就好了！
二、項目創建
1、創建項目文件夾
新建終端，創建項目並創建編譯文件夾
cd ~/lenovo/opencv-3.4.1
mkdir test
cd test
mkdir build
cd build
2、創建三角測量的cpp文件
1）、創建三角測量算法的cpp文件夾
touch testTriang.cpp
gedit testTriang.cpp
2）、編寫三角測量算法代碼
#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,
vector<KeyPoint>& keypoints_1,
vector<KeyPoint>& keypoints_2,
vector<DMatch>& matches)
{
//初始化
Mat descriptors_1,descriptors_2;
Ptr<FeatureDetector> detector=ORB::create();
Ptr<DescriptorExtractor> descriptor=ORB::create();
Ptr<DescriptorMatcher> matcher=DescriptorMatcher::create("BruteForce-Hamming");
//第一步:檢測 Oriented FAST 角點位置
detector->detect(img_1,keypoints_1);
detector->detect(img_2,keypoints_2);
//第二步:根據角點位置計算 BRIEF 描述子
descriptor->compute(img_1,keypoints_1,descriptors_1);
descriptor->compute(img_2,keypoints_2,descriptors_2);
//第三步:對兩幅圖像中的BRIEF描述子進行匹配，使用 Hamming 距離
vector<DMatch> match;
matcher->match(descriptors_1,descriptors_2,match);
//第四步:匹配點對篩選
double min_dist=1000,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]);
}
}
cout<<"一共找到了"<<matches.size() <<"組匹配點"<<endl;
}
//估計兩張圖像間運動
void pose_estimation_2d2d(const vector<KeyPoint>& keypoints_1,
const vector<KeyPoint>& keypoints_2,
const vector<DMatch>& matches,
Mat& R,Mat& t)
{
//相機內參,TUM Freiburg2
Mat K=(Mat_<double> (3,3)<<520.9,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 ="<<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);
cout<<"essential_matrix = "<<endl<<essential_matrix<<endl;
//計算單應矩陣
Mat homography_matrix;
homography_matrix=findHomography(points1,points2,RANSAC,3);
cout<<"homography_matrix = "<<homography_matrix<<endl;
//從本質矩陣中恢復旋轉和平移信息
recoverPose(essential_matrix,points1,points2,R,t,focal_length,principal_point);
cout<<"R = "<<endl<<R<<endl;
cout<<"t = "<<endl<<t<<endl;
}
Point2f pixel2cam(const Point2d& p,const Mat& K)
{
return Point2f(
(p.x-K.at<double>(0,2))/K.at<double>(0,0),
(p.y-K.at<double>(1,2))/K.at<double>(1,1)
);
}
void triangulation(const vector<KeyPoint>& keypoint_1,
const vector<KeyPoint>& keypoint_2,
const vector<DMatch>& matches,
const Mat& R,const Mat& t,
vector<Point3d>& points)
{
Mat T1=(Mat_<float> (3,4)<<
1,0,0,0,
0,1,0,0,
0,0,1,0);
Mat T2=(Mat_<float>(3,4)<<
R.at<double>(0,0),R.at<double>(0,1),R.at<double>(0,2),t.at<double>(0,0),
R.at<double>(1,0),R.at<double>(1,1),R.at<double>(1,2),t.at<double>(1,0),
R.at<double>(2,0),R.at<double>(2,1),R.at<double>(2,2),t.at<double>(2,0)
);
Mat K=(Mat_<double>(3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1);
vector<Point2f> 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;
triangulatePoints(T1,T2,pts_1,pts_2,pts_4d);
cout<<pts_4d.cols<<endl;
//轉換成非齊次座標
for(int i=0;i<pts_4d.cols;++i)
{
Mat x=pts_4d.col(i);
x/=x.at<float>(3,0);//歸一化
Point3d p(
x.at<float>(0,0),
x.at<float>(1,0),
x.at<float>(2,0)
);
cout<<"p = "<<endl<<p<<endl;
points.push_back(p);
}
}
int main(int argc,char** argv)
{
if(argc!=3)
{
cout<<"usage:triangulation 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);
//估計兩張圖像間運動
Mat R,t;
pose_estimation_2d2d(keypoints_1,keypoints_2,matches,R,t);
//三角化
vector<Point3d> points;
triangulation(keypoints_1,keypoints_2,matches,R,t,points);
//驗證三角化點與特徵點的重投影關係
Mat K=(Mat_<double>(3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1);
for(int i=0;i<matches.size();++i)
{
Point2d pt1_cam=pixel2cam(keypoints_1[matches[i].queryIdx].pt,K);
Point2d pt1_cam_3d(
points[i].x/points[i].z,
points[i].y/points[i].z
);
cout<<"point in the first camera frame: "<<pt1_cam<<endl;
cout<<"point projected from 3D"<<pt1_cam_3d<<",d="<<points[i].z<<endl;
//第二個圖
Point2f pt2_cam=pixel2cam(keypoints_2[matches[i].trainIdx].pt,K);
Mat pt2_trans=R*(Mat_<double>(3,1)<<points[i].x,points[i].y,points[i].z)+t;
pt2_trans/=pt2_trans.at<double>(2,0);
cout<<"point in the second camera a frame: "<<pt2_cam<<endl;
cout<<"point projected from second frame: "<<pt2_trans.t()<<endl;
cout<<endl;
}
return 0;
}
3、創建PnP法的cpp文件
1）、創建PnP法的cpp文件夾
touch testPnp.cpp
gedit testPnp.cpp
2）、編寫PnP法的c++代碼
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>
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 );
// 像素座標轉相機歸一化座標
Point2d pixel2cam ( const Point2d& p, const Mat& K );
int main ( int argc, char** argv )
{
if ( argc != 4 )
{
cout<<"usage: pose_estimation_3d2d img1 img2 depth1"<<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;
// 建立3D點
Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED ); // 深度圖爲16位無符號數，單通道圖像
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
vector<Point3f> pts_3d;
vector<Point2f> pts_2d;
for ( DMatch m:matches )
{
ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
if ( d == 0 ) // bad depth
continue;
float dd = d/5000.0;
Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
}
cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;
Mat r, t;
solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 調用OpenCV 的 PnP 求解，可選擇EPNP，DLS等方法
Mat R;
cv::Rodrigues ( r, R ); // r爲旋轉向量形式，用Rodrigues公式轉換爲矩陣
cout<<"R="<<endl<<R<<endl;
cout<<"t="<<endl<<t<<endl;
}
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;
// used in OpenCV3
Ptr<FeatureDetector> detector = ORB::create();
Ptr<DescriptorExtractor> descriptor = ORB::create();
// use this if you are in OpenCV2
// Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create ( "BruteForce-Hamming" );
//-- 第一步:檢測 Oriented FAST 角點位置
detector->detect ( img_1,keypoints_1 );
detector->detect ( img_2,keypoints_2 );
//-- 第二步:根據角點位置計算 BRIEF 描述子
descriptor->compute ( img_1, keypoints_1, descriptors_1 );
descriptor->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] );
}
}
}
Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
return Point2d
(
( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
);
}
4、創建CMakeLists.txt配置文件
1）、創建CMakeLists.txt配置文件
touch CMakeLists.txt
gedit CMakeLists.txt
2）、編寫CMakeLists.txt配置文件內容
cmake_minimum_required(VERSION 2.8)
project(test)
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
set(CMAKE_CXX_STANDARD 11)
set( G2O_ROOT /usr/local/include/g2o )
find_package( OpenCV REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(G2O REQUIRED)
find_package( CSparse)
include_directories(
${EIGEN3_INCLUDE_DIR}
${OpenCV_INCLUDE_DIRS}
${G2O_INCLUDE_DIRS}
${CSPARSE_INCLUDE_DIR}
)
add_executable(testPnp testPnp.cpp)
add_executable(testTriang testTriang.cpp)
target_link_libraries( testTriang ${OpenCV_LIBS})
target_link_libraries( testPnp ${EIGEN3_LIB} ${OpenCV_LIBS} ${G2O_LIB} ${CSPARSE_LIB})
target_link_libraries(testPnp g2o_core g2o_stuff)
三、編譯項目
1、文件準備
1）、將上面我們編譯g2o中的cmake_modules文件夾複製到我們test項目文件夾中，如下所示：
2）、在test項目下的build中添加兩張圖片，如下所示：
3）、將上面的兩張拍攝的圖片變爲深度圖(灰度圖)，後存放該處：
什麼是深度圖： 深度圖爲16位無符號數，單通道圖像，也就是，我們只需要將我們的圖片變爲灰度圖就行了，方法很多，小夥伴們可以自己摸索哦！
圖片的拍攝必須是同一個物體，不同的角度，也就是要具有相同的特徵點
2、項目進行編譯
1）、進入build文件夾進行編譯
cd build
2）、編譯
cmake ..
make -j4
3）、編譯後的整體項目如下：
test目錄下：
build目錄下：
如上所示出現百分百則爲成功，由於林君學長是編譯了一次的，所以編譯很快，小夥伴則需要等待一小會兒
2、結果運行
1）、三角測量算法的結果運行
./testTriang 1.jpg 2.jpg
2）、PnP法的結果運行
./testPnp 1.jpg 2.jpg 11.jpg
以上就是本次博客的全部內容啦，希望小夥伴們對本次博客的閱讀可以幫助大家理解SLAM中三角測量和PnP法估計特徵點的空間位置，瞭解求解機制，並知道如何編寫make編譯的項目哦！
遇到問題的小夥伴記得評論區留言，林君學長看到會爲大家解答的，這個學長不太冷！
陳一月的又一天編程歲月^ _ ^