Python-PCL点云拼接(视觉SLAM第五讲案例)

最近在看高翔老师的《视觉SLAM十四讲》, 上面有C++的点云拼接, 看了之后也想用Python实现一下.

环境

于是去下了python-pcl, 安装这个库的过程还是比较折腾的. 最后还是按照这个issue里面讲得才安装成功.
这个库的文件写得比较简陋, 很多东西还是要查询PCL.
具体的C++, python代码还要案例使用的RGB图片和深度图都在GitHub, 大家可以参考.

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 <python-modules-team@lists.alioth.debian.org>
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.

再就是opencv-python (3.4.1.15)

四元数

使用Python写这个由于没找到现成的类似eigen3的库, 所以自己写了个四元数, 用来从四元数转换到旋转矩阵, 顺便把平移的translation也写在这里面了.
里面这个四元数转换的旋转矩阵一开始按照《视觉SLAM十四讲》上面的公式, 后来发现用起来有点问题, 因为我们numpy array都用列向量来表示, 所以那个矩阵要转置一下, 不然之后的点的方位会有错误.

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

点云拼接

python-pcl 的 PointCloud_PointXYZRGB有几个方法:

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

这里我们使用了from_list, 或者转换成numpy array然后使用from_array也可以.
每个点的表示方式是float32格式的[X, Y, Z, RGB], RGB是一整个整数, 一开始我还以为是简单的三个255拼在一起, 后来发现不对劲. 于是看了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);

可以看到RGB是rgb三个整数经过位运算后编码而成的一个整数

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())

最终结果是这样的, 跟C++的结果一样.