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

发布时间:2017年07月27日 15:49:55    浏览数:155次    来自: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 准备

在上一篇中,我们利用比对两张图片的特征点,估计相机运动,最后得出了旋转向量和位移向量。在本篇中,我们将使用这两个向量,将两张图像的点云拼接起来,组合成一个更大的点云。

首先,先将上一篇估计相机运动的内容进行封装,这里我们定义了一个名为SolvePnP的类Python没有C/C++中结构体的概念): 

class SolvePnP(object):
	def __init__(self, RGBFileNameList, DepthFileNameList, distCoeffs, CameraIntrinsicData, camera):
		super(SolvePnP, self).__init__()
		self.RGBFileNameList = RGBFileNameList
		self.DepthFileNameList = DepthFileNameList
		self.distCoeffs = distCoeffs
		self.CameraIntrinsicData = CameraIntrinsicData
		self.camera = camera

在本篇中还是使用上一篇的两张图片作为数据,为了简化操作,在类的初始化时,输入RGB图像文件名列表和深度图像的文件名列表以及通过标定获得的摄像头畸变参数与摄像头内参矩阵。camera是从相机内参矩阵中分离出来的对应参数,在之前图像转点云中使用这些参数进行运算(本篇中也会使用)。在这个类中,定义一个readImgFiles的方法,将输入进来的文件名列表按顺序使用OpenCV读取图片信息,返回的是RGB图像信息列表和深度图像信息列表(C++不同的是,Pythonreturn可以返回多个变量):

         def readImgFiles(self, 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

将上一篇实现的计算图像特征点来估算相机运动的代码封装进这个类中,并返回旋转向量,平移向量和inliers

def ResultOfPnP(self, rgb1, rgb2, depth, distCoeffs, CameraIntrinsicData, camera):
                   sift = cv2.xfeatures2d.SIFT_create()
                   kp1, des1 = sift.detectAndCompute(rgb1, None)
                   kp2, des2 = sift.detectAndCompute(rgb2, None)
 
                   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.")
 
                   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)))
 
                   pts_obj = []
                   pts_img = []
 
                   for i in range(0, len(goodMatches)):
                            p = kp1[goodMatches[i].queryIdx].pt
                            d = depth[0][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 = 1.76 )
                   return rvec, tvec, inliers

0x01 拼接点云(数学部分)

1.png


0x02 拼接点云(编程实现)

在高翔的博客中,使用PCL自带的点云变换函数进行点云旋转与平移,但是Python中的PCL库并没有这个函数的接口,所以我们得自己编程实现:

def transformPointCloud(src, T):
         pointcloud = []
         for item in src:
                   a = list(item)
                   a.append(1)
                   a = np.matrix(a)
                   a = a.reshape((-1, 1))
                   temp = T * a
                   temp = temp.reshape((1, -1))
                   temp = np.array(temp)
                   temp = list(temp[0])
                   pointcloud.append(temp[0:3])
         return pointcloud

2.png

class SolvePnP(object):
         ……
         def transformMatrix(self, rvec, tvec):
                   temp = np.matrix([0, 0, 0, 1])
                   r = np.matrix(rvec)
                   t = np.matrix(tvec)
                   dst, jacobian = cv2.Rodrigues(r)
                   c = np.hstack((dst, t))
                   T = np.vstack((c, temp))
                   return T

其中,rvec是旋转向量,tvec是平移向量。OpenCV中的罗德里格斯变换会返回一个旋转矩阵和雅可比矩阵(这里并不使用,但是没有接收变量Python会报错),最后使用numpy库对矩阵进行合并。在SolvePnP类的构造函数中添加矩阵变换和求解PnP的调用:

class SolvePnP(object):
def __init__(self, RGBFileNameList, DepthFileNameList, distCoeffs, CameraIntrinsicData, camera):
                   super(SolvePnP, self).__init__()
                   ……
self.rvec, self.tvec, self.inliers = self.ResultOfPnP(self.frame1.kps, self.frame2.kps, self.frame1.des, self.frame2.des, self.frame1.depth, self.distCoeffs, self.CameraIntrinsicData, self.camera)
                   self.T = self.transformMatrix(self.rvec, self.tvec)

最后编写程序入口joinPointCloud.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
 
import slamBase
from pcl import save
import readyaml
 
RGBFileNameList = ['./data/rgb1.png', './data/rgb2.png']
DepthFileNameList = ['./data/depth1.png', './data/depth2.png']
CalibrationDataFile = './calibration/asus/camera.yml'
CloudFilename = './data/cloud.pcd'
 
def main():
         CameraIntrinsicData, DistortionCoefficients = readyaml.parseYamlFile(CalibrationDataFile)
         camera = slamBase.CameraIntrinsicParameters(CameraIntrinsicData[0][2], CameraIntrinsicData[1][2], CameraIntrinsicData[0][0], CameraIntrinsicData[1][1], 1000.0)
         frame1 = slamBase.Frame(RGBFileNameList[0], DepthFileNameList[0])
         frame2 = slamBase.Frame(RGBFileNameList[1], DepthFileNameList[1])
         pnp = slamBase.SolvePnP(DistortionCoefficients, CameraIntrinsicData, camera, frame1, frame2)
         p0, c0 = slamBase.imageToPointCloud(RGBFileNameList[0], DepthFileNameList[0], camera)
         p1, c1 = slamBase.imageToPointCloud(RGBFileNameList[1], DepthFileNameList[1], camera)
         colors = c0 + c1
         p = slamBase.transformPointCloud(p0, pnp.T)
         pointcloud = slamBase.addPointCloud(p, p1)
         save(pointcloud, CloudFilename, format = 'pcd')
         slamBase.addColorToPCDFile(CloudFilename, colors)
 
if __name__ == '__main__':
         main()

这样,我们就实现了一个只有两帧的SLAM程序,这已经是一个视觉里程计的雏形,只要把后续新的数据与之前的数据不断进行比对,就可以得到完整的点云数据和地图数据了。

0x03 优化:

定义一个名为Frame的类(一个类能解决的事,就不浪费那么多变量和方法了),将读取图片信息和计算关键点(KeyPoints)和描述子(Descriptors)方法封装进去:

class Frame(object):
         def __init__(self, RGBFilename, DepthFilename):
                   super(Frame, self).__init__()
                   self.rgb = self.ReadImg(RGBFilename)
                   self.depth = self.ReadImg(DepthFilename)
                   self.kps, self.des = self.ComputeKPointsAndDescriptors(self.rgb)
 
         def ReadImg(self, filename):
                   if 'depth' in filename:
                            img = cv2.imread(filename)
                            return cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
                   else:
                            return cv2.imread(filename)
 
         def ComputeKPointsAndDescriptors(self, rgb):
                   sift = cv2.xfeatures2d.SIFT_create()
                   kps, des = sift.detectAndCompute(rgb, None)
                   return kps, des

这样我们可以通过frame = Frame(RGBFilename, DepthFilename)实现获取图像内容以及关键点和描述子的操作。

参考资料:

[1] 乾坤有数的博客 - OpenCV学习笔记(一)——旋转向量与旋转矩阵相互转化http://blog.sina.com.cn/s/blog_5fb3f125010100hp.html

[2] 一起做RGB-D SLAM (4) - 半闲居士 - 博客园 http://www.cnblogs.com/gaoxiang12/p/4669490.html



标签: slampython

评论共0条评论

登录后再评论!

全部评论

目前没有评论