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
|
function [A, b, newpos] = regpt2surf(node, elem, p, pmask, A0, b0, cmask, maxiter)
% [A,b,newpos]=regpt2surf(node,elem,p,pmask,A0,b0,cmask,maxiter)
% Perform point cloud registration to a triangular surface
% (surface can be either triangular or cubic), Gauss-Newton method
% is used for the calculation
%
% author: Qianqian Fang <q.fang at neu.edu>
% date: 12/12/2008
%
% parameters:
% node: node coordinate of the surface mesh (nn x 3)
% elem: element list of the surface mesh (3 columns for
% triangular mesh, 4 columns for cubic surface mesh)
% p: points to be registered, 3 columns for x,y and z respectively
% pmask: a mask vector with the same length as p, determines the
% method to handle the point, if pmask(i)=-1, the point is a free
% node and can be move by the optimization, if pmask(i)=0, the
% point is fixed; if pmask(i)=n>0, the distance between p(i,:)
% and node(n,:) will be part of the object function and be optimized
% A0: a 3x3 matrix, as the initial guess for the affine A matrix (rotation&scaling)
% b0: a 3x1 vector, as the initial guess for the affine b vector (translation)
% cmask: a binary 12x1 vector, determines which element of [A(:);b] will be optimized
% if cmask(i)=0, the corresponding coefficient will not be updated
% maxiter: a integer, specifying the optimization iterations
%
% outputs:
% A: 3x3 matrix, the updated affine A matrix
% b: 3x1 vector, the updated affine b vector
% newpos: the registered positions for p, newpos=A*p'+b
%
% Please find more information at http://iso2mesh.sf.net/cgi-bin/index.cgi?metch
%
% this function is part of "metch" toobox, see COPYING for license
A = A0;
b = b0(:);
% for simplicity, I wrap A and b into one single vector C
C = [A(:); b];
delta = 1e-4;
newpos = (reshape(C(1:9), 3, 3) * p' + repmat(C(end - 2:end), 1, size(p, 1)))';
nv = nodesurfnorm(node, elem);
clen = length(C);
cuplist = find(cmask == 1);
pfree = find(pmask < 0);
pfix = find(pmask > 0);
% start Gauss-Newton iterations
for iter = 1:maxiter
% calculate the current residual: the sum of distances to the surface
dist0 = zeros(length(pfree) + length(pfix), 1);
if (~isempty(pfree))
[dist0(pfree), cn0] = dist2surf(node, nv, newpos(pfree, :));
end
if (~isempty(pfix))
fixdist = node(pmask(pfix), :) - newpos(pfix, :);
dist0(pfix) = sqrt(sum(fixdist .* fixdist, 2));
end
fprintf('iter=%d error=%f\n', iter, sum(abs(dist0)));
% build the Jacobian (sensitivity) matrix
J = zeros(length(dist0), length(C));
for i = 1:clen
if (cmask(i) == 0)
continue
end
dC = C;
if (C(i))
dC(i) = C(i) * (1 + delta);
else
dC(i) = C(i) + delta;
end
newp = (reshape(dC(1:9), 3, 3) * p' + repmat(dC(end - 2:end), 1, size(p, 1)))';
dist = zeros(length(pfree) + length(pfix), 1);
if (~isempty(pfree))
if (length(cn0) == length(pfree))
dist(pfree) = dist2surf(node, nv, newp(pfree, :), cn0);
else
dist(pfree) = dist2surf(node, nv, newp(pfree, :));
end
end
if (~isempty(pfix))
fixdist = node(pmask(pfix), :) - newp(pfix, :);
dist(pfix) = sqrt(sum(fixdist .* fixdist, 2));
end
% J=dL/dC
J(:, i) = (dist - dist0) / (dC(i) - C(i));
end
% weight the matrix (normalization)
wj = sqrt(sum(J .* J));
J(:, cuplist) = J(:, cuplist) ./ repmat(wj(cuplist), length(dist0), 1);
% calculate the update: J*dC=dL
dC = (J(:, cuplist) \ dist0) ./ wj(cuplist)';
C(cuplist) = C(cuplist) - 0.5 * dC;
% get the updated positions with the calculated A and b
newpos = (reshape(C(1:9), 3, 3) * p' + repmat(C(end - 2:end), 1, size(p, 1)))';
end
A = reshape(C(1:9), 3, 3);
b = C(end - 2:end);
|
