├── README.md ├── cues2.m ├── curved_road2.m ├── measurementLikelihoodFcn3.m ├── movie.avi ├── paramLikelihoodFcn.m ├── paramStateTransitionFcn.m ├── particle_filter5.m ├── ransac2.m ├── ransaclanes2.m ├── stateTransitionFcn3.m └── tform1.mat /README.md: -------------------------------------------------------------------------------- 1 | # Lane-Detection-using-particle-filters 2 | [[Report]](https://drive.google.com/file/d/0B5MuN8e-RsTrR01ZTmp0NWhfNE0/view?usp=sharing) 3 | 4 | [[Video]](https://www.youtube.com/watch?v=arXEdqjhSaI) 5 | 6 | This is a complete project developed on MATLAB that tracks both the position and the curvature of the lane, where the car is currentlly on, in dash-cam like videos 7 | -------------------------------------------------------------------------------- /cues2.m: -------------------------------------------------------------------------------- 1 | function [ lower_map,edge_map_small, img_crop, img_tf] = cues2( img, tform1) 2 | 3 | % Crop and transform it 4 | %Crop the upper part of the image 5 | img_crop=imcrop(img,[1, 0.6*size(img,1), size(img,2), 0.4*size(img,1)]); 6 | 7 | %Sharpen image 8 | img_crop = imsharpen(img_crop,'Amount',1.8); 9 | 10 | %Transform the image - bird-eye view 11 | img_tf = imtransform(img_crop,tform1,'XData',[-199,size(img_crop,2)+200],'YData', [-500,160],'XYScale',1); 12 | 13 | %Downscale transformed image 14 | img_tf_small = imresize(img_tf,0.5); 15 | 16 | grayim = rgb2gray(img_tf); % 255 shades of Grey 17 | 18 | edge_map = zeros(size(grayim,1),size(grayim,2)); % Initialize edge map 19 | 20 | %Downscaled edge map for upper part 21 | edge_map_small = edge(rgb2gray(img_tf_small),'sobel'); 22 | 23 | % Distance of assessment, depends on image resolution 24 | % Roughly, the bigger the m, more edges are found 25 | m = 20; 26 | % Treshold of value difference 27 | % Quite robust to change in lighting, the bigger the less edges are found 28 | T = 20; 29 | 30 | for i=1:size(edge_map,1) 31 | for j=m+1:size(edge_map,2)-(m+1) % columns are the horizontal dimension 32 | 33 | b_plus = grayim(i,j) - grayim(i,j+m); 34 | b_minus = grayim(i,j) - grayim(i,j-m); 35 | 36 | if (b_plus > 0 && b_minus > 0) 37 | if (b_plus + b_minus >= T) 38 | edge_map(i,j) = 1; 39 | end 40 | end 41 | end 42 | end 43 | 44 | 45 | % Neighborhood deletion 46 | n = 250; % Maximum number of pixels in the neigborhood 47 | edge_map = bwareaopen(edge_map,n); 48 | 49 | 50 | % Hysteresis reconstuction -----------------> Thin Lines get destroyed 51 | s = 2; % Size of the structure element 52 | SE = strel('disk', s); % Structure element 53 | seed = imerode(edge_map,SE); % Erode 54 | edge_map = imreconstruct(seed,edge_map); % use Seeds to reconstruct 55 | %figure();imshow(edge_map); 56 | % figure();imshow(img_tf); 57 | 58 | %TODO 59 | % %% Color space detection 60 | % 61 | % %HSI color space, taken from internet 62 | % imhsi = rgbtohsi(im_crop); 63 | % whiteareas = imhsi(:,:,2)<0.03 & imhsi(:,:,3)>0.65; 64 | % % figure(); imshow(whiteareas); 65 | % 66 | % yellowareas = imhsi(:,:,1)>0.05 & imhsi(:,:,1)<0.17 & imhsi(:,:,2)>0.2 & imhsi(:,:,2)<0.5 & imhsi(:,:,3)>0.5; 67 | % % figure(); imshow(yellowareas); 68 | % 69 | % mask = yellowareas | whiteareas; 70 | % % figure(); imshow(mask); 71 | % 72 | % % Results 73 | % if (show_figs == true) 74 | % figure(); imshow(mask); 75 | % title('Color Edge map'); 76 | % end 77 | 78 | %% Final Edge map 79 | 80 | %Merge both edge maps 81 | y_half = round(size(edge_map,1)*0.7); 82 | lower_map = edge_map(y_half:end,:); 83 | 84 | 85 | end 86 | 87 | -------------------------------------------------------------------------------- /curved_road2.m: -------------------------------------------------------------------------------- 1 | function [pr,pl,estDiff ] = curved_road2(edge_map,est_left,est_right) 2 | 3 | 4 | 5 | est_left = round((est_left+200)/2); 6 | est_right = round((est_right+200+360)/2); 7 | 8 | estDiff = abs(est_left- est_right); 9 | 10 | % est_left =190; 11 | % est_right = 450; 12 | 13 | hist_mean_right = zeros(32,2); 14 | hist_mean_left = zeros(32,2); 15 | 16 | %Compute the histograms of the small patches 17 | intial_witdh = 160; 18 | stepSize_ver = 10; 19 | stepSize_hor = 4; 20 | 21 | for i = 1:23 22 | 23 | x_right = max(est_right-intial_witdh/2+i*stepSize_hor/2+1,1):min(est_right+intial_witdh/2-i*stepSize_hor/2,size(edge_map,2)); 24 | x_left = max(est_left-intial_witdh/2+i*stepSize_hor/2+1,1):min(est_left+intial_witdh/2-i*stepSize_hor/2,size(edge_map,2)); 25 | 26 | small_edge_map_right = edge_map(1+(i-1)*stepSize_ver:i*stepSize_ver, x_right); 27 | small_edge_map_left= edge_map(1+(i-1)*stepSize_ver:i*stepSize_ver, x_left); 28 | 29 | hist_right = sum(small_edge_map_right); 30 | hist_left = sum(small_edge_map_left); 31 | 32 | hist_mean_right(i,1) = sum(hist_right.*x_right)/sum(hist_right); 33 | hist_mean_right(i,2) = 10*(i-1); 34 | 35 | hist_mean_left(i,1) = sum(hist_left.*x_left)/sum(hist_left); 36 | hist_mean_left(i,2) = 10*(i-1); 37 | 38 | end 39 | 40 | for i = 24:32 41 | 42 | small_edge_map_right = edge_map(1+(i-1)*stepSize_ver:i*stepSize_ver, x_right); 43 | hist_right = sum(small_edge_map_right); 44 | 45 | small_edge_map_left = edge_map(1+(i-1)*stepSize_ver:i*stepSize_ver, x_left); 46 | hist_left = sum(small_edge_map_left); 47 | 48 | hist_mean_right(i,1) = sum(hist_right.*x_right)/sum(hist_right); 49 | hist_mean_right(i,2) = 10*(i-1); 50 | 51 | hist_mean_left(i,1) = sum(hist_left.*x_left)/sum(hist_left); 52 | hist_mean_left(i,2) = 10*(i-1); 53 | end 54 | 55 | 56 | 57 | %Get rid of NaN's 58 | hist_mean_right = hist_mean_right(find(~isnan(hist_mean_right(:,1))),:); 59 | hist_mean_left = hist_mean_left(find(~isnan(hist_mean_left(:,1))),:); 60 | 61 | 62 | % figure(); 63 | % imshow(edge_map); hold on; 64 | % scatter(hist_mean_right(:,1),hist_mean_right(:,2)); 65 | 66 | 67 | % [minimumX, maximumX, p] = ransaclanes2(hist_mean, ... 68 | % [input_right*ones(1,1), size(upper_map,1)]); 69 | 70 | %% Ransac 71 | [minXr, maxXr, pr] = ransaclanes2(fliplr(hist_mean_right),[]); 72 | [minXl, maxXl, pl] = ransaclanes2(hist_mean_left,[]); 73 | 74 | 75 | end 76 | -------------------------------------------------------------------------------- /measurementLikelihoodFcn3.m: -------------------------------------------------------------------------------- 1 | function likelihood = measurementLikelihoodFcn3(PF,predictParticles,measurement_right,measurement_left,varargin) 2 | 3 | likelihood = ones(PF.NumParticles,1); 4 | N1 = length(measurement_right); 5 | N2 = length(measurement_left); 6 | 7 | for i=1:PF.NumParticles 8 | particle_indexL = round(predictParticles(i,1)); 9 | particle_indexL = min(max(particle_indexL,1)+200,N2); 10 | 11 | particle_indexR = round(predictParticles(i,2)); 12 | particle_indexR = min(max(particle_indexR,1),N1); 13 | 14 | likelihood(i,1) = measurement_right(particle_indexR)+measurement_left(particle_indexL); 15 | %likelihood(i,1) = measurement_left(particle_indexL+200); 16 | %likelihood(i,1) = measurement_right(particle_indexR); 17 | end 18 | 19 | 20 | end -------------------------------------------------------------------------------- /movie.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/0zgur0/Lane_Tracking_using_Particle_Filters/9c61e5385a520da4568c1baf32c590494bf10507/movie.avi -------------------------------------------------------------------------------- /paramLikelihoodFcn.m: -------------------------------------------------------------------------------- 1 | function likelihood = paramLikelihoodFcn( PF,predictParticles,stateEst,pr,pl,varargin ) 2 | 3 | likelihood = ones(PF.NumParticles,1); 4 | 5 | est_right = (stateEst(2)+200+360)/2; 6 | 7 | % mu1 = pr(1); 8 | % mu2 = pr(2); 9 | mu3 = 512; 10 | 11 | sigma3 = 10; 12 | sigma2 = 0.1; 13 | sigma1 = 0.001; 14 | 15 | invCv = zeros(3); 16 | invCv(1,1) = 1/sigma1^2; 17 | invCv(2,2) = 1/sigma2^2; 18 | invCv(3,3) = 1/sigma3^2; 19 | 20 | for i=1:PF.NumParticles 21 | res = (predictParticles(i,:)-pr)'; 22 | likelihood(i,1) = exp(-0.5*res'*(invCv*res)); 23 | end 24 | 25 | % likelihood(:,1) = normpdf(predictParticles(:,3),mu3,sigma3); 26 | end 27 | 28 | -------------------------------------------------------------------------------- /paramStateTransitionFcn.m: -------------------------------------------------------------------------------- 1 | function predictParticles = paramStateTransitionFcn( pf,prevParticles,stateEst,varargin ) 2 | 3 | sigma1 = 0.001; 4 | sigma2 = 0.05; 5 | sigma3 = 20; 6 | 7 | 8 | N = pf.NumParticles; 9 | 10 | predictParticles = zeros(N,3); 11 | 12 | for i=1:N 13 | predictParticles(i,1) = prevParticles(i,1) + sigma1*randn(); 14 | predictParticles(i,2) = prevParticles(i,2) + sigma2*randn(); 15 | predictParticles(i,3) = prevParticles(i,3) + sigma3*randn(); 16 | end 17 | 18 | 19 | end 20 | 21 | -------------------------------------------------------------------------------- /particle_filter5.m: -------------------------------------------------------------------------------- 1 | clear; %close all;clc; 2 | %Load projective transform matrix 3 | load('tform.mat'); 4 | %load('f00123.png.mat'); 5 | 6 | %Image width 7 | img_width = 360; 8 | 9 | %SkySize 10 | sky = 0.6*404; 11 | 12 | %Image directory 13 | img_directory = 'our_data/'; 14 | 15 | %Create a particle filter object 16 | pf = robotics.ParticleFilter; 17 | pfC = robotics.ParticleFilter; 18 | 19 | %Choose resmapling policy 20 | policy = robotics.ResamplingPolicy; 21 | %policy.TriggerMethod = 'interval'; 22 | %policy.SamplingInterval = 2; 23 | pf.ResamplingPolicy = policy; 24 | %pfC.ResamplingPolicy = policy; 25 | %pfC.StateEstimationMethod = 'maxweight'; 26 | 27 | %Choose the likelihood function for particle filter 28 | pf.MeasurementLikelihoodFcn = @measurementLikelihoodFcn3; 29 | pfC.MeasurementLikelihoodFcn = @paramLikelihoodFcn; 30 | 31 | %Define the state transition function 32 | pf.StateTransitionFcn = @stateTransitionFcn3; 33 | pfC.StateTransitionFcn = @paramStateTransitionFcn; 34 | 35 | 36 | 37 | %% Initilize the particle filter 38 | mean = [img_width/4,img_width*(3/4)]; 39 | covariance = img_width/4*eye(2); 40 | initialize(pf,1000,mean,covariance); 41 | 42 | %Initilize second particle filter-pfC 43 | %[0.0007 -0.2586 422.3815]; 44 | initilParam = [0 0 422]'; 45 | initialParamCov = zeros(3); 46 | initialParamCov(1,1) = 0.001; 47 | initialParamCov(2,2) = 0.2; 48 | initialParamCov(3,3) = 10; 49 | initialize(pfC,1000,initilParam,initialParamCov); 50 | 51 | %Define counter for image writing 52 | ii= 1; 53 | 54 | %Define state Estimate 55 | stateEst =zeros(1,2); 56 | stateEstC = zeros(1,3); 57 | 58 | %Initial and last time frame 59 | iT = 835; 60 | T = 7000; 61 | 62 | %Particle filter iteration 63 | for i=iT:T %i=time stamp 64 | tic, 65 | %% Prediction - (drift and diffusion) 66 | %figure();plot(pf_Right.Particles);hold on; 67 | [statePredicted,stateCov] = predict(pf,pf.Particles,stateEst); 68 | %plot(pf_Right.Particles,'r'); 69 | 70 | %% Measurement from current image 71 | %For now measuremnet is horizontal histogram 72 | 73 | %Read the image 74 | img_name = strcat(img_directory,num2str(i),'.jpg'); 75 | img= imread(img_name); 76 | 77 | %Find a histogram for lower image 78 | [lower_map,edge_map_small,img_crop ,img_tf] = cues2( img,tform1); 79 | 80 | lower_map_right = lower_map(:,size(lower_map,2)/2:end); 81 | lower_map_left = lower_map(:,1:size(lower_map,2)/2); 82 | 83 | %measurement is histogram 84 | measurement_right = sum(lower_map_right); 85 | if norm(measurement_right) >0 86 | measurement_right = measurement_right/norm(measurement_right)+ 2/length(measurement_right); 87 | else 88 | measurement_right = (5/length(measurement_right))*ones(1,size(lower_map_right,2)); 89 | end 90 | 91 | measurement_left = sum(lower_map_left); 92 | if norm(measurement_left) >0 93 | measurement_left = measurement_left/norm(measurement_left)+ 2/length(measurement_left); 94 | else 95 | measurement_left = (5/length(measurement_left))*ones(1,size(lower_map_left,2)); 96 | end 97 | 98 | %% Prediction - pfC 99 | [statePredictedC,stateCovC] = predict(pfC,pfC.Particles,stateEstC); 100 | 101 | %% Correct weights by using measurement 102 | [stateCorrected,stateCov] = correct(pf,measurement_right,measurement_left); 103 | 104 | 105 | %% Estimate the state according to selected estimation method (default:mean) 106 | stateEst = getStateEstimate(pf); 107 | 108 | 109 | %% -------------------------------------------------------------------------------- 110 | 111 | %% Upper part - could be curved 112 | % Measurement from RANSAC 113 | [pr,pl,estDiff] = curved_road2(edge_map_small,stateEst(1),stateEst(2)); 114 | 115 | %% Correct weights by using measurement 116 | [stateCorrectedC,stateCovC] = correct(pfC,stateEst,pr,pl); 117 | 118 | %% Estimate the state according to selected estimation method (default:mean) 119 | stateEstC = getStateEstimate(pfC); 120 | 121 | %% Consistency Check 122 | % if ii==1; 123 | % previousPoly = pr; 124 | % end 125 | % [consistent, previousPoly] = consistency_check( pr,pl, previousPoly,stateEst ); 126 | % 127 | % pr = consistent*pr + (1-consistent)*previousPoly; 128 | 129 | %% Generate points on lines 130 | yr = (-400:10:250)'; 131 | xr = polyval(stateEstC,yr); 132 | 133 | yr = 2*yr; 134 | xr = 2*xr; 135 | 136 | %xr = xr - (xr(find(yr == 160))-(stateEst(2)+560)); 137 | yl = yr; 138 | xl = xr - 2*estDiff; 139 | 140 | %Find the point in the real image 141 | [uR,vR] = tforminv(tform1,xr-200,yr-500); 142 | [uL,vL] = tforminv(tform1,xl-200,yl-500); 143 | 144 | vR = vR+242; 145 | vL = vL+242; 146 | 147 | uvR = [uR,vR]; 148 | uvL = [uL,vL]; 149 | 150 | 151 | %% Visulize 152 | %Find the point in the real image 153 | [uR,vR] = tforminv(tform1,stateEst(2)+360,160); 154 | [uL,vL] = tforminv(tform1,stateEst(1),160); 155 | 156 | %Extra 2 points for drawing 157 | [uRE,vRE] = tforminv(tform1,stateEst(2)+360,-10); 158 | [uLE,vLE] = tforminv(tform1,stateEst(1),-10); 159 | 160 | %img_mark = insertShape(img,'FilledCircle',[uL,size(img,1)-10,5],'LineWidth',3, 'Color','blue'); 161 | %img_mark = insertShape(img_mark,'FilledCircle',[uR,size(img,1)-10,5],'LineWidth',3, 'Color','red'); 162 | 163 | %Draw polygon on the input image 164 | % polygon_points = [uL,vL+sky,uLE,vLE+sky,uRE,vRE+sky,uR,vR+sky]; 165 | % img_mark = insertShape(img,'FilledPolygon',polygon_points,'LineWidth',1, 'Color','green'); 166 | % 167 | % %polygon_points = [[uL,vL+sky;uLE,vLE+sky];flipud(uvL); uvR;[uRE,vRE+sky;uR,vR+sky]]; 168 | % polygon_points = [uvL; flipud(uvR)]; 169 | % polygon_points = polygon_points'; 170 | % polygon_points = polygon_points(:); 171 | % polygon_points = polygon_points'; 172 | % img_mark = insertShape(img_mark,'FilledPolygon',polygon_points,'LineWidth',1, 'Color','red'); 173 | 174 | %img_mark = insertShape(img_tf,'FilledCircle',[stateEst(1)+200,size(img_tf,1)-20,5],'LineWidth',3,'Color','red'); 175 | %img_mark = insertShape(img_mark,'FilledCircle',[[uvL;uvR],2*ones(size(uvL,1)+size(uvR,1),1)],'LineWidth',1); 176 | %imshow(img_mark); 177 | 178 | % %Image Writing 179 | % filename = [sprintf('%03d',ii) '.jpg']; 180 | % fullname = fullfile('curved2/',filename); 181 | % imwrite(img_mark,fullname) % Write out to a JPEG file (img1.jpg, img2.jpg, etc.) 182 | % ii = ii+1; 183 | toc 184 | end 185 | -------------------------------------------------------------------------------- /ransac2.m: -------------------------------------------------------------------------------- 1 | % RANSAC - Robustly fits a model to data with the RANSAC algorithm 2 | % 3 | % Usage: 4 | % 5 | % [M, inliers] = ransac(x, fittingfn, distfn, degenfn s, t, feedback, ... 6 | % maxDataTrials, maxTrials) 7 | % 8 | % Arguments: 9 | % x - Data sets to which we are seeking to fit a model M 10 | % It is assumed that x is of size [d x Npts] 11 | % where d is the dimensionality of the data and Npts is 12 | % the number of data points. 13 | % 14 | % fittingfn - Handle to a function that fits a model to s 15 | % data from x. It is assumed that the function is of the 16 | % form: 17 | % M = fittingfn(x) 18 | % Note it is possible that the fitting function can return 19 | % multiple models (for example up to 3 fundamental matrices 20 | % can be fitted to 7 matched points). In this case it is 21 | % assumed that the fitting function returns a cell array of 22 | % models. 23 | % If this function cannot fit a model it should return M as 24 | % an empty matrix. 25 | % 26 | % distfn - Handle to a function that evaluates the 27 | % distances from the model to data x. 28 | % It is assumed that the function is of the form: 29 | % [inliers, M] = distfn(M, x, t) 30 | % This function must evaluate the distances between points 31 | % and the model returning the indices of elements in x that 32 | % are inliers, that is, the points that are within distance 33 | % 't' of the model. Additionally, if M is a cell array of 34 | % possible models 'distfn' will return the model that has the 35 | % most inliers. If there is only one model this function 36 | % must still copy the model to the output. After this call M 37 | % will be a non-cell object representing only one model. 38 | % 39 | % degenfn - Handle to a function that determines whether a 40 | % set of datapoints will produce a degenerate model. 41 | % This is used to discard random samples that do not 42 | % result in useful models. 43 | % It is assumed that degenfn is a boolean function of 44 | % the form: 45 | % r = degenfn(x) 46 | % It may be that you cannot devise a test for degeneracy in 47 | % which case you should write a dummy function that always 48 | % returns a value of 1 (true) and rely on 'fittingfn' to return 49 | % an empty model should the data set be degenerate. 50 | % 51 | % s - The minimum number of samples from x required by 52 | % fittingfn to fit a model. 53 | % 54 | % t - The distance threshold between a data point and the model 55 | % used to decide whether the point is an inlier or not. 56 | % 57 | % feedback - An optional flag 0/1. If set to one the trial count and the 58 | % estimated total number of trials required is printed out at 59 | % each step. Defaults to 0. 60 | % 61 | % maxDataTrials - Maximum number of attempts to select a non-degenerate 62 | % data set. This parameter is optional and defaults to 100. 63 | % 64 | % maxTrials - Maximum number of iterations. This parameter is optional and 65 | % defaults to 1000. 66 | % 67 | % Returns: 68 | % M - The model having the greatest number of inliers. 69 | % inliers - An array of indices of the elements of x that were 70 | % the inliers for the best model. 71 | % 72 | % If no solution could be found M and inliers are both returned as empty 73 | % matrices and a warning reported. 74 | % 75 | % Note that the desired probability of choosing at least one sample free from 76 | % outliers is set at 0.99. You will need to edit the code should you wish to 77 | % change this (it should probably be a parameter) 78 | % 79 | % For an example of the use of this function see RANSACFITHOMOGRAPHY or 80 | % RANSACFITPLANE 81 | 82 | % References: 83 | % M.A. Fishler and R.C. Boles. "Random sample concensus: A paradigm 84 | % for model fitting with applications to image analysis and automated 85 | % cartography". Comm. Assoc. Comp, Mach., Vol 24, No 6, pp 381-395, 1981 86 | % 87 | % Richard Hartley and Andrew Zisserman. "Multiple View Geometry in 88 | % Computer Vision". pp 101-113. Cambridge University Press, 2001 89 | 90 | % Copyright (c) 2003-2013 Peter Kovesi 91 | % Centre for Exploration Targeting 92 | % The University of Western Australia 93 | % peter.kovesi at uwa edu au 94 | % http://www.csse.uwa.edu.au/~pk 95 | % 96 | % Permission is hereby granted, free of charge, to any person obtaining a copy 97 | % of this software and associated documentation files (the "Software"), to deal 98 | % in the Software without restriction, subject to the following conditions: 99 | % 100 | % The above copyright notice and this permission notice shall be included in 101 | % all copies or substantial portions of the Software. 102 | % 103 | % The Software is provided "as is", without warranty of any kind. 104 | % 105 | % May 2003 - Original version 106 | % February 2004 - Tidied up. 107 | % August 2005 - Specification of distfn changed to allow model fitter to 108 | % return multiple models from which the best must be selected 109 | % Sept 2006 - Random selection of data points changed to ensure duplicate 110 | % points are not selected. 111 | % February 2007 - Jordi Ferrer: Arranged warning printout. 112 | % Allow maximum trials as optional parameters. 113 | % Patch the problem when non-generated data 114 | % set is not given in the first iteration. 115 | % August 2008 - 'feedback' parameter restored to argument list and other 116 | % breaks in code introduced in last update fixed. 117 | % December 2008 - Octave compatibility mods 118 | % June 2009 - Argument 'MaxTrials' corrected to 'maxTrials'! 119 | % January 2013 - Separate code path for Octave no longer needed 120 | 121 | function [M, inliers] = ransac2(x, constraintx, fittingfn, distfn, degenfn, s, t, feedback, ... 122 | maxDataTrials, maxTrials) 123 | 124 | % Test number of parameters 125 | error ( nargchk ( 6, 10, nargin ) ); 126 | 127 | if nargin < 10; maxTrials = 1000; end; 128 | if nargin < 9; maxDataTrials = 100; end; 129 | if nargin < 8; feedback = 0; end; 130 | 131 | [rows, npts] = size(x); 132 | p = 0.99; % Desired probability of choosing at least one sample 133 | % free from outliers (probably should be a parameter) 134 | 135 | bestM = NaN; % Sentinel value allowing detection of solution failure. 136 | trialcount = 0; 137 | bestscore = 0; 138 | N = 1; % Dummy initialisation for number of trials. 139 | 140 | while N > trialcount 141 | 142 | % Select at random s datapoints to form a trial model, M. 143 | % In selecting these points we have to check that they are not in 144 | % a degenerate configuration. 145 | degenerate = 1; 146 | count = 1; 147 | while degenerate 148 | % Generate s random indicies in the range 1..npts 149 | % (If you do not have the statistics toolbox with randsample(), 150 | % use the function RANDOMSAMPLE from my webpage) 151 | if ~exist('randsample', 'file') 152 | ind = randomsample(npts, s); 153 | else 154 | ind = randsample(npts, s); 155 | end 156 | 157 | % Test that these points are not a degenerate configuration. 158 | degenerate = feval(degenfn, x(:,ind)); 159 | 160 | if ~degenerate 161 | % Fit model to this random selection of data points. 162 | % Note that M may represent a set of models that fit the data in 163 | % this case M will be a cell array of models 164 | M = feval(fittingfn, [x(:,ind),constraintx]); 165 | %M = feval(fittingfn, x(:,ind)); 166 | % Depending on your problem it might be that the only way you 167 | % can determine whether a data set is degenerate or not is to 168 | % try to fit a model and see if it succeeds. If it fails we 169 | % reset degenerate to true. 170 | if isempty(M) 171 | degenerate = 1; 172 | 173 | end 174 | end 175 | 176 | % Safeguard against being stuck in this loop forever 177 | count = count + 1; 178 | if count > maxDataTrials 179 | warning('Unable to select a nondegenerate data set'); 180 | break 181 | end 182 | end 183 | 184 | % Once we are out here we should have some kind of model... 185 | % Evaluate distances between points and model returning the indices 186 | % of elements in x that are inliers. Additionally, if M is a cell 187 | % array of possible models 'distfn' will return the model that has 188 | % the most inliers. After this call M will be a non-cell object 189 | % representing only one model. 190 | [inliers, M] = feval(distfn, M, [x,constraintx], t); 191 | 192 | % Find the number of inliers to this model. 193 | ninliers = length(inliers); 194 | 195 | if ninliers > bestscore %&& abs(M(1))<0.3 % Largest set of inliers so far... 196 | bestscore = ninliers; % Record data for this model 197 | bestinliers = inliers; 198 | bestM = M; 199 | 200 | % Update estimate of N, the number of trials to ensure we pick, 201 | % with probability p, a data set with no outliers. 202 | fracinliers = ninliers/npts; 203 | pNoOutliers = 1 - fracinliers^s; 204 | pNoOutliers = max(eps, pNoOutliers); % Avoid division by -Inf 205 | pNoOutliers = min(1-eps, pNoOutliers);% Avoid division by 0. 206 | N = log(1-p)/log(pNoOutliers); 207 | end 208 | 209 | trialcount = trialcount+1; 210 | if feedback 211 | fprintf('trial %d out of %d \r',trialcount, ceil(N)); 212 | end 213 | 214 | % Safeguard against being stuck in this loop forever 215 | if trialcount > maxTrials 216 | warning( ... 217 | sprintf('ransac reached the maximum number of %d trials',... 218 | maxTrials)); 219 | break 220 | end 221 | end 222 | 223 | if feedback, fprintf('\n'); end 224 | 225 | if ~isnan(bestM) % We got a solution 226 | M = bestM; 227 | inliers = bestinliers; 228 | else 229 | M = []; 230 | inliers = []; 231 | warning('ransac was unable to find a useful solution'); 232 | end 233 | -------------------------------------------------------------------------------- /ransaclanes2.m: -------------------------------------------------------------------------------- 1 | function [minimumX, maximumX, p] = ransaclanes2(points,constraintpoints) 2 | 3 | global minX maxX 4 | minX = min(points(:,1)); 5 | maxX = max(points(:,1)); 6 | 7 | numSample = round(size(points,1)/2); 8 | 9 | % Ransac 10 | [M, inliers] = ransac2(points', constraintpoints', @fittingfn, @distfn, @degenfn, numSample, 10, 0, 100, 2000); 11 | 12 | % disp 'Number of inliers' 13 | % size(inliers) 14 | 15 | if size(M,2)>1 16 | p = M; 17 | minimumX = min(inliers(:,1)); 18 | maximumX = min(inliers(:,2)); 19 | 20 | % Plot inliers and outliers 21 | % %figure(6); 22 | % x1 = linspace(minX-50,maxX+50); 23 | % y1 = polyval(M,x1); 24 | % plot(y1, x1, 'Color', 'blue'); 25 | % hold on 26 | % plot(points(:,2), points(:,1),'.','Color','red'); 27 | % plot(inliers(:,2), inliers(:,1),'.','Color','green'); 28 | % % legend('inliers', 'outliers'); 29 | % title('RANSAC Result'); 30 | % hold off 31 | 32 | else 33 | minimumX = 0; 34 | maximumX = 0; 35 | p = []; 36 | end 37 | 38 | %% Fitting function 39 | function [M] = fittingfn(x) 40 | M = polyfit(x(1,:),x(2,:),2); 41 | 42 | %M = mmpolyfit(x(1,:), x(2,:),2,'Point',constraintx'); 43 | 44 | 45 | 46 | %% Distance function 47 | function [inliers, M] = distfn(M, x, t) 48 | %This function must evaluate the distances between points 49 | %and the model returning the indices of elements in x that 50 | %are inliers, that is, the points that are within distance 51 | %'t' of the model 52 | M = M; 53 | x2 = linspace(min(x(1,:)),max(x(1,:)),30); 54 | y2 = polyval(M,x2); 55 | 56 | 57 | X = [x2' y2']; 58 | XI = x'; 59 | k = dsearchn(X, XI); 60 | distances = diag(pdist2(XI, X(k,:))); 61 | inliers = XI(find(distances=t),:); 63 | 64 | 65 | % Check if degenerate function 66 | function [r] = degenfn(x,M) 67 | 68 | r = 0; 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /stateTransitionFcn3.m: -------------------------------------------------------------------------------- 1 | function predictParticles = stateTransitionFcn3(pf,prevParticles,stateEst,varargin) 2 | 3 | sigma1 = 10; 4 | sigma2 = 10; 5 | 6 | thL = 300; 7 | thR = 40; 8 | 9 | N = pf.NumParticles; 10 | 11 | predictParticles = zeros(N,2); 12 | 13 | witdh = 520; 14 | 15 | if stateEst(1) > thL 16 | for i=1:N 17 | predictParticles(i,2) = prevParticles(i,1) + sigma2*randn() - 360; 18 | predictParticles(i,1) = prevParticles(i,1) + sigma1*randn() - witdh; 19 | end 20 | else 21 | for i=1:N 22 | predictParticles(i,1) = prevParticles(i,1) + sigma1*randn(); 23 | predictParticles(i,2) = prevParticles(i,2) + sigma2*randn(); 24 | end 25 | 26 | end 27 | 28 | 29 | end -------------------------------------------------------------------------------- /tform1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/0zgur0/Lane_Tracking_using_Particle_Filters/9c61e5385a520da4568c1baf32c590494bf10507/tform1.mat --------------------------------------------------------------------------------