Lines intersections in 3D (MATLAB)

lines.m

%%%%%%%%% 
% Generate some test data, here 4 points p with directions d

% the position of the offset of the lines
p=[0 0; 1 0; 0 1; 1 1]';

% the direction for the individual lines, not sure if this is the format that is needed or if you have the rotation matrix.
% If the latter then simply multiply take the first row of the rotation matrix as d_i
d=[1 1; -1 1; 1 -1; -1 -1]'; 

% add some noise to direction to make realistic
d=d+(randn(size(d)))*0.05;

% compute length of all d vectors (only for normalisation, not needed for anything really)
d_l = sqrt(d(1,:).^2+d(2,:).^2);
% normalise d (this is just for illustration, the length doesn't matter obviously, but this makes it look like it came from a rotation matrix)
d=d./repmat(d_l,size(d,1),1);



% OK, now we have some good test data to work with
%%%%%%%%%
% First make everything using homogeneous coordinates, so it's easier to work with!
d(3,:)=0

p(3,:)=1

% We will represent a line in the most general form: ax + by + c = 0, with a and b being the coefficients that define the line, 
% and x and y being the variables. e.g. 

l=cross(p, p+d)


% this can also be computed (manually:)
lm(1,:)=p(2,:).*(p(3,:)+d(3,:)) - p(3,:).*(p(2,:)+d(2,:));

% so a is simply -d(2,:):
lm(1,:)=-d(2,:);

% so b is simply d(1,:):
lm(2,:)=-p(1,:).*(p(3,:)+d(3,:)) + p(3,:).*(p(1,:)+d(1,:));
lm(2,:)=d(1,:);

% c is a bit complicated as it's the linear combination:
lm(3,:)=p(1,:).*(p(2,:)+d(2,:)) - p(2,:).*(p(1,:)+d(1,:));
lm(3,:)=p(1,:).*p(2,:) + p(1,:).*d(2,:) - p(2,:).*p(1,:) - p(2,:).*d(1,:);

% in the end, l is the same as lm, and a good representation of the lines, as now all we have to do is solve the linear system:

%simple for two lines, simply the cross product of the general lines representation:

intersection=[];


% no find the intersection between all possible combinations
combinations = nchoosek(1:size(l,2),2);
for i=1:size(combinations,1)
	intersection(:,i)=cross(l(:,combinations(i,1)),l(:,combinations(i,2)));
	% alternatively, using the inverse matrix:
	alt_intersection(:,i)=inv(l(1:2,combinations(i,:))') * -l(3,combinations(i,:))';
end

% de-homogenise the coordinates:
intersection=intersection(1:2,:)./repmat(intersection(3,:),2,1)
alt_intersection

% a good approximation is the median!
approx_intersection=median(intersection,2)


% using the pseudo inverse we can solve the best least-square fit, that's easy in matlab (and a pain in C++):
best_intersection=pinv(l(1:2,:)') * -l(3,:)'

% This gives us the "distance" of all intersection points from the lines:
ih=[intersection approx_intersection best_intersection];
ih(3,:)=1;
distances=l'*ih