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

最近需要使用迭代最近点算法计算两帧二维点云数据的转换矩阵TTT，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安装成功。

一个例子

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

经计算，可得真实的相对位姿为：δtrue{δ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]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]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]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]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 inlinescan1 = 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))).TT = 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算法得到的相对位姿为δpredict={δ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}

与δtrue{δ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算法，测试了算法的效果。