Added tool to project lidar Pointclouds into spherical and image coor…

…dinates

tools/ProjectionTools/Lidar2RGB/lib/utils.py

@@ -0,0 +1,213 @@
+import numpy as np
+import cv2
+import scipy.spatial
+from sklearn.linear_model import RANSACRegressor
+
+
+def project_pointcloud(lidar, vtc, velodyne_to_camera, image_shape, init=None, draw_big_circle=False):
+    def py_func_project_3D_to_2D(points_3D, P):
+        # Project on image
+        points_2D = np.matmul(P, np.vstack((points_3D, np.ones([1, np.shape(points_3D)[1]]))))
+
+        # scale projected points
+        points_2D[0][:] = points_2D[0][:] / points_2D[2][:]
+        points_2D[1][:] = points_2D[1][:] / points_2D[2][:]
+
+        points_2D = points_2D[0:2]
+        return points_2D.transpose()
+
+    def py_func_create_lidar_img(lidar_points_2D, lidar_points, img_width=1248, img_height=375, init=None):
+        within_image_boarder_width = np.logical_and(img_width > lidar_points_2D[:, 0], lidar_points_2D[:, 0] >= 0)
+        within_image_boarder_height = np.logical_and(img_height > lidar_points_2D[:, 1], lidar_points_2D[:, 1] >= 0)
+
+        valid_points = np.logical_and(within_image_boarder_width, within_image_boarder_height)
+        coordinates = np.where(valid_points)[0]
+
+        values = lidar_points[:, coordinates]
+        if init is None:
+            image = -120.0 * np.ones((img_width, img_height, 3))
+        else:
+            print(init.shape)
+            image = init.transpose((1, 0, 2))
+
+        img_coordinates = lidar_points_2D[coordinates, :].astype(dtype=np.int32)
+
+        if not draw_big_circle:
+            image[img_coordinates[:, 0], img_coordinates[:, 1], :] = values.transpose()
+        else:
+            # Slow elementwise circle drawing through opencv
+            len = img_coordinates.shape[0]
+
+            image = image.transpose([1, 0, 2]).squeeze().copy()
+            import matplotlib as mpl
+            import matplotlib.cm as cm
+            norm = mpl.colors.Normalize(vmin=0, vmax=80)
+            cmap = cm.jet
+            m = cm.ScalarMappable(norm, cmap)
+
+            depth_map_color = values.transpose()[:, 1]
+            depth_map_color = m.to_rgba(depth_map_color)
+
+            depth_map_color = (255 * depth_map_color).astype(dtype=np.uint8)
+            for idx in range(len):
+                x, y = img_coordinates[idx, :]
+                value = depth_map_color[idx]
+                # print value
+                tupel_value = (int(value[0]), int(value[1]), int(value[2]))
+                # print tupel_value
+                cv2.circle(image, (x, y), 3, tupel_value, -1)
+
+            return image
+
+        return image.transpose([1, 0, 2]).squeeze()
+
+    def py_func_lidar_projection(lidar_points_3D, vtc, velodyne_to_camera, shape, init=None):  # input):
+
+        img_width = shape[1]
+        img_height = shape[0]
+        # print img_height, img_width
+        lidar_points_3D = lidar_points_3D[:, 0:4]
+
+        # Filer away all points behind image plane
+        min_x = 2.5
+        valid = lidar_points_3D[:, 0] > min_x
+        # extend projection matrix to 5d to efficiently parse intensity
+        lidar_points_3D = lidar_points_3D[np.where(valid)]
+        lidar_points_3D2 = np.ones((lidar_points_3D.shape[0], lidar_points_3D.shape[1] + 1))
+        lidar_points_3D2[:, 0:3] = lidar_points_3D[:, 0:3]
+        lidar_points_3D2[:, 4] = lidar_points_3D[:, 3]
+        # Extend projection matric to pass trough intensities
+        velodyne_to_camera2 = np.zeros((5, 5))
+        velodyne_to_camera2[0:4, 0:4] = velodyne_to_camera
+        velodyne_to_camera2[4, 4] = 1
+
+        lidar_points_2D = py_func_project_3D_to_2D(lidar_points_3D.transpose()[:][0:3], vtc)
+
+        pts_3D = np.matmul(velodyne_to_camera2, lidar_points_3D2.transpose())
+        # detelete placeholder 1 axis
+        pts_3D = np.delete(pts_3D, 3, axis=0)
+
+        pts_3D_yzi = pts_3D[1:, :]
+
+        return py_func_create_lidar_img(lidar_points_2D, pts_3D_yzi, img_width=img_width,
+                                        img_height=img_height, init=init)
+
+    return py_func_lidar_projection(lidar, vtc, velodyne_to_camera, image_shape, init=init)
+
+
+def find_missing_points(last, strongest):
+    last_set = set([tuple(x) for x in last])
+    strong_set = set([tuple(x) for x in strongest])
+    remaining_last = np.array([x for x in last_set - strong_set])
+    remaining_strong = np.array([x for x in strong_set - last_set])
+
+    return remaining_last, remaining_strong
+
+
+def transform_coordinates(xyz):
+    """
+    Takes as input a Pointcloud with xyz coordinates and appends spherical coordinates as columns
+    :param xyz:
+    :return: Pointcloud with following columns, r, phi, theta, ring, intensity, x, y, z, intensity, ring
+    """
+
+    ptsnew = np.hstack((np.zeros_like(xyz), xyz))
+    r_phi = xyz[:, 0] ** 2 + xyz[:, 1] ** 2
+    ptsnew[:, 0] = np.sqrt(r_phi + xyz[:, 2] ** 2)
+    ptsnew[:, 2] = np.pi / 2 - np.arctan2(np.sqrt(r_phi), xyz[:, 2])  # for elevation angle defined from Z-axis down
+
+    ptsnew[:, 1] = np.arctan2(xyz[:, 1], xyz[:, 0])
+    ptsnew[:, 3] = xyz[:, 4]
+    ptsnew[:, 4] = xyz[:, 3]
+
+    return ptsnew
+
+
+def find_closest_neighbors(x, reference):
+    """
+    This function allows you to match strongest and last echos and reason about scattering distributions.
+
+    :param x: Pointcloud which should be matched
+    :param reference: Reference Pointcloud
+    :return: returns valid matching indexes
+    """
+    tree = scipy.spatial.KDTree(reference[:, 1:4])
+    distances, indexes = tree.query(x[:, 1:4], p=2)
+    print('indexes', indexes)
+    print('found matches', len(indexes), len(set(indexes)))
+    # return 0
+    valid = []
+    # not matching contains all not explainable scattered mismatching particles
+    not_matching = []
+    for idx, i in enumerate(indexes):
+
+        delta = reference[i, :] - x[idx, :]
+        # Laser Ring has to match
+        if delta[-1] == 0:
+
+            # Follows assumption that strongest echo has higher intensity than last and that the range is more distant
+            # for the last return. The sensor can report 2 strongest echo if strongest and last echo are matching.
+            # Here those points are not being matched.
+            if delta[-2] < 0 and delta[0] > 0:
+                valid.append((i, idx))
+            else:
+                not_matching.append((i, idx))
+        else:
+            not_matching.append((i, idx))
+
+    return valid
+
+
+def filter(lidar_data, distance):
+    """
+    Takes lidar Pointcloud as ibnput and filters point below distance threshold
+    :param lidar_data: Input Pointcloud
+    :param distance: Minimum distance for filtering
+    :return: Filtered Pointcloud
+    """
+
+    r = np.sqrt(lidar_data[:, 0] ** 2 + lidar_data[:, 1] ** 2 + lidar_data[:, 2] ** 2)
+    true_idx = np.where(r > distance)
+
+    lidar_data = lidar_data[true_idx, :]
+
+    return lidar_data[0]
+
+
+def read_split(split):
+    with open(split, 'r') as f:
+        entry_ids = f.readlines()
+
+    entry_ids = [i.replace('\n', '') for i in entry_ids]
+    return entry_ids
+
+
+def filter_below_groundplane(pointcloud, tolerance=1):
+    valid_loc = (pointcloud[:, 2] < -1.4) & \
+                (pointcloud[:, 2] > -1.86) & \
+                (pointcloud[:, 0] > 0) & \
+                (pointcloud[:, 0] < 40) & \
+                (pointcloud[:, 1] > -15) & \
+                (pointcloud[:, 1] < 15)
+    pc_rect = pointcloud[valid_loc]
+    print(pc_rect.shape)
+    if pc_rect.shape[0] <= pc_rect.shape[1]:
+        w = [0, 0, 1]
+        h = -1.55
+    else:
+        reg = RANSACRegressor().fit(pc_rect[:, [0, 1]], pc_rect[:, 2])
+        w = np.zeros(3)
+        w[0] = reg.estimator_.coef_[0]
+        w[1] = reg.estimator_.coef_[1]
+        w[2] = 1.0
+        h = reg.estimator_.intercept_
+        w = w / np.linalg.norm(w)
+
+        print(reg.estimator_.coef_)
+        print(reg.get_params())
+        print(w, h)
+    height_over_ground = np.matmul(pointcloud[:, :3], np.asarray(w))
+    height_over_ground = height_over_ground.reshape((len(height_over_ground), 1))
+    above_ground = np.matmul(pointcloud[:, :3], np.asarray(w)) - h > -tolerance
+    print(above_ground.shape)
+    return np.hstack((pointcloud[above_ground, :], height_over_ground[above_ground]))

tools/ProjectionTools/Lidar2RGB/lib/visi.py

@@ -0,0 +1,59 @@
+import matplotlib as mpl
+import matplotlib.pyplot as plt
+import matplotlib.cm as cm
+from tools.ProjectionTools.Lidar2RGB.lib.utils import project_pointcloud
+from tools.ProjectionTools.Lidar2RGB.lib.utils import transform_coordinates
+import numpy as np
+from tools.CreateTFRecords.generic_tf_tools.resize import resize
+
+
+def plot_spherical_scatter_plot(pointlcoud, pattern='hot', plot_show=True, title=None):
+    norm = mpl.colors.Normalize(vmin=0, vmax=80)
+    cmap = None
+    if pattern == 'hot':
+        cmap = cm.hot
+    elif pattern == 'cool':
+        cmap = cm.cool
+    elif pattern == 'jet':
+        cmap = cm.jet
+    else:
+        print('Wrong color map specified')
+        exit()
+    m = cm.ScalarMappable(norm, cmap)
+    transformed_pointcloud = transform_coordinates(pointlcoud)
+    depth_map_color = m.to_rgba(transformed_pointcloud[:, 0])
+
+    plt.scatter(transformed_pointcloud[:, 1],
+                transformed_pointcloud[:, 2], c=depth_map_color, s=1)
+
+    if title is not None:
+        plt.title(title)
+
+    if plot_show:
+        plt.show()
+
+
+def plot_image_projection(pointcloud, vtc, velodyne_to_camera, frame='default', title=None):
+
+    # Resize image to other crop
+    r = resize(frame)
+
+    lidar_image = project_pointcloud(pointcloud, np.matmul(r.get_image_scaling(), vtc), velodyne_to_camera,
+                                     list(r.dsize)[::-1] + [3], init=np.zeros(list(r.dsize)[::-1] + [3]),
+                                     draw_big_circle=True)
+
+    if title is not None:
+        plt.title(title)
+
+    plt.imshow(lidar_image)
+    plt.show()
+
+
+def plot_3d_scatter(pointlcoud, plot_show=True):
+    fig = plt.figure()
+    ax = plt.axes(projection="3d")
+
+    ax.scatter3D(pointlcoud[:, 0], pointlcoud[:, 1], pointlcoud[:, 2], c='black', alpha=1, s=0.01)
+
+    if plot_show:
+        plt.show()

tools/ProjectionTools/Lidar2RGB/run_2d_projection.py

@@ -0,0 +1,69 @@
+from tools.DatasetViewer.lib.read import load_velodyne_scan
+from tools.DatasetViewer.lib.read import load_calib_data
+from tools.ProjectionTools.Lidar2RGB.lib.utils import filter, \
+    find_closest_neighbors, find_missing_points, transform_coordinates
+from tools.ProjectionTools.Lidar2RGB.lib.visi import plot_spherical_scatter_plot, plot_image_projection
+# import cv2
+import numpy as np
+
+import os
+import argparse
+
+
+def parsArgs():
+    parser = argparse.ArgumentParser(description='Lidar 2d projection tool')
+    parser.add_argument('--root', '-r', help='Enter the root folder')
+    parser.add_argument('--lidar_type', '-t', help='Enter the root folder', default='lidar_hdl64',
+                        choices=['lidar_hdl64', 'lidar_vlp32'])
+    args = parser.parse_args()
+
+    return args
+
+
+interesting_samples = [
+    '2018-02-06_14-25-51_00400',
+    '2019-09-11_16-39-41_01770',
+    '2018-02-12_07-16-32_00100',
+    '2018-10-29_16-42-03_00560',
+]
+
+echos = [
+    ['last', 'strongest'],
+]
+
+if __name__ == '__main__':
+
+    args = parsArgs()
+
+    velodyne_to_camera, camera_to_velodyne, P, R, vtc, radar_to_camera, zero_to_camera = load_calib_data(
+        args.root, name_camera_calib='calib_cam_stereo_left.json', tf_tree='calib_tf_tree_full.json',
+        velodyne_name='lidar_hdl64_s3_roof' if args.lidar_type == 'lidar_hdl64' else 'lidar_vlp32_roof')
+
+    for interesting_sample in interesting_samples:
+        velo_file_last = os.path.join(args.root, args.lidar_type + '_' + echos[0][0],
+                                      interesting_sample + '.bin')
+        velo_file_strongest = os.path.join(args.root, args.lidar_type + '_' + echos[0][1],
+                                           interesting_sample + '.bin')
+        lidar_data_last = load_velodyne_scan(velo_file_last)
+        lidar_data_strongest = load_velodyne_scan(velo_file_strongest)
+
+        print('last shape:', lidar_data_last.shape)
+        lidar_data_last = filter(lidar_data_last, 1.5)
+        print('strongest shape:', lidar_data_strongest.shape)
+        lidar_data_strongest = filter(lidar_data_strongest, 1.5)
+
+        remaining_last, remaining_strong = find_missing_points(lidar_data_last, lidar_data_strongest)
+        valid = find_closest_neighbors(transform_coordinates(remaining_strong), transform_coordinates(remaining_last))
+
+        plot_spherical_scatter_plot(lidar_data_last, pattern='hot', title='Spherical Plot Last Echo')
+        plot_spherical_scatter_plot(lidar_data_strongest, pattern='cool', title='Spherical Plot Strongest Echo')
+
+        print(len(remaining_strong), '/', len(lidar_data_strongest), len(remaining_last), '/', len(lidar_data_last))
+        print("intensity_mean", np.mean(lidar_data_strongest[:, 3]), np.mean(lidar_data_last[:, 3]))
+        print("intensity_mean", np.mean(remaining_strong[:, 3]), np.mean(remaining_last[:, 3]))
+
+        plot_spherical_scatter_plot(remaining_last, pattern='hot', plot_show=False)
+        plot_spherical_scatter_plot(remaining_strong, pattern='cool', title='Not matching echos')
+
+        plot_image_projection(lidar_data_last, vtc, velodyne_to_camera, title='Camera Projection Last Echo')
+        plot_image_projection(lidar_data_strongest, vtc, velodyne_to_camera, title='Camera Projection Strongest Echo')