Установите и используйте python-pcl для вызова алгоритма ICP|отладки

В последнее время необходимо использовать итеративный алгоритм ближайших точек для вычисления матрицы преобразования двух кадров двумерных данных облака точек. $T$ , в библиотеке PCL есть свой алгоритм ICP.Поскольку все программы написаны на питоне, установите python-pcl. в соответствии софициальныйПри установке python-pcl в 64-битной системе Ubuntu 14.04 была обнаружена ошибка. Эта система дает шаги установки, проблемы и решения. Наконец, для иллюстрации метода использования приведен пример использования алгоритма ICP в python-pcl.

Система: убунту 14.04 64бит

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 клонировать 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). мобильные роботы $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\}$ .

$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, равно $\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, она в основном перекрывается.

Эпилог

В этой статье ubuntu 14.04 дает возможность установить библиотеку python-pcl и предлагает возможные решения проблем, которые могут возникнуть на этапах. Наконец, с помощью алгоритма ICP в библиотеке PCL проверяется эффект алгоритма.