KPConv-PyTorch/datasets/NCLT.py

503 lines
14 KiB
Python
Raw Normal View History

2020-04-23 13:51:16 +00:00
#
#
# 0=================================0
# | Kernel Point Convolutions |
# 0=================================0
#
#
# ----------------------------------------------------------------------------------------------------------------------
#
# Class handling SemanticKitti dataset.
# Implements a Dataset, a Sampler, and a collate_fn
#
# ----------------------------------------------------------------------------------------------------------------------
#
# Hugues THOMAS - 11/06/2018
#
# ----------------------------------------------------------------------------------------------------------------------
#
# Imports and global variables
# \**********************************/
#
# Common libs
import sys
import struct
import scipy
import time
import numpy as np
import pickle
import torch
import yaml
#from mayavi import mlab
from multiprocessing import Lock
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# OS functions
from os import listdir
from os.path import exists, join, isdir, getsize
# Dataset parent class
from datasets.common import *
from torch.utils.data import Sampler, get_worker_info
from utils.mayavi_visu import *
from utils.metrics import fast_confusion
from datasets.common import grid_subsampling
from utils.config import bcolors
def ssc_to_homo(ssc, ssc_in_radians=True):
# Convert 6-DOF ssc coordinate transformation to 4x4 homogeneous matrix
# transformation
if ssc.ndim == 1:
reduce = True
ssc = np.expand_dims(ssc, 0)
else:
reduce = False
if not ssc_in_radians:
ssc[:, 3:] = np.pi / 180.0 * ssc[:, 3:]
sr = np.sin(ssc[:, 3])
cr = np.cos(ssc[:, 3])
sp = np.sin(ssc[:, 4])
cp = np.cos(ssc[:, 4])
sh = np.sin(ssc[:, 5])
ch = np.cos(ssc[:, 5])
H = np.zeros((ssc.shape[0], 4, 4))
H[:, 0, 0] = ch*cp
H[:, 0, 1] = -sh*cr + ch*sp*sr
H[:, 0, 2] = sh*sr + ch*sp*cr
H[:, 1, 0] = sh*cp
H[:, 1, 1] = ch*cr + sh*sp*sr
H[:, 1, 2] = -ch*sr + sh*sp*cr
H[:, 2, 0] = -sp
H[:, 2, 1] = cp*sr
H[:, 2, 2] = cp*cr
H[:, 0, 3] = ssc[:, 0]
H[:, 1, 3] = ssc[:, 1]
H[:, 2, 3] = ssc[:, 2]
H[:, 3, 3] = 1
if reduce:
H = np.squeeze(H)
return H
def verify_magic(s):
magic = 44444
m = struct.unpack('<HHHH', s)
return len(m)>=4 and m[0] == magic and m[1] == magic and m[2] == magic and m[3] == magic
def test_read_hits():
data_path = '../../Data/NCLT'
velo_folder = 'velodyne_data'
day = '2012-01-08'
hits_path = join(data_path, velo_folder, day, 'velodyne_hits.bin')
all_utimes = []
all_hits = []
all_ints = []
num_bytes = getsize(hits_path)
current_bytes = 0
with open(hits_path, 'rb') as f_bin:
total_hits = 0
first_utime = -1
last_utime = -1
while True:
magic = f_bin.read(8)
if magic == b'':
break
if not verify_magic(magic):
print('Could not verify magic')
num_hits = struct.unpack('<I', f_bin.read(4))[0]
utime = struct.unpack('<Q', f_bin.read(8))[0]
# Do not convert padding (it is an int always equal to zero)
padding = f_bin.read(4)
total_hits += num_hits
if first_utime == -1:
first_utime = utime
last_utime = utime
hits = []
ints = []
for i in range(num_hits):
x = struct.unpack('<H', f_bin.read(2))[0]
y = struct.unpack('<H', f_bin.read(2))[0]
z = struct.unpack('<H', f_bin.read(2))[0]
i = struct.unpack('B', f_bin.read(1))[0]
l = struct.unpack('B', f_bin.read(1))[0]
hits += [[x, y, z]]
ints += [i]
utimes = np.full((num_hits,), utime - first_utime, dtype=np.int32)
ints = np.array(ints, dtype=np.uint8)
hits = np.array(hits, dtype=np.float32)
hits *= 0.005
hits += -100.0
all_utimes.append(utimes)
all_hits.append(hits)
all_ints.append(ints)
if 100 * current_bytes / num_bytes > 0.1:
break
current_bytes += 24 + 8 * num_hits
print('{:d}/{:d} => {:.1f}%'.format(current_bytes, num_bytes, 100 * current_bytes / num_bytes))
all_utimes = np.hstack(all_utimes)
all_hits = np.vstack(all_hits)
all_ints = np.hstack(all_ints)
write_ply('test_hits',
[all_hits, all_ints, all_utimes],
['x', 'y', 'z', 'intensity', 'utime'])
print("Read %d total hits from %ld to %ld" % (total_hits, first_utime, last_utime))
return 0
def frames_to_ply(show_frames=False):
# In files
data_path = '../../Data/NCLT'
velo_folder = 'velodyne_data'
days = np.sort([d for d in listdir(join(data_path, velo_folder))])
for day in days:
# Out files
ply_folder = join(data_path, 'frames_ply', day)
if not exists(ply_folder):
makedirs(ply_folder)
day_path = join(data_path, velo_folder, day, 'velodyne_sync')
f_names = np.sort([f for f in listdir(day_path) if f[-4:] == '.bin'])
N = len(f_names)
print('Reading', N, 'files')
for f_i, f_name in enumerate(f_names):
ply_name = join(ply_folder, f_name[:-4] + '.ply')
if exists(ply_name):
continue
t1 = time.time()
hits = []
ints = []
with open(join(day_path, f_name), 'rb') as f_bin:
while True:
x_str = f_bin.read(2)
# End of file
if x_str == b'':
break
x = struct.unpack('<H', x_str)[0]
y = struct.unpack('<H', f_bin.read(2))[0]
z = struct.unpack('<H', f_bin.read(2))[0]
intensity = struct.unpack('B', f_bin.read(1))[0]
l = struct.unpack('B', f_bin.read(1))[0]
hits += [[x, y, z]]
ints += [intensity]
ints = np.array(ints, dtype=np.uint8)
hits = np.array(hits, dtype=np.float32)
hits *= 0.005
hits += -100.0
write_ply(ply_name,
[hits, ints],
['x', 'y', 'z', 'intensity'])
t2 = time.time()
print('File {:s} {:d}/{:d} Done in {:.1f}s'.format(f_name, f_i, N, t2 - t1))
if show_frames:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(hits[:, 0], hits[:, 1], -hits[:, 2], c=-hits[:, 2], s=5, linewidths=0)
plt.show()
return 0
def merge_day_pointclouds(show_day_trajectory=False, only_SLAM_nodes=False):
"""
Recreate the whole day point cloud thks to gt pose
Generate gt_annotation of mobile objects
"""
# In files
data_path = '../../Data/NCLT'
gt_folder = 'ground_truth'
cov_folder = 'ground_truth_cov'
# Transformation from body to velodyne frame (from NCLT paper)
x_body_velo = np.array([0.002, -0.004, -0.957, 0.807, 0.166, -90.703])
H_body_velo = ssc_to_homo(x_body_velo, ssc_in_radians=False)
H_velo_body = np.linalg.inv(H_body_velo)
x_body_lb3 = np.array([0.035, 0.002, -1.23, -179.93, -0.23, 0.50])
H_body_lb3 = ssc_to_homo(x_body_lb3, ssc_in_radians=False)
H_lb3_body = np.linalg.inv(H_body_lb3)
# Get gt files and days
gt_files = np.sort([gt_f for gt_f in listdir(join(data_path, gt_folder)) if gt_f[-4:] == '.csv'])
cov_files = np.sort([cov_f for cov_f in listdir(join(data_path, cov_folder)) if cov_f[-4:] == '.csv'])
days = [d[:-4].split('_')[1] for d in gt_files]
# Load all gt poses
print('\nLoading days groundtruth poses...')
t0 = time.time()
gt_H = []
gt_t = []
for d, gt_f in enumerate(gt_files):
t1 = time.time()
gt_pkl_file = join(data_path, gt_folder, gt_f[:-4] + '.pkl')
if exists(gt_pkl_file):
# Read pkl
with open(gt_pkl_file, 'rb') as f:
day_gt_t, day_gt_H = pickle.load(f)
else:
# File paths
gt_csv = join(data_path, gt_folder, gt_f)
# Load gt
gt = np.loadtxt(gt_csv, delimiter=',')
# Convert gt to homogenous rotation/translation matrix
day_gt_t = gt[:, 0]
day_gt_H = ssc_to_homo(gt[:, 1:])
# Save pickle
with open(gt_pkl_file, 'wb') as f:
pickle.dump([day_gt_t, day_gt_H], f)
t2 = time.time()
print('{:s} {:d}/{:d} Done in {:.1f}s'.format(gt_f, d, gt_files.shape[0], t2 - t1))
gt_t += [day_gt_t]
gt_H += [day_gt_H]
if show_day_trajectory:
cov_csv = join(data_path, cov_folder, cov_files[d])
cov = np.loadtxt(cov_csv, delimiter=',')
t_cov = cov[:, 0]
t_cov_bool = np.logical_and(t_cov > np.min(day_gt_t), t_cov < np.max(day_gt_t))
t_cov = t_cov[t_cov_bool]
# Note: Interpolation is not needed, this is done as a convinience
interp = scipy.interpolate.interp1d(day_gt_t, day_gt_H[:, :3, 3], kind='nearest', axis=0)
node_poses = interp(t_cov)
plt.figure()
plt.scatter(day_gt_H[:, 1, 3], day_gt_H[:, 0, 3], 1, c=-day_gt_H[:, 2, 3], linewidth=0)
plt.scatter(node_poses[:, 1], node_poses[:, 0], 1, c=-node_poses[:, 2], linewidth=5)
plt.axis('equal')
plt.title('Ground Truth Position of Nodes in SLAM Graph')
plt.xlabel('East (m)')
plt.ylabel('North (m)')
plt.colorbar()
plt.show()
t2 = time.time()
print('Done in {:.1f}s\n'.format(t2 - t0))
# Out files
out_folder = join(data_path, 'day_ply')
if not exists(out_folder):
makedirs(out_folder)
# Focus on a particular point
p0 = np.array([-220, -527, 12])
center_radius = 10.0
point_radius = 50.0
# Loop on days
for d, day in enumerate(days):
#if day != '2012-02-05':
# continue
day_min_t = gt_t[d][0]
day_max_t = gt_t[d][-1]
frames_folder = join(data_path, 'frames_ply', day)
f_times = np.sort([float(f[:-4]) for f in listdir(frames_folder) if f[-4:] == '.ply'])
# If we want, load only SLAM nodes
if only_SLAM_nodes:
# Load node timestamps
cov_csv = join(data_path, cov_folder, cov_files[d])
cov = np.loadtxt(cov_csv, delimiter=',')
t_cov = cov[:, 0]
t_cov_bool = np.logical_and(t_cov > day_min_t, t_cov < day_max_t)
t_cov = t_cov[t_cov_bool]
# Find closest lidar frames
t_cov = np.expand_dims(t_cov, 1)
diffs = np.abs(t_cov - f_times)
inds = np.argmin(diffs, axis=1)
f_times = f_times[inds]
# Is this frame in gt
f_t_bool = np.logical_and(f_times > day_min_t, f_times < day_max_t)
f_times = f_times[f_t_bool]
# Interpolation gt poses to frame timestamps
interp = scipy.interpolate.interp1d(gt_t[d], gt_H[d], kind='nearest', axis=0)
frame_poses = interp(f_times)
N = len(f_times)
world_points = []
world_frames = []
world_frames_c = []
print('Reading', day, ' => ', N, 'files')
for f_i, f_t in enumerate(f_times):
t1 = time.time()
#########
# GT pose
#########
H = frame_poses[f_i].astype(np.float32)
# s = '\n'
# for cc in H:
# for c in cc:
# s += '{:5.2f} '.format(c)
# s += '\n'
# print(s)
#############
# Focus check
#############
if np.linalg.norm(H[:3, 3] - p0) > center_radius:
continue
###################################
# Local frame coordinates for debug
###################################
# Create artificial frames
x = np.linspace(0, 1, 50, dtype=np.float32)
points = np.hstack((np.vstack((x, x*0, x*0)), np.vstack((x*0, x, x*0)), np.vstack((x*0, x*0, x)))).T
colors = ((points > 0.1).astype(np.float32) * 255).astype(np.uint8)
hpoints = np.hstack((points, np.ones_like(points[:, :1])))
hpoints = np.matmul(hpoints, H.T)
hpoints[:, 3] *= 0
world_frames += [hpoints[:, :3]]
world_frames_c += [colors]
#######################
# Load velo point cloud
#######################
# Load frame ply file
f_name = '{:.0f}.ply'.format(f_t)
data = read_ply(join(frames_folder, f_name))
points = np.vstack((data['x'], data['y'], data['z'])).T
#intensity = data['intensity']
hpoints = np.hstack((points, np.ones_like(points[:, :1])))
hpoints = np.matmul(hpoints, H.T)
hpoints[:, 3] *= 0
hpoints[:, 3] += np.sqrt(f_t - f_times[0])
# focus check
focus_bool = np.linalg.norm(hpoints[:, :3] - p0, axis=1) < point_radius
hpoints = hpoints[focus_bool, :]
world_points += [hpoints]
t2 = time.time()
print('File {:s} {:d}/{:d} Done in {:.1f}s'.format(f_name, f_i, N, t2 - t1))
if len(world_points) < 2:
continue
world_points = np.vstack(world_points)
###### DEBUG
world_frames = np.vstack(world_frames)
world_frames_c = np.vstack(world_frames_c)
write_ply('testf.ply',
[world_frames, world_frames_c],
['x', 'y', 'z', 'red', 'green', 'blue'])
###### DEBUG
print(world_points.shape, world_points.dtype)
# Subsample merged frames
# world_points, features = grid_subsampling(world_points[:, :3],
# features=world_points[:, 3:],
# sampleDl=0.1)
features = world_points[:, 3:]
world_points = world_points[:, :3]
print(world_points.shape, world_points.dtype)
write_ply('test' + day + '.ply',
[world_points, features],
['x', 'y', 'z', 't'])
# Generate gt annotations
# Subsample day ply (for visualization)
# Save day ply
# a = 1/0