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

发布时间:2017年03月13日 00:54:19    浏览数:6202次    来自:ROSClub-Ray
最近开始学习ros,然后搜索slam教程,看到高翔大神写的《一起做rgbd-slam》系列,很有启发,也很佩服,但是高翔大神用的都是c++,本人比较喜欢python,所以想把文章中的代码改成python版本,也好记录一下自己的学习心得。

章节目录:

        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版权所有。

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


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

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

另外:Python是最好的语言。

第一篇:从图像到点云(Python版)

参考链接:

http://www.cnblogs.com/gaoxiang12/p/4652478.html

http://rosclub.cn/post-75.html

0x00 准备

软件:ubuntu14.04、ros-indigo-desktop-full、python2.7、pip等

硬件:asus xtion pro、电脑

依赖库:OpenCV(强烈建议使用3.0以上版本,本教程所有代码使用的是OpenCV3.2.0版)、Numpy、PCL

依赖库安装:

pip:

sudo apt-get install python-pip

OpenCV:

(1)如果安装了ros,用自带的就可以

(2)编译安装OpenCV(推荐使用

编译安装步骤(参考地址:http://www.cnblogs.com/asmer-stone/p/5089764.html

1.依赖关系:

    GCC 4.4.x or later

    CMake 2.8.7 or higher

    Git

    GTK+2.x or higher, including headers (libgtk2.0-dev)

    pkg-config

    Python 2.6 or later and Numpy 1.5 or later with developer packages (python-dev, python-numpy)

    ffmpeg or libav development packages: libavcodec-dev, libavformat-dev, libswscale-dev

    [optional] libtbb2 libtbb-dev

    [optional] libdc1394 2.x

    [optional] libjpeg-dev, libpng-dev, libtiff-dev, libjasper-dev, libdc1394-22-dev

注:官方文档中虽然说其中一些依赖包是可选的,但是最好还是都装上,以防出问题。

以上依赖包可使用以下命令安装:

sudo apt-get install build-essential
sudo apt-get install cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev
sudo apt-get install python-dev python-numpy libtbb2 libtbb-dev libjpeg-dev libpng-dev libtiff-dev libjasper-dev libdc1394-22-dev

2.安装cmake-gui(用于生成OpenCV编译文件):

sudo apt-get install cmake-gui

3.下载OpenCV源代码:

可以从OpenCV官网直接下载:http://opencv.org/downloads.html

也可以使用git命令行直接clone:

cd ~/<my_working_directory> 
//比如工作目录为opencv即,cd ~/opencv
git clone https://github.com/Itseez/opencv.git
git clone https://github.com/Itseez/opencv_contrib.git

进入OpenCV源代码目录,创建build文件夹:

cd ~/<my_working_directiory>/opencv
mkdir build

使用cmake-gui生成编译文件:

打开cmake-gui后,在source code栏选择OpenCV源代码所在目录(例如:/home/username/workplace/opencv),在build栏中填写刚才新建的build文件夹(例如:/home/username/workplace/opencv/build)如下图所示:

1.png

点击Configure按钮,会弹出如下图所示的提示框,点击Finish按钮即可:

2.png

接下来cmake会检查所有依赖库以及其他各可选部分版本,这个过程中cmake会下载一个名为ippicv的库,但经常会出现下载失败而导致configure失败,建议手动下载下载地址:

https://raw.githubusercontent.com/Itseez/opencv_3rdparty/81a676001ca8075ada498583e4166079e5744668/ippicv/ippicv_linux_20151201.tgz

后放入/home/username/workplace/opencv/3rdparty/ippicv/downloads/linux-808b791a6eac9ed78d32a7666804320e文件夹中。检查完毕后需要修改OPENCV_EXTRA_MODULES_PATH内容,如下图所示:

3.png

修改完后再点击Configure按钮,如果依旧有红色高亮显示条目,则再次点击Configure按钮,等待所有条目均不再高亮显示后点击Generate按钮,退出cmake-gui程序,在命令行里进入/home/username/workplace/opencv/build目录,输入以下命令:

make -j8
sudo make install
sudo ldconfig

[注] 建议勾选BUILD_EXAMPLE选项,选用了不同的摄像头时需要先对摄像头进行标定,OpenCV示例中有提供标定程序。

Numpy:pip install numpy (可能需要管理员权限)

PCL:先安装基本pcl库,再去git上下载python版pcl接口

步骤:(Ubuntu下,其他Linux发行版本安装办法参见:http://pointclouds.org/downloads/linux.html

1)  通过PPA安装完整PCL库

sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl
sudo apt-get update
sudo apt-get install libpcl-all

2)  安装python-pcl

在github上下载python-pcl(地址:https://github.com/strawlab/python-pcl

解压后进入python-pcl目录,输入命令:sudo python setup.py install即可,如果报错,可能缺少依赖库。使用命令sudo pip install cython安装python-pcl的依赖库后再执行安装命令即可。

在ROS中,我们输入以下命令可以让kinect运行:

roslaunch openni_launch openni.launch

如果kinect连接在电脑上,可以通过输入下面命令:

rosrun image_view image_view image:=/camera/rgb/image_color

这样在启动起来的rviz界面中就可以看到kinect获取的图像信息

不过,如果手头没有kinect,可以使用下面提供的两张图片

(来源:http://www.cnblogs.com/gaoxiang12/p/4652478.html)来作为测试数据使用,该组图片的相机内参矩阵为: 

C=[[518.0,0,325.0],[0,519.0,253.5],[0,0,1]]

4.png

5.png

或者使用自己手中深度摄像头获取的图片信息来作为测试数据使用(需要提前对摄像头进行标定):

或者使用自己手中深度摄像头获取的图片信息来作为测试数据使用(需要提前对摄像头进行标定):

或者使用自己手中深度摄像头获取的图片信息来作为测试数据使用(需要提前对摄像头进行标定):

rgb.png

depth.png

这里的两张图片中,左边是RGB数据图片,右边的是深度数据图片

接下来,我们将编写一段python脚本将这两张图片数据转换成3D点云数据。

0x01 获取相机内参矩阵(标定相机)

不同的深度摄像头具有不同的特征参数,在计算机视觉里面,将这组参数成为相机的内参矩阵C。格式为:

8.png




其中,fx,fy指相机在x轴和y轴上的焦距,cx,cy是相机的光圈中心,这组参数是摄像头生产制作之后就固定的。获取这组参数,只有通过相机标定。

1、首先,在一张A4大小的纸上打印8*6的黑白棋盘格作为标定板,使用摄像头拍摄不同角度的带有完整标定板的图片,数量在10~20张之间,图片上一定要能够完整识别黑白格的边界交点。将OpenCV中编译好的示例程序中的cpp-example-calibration和cpp-example-imagelist-creator的二进制程序(二进制程序保存在<opencv_source>/build/bin下)复制到图片目录中,在终端中切换到图片目录,输入以下命令:

./cpp_example_imagelist_creator image_list.xml *.png
./cpp_example_calibration -w=7 -h=5 -n=12 -o=camera.yml -op -oe image_list.xml

第一条命令是用于生成标定程序所需的图片列表,图片信息保存在image_list.xml中;

第二条命令是根据图片列表进行标定,

-w表示标定板横向有7个交点,

-h表示纵向有5个交点,

-n表示图片一共有12张,

-o是输出文件保存在camera.yml中,

-op和-oe是将检测到的点和相机外参一并输出到文件中。

camera.yml文件内容如下(完整文件数据内容过长,省略部分数据):

%YAML: 1.0
---
calibration_time: "Wed 08 Mar 2017 09:35:01 AM CST"
nframes: 12
image_width: 640
image_height: 480
board_width: 7
board_height: 5
square_size: 3.2869998931884766e+01
aspectRatio: 1.
flags: 2
camera_matrix: !!opencv-matrix
         rows: 3
         cols: 3
         dt: d
         data: [ 6.0782982475382448e+02, 0., 3.6927587384670619e+02, 0.,
         6.0782982475382448e+02, 2.0049685183455608e+02, 0., 0., 1. ]
distortion_coefficients: !!opencv-matrix
         rows: 5
         cols: 1
         dt: d
         data: [ 2.6097424564484067e-01, -3.0826179906924500e-01,
         -3.6561830159047480e-02, 2.8149494385205569e-02,
         9.0272371325874165e-02 ]
avg_reprojection_error: 6.6092895845048028e-01
per_view_reprojection_errors: !!opencv-matrix
         rows: 12
         cols: 1
         dt: f
         data: [ 1.12569833e+00, 3.25649649e-01, 3.52165848e-01,
         2.45700657e-01, 2.78621674e-01, 5.25273621e-01, 7.99378633e-01,
         1.01514637e+00, 6.92025602e-01, 5.77147245e-01, 8.56786847e-01,
         3.39320183e-01 ]
extrinsic_parameters: !!opencv-matrix
         rows: 12
         cols: 6
         dt: d
         data: [ -8.6337003636994508e-02, -1.1783451602046391e+00,
         2.7303741738649485e+00, 7.2512051125174594e+01,
         ……
         5.2296340497370135e-02, -3.3999347389601979e+02,
         -1.0395036593678920e+02, 8.6211111700661570e+02 ]
image_points: !!opencv-matrix
         rows: 12
         cols: 35
         dt: "2f"
         data: [ 4.20297913e+02, 1.73744476e+02, 3.97531860e+02,
         1.79406143e+02, 3.75055542e+02, 1.85217300e+02, 3.52161591e+02,
         ……
         2.22855743e+02, 2.41747681e+02, 2.25628357e+02, 2.64387665e+02,
         2.28642029e+02 ]

这里,camera_matrix就是我们所需要的相机内参。我们可以使用python的yaml(如果提示ImportError: No module named yaml,可以使用sudo pip install yaml安装)库对yml文件进行读取解析操作。

代码如下: 

#!/usr/bin/env python
# -*- coding: utf-8 -*-
 
import yaml, sys
import numpy as np
 
def fixYamlFile(filename):
         with open(filename, 'rb') as f:
                   lines = f.readlines()
         if lines[0] != '%YAML 1.0\n':
                   lines[0] = '%YAML 1.0\n'
         for line in lines:
                   if ' !!opencv-matrix' in line:
                            lines[lines.index(line)] = line.split(' !!opencv-matrix')[0] + '\n'
         with open(filename, 'wb') as fw:
                   fw.writelines(lines)
 
def parseYamlFile(filename):
         fixYamlFile(filename)
         f = open(filename)
         x = yaml.load(f)
         f.close()
         arr = np.array(x['camera_matrix']['data'], dtype = np.float32)
         return (arr.reshape(3, 3))
 
if __name__ == '__main__':
         print(parseYamlFile(sys.argv[1]))

OpenCV生成的YAML文件会带有一些特殊标记,而python的yaml库会将这些标记认为文件错误,所以首先先将这些标记去除,然后在进行解析,最后使用numpy库将相机内参矩阵一维数组转换成3*3的矩阵。将脚本保存为readyaml.py,留作自定义库在后续代码中引入调用。

0x02 数学模型

在计算机中,我们需要把相机转换为一种数学模型,这样我们把图片信息交给计算机才会被正确的识别出来。在SLAM中,相机被简化成针孔相机模型,如下图所示(图片来自http://www.comp.nus.edu.sg/~cs4243/lecture/camera.pdf):

9.png

0x03 2D到3D(编程)

首先我们需要对相机内参加以封装,便于程序调用。在python中,一切都是为对象(object),那么我们就先定义个名为CameraIntrinsicParameters的类来对相机内参进行封装:

class CameraIntrinsicParameters(object):
         def __init__(self, cx, cy, fx, fy, scale):
                   super(CameraIntrinsicParameters, self).__init__()
                   self.cx = cx
                   self.cy = cy
                   self.fx = fx
                   self.fy = fy
                   self.scale = scale

把相机内参封装好了之后,可以通过将标定好的相机内参数据传递进来保存。

CameraIntrinsicData = readyaml.parseYamlFile('./calibration_data/asua/camera.yml')
camera = CameraIntrinsicParameters(CameraIntrinsicData[0][2], CameraIntrinsicData[1][2], CameraIntrinsicData[0][0], CameraIntrinsicData[1][1], 1000.0)

这样,指定的相机内参就保存在名为camera的类变量里面了。

根据之前的数学模型,我们需要读取两张图片内的信息,这里就需要借助OpenCV库来获取图片信息了:

import cv2
rgb = cv2.imread(“rgb.png”)
depth = cv2.imread( “depth.png”, -1 )

在python中,cv2.imread()函数会将图片数据存储为一个多维列表(list),rgb存储了图像的颜色信息,但cv2会将单个像素的颜色信息存储为[b, g, r]格式。有了图像信息,我们就可以开始转换点云了,这里需要借助名为PCL(点云库)的第三方库来生成点云。首先使用PCL库创建一个空白的点云:

import pcl
cloud = pcl.PointCloud()

获取图像的长度和宽度并新建一个名为pointcloud的空列表,以便后续程序遍历整个图像使用:

rows = len(depth)
cols = len(depth[0])
pointcloud = []

接下来遍历图像,根据之前推导出来的公式计算点云信息:

for m in range(0, rows):
         for n in range(0, cols):
                   d = depth[m][n]
                   if d == 0:
                            pass
                   else:
                            z = float(d) / camera.scale
                            x = (n - camera.cx) * z / camera.fx
                            y = (m - camera.cy) * z / camera.fy
                            points = [x, y, z]
                            pointcloud.append(points)

由于pcl库并不会直接识别列表格式的点云数据,所以我们需要使用numpy库进行数据格式转换,并将点云保存在文本文件中:

import numpy as np
pointcloud = np.array(pointcloud, dtype = np.float32)
cloud.from_array(pointcloud)
pcl.save(cloud, “cloud.pcd”, format = 'pcd')

这样,一个简单的将图像转换成点云的脚本就写好了。

0x04 优化

当我们执行完上述脚本后,通过pcl_viewer打开pcd文件,会发现生成的点云并没有颜色信息。接下来我们将对代码进行优化,让生成的点云附带有rgb颜色信息。

要想让点云带有颜色信息,需要先来研究下pcd的文件格式。我们可以使用文本编辑器(如Ubuntu下的gedit,sublime text等)打开cloud.pcd。这时我们会发现这个文件的前几行起到了文件头的作用。通过查询网络,这些文件头有如下含义:

(以下内容来自:http://www.pclcn.org/study/shownews.php?lang=cn&id=54

VERSION – 指定PCD文件版本

· FIELDS – 指定一个点可以有的每一个维度和字段的名字。例如:

FIELDS x y z                                       # XYZ data
FIELDS x y z rgb                                   # XYZ + colors
FIELDS x y z normal_x normal_y normal_z            # XYZ + surface normals
FIELDS j1 j2 j3                                    # moment invariants
...

· SIZE – 用字节数指定每一个维度的大小。例如:

unsigned char/char has 1 byte
unsigned short/short has 2 bytes
unsignedint/int/float has 4 bytes
double has 8 bytes

· TYPE – 用一个字符指定每一个维度的类型。现在被接受的类型有:

    I – 表示有符号类型int8(char)、int16(short)和int32(int);

    U – 表示无符号类型uint8(unsigned char)、uint16(unsigned short)和uint32(unsigned int);

    F – 表示浮点类型。

· COUNT – 指定每一个维度包含的元素数目。

    例如,x这个数据通常有一个元素,但是像VFH这样的特征描述子就有308个。实际上这是在给每一点引入n维直方图描述符的方法,把它们当做单个的连续存储块。默认情况下,如果没有COUNT,所有维度的数目被设置成1。

· WIDTH – 用点的数量表示点云数据集的宽度。根据是有序点云还是无序点云,WIDTH有两层解释:

    1)它能确定无序数据集的点云中点的个数(和下面的POINTS一样);

    2)它能确定有序点云数据集的宽度(一行中点的数目)。

    例如:

        WIDTH 640       # 每行有640个点

· HEIGHT – 用点的数目表示点云数据集的高度。类似于WIDTH ,HEIGHT也有两层解释:

    1)它表示有序点云数据集的高度(行的总数);

    2)对于无序数据集它被设置成1(被用来检查一个数据集是有序还是无序)。

    有序点云例子:

    WIDTH 640       # 像图像一样的有序结构,有640行和480列,

    HEIGHT 480      # 这样该数据集中共有640*480=307200个点

    无序点云例子:

    WIDTH 307200

    HEIGHT 1        # 有307200个点的无序点云数据集

· VIEWPOINT – 指定数据集中点云的获取视点。

    VIEWPOINT有可能在不同坐标系之间转换的时候应用,在辅助获取其他特征时也比较有用,例如曲面法线,在判断方向一致性时,需要知道视点的方位,视点信息被指定为平移(txtytz)+ 四元数(qwqxqyqz)。默认值是:VIEWPOINT 0 0 0 1 0 0 0

· POINTS – 指定点云中点的总数。从0.7版本开始,该字段就有点多余了,因此有可能在将来的版本中将它移除。

    例如:

    POINTS 307200   #点云中点的总数为307200

· DATA – 指定存储点云数据的数据类型。从0.7版本开始,支持两种数据类型:ascii和二进制。

    注意:文件头最后一行(DATA)的下一个字节就被看成是点云的数据部分了,它会被解释为点云数据。

    根据这段文件头信息的解释,我们可以编写一段代码对生成的点云数据进行添加颜色的处理。

    使用文本编辑器打开cloud.pcd文件,文件头信息如下:

# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH 204186
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 204186
DATA ascii

这里面需要修改的只有第三、第四和第五行。

将第三行的信息修改为:FIELDS x y z rgb;

将第四行的信息修改为:SIZE 4 4 4 4

将第五行的信息修改为:TYPE F F F I

这些修改是为后续向数据添加颜色信息做准备。对于代码,首先我们要定义一个名为AddColorToPCDFile的函数,用来向生成后的pcd文件内添加颜色数据,首先先修改文件头信息并保存:

def AddColorToPCDFile(filename):
         with open(filename, 'rb') as f:
                   lines = f.readlines()
         lines[2] = lines[2].split('\n')[0] + ' rgb\n'
         lines[3] = lines[3].split('\n')[0] + ' 4\n'
         lines[4] = lines[4].split('\n')[0] + ' I\n'
         lines[5] = lines[5].split('\n')[0] + ' 1\n
         with open(filename, 'wb') as fw:
                   fw.writelines(lines)

之前我们提到过,rgb颜色信息被存在名为rgb的变量里,那么我们需要将每个像素的颜色信息分离出来,并将分离好的每个像素的颜色信息转换成十六进制格式后保存进一个列表中:

def ImageToPointCloud(RGBFilename, DepthFilename, CloudFilename, camera):
         rgb = cv2.imread( RGBFilename )
         depth = cv2.imread( DepthFilename, -1 )
         cloud = pcl.PointCloud()
         rows = len(depth)
         cols = len(depth[0])
         pointcloud = []
         colors = []
         for m in range(0, rows):
                   for n in range(0, cols):
                            d = depth[m][n]
                            if d == 0:
                                     pass
                            else:
                                     z = float(d) / camera.scale
                                     x = (n - camera.cx) * z / camera.fx
                                     y = (m - camera.cy) * z / camera.fy
                                     points = [x, y, z]
                                     pointcloud.append(points)
                                     b = rgb[m][n][0]
                                     g = rgb[m][n][1]
                                     r = rgb[m][n][2]
                                     color = (r << 16) | (g << 8) | b
                                     colors.append(int(color))
         pointcloud = np.array(pointcloud, dtype = np.float32)
         cloud.from_array(pointcloud)
         pcl.save(cloud, CloudFilename, format = 'pcd')

由于保存颜色信息的序列顺序和生成点云的序列顺序完全一样,那么我们就可以遍历cloud.pcd的data区域,逐个添加颜色信息:

def AddColorToPCDFile(filename, colors):
         with open(filename, 'rb') as f:
                   lines = f.readlines()
         lines[2] = lines[2].split('\n')[0] + ' rgb\n'
         lines[3] = lines[3].split('\n')[0] + ' 4\n'
         lines[4] = lines[4].split('\n')[0] + ' I\n'
         lines[5] = lines[5].split('\n')[0] + ' 1\n'
         for i in range(11, len(colors) + 11):
                   lines[i] = lines[i].split('\n')[0] + ' ' + str(colors[i - 11]) + '\n'
         with open(filename, 'wb') as fw:
                   fw.writelines(lines)

最后,在ImageToPointCloud函数结尾添加修改颜色的调用AddColorToPCDFile(CloudFilename, colors)即可。这样对生成的点云进行的添加颜色的优化就完成了。

参考资料:

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

[2] PCD(点云数据)文件格式 - 地址:http://www.pclcn.org/study/shownews.php?lang=cn&id=54 

[3] 高博博客:http://www.cnblogs.com/gaoxiang12/tag/%E4%B8%80%E8%B5%B7%E5%81%9ARGB-D%20SLAM/

[4] ROSClub链接: http://rosclub.cn/post-75.html



标签: 用python学习rgbd-slam系列slam自己挖坑自己填系列

评论共10条评论

登录后再评论!

全部评论

木下雄狮 2017年07月23日 19:49:12

很有帮助的文章 刚好我们也在做类似的项目 麻烦问下作者 无人机搭载这个激光雷达的话处理方法有什么区别吗

zhoutao133474 2017年07月11日 09:15:41

博士你好,我想站在学术的位面请教你一些问题,加我qq:1172+++++++++++++++++++++5423++++++++++++++04

咖啡 2017年06月30日 20:18:43

这个目录tree中,其他srv msg 是不是应该在src/package下

2017年06月30日 14:35:48

你好~这个代码链接失效了,可以重新发送吗,谢谢啦~~

dajianli 2017年06月11日 18:50:26

https://github.com/zsirui/slam-python.git 代码都在这里

Tolerance_Gui 2017年06月01日 10:20:30

挺好的课程,受益颇多! 但是下载资源的链接失效了,可以更新一下下载链接或发一份到邮箱,714096525@qq.com。 谢谢了

hades 2017年04月29日 18:21:21

很好很强大。。。

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

66666666

dajianli 2017年03月13日 17:03:55

作者可能还没有女朋友

静庵 2017年03月13日 17:03:21

666666666666666666