用python学习slam系列(二)特征提取与配准

发布时间:2017年03月14日 09:42:09    浏览数:1820次    来自:ROSClub-Ray
上一篇(从图像到点云)介绍了如何将平面图像转换为立体点云,在本篇中,我们将上一篇的代码进行封装,然后在本篇中调用封装好的函数接口。

章节目录:

        1、用python学习slam系列(一)从图像到点云

        2、用python学习slam系列(二)特征提取与配准

        3、用python学习slam系列(三)点云拼接(Python版本)

        4、用python学习slam系列(四)视觉里程计(Python版)

时间规划:

         《一起做rgbd-slam》高博原文一共9篇,加上我个人水平有限,数学也不高,所以前期先做原文代码验证及其自己补充学习,有些可能写的不对,这个也在所难免,所以预计时间为一个月,大概从2017年2月10日-2017年3月30日,这里先立一个flag,万一呢。

永久链接:

         http://www.rosclub.cn

相关代码及数据:

         https://github.com/zsirui/slam-python.git

转载请著名出处,ROSClub版权所有。

编者水平有限,有些描述的可能不对,大家轻喷。


第二篇:特征提取与配准(Python版)

0x00 准备

上一篇(从图像到点云)介绍了如何将平面图像转换为立体点云,在本篇中,我们将上一篇的代码进行封装,然后在本篇中调用封装好的函数接口。

0x01 图像配准(数学部分)

SLAM算法由定位(Localization)和建图(Mapping)构成。要求解机器人的运动问题,首先要解决如何从给定的两张图片中求出图像的运动关系。

1.png

0x02 图像配准(编程实现)

首先我们先通过深度摄像头获取一组带有明显位移的图像(RGB图像和深度图像)数据,分别命名为rgb1.png,rgb2.png,depth1.png,depth2.png。编写函数读取图像信息:

#!/usr/bin/env python    
# -*- coding: utf-8 -*-    
import cv2    
import numpy as np    
from slamBase import point2dTo3d, CameraIntrinsicParameters    
import readyaml    
def readImgFiles(RGBFilenames, DepthFilenames, paras):    
	rgbs = []    
	depths = []    
	for i in range(0, len(RGBFilenames)):    
		rgbs.append(cv2.imread(RGBFilenames[i]))    
	for j in range(0, len(DepthFilenames)):    
		depth = cv2.imread(DepthFilenames[i], paras)    
		# ROS中rqt保存的深度摄像头的图片是rgb格式,需要转换成单通道灰度格式    
		if len(depth[0][0]) == 3:    
			depths.append(cv2.cvtColor(depth, cv2.COLOR_BGR2GRAY))    
		else:    
			depths.append(depth)    
	return rgbs, depths

这里定义了两个空列表rgbs和depths,按读取顺序将图像信息保存进去。需要注意的是,使用ROS的rqt获取的深度图像是RGB三通道格式,后续代码需要单通道灰度格式图像,这里通过OpenCV进行一次格式转换才能使数据在后续代码中继续使用。

rgb1.png

rgb2.png

在这里,我们会使用OpenCV中的SIFT算法来进行特征提取并计算描述子。在OpenCV3.0以后的版本中,SIFT算法被移动到了OpenCV_Contrib库里面,需要在编译OpenCV的时候指定这个库的路径(OPENCV_EXTRA_MODULES_PATH键值)后才能使用。

具体代码如下:

def computeMatches(rgb1, rgb2, depth1, depth2, CameraIntrinsicData, distCoeffs, camera):    
	sift = cv2.xfeatures2d.SIFT_create()    
	kp1, des1 = sift.detectAndCompute(rgb1, None)    
	kp2, des2 = sift.detectAndCompute(rgb2, None)    
	print("Key points of two images: " + str(len(kp1)) + ", " + str(len(kp2)))

将提取出来的关键点(KeyPoints)在对应图片上标记后并展示出来:

	imgShow = None    
	imgShow = cv2.drawKeypoints(rgb1, kp1, imgShow, flags = cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)    
	cv2.imshow( "keypoints_1", imgShow )    
	cv2.imwrite( "./data/keypoints_1.png", imgShow )    
	cv2.waitKey(0)    
	imgShow = None    
	imgShow = cv2.drawKeypoints(rgb2, kp2, imgShow, flags = cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)    
	cv2.imshow( "keypoints_2", imgShow )    
	cv2.imwrite( "./data/keypoints_2.png", imgShow )    
	cv2.waitKey(0)

keypoints_1.png

keypoints_2.png

关键点(KeyPoints)是OpenCV中的一种特殊的数据结构,拥有点坐标,半径,角度等成员。将关键点画在图像上就是一个个不同半径的圆。标记圈的半径长短和特征点所在尺度有关,那条半径是特征点的方向。

接下来使用FLANN算法对提取后的特征点的描述子进行特征匹配运算,并用drawMatches函数画出运算后的结果:

	FLANN_INDEX_KDTREE = 0    
	index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)    
	search_params = dict(checks = 50)   # or pass empty dictionary    
	matcher = cv2.FlannBasedMatcher(index_params, search_params)    
	matches = matcher.match(des1, des2)    
	print("Find total " + str(len(matches)) + " matches.")    
	imgMatches = None    
	imgMatches = cv2.drawMatches( rgb1, kp1, rgb2, kp2, matches, imgMatches )    
	cv2.imshow( "matches", imgMatches )    
	cv2.imwrite( "./data/matches.png", imgMatches )    
	cv2.waitKey(0)

matches.png

从匹配结果来看,大部分的匹配并不是我们想象中的那样,把许多不相似的地方匹配了起来,属于误匹配。因此,我们需要将这些误匹配筛选掉。例如:筛选掉大于最小距离4倍的匹配。

	goodMatches = []    
	minDis = 9999.0    
	for i in range(0, len(matches)):    
		if matches[i].distance < minDis:    
			minDis = matches[i].distance    
	for i in range(0, len(matches)):    
		if matches[i].distance < (minDis * 4):    
			goodMatches.append(matches[i])    
	print("good matches = " + str(len(goodMatches)))    
	imgMatches = None    
	imgMatches = cv2.drawMatches( rgb1, kp1, rgb2, kp2, goodMatches, imgMatches )    
	cv2.imshow( "good_matches", imgMatches )    
	cv2.imwrite( "./data/good_matches.png", imgMatches )    
	cv2.waitKey(0)

good_matches.png

筛选过后,误匹配的情况大大减少。对筛选过后的匹配点进行PnP求解,计算两张图片的旋转矩阵R和位移矢量t:

OpenCVsolvePnPRansac API:

http://docs.opencv.org/3.0-beta/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?highlight=solvepnpransac#cv2.solvePnPRansac):

2.png

求解PnP需要输入一组匹配好的三维点objectPoints和一组二维图像点imagePoints,返回的结果是旋转向量rvec和平移向量tvec,cameraMatrix是相机内参矩阵,通过标定相机获得,distCoeffs是相机畸变参数,标定后会和相机内参矩阵同时生成写入文件中,我们可以使用readyaml.py来获取相机的畸变参数,reprojectionError参数在使用示例标定相机时最后会被打印在屏幕上 。接下来我们可以将筛选过后的goodmatches通过上一章的函数转变为三维点。

	pts_obj = []    
	pts_img = []    
	for i in range(0, len(goodMatches)):    
		p = kp1[goodMatches[i].queryIdx].pt    
		d = depth1[int(p[1])][int(p[0])]    
		if d == 0:    
			pass    
		else:    
			pts_img.append(kp2[goodMatches[i].trainIdx].pt)    
			pd = point2dTo3d(p[0], p[1], d, camera)    
			pts_obj.append(pd)    
	pts_obj = np.array(pts_obj)    
	pts_img = np.array(pts_img)    
	cameraMatrix = np.matrix(CameraIntrinsicData)    
	rvec = None    
	tvec = None    
	inliers = None    
	retval, rvec, tvec, inliers = cv2.solvePnPRansac( pts_obj, pts_img, cameraMatrix, distCoeffs, useExtrinsicGuess = False, iterationsCount = 100, reprojectionError = 0.66 )    
	print("inliers: " + str(len(inliers)))    
	print("R=" + str(rvec))    
	print("t=" + str(tvec))

求解完成后,我们就有了rvec和tvec信息,这样我们就算出了两个图像之间的运动关系了:

terminal.png

尽管经过了筛选,匹配结果中还是存在一定的误匹配情况,在OpenCV中,会利用“随机采样一致性”,在现有的匹配结果中随机提取一部分,估计其运动,在本例中,这对图像的inlier是这样的:

inliers.png

本节中,我们介绍了如何提取、匹配图像的特征,并通过这些匹配,使用ransac方法估计图像的运动。下一节,我们将介绍如何使用刚得到的平移、旋转向量来拼接点云。至此,我们就完成了一个只有两帧的迷你SLAM程序 

参考资料:

[1] 一起做RGB-D SLAM (3) - 半闲居士 - 博客园

[2] OpenCV API Reference - Camera Calibration and 3D Reconstruction 



标签:

评论共1条评论

登录后再评论!

全部评论

zhangrelay 2017年03月23日 21:56:27

眼睛已经花了