安装使用python-pcl调用ICP算法|debug

windSeS

已于 2022-05-25 11:09:34 修改

阅读量6.1k

点赞数 7

分类专栏： coding 文章标签： python-pcl pcl icp

于 2019-04-10 14:10:16 首次发布

本文为博主原创文章，未经博主允许不得转载。

本文链接：https://blog.csdn.net/u013468614/article/details/89181871

版权

coding 专栏收录该内容

26 篇文章 13 订阅

订阅专栏

安装使用python-pcl调用ICP算法|debug

最近需要使用迭代最近点算法计算两帧二维点云数据的转换矩阵 $T$ ，PCL库中自带ICP算法，由于当程序都是用python编写，所以安装python-pcl。按照官方的安装步骤，在ubuntu 14.04 64bit系统上安装python-pcl时，发现有错误。本篇系统的给出安装步骤、出现的问题、以及解决方法。最后，给出python-pcl中，ICP算法使用的例子，说明使用方法。

系统：ubuntu 14.04 64bit

1.安装PCL模块

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

2.安装其他依赖模块

$ sudo apt-get install python-pip
$ sudo apt-get install python-dev
$ sudo pip install Cython==0.25.2
$ sudo pip install numpy
$ sudo apt-get install git

3.git clone python-pcl到本地

$ git clone https://github.com/strawlab/python-pcl.git
$ cd python-pcl/

4. 编译python-pcl

$ python setup.py build_ext -i

若出现以下错误：

感谢网友提供的解决方案build error with ubuntu14.04 。

running build_ext
skipping 'pcl/_pcl_172.cpp' Cython extension (up-to-date)
building 'pcl._pcl' extension
x86_64-linux-gnu-gcc -pthread -fno-strict-aliasing -DNDEBUG -g -fwrapv -O2 -Wall -Wstrict-prototypes -fPIC -DEIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET=1 -I/usr/local/lib/python2.7/dist-packages/numpy/core/include -I/usr/include/pcl-1.7 -I/usr/include/eigen3 -I/usr/include/ni -I/usr/include/python2.7 -c pcl/_pcl_172.cpp -o build/temp.linux-x86_64-2.7/pcl/_pcl_172.o
cc1plus: warning: command line option ‘-Wstrict-prototypes’ is valid for C/ObjC but not for C++ [enabled by default]
In file included from /usr/local/lib/python2.7/dist-packages/numpy/core/include/numpy/ndarraytypes.h:1809:0,
                 from /usr/local/lib/python2.7/dist-packages/numpy/core/include/numpy/ndarrayobject.h:18,
                 from /usr/local/lib/python2.7/dist-packages/numpy/core/include/numpy/arrayobject.h:4,
                 from pcl/_pcl_172.cpp:526:
/usr/local/lib/python2.7/dist-packages/numpy/core/include/numpy/npy_1_7_deprecated_api.h:15:2: warning: #warning "Using deprecated NumPy API, disable it by " "#defining NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION" [-Wcpp]
 #warning "Using deprecated NumPy API, disable it by " \
  ^
pcl/_pcl_172.cpp:618:31: fatal error: pcl/features/cppf.h: 没有那个文件或目录
 #include "pcl/features/cppf.h"
                               ^
compilation terminated.
error: command 'x86_64-linux-gnu-gcc' failed with exit status 1n x86_64-linux-gnu-gcc -pthread -fno-strict-alia

则运行以下命令对安装的PCL库进行更新：

$ sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl
$ sudo apt-get update
$ sudo apt-get upgrade libpcl-features-dev libpcl-io-1.7 libpcl-io-1.7-dev

更新完成后，清除之前的编译，并重新编译：

$ sudo python setup.py clean
$ sudo make clean
$ python setup.py build_ext -i

5.安装python-pcl

$ sudo python setup.py install

6.测试

$ python
$ import pcl

如果没有报错，说明python-pcl安装成功。

一个例子

任务描述：移动机器人在环境中行驶，假定在室内环境的两个位置点 $p_1=(x_1, y_1, \theta_1)$ 与 $p_2=(x_2, y_2, \theta_2)$ 。移动机器人装载一个单线激光(例如：hokuyo UTM-30LX laser)。移动机器人分别在 $p_1$ 与 $p_2$ 采了一帧激光数据 $R_1^{m \times 1}$ 与 $R_2^{m \times 1}$ ，为m维的向量,表示m个方向上的障碍物距离值，当某向的值为激光感知最大距离时，表明当前方向没有感知到障碍物。现在通过ICP算法计算，机器人从点 $p_1$ 到 $p_2$ 的相对位移与转向。为了说明ICP的有效性，机器人记录了两点的真实世界坐标系的位置与朝向角度。相关真实位姿与激光数据给出如下：
$p_1 = (1.76395615838,1.71348083379, -0.0887520083811)$
$p_1 = (2.49526545944,1.66333922345,-0.0257761114877)$

经计算，可得真实的相对位姿为： $\delta_{true}\{\delta x = 0.6216818163210198, \delta y = 0.10359883947861515,,\delta \theta = 0.06297589689340001\}$ 。

import numpy as np
import time
import pcl
import copy
import matplotlib.pyplot as plt
import cPickle as pickle
from scipy.spatial import cKDTree
%matplotlib inline

scan1 = np.loadtxt("datas/scan1.txt")
scan_num1 = len(scan1)
thetas1 = np.linspace(-np.pi, np.pi, scan_num1)

indexs1 = [i for i in range(len(scan1)) if scan1[i] < 5.0]

X1 = scan1[indexs1]*np.cos(thetas1[indexs1])
Y1 = scan1[indexs1]*np.sin(thetas1[indexs1])
Z1 = np.zeros(len(X1))

D1 = np.column_stack((X1, Y1, Z1))

D1 = np.array(D1, dtype=np.float32)

p1 = pcl.PointCloud(D1)

scan2 = np.loadtxt("datas/scan2.txt")
scan_num2 = len(scan2)
thetas2 = np.linspace(-np.pi, np.pi, scan_num2)

indexs2 = [i for i in range(len(scan2)) if scan2[i] < 5.0]

X2 = scan2[indexs2]*np.cos(thetas2[indexs2])
Y2 = scan2[indexs2]*np.sin(thetas2[indexs2])
Z2 = np.zeros(len(X2))

D2 = np.column_stack((X2, Y2, Z2))

D2 = np.array(D2, dtype=np.float32)

p2 = pcl.PointCloud(D2)

import time
icp = pcl.IterativeClosestPoint()
T = icp.icp(p2, p1)
from matplotlib import pyplot as plt
plt.plot(D1[:,0], D1[:,1], 'b*')
plt.plot(D2[:,0], D2[:,1], 'ro')
plt.show()

Ones = np.ones(len(D2))
new_D2 = np.matrix(np.column_stack((X2, Y2, Z2, Ones))).T

T = np.matrix(T[1])

new_p2 = np.dot(T, new_D2)[0:3]
plt.plot(D1[:,0], D1[:,1], 'b*')
plt.plot(new_p2[0,:], new_p2[1,:], 'r*')
plt.show()

由ICP算法得到的相对位姿为 $\delta_{predict}=\{\delta x = 0.65074205, \delta y = 0.01502177 , \delta \theta = 0.00580376123744\}$

与 $\delta_{true}\{\delta x = 0.6216818163210198, \delta y = 0.10359883947861515,,\delta \theta = 0.06297589689340001\}$ 对比。数据上看还是有点不足。也就这样了，只是一个演示例子。
原始两帧数据，在各自的坐标系下显示。
在这里插入图片描述
经ICP算法计算的转换矩阵修正后，基本重迭。