Skip to content

Commit 0b61a5c

Browse files
author
zero
committed
ch5
1 parent 4e68966 commit 0b61a5c

15 files changed

+865
-0
lines changed

‎part V/CMakeLists.txt

+13
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
CMAKE_MINIMUM_REQUIRED( VERSION 2.8 )
2+
PROJECT( slam )
3+
4+
SET(CMAKE_CXX_COMPILER "g++")
5+
SET( CMAKE_BUILD_TYPE Debug )
6+
SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
7+
SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
8+
9+
INCLUDE_DIRECTORIES( ${PROJECT_SOURCE_DIR}/include )
10+
LINK_DIRECTORIES( ${PROJECT_SOURCE_DIR}/lib)
11+
12+
ADD_SUBDIRECTORY( ${PROJECT_SOURCE_DIR}/src )
13+

‎part V/bin/detectFeatures

6.69 MB
Binary file not shown.

‎part V/bin/generate_pointcloud

1.87 MB
Binary file not shown.

‎part V/bin/joinPointCloud

7.86 MB
Binary file not shown.

‎part V/bin/main

18.8 KB
Binary file not shown.

‎part V/bin/visualOdometry

7.49 MB
Binary file not shown.

‎part V/include/slamBase.h

+139
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,139 @@
1+
/*************************************************************************
2+
> File Name: rgbd-slam-tutorial-gx/part III/code/include/slamBase.h
3+
> Author: xiang gao
4+
> Mail: [email protected]
5+
> Created Time: 2015年07月18日 星期六 15时14分22秒
6+
> 说明：rgbd-slam教程所用到的基本函数（C风格）
7+
************************************************************************/
8+
# pragma once
9+
10+
// 各种头文件
11+
// C++标准库
12+
#include <fstream>
13+
#include <vector>
14+
#include <map>
15+
using namespace std;
16+
17+
// Eigen
18+
#include <Eigen/Core>
19+
#include <Eigen/Geometry>
20+
21+
// OpenCV
22+
#include <opencv2/core/core.hpp>
23+
#include <opencv2/highgui/highgui.hpp>
24+
#include <opencv2/calib3d/calib3d.hpp>
25+
#include <opencv2/nonfree/nonfree.hpp>
26+
#include <opencv2/core/eigen.hpp>
27+
28+
29+
// PCL
30+
#include <pcl/io/pcd_io.h>
31+
#include <pcl/point_types.h>
32+
#include <pcl/common/transforms.h>
33+
#include <pcl/visualization/cloud_viewer.h>
34+
#include <pcl/filters/voxel_grid.h>
35+
36+
// 类型定义
37+
typedef pcl::PointXYZRGBA PointT;
38+
typedef pcl::PointCloud<PointT> PointCloud;
39+
40+
// 相机内参结构
41+
struct CAMERA_INTRINSIC_PARAMETERS
42+
{
43+
double cx, cy, fx, fy, scale;
44+
};
45+
46+
// 帧结构
47+
struct FRAME
48+
{
49+
cv::Mat rgb, depth; //该帧对应的彩色图与深度图
50+
cv::Mat desp; //特征描述子
51+
vector<cv::KeyPoint> kp; //关键点
52+
};
53+
54+
// PnP 结果
55+
struct RESULT_OF_PNP
56+
{
57+
cv::Mat rvec, tvec;
58+
int inliers;
59+
};
60+
61+
// 函数接口
62+
// image2PonitCloud 将rgb图转换为点云
63+
PointCloud::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera );
64+
65+
// point2dTo3d 将单个点从图像坐标转换为空间坐标
66+
// input: 3维点Point3f (u,v,d)
67+
cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera );
68+
69+
// computeKeyPointsAndDesp 同时提取关键点与特征描述子
70+
void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor );
71+
72+
// estimateMotion 计算两个帧之间的运动
73+
// 输入：帧1和帧2, 相机内参
74+
RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera );
75+
76+
// cvMat2Eigen
77+
Eigen::Isometry3d cvMat2Eigen( cv::Mat& rvec, cv::Mat& tvec );
78+
79+
// joinPointCloud
80+
PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera ) ;
81+
82+
// 参数读取类
83+
class ParameterReader
84+
{
85+
public:
86+
ParameterReader( string filename="./parameters.txt" )
87+
{
88+
ifstream fin( filename.c_str() );
89+
if (!fin)
90+
{
91+
cerr<<"parameter file does not exist."<<endl;
92+
return;
93+
}
94+
while(!fin.eof())
95+
{
96+
string str;
97+
getline( fin, str );
98+
if (str[0] == '#')
99+
{
100+
// 以‘＃’开头的是注释
101+
continue;
102+
}
103+
104+
int pos = str.find("=");
105+
if (pos == -1)
106+
continue;
107+
string key = str.substr( 0, pos );
108+
string value = str.substr( pos+1, str.length() );
109+
data[key] = value;
110+
111+
if ( !fin.good() )
112+
break;
113+
}
114+
}
115+
string getData( string key )
116+
{
117+
map<string, string>::iterator iter = data.find(key);
118+
if (iter == data.end())
119+
{
120+
cerr<<"Parameter name "<<key<<" not found!"<<endl;
121+
return string("NOT_FOUND");
122+
}
123+
return iter->second;
124+
}
125+
public:
126+
map<string, string> data;
127+
};
128+
129+
inline static CAMERA_INTRINSIC_PARAMETERS getDefaultCamera()
130+
{
131+
ParameterReader pd;
132+
CAMERA_INTRINSIC_PARAMETERS camera;
133+
camera.fx = atof( pd.getData( "camera.fx" ).c_str());
134+
camera.fy = atof( pd.getData( "camera.fy" ).c_str());
135+
camera.cx = atof( pd.getData( "camera.cx" ).c_str());
136+
camera.cy = atof( pd.getData( "camera.cy" ).c_str());
137+
camera.scale = atof( pd.getData( "camera.scale" ).c_str() );
138+
return camera;
139+
}

‎part V/parameters.txt

+37
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
# 这是一个参数文件
2+
# 去你妹的yaml! 我再也不用yaml了！简简单单多好！
3+
4+
# part 4 里定义的参数
5+
# 特征类型
6+
detector=SIFT
7+
descriptor=SIFT
8+
# 筛选good match的倍数
9+
good_match_threshold=4
10+
11+
# camera
12+
camera.cx=325.5;
13+
camera.cy=253.5;
14+
camera.fx=518.0;
15+
camera.fy=519.0;
16+
camera.scale=1000.0;
17+
18+
# part 5
19+
# 数据相关
20+
# 起始与终止索引
21+
start_index=1
22+
end_index=700
23+
# 数据所在目录
24+
rgb_dir=../data/rgb_png/
25+
rgb_extension=.png
26+
depth_dir=../data/depth_png/
27+
depth_extension=.png
28+
# 点云分辨率
29+
voxel_grid=0.02
30+
# 是否实时可视化
31+
visualize_pointcloud=yes
32+
# 最小匹配数量
33+
min_good_match=10
34+
# 最小内点
35+
min_inliers=5
36+
# 最大运动误差
37+
max_norm=0.1

‎part V/src/CMakeLists.txt

+42
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
# 增加一个可执行的二进制
2+
ADD_EXECUTABLE( main main.cpp )
3+
4+
# 增加PCL库的依赖
5+
FIND_PACKAGE( PCL REQUIRED COMPONENTS common io visualization filters )
6+
7+
# 增加opencv的依赖
8+
FIND_PACKAGE( OpenCV REQUIRED )
9+
10+
# 添加头文件和库文件
11+
ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
12+
INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS} )
13+
LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )
14+
15+
# INCLUDE_DIRECTORIES( ${PROJECT_SOURSE_DIR}/include )
16+
17+
ADD_EXECUTABLE( generate_pointcloud generatePointCloud.cpp )
18+
TARGET_LINK_LIBRARIES( generate_pointcloud ${OpenCV_LIBS}
19+
${PCL_LIBRARIES} )
20+
21+
ADD_LIBRARY( slambase slamBase.cpp )
22+
TARGET_LINK_LIBRARIES( slambase
23+
${OpenCV_LIBS}
24+
${PCL_LIBRARIES} )
25+
26+
ADD_EXECUTABLE( detectFeatures detectFeatures.cpp )
27+
TARGET_LINK_LIBRARIES( detectFeatures
28+
slambase
29+
${OpenCV_LIBS}
30+
${PCL_LIBRARIES} )
31+
32+
ADD_EXECUTABLE( joinPointCloud joinPointCloud.cpp)
33+
TARGET_LINK_LIBRARIES( joinPointCloud
34+
slambase
35+
${OpenCV_LIBS}
36+
${PCL_LIBRARIES} )
37+
38+
ADD_EXECUTABLE( visualOdometry visualOdometry.cpp)
39+
TARGET_LINK_LIBRARIES( visualOdometry
40+
slambase
41+
${OpenCV_LIBS}
42+
${PCL_LIBRARIES} )

‎part V/src/detectFeatures.cpp

+151
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,151 @@
1+
/*************************************************************************
2+
> File Name: detectFeatures.cpp
3+
> Author: xiang gao
4+
> Mail: [email protected]
5+
> 特征提取与匹配
6+
> Created Time: 2015年07月18日 星期六 16时00分21秒
7+
************************************************************************/
8+
9+
#include<iostream>
10+
#include "slamBase.h"
11+
using namespace std;
12+
13+
// OpenCV 特征检测模块
14+
#include <opencv2/features2d/features2d.hpp>
15+
#include <opencv2/nonfree/nonfree.hpp>
16+
#include <opencv2/calib3d/calib3d.hpp>
17+
18+
int main( int argc, char** argv )
19+
{
20+
// 声明并从data文件夹里读取两个rgb与深度图
21+
cv::Mat rgb1 = cv::imread( "./data/rgb1.png");
22+
cv::Mat rgb2 = cv::imread( "./data/rgb2.png");
23+
cv::Mat depth1 = cv::imread( "./data/depth1.png", -1);
24+
cv::Mat depth2 = cv::imread( "./data/depth2.png", -1);
25+
26+
// 声明特征提取器与描述子提取器
27+
cv::Ptr<cv::FeatureDetector> _detector;
28+
cv::Ptr<cv::DescriptorExtractor> _descriptor;
29+
30+
// 构建提取器，默认两者都为sift
31+
// 构建sift, surf之前要初始化nonfree模块
32+
cv::initModule_nonfree();
33+
_detector = cv::FeatureDetector::create( "SIFT" );
34+
_descriptor = cv::DescriptorExtractor::create( "SIFT" );
35+
36+
vector< cv::KeyPoint > kp1, kp2; //关键点
37+
_detector->detect( rgb1, kp1 ); //提取关键点
38+
_detector->detect( rgb2, kp2 );
39+
40+
cout<<"Key points of two images: "<<kp1.size()<<", "<<kp2.size()<<endl;
41+
42+
// 可视化， 显示关键点
43+
cv::Mat imgShow;
44+
cv::drawKeypoints( rgb1, kp1, imgShow, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS );
45+
cv::imshow( "keypoints", imgShow );
46+
cv::imwrite( "./data/keypoints.png", imgShow );
47+
cv::waitKey(0); //暂停等待一个按键
48+
49+
// 计算描述子
50+
cv::Mat desp1, desp2;
51+
_descriptor->compute( rgb1, kp1, desp1 );
52+
_descriptor->compute( rgb2, kp2, desp2 );
53+
54+
// 匹配描述子
55+
vector< cv::DMatch > matches;
56+
cv::FlannBasedMatcher matcher;
57+
matcher.match( desp1, desp2, matches );
58+
cout<<"Find total "<<matches.size()<<" matches."<<endl;
59+
60+
// 可视化：显示匹配的特征
61+
cv::Mat imgMatches;
62+
cv::drawMatches( rgb1, kp1, rgb2, kp2, matches, imgMatches );
63+
cv::imshow( "matches", imgMatches );
64+
cv::imwrite( "./data/matches.png", imgMatches );
65+
cv::waitKey( 0 );
66+
67+
// 筛选匹配，把距离太大的去掉
68+
// 这里使用的准则是去掉大于四倍最小距离的匹配
69+
vector< cv::DMatch > goodMatches;
70+
double minDis = 9999;
71+
for ( size_t i=0; i<matches.size(); i++ )
72+
{
73+
if ( matches[i].distance < minDis )
74+
minDis = matches[i].distance;
75+
}
76+
77+
for ( size_t i=0; i<matches.size(); i++ )
78+
{
79+
if (matches[i].distance < 4*minDis)
80+
goodMatches.push_back( matches[i] );
81+
}
82+
83+
// 显示 good matches
84+
cout<<"good matches="<<goodMatches.size()<<endl;
85+
cv::drawMatches( rgb1, kp1, rgb2, kp2, goodMatches, imgMatches );
86+
cv::imshow( "good matches", imgMatches );
87+
cv::imwrite( "./data/good_matches.png", imgMatches );
88+
cv::waitKey(0);
89+
90+
// 计算图像间的运动关系
91+
// 关键函数：cv::solvePnPRansac()
92+
// 为调用此函数准备必要的参数
93+
94+
// 第一个帧的三维点
95+
vector<cv::Point3f> pts_obj;
96+
// 第二个帧的图像点
97+
vector< cv::Point2f > pts_img;
98+
99+
// 相机内参
100+
CAMERA_INTRINSIC_PARAMETERS C;
101+
C.cx = 325.5;
102+
C.cy = 253.5;
103+
C.fx = 518.0;
104+
C.fy = 519.0;
105+
C.scale = 1000.0;
106+
107+
for (size_t i=0; i<goodMatches.size(); i++)
108+
{
109+
// query 是第一个, train 是第二个
110+
cv::Point2f p = kp1[goodMatches[i].queryIdx].pt;
111+
// 获取d是要小心！x是向右的，y是向下的，所以y才是行，x是列！
112+
ushort d = depth1.ptr<ushort>( int(p.y) )[ int(p.x) ];
113+
if (d == 0)
114+
continue;
115+
pts_img.push_back( cv::Point2f( kp2[goodMatches[i].trainIdx].pt ) );
116+
117+
// 将(u,v,d)转成(x,y,z)
118+
cv::Point3f pt ( p.x, p.y, d );
119+
cv::Point3f pd = point2dTo3d( pt, C );
120+
pts_obj.push_back( pd );
121+
}
122+
123+
double camera_matrix_data[3][3] = {
124+
{C.fx, 0, C.cx},
125+
{0, C.fy, C.cy},
126+
{0, 0, 1}
127+
};
128+
129+
// 构建相机矩阵
130+
cv::Mat cameraMatrix( 3, 3, CV_64F, camera_matrix_data );
131+
cv::Mat rvec, tvec, inliers;
132+
// 求解pnp
133+
cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 100, inliers );
134+
135+
cout<<"inliers: "<<inliers.rows<<endl;
136+
cout<<"R="<<rvec<<endl;
137+
cout<<"t="<<tvec<<endl;
138+
139+
// 画出inliers匹配
140+
vector< cv::DMatch > matchesShow;
141+
for (size_t i=0; i<inliers.rows; i++)
142+
{
143+
matchesShow.push_back( goodMatches[inliers.ptr<int>(i)[0]] );
144+
}
145+
cv::drawMatches( rgb1, kp1, rgb2, kp2, matchesShow, imgMatches );
146+
cv::imshow( "inlier matches", imgMatches );
147+
cv::imwrite( "./data/inliers.png", imgMatches );
148+
cv::waitKey( 0 );
149+
150+
return 0;
151+
}

0 commit comments

Comments
 (0)