Python-PCL point cloud stitching (visual SLAM fifth lecture case)

Recently, I was reading "Visual SLAM Fourteen Lectures" by Gao Xiang, which has C++ point cloud stitching. After reading it, I also want to use Python to implement it.

environment

So I downloaded python-pcl , and the process of installing this library was quite frustrating. Finally, the installation was successful according to the instructions in this issue
. The files of this library are relatively simple, and many things still need to query PCL.
For specific C++, The python code and the RGB picture and depth map used in the case are all on GitHub , you can refer to it.

Package: python3-pcl
Version: 0.3.0~rc1+dfsg-7~ubuntu18.04.2
Priority: optional
Section: python
Source: python-pcl
Maintainer: Debian Python Modules Team <[email protected]>
Installed-Size: 2,193 kB
Provides: python3.6-pcl
Depends: cython3, python3 (<< 3.7), python3 (>= 3.6~), python3-filelock, python3-nose, python3-numpy, python3:any (>= 3.3.2-2~), libboost-system1.65.1, libc6 (>= 2.14), libflann1.9, libgcc1 (>= 1:3.0), libpcl-common1.8, libpcl-features1.8, libpcl-filters1.8, libpcl-io1.8, libpcl-kdtree1.8, libpcl-octree1.8, libpcl-sample-consensus1.8, libpcl-search1.8, libpcl-segmentation1.8, libpcl-surface1.8, libpcl-visualization1.8, libstdc++6 (>= 5.2)
Python-Version: 3.6
Download-Size: 463 kB
APT-Manual-Installed: yes
APT-Sources: http://ppa.launchpad.net/sweptlaser/python3-pcl/ubuntu bionic/main amd64 Packages
Description: Python 3 binding to the Pointcloud library (PCL)
 The following parts of the API are wrapped (all methods operate on PointXYZ)
 point types:
 - I/O and integration; saving and loading PCD files
 - segmentation
 - SAC
 - smoothing
 - filtering
 - registration (ICP, GICP, ICP_NL)
 .
 This package installs the library for Python 3.

Then there isopencv-python (3.4.1.15)

Quaternion

Use Python to write this because I didn’t find a ready-made library similar to eigen3, so I wrote a quaternion to convert from a quaternion to a rotation matrix. By the way, the translation of the translation is also written in it. The quaternion
inside The rotation matrix of digital conversion was initially based on the formula above in "Visual SLAM Fourteen Lectures", but later found that it was a bit problematic to use, because our numpy array is represented by column vectors, so the matrix needs to be transposed, otherwise the subsequent points Orientation will be wrong.

import numpy as np

class Quaternion:
    def __init__(self, w, x, y, z):
        '''
        @param: w, x, y, z
        '''
        self.w = w
        self.x = x
        self.y = y
        self.z = z
        self.vector = np.array([w, x, y, z])

    def __str__(self):
        imaginary = [' ', 'i ', 'j ', 'k]']
        result = '['
        for i in range(4):
            result = result + str(self.vector[i]) + imaginary[i]
        return result

    def __add__(self, quater):
        q = self.vector + quater.vector
        return Quaternion(q[0], q[1], q[2], q[3])

    def transformToRotationMatrix(self):
        q = self.vector.copy()
        # w, x, y, z = q[0], q[1], q[2], q[3]
        # the rotation matrix below was transposed
        r = np.array([
            [1 - 2*q[2]*q[2] - 2*q[3]*q[3], 2*q[1]*q[2] + 2*q[0]*q[3], 2*q[1]*q[3]-2*q[0]*q[2],   0],
            [2*q[1]*q[2] - 2*q[0]*q[3], 1 - 2*q[1]*q[1] - 2*q[3]*q[3], 2*q[2]*q[3] + 2*q[0]*q[1], 0],
            [2*q[1]*q[3] + 2*q[0]*q[2], 2*q[2]*q[3] - 2*q[0]*q[1], 1 - 2*q[1]*q[1] - 2*q[2]*q[2], 0],
            [0,     0,      0,      1]
        ])
       
        return r.transpose()

def translation(position):
    '''
    @param: position: a 1x3 numpy array, indicates the location of a point (x,y,z)
    '''
    x, y, z = position[0], position[1], position[2]

    T = np.array([
        [1, 0, 0, x],
        [0, 1, 0, y],
        [0, 0, 1, z],
        [0, 0, 0, 1]
    ])
    return T

Point cloud stitching

python-pcl PointCloud_PointXYZRGBhas several methods:

from_array(self, ndarray arr)
Fill this object from a 2D numpy array (float32)

from_file(self, char *f)
Fill this pointcloud from a file (a local path). Only pcd files supported currently.

from_list(self, _list)
Fill this pointcloud from a list of 3-tuples

Here we use it from_list, or convert it into numpy array and use from_arrayit.
The representation of each point is [X, Y, Z, RGB] in float32 format, and RGB is a whole integer. At first I thought it was a simple three Put together 255, and later found something wrong. So I read the description of PCL::PointXYZRGB

A point structure representing Euclidean xyz coordinates, and the RGB color.
Due to historical reasons (PCL was first developed as a ROS package), the RGB information is packed into an integer and casted to a float. This is something we wish to remove in the near future, but in the meantime, the following code snippet should help you pack and unpack RGB colors in your PointXYZRGB structure:

// pack r/g/b into rgb
std::uint8_t r = 255, g = 0, b = 0;    // Example: Red color
std::uint32_t rgb = ((std::uint32_t)r << 16 | (std::uint32_t)g << 8 | (std::uint32_t)b);
p.rgb = *reinterpret_cast<float*>(&rgb);

It can be seen that RGB is an integer encoded by three integers of rgb after bit operation

import cv2
import pcl
import pcl.pcl_visualization
import numpy as np
import quaternion

# camera intrinsics
cx = 325.5
cy = 253.5
fx = 518.0
fy = 519.0
depthScale = 1000.0

colorImgs, depthImgs = [], []

# read pose.txt
pose = []
with open('pose.txt', 'r') as f:
    for line in f.readlines():
        line = line.replace('\n', '')  # remove returns
        line = line.split(' ')  # split into 7 items
        vector = []
        for num in line:
            vector.append(float(num))
        vector = np.array(vector)
    # compute Rotation matrix based on quaternion
        quater = quaternion.Quaternion(vector[6], vector[3], vector[4], vector[5])
        R = quater.transformToRotationMatrix()
    # translation matrix
        position = np.array([vector[0], vector[1], vector[2]])  #x, y, z
        trans = quaternion.translation(position)
    # pose matrix
        # T =  trans * R  # wrong multiplication, should use dot
        
        T = np.dot(trans, R)
        pose.append(T)

# read color and depth images
view = []
for i in range(1,6):
    colorImg = cv2.imread(f"color/{i}.png")
    # cv2.imshow(f"Image{i}", colorImg)
    # cv2.waitKey(0)
    # cv2.destroyAllWindows()

    depthImg = cv2.imread(f"depth/{i}.pgm", cv2.IMREAD_UNCHANGED)

    height, width, channel  = colorImg.shape
    for v in range(height):
        for u in range(width):
        # get RBG value
            b = colorImg.item(v, u, 0)
            g = colorImg.item(v, u, 1)
            r = colorImg.item(v, u, 2)
            # pack RGB into PointXYZRGB structure, refer to PCL
            # http://pointclouds.org/documentation/structpcl_1_1_point_x_y_z_r_g_b.html#details
            # rgb = int(str(r)+str(g)+str(b))
            rgb = r << 16 | g << 8 | b

            depth = depthImg[v,u]
            if depth == 0:
                continue
            z = depth / depthScale
            x = (u - cx) * z/fx
            y = (v - cy) * z/fy
            # using homogeneous coordinate
            point = np.array([x, y, z, 1])
            point_world = np.dot(pose[i-1], point)
            # x,y,z,rgb
            # scene = np.insert(point_world, 3, rgb)
            scene = np.array([point_world[0], point_world[1], point_world[2], rgb], dtype=np.float32)
            view.append(scene)

colorCloud = pcl.PointCloud_PointXYZRGB()
colorCloud.from_list(view)
visual = pcl.pcl_visualization.CloudViewing()
visual.ShowColorCloud(colorCloud, b"cloud")
v = True
while v:
    v = not(visual.WasStopped())

The final result is this, which is the same as the result of C++.