0001 function M = essentialfactory(k, strSigned)
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
0031
0032
0033
0034
0035
0036
0037
0038
0039
0040
0041
0042
0043
0044
0045
0046
0047
0048
0049
0050
0051
0052
0053
0054
0055
0056
0057
0058
0059
0060
0061
0062
0063
0064
0065
0066
0067 if ~exist('strSigned', 'var') || isempty(strSigned)
0068 strSigned = 'signed';
0069 end
0070 switch(strSigned)
0071 case 'signed'
0072 flagSigned = true;
0073 case 'unsigned'
0074 flagSigned = false;
0075 otherwise
0076 error('Second argument can be either empty, ''signed'', or ''unsigned''.');
0077 end
0078
0079
0080 if ~exist('k', 'var') || isempty(k)
0081 k = 1;
0082 end
0083
0084 if k == 1
0085 M.name = @() sprintf('Quotient representation of the essential manifold, %s', strSigned);
0086 elseif k > 1 && k == round(k)
0087 M.name = @() sprintf('Product of %d quotient representations of the essential manifold, %s', k, strSigned);
0088 else
0089 error('k must be an integer no less than 1.');
0090 end
0091
0092 M.dim = @() k*5;
0093
0094 M.inner = @(x, d1, d2) d1(:).'*d2(:);
0095
0096 M.norm = @(x, d) norm(d(:));
0097
0098 M.typicaldist = @() pi*sqrt(2*k);
0099
0100 M.proj = @tangentProjection;
0101 function HProjHoriz=tangentProjection(X,H)
0102
0103 HProj = essential_sharp(multiskew(multiprod(multitransp(essential_flat(X)), essential_flat(H))));
0104
0105
0106 p = vertproj(X, HProj);
0107
0108 HProjHoriz = HProj - multiprod(p/2,[essential_hat3(permute(X(3,1:3,:),[2 3 1])) essential_hat3(permute(X(3,4:6,:),[2 3 1]))]);
0109 end
0110
0111
0112 M.tangent = @(X, H) essential_sharp(multiskew(essential_flat(H)));
0113
0114 M.egrad2rgrad=@egrad2rgrad;
0115 function rgrad = egrad2rgrad(X, egrad)
0116 rgrad = M.proj(X, egrad);
0117 end
0118
0119 M.ehess2rhess = @ehess2rhess;
0120 function rhess = ehess2rhess(X, egrad, ehess, S)
0121
0122
0123 RA = p1(X);
0124 RB = p2(X);
0125 SA = p1(S);
0126 SB = p2(S);
0127
0128 G = egrad;
0129 GA = p1(G);
0130 GB = p2(G);
0131
0132 H = ehess;
0133
0134
0135
0136
0137
0138
0139 connection = tangent2ambient(X,-cat(2,...
0140 multiprod(multisym(multiprod(multitransp(GA), RA)), SA),...
0141 multiprod(multisym(multiprod(multitransp(GB), RB)), SB)));
0142 rhess = M.proj(X,H + connection);
0143 end
0144
0145
0146
0147 M.exp = @exponential;
0148 function Y = exponential(X, U, t)
0149 if nargin == 3
0150 U = t*U;
0151 end
0152
0153 UFlat = essential_flat(U);
0154 exptUFlat = rot3_exp(UFlat);
0155 Y = essential_sharp(multiprod(essential_flat(X), exptUFlat));
0156 end
0157
0158 M.retr = @exponential;
0159
0160 M.log = @logarithm;
0161 function U = logarithm(X, Y)
0162
0163 QX = [X(:,1:3,:);X(:,4:6,:)];
0164 QY = [Y(:,1:3,:);Y(:,4:6,:)];
0165 QYr = essential_closestRepresentative(QX,QY,'flagSigned',flagSigned);
0166 Yr = [QYr(1:3,:,:) QYr(4:6,:,:)];
0167 U = zeros(size(X));
0168 U(:,1:3,:) = rot3_log(multiprod(multitransp(X(:,1:3,:)),Yr(:,1:3,:)));
0169 U(:,4:6,:) = rot3_log(multiprod(multitransp(X(:,4:6,:)),Yr(:,4:6,:)));
0170 end
0171
0172 M.hash = @(X) ['z' hashmd5(X(:))];
0173
0174 M.rand = @() randessential(k);
0175 function Q = randessential(N)
0176
0177
0178
0179
0180
0181
0182
0183 if nargin < 1
0184 N = 1;
0185 end
0186
0187 Q = [randrot(3,N) randrot(3,N)];
0188 end
0189
0190 M.randvec = @randomvec;
0191 function U = randomvec(X)
0192 U = tangentProjection(X, essential_sharp(randskew(3, 2*k)));
0193 U = U / sqrt(M.inner([],U,U));
0194 end
0195
0196 M.lincomb = @matrixlincomb;
0197
0198 M.zerovec = @(x) zeros(3, 6, k);
0199
0200 M.transp = @transport;
0201 function S2 = transport(X1, X2, S1)
0202
0203
0204
0205
0206
0207
0208 X12 = essential_sharp(multiprod(multitransp(essential_flat(X2)),essential_flat(X1)));
0209 X12Flat = essential_flat(X12);
0210
0211
0212 S2 = essential_sharp(multiprod(X12Flat,multiprod(essential_flat(S1),multitransp(X12Flat))));
0213 end
0214
0215 M.pairmean = @pairmean;
0216 function Y = pairmean(X1, X2)
0217 V = M.log(X1, X2);
0218 Y = M.exp(X1, .5*V);
0219 end
0220
0221 M.dist = @(x, y) M.norm(x, M.log(x, y));
0222
0223 M.vec = @(x, u_mat) u_mat(:);
0224 M.mat = @(x, u_vec) reshape(u_vec, [3, 6, k]);
0225 M.vecmatareisometries = @() true;
0226
0227 M.tangent2ambient = @tangent2ambient;
0228
0229 end
0230
0231
0232
0233
0234 function v = vee3(V)
0235 v = squeeze([V(3,2,:)-V(2,3,:); V(1,3,:)-V(3,1,:); V(2,1,:)-V(1,2,:)])/2;
0236 end
0237
0238
0239
0240
0241
0242 function R = rot3_exp(V)
0243 v = vee3(V);
0244 nv = cnorm(v);
0245 idxZero = nv < 1e-15;
0246 nvMod = nv;
0247 nvMod(idxZero) = 1;
0248
0249 vNorm = v./([1;1;1]*nvMod);
0250
0251
0252 nv = shiftdim(nv,-1);
0253 c = cos(nv);
0254 s = sin(nv);
0255 [VNorm,vNormShift] = essential_hat3(vNorm);
0256 vNormvNormT = multiprod(vNormShift,multitransp(vNormShift));
0257 R=multiprod(eye(3),c)+multiprod(VNorm,s)+multiprod(vNormvNormT,1-c);
0258 end
0259
0260
0261
0262
0263
0264
0265 function V = rot3_log(R)
0266 skewR = multiskew(R);
0267 ctheta = (multitrace(R)'-1)/2;
0268 stheta = cnorm(vee3(skewR));
0269 theta = atan2(stheta,ctheta);
0270
0271 V=skewR;
0272 for ik=1:size(R,3)
0273 V(:,:,ik)=V(:,:,ik)/sincN(theta(ik));
0274 end
0275 end
0276
0277
0278 function sx = sincN(x)
0279 sx = sin(x)./x;
0280 sx(x==0) = 1;
0281 end
0282
0283 function nv = cnorm(v)
0284 nv = sqrt(sum(v.^2));
0285 end
0286
0287 function M = vertproj(X, H)
0288 M = multiprod(X(3, 1:3, :), permute(vee3(H(:, 1:3, :)), [1 3 2])) + ...
0289 multiprod(X(3, 4:6, :), permute(vee3(H(:, 4:6, :)), [1 3 2]));
0290 end
0291
0292 function M = p1(X)
0293 M = X(:, 1:3, :);
0294 end
0295
0296 function M = p2(X)
0297 M = X(:, 4:6, :);
0298 end
0299
0300 function U = tangent2ambient(X, H)
0301 U = essential_sharp(multiprod(essential_flat(X), essential_flat(H)));
0302 end