initial commit

README.MD

@@ -0,0 +1,11 @@
+# Usage
+
+Require MATLAB
+
+Run `lidar_pipeline.m` to show the setup of the problem
+
+# Question
+
+Problem I am trying to solve is stated in `lidar_pipeline.m` line 19. 
+
+You may wanna look at `project3to2.m` and `projectPoints.m`.

coord_trans.m

@@ -0,0 +1,38 @@
+function points_trans = coord_trans(points)
+%COORD_TRANS Summary of this function goes here
+%   Detailed explanation goes here
+% Lidar pose w.r.t Global, G2L: Global to Lidar
+d2r = pi/180;
+tr_vec_G2L =[-0.15, -22.1, 6.5];
+eul_angles_G2L = [-50.0, 25.0, 120.0] * d2r;
+
+H_G2L = homotrafo(eul_angles_G2L, tr_vec_G2L);
+%H_L2G = inv(H_G2L);
+
+% Stereo Unit w.r.t Global, G2S: Global to Stereo
+tr_vec_G2S = [-0.15,-22.2,6.3];
+eul_angles_G2S = [-50.0,15.0,120.0] * d2r;
+H_G2S = homotrafo(eul_angles_G2S, tr_vec_G2S);
+
+% Left camera w.r.t to Stereo Unit, S2L: Stereo to Left Camera
+
+tr_vec_S2LC = [0.10, 0.20, 0.0];
+eul_angles_S2LC = [0.0, 0.0, 0.0];
+H_S2LC = homotrafo(eul_angles_S2LC, tr_vec_S2LC);
+
+% Transformation from Lidar to Left Camera
+H_L2LC = H_G2L \ H_G2S * H_S2LC;
+
+XYZ = points(:, 1:3);
+range = points(:, 4);
+%XYZ = lidar_bg_xyz
+rows = size(XYZ,1);
+XYZ_aug = [XYZ,ones(rows,1)];
+XYZ_aug_t = XYZ_aug';
+XYZ_aug_t_cam = inv(H_L2LC) * XYZ_aug_t; %%
+XYZ1_aug_cam = XYZ_aug_t_cam';
+XYZ_cam = XYZ1_aug_cam(:,1:3);
+points_trans = [XYZ_cam, range];
+
+end
+

edge_points.m

@@ -0,0 +1,40 @@
+function points_edge = edge_points(points)
+%EDGE_POINTS Summary of this function goes here
+%   Detailed explanation goes here
+
+pc_xyz = points(:,1:3);
+pc_range = points(:, 4);
+% figure;
+% pcshow(pc(:,1:3));
+layers_range = reshape(pc_range, [size(pc_range, 1)/8, 8]);
+
+% range difference for each point compared to its previous point and next point
+prev = abs(layers_range - shift_down(layers_range));
+next = abs(layers_range - shift_up(layers_range));
+
+% remove first & last row
+prev = prev(2:end-1,:);
+next = next(2:end-1,:);
+
+% get the discountinuity values for each lidar point
+%layers_range = layers_range(2:end-1,:);
+X_val = abs(prev - next)./(prev + next);
+% normalize vector
+X_val = mat2gray(X_val);
+
+% cut out first and last point in each layer in xyz list
+pc_xyz = reshape(pc_xyz, [size(pc_xyz, 1)/8, 8, 3]);
+pc_xyz = pc_xyz(2:end-1, :, :);
+pc_xyz = reshape(pc_xyz, [size(pc_xyz, 1) * size(pc_xyz, 2), 3]);
+
+% attach discontinuity value to xyz points
+X_val = reshape(X_val, [], 1);
+pc = [pc_xyz, X_val];
+
+% filter points by: discontinuity > threshold
+threshold = 0.4;
+pc_filtered = pc(pc(:, 4) > threshold, :);
+points_edge = pc_filtered(any(pc_filtered(:, 1:3),2),:);
+
+end
+

homotrafo.m

@@ -0,0 +1,11 @@
+function H = homotrafo(eul_angles, tr_vec)
+eul_x = eul_angles(1);
+eul_y = eul_angles(2);
+eul_z = eul_angles(3);
+rot_x = [1 0 0;0 cos(eul_x) -sin(eul_x);0 sin(eul_x) cos(eul_x)];% rotation matrix respect to x
+rot_y = [cos(eul_y) 0 sin(eul_y);0 1 0;-sin(eul_y) 0 cos(eul_y)];% rotation matrix respect to y
+rot_z = [cos(eul_z) -sin(eul_z) 0;sin(eul_z) cos(eul_z) 0;0 0 1];% rotation matrix respect to z
+rot = rot_z * rot_y * rot_x;% rotation matrix according to the ZYX euler angle
+%rot = eul2rotm(eul_angles);
+H = rotm2tform(rot);% from rotation matrix to homogenous transformation matrix
+H(1:3,4)=tr_vec';% add transition vector to homogenous transformation matrix

lidar_pipeline.m

@@ -0,0 +1,44 @@
+points_raw = csvread('1.csv');
+d2r = pi/180;
+eul_angles_G2LC =[-50.0*d2r,15.0*d2r,120.0*d2r]; % stereo camera or left camera w.r.t global coord
+tr_vec_G2LC = [-0.28,-22.2150,6.1261];
+% eul_angles = [0.0,0.0,0.0]*d2r;
+
+% transfrom raw lidar points into camera's coordinate system
+
+% points_trans = coord_trans(points_raw);
+points_trans = trans_global(points_raw);
+pcshow(points_trans(:, 1:3))
+hold on;
+% rot_G2LC = rottrafo(eul_angles_G2LC);
+plotCamera('Location',tr_vec_G2LC,'Orientation', angle2dcm(50.0*d2r,-15.0*d2r,-120.0*d2r),'Opacity',0);
+% angle2dcm(-50.0*d2r,15.0*d2r,120.0*d2r)
+plot3(-0.28,-22.2150,6.1261,'b*');
+plot3(-7.8515,-10.0,1.1459279, 'r*') % the local of the car
+hold off;
+
+% set(gca,'CameraUpVector',[0 0 -1]);
+
+% preprocess lidar points to get edge points
+points_edge = edge_points(points_trans);
+
+figure
+pcshow(points_edge(:, 1:3))
+hold on
+cam = plotCamera('Location',tr_vec_G2LC,'Orientation', angle2dcm(50.0*d2r,-15.0*d2r,-120.0*d2r),'Opacity',0);
+% angle2dcm(-50.0*d2r,15.0*d2r,120.0*d2r)
+plot3(-0.28,-22.2150,6.1261,'b*');
+plot3(-7.8515,-10.0,1.1459279, 'r*') % the local of the car
+hold off
+
+% project 3d points to 2d
+points_2d = project3to2(points_edge);
+
+%show the projection
+figure;
+scatter(points_2d(:,1),points_2d(:,2), 10, points_2d(:,3));
+xlim([0 256]);
+ylim([0 256]);
+axis equal;
+title('Points projected with camera model');
+

project3to2.m

@@ -0,0 +1,29 @@
+function points_2d = project3to2(points_3d)
+%project3to2 project 3d points to 2d points
+%   Detailed explanation goes here
+% setup camera with focal length 280, centre 128,128
+cam = [280,0,128;0,280,128;0,0,1];
+
+%setup image
+imageSize = [256,256];
+
+% create a tform matrix
+% angles = [45,45,45]*pi/180;
+% position = [0,0,0];
+angles = [-50.0,15.0,120.0]*pi/180;
+% angles = [-5.0,60.0,165.0]*pi/180;
+position = [-0.28,-22.2150,6.1261];
+
+tform = eye(4);
+tform(1:3,1:3) = angle2dcm(angles(1),angles(2),angles(3));
+tform(1:3,4) = position;
+
+%add a little lens distortion
+dist = [0.0,0.000];
+
+%project the points into image coordinates
+[projected, valid] = projectPoints(points_3d, cam, tform, dist, imageSize, true);
+points_2d = projected(valid,:); % ??
+% points_2d = projected;
+end
+

projectPoints.m

@@ -0,0 +1,178 @@
+function [ projected, valid ] = projectPoints( points, K, T, D, imageSize, sortPoints )
+%PROJECTPOINTS Projects 3d points onto a plane using a camera model.
+%   Applies a standard pin-point camera model with optional distortion
+%   parameters to the points.
+%--------------------------------------------------------------------------
+%   Required Inputs:
+%--------------------------------------------------------------------------
+%   points- nxm set of 3D points, the first 3 columns are the point (x,y,z)
+%       locations and the remaining columns the intensity values 
+%       (i1,i2,etc)
+%   K- either a 3x3 or 3x4 camera matrix
+%
+%--------------------------------------------------------------------------
+%   Optional Inputs: (will give default values on empty array [])
+%--------------------------------------------------------------------------
+%   T- 4x4 transformation matrix to apply to points before projecting them
+%       (default identity matrix)
+%   D- 1xq distortion vector that matches opencv's implementation 
+%       [k1,k2,p1,p2,k3], q can be from 1 to 5 and any values not given are
+%       set to 0 (default [0,0,0,0,0])
+%   imageSize- 1x2 vector that gives the size of image (y,x) the points
+%       will be projected onto, if it is given points that are outside this
+%       size or negitive will be treated as invalid (default [])
+%   sortPoints- 1x1 binary value, if true points are sorted by their
+%       distance (furthest away first) from the camera (useful for 
+%       rendering points), (default false)
+%
+%--------------------------------------------------------------------------
+%   Outputs:
+%--------------------------------------------------------------------------
+%   projected- nx(m-1) set of 2D points, the first 2 columns give the
+%       points (x,y) location and the remaining columns the intensity
+%       values (i1,i2,etc). Points behind the camera are assigned nan as
+%       their position.
+%   valid- nx1 binary vector, true if a 2d point is in front of the camera
+%       and projects onto a plane of size imageSize.
+%
+%--------------------------------------------------------------------------
+%   References:
+%--------------------------------------------------------------------------
+%   The equations used in this implementation were taken from
+%   http://docs.opencv.org/doc/tutorials/calib3d/camera_calibration/camera_calibration.html
+%   http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html
+%
+%   This code is a slightly more user friendly version of the code I used
+%   in generating results for the conference paper Motion-Based Calibration
+%   of Multimodal Sensor Arrays 
+%   http://www.zjtaylor.com/welcome/download_pdf?pdf=ICRA2015.pdf as well
+%   as several of my other publications.
+%
+%   This code was written by Zachary Taylor
+%   zacharyjeremytaylor@gmail.com
+%   http://www.zjtaylor.com
+
+%% check inputs
+
+validateattributes(points, {'numeric'},{'2d'});
+if(size(points,2) < 3)
+    error('points must have at least 3 columns, currently has %i',size(points,2));
+end
+points = double(points);
+
+if(size(K,2) == 3)
+    K(end,4) = 0;
+end
+validateattributes(K, {'numeric'},{'size',[3,4]});
+K = double(K);
+
+if(nargin < 3)
+    T = [];
+end
+if(isempty(T))
+    T = eye(4);
+else
+    validateattributes(T, {'numeric'},{'size',[4,4]});
+end
+T = double(T);
+
+if(nargin < 4)
+    D = [];
+end
+if(isempty(D))
+    D = [0,0,0,0,0];
+else
+    validateattributes(D, {'numeric'},{'nrows',1});
+end
+if(size(D,2) > 5)
+    error('distortion vector D, must have 5 or less columns currently has %i',size(D,2));
+end
+D = double(D);
+
+if(nargin < 5)
+    imageSize = [];
+end
+if(~isempty(imageSize))
+    validateattributes(imageSize, {'numeric'},{'size',[1,2],'positive'});
+end
+
+if(nargin < 6)
+    sortPoints = [];
+end
+if(isempty(sortPoints))
+    sortPoints = false;
+else
+    validateattributes(sortPoints, {'logical'},{'scalar'});
+end
+
+%% project points
+
+%split distortion into radial and tangential
+if(size(D,2) < 5)
+    D(1,5) = 0;
+end
+k = [D(1),D(2),D(5)];
+p = [D(3),D(4)];
+
+%split into point locations and colour
+if(size(points,2) > 3)
+    colour = points(:,4:end);
+    points = points(:,1:3);
+else
+    colour = zeros(size(points,1),0);
+end
+
+%transform points
+points = (T*[points,ones(size(points,1),1)]')';
+
+if(sortPoints)
+    %sort points by distance from camera
+    dist = sum(points(:,1:3).^2,2);
+    [~,idx] = sort(dist,'descend');
+    points = points(idx,:);
+    colour = colour(idx,:);
+end
+
+%reject points behind camera
+valid = points(:,3) > 0;
+points = points(valid,:);
+colour = colour(valid,:);
+
+%project onto a plane using normalized image coordinates
+x = points(:,1)./points(:,3);
+y = points(:,2)./points(:,3);
+
+%find radial distance
+r2 = x.^2 + y.^2;
+
+%find tangential distortion
+xTD = 2*p(1)*x.*y + p(2).*(r2 + 2*x.^2);
+yTD = p(1)*(r2 + 2*y.^2) + 2*p(2)*x.*y;
+
+%find radial distortion
+xRD = x.*(1 + k(1)*r2 + k(2)*r2.^2 + k(3)*r2.^3); 
+yRD = y.*(1 + k(1)*r2 + k(2)*r2.^2 + k(3)*r2.^3); 
+
+%combine distorted points
+x = xRD + xTD;
+y = yRD + yTD;
+
+%project distorted points back into 3D
+points = [x,y,ones(size(x,1),1)].*repmat(points(:,3),1,3);
+
+%project using camera matrix
+points = (K*[points, ones(size(points,1),1)]')';
+points = points(:,1:2)./repmat(points(:,3),1,2);
+
+%output nans for points behind camera
+projected = nan(size(valid,1),2+size(colour,2));
+projected(valid,:) = [points,colour];
+
+%set points outside of the image region as invalid
+if(~isempty(imageSize))
+    inside = points(:,1) < imageSize(2);
+    inside = and(inside, points(:,2) < imageSize(1));
+    inside = and(inside, points(:,1) >= 0);
+    inside = and(inside, points(:,2) >= 0);
+    valid(valid) = inside;
+end

rottrafo.m

@@ -0,0 +1,8 @@
+function rot = rottrafo(eul_angles)
+eul_x = eul_angles(1);
+eul_y = eul_angles(2);
+eul_z = eul_angles(3);
+rot_x = [1 0 0;0 cos(eul_x) -sin(eul_x);0 sin(eul_x) cos(eul_x)];% rotation matrix respect to x
+rot_y = [cos(eul_y) 0 sin(eul_y);0 1 0;-sin(eul_y) 0 cos(eul_y)];% rotation matrix respect to y
+rot_z = [cos(eul_z) -sin(eul_z) 0;sin(eul_z) cos(eul_z) 0;0 0 1];% rotation matrix respect to z
+rot = rot_z * rot_y * rot_x;% rotation matrix according to the ZYX euler angle

shift_down.m

@@ -0,0 +1,6 @@
+function matrix_out = shift_down(matrix_in)
+    n = size(matrix_in, 2);
+    add_on = zeros(1, n);
+    matrix_out = [add_on; matrix_in];
+    matrix_out = matrix_out(1:end-1, :);
+end

shift_up.m

@@ -0,0 +1,6 @@
+function matrix_out = shift_up(matrix_in)
+    n = size(matrix_in, 2);
+    add_on = zeros(1, n);
+    matrix_out = [matrix_in; add_on];
+    matrix_out = matrix_out(2:end, :);
+end