安装使用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)$

经计算，可得真实的相对位姿为： δ t r u e { δ x = 0.6216818163210198 , δ y = 0.10359883947861515 , , δ θ = 0.06297589689340001 } \delta_{true}\{\delta x = 0.6216818163210198, \delta y = 0.10359883947861515,,\delta \theta = 0.06297589689340001\} δtrue​{δx=0.6216818163210198,δy=0.10359883947861515,,δθ=0.06297589689340001}。

$$scan1=[1.23,1.23,1.23,1.23,1.23,1.22,1.22,1.22,1.22,1.22,1.22,1.22,1.22,1.22,1.23,1.23,1.23,1.23,1.23,1.23,1.23,1.23,1.23,1.24,1.24,1.24,1.24,1.24,1.25,1.25,1.25,1.25,1.26,1.26,1.26,1.26,1.27,1.27,1.28,1.28,1.28,1.29,1.29,1.3,1.3,1.3,1.31,1.31,1.3,1.32,1.33,1.34,1.34,1.35,1.35,1.36,1.37,1.37,1.38,1.39,1.4,1.4,1.41,1.42,1.43,1.44,1.44,1.45,1.46,1.47,1.48,1.49,1.5,1.51,1.52,1.54,1.55,1.56,1.57,1.58,1.6,1.61,1.62,1.64,1.63,1.61,1.6,1.58,1.58,1.57,1.56,1.54,1.53,1.51,1.5,1.49,1.48,1.46,1.45,1.44,1.43,1.42,1.41,1.4,1.39,1.38,1.37,1.36,1.35,1.34,1.33,1.32,1.31,1.3,1.3,1.29,1.28,1.28,1.27,1.26,1.26,1.25,1.24,1.24,1.22,1.23,1.22,1.21,1.21,1.2,1.2,1.19,1.19,1.19,1.18,1.18,1.17,1.17,1.17,1.16,1.16,1.16,1.15,1.15,1.15,1.14,1.14,1.14,1.14,1.13,1.13,1.13,1.13,1.13,1.13,1.12,1.12,1.12,1.12,1.12,1.12,1.12,1.12,1.12,1.11,1.11,1.11,1.11,1.11,1.11,1.11,1.11,1.11,1.11,1.11,1.12,1.12,1.12,1.12,1.12,1.12,1.12,1.12,1.12,1.11,1.13,1.13,1.13,1.13,1.14,1.14,1.14,1.14,1.15,1.15,1.15,1.15,1.16,1.16,1.16,1.17,1.17,1.18,1.18,1.18,1.19,1.19,1.2,1.2,1.21,1.21,1.22,1.22,1.23,1.23,1.24,1.25,1.25,1.26,1.26,1.27,1.28,1.29,1.29,1.3,1.31,1.32,1.32,1.33,1.34,1.35,1.36,1.37,1.38,1.39,1.4,1.41,1.42,1.43,1.44,1.46,1.47,1.48,1.49,1.51,1.52,1.54,1.55,1.56,1.58,1.59,1.6,1.61,1.63,1.64,1.65,1.7,1.71,1.73,1.75,1.76,1.78,1.8,1.82,1.87,1.9,1.92,1.94,1.97,1.99,2.02,2.07,2.09,2.12,2.17,2.2,2.24,2.27,2.31,2.37,2.41,2.45,2.5,2.54,2.61,2.67,2.72,2.78,2.84,2.92,2.94,3.06,3.13,3.23,3.32,3.4,3.52,3.62,3.73,3.85,3.99,4.13,4.27,4.45,4.62,4.82,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,4.84,4.83,4.83,4.84,4.85,4.85,4.86,4.87,4.88,4.89,4.89,4.9,4.92,4.93,4.94,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,2.03,1.81,1.76,1.71,1.67,1.64,1.59,1.55,1.52,1.5,1.47,1.42,1.4,1.4,1.4,1.4,1.43,1.4,1.38,1.36,1.32,1.3,1.28,1.26,1.24,1.25,1.25,1.26,1.24,1.22,1.21,1.19,1.2,1.21,1.21,1.22,1.23,1.21,1.19,1.17,1.15,1.16,1.17,1.17,1.18,1.17,1.16,1.15,1.14,1.13,1.12,1.11,1.1,1.12,1.13,1.15,1.14,1.13,1.12,1.12,1.13,1.15,1.16,1.15,1.14,1.14,1.13,1.15,1.17,1.17,1.17,1.16,1.16,1.15,1.17,1.17,1.16,1.16,1.18,1.2,1.22,1.26,1.26,1.23,1.24,1.24,1.24,1.28,1.3,1.33,1.31,1.31,1.33,1.36,1.41,1.46,1.51,1.56,1.61,4.23,4.22,4.21,4.2,4.19,4.18,4.17,4.16,4.15,4.15,4.14,4.13,4.13,4.12,4.12,4.35,4.75,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,4.85,4.67,4.45,4.37,4.22,4.08,3.95,3.85,3.73,3.62,3.52,3.44,3.34,3.27,3.14,3.12,3.05,2.98,2.92,2.85,2.8,2.73,2.69,2.63,2.57,2.53,2.49,2.46,2.4,2.37,2.34,2.3,2.25,2.22,2.2,2.15,2.13,2.1,2.07,2.05,2.01,1.98,1.97,1.93,1.91,1.9,1.88,1.84,1.83,1.82,1.8,1.76,1.75,1.74,1.73,1.72,1.7,1.68,1.67,1.65,1.64,1.63,1.61,1.6,1.59,1.57,1.56,1.55,1.54,1.53,1.51,1.5,1.49,1.48,1.47,1.46,1.45,1.45,1.44,1.43,1.42,1.41,1.4,1.4,1.39,1.38,1.37,1.37,1.36,1.35,1.35,1.34,1.34,1.33,1.33,1.32,1.31,1.31,1.3,1.3,1.3,1.29,1.29,1.28,1.28,1.28,1.27,1.27,1.27,1.26,1.26,1.26,1.25,1.25,1.25,1.25,1.24,1.24,1.24,1.24,1.24,1.23,1.23,1.23,1.23,1.23]$$

$scan2=[1.96,1.96,1.96,1.96,1.96,1.96,1.96,1.96,1.96,1.96,1.96,1.96,1.96,1.97,1.97,1.97,1.97,1.97,1.98,1.98,1.98,1.99,1.99,2.0,2.0,2.0,2.01,2.01,2.02,2.02,2.03,2.03,2.04,2.05,2.05,2.06,2.07,2.07,2.08,2.09,2.1,2.1,2.11,2.12,2.13,2.14,2.15,2.16,2.17,2.18,2.19,2.2,2.21,2.2,2.17,2.13,2.11,2.08,2.04,2.0,1.96,1.95,1.92,1.89,1.86,1.83,1.82,1.78,1.77,1.74,1.71,1.7,1.66,1.63,1.63,1.62,1.59,1.58,1.55,1.54,1.54,1.51,1.51,1.5,1.48,1.47,1.45,1.44,1.43,1.42,1.4,1.39,1.38,1.37,1.36,1.35,1.34,1.33,1.32,1.31,1.3,1.29,1.28,1.27,1.27,1.26,1.25,1.24,1.24,1.23,1.22,1.22,1.21,1.2,1.2,1.19,1.18,1.18,1.17,1.17,1.16,1.16,1.15,1.15,1.14,1.14,1.14,1.13,1.13,1.12,1.12,1.12,1.11,1.11,1.11,1.1,1.1,1.1,1.09,1.09,1.09,1.09,1.09,1.08,1.08,1.08,1.08,1.08,1.07,1.07,1.07,1.07,1.07,1.07,1.07,1.07,1.07,1.06,1.06,1.06,1.06,1.06,1.06,1.06,1.06,1.06,1.06,1.06,1.06,1.07,1.07,1.07,1.07,1.07,1.07,1.07,1.07,1.07,1.08,1.08,1.08,1.08,1.08,1.09,1.09,1.09,1.09,1.1,1.1,1.1,1.1,1.11,1.11,1.11,1.12,1.12,1.12,1.13,1.13,1.14,1.14,1.14,1.15,1.15,1.16,1.16,1.17,1.17,1.18,1.19,1.19,1.2,1.2,1.21,1.22,1.22,1.23,1.24,1.24,1.25,1.26,1.27,1.28,1.28,1.29,1.3,1.31,1.32,1.33,1.34,1.35,1.36,1.37,1.38,1.4,1.41,1.42,1.43,1.44,1.46,1.47,1.49,1.5,1.51,1.51,1.54,1.55,1.55,1.58,1.59,1.62,1.63,1.66,1.67,1.7,1.71,1.71,1.78,1.78,1.82,1.83,1.87,1.9,1.91,1.95,1.99,2.0,2.04,2.08,2.12,2.14,2.2,2.22,2.26,2.31,2.36,2.41,2.46,2.51,2.57,2.63,2.67,2.74,2.81,2.84,2.95,3.04,3.12,3.2,3.29,3.39,3.42,3.62,3.74,3.86,4.01,4.14,4.31,4.5,4.68,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,4.68,4.26,4.1,4.11,4.11,4.12,4.13,4.14,4.14,4.15,4.16,4.17,4.18,4.19,4.2,4.21,4.22,4.23,4.25,4.26,4.27,4.29,4.3,4.32,4.33,4.35,4.36,4.38,4.4,4.42,4.44,4.45,4.47,4.5,4.52,4.54,4.56,4.58,4.61,4.63,4.66,4.68,1.72,1.49,1.48,1.37,1.35,1.34,1.32,1.19,1.17,1.15,1.14,1.12,1.1,1.09,1.09,1.08,1.06,1.04,1.03,1.01,1.02,1.0,0.98,0.96,0.97,0.95,0.96,0.95,0.94,0.93,0.93,0.92,0.91,0.9,0.89,0.89,0.88,0.87,0.87,0.86,0.85,0.85,0.87,0.88,0.9,0.9,0.89,0.88,0.88,0.87,0.87,0.86,0.86,0.85,0.85,0.84,0.84,0.83,0.83,0.83,0.82,0.82,0.81,0.81,0.81,0.8,0.82,0.84,0.84,0.83,0.83,0.83,0.83,0.82,0.82,0.82,0.81,0.81,0.81,0.81,0.85,0.87,0.86,0.86,0.86,0.86,0.86,0.85,0.85,0.85,0.85,0.85,0.85,0.89,0.89,0.89,0.88,0.88,0.88,0.88,0.88,0.88,0.88,0.88,0.88,0.94,0.94,0.94,0.94,0.94,0.94,0.94,0.98,0.98,0.98,0.98,1.05,1.05,1.05,1.05,1.05,1.09,1.09,1.1,1.2,1.2,1.2,1.21,1.31,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,4.83,4.69,4.62,4.52,4.45,4.35,4.28,4.19,4.11,4.03,3.97,3.91,3.83,3.77,3.72,3.66,3.61,3.51,3.48,3.43,3.38,3.36,3.29,3.27,3.22,3.18,3.13,3.09,3.05,3.03,2.99,2.95,2.91,2.9,2.86,2.82,2.81,2.77,2.76,2.73,2.71,2.68,2.66,2.63,2.61,2.59,2.57,2.54,2.52,2.5,2.48,2.47,2.45,2.43,2.41,2.4,2.38,2.36,2.35,2.33,2.32,2.3,2.29,2.28,2.26,2.25,2.24,2.23,2.21,2.2,2.19,2.18,2.15,2.16,2.15,2.14,2.13,2.12,2.11,2.11,2.1,2.09,2.08,2.07,2.07,2.06,2.05,2.05,2.04,2.04,2.03,2.02,2.02,2.01,2.01,2.0,2.0,2.0,1.99,1.99,1.98,1.98,1.98,1.98,1.97,1.97,1.97,1.97,1.96,1.96,1.96,1.96,1.96,1.96,1.96,1.96]$$

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算法得到的相对位姿为 δ p r e d i c t = { δ x = 0.65074205 , δ y = 0.01502177 , δ θ = 0.00580376123744 } \delta_{predict}=\{\delta x = 0.65074205, \delta y = 0.01502177 , \delta \theta = 0.00580376123744\} δpredict​={δx=0.65074205,δy=0.01502177,δθ=0.00580376123744}

与 δ t r u e { δ x = 0.6216818163210198 , δ y = 0.10359883947861515 , , δ θ = 0.06297589689340001 } \delta_{true}\{\delta x = 0.6216818163210198, \delta y = 0.10359883947861515,,\delta \theta = 0.06297589689340001\} δtrue​{δx=0.6216818163210198,δy=0.10359883947861515,,δθ=0.06297589689340001}对比。数据上看还是有点不足。也就这样了，只是一个演示例子。
原始两帧数据，在各自的坐标系下显示。

经ICP算法计算的转换矩阵修正后，基本重迭。

结语

本文给出ubuntu 14.04安装python-pcl库，以及对步骤中可能出现的问题，提供了可行的解决方法。最后，利用PCL库中的ICP算法，测试了算法的效果。