点云坐标和图像坐标关系 python

点云数据（Point Cloud Data）:

         点云数据表示为至少含有3列（x, y, z）的N行数组，其中(x, y, z)代表扫描物体表面点的三维空间位置，如果点云信息来自激光雷达传感器等，也可加上强度信息（intensity）和深度信息（depth）等，N代表点云的数量。

坐标系：图片VS点云

关于图片：

• 图片的坐标值恒为正；
• 图像坐标原点在图片左上角；
• 坐标值为整数
• x轴向右，y轴向下

关于点云坐标：

• 点云的坐标值有正有负；
• 点云值为真实的数值；
• x轴向前，y轴向左，z轴向上；

创建点云鸟瞰图

关于鸟瞰图相关的坐标说明，其中蓝色坐标为图像的坐标，黄色为LIDAR坐标

注意：

• 图像的X轴与LIDAR的Y轴相反，图像的Y轴与LIDAR的X轴相反；
• 需要将值转化，才能保证（0,0）为最小值，即去掉负值；

需关注的矩形限制区域

       因时常只关注点云的某一个特殊区域，所以可以创建一个过滤器以便保留我们感兴趣区域的点云。因为我们从上方观察数据并且我们希望将其转化为图像，所以应使用与图像坐标轴更一致的方向。下面，将具体说明我所关注的与原点有关的范围内的值。原点的左边作为负数，右边作为正数，点云的X轴将作为前进的方向（鸟瞰图向上的方向）。

以下代码设置了感兴趣的矩形区域为原点两侧10米，以及远点前方20米。

side_range=(-10, 10)      # 左右边界分别为-10米-10米
fwd_range=(0, 20)   # 前后边界为0米-20米

然后创建过滤器，只保留我们所关注的矩形区域内的点

# 提取每个坐标轴的点
x_points = points[:, 0]
y_points = points[:, 1]
z_points = points[:, 2]

# 过滤器 - 为了返回想要区域的点云切片，建立三个过滤器
# 前-后，边-边，高度范围
# 注意：左边是LIDAR的Y轴的正方向
f_filt = np.logical_and((x_points > fwd_range[0]), (x_points < fwd_range[1]))
s_filt = np.logical_and((y_points > -side_range[1]), (y_points < -side_range[0]))
filter = np.logical_and(f_filt, s_filt)
indices = np.argwhere(filter).flatten()

# KEEPERS
x_points = x_points[indices]
y_points = y_points[indices]
z_points = z_points[indices]

映射点云位置到像素位置

        现在，我们有一堆数值为实数的点云数据，为了实现映射需要将这些数值映射到整数位置，我们可以单纯的将X和Y数值转化成整数，但是会缺少一些分辨率。例如，如果点云的测量单元是米，那么每个像素在点云中将代表一个1X1的矩阵，将丢失逼着更小的细节部分。如果你有向山景一样的点云那就还好。但是如果你需要捕捉更细微的细节并识别出人、车甚至更小的事情，那么饿这种方法就不可行。       

然而可以将上述方法稍加修改，就可以得到我们想要的分辨率。在数据类型转化为整数之前，我们可以缩放数据，，举例来说：如果测量单位为米，我们想要5厘米的分辨率，可通过下述实现：

res = 0.05
# 基于分辨率转换像素位置的值
x_img = (-y_points / res).astype(np.int32)  # x axis is -y in LIDAR
y_img = (-x_points / res).astype(np.int32)  # y axis is -x in LIDAR

你可以发现，X轴Y轴发生互换，并且方向也发生改变以便于进行图像坐标系处理。

改变新的原点

因为x和y数据仍有负值，所以暂时还不可以投影到图片数据上，因此需要将数据转化，让（0，0）为最小值。

# SHIFT PIXELS TO HAVE MINIMUM BE (0,0)
# floor and ceil used to prevent anything being rounded to below 0 after shift
x_img -= int(np.floor(side_range[0] / res))
y_img += int(np.ceil(fwd_range[1] / res))

对数据进行验证：

>>> x_img.min()
7
>>> x_img.max()
199
>>> y_img.min()
1
>>> y_img.max()
199

像素值

目前为止，我们使用点云数据明确了图像中的x和y的位置。现在需要明确在图片中的像素位置上需要填充什么样的值。其中一种方法是填充高度值。此时需要保证：

• 像素值为整数；
• 像素值应在0-255之后。

可以从数据中获得最大值和最小值，并重新缩放数值使其在0-255之间。另一种方法是，将我们关注的高度值设定一个范围，任何高于或低于该范围的值将被设置成最大值或者最小值。因为者可以让我们获得感兴趣范围内的最大限度的细节，所以这方法比较有用。

在下边的代码中，将范围设置在原点的2米以下以及原点上方0.5米。

height_range = (-2, 0.5)  # bottom-most to upper-most

# CLIP HEIGHT VALUES - to between min and max heights
pixel_values = np.clip(a = z_points,
                           a_min=height_range[0],
                           a_max=height_range[1])

然后将这些值缩放到0-255之间，数据类型设为整数。

def scale_to_255(a, min, max, dtype=np.uint8):
    """ Scales an array of values from specified min, max range to 0-255
        Optionally specify the data type of the output (default is uint8)
    """
    return (((a - min) / float(max - min)) * 255).astype(dtype)

# RESCALE THE HEIGHT VALUES - to be between the range 0-255
pixel_values  = scale_to_255(pixel_values, min=height_range[0], max=height_range[1])

创建图像数组

现在创建图片，初始化一个数组，其维度取决于数值的范围以及所选感兴趣区域的分辨率，然后使用x和y的点云数据转化为像素位置，并具体明确数组中的切片位置。

# INITIALIZE EMPTY ARRAY - of the dimensions we want
x_max = 1+int((side_range[1] - side_range[0])/res)
y_max = 1+int((fwd_range[1] - fwd_range[0])/res)
im = np.zeros([y_max, x_max], dtype=np.uint8)

# FILL PIXEL VALUES IN IMAGE ARRAY
im[y_img, x_img] = pixel_values

可视化

此时，图像已经被存储为一个好数组，利用PIL 图像可以将其可视化。

# CONVERT FROM NUMPY ARRAY TO A PIL IMAGE
from PIL import Image
im2 = Image.fromarray(im)
im2.show()

作为人类并不能很好的说出灰色和阴影的区别，可以利用彩色图来说明值之间的差异，可以利用matplotlib.

import matplotlib.pyplot as plt
plt.imshow(im, cmap="spectral", vmin=0, vmax=255)
plt.show()

实际上，利用该方法 绘制图行和利用PIL绘制图像编码了同样的信息量，因此机器学习算法可以在高度上进行辨别甚至人类都不能清晰地看出差异。

完整代码

为了方便，将以上代码放入一个功能函数返回值是一个数组表示的鸟瞰图，你可以利用你喜欢的方法可视化，也可以将其作为数组放进机器学习算法里。

import numpy as np


# ==============================================================================
#                                                                   SCALE_TO_255
# ==============================================================================
def scale_to_255(a, min, max, dtype=np.uint8):
    """ Scales an array of values from specified min, max range to 0-255
        Optionally specify the data type of the output (default is uint8)
    """
    return (((a - min) / float(max - min)) * 255).astype(dtype)


# ==============================================================================
#                                                         POINT_CLOUD_2_BIRDSEYE
# ==============================================================================
def point_cloud_2_birdseye(points,
                           res=0.1,
                           side_range=(-10., 10.),  # left-most to right-most
                           fwd_range = (-10., 10.), # back-most to forward-most
                           height_range=(-2., 2.),  # bottom-most to upper-most
                           ):
    """ Creates an 2D birds eye view representation of the point cloud data.

    Args:
        points:     (numpy array)
                    N rows of points data
                    Each point should be specified by at least 3 elements x,y,z
        res:        (float)
                    Desired resolution in metres to use. Each output pixel will
                    represent an square region res x res in size.
        side_range: (tuple of two floats)
                    (-left, right) in metres
                    left and right limits of rectangle to look at.
        fwd_range:  (tuple of two floats)
                    (-behind, front) in metres
                    back and front limits of rectangle to look at.
        height_range: (tuple of two floats)
                    (min, max) heights (in metres) relative to the origin.
                    All height values will be clipped to this min and max value,
                    such that anything below min will be truncated to min, and
                    the same for values above max.
    Returns:
        2D numpy array representing an image of the birds eye view.
    """
    # EXTRACT THE POINTS FOR EACH AXIS
    x_points = points[:, 0]
    y_points = points[:, 1]
    z_points = points[:, 2]

    # FILTER - To return only indices of points within desired cube
    # Three filters for: Front-to-back, side-to-side, and height ranges
    # Note left side is positive y axis in LIDAR coordinates
    f_filt = np.logical_and((x_points > fwd_range[0]), (x_points < fwd_range[1]))
    s_filt = np.logical_and((y_points > -side_range[1]), (y_points < -side_range[0]))
    filter = np.logical_and(f_filt, s_filt)
    indices = np.argwhere(filter).flatten()

    # KEEPERS
    x_points = x_points[indices]
    y_points = y_points[indices]
    z_points = z_points[indices]

    # CONVERT TO PIXEL POSITION VALUES - Based on resolution
    x_img = (-y_points / res).astype(np.int32)  # x axis is -y in LIDAR
    y_img = (-x_points / res).astype(np.int32)  # y axis is -x in LIDAR

    # SHIFT PIXELS TO HAVE MINIMUM BE (0,0)
    # floor & ceil used to prevent anything being rounded to below 0 after shift
    x_img -= int(np.floor(side_range[0] / res))
    y_img += int(np.ceil(fwd_range[1] / res))

    # CLIP HEIGHT VALUES - to between min and max heights
    pixel_values = np.clip(a=z_points,
                           a_min=height_range[0],
                           a_max=height_range[1])

    # RESCALE THE HEIGHT VALUES - to be between the range 0-255
    pixel_values = scale_to_255(pixel_values,
                                min=height_range[0],
                                max=height_range[1])

    # INITIALIZE EMPTY ARRAY - of the dimensions we want
    x_max = 1 + int((side_range[1] - side_range[0]) / res)
    y_max = 1 + int((fwd_range[1] - fwd_range[0]) / res)
    im = np.zeros([y_max, x_max], dtype=np.uint8)

    # FILL PIXEL VALUES IN IMAGE ARRAY
    im[y_img, x_img] = pixel_values

    return im