initial commit

README.md

@@ -0,0 +1,4 @@
+Robotcar Dataset SDK
+====================
+This repo contains sample MATLAB code for viewing and manipulating data from the [Oxford Robotcar Dataset](http://robotcar-dataset.robots.ox.ac.uk).
+

extrinsics/ins.txt

@@ -0,0 +1 @@
+-1.7132 0.1181 1.1948 -0.0125 0.0400 0.0050

extrinsics/ldmrs.txt

@@ -0,0 +1 @@
+1.5349 0.0090 1.3777 0.0205 0.0767 -0.0299

extrinsics/lms_front.txt

@@ -0,0 +1 @@
+1.7589 0.2268 1.0411 -0.0437 -1.4572 0.0356

extrinsics/lms_rear.txt

@@ -0,0 +1 @@
+-2.5850 0.2852 1.0885 -2.8405 -1.5090 -0.3614

extrinsics/mono_left.txt

@@ -0,0 +1 @@
+-0.0905 1.6375 0.2803 0.2079 -0.2339 1.2321

extrinsics/mono_rear.txt

@@ -0,0 +1 @@
+-2.0582 0.0894 0.3675 -0.0119 -0.2498 3.1283

extrinsics/mono_right.txt

@@ -0,0 +1 @@
+-0.2587 -1.6810 0.3226 -0.1961 -0.2469 -1.2675

extrinsics/stereo.txt

@@ -0,0 +1 @@
+0 0 0 0 0 0

matlab/BuildPointcloud.m

@@ -0,0 +1,155 @@
+function [pointcloud, reflectance] = BuildPointcloud(laser_dir, ins_file, extrinsics_dir, start_timestamp, end_timestamp, origin_timestamp)
+
+% BuildPointcloud - builds a 3-dimensional pointcloud from multiple 
+%   2-dimensional LIDAR scans
+%
+% [pointcloud, reflectance] = BuildPointcloud(laser_dir, ins_file, extrinsics_dir,
+%   start_timestamp, end_timestamp, origin_timestamp)
+%
+% INPUTS:
+%   laser_dir: directory containing LIDAR scans in binary format
+%   ins_file: csv file containing INS data
+%   extrinsics_dir: directory containing sensor-to-sensor extrinsics
+%   start_timestamp (optional): UNIX timestamp of start of window over which to build 
+%     pointcloud. Defaults to the first timestamp in laser_dir.
+%   end_timestamp (optional): UNIX timestamp of end of window. Defaults to 
+%     start_timestamp + 20 seconds
+%   origin_timestamp (optional): timestamp for origin of coordinate frame.
+%     If no origin timestamp is supplied, the origin of the coordinate frame is
+%     placed at the start of the window.
+%
+% OUTPUTS:
+%   pointcloud: 3xN array containing XYZ values of pointcloud, relative to the
+%     INS frame at origin_timestamp
+%   reflectance: 1xN array containing reflectance values for each point in 
+%     pointcloud
+%
+% NOTE:
+%   If no outputs are specified, function will plot pointcloud to screen
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% Copyright (c) 2016 University of Oxford
+% Authors: 
+%  Geoff Pascoe ([email protected])
+%  Will Maddern ([email protected])
+%
+% 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.
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+  if laser_dir(end) ~= '/'
+    laser_dir = [laser_dir '/'];
+  end
+
+  if extrinsics_dir(end) ~= '/'
+    extrinsics_dir = [extrinsics_dir '/'];
+  end
+
+
+  laser = regexp(laser_dir, '(lms_front|lms_rear|ldmrs)', 'match');
+  laser = laser{end};
+  laser_timestamps = dlmread([laser_dir '../' laser '.timestamps']);
+
+  if ~exist('start_timestamp', 'var')
+    % Search for first chunk with data
+    for chunk = 1:laser_timestamps(end,2)
+      timestamp_index = find(laser_timestamps(:,2) == chunk, 1, 'first');
+      if isempty(timestamp_index)
+        error('No laser scans found in specified directory');
+      end
+      start_timestamp = laser_timestamps(timestamp_index, 1);
+      if exist([laser_dir num2str(start_timestamp) '.bin'], 'file')
+        break;
+      end
+    end
+  end
+
+  if ~exist('end_timestamp', 'var')
+    end_timestamp = start_timestamp + 10e6;
+  end
+
+  if ~exist('origin_timestamp', 'var')
+    origin_timestamp = start_timestamp;
+  end
+
+  start_timestamp = max(start_timestamp, laser_timestamps(1,1));
+  end_timestamp = min(end_timestamp, laser_timestamps(end,1));
+
+  start_index = find(laser_timestamps(:,1) >= start_timestamp, 1, 'first');
+  end_index = find(laser_timestamps(:,1) <= end_timestamp, 1, 'last');
+  laser_timestamps = laser_timestamps(start_index:end_index, :);
+
+  % Load transforms between laser/ins and vehicle
+  laser_extrinisics = dlmread([extrinsics_dir laser '.txt']);
+  ins_extrinsics = dlmread([extrinsics_dir 'ins.txt']);
+
+  % Find pose for each LIDAR scan, relative to origin
+  if (strfind(ins_file, 'ins.csv'))
+    ins_poses = InterpolatePoses(ins_file, laser_timestamps(:,1)', ...
+      origin_timestamp);
+    G_ins_laser = SE3MatrixFromComponents(ins_extrinsics) \ ...
+      SE3MatrixFromComponents(laser_extrinisics);
+  else
+    ins_poses = RelativeToAbsolutePoses(ins_file, laser_timestamps(:,1)', ...
+      origin_timestamp);
+    G_ins_laser = SE3MatrixFromComponents(laser_extrinisics);
+  end
+
+
+
+  n = size(laser_timestamps,1);
+  pointcloud = [];
+  reflectance = [];
+  for i=1:n
+    scan_path = [laser_dir num2str(laser_timestamps(i,1)) '.bin'];
+    if ~exist(scan_path, 'file')
+      continue;
+    end
+    scan_file = fopen(scan_path);
+    scan = fread(scan_file, 'double');
+    fclose(scan_file);
+
+    % The scan file contains repeated tuples of three values
+    scan = reshape(scan, [3 numel(scan)/3]);
+    if regexp(laser_dir, '(lms_rear|lms_front)')
+      % LMS tuples are of the form (x, y, R)
+      reflectance = [reflectance scan(3,:)];
+      scan(3,:) = zeros(1, size(scan,2));
+    end
+
+    % Transform scan to INS frame, move to the INS pose at the scan's timestamp,
+    % then transform back to LIDAR frame
+    scan = ins_poses{i} * G_ins_laser * [scan; ones(1, size(scan,2))];
+    pointcloud = [pointcloud scan(1:3,:)];
+
+  end
+
+  if size(pointcloud) == 0
+    error(['No valid scans found. Missing chunk ' num2str(laser_timestamps(end,2))]);
+  end
+
+  % If nobody is saving the result, plot the pointcloud
+  if nargout == 0
+    if reflectance
+      % Increase contrast in reflectance values
+      colours = (reflectance - min(reflectance)) / ...
+          (max(reflectance) - min(reflectance));
+      colours = 1 ./ (1 + exp(-10*(colours - mean(colours))));
+    else
+      colours = 0.5 * ones(1, size(pointcloud,2));
+    end
+
+    figure('name','Pointcloud Viewer');
+    colormap gray;
+
+    % Using these axes makes the plot easy to view in matlab
+    scatter3(-pointcloud(2,:), -pointcloud(1,:), -pointcloud(3,:), 1, ...
+      colours, '.');
+    axis equal;
+  end
+end

matlab/InterpolatePoses.m

@@ -0,0 +1,151 @@
+function [poses] = InterpolatePoses(ins_file, pose_timestamps, origin_timestamp)
+
+% InterpolatePoses - interpolate INS poses to find poses at given timestamps
+%
+% [poses] = InterpolatePoses(ins_file, pose_timestamps, origin_timestamp)
+%
+% INPUTS:
+%   ins_file: csv file containing INS data
+%   pose_timestamps: array of UNIX timestamps at which interpolated poses are 
+%     required
+%   origin_timestamp: timestamp for origin frame, relative to which poses are
+%     reported
+%
+% OUTPUTS:
+%   poses: cell array of 4x4 matrices, representing SE3 poses at the times 
+%     specified in pose_timestamps
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% Copyright (c) 2016 University of Oxford
+% Authors: 
+%  Geoff Pascoe ([email protected])
+%  Will Maddern ([email protected])
+%
+% 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.
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+  ins_file_id = fopen(ins_file);
+  headers = textscan(ins_file_id, '%s', 15, 'Delimiter',',');
+  ins_data = textscan(ins_file_id, ...
+      '%u64 %s %f %f %f %f %f %f %s %f %f %f %f %f %f','Delimiter',',');
+  fclose(ins_file_id);
+
+  ins_timestamps = ins_data{1};
+
+  lower_index = max(find(ins_timestamps(:,1)<=min(pose_timestamps), 1, ...
+      'last')-1,1);
+  upper_index = find(ins_timestamps(:,1)>max(pose_timestamps), 1, 'first')+1;
+
+  lower_index = max(lower_index, 1);
+  upper_index = min(upper_index, numel(ins_timestamps));
+
+  ins_poses = cell(1, upper_index - lower_index + 1);
+  ins_quaternions = cell(1, upper_index - lower_index + 1);
+
+  northings = ins_data{6};
+  eastings = ins_data{7};
+  downs = ins_data{8};
+  rolls = ins_data{13};
+  pitches = ins_data{14};
+  yaws = ins_data{15};
+
+  pose_timestamps = [origin_timestamp pose_timestamps];
+
+  for i=1:upper_index
+    ins_poses{i} = SE3MatrixFromComponents(...
+      northings(i), eastings(i), downs(i), rolls(i), pitches(i), yaws(i));
+    ins_quaternions{i} = SO3ToQuaternion(ins_poses{i}(1:3,1:3))';
+  end
+
+  [lower_index_rows, lower_index_cols] = ...
+    find(and(bsxfun(@le,ins_timestamps,pose_timestamps)',...
+    circshift(bsxfun(@gt,ins_timestamps,pose_timestamps)',[0 -1])));
+
+  lower_indices = zeros(size(lower_index_rows));
+  lower_indices(lower_index_rows) = lower_index_cols;
+  lower_indices = max(lower_indices, 1);
+
+  ins_timestamps = cast(ins_timestamps, 'double');
+  pose_timestamps = cast(pose_timestamps, 'double');
+  fractions = (pose_timestamps - ins_timestamps(lower_indices)')./...
+    (ins_timestamps(lower_indices+1)'-ins_timestamps(lower_indices)');
+
+  quaternions_lower = [ins_quaternions{lower_indices}];
+  quaternions_upper = [ins_quaternions{lower_indices+1}];
+
+  % interpolate quaternions
+  d_array = sum(quaternions_lower.*quaternions_upper,1);
+
+  linear_interp_indices = find(~(abs(d_array)<1.0));
+  sin_interp_indices = find(abs(d_array)<1.0);
+
+  scale0_array = zeros(size(d_array));
+  scale1_array = zeros(size(d_array));
+
+  scale0_array(linear_interp_indices) = 1-fractions(linear_interp_indices);
+  scale1_array(linear_interp_indices) = fractions(linear_interp_indices);
+
+  theta_array = acos(abs(d_array(sin_interp_indices)));
+
+  scale0_array(sin_interp_indices) = sin((1-fractions(sin_interp_indices)).*...
+      theta_array)./sin(theta_array);
+  scale1_array(sin_interp_indices) = sin(fractions(sin_interp_indices).*...
+      theta_array)./sin(theta_array);
+
+  negative_d_indices = find(d_array < 0);
+  scale1_array(negative_d_indices) = -scale1_array(negative_d_indices);
+
+  quaternions_interp = repmat(scale0_array,4,1).*quaternions_lower + ...
+      repmat(scale1_array,4,1).*quaternions_upper;
+
+  % interpolate positions
+  ins_poses_array = [ins_poses{:}];
+  positions_lower = ins_poses_array(1:3,4*lower_indices);
+  positions_upper = ins_poses_array(1:3,4*(lower_indices+1));
+  positions_interp = repmat((1-fractions),3,1).*positions_lower + ...
+      repmat(fractions,3,1).*positions_upper;
+
+  poses_array = zeros(4, 4*numel(pose_timestamps));
+  poses_array(1,1:4:end) = 1-2*quaternions_interp(3,:).^2 ...
+      -2*quaternions_interp(4,:).^2;
+  poses_array(1,2:4:end) = 2*quaternions_interp(2,:).*...
+    quaternions_interp(3,:) - 2*quaternions_interp(4,:).*...
+    quaternions_interp(1,:);
+  poses_array(1,3:4:end) = 2*quaternions_interp(2,:).*...
+      quaternions_interp(4,:) + 2*quaternions_interp(3,:).*...
+      quaternions_interp(1,:);
+
+  poses_array(2,1:4:end) = 2*quaternions_interp(2,:).*...
+      quaternions_interp(3,:) + 2*quaternions_interp(4,:).*...
+      quaternions_interp(1,:);
+  poses_array(2,2:4:end) = 1-2*quaternions_interp(2,:).^2 ...
+      -2*quaternions_interp(4,:).^2;
+  poses_array(2,3:4:end) = 2*quaternions_interp(3,:).*...
+      quaternions_interp(4,:) - 2*quaternions_interp(2,:).*...
+      quaternions_interp(1,:);
+
+  poses_array(3,1:4:end) = 2*quaternions_interp(2,:).*...
+      quaternions_interp(4,:) - 2*quaternions_interp(3,:).*...
+      quaternions_interp(1,:);
+  poses_array(3,2:4:end) = 2*quaternions_interp(3,:).*...
+      quaternions_interp(4,:) + 2*quaternions_interp(2,:).*...
+      quaternions_interp(1,:);
+  poses_array(3,3:4:end) = 1-2*quaternions_interp(2,:).^2 ...
+      -2*quaternions_interp(3,:).^2;
+
+  poses_array(1:3,4:4:end) = positions_interp;
+  poses_array(4,4:4:end) = 1;
+  poses_array = poses_array(1:4,1:4) \ poses_array;
+
+  poses = cell(numel(pose_timestamps)-1,1);
+  for i=2:numel(pose_timestamps)
+    poses{i-1} = poses_array(:,(i-1)*4+1:i*4);
+  end
+
+end

matlab/LoadImage.m

@@ -0,0 +1,63 @@
+function [image] = LoadImage(directory, timestamp, LUT)
+
+% LoadImage - load a rectified image from disk
+%
+% [image] = LoadImage(directory, timestamp, LUT)
+%
+% eg.
+% timestamps = dlmread('<dataset_root>/stereo.timestamps');
+% [ ~, ~, ~, ~, ~, LUT] = ...
+%     ReadCameraModel('<models_dir>/stereo_wide_left_undistortion.bin');
+% image = LoadImage('<dataset_root>/stereo/left', timestamps(100,1), LUT);
+%
+% INPUTS:
+%   directory: directory containing images named <timestamp>.png
+%   timestamp: timestamp of image to load
+%   LUT (optional): lookup table for image rectification, as returned from 
+%     ReadCameraModel. If not supplied, original distorted image will be 
+%     returned.
+%     See ReadCameraModel and UndistortImage
+%   
+% OUTPUTS:
+%   image: image at the given timestamp. If an undistortion lookup table is
+%     supplied, image will be undistorted.
+%
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% Copyright (c) 2016 University of Oxford
+% Authors: 
+%  Geoff Pascoe ([email protected])
+%  Will Maddern ([email protected])
+%
+% 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.
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+  if directory(end) ~= '/'
+    directory = [directory '/'];
+  end
+
+  path = [directory num2str(timestamp) '.png'];
+  if ~exist(path, 'file')
+    image = false;
+    return;
+  end
+
+  if regexp(directory, 'stereo')
+    bayer_pattern = 'gbrg';
+  else
+    bayer_pattern = 'rggb';
+  end
+
+  image = demosaic(imread(path), bayer_pattern);
+
+  if exist('LUT', 'var')
+    image = UndistortImage(image, LUT);
+  end
+
+end