Installation using the python – photo call ICP algorithm | debug

Recently, it is necessary to use the iterative nearest point algorithm to calculate the transformation matrix TTT of two frames of two-dimensional point cloud data. The PCL library has its own ICP algorithm. Since all programs are written in Python, python-PCL is installed. An error was detected when installing Python-pcl on the Ubuntu 14.04 64-bit OS according to the official installation procedure. This system gives the installation steps, problems, and solutions. Finally, an example of ICP algorithm in Python-PCL is given to illustrate how to use it.

---

OS: Ubuntu 14.04 64bit

1. Install the PCL module

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

2. Install other dependent modules

$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 gitCopy the code

Git clone python-pcl

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

4. Compile python – photo

$ python setup.py build_ext -i
Copy the code

If the following error occurs:

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/python3 -I/usr/include/ni -I/usr/include/python2.7 -c PCL /_pcl_172. CPP -o Build/temp. Linux - x86_64-2.7 / photo / _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, The from/usr/local/lib/python2.7 / dist - packages/numpy/core/include/numpy/ndarrayobject. H: 18, The from/usr/local/lib/python2.7 / dist - packages/numpy/core/include/numpy/arrayobject. H: 4, from photo / _pcl_172 CPP: 526: / usr/local/lib/python2.7 / dist - packages/numpy/core/include/numpy/npy_1_7_deprecated_api. H: ": 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. Error: command 'x86_64-linux-gnu-gcc' failed with exit status 1n x86_64-linux-gnu-gcc -pthread -fno-strict-aliaCopy the code

Run the following command to update the installed PCL library:

$ sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl $ sudo apt-get update $ sudo apt-get upgrade Libpcl - the features - dev libpcl - IO - 1.7 libpcl - IO - 1.7 - devCopy the code

When the update is complete, clear the previous compilation and recompile:

$ sudo python setup.py clean
$ sudo make clean
$ python setup.py build_ext -i
Copy the code

5. Install python – photo

$ sudo python setup.py install
Copy the code

Test 6.

$ python
$ import pcl
Copy the code

If no error message is displayed, python-pcl is successfully installed.

A case in point

Mission Description: Mobile robots drive around the environment, Assumed in the indoor environment of the two points p1 = (x1, y1, theta 1) p_1 = (x_1, y_1, \ theta_1) p1 = (x1, y1, theta. 1) and (p2) = (x2, y2, theta. 2) p_2 = (x_2, y_2, \ theta_2) p2 = (x2, y2, theta. 2). The mobile robot is equipped with a single-line laser (e.g. Hokuyo UTM-30LX Laser). The mobile robot collected a frame of laser data R1m×1R_1^{m \times 1}R1m×1 and R2m×1R_2^{m \times 1}R2m×1 at p1p_1P1 and P2p_2P2 respectively, representing the distance values of obstacles in m directions. When the value of a direction is the maximum laser perceived distance, it indicates that no obstacle is perceived in the current direction. Now the relative displacement and steering of the robot from point P1p_1P1 to p2p_2P2 are calculated by ICP algorithm. To illustrate the effectiveness of ICP, the robot recorded the position and orientation of the real world coordinate system of two points. The relevant real pose and laser data are given as follows: P1 = (1.76395615838, 1.71348083379-0.0887520083811) p_1 = (1.76395615838, 1.71348083379, 0.0887520083811) p1 = (1.76395615838, 1.71348083379-0.0887520083811) p1 = p_1 (2.49526545944, 1.66333922345-0.0257761114877) P1 = (2.49526545944, 1.66333922345, 0.0257761114877) = (2.49526545944, 1.66333922345-0.0257761114877)

After calculation, the real relative pose can be obtained as follows: The delta true {delta x = 0.6216818163210198, the delta y = 0.10359883947861515, the delta theta = 0.06297589689340001} \ delta_ {true} \ {\ delta x = 0.6216818163210198, \delta y = 0.10359883947861515,,\delta \theta = 0.06297589689340001 \} the delta true {delta x = 0.6216818163210198, the delta y = 0.10359883947861515, the 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()
Copy the code

The relative pose obtained by ICP algorithm is δx= {δ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}

with
$\delta_{true}\{delta x = 0.6216818163210198, \delta y = 0.10359883947861515, \delta theta = 0.06297589689340001\}$Contrast. It’s still a little statistical. That’s it. It’s just a demo. The original two frames of data are displayed in their respective coordinate systems.After modifying the transformation matrix calculated by ICP algorithm, the basic overlap.

conclusion

This article shows you how to install the Python-pcl library for Ubuntu 14.04 and provides possible solutions to the problems that may arise during the process. Finally, the ICP algorithm in PCL library is used to test the effectiveness of the algorithm.