Exercise 7, Problem 5

Make a function line=lsqline(points) that outputs the estimated (alfa, r) for the line.

%% lsqline.m

function line = lsqline(points) %returns (alpha, r) from a set of points
Xs = points(1,:);
Ys = points(2,:);
sumXs = sum(Xs);
sumYs = sum(Ys);
sumXY = Xs*Ys';
lgnth = length(Xs);
sumXsq = Xs * Xs';
sumYsq = Ys * Ys';
alpha = 0.5 * atan2( (2*sumXs*sumYs-2*lgnth*sumXY) , (sumXs^2-sumYs^2-lgnth*sumXsq+lgnth*sumYsq) );
rsX = (1/lgnth) * sumXs;
rsY = (1/lgnth) * sumYs;
r = rsX*cos(alpha) + rsY*sin(alpha);
if r < 0
    r = -1 * r;
    alpha = alpha - sign(alpha) * pi;
end
line = [alpha r];
end

Testing with the lines x=5, y=3, x+y+3=0:

scannerPose = [0 0 0];
y1 = -10:1:10; x1 =  x1*0+5;
x2 = -10:1:10; y2 =  x2*0+3;
x3 = -10:1:10; y3 = -x3-3;
line1 = cat(1,x1,y1);
line2 = cat(1,x2,y2);
line3 = cat(1,x3,y3);
estimated1 = lsqline(line1)
estimated2 = lsqline(line2)
estimated3 = lsqline(line3)
figure(6);hold on;title('Exercise 7 Problem 5')
plot(line1(1,:),line1(2,:),'o', [scannerPose(1) estimated1(2)*cos(estimated1(1))],[scannerPose(2) estimated1(2)*sin(estimated1(1))],'LineWidth',2,'Color','m');
plot(line2(1,:),line2(2,:),'o', [scannerPose(1) estimated2(2)*cos(estimated2(1))],[scannerPose(2) estimated2(2)*sin(estimated2(1))],'LineWidth',2,'Color','b');
plot(line3(1,:),line3(2,:),'o', [scannerPose(1) estimated3(2)*cos(estimated3(1))],[scannerPose(2) estimated3(2)*sin(estimated3(1))],'LineWidth',2,'Color','r');
grid on; axis equal; axis tight;

estimatedx =    alpha      r
estimated1 =     0           5
estimated2 =    1.5708    3.0000
estimated3 =   -2.3562    2.1213