0001 function [T] = coregister_fiducials(P,Q)
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023
0024
0025
0026
0027
0028
0029
0030 error('under construction');
0031
0032
0033 DB = 1;
0034
0035 if DB,
0036
0037
0038
0039
0040
0041
0042 P = [ 1 0 0; 0 1 0; 0 -1 0]';
0043 Q = [ 0 1 0; -1 0 0; 1 0 0]';
0044
0045
0046 P(1,:) = P(1,:) + 0.5;
0047
0048 else
0049 if ~exist('P','var') | ~exist('Q','var'),
0050 msg = sprintf('coregister_fiducials: no input P or Q\n');
0051 error(msg);
0052 elseif isempty(P) | isempty(Q),
0053 msg = sprintf('coregister_fiducials: empty input P or Q\n');
0054 error(msg);
0055 elseif ~(isequal(size(P),size(ones(3,3)))),
0056 msg = sprintf('coregister_fiducials: P must be 3x3 matrix\n');
0057 error(msg);
0058 elseif ~(isequal(size(Q),size(ones(3,3)))),
0059 msg = sprintf('coregister_fiducials: Q must be 3x3 matrix\n');
0060 error(msg);
0061 end
0062 end
0063
0064
0065
0066
0067
0068
0069 fig = findobj('tag','fiducials start');
0070 if fig,
0071
0072 close(fig)
0073 end
0074
0075
0076 figure('name','fiducials start','tag','fiducials start')
0077 Color = eye(3);
0078 scatter3(P(1,:),P(2,:),P(3,:),80,Color,'filled');
0079 text(P(1,1),P(2,1),P(3,1),'P nasion');
0080 text(P(1,2),P(2,2),P(3,2),'P left');
0081 text(P(1,3),P(2,3),P(3,3),'P right');
0082 view(3); hold on
0083 Color = [ 0 1 1; 1 0 1; 1 1 0 ];
0084 scatter3(Q(1,:),Q(2,:),Q(3,:),80,Color,'filled');
0085 text(Q(1,1),Q(2,1),Q(3,1),'Q nasion');
0086 text(Q(1,2),Q(2,2),Q(3,2),'Q left');
0087 text(Q(1,3),Q(2,3),Q(3,3),'Q right');
0088 legend('P Nasion','P Left','P Right','Q Nasion','Q Left','Q Right',-1)
0089
0090
0091
0092
0093
0094
0095
0096
0097
0098 PN = P(:,1);
0099 PL = P(:,2);
0100 PR = P(:,3);
0101
0102 fprintf('...P Nasion = [ %8.4f %8.4f %8.4f ]\n',PN);
0103 fprintf('...P Left = [ %8.4f %8.4f %8.4f ]\n',PL);
0104 fprintf('...P Right = [ %8.4f %8.4f %8.4f ]\n',PR);
0105
0106 QN = Q(:,1);
0107 QL = Q(:,2);
0108 QR = Q(:,3);
0109
0110 fprintf('...Q Nasion = [ %8.4f %8.4f %8.4f ]\n',QN);
0111 fprintf('...Q Left = [ %8.4f %8.4f %8.4f ]\n',QL);
0112 fprintf('...Q Right = [ %8.4f %8.4f %8.4f ]\n',QR);
0113
0114
0115
0116
0117
0118
0119
0120 PL2PR = PR - PL;
0121 PL2PN = PN - PL;
0122
0123
0124
0125 QL2QR = QR - QL;
0126 QL2QN = QN - QL;
0127
0128
0129
0130 strang = 1;
0131 if strang,
0132
0133 Po = PL + ( ( dot(PL2PR,PL2PN) / ( norm(PL2PR) ^2 ) ) * PL2PR );
0134 Qo = QL + ( ( dot(QL2QR,QL2QN) / ( norm(QL2QR) ^2 ) ) * QL2QR );
0135
0136 else
0137
0138
0139
0140
0141
0142
0143 Pcos_theta = dot(PL2PR,PL2PN) / ( norm(PL2PR) * norm(PL2PN) );
0144 Qcos_theta = dot(QL2QR,QL2QN) / ( norm(QL2QR) * norm(QL2QN) );
0145
0146
0147
0148
0149 Padjacent_length = norm( PL2PN ) * Pcos_theta;
0150 Qadjacent_length = norm( QL2QN ) * Qcos_theta;
0151
0152
0153 PL2PR_unit_vector = PL2PR / norm( PL2PR );
0154 QL2QR_unit_vector = QL2QR / norm( QL2QR );
0155
0156
0157
0158 Po = PL + (PL2PR_unit_vector * Padjacent_length);
0159 Qo = QL + (QL2QR_unit_vector * Qadjacent_length);
0160
0161 end
0162
0163
0164
0165 T.Po = Po;
0166 T.Qo = Qo;
0167
0168 if DB,
0169 scatter3(Po(1),Po(2),Po(3),60,'k','filled');
0170 scatter3(Qo(1),Qo(2),Qo(3),60,'k','filled');
0171 end
0172
0173
0174
0175
0176
0177 PX = PN - Po;
0178 PY = PL - Po;
0179 PZ = cross( PX, PY );
0180
0181 QX = QN - Qo;
0182 QY = QL - Qo;
0183 QZ = cross( QX, QY );
0184
0185
0186 PXunit = PX / norm(PX);
0187 PYunit = PY / norm(PY);
0188 PZunit = cross ( PXunit , PYunit );
0189
0190 QXunit = QX / norm(QX);
0191 QYunit = QY / norm(QY);
0192 QZunit = cross ( QXunit , QYunit );
0193
0194 angle = acos( dot(PZunit , QZunit) ) * (180 / pi)
0195
0196 fprintf('...P Nasion unit vector = [ %8.4f %8.4f %8.4f ]\n',PXunit);
0197 fprintf('...P Left unit vector = [ %8.4f %8.4f %8.4f ]\n',PYunit);
0198 fprintf('...P Right unit vector = [ %8.4f %8.4f %8.4f ]\n',PZunit);
0199
0200 fprintf('...Q Nasion unit vector = [ %8.4f %8.4f %8.4f ]\n',QXunit);
0201 fprintf('...Q Left unit vector = [ %8.4f %8.4f %8.4f ]\n',QYunit);
0202 fprintf('...Q Right unit vector = [ %8.4f %8.4f %8.4f ]\n',QZunit);
0203
0204
0205 if dot(PXunit,PYunit), error('PX unit vector is not orthogonal to PY unit vector'); end
0206 if dot(PXunit,PZunit), error('PX unit vector is not orthogonal to PZ unit vector'); end
0207 if dot(PYunit,PZunit), error('PY unit vector is not orthogonal to PZ unit vector'); end
0208 if dot(QXunit,QYunit), error('QX unit vector is not orthogonal to QY unit vector'); end
0209 if dot(QXunit,QZunit), error('QX unit vector is not orthogonal to QZ unit vector'); end
0210 if dot(QYunit,QZunit), error('QY unit vector is not orthogonal to QZ unit vector'); end
0211
0212
0213
0214
0215
0216
0217
0218
0219
0220
0221
0222
0223
0224 Pmatrix = [ PXunit PYunit PZunit ];
0225 Qmatrix = [ QXunit QYunit QZunit ];
0226
0227 T.rot.P2Q = Qmatrix;
0228 T.rot.Q2P = Qmatrix';
0229
0230
0231
0232
0233
0234
0235
0236 fig = findobj('tag','fiducials end');
0237 if fig,
0238
0239 close(fig)
0240 end
0241
0242 P2Q = (T.rot.P2Q * (P - repmat(T.Po,1,size(P,2)) ) ) + repmat(T.Qo,1,size(P,2))
0243 Q2P = (T.rot.Q2P * (Q - repmat(T.Qo,1,size(P,2)) ) ) + repmat(T.Po,1,size(P,2))
0244
0245
0246 figure('name','fiducials end','tag','fiducials end');
0247 Color = eye(3);
0248 scatter3(P2Q(1,:),P2Q(2,:),P2Q(3,:),80,Color,'filled');
0249 text(P2Q(1,1),P2Q(2,1),P2Q(3,1),'P2Q nasion');
0250 text(P2Q(1,2),P2Q(2,2),P2Q(3,2),'P2Q left');
0251 text(P2Q(1,3),P2Q(2,3),P2Q(3,3),'P2Q right');
0252 view(3); hold on
0253 Color = [ 0 1 1; 1 0 1; 1 1 0 ];
0254 scatter3(Q2P(1,:),Q2P(2,:),Q2P(3,:),80,Color,'filled');
0255 text(Q2P(1,1),Q2P(2,1),Q2P(3,1),'Q2P nasion');
0256 text(Q2P(1,2),Q2P(2,2),Q2P(3,2),'Q2P left');
0257 text(Q2P(1,3),Q2P(2,3),Q2P(3,3),'Q2P right');
0258 legend('P2Q Nasion','P2Q Left','P2Q Right','Q2P Nasion','Q2P Left','Q2P Right',-1)
0259
0260
0261
0262 return