weightedleastsquare.m

weighted least square line extraction with original sensor data

function g=weightedLeastSquare
clc;
clear all;
data = load('laserdata_X-Y.txt');
x1 = data(1:76,:);
x2 = data(77:119,:);
x3 = data(120:181,:);
[a1,b1] = weightedLS(x1);
[a2,b2] = weightedLS(x2);
[a3,b3] = weightedLS(x3);
figure
hold on
axis([-2000 2000 -100 2500])
plot(data(:,1),data(:,2),'b.')
plot(a1,b1,'r-',a2,b2,'r-',a3,b3,'r-')
title('Weighted Least Square line extraction');      


function [a,b] = weightedLS(x)
% line extraction based on weighted least square method
% weight is calculated by the least square result error variance 
% (x,y) is the input data point in in Cartesian coordinates

x1=x(:,1);
y1=x(:,2);
n = size(x,1);
x = x1;
y = y1;

w=ones(n,1);% weight is 1, lease square method

%------calculate alpha and rho by weighted least square method--------
% there are 2 cycles, first cycle weight is set to 1 to calculate distance
% from erery point to line, second cycle is weighted lease square.of
% course, we can set more cycle to improve the accuracy.
for i=1:5 
    
    [theta,rho] = cart2pol(x,y);

    A = zeros(n,1);
    B = zeros(n,1);
    for i = 1:n
        A(i) = w(i)*rho(i)*rho(i)*sin(2*theta(i));
        B(i) = w(i)*rho(i)*rho(i)*cos(2*theta(i));
    end

    C = zeros(n,1);
    D = zeros(n,1);
    for i = 1:n
        for j = 1:n
            C(i) = C(i)+w(i)*w(j)*rho(i)*rho(j)*cos(theta(i))*sin(theta(j));
            D(i) = D(i)+w(i)*w(j)*rho(i)*rho(j)*cos(theta(i)+theta(j));
        end
    end

    E = sum(A)-2*sum(C)./sum(w);
    F = sum(B)-sum(D)./sum(w);
    alpha = atan2(E,F)./2;
    alpha = pi./2+alpha;
    
    for i = 1:n
        H(i) = w(i)*rho(i)*cos(theta(i)-alpha);
    end
    r = sum(H)./sum(w);
    
    for i = 1:n
        distance(i) = abs(rho(i)*cos(theta(i)-alpha)-r);
        distance(i) = distance(i)*distance(i);
    end
    w = 1./distance;
end
r
alpha
%------extracted line data in Cartesian coordinates-------------------

theta1 = (min(theta)-1):0.01:(max(theta)+1);
rho1 = r./cos(theta1-alpha);
[a,b] = pol2cart(theta1,rho1);