################################################################################
#
# Copyright (c) 2017 University of Oxford
# Authors:
# Geoff Pascoe (gmp@robots.ox.ac.uk)
#
# This work is licensed under the Creative Commons
# Attribution-NonCommercial-ShareAlike 4.0 International License.
# To view a copy of this license, visit
# http://creativecommons.org/licenses/by-nc-sa/4.0/ or send a letter to
# Creative Commons, PO Box 1866, Mountain View, CA 94042, USA.
#
################################################################################
import os
import re
import numpy as np
import matplotlib.pyplot as plt
import argparse
sys.path.append('../dataset_loaders')
from robotcar_sdk.build_pointcloud import build_pointcloud
from robotcar_sdk.transform import build_se3_transform
from robotcar_sdk.image import load_image
from robotcar_sdk.camera_model import CameraModel
image_dir='../data/deepslam_data/RobotCar/loop_ldmrs/2014-06-26-09-24-58/stereo/centre/'
laser_dir='../data/deepslam_data/RobotCar/loop_ldmrs/2014-06-26-09-24-58/ldmrs'
poses_file='../data/deepslam_data/RobotCar/loop_ldmrs/2014-06-26-09-24-58/vo/vo.csv' # or '../data/deepslam_data/RobotCar/loop_ldmrs/2014-06-26-09-24-58/gps/ins.csv' #
models_dir='../data/robotcar_camera_models'
extrinsics_dir='/mnt/lustre/sunjiankai/App/robotcar-dataset-sdk/extrinsics/'
save_dir='../data/deepslam_data/RobotCar/loop_ldmrs/2014-06-26-09-24-58/depth/'
# args = parser.parse_args()
model = CameraModel(models_dir, image_dir)
extrinsics_path = os.path.join(extrinsics_dir, model.camera + '.txt')
with open(extrinsics_path) as extrinsics_file:
extrinsics = [float(x) for x in next(extrinsics_file).split(' ')]
G_camera_vehicle = build_se3_transform(extrinsics)
G_camera_posesource = None
poses_type = re.search('(vo|ins)\.csv', poses_file).group(1)
if poses_type == 'ins':
with open(os.path.join(extrinsics_dir, 'ins.txt')) as extrinsics_file:
extrinsics = next(extrinsics_file)
G_camera_posesource = G_camera_vehicle * build_se3_transform([float(x) for x in extrinsics.split(' ')])
else:
# VO frame and vehicle frame are the same
G_camera_posesource = G_camera_vehicle
print('G_camera_posesource: ', G_camera_posesource)
timestamps_path = os.path.join(image_dir, os.pardir, model.camera + '.timestamps')
if not os.path.isfile(timestamps_path):
timestamps_path = os.path.join(image_dir, os.pardir, os.pardir, model.camera + '.timestamps')
timestamp_list = []
with open(timestamps_path) as timestamps_file:
for i, line in enumerate(timestamps_file):
timestamp_list.append(int(line.split(' ')[0]))
for timestamp in timestamp_list[1000:]:
pointcloud, reflectance = build_pointcloud(laser_dir, poses_file, extrinsics_dir,
timestamp - 1e7, timestamp + 1e7, timestamp)
pointcloud = np.dot(G_camera_posesource, pointcloud)
print('pointcloud.shape: ', pointcloud[:, 0].max(), pointcloud[:, 1].max(), pointcloud[:, 2].max(), pointcloud[:, 0].min(), pointcloud[:, 1].min(), pointcloud[:, 2].min())
image_path = os.path.join(image_dir, str(timestamp) + '.png')
image = load_image(image_path, model)
uv, depth = model.project(pointcloud, image.shape)
plt.imshow(image)
plt.hold(True)
plt.scatter(np.ravel(uv[0, :]), np.ravel(uv[1, :]), s=2, c=depth, edgecolors='none', cmap='jet')
plt.xlim(0, image.shape[1])
plt.ylim(image.shape[0], 0)
plt.xticks([])
plt.yticks([])
plt.show()
Hi, RobotCar Team,
Firstly, thank you for your great dataset!
I followed the https://robotcar-dataset.robots.ox.ac.uk/documentation/#pointcloud-projection-into-images to project pointcloud into images using the Python API provided by the SDK. However, the result seems incorrect as the attachment shows. The code is also in the attachment. Do you have any suggestions?
The code is:
Thank you!