triangulations

example.m

rng(3);  % For reproducibility
X = 150*rand(30,2)-75;
X(14,:)=[-50 60];
X(12,:)=[36 65];
% put other nodes
X(31,:)=[50 40];
X(32,:)=[53 2];
X(33,:)=[65 -42];
X(34,:)=[21 -70];
X(35,:)=[-17 -69];

n=size(X,1);

radius = 40;

D = zeros(n, n); % the Distances matrix
for i=1:n
  for j=1:n
    D(i,j) = sqrt(sum((X(i,:)-X(j,:)).^2));
  end
end
Noise = triu(1.7*randn(size(D)), 1);
Noise = Noise + Noise';
D = D + Noise;
D(D>radius) = nan;

P = cell(n,1); % the Positions array (a cell because we accept multiple possible positions)

scatter(X(:,1),X(:,2), 'bo')
text(X(:,1), X(:,2), num2str((1:n)','%d'), 'horizontal','left', 'vertical','bottom')
hold on
for i=1:n
  for j=find(D(i,:)>0)
    line([X(i,1) X(j,1)],[X(i,2) X(j,2)])
  end
end

I = [1;16;12]; % intial set of known positions
for i=I'
  P{i}=X(i,:);
end
scatter(X(I,1),X(I,2), 'filled')


delta = 3; % measure error estimation
for t=1:6
  J = neighborhood(I, D);
  [P, K, placed, aliased] = triangulates(I, J, P, D, radius, delta);

  if ~isempty(K)
    x = cell2mat(P(K));
    scatter(x(:,1),x(:,2), 'filled')
  end

  I = union(I, K);
end

neighborhood.m

function J = neighborhood(I, D)
%   returns neigbors of set I 
J=[];
for i=I(:)'
  J = union(J, setdiff(find(D(i,:)>0), I));
end

J =J(:);

triangulate.m

function P=triangulate(i, j, k, P, D, radius, delta) 
% tries to find i's position given j and k's positions and distance to them

x1 = P{j}(1,1);
y1 = P{j}(1,2);
x2 = P{k}(1,1);
y2 = P{k}(1,2);
B = (sum(P{k}(1,:).^2)-sum(P{j}(1,:).^2)-D(i,k)^2+D(i,j)^2)/2;

% retrieve x,y knowing the distances r1,r2 and j,k positions

% (x-x1)^2 + (y-y1)^2 =r1^2  (1)
% (x-x2)^2 + (y-y2)^2 =r2^2  (2)
% (2) - (1) to have a relation between x and y
% x(x2-x1) + y(y2-y1) = B
% we replace this relation in (1)

if y2-y1~=0
  C = B/(y2-y1)-y1;
  tau = (x2-x1)/(y2-y1);

  a = 1+tau^2;
  b = -2*(x1+ tau*C);
  c = x1^2+C^2-D(i,j)^2;

  discriminant = b^2-4*a*c;
  if discriminant<=0
    'warning discriminant<=0'
    [i j k]
    % skip them
    return
  end
  %avoid aligned triples? or replace with better?

  x = (sqrt(discriminant)*[1 -1] - b)/(2*a); % the 2 solutions for x
  y = (B-x*(x2-x1)) / (y2-y1);

else % j and k vertically aligned
  x = B/(x2-x1);
  D = sqrt(r1^2-(x - x1)^2);
  y = y1 + [1 -1]*D;
  x = [x x];
end



% inferring step


X = [x(1) y(1);x(2) y(2)];

if size(P{i},1)==2 % we already had 2 positions, choose the one that maches
  
  p = pdist2(X, P{i});
  [u, v] = min(p(:));
  [pi, pj] = ind2sub(size(p), v);
  % check that p(pi,pj) is lower than 2nd lowest - delta
  
  p(pi,pj)
  
  %todo 1 position if we are sure
  % else keep the 2 first or all
  
  % combine X(pi,:) and P{i}(pj,:)
  P{i} = mean([X(pi,:); P{i}(pj,:)]);
  
elseif isempty(P{i})
  P{i} = X;
end

% try inferring if possible i.e. reject one that fail to match neighborhoods

err1=0;
err2=0;

hop1 = find(D(i,:)>0);
hop2 = neighborhood(union(hop1,[i]), D)';
hop2 = setdiff(hop2, [i j k]);
hop1 = setdiff(hop1, [j k]);

for h=hop1
  if size(P{h},1)==1
    d1 = abs(norm(X(1,:) - P{h}) - D(i, h));
    d2 = abs(norm(X(2,:) - P{h}) - D(i, h));
    if d1 - d2 > delta
      err1=err1+1;
    end
    if d2 - d1 > delta
      err2=err2+1;
    end
  end
end
for h=hop2
  if size(P{h},1)==1
    if norm(X(1,:) - P{h})<=radius-delta
      err1=err1+1;
    end
    if norm(X(2,:) - P{h})<=radius-delta
      err2=err2+1;
    end
  end
end
if err1==0 && err1 < err2
  P{i}=X(1,:);
elseif err2==0 && err2 < err1
  P{i}=X(2,:);
elseif err2 ~= err1
  'warning err'
  [i j k err1 err2]
end
  

triangulates.m

function [P, K, placed, aliased ] = triangulates( I, J, P, D, radius, delta)
% multiple triangulate calls over sets J and I (already positioned nodes)
placed=0;
aliased=0;
K=[];

for i=J'
  if size(P{i},1)~=1
    vi = intersect(union(I,J), find(D(i,:)>0));
    if length(vi)>=2
      combis = nchoosek(vi, 2);
      for idx=1:size(combis,1)
        j = combis(idx,1);
        k = combis(idx,2);
        % only consider j and k with 1 pos?
        if size(P{j},1)==1 && size(P{k},1)==1
          P=triangulate(i, j, k, P, D, radius, delta);
          if size(P{i},1)>1
            aliased = aliased+1;
          elseif size(P{i},1)==1
            placed = placed+1;
            K=[K;i];
            break;
          end
        end
      end
    end
  end
end

Triangulations.md

The positions of green nodes are initially set, other nodes are guessed incrementally from neighbors distance. A noise is added to distances, up to N(0, 1.7^2) to simulate measurement error, greater values can make the nodes positions diverge importantly
To fix this stability problem, it's possible to avoid 'flat' triangles (where the 3 points are almost collinear)
Other perspective: make it work in 3D, or more, with the intersection of (n+1) n-spheres, compare with more robust optimizations in litterature
Another perspective, average the result of multiple different estimations for a node positions, to eliminate the noise better