st.statistics - Regression problem/detect outliers

RANSAC can be used to find the best fit for data which includes outliers.

---

Below is what I did to solve a line fitting problem in robotic vision using RANSAC. Dr. Mariottini was the professor who taught us the approach and provided some templates for implementation. The credit goes to him and not me. The code is for MATLAB, and the reference to Z.mat is for a file containing the detected points along the wall to be followed which also contains outliers to be excluded.

%
%%
%% CSE 4392-5369 University of Texas at Arlington  
%% Dr. Gian Luca Mariottini
%%
%% 

% Robust line fitting (using RANSAC)
close all
clear all
clc

load Z.mat

X = Z;

figure(1)
axis equal
plot(X(1,:), X(2,:), 'r+')
hold on
len=length(X(1,:));
maxp=max(X(1,:));


%% RANSAC
t=1;
T=6;
N=100;
[m_r,q_r,inliers] = f_fitlinerobust(X',t,T,N);
plot([1 maxp],[m_r*1+q_r, m_r*maxp+q_r],'b');
plot(X(1,inliers), X(2,inliers),'gO');


% Wall parameters
theta=atan2(m_r,-1);
rho=q_r * sin(theta);
m=-cos(theta)/sin(theta);
q=rho/sin(theta);
x_w=[-2,20]';
y_w=m*x_w+q;
figure(2)
hold on
plot(x_w,y_w,'r')
axis equal

% Initial robot pose
x_r(1)=0.5;
y_r(1)=7;
phi_r(1)=0%pi/100;
Dt=0.1;
% Desired pose
delta_des=2;

i=1;
for t=0:150;

    figure(1)
    hold on
    plot(x_w,y_w,'r')
    axis equal
    % Measurement model
    delta(i) = x_r(i)*cos(theta)+y_r(i)*sin(theta)-rho;% + randn(1)*0.1;
    alpha(i) = theta - phi_r(i);%+randn(1)*0.01;

    % Plot reference frame
    w_R_r = rotoz(-(pi/2-phi_r(i)));
    w_t_r = [x_r(i); y_r(i);0];
    H=[w_R_r w_t_r;
       0 0 0   1];
    f_3Dframe(H,'b',2,'_{r}');

% Control
    err([1:2],i)=[delta(i)-delta_des; -alpha(i)+pi/2];
    lambda_1=0.3;
    lambda_2=0.3;
    L=[lambda_1 0;
        0 lambda_2];
    A=[1/cos(alpha(i)) 0;
        0              1];
    U_r([1:2],i) = -A*L*(err(:,i));

%% Velocities    
v_r(i) = U_r(1,i);
w_r(i) = U_r(2,i);

    % Actuate velocities
    x_r(i+1) = x_r(i) + v_r(i)*cos(phi_r(i));
    y_r(i+1) = y_r(i) + v_r(i)*sin(phi_r(i));
    phi_r(i+1) = phi_r(i) + w_r(i);

    i=i+1;
    pause(Dt)
    clf

    end

figure
plot(err(1,:))
title('Error distance')

%figure
%plot(err(2,:))
%title('Error angle')

figure
hold on
plot(v_r,'g')
plot(w_r,'r');
title('Translational/Angular velocities')

%%
%% CSE 4392-5369 University of Texas at Arlington  
%% Dr. Gian Luca Mariottini
%%
%% See the RANSAC alg. in the slides for the parameters U, t, T and N
function [m,q,inliers] = f_fitlinerobust(U,t,T,N);

% Create a list of possible combinations of the input points (2 at a time)
n=length(U(:,1));
inliers=[];
%indexes=nchoosek([1:n], 2); % all possible pair combinations 

max_length=0;
for i=1:N,
    display(['Iteration=',int2str(i)]);
    % (i) Sample minimum number of points
    ind1=randsample(n,1);
    x1=U(ind1,1);
    y1=U(ind1,2);
    ind2=randsample(n,1);
    x2=U(ind2,1);
    y2=U(ind2,2);
    % Fit a model to these points
    [m,q]=f_fitline([x1 x2; 
                     y1 y2]);
    % (ii) Compute the distance of each point to the model (m,q) and find S
    distances = abs( m*U(:,1)+q - U(:,2) );
    indS=find(distances < t); %index of the Consensus set= inliers
    if ~isempty(indS),
      % Take the current consensus index and see if it is > than maximum so far
      if length(indS)>max_length,
        max_ind = indS;
        max_length= length(indS);
      end;
      % (iii) size(S)>T
      if length(indS)>T,
         % Final estimate
         [m,q]=f_fitline(U(indS,:));
         inliers=max_ind;
         break;
      else
          if i==N,
             [m,q]=f_fitline(U(max_ind,:)); 
             inliers=max_ind;
          end
      end
      indS=[];
    end
end
if isempty(inliers),
    display('Inlier set is empty!');
end