ANIMA  4.0
animaMatrixLogExp.hxx
Go to the documentation of this file.
1 #pragma once
2
3 #include "animaMatrixLogExp.h"
4 #include <iostream>
5 #include <vnl/algo/vnl_determinant.h>
6 #include <vnl/algo/vnl_real_eigensystem.h>
7 #include <vnl/algo/vnl_matrix_inverse.h>
8 #include <vnl/vnl_inverse.h>
10
11 namespace anima
12 {
13 template <class T> vnl_matrix <T> GetSquareRoot(const vnl_matrix <T> & m, const double precision, vnl_matrix <T> & resultM)
14 {
15  // declarations
16
17  double energy;
18  unsigned int D = m.rows();
19
20  vnl_matrix <T> Mk1(D,D), Mk(D,D), Yk1(D,D), Yk(D,D), interm(D,D), invMk(D,D), Id(D,D);
21  int niter=1;
22  int niterMax=60;
23
24  // initializations
25
26  Mk=m; Yk=m; Id=m;
27  Id.set_identity();
28
29  interm=Yk*Yk-m;
30  energy=interm.frobenius_norm();
31
32  //T n = m.Norm();
33  T n = D;
34
35  // loop
36
37  while( (niter <= niterMax) && (energy > precision) )
38  {
39  T gamma=(T)fabs(pow(vnl_determinant(Mk),-1.0/(2.0*n)));
40  vnl_matrix_inverse <T> inverter(Mk);
41  invMk= inverter.inverse();
42  Mk1=(Id+ (Mk*(gamma*gamma)+invMk*(1./(gamma*gamma)) )*0.5 )*0.5;
43  Yk1=Yk*(Id+invMk*(1./(gamma*gamma)) )*0.5*gamma;
44
45  Yk=Yk1; Mk = Mk1;
46
47  niter++;
48
49  interm=Yk*Yk-m;
50  energy=interm.frobenius_norm();
51  }
52
53  if (niter == niterMax)
54  {
55  std::cout <<"\nWarning, max number of iteration reached in sqrt computation. Final energy is:" << energy << ".\n";
56  }
57
58  // std::cout << "niter="<<niter<<".\n";
59
60  resultM=Mk;
61
62  return Yk;
63 }
64
65 template <class T> vnl_matrix <T> GetPadeLogarithm(const vnl_matrix <T> & m,const int numApprox)
66 {
67  unsigned int D = m.rows();
68
69  vnl_matrix <T> log=m;
70  vnl_matrix <T> Id(D,D);
71  Id.set_identity();
72  vnl_matrix <T> diff(D,D),interm2(D,D),interm3(D,D),sqr(D,D),cube(D,D);
73
74  diff=Id-m;
75  T energy=diff.frobenius_norm();
76
77  if (energy > 0.5)
78  {
79  std::cout <<"Warning, matrix is not close enough to Id to call Pade approximation. Frobenius Distance=" << energy <<". Returning original matrix.\n";
80  return log;
81  }
82
83  switch (numApprox)
84  {
85
86  case 1:
87  interm2=diff*(-1.);
88  interm3=(Id-diff*0.5);
89  break;
90
91  case 2:
92  sqr=diff*diff;
93
94  interm2=diff*(-1.0)+sqr*(0.5);
95  interm3=(Id-diff+sqr);
96  break;
97
98  case 3:
99  sqr=diff*diff;
100  cube=sqr*diff;
101
102  interm2=diff*(-1.0)+sqr+cube*(11.0/60.0);
103  interm3=(Id-diff*(1.5)+sqr*0.6+cube*(-0.05));
104  break;
105
106
107  default:
108  sqr=diff*diff;
109  cube=sqr*diff;
110  std::cout <<"\n\ndefault is taken in log Pade\n\n";
111  interm2=diff*(-1.0)+sqr+cube*(11.0/60.0);
112  interm3=(Id-diff*(1.5)+sqr*0.6+cube*(-0.05));
113  break;
114  }
115
116  log=interm2 * vnl_inverse(interm3);
117
118  return log;
119 }
120
121 /*
122 template <class T> vnl_matrix <T> GetLogarithm(const vnl_matrix <T> & m, const double precision, const int numApprox)
123 {
124  // New version : this seems to work for non pathological matrices
125  unsigned int ndim = m.rows();
126
127  vnl_real_eigensystem eig(m);
128  vnl_matrix < vcl_complex<T> > Vinv = vnl_matrix_inverse < vcl_complex<T> > ( eig.V );
129
130  vnl_matrix < vcl_complex<T> > logMat = eig.D.asMatrix();
131  for (unsigned int i = 0;i < ndim;++i)
132  logMat(i,i) = vcl_log(logMat(i,i));
133
134  logMat = eig.V * logMat * Vinv;
135
136  vnl_matrix <T> resMat (ndim,ndim);
137  for (unsigned int i = 0;i < ndim;++i)
138  for (unsigned int j = 0;j < ndim;++j)
139  resMat(i,j) = vcl_real <T> (logMat(i,j));
140
141  return resMat;
142 }
143 */
144
145 template <class T> vnl_matrix <T> GetLogarithm(const vnl_matrix <T> & m, const double precision, const int numApprox)
146 {
147  unsigned int D = m.rows();
148  //typedef double T;
149
150  T factor=1.0;
151  vnl_matrix <double> log=m;
152  T energy;
153  vnl_matrix <double> Id(D,D);
154  Id.set_identity();
155  vnl_matrix <double> interm(D,D),Yi(D,D),Yi1(D,D),resultM(D,D);
156  vnl_matrix <double> matrix_sum=m;
157
158  int niterMax=60; int niter=1;
159
160  Yi=m;
161  interm=Yi-Id;
162  energy=interm.frobenius_norm();
163  matrix_sum.fill(0);
164
165  while ( (energy > 0.5) && (niter <= niterMax))
166  {
167  // std::cout <<"\nniter=" << niter <<", energy="<<energy<<".\n";
168
169  Yi1=GetSquareRoot(Yi,precision,resultM);
170
171  if ((!std::isfinite(resultM(0,0))) || (!std::isfinite(Yi(0,0))))
172  {
173  log.fill(0);
174  return log;
175  }
176
177  matrix_sum+= (Id-resultM)*factor;
178
179  Yi=Yi1;
180  interm=Yi-Id;
181  energy=interm.frobenius_norm();
182
183  factor*=2.0;
184  niter++;
185  }
186
188  if (!std::isfinite(log(0,0)))
189  {
190  log.fill(0);
191  return log;
192  }
193
194  return log*factor+matrix_sum;
195 }
196
197
198 template <class T> vnl_matrix <T> GetExponential(const vnl_matrix <T> & m, const int numApprox)
199 {
200  unsigned int D = m.rows();
201
202  T factor;
203  vnl_matrix <T> exp=m;
204  vnl_matrix <T> Id(D,D); Id.set_identity();
205  vnl_matrix <T> interm(D,D),interm2(D,D),interm3(D,D),sqr(D,D),cube(D,D);
206
207  T norm=m.frobenius_norm();
208  int k;
209
210  if(norm > 1)
211  {
212  k=1 + (int) ceil(log(m.frobenius_norm())/log(2.0));
213  }
214  else if(norm >0.5)
215  {
216  k=1;
217  }
218  else
219  {
220  k=0;
221  }
222
223  //std::cout << "\n The famous k=" << ceil(log(m.GetVnlMatrix().frobenius_norm())/log(2.0)) << ". In effect norm=" << m.GetVnlMatrix().frobenius_norm()<< ".\n";
224
225  factor=pow(2.0,(double) k);
226  interm=m/factor;
227
228
229  switch(numApprox)
230  {
231  case 1:
232  interm2=Id+interm*0.5;
233  interm3=Id-interm*0.5;
234  break;
235
236  case 2:
237  sqr=interm*interm;
238
239  interm2=Id+interm*0.5+sqr*(1.0/12.0);
240  interm3=Id-interm*0.5+sqr*(1.0/12.0);
241  break;
242
243  case 3:
244  sqr=interm*interm;
245  cube=sqr*interm;
246
247  interm2=Id+interm*0.5+sqr*(1.0/10.0)+cube*(1.0/120.0);
248  interm3=Id-interm*0.5+sqr*(1.0/10.0)-cube*(1.0/120.0);;
249  break;
250
251  default:
252  std::cout <<"\n\ndefault is taken in exp Pade\n\n";
253
254  sqr=interm*interm;
255  cube=sqr*interm;
256
257  interm2=Id+interm*0.5+sqr*(1.0/10.0)+cube*(1.0/120.0);
258  interm3=Id-interm*0.5+sqr*(1.0/10.0)-cube*(1.0/120.0);;
259  break;
260  }
261
262  interm=interm2 * vnl_inverse(interm3);
263
264  for(int i=1;i<=k;i++)
265  interm*=interm;
266
267  exp=interm;
268
269  return exp;
270 }
271
272 template <class T> void Get3DRotationLogarithm(const vnl_matrix <T> &rotationMatrix, std::vector <T> &outputAngles)
273 {
274  const unsigned int dataDim = 3;
277
278  double rotationTrace = 0;
279  for (unsigned int i = 0;i < dataDim;++i)
280  rotationTrace += rotationMatrix(i,i);
281
282  double cTheta = (rotationTrace - 1.0) / 2.0;
283  double theta = std::acos(cTheta);
284  double sTheta = std::sin(theta);
285
286  unsigned int pos = 0;
287  for (int i = dataDim - 1;i >= 0;--i)
288  {
289  for (int j = dataDim - 1;j > i;--j)
290  {
291  outputAngles[pos] = theta * (rotationMatrix(i,j) - rotationMatrix(j,i)) / (2.0 * sTheta);
292  if (pos % 2 == 0)
293  outputAngles[pos] *= -1;
294
295  ++pos;
296  }
297  }
298 }
299
300 template <class T> void Get3DRotationExponential(const std::vector <T> &angles, vnl_matrix <T> &outputRotation)
301 {
302  const unsigned int dataDim = 3;
304  outputRotation.set_identity();
305
306  double thetaValue = 0;
307  for (unsigned int i = 0;i < dataDim;++i)
308  thetaValue += angles[i] * angles[i];
309
310  if (thetaValue == 0)
311  return;
312
313  double sqrtTheta = std::sqrt(thetaValue);
314  double firstAddConstant = std::sin(sqrtTheta) / sqrtTheta;
315  double secondAddConstant = (1.0 - std::cos(sqrtTheta)) / thetaValue;
316
317  for (unsigned int i = 0;i < dataDim;++i)
318  {
319  for (unsigned int j = 0;j < dataDim;++j)
320  {
321  if (j != i)
322  {
323  outputRotation(i,i) -= secondAddConstant * angles[j] * angles[j];
324  outputRotation(i,j) += secondAddConstant * angles[i] * angles[j];
325  }
326  }
327  }
328
329  // Add skew symmetric part
330  unsigned int pos = dataDim - 1;
331  for (unsigned int i = 0;i < dataDim;++i)
332  {
333  for (unsigned int j = i + 1;j < dataDim;++j)
334  {
335  if ((i + j) % 2 != 0)
336  {
337  outputRotation(i,j) -= angles[pos] * firstAddConstant;
338  outputRotation(j,i) += angles[pos] * firstAddConstant;
339  }
340  else
341  {
342  outputRotation(i,j) += angles[pos] * firstAddConstant;
343  outputRotation(j,i) -= angles[pos] * firstAddConstant;
344  }
345
346  --pos;
347  }
348  }
349 }
350
352 template <class TInputScalarType, class TOutputScalarType, unsigned int NDimensions, unsigned int NDegreesOfFreedom>
353 void
356 {
357  if (!m_Modified)
358  return;
359
360  m_Output.resize(m_InputTransforms.size());
361
363
365  tmpStr->loggerFilter = this;
366
368
372
373  delete tmpStr;
374
375  m_Modified = false;
376 }
377
378 template <class TInputScalarType, class TOutputScalarType, unsigned int NDimensions, unsigned int NDegreesOfFreedom>
382 {
384
386  unsigned int nbProcs = threadArgs->NumberOfWorkUnits;
387
389
391
393 }
394
395 template <class TInputScalarType, class TOutputScalarType, unsigned int NDimensions, unsigned int NDegreesOfFreedom>
396 void
399 {
401
404
405  if (threadId+1==numThreads) // Ensure we got up to the last trsf
406  endTrsf = m_InputTransforms.size();
407
408  typename BaseInputMatrixTransformType::MatrixType affinePart;
409  itk::Vector <TInputScalarType,NDimensions> offsetPart;
410  vnl_matrix <TInputScalarType> tmpMatrix(NDimensions+1,NDimensions+1,0), logMatrix(NDimensions+1,NDimensions+1,0);
411  tmpMatrix(NDimensions,NDimensions) = 1;
412
413  for (unsigned int i = startTrsf; i < endTrsf; ++i)
414  {
415  BaseInputMatrixTransformType *tmpTrsf = (BaseInputMatrixTransformType *)m_InputTransforms[i].GetPointer();
416  affinePart = tmpTrsf->GetMatrix();
417  offsetPart = tmpTrsf->GetOffset();
418
419  for (unsigned int j = 0;j < NDimensions;++j)
420  {
421  tmpMatrix(j,NDimensions) = offsetPart[j];
422  for (unsigned int k = 0;k < NDimensions;++k)
423  tmpMatrix(j,k) = affinePart(j,k);
424  }
425
426  logMatrix = GetLogarithm(tmpMatrix);
427
428  unsigned int pos = 0;
429  if (m_UseRigidTransforms)
430  {
431  for (unsigned int j = 0;j < NDimensions;++j)
432  {
433  for (unsigned int k = j+1;k < NDimensions;++k)
434  {
435  m_Output[i][pos] = logMatrix(j,k);
436  ++pos;
437  }
438  }
439
440  for (unsigned int j = 0;j < NDimensions;++j)
441  {
442  m_Output[i][pos] = logMatrix(j,NDimensions);
443  ++pos;
444  }
445
446  }
447  else
448  {
449  for (unsigned int j = 0;j < NDimensions;++j)
450  {
451  for (unsigned int k = 0;k <= NDimensions;++k)
452  {
453  m_Output[i][pos] = logMatrix(j,k);
454  ++pos;
455  }
456  }
457  }
458  }
459 }
460
461 } // end of namespace anima
void Get3DRotationLogarithm(const vnl_matrix< T > &rotationMatrix, std::vector< T > &outputAngles)
Computation of a 3D rotation matrix logarithm. Rodrigues&#39; explicit formula.
vnl_matrix< T > GetExponential(const vnl_matrix< T > &m, const int numApprox=1)
Computation of the matrix exponential. Algo: classical scaling and squaring, as in Matlab...