- 携带摄像头的机器人在运动过程当中,会连续性地获取多帧图像,辅助其感知周围环境和自身运动。时间序列上相连的两幅或多幅图像,一般存在相同的景物,只是它们在图像中的位置不一样。而位置的变换偏偏暗含了相机的运动,这时就须要相邻图像间的类似性匹配。
- 选取一大块图像区域进行运动估计是不可取的。已知图像在计算机内部是以数字矩阵的形式存储的,[如灰度图的每一个元素表明了单个像素的灰度值]。而对于点的提取和匹配较为方便,且和数字值对应,因此引入图像特征点辅助估计相机的运动。
特征点的提取是与关键点和描述子牢牢关联的;前端
这里以ORB为例展现特征点的提取过程。python
能够被认为是特征点 (N 一般取 12,即为 FAST-12。其它经常使用的 N 取值为 9 和 11, 他们分别被称为 FAST-9, FAST-11)。
下图较为清晰的描述了角点的检测:ios
为了提升算法效率,能够跳跃式检测每一个像素邻域圆上的像素值,如第1,5,9,13个。算法
为了不角点集中,需采用非极大值抑制,即在必定区域内仅保留响应极大值的角点。后端
Oriented FAST关键点:因为FAST角点存在必定的问题,ORB关键点对其进行了改进。优化
- FAST特征点数量大且不肯定。
咱们每每但愿对图像提取固定数量的特征,这样易于计算和匹配。改进的方式为:指定最终要提取的角点数量N,对原始提取的FAST角点分别计算Harris响应值,而后选取前N个具备最大响应值的角点。ui
- FAST角点不具备方向信息,并且选取半径为3的邻域圆也存在尺度问题。
ORB关键点添加了对尺度和旋转的描述。其中,尺度不变性经过构建图像金字塔 [ 同一图像的不一样分辨率表示 ],在每一层进行角点检测;spa
旋转不变性采用灰度质心法实现,质心是指以图像块灰度值做为权重的中心,其具体步骤以下——来自视觉SLAM十四讲:设计
在一个小的图像块 B 中,定义图像块的矩为:3d
这里的矩实际为HU矩中的原点矩。其通常用来识别图像中大的物体,可以较好地描述物体的形状,要求图像的纹理特征不能太复杂。
经过矩能够找到图像块的质心:
链接图像块的几何中心 O 与质心 C,获得一个方向向量,因而特征点的方向能够定义为:
这里θ即为旋转的角度描述。
知识点拓展:图像的几何矩
- 在数学中,矩的本质是数学指望,即:
\[ \int x · f(x) dx \]
其中f(x)表明x的几率密度。
在灰度图像中,每一个像素点的值能够当作该处的密度。对某个像素点的值求指望即获得了图像在该点处的矩。矩有原点矩(零阶矩)、一阶矩、二阶矩;由零阶矩和一阶矩能够计算某个形状的重心,而二阶矩能够计算形状的方向。
\[ 零阶矩:M_{00} = \sum_{x,y} I(x,y) \]\[ 一阶矩:M_{10} = \sum_{x,y} x·I(x,y);M_{01} = \sum_{x,y}y·I(x,y) \]
\[ 推出图像的重心(质心)坐标:x_c = \frac{M_{10}}{M_{00}};y_c = \frac{M_{01}}{M_{00}} \]
这种质心计算方法的优势为:对噪声不敏感。
\[ 二阶矩:M_{20} = \sum x^2 · I(x,y);M_{02} = \sum y^2 · I(x,y);M_{11}=\sum x·y·I(x,y) \]
\[ 则物体的形状方向:θ = \frac{1}{2}arctan(\frac{2b}{a-c}),θ∈[-90°,90°] \]
\[ 其中,a=\frac{M_{20}}{M_{00}}-x_c^2;b=\frac{M_{11}}{M_{00}}-x_cy_c;c=\frac{M_{02}}{M_{00}}-y_c^2; \]
图像的矩具备旋转、平移、尺度等的不变性,又称为不变矩。
BRIEF描述子:一种二进制描述子,向量由多个0和1组成,这里0和1表示关键点附近的两个像素(如p,q)的大小关系——p>q,取1;p<q,取0。而p和q的选取是按照某种几率分布随机选取。
优势:速度快,存储方便;适用于实时的图像匹配。
改进BRIEF描述子(rBRIEF):加入旋转因子,改进旋转不变性;改进特征点描述子的相关性(便可区分性),对误匹配率影响较大。
特征匹配实际上描述的是连续的两张图像中景物的对应关系,以此来估计相机的运动。
匹配方法:
暴力匹配。即图像1中取特征点集合A,图像2中取特征点集合B,对A中的每个特征点,依次与B中全部的特征点测量二者描述子的距离,而后排序,取距离最近的做为匹配的两个特征点。
这里,描述子距离表示了两张图片两个特征点之间的类似程度,在实际应用中能够取不一样的距离度量范数。如:对于浮点类型,使用欧氏距离;对于二进制类型,使用汉明距离[即两个二进制串之间不一样位数的个数]。
缺点:计算量随特征点数量增大呈递增趋势。
快速近似最近邻(FLANN)。集成于OpenCV,适合于匹配点数量极多的状况。
注释写在了源代码里。
#include <iostream> #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; } //-- 读取图像,以RGB格式读取 Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR ); Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR ); //-- 初始化 /* * 1. FeatureDetector特征检测器类,经过建立该类对象来使用多种特征检测方法。【用于检测指定特征点的角点位置】 OpenCV2.4.3中提供了10种特征检测方法:FAST,SIFT,SURF,ORB,HARRIS,SimpleBlob,STAR,MSER,GFTT,Dense。 这里使用ORB特征检测方法 * 2. DescriptorExtractor特征描述子提取类,提供了一些特征描述子提取的算法。 其能够针对图像关键点,计算其特征描述子,其能够被表达成密集(dense),即固定维数的向量。 其计算方式为每隔固定个像素计算。 一个特征描述子是一个向量,特征描述子的集合为Mat格式,每一行是一个关键点的特征描述子。 OpenCV支持四种类型的特征描述子提取方法:SIFT,SURF,ORB,BRIEF。 * 3. DescriptorMatcher特征匹配类,提供了一些特征点匹配的方法。 该类主要包含图像对之间的匹配以及图像和一个图像集之间的匹配。 OpenCV2中的特征匹配方法都继承自该类。 对于误匹配状况,提供了KNNMatch方法。 */ std::vector<KeyPoint> keypoints_1, keypoints_2; // 声明存放两张图片各自关键点的向量 Mat descriptors_1, descriptors_2; // 两张图片特征点的描述子,形式为向量 Ptr<FeatureDetector> detector = ORB::create(); // 建立FeatureDetector类型的指针detector Ptr<DescriptorExtractor> descriptor = ORB::create(); // ORB特征描述子提取指针 // Ptr<FeatureDetector> detector = FeatureDetector::create(detector_name); // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create(descriptor_name); Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create ( "BruteForce-Hamming" ); // 匹配时计算描述子之间的距离使用汉明距离 //-- 第一步:检测 Oriented FAST 角点位置,使用detect方法,将其存入Keypoints变量中 detector->detect ( img_1,keypoints_1 ); detector->detect ( img_2,keypoints_2 ); //-- 第二步:根据角点位置计算 BRIEF 描述子,使用compute方法,将其存入descriptors变量中 descriptor->compute ( img_1, keypoints_1, descriptors_1 ); descriptor->compute ( img_2, keypoints_2, descriptors_2 ); /* * 绘制第一幅图像的ORB特征点,注意这里要把图片的名称全改成英文才能正常显示。 */ Mat outimg1; drawKeypoints( img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT ); imshow("ORB features.jpg",outimg1); //-- 第三步:对两幅图像中的BRIEF描述子进行匹配,使用 Hamming 距离 /* * Brute-Force(BF)匹配器,取第一个集合里一个特征的描述子并与第二个集合里全部特征的描述子计算距离,返回最近的特征点。 其有两个方法:BFMatcher.match()和BFMatcher.knnMatch(),前者返回最匹配的特征点(仅一个),后者返回k个最匹配的特征点,k由用户指定。 */ vector<DMatch> matches; // DMatch类型的向量。 //BFMatcher matcher ( NORM_HAMMING ); matcher->match ( descriptors_1, descriptors_2, matches ); // 使用match方法,获取最匹配的两个特征点(匹配的特征点能够有多对,均存放在matche中) //-- 第四步:匹配点对筛选 double min_dist=10000, max_dist=0; //找出全部匹配之间的最小距离和最大距离, 便是最类似的和最不类似的两组点之间的距离 /* * descriptors的每行为一个特征点的描述子向量;图像1中有多少个特征点,matches中存储多少个匹配的图像2的特征点相关类型 */ for ( int i = 0; i < descriptors_1.rows; i++ ) { double dist = matches[i].distance; // 得出每对匹配的描述子间的距离 if ( dist < min_dist ) min_dist = dist; if ( dist > max_dist ) max_dist = dist; } // 仅供娱乐的写法 min_dist = min_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance; max_dist = max_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance; printf ( "-- Max dist : %f \n", max_dist ); printf ( "-- Min dist : %f \n", min_dist ); /* * 对于误匹配状况采起的措施 */ //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会很是小,设置一个经验值30做为下限. std::vector< DMatch > good_matches; for ( int i = 0; i < descriptors_1.rows; i++ ) { if ( matches[i].distance <= max ( 2*min_dist, 30.0 ) ) { good_matches.push_back ( matches[i] ); } } //-- 第五步:绘制匹配结果,数量分别为matches和good_matches Mat img_match; Mat img_goodmatch; drawMatches ( img_1, keypoints_1, img_2, keypoints_2, matches, img_match ); drawMatches ( img_1, keypoints_1, img_2, keypoints_2, good_matches, img_goodmatch ); imshow ( "all match features.jpg", img_match ); // 注意这里图片名要改为英文 imshow ( "good match features.jpg", img_goodmatch ); waitKey(0); return 0; }
- 视觉SLAM主要分为视觉前端和优化后端,前端即视觉里程计(VO),其做用是根据相邻图像的信息,估计相机运动,提供初始值给后端。
- 根据是否须要提取特征,VO分为特征点法和直接法。
用于3D-3D的位姿估计,即两张图像为3D图像(如RGB-D相机获取的图像),其中的3D特征点集合已经提取和匹配好。
ICP即为迭代最近点,仅考虑两组3D点之间的变换,此时和相机无关。特征点方法正好为咱们提供了两张图像之间良好的匹配关系,ICP能够据此进行运动估计。
利用线性代数方法的求解(SVD):由两张图片,获得两组匹配好的3D特征点集合,这两组一一对应的点之间具备欧氏变换关系,即集合1中的点能由集合2中的点经过变换矩阵获得,即有R和T表示两个点之间的关系。
匹配好的特征点对会存在必定的偏差,定义偏差项,由偏差项构建最小二乘问题,即表示偏差平方和,求一个最优化问题,这里是求使偏差平方和达到极小的R,t。
【理解:由于ICP问题求解的目的是得出相机的位姿估计,能够理解为当拍摄到第二帧图像时相机的位置与拍摄第一帧图像时相机原位置的变换关系,而变换关系能够用欧氏矩阵R和t描述,而由匹配的特征点对能够描述多个相机位姿,这里就涉及一个最优化问题,即构建最小二乘,求解使相机位姿相对最精确的R和t。】
[注:可是具体的求解数学公式没看懂。]
利用非线性优化方法的求解:以迭代的方式找最优值。[一样数学式没看懂。]
源代码提供了SVD和非线性优化两种方法。
添加了一点关键注释。
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 ); void pose_estimation_3d3d ( const vector<Point3f>& pts1, const vector<Point3f>& pts2, Mat& R, Mat& t ); void bundleAdjustment( const vector<Point3f>& points_3d, const vector<Point3f>& points_2d, Mat& R, Mat& t );
// g2o edge /* * g2o是一个图优化库,图优化的本质仍然是非线性优化,而用图的方式表示,可使问题可视化。 */ class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : _point(point) {} virtual void computeError() { const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] ); // measurement is p, point is p' _error = _measurement - pose->estimate().map( _point ); } virtual void linearizeOplus() { g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]); g2o::SE3Quat T(pose->estimate()); Eigen::Vector3d xyz_trans = T.map(_point); double x = xyz_trans[0]; double y = xyz_trans[1]; double z = xyz_trans[2]; _jacobianOplusXi(0,0) = 0; _jacobianOplusXi(0,1) = -z; _jacobianOplusXi(0,2) = y; _jacobianOplusXi(0,3) = -1; _jacobianOplusXi(0,4) = 0; _jacobianOplusXi(0,5) = 0; _jacobianOplusXi(1,0) = z; _jacobianOplusXi(1,1) = 0; _jacobianOplusXi(1,2) = -x; _jacobianOplusXi(1,3) = 0; _jacobianOplusXi(1,4) = -1; _jacobianOplusXi(1,5) = 0; _jacobianOplusXi(2,0) = -y; _jacobianOplusXi(2,1) = x; _jacobianOplusXi(2,2) = 0; _jacobianOplusXi(2,3) = 0; _jacobianOplusXi(2,4) = 0; _jacobianOplusXi(2,5) = -1; } bool read ( istream& in ) {} bool write ( ostream& out ) const {} protected: Eigen::Vector3d _point; };
int main ( int argc, char** argv ) { if ( argc != 5 ) { cout<<"usage: pose_estimation_3d3d img1 img2 depth1 depth2"<<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 depth1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED ); // 深度图为16位无符号数,单通道图像 Mat depth2 = imread ( argv[4], CV_LOAD_IMAGE_UNCHANGED ); // 深度图为16位无符号数,单通道图像 Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); // 理解这里的K为相机内参 vector<Point3f> pts1, pts2; for ( DMatch m:matches ) { ushort d1 = depth1.ptr<unsigned short> ( int ( keypoints_1[m.queryIdx].pt.y ) ) [ int ( keypoints_1[m.queryIdx].pt.x ) ]; ushort d2 = depth2.ptr<unsigned short> ( int ( keypoints_2[m.trainIdx].pt.y ) ) [ int ( keypoints_2[m.trainIdx].pt.x ) ]; if ( d1==0 || d2==0 ) // bad depth continue; Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K ); // 经过相机内参将两幅图像特征点的像素坐标转化为相机坐标 Point2d p2 = pixel2cam ( keypoints_2[m.trainIdx].pt, K ); float dd1 = float ( d1 ) /5000.0; // 深度值尺度缩放 float dd2 = float ( d2 ) /5000.0; pts1.push_back ( Point3f ( p1.x*dd1, p1.y*dd1, dd1 ) ); // 获得特征点的3D坐标 pts2.push_back ( Point3f ( p2.x*dd2, p2.y*dd2, dd2 ) ); } cout<<"3d-3d pairs: "<<pts1.size() <<endl; Mat R, t; pose_estimation_3d3d ( pts1, pts2, R, t ); // 求得最优的R和t cout<<"ICP via SVD results: "<<endl; cout<<"R = "<<R<<endl; cout<<"t = "<<t<<endl; cout<<"R_inv = "<<R.t() <<endl; cout<<"t_inv = "<<-R.t() *t<<endl; cout<<"calling bundle adjustment"<<endl; bundleAdjustment( pts1, pts2, R, t ); // 非线性优化方法求解ICP问题 // verify p1 = R*p2 + t for ( int i=0; i<5; i++ ) { cout<<"p1 = "<<pts1[i]<<endl; cout<<"p2 = "<<pts2[i]<<endl; cout<<"(R*p2+t) = "<< R * (Mat_<double>(3,1)<<pts2[i].x, pts2[i].y, pts2[i].z) + t <<endl; cout<<endl; } }
还有feature_extraction.cpp中的特征点提取、匹配和跟踪的代码,这个上边已经分析过了。
绘制了光流法的原理图