function [c,dc] = matchCH2(Q,objQ,optM, dQ,delta) switch nargin case 5 switch optM case 'max_eu' % p-norm, min max Euclidean norm p = 16; diff = (Q-objQ).^2; c = mean(mean(diff.^p))^(1/p)/delta-1; % p-norm dcdd = 1/9*mean(mean(diff.^p))^(1/p-1)*diff.^(p-1)/delta; dc = 0; for i = 1:3 for j = 1:3 dddQ = 2*(Q(i,j)-objQ(i,j))*dQ{i,j}; dc = dc+ dcdd(i,j)*dddQ; end end case 'sum_eu' diff = (Q-objQ).^2; c = sum(diff(:))/delta-1; dcdd = 2*(Q-objQ)/delta; dc = 0; for i = 1:3 for j = 1:3 dc = dc + dcdd(i,j)*dQ{i,j}; end end case 'frob2' % Frobenius norm,to take into account material rotations diff = Q-objQ; % c = trace(diff'*diff).^0.5/delta-1; c = trace(diff'*diff)/delta-1; if c+1<1e-4 dc = zeros(size(dQ{1,1})); else dc = 0; % dfdd =0.5*trace(diff'*diff)^(0.5-1)/delta; dfdd = 1/delta; for i = 1:3 for j = 1:3 dc = dc+ 2*diff(i,j)*dfdd*dQ{i,j}; end end end end case 4 % no delta switch optM case 'stein2' c = log(det((Q+objQ)/2)) - 0.5*log(det(Q*objQ)); df = inv((Q+objQ)/2)'/2 - 0.5*inv(Q*objQ)'*objQ; dc = 0; for i = 1:3 for j = 1:3 dc = dc+ df(i,j)*dQ{i,j}; end end case 'stein' c2 = log(det((Q+objQ)/2)) - 0.5*log(det(Q*objQ)); c = sqrt(c2); df1 = inv((Q+objQ)/2)'/2 - 0.5*inv(Q*objQ)'*objQ; df = 0.5*c2^(-0.5) * df1; dc = 0; for i = 1:3 for j = 1:3 dc = dc+ df(i,j)*dQ{i,j}; end end case 'geo2' % c = geodist(Q,objQ); % dc = geodistDerivative(Q,objQ); % riemannian dist = trace((log(M^(-1)*N))^2) M = objQ; dd = diag(M\Q); c = sum((log(dd)).^2); % 2-norm dfdd = c * diag((2*log(dd)) .* (1./dd)) .* inv(M)'; dc = 0; for i = 1:3 for j = 1:3 dc = dc+ dfdd(i,j)*dQ{i,j}; end end case 'geo' [c,dfdd] = geodistDerivative(objQ,Q,optM); % riemannian dist = sqrt(trace((log(M^(-1)*N))^2)) % M = objQ; % dd = diag(M\Q); % c1 = sum((log(dd)).^2); % c = sqrt(c1); % dfdd = 1/2*c1^(-1/2) * c1 * diag((2*log(dd)) .* (1./dd)) .* inv(M)'; dc = 0; for i = 1:3 for j = 1:3 dc = dc+ dfdd(i,j)*dQ{i,j}; end end case 'max_eu' % p-norm, min max Euclidean norm p = 16; diff = (Q-objQ).^2; c = sum(diff(:).^p)^(1/p); % p-norm dcdd = sum(diff(:).^p)^(1/p-1).*(diff.^(p-1)); dc = 0; for i = 1:3 for j = 1:3 dddQ = 2*(Q(i,j)-objQ(i,j))*dQ{i,j}; dc = dc+ dcdd(i,j)*dddQ; end end case 'sum_eu' diff = (Q-objQ).^2; c = sum(diff(:)); dcdd = 2*(Q-objQ); dc = 0; for i = 1:3 for j = 1:3 dc = dc + dcdd(i,j)*dQ{i,j}; end end case 'eu' diff = Q-objQ; c = sum(diff(:)); % dcdd = ; dc = 0; for i = 1:3 for j = 1:3 dc = dc + dQ{i,j}; end end case 'frob' % Frobenius norm,to take into account material rotations diff = Q-objQ; c = trace(diff'*diff)^0.5; if c<1e-20 dc = zeros(size(dQ{1,1})); else dc = 0; dfdd =0.5*trace(diff'*diff)^(0.5-1)*2.*diff'; for i = 1:6 for j = 1:6 dc = dc+ dfdd(i,j)*dQ{i,j}; end end end case 'frob2' % Frobenius norm,to take into account material rotations diff = Q-objQ; c = trace(diff'*diff); dc = 0; dfdd = 2*diff'; % dfdd =0.5*trace(diff'*diff)^(0.5-1); for i = 1:6 for j = 1:6 dc = dc+ dfdd(i,j)*dQ{i,j}; end end case 'frob_ill' p = 16; diff = (Q-objQ).^2; diff2 = [diff(1,1),0,0; 0,diff(2,2),0; 0,0,diff(3,3)]; c = sum(diff2(:).^p)^(1/p); % p-norm dcdd = sum(diff2(:).^p)^(1/p-1).*(diff2.^(p-1)); dc = 0; for i = 1:3 for j = 1:3 dddQ = 2*(Q(i,j)-objQ(i,j))*dQ{i,j}; dc = dc+ dcdd(i,j)*dddQ; end end end case 3 % no dc switch optM case 'max_eu' % p-norm, min max Euclidean norm p = 16; diff = (Q-objQ).^2; c = mean(mean(diff.^p))^(1/p); % p-norm case 'sum_eu' diff = (Q-objQ).^2; c = sum(diff(:)); case 'frob' % Frobenius norm,to take into account material rotations diff = Q-objQ; c = trace(diff'*diff)^0.5; case 'frob2' % Frobenius norm,to take into account material rotations diff = Q-objQ; c = trace(diff'*diff); end dc = 0; end end