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

发布时间:2017年07月27日 17:27:31    浏览数:229次    来自:admin
上一篇我们已经成功的实现了一个只有两帧的简单slam程序,但是真正的slam程序是不会只有两帧画面,本篇将会使用更多的数据,同时制作一个简易的视觉里程计。

章节目录:

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

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

0x00 准备:

上一篇我们已经成功的实现了一个只有两帧的简单slam程序,但是真正的slam程序是不会只有两帧画面,本篇将会使用更多的数据,同时制作一个简易的视觉里程计。

本篇所使用的数据来自nyuv2数据集

http://cs.nyu.edu/~silberman/datasets/nyu_depth_v2.html)。完整数据一共700多张图片,在本篇中只使用其中的20张作为测试样例(图片越多,程序需要的时间越久),具体数据参见github(https://github.com/zsirui/slam-python)。

视觉里程计,简单来说,就是用新帧数据和上一帧数据对比匹配,通过匹配运算,估算摄像头的运动轨迹,然后一遍遍的重复将源源不断的新数据与老数据匹配计算得到的运动数据累加起来,得到的就是相机的连续运动。整个过程的示意图如下图所示:(图片来自:http://www.cnblogs.com/gaoxiang12/p/4719156.html

12.jpg

在上一篇中,我们已经将拼接点云的代码封装为了slamBase.py的自定义库,我们将使用这个自定义库来实现一个简易的Visual Odometry(视觉里程计)程序。

0x01 代码实现:

首先我们将上一篇中的joinPointCloud.py进行改写,增加部分常量定义:

#!/usr/bin/env python
# -*- coding: utf-8 -*-
 
import slamBase
import pcl
import readyaml
from numpy import array, float32, pi
from os import path
from cv2 import norm
 
RGBFileNamePath = './data/rgb_png'
DepthFileNamePath = './data/depth_png'
CalibrationDataFile = './calibration/camera.yml'
CloudFilename = './data/cloud.pcd'
# 点云分辨率
GRIDSIZE = 0.02
# 起始与终止索引
START_INDEX = 1
END_INDEX = 20
# 最小匹配数量
MIN_GOOD_MATCH = 10
# 最小内点
MIN_INLIERS = 5
# 最大运动误差
MAX_NORM = 0.3
 
C = array([[518.0,0,325.0],[0,519.0,253.5],[0,0,1]])

这里数据一共20组(初期测试可以选择较少数据,数据量越大,程序所需时间越长,本人在性能较好的计算机上700组数据依旧需要将近四小时半的时间),每组数据分别包含一张RGB图像和深度图像,分别保存在/data/rgb_png/data/depth_png路径下,相机标定文件存放在/calibration/路径下,文件名为camera.yml,(当使用自己数据的时候,这个文件才会用到)GRIDSIZE为滤波器使用的点云分辨率,START_INDEXEND_INDEX为数据索引起始,MIN_GOOD_MATCH为最小匹配数量,MIN_INLIERS为最小内点,MAX_NORM为最大运动误差。修改这些常量可以得到不同的点云结果,这里不做细致研究。定义normofTransform函数,用于度量运动的大小:

def normofTransform(rvec, tvec):
         return abs(min(norm(rvec), pi * 2 - norm(rvec))) + abs(norm(tvec))

定义joinPointCloud函数,将新帧数据和上一帧数据进行匹配计算并合并点云:

def joinPointCloud(pointCloud, color, RGBFileName, DepthFileName, lastFrame, CameraIntrinsicData, DistortionCoefficients, camera):
         currentFrame = slamBase.Frame(RGBFileName, DepthFileName)
         pnp = slamBase.SolvePnP(DistortionCoefficients, CameraIntrinsicData, camera, lastFrame, currentFrame)
 
         try:
                   len(pnp.inliers)
         except:
                   pass
         else:
                   if len(pnp.inliers) < MIN_INLIERS:
                            pass
                   else:
                            Norm = normofTransform(pnp.rvec, pnp.tvec)
                            print('norm = ' + str(Norm))
                            if Norm >= MAX_NORM:
                                     pass
                            else:
                                     p0, c0 = slamBase.imageToPointCloud(RGBFileName, DepthFileName, camera)
                                     colors = color + c0
                                     p = slamBase.transformPointCloud(p0, pnp.T)
                                     pointCloud = slamBase.addPointCloud(pointCloud.to_list(), p)
                                     voxel = pointCloud.make_voxel_grid_filter()
                                     voxel.set_leaf_size( GRIDSIZE, GRIDSIZE, GRIDSIZE )
                                     pointCloud = voxel.filter()
                                     lastFrame = currentFrame
                   return pointCloud, lastFrame, colors

这里需要注意一点,python版的pcl库的Voxel滤波器和C++版的代码有所不同,python版中,Voxel滤波器的初始化被内置到了PointCloud这个类中,通过PointCloud.make_voxel_grid_filter()实现滤波器初始化。部分方法名和C++代码完全不同,遇到不知道的地方可以在python命令行中输入help命令来查看(第三方库需要先用import引入后再用help命令查看)。

接下来是最关键的VisualOdometry实现:

def visualOdometry():
         CameraIntrinsicData, DistortionCoefficients = readyaml.parseYamlFile(CalibrationDataFile)
         DistortionCoefficients = array([0,0,0,0], dtype = float32)
         camera = slamBase.CameraIntrinsicParameters(C[0][2], C[1][2], C[0][0], C[1][1], 1000.0)
         frame = slamBase.Frame(path.join(RGBFileNamePath, str(START_INDEX) + '.png'), path.join(DepthFileNamePath, str(START_INDEX) + '.png'))
         p, colors = slamBase.imageToPointCloud(path.join(RGBFileNamePath, str(START_INDEX) + '.png'), path.join(DepthFileNamePath, str(START_INDEX) + '.png'), camera)
         pcd = pcl.PointCloud()
         pcd.from_array(array(p, dtype = float32))
         for i in range(START_INDEX, END_INDEX):
                   try:
                            pcd, frame, colors = joinPointCloud(pcd, colors, path.join(RGBFileNamePath, str(i + 1) + '.png'), path.join(DepthFileNamePath, str(i + 1) + '.png'), frame, C, DistortionCoefficients, camera)
                   except:
                            pass
                  
         pcl.save(pcd, CloudFilename, format = 'pcd')

这样简单的一个VisualOdometry(视觉里程计)程序就基本实现了。然而,查看了点云后就会发现,点云并没有颜色。这是pythonpcl接口的问题,pythonpcl中对PointCloud封装的时候,并没有将颜色(RGB)封装进去,这样通过PointCloud产生的点云,点云的每个点仅仅只有x, y, z坐标,再通过Voxel滤波器滤波后,更无法将点云的颜色添加回去。考虑到后续代码所用到的g2o库并没有python接口,所以打算抛弃python版的pcl接口,自行编写一个C++代码,并为python代码留出相应接口以便调用。然而由于作者本人知识所限,暂时无法对pcl库进行大面积的接口改造。而后续的g2o库扩展接口更为复杂,故后续不再介绍使用g2o库对视觉里程计的优化。

 

目前C++python的混合编程一般有以下几种方式:python自带的ctypes库,需要对C++代码编写C风格接口;SWIG,一个可以分析代码并自动生成python接口的库,需要自行编写特殊的解释文档;Boost.PythonBoost库中的一套用于C++python交互的接口,需要针对C++源码编写一段接口转换代码。现在简单介绍一下使用Boost.Python库对pcl库进行接口改造,首先是头文件和命名空间及类型定义:

#include <iostream>
 
//PCL
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/PCLHeader.h>
#include <pcl/common/transforms.h>
 
//boost
#include <boost/python.hpp>
#include <boost/python/list.hpp>
 
//Eigen
#include <Eigen/StdVector>
#include <Eigen/Geometry>
 
using namespace std;
using namespace boost::python;
 
// 类型定义
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef boost::shared_ptr < pcl::PointXYZRGBA > PointXYZRGBA_ptr;
typedef boost::shared_ptr < PointCloud::Ptr > PointCloud_ptr;
typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > VectorType;

Boost库中的Python模块可以自动将C++的数据类型转换成Python可识别的类型,不过需要编写一些简单的转换语句。例如现在我们要将PointCloud这个类进行接口转换: 

BOOST_PYTHON_MODULE(lib_pcl)
{
         register_ptr_to_python <PointXYZRGBA_ptr>();
         register_ptr_to_python <PointCloud_ptr>();
         def("showList", showList);
class_<pcl::PCLHeader>("PCLHeader")
                   .def_readwrite("seq", &pcl::PCLHeader::seq)
                   .def_readwrite("stamp", &pcl::PCLHeader::stamp)
                   .def_readwrite("frame_id", &pcl::PCLHeader::frame_id);
 
         class_<pcl::PointXYZRGBA>("PointXYZRGBA")
                   .def_readwrite("x", &pcl::PointXYZRGBA::x)
                   .def_readwrite("y", &pcl::PointXYZRGBA::y)
                   .def_readwrite("z", &pcl::PointXYZRGBA::z)
                   .def_readwrite("r", &pcl::PointXYZRGBA::r)
                   .def_readwrite("g", &pcl::PointXYZRGBA::g)
                   .def_readwrite("b", &pcl::PointXYZRGBA::b)
                   .def_readwrite("a", &pcl::PointXYZRGBA::a)
                   .def_readwrite("rgba", &pcl::PointXYZRGBA::rgba);
 
         class_<VectorType>("VectorType", init<>());
 
         class_<PointCloud>("PointCloud", init<>())
                   .def_readwrite("width", &PointCloud::width)
                   .def_readwrite("height", &PointCloud::height)
                   .def_readwrite("is_dense", &PointCloud::is_dense)
                   .def_readwrite("header", &PointCloud::header)
                   .def_readwrite("points", &PointCloud::points)
                   .def(self + PointCloud())
                   .def("size", &PointCloud::size)
                   .def("Ptr", &PointCloud::makeShared)
                   .def("push_back", &PointCloud::push_back)
                   .def("clear", &PointCloud::clear)
                   .def("resize", &PointCloud::resize);
}

通过查询pcl源码,我们可以得到PointCloud类的详细定义,这里我们只需要将源码中public中公开的变量和方法接口通过Boost.Python进行转换即可。class_<T>()就是将C++的类/结构体转换为Python的类的方法。这里,T对应的是类型。.def_readwrite用来向Python类中添加Public变量,.def_readonly用来添加Private变量,.def用来添加类中的方法。

BOOST_PYTHON_MODULE对应自定义的Python模块的名字,这里我们定义模块名为lib_pclshowList是一个测试函数,传递的参数是一个Pythonlist,通过toPointXYZRGBA函数,可以将对应的数据在C++中存为pcl:: PointXYZRGBA类型并打印输出,showList的函数内容如下:

void showList(const boost::python::list& pyList)
{
         PointT p;
         p = toPointXYZRGBA(boost::python::extract<long double>(pyList[0]), boost::python::extract<long double>(pyList[1]), boost::python::extract<long double>(pyList[2]), boost::python::extract<uint8_t>(pyList[3]), boost::python::extract<uint8_t>(pyList[4]), boost::python::extract<uint8_t>(pyList[5]));
         cout<<"p.x "<<p.x<<endl;
         cout<<"p.y "<<p.y<<endl;
         cout<<"p.z "<<p.z<<endl;
         cout<<"p.rgba "<<p.rgba<<endl;
}
 
PointT toPointXYZRGBA(long double x, long double y, long double z, uint8_t r, uint8_t g, uint8_t b)
{
         PointT p;
         p.x = x;
         p.y = y;
         p.z = z;
         p.r = r;
         p.g = g;
         p.b = b;
         return p;
}

最后将代码保存为lib_pcl.cpp(文件名一定要和代码中的模块名一致),在命令行中输入以下命令:

g++ -c -f PIC lib_pcl.cpp -o lib_pcl.o
g++ -shared -Wl,-soname,lib_pcl.so -o lib_pcl.so  lib_pcl.o -lpython2.7 -lboost_python -lboost_system

最后会在目录中生成lib_pcl.so文件,在python命令行中输入import lib_pcl就可以将编写好的C++库引入到python中了:

Python 2.7.6 (default, Oct 26 2016, 20:30:19)
[GCC 4.8.4] on linux2
Type "help", "copyright", "credits" or "license" for more information.
>>> import lib_pcl
>>> PointT = lib_pcl.PointXYZRGBA()
>>> PointT.x = 1
>>> PointT.y = 2
>>> PointT.z = 3
>>> PointT.r = 128
>>> PointT.g = 255
>>> PointT.b = 127
>>> PointT.rgba
4286644095
>>> PointT.a
255
>>> lib_pcl.showList([1,2,3,128,255,127])
p.x 1
p.y 2
p.z 3
p.rgba 4286644095



标签: ROSClubslam

评论共3条评论

登录后再评论!

全部评论

zhangrelay 2017年08月07日 22:25:42

赞~

aga168 2017年07月30日 15:11:06

ppt和视频网盘地址链接失效了

aga168 2017年07月30日 15:08:05

ppt和视频下载地址链接都失效了,有资料的小伙伴请发一份致aga168@qq.com邮箱