-
Notifications
You must be signed in to change notification settings - Fork 10
/
Copy pathLineCurvature2D.m
107 lines (93 loc) · 4.08 KB
/
LineCurvature2D.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
function k=LineCurvature2D(Vertices,Lines)
% This function calculates the curvature of a 2D line. It first fits
% polygons to the points. Then calculates the analytical curvature from
% the polygons;
%
% k = LineCurvature2D(Vertices,Lines)
%
% inputs,
% Vertices : A M x 2 list of line points.
% (optional)
% Lines : A N x 2 list of line pieces, by indices of the vertices
% (if not set assume Lines=[1 2; 2 3 ; ... ; M-1 M])
%
% outputs,
% k : M x 1 Curvature values
%
%
%
% Example, Circle
% r=sort(rand(15,1))*2*pi;
% Vertices=[sin(r) cos(r)]*10;
% Lines=[(1:size(Vertices,1))' (2:size(Vertices,1)+1)']; Lines(end,2)=1;
% k=LineCurvature2D(Vertices,Lines);
%
% figure, hold on;
% N=LineNormals2D(Vertices,Lines);
% k=k*100;
% plot([Vertices(:,1) Vertices(:,1)+k.*N(:,1)]',[Vertices(:,2) Vertices(:,2)+k.*N(:,2)]','g');
% plot([Vertices(Lines(:,1),1) Vertices(Lines(:,2),1)]',[Vertices(Lines(:,1),2) Vertices(Lines(:,2),2)]','b');
% plot(sin(0:0.01:2*pi)*10,cos(0:0.01:2*pi)*10,'r.');
% axis equal;
%
% Example, Hand
% load('testdata');
% k=LineCurvature2D(Vertices,Lines);
%
% figure, hold on;
% N=LineNormals2D(Vertices,Lines);
% k=k*100;
% plot([Vertices(:,1) Vertices(:,1)+k.*N(:,1)]',[Vertices(:,2) Vertices(:,2)+k.*N(:,2)]','g');
% plot([Vertices(Lines(:,1),1) Vertices(Lines(:,2),1)]',[Vertices(Lines(:,1),2) Vertices(Lines(:,2),2)]','b');
% plot(Vertices(:,1),Vertices(:,2),'r.');
% axis equal;
%
% Function is written by D.Kroon University of Twente (August 2011)
% If no line-indices, assume a x(1) connected with x(2), x(3) with x(4) ...
if(nargin<2)
Lines=[(1:(size(Vertices,1)-1))' (2:size(Vertices,1))'];
end
% Get left and right neighbor of each points
Na=zeros(size(Vertices,1),1); Nb=zeros(size(Vertices,1),1);
Na(Lines(:,1))=Lines(:,2); Nb(Lines(:,2))=Lines(:,1);
% Check for end of line points, without a left or right neighbor
checkNa=Na==0; checkNb=Nb==0;
Naa=Na; Nbb=Nb;
Naa(checkNa)=find(checkNa); Nbb(checkNb)=find(checkNb);
% If no left neighbor use two right neighbors, and the same for right...
Na(checkNa)=Nbb(Nbb(checkNa)); Nb(checkNb)=Naa(Naa(checkNb));
% Correct for sampeling differences
Ta=-sqrt(sum((Vertices-Vertices(Na,:)).^2,2));
Tb=sqrt(sum((Vertices-Vertices(Nb,:)).^2,2));
% If no left neighbor use two right neighbors, and the same for right...
Ta(checkNa)=-Ta(checkNa); Tb(checkNb)=-Tb(checkNb);
% Fit a polygons to the vertices
% x=a(3)*t^2 + a(2)*t + a(1)
% y=b(3)*t^2 + b(2)*t + b(1)
% we know the x,y of every vertice and set t=0 for the vertices, and
% t=Ta for left vertices, and t=Tb for right vertices,
x = [Vertices(Na,1) Vertices(:,1) Vertices(Nb,1)];
y = [Vertices(Na,2) Vertices(:,2) Vertices(Nb,2)];
M = [ones(size(Tb)) -Ta Ta.^2 ones(size(Tb)) zeros(size(Tb)) zeros(size(Tb)) ones(size(Tb)) -Tb Tb.^2];
invM=inverse3(M);
a(:,1)=invM(:,1,1).*x(:,1)+invM(:,2,1).*x(:,2)+invM(:,3,1).*x(:,3);
a(:,2)=invM(:,1,2).*x(:,1)+invM(:,2,2).*x(:,2)+invM(:,3,2).*x(:,3);
a(:,3)=invM(:,1,3).*x(:,1)+invM(:,2,3).*x(:,2)+invM(:,3,3).*x(:,3);
b(:,1)=invM(:,1,1).*y(:,1)+invM(:,2,1).*y(:,2)+invM(:,3,1).*y(:,3);
b(:,2)=invM(:,1,2).*y(:,1)+invM(:,2,2).*y(:,2)+invM(:,3,2).*y(:,3);
b(:,3)=invM(:,1,3).*y(:,1)+invM(:,2,3).*y(:,2)+invM(:,3,3).*y(:,3);
% Calculate the curvature from the fitted polygon
k = 2*(a(:,2).*b(:,3)-a(:,3).*b(:,2)) ./ ((a(:,2).^2+b(:,2).^2).^(3/2));
function Minv = inverse3(M)
% This function does inv(M) , but then for an array of 3x3 matrices
adjM(:,1,1)= M(:,5).*M(:,9)-M(:,8).*M(:,6);
adjM(:,1,2)= -(M(:,4).*M(:,9)-M(:,7).*M(:,6));
adjM(:,1,3)= M(:,4).*M(:,8)-M(:,7).*M(:,5);
adjM(:,2,1)= -(M(:,2).*M(:,9)-M(:,8).*M(:,3));
adjM(:,2,2)= M(:,1).*M(:,9)-M(:,7).*M(:,3);
adjM(:,2,3)= -(M(:,1).*M(:,8)-M(:,7).*M(:,2));
adjM(:,3,1)= M(:,2).*M(:,6)-M(:,5).*M(:,3);
adjM(:,3,2)= -(M(:,1).*M(:,6)-M(:,4).*M(:,3));
adjM(:,3,3)= M(:,1).*M(:,5)-M(:,4).*M(:,2);
detM=M(:,1).*M(:,5).*M(:,9)-M(:,1).*M(:,8).*M(:,6)-M(:,4).*M(:,2).*M(:,9)+M(:,4).*M(:,8).*M(:,3)+M(:,7).*M(:,2).*M(:,6)-M(:,7).*M(:,5).*M(:,3);
Minv=bsxfun(@rdivide,adjM,detM);