6 #include <vnl/vnl_inverse.h> 9 #include <itkMatrixOffsetTransformBase.h> 10 #include <itkTranslationTransform.h> 11 #include <itkContinuousIndex.h> 16 template <
class TInput,
class TScalarType,
unsigned int NDimensions>
18 std::vector < itk::Point<TInput,NDimensions> > &inputTransformed,
19 std::vector <TInput> &weights,
20 typename itk::AffineTransform<TScalarType,NDimensions>::Pointer &resultTransform)
22 unsigned int nbPts = inputOrigins.size();
23 itk::Point <TInput,NDimensions> barX, barY;
25 vnl_matrix <TInput> AMatrix(4,4,0);
26 vnl_matrix <TInput> tmpMatrix(4,4);
28 for (
unsigned int j = 0;j < NDimensions;++j)
34 double sumWeights = 0;
35 for (
unsigned int i = 0;i < nbPts;++i)
37 for (
unsigned int j = 0;j < NDimensions;++j)
39 barX[j] += weights[i] * inputOrigins[i][j];
40 barY[j] += weights[i] * inputTransformed[i][j];
43 sumWeights += weights[i];
46 for (
unsigned int j = 0;j < NDimensions;++j)
48 barX[j] /= sumWeights;
49 barY[j] /= sumWeights;
52 itk::Vector <TScalarType,NDimensions> translationPart;
53 for (
unsigned int i = 0;i < NDimensions;++i)
54 translationPart[i] = barY[i] - barX[i];
56 resultTransform = itk::AffineTransform<TScalarType,NDimensions>::New();
58 resultTransform->SetIdentity();
59 resultTransform->SetOffset(translationPart);
62 template <
class TInput,
class TScalarType,
unsigned int NDimensions>
64 std::vector < itk::Point<TInput,NDimensions> > &inputTransformed,
65 std::vector <TInput> &weights,
66 typename itk::AffineTransform<TScalarType,NDimensions>::Pointer &resultTransform)
68 unsigned int nbPts = inputOrigins.size();
69 itk::Point <TInput, NDimensions> barX, barY;
71 vnl_matrix <TInput> AMatrix(4,4,0);
72 vnl_matrix <TInput> tmpMatrix(4,4);
74 for (
unsigned int j = 0;j < NDimensions;++j)
80 double sumWeights = 0;
81 for (
unsigned int i = 0;i < nbPts;++i)
83 for (
unsigned int j = 0;j < NDimensions;++j)
85 barX[j] += weights[i] * inputOrigins[i][j];
86 barY[j] += weights[i] * inputTransformed[i][j];
89 sumWeights += weights[i];
92 for (
unsigned int j = 0;j < NDimensions;++j)
94 barX[j] /= sumWeights;
95 barY[j] /= sumWeights;
98 itk::Vector <TInput,NDimensions> xVector, yVector;
99 for (
unsigned int i = 0;i < nbPts;++i)
101 xVector = inputOrigins[i] - barX;
102 yVector = inputTransformed[i] - barY;
104 AMatrix += weights[i]*tmpMatrix.transpose()*tmpMatrix;
107 itk::SymmetricEigenAnalysis < vnl_matrix <TInput>, vnl_diag_matrix<TInput>, vnl_matrix <TInput> > eigenSystem(4);
108 vnl_matrix <double> eVec(4,4);
109 vnl_diag_matrix <double> eVals(4);
111 eigenSystem.SetOrderEigenValues(
true);
112 eigenSystem.ComputeEigenValuesAndVectors(AMatrix, eVals, eVec);
114 vnl_matrix <TScalarType> rotationMatrix = anima::computeRotationFromQuaternion<TInput,TScalarType>(eVec.get_row(0));
116 itk::Vector <TScalarType,NDimensions> translationPart;
117 for (
unsigned int i = 0;i < NDimensions;++i)
119 translationPart[i] = barY[i];
120 for (
unsigned int j = 0;j < NDimensions;++j)
121 translationPart[i] -= rotationMatrix(i,j)*barX[j];
124 resultTransform = itk::AffineTransform<TScalarType,NDimensions>::New();
126 resultTransform->SetMatrix(rotationMatrix);
127 resultTransform->SetOffset(translationPart);
130 template <
class TInput,
class TScalarType,
unsigned int NDimensions>
132 std::vector < itk::Point<TInput, NDimensions> > &inputTransformed,
133 std::vector <TInput> &weights,
134 typename itk::AffineTransform<TScalarType, NDimensions>::Pointer &resultTransform,
135 vnl_matrix <double> &UMatrix)
137 unsigned int nbPts = inputOrigins.size();
138 itk::Point <TInput, NDimensions> barX, barY;
140 vnl_matrix <TInput> AMatrix(4, 4, 0);
141 vnl_matrix <TInput> covInputOrigins(NDimensions, NDimensions, 0);
142 vnl_matrix <TInput> tmpMatrix(4, 4);
144 for (
unsigned int j = 0; j < NDimensions; ++j)
150 double sumWeights = 0;
151 for (
unsigned int i = 0; i < nbPts; ++i)
153 for (
unsigned int j = 0; j < NDimensions; ++j)
155 barX[j] += weights[i] * inputOrigins[i][j];
156 barY[j] += weights[i] * inputTransformed[i][j];
158 sumWeights += weights[i];
161 for (
unsigned int j = 0; j < NDimensions; ++j)
163 barX[j] /= sumWeights;
164 barY[j] /= sumWeights;
167 vnl_vector_fixed <TInput, NDimensions> xVector, xiVector, yVector;
168 unsigned int iter = 0;
170 double diffRot = eps + 1;
171 double diffScal = eps + 1;
172 vnl_diag_matrix <double> scal(NDimensions);
173 scal.fill_diagonal(1);
174 vnl_diag_matrix <double> oldscal(NDimensions);
175 vnl_vector <double> q, oldq;
176 std::vector<vnl_matrix<double>> sumdScal(NDimensions);
177 for (
unsigned int j = 0; j < NDimensions; ++j)
178 sumdScal[j].set_size(NDimensions + 1, NDimensions + 1);
179 vnl_vector < double > sumx2(3, 0);
180 itk::SymmetricEigenAnalysis < vnl_matrix <TInput>, vnl_diag_matrix<TInput>, vnl_matrix <TInput> > eigenSystem2(4);
181 vnl_matrix <double> eVec(4, 4);
182 vnl_diag_matrix <double> eVals(4);
183 eigenSystem2.SetOrderEigenValues(
true);
186 while (((diffRot > eps) || (diffScal > eps)) && (iter < 100))
191 for (
unsigned int i = 0;i < nbPts;++i)
193 xiVector = scal*UMatrix.transpose()*(inputOrigins[i].GetVnlVector() - barX.GetVnlVector());
194 yVector = inputTransformed[i].GetVnlVector() - barY.GetVnlVector();
196 AMatrix += weights[i] * tmpMatrix.transpose()*tmpMatrix;
199 eigenSystem2.ComputeEigenValuesAndVectors(AMatrix, eVals, eVec);
204 diffRot = (q - oldq).two_norm();
208 for (
unsigned int j = 0;j < NDimensions;++j)
212 for (
unsigned int i = 0;i < nbPts;++i)
214 yVector = inputTransformed[i].GetVnlVector() - barY.GetVnlVector();
215 xVector = UMatrix.transpose()*(inputOrigins[i].GetVnlVector() - barX.GetVnlVector());
216 for (
unsigned int j = 0;j < NDimensions;++j)
219 sumdScal[j] += weights[i] * tmpMatrix;
220 sumx2[j] += weights[i] * xVector[j] * xVector[j] ;
225 for (
unsigned int j = 0;j < NDimensions;++j)
227 for (
unsigned int k = 0;k < NDimensions + 1;++k)
229 for (
unsigned int l = 0;l < NDimensions + 1;++l)
230 scal[j] += (1 / sumx2[j]) * q[k] * sumdScal[j](k, l) * q[l];
235 diffScal = (scal.get_diagonal() - oldscal.get_diagonal()).two_norm();
241 vnl_matrix <TScalarType> rotationMatrix = anima::computeRotationFromQuaternion<TInput, TScalarType>(q);
242 vnl_matrix <TScalarType> linearPartMatrix = rotationMatrix * scal * UMatrix.transpose();
244 itk::Vector <TScalarType, NDimensions> translationPart;
245 for (
unsigned int i = 0;i < NDimensions;++i)
247 translationPart[i] = barY[i];
248 for (
unsigned int j = 0;j < NDimensions;++j)
249 translationPart[i] -= linearPartMatrix(i,j)*barX[j];
252 resultTransform = itk::AffineTransform <TScalarType, NDimensions>::New();
254 resultTransform->SetMatrix(linearPartMatrix);
255 resultTransform->SetOffset(translationPart);
260 template <
class TInput,
class TScalarType,
unsigned int NDimensions>
262 typename itk::AffineTransform<TScalarType,NDimensions>::Pointer &resultTransform)
264 unsigned int nbPts = inputTransforms.size();
266 vnl_matrix <TInput> resultMatrix(NDimensions+1,NDimensions+1,0);
267 vnl_matrix <TInput> tmpMatrix;
268 double sumWeights = 0;
269 for (
unsigned int i = 0;i < nbPts;++i)
271 sumWeights += weights[i];
272 resultMatrix += weights[i] * inputTransforms[i];
275 resultMatrix /= sumWeights;
279 resultTransform = itk::AffineTransform<TScalarType,NDimensions>::New();
281 vnl_matrix <TScalarType> affinePart(NDimensions,NDimensions);
282 itk::Vector <TScalarType,NDimensions> translationPart;
284 for (
unsigned int i = 0;i < NDimensions;++i)
286 translationPart[i] = resultMatrix(i,NDimensions);
287 for (
unsigned int j = 0;j < NDimensions;++j)
288 affinePart(i,j) = resultMatrix(i,j);
291 resultTransform->SetMatrix(affinePart);
292 resultTransform->SetOffset(translationPart);
295 template <
class TInput,
class TScalarType,
unsigned int NDimensions>
297 std::vector < itk::Point<TInput,NDimensions> > &inputTransformed,
298 std::vector <TInput> &weights,
299 typename itk::AffineTransform<TScalarType,NDimensions>::Pointer &resultTransform)
301 unsigned int nbPts = inputOrigins.size();
302 itk::Point <TInput,NDimensions> barX, barY;
304 vnl_matrix <TInput> SigmaXXMatrix(NDimensions,NDimensions,0);
305 vnl_matrix <TInput> SigmaYXMatrix(NDimensions,NDimensions,0);
307 for (
unsigned int j = 0;j < NDimensions;++j)
313 double sumWeights = 0;
314 for (
unsigned int i = 0;i < nbPts;++i)
316 for (
unsigned int j = 0;j < NDimensions;++j)
318 barX[j] += weights[i] * inputOrigins[i][j];
319 barY[j] += weights[i] * inputTransformed[i][j];
322 sumWeights += weights[i];
325 for (
unsigned int j = 0;j < NDimensions;++j)
327 barX[j] /= sumWeights;
328 barY[j] /= sumWeights;
331 for (
unsigned int i = 0;i < nbPts;++i)
333 for (
unsigned int j = 0;j < NDimensions;++j)
334 for (
unsigned int k = 0;k < NDimensions;++k)
336 SigmaXXMatrix(j,k) += weights[i]*(inputOrigins[i][j] - barX[j])*(inputOrigins[i][k] - barX[k]);
337 SigmaYXMatrix(j,k) += weights[i]*(inputTransformed[i][j] - barY[j])*(inputOrigins[i][k] - barX[k]);
341 vnl_matrix <TInput> affineMatrix = SigmaYXMatrix * vnl_inverse (SigmaXXMatrix);
343 vnl_matrix <TScalarType> outMatrix(NDimensions,NDimensions);
344 itk::Vector <TScalarType,NDimensions> translationPart;
345 for (
unsigned int i = 0;i < NDimensions;++i)
347 translationPart[i] = barY[i];
348 for (
unsigned int j = 0;j < NDimensions;++j)
350 outMatrix(i,j) = affineMatrix(i,j);
351 translationPart[i] -= affineMatrix(i,j)*barX[j];
355 resultTransform = itk::AffineTransform<TScalarType,NDimensions>::New();
357 resultTransform->SetMatrix(outMatrix);
358 resultTransform->SetOffset(translationPart);
365 double normVector = 0;
367 for (
unsigned int i = 0;i < eigenVector.size();++i)
368 normVector += eigenVector[i]*eigenVector[i];
370 vnl_matrix <TOutput> resVal(3,3,0);
372 resVal(0,0) = (1.0/normVector) * (eigenVector[0]*eigenVector[0] + eigenVector[1]*eigenVector[1] - eigenVector[2]*eigenVector[2] - eigenVector[3]*eigenVector[3]);
373 resVal(0,1) = (2.0/normVector) * (eigenVector[1]*eigenVector[2] - eigenVector[0]*eigenVector[3]);
374 resVal(0,2) = (2.0/normVector) * (eigenVector[1]*eigenVector[3] + eigenVector[0]*eigenVector[2]);
376 resVal(1,0) = (2.0/normVector) * (eigenVector[1]*eigenVector[2] + eigenVector[0]*eigenVector[3]);
377 resVal(1,1) = (1.0/normVector) * (eigenVector[0]*eigenVector[0] - eigenVector[1]*eigenVector[1] + eigenVector[2]*eigenVector[2] - eigenVector[3]*eigenVector[3]);
378 resVal(1,2) = (2.0/normVector) * (eigenVector[2]*eigenVector[3] - eigenVector[0]*eigenVector[1]);
380 resVal(2,0) = (2.0/normVector) * (eigenVector[1]*eigenVector[3] - eigenVector[0]*eigenVector[2]);
381 resVal(2,1) = (2.0/normVector) * (eigenVector[2]*eigenVector[3] + eigenVector[0]*eigenVector[1]);
382 resVal(2,2) = (1.0/normVector) * (eigenVector[0]*eigenVector[0] - eigenVector[1]*eigenVector[1] - eigenVector[2]*eigenVector[2] + eigenVector[3]*eigenVector[3]);
387 template <
class TInput,
class TOutput,
unsigned int NDimensions>
388 void pairingToQuaternion(
const vnl_vector_fixed <TInput,NDimensions> &inputPoint,
const vnl_vector_fixed <TInput,NDimensions> &inputTransformedPoint,
389 vnl_matrix <TOutput> &outputMatrix)
394 template <
class TInput,
class TOutput,
unsigned int NDimensions>
395 void pairingToQuaternion(
const itk::Vector <TInput,NDimensions> &inputPoint,
const itk::Vector <TInput,NDimensions> &inputTransformedPoint,
396 vnl_matrix <TOutput> &outputMatrix)
401 template <
class Po
intType,
class TOutput>
402 void pairingToQuaternion(
const PointType &inputPoint,
const PointType &inputTransformedPoint, vnl_matrix <TOutput> &outputMatrix,
unsigned int ndim)
404 outputMatrix.set_size(ndim+1,ndim+1);
405 outputMatrix.fill(0);
407 for (
unsigned int i = 0;i < ndim;++i)
412 outputMatrix(0,1) = inputPoint[i] - inputTransformedPoint[i];
413 outputMatrix(2,3) = - (inputPoint[i] + inputTransformedPoint[i]);
414 outputMatrix(1,0) = - outputMatrix(0,1);
415 outputMatrix(3,2) = - outputMatrix(2,3);
419 outputMatrix(0,2) = inputPoint[i] - inputTransformedPoint[i];
420 outputMatrix(1,3) = inputPoint[i] + inputTransformedPoint[i];
421 outputMatrix(2,0) = - outputMatrix(0,2);
422 outputMatrix(3,1) = - outputMatrix(1,3);
426 outputMatrix(0,3) = inputPoint[i] - inputTransformedPoint[i];
427 outputMatrix(1,2) = - (inputPoint[i] + inputTransformedPoint[i]);
428 outputMatrix(2,1) = - outputMatrix(1,2);
429 outputMatrix(3,0) = - outputMatrix(0,3);
438 template <
class TInput,
class TOutput,
unsigned int NDimensions>
440 vnl_matrix <TOutput> &outputMatrix,
const int &dimScal)
445 template <
class TInput,
class TOutput,
unsigned int NDimensions>
447 vnl_matrix <TOutput> &outputMatrix,
const int &dimScal)
452 template <
class Po
intType,
class TOutput>
455 outputMatrix.set_size(ndim + 1, ndim + 1);
456 outputMatrix.fill(0);
461 outputMatrix(0, 2) = -inputTransformedPoint[2];
462 outputMatrix(0, 3) = inputTransformedPoint[1];
463 outputMatrix(1, 2) = inputTransformedPoint[1];
464 outputMatrix(1, 3) = inputTransformedPoint[2];
465 outputMatrix += outputMatrix.transpose();
466 outputMatrix(0, 0) = inputTransformedPoint[0];
467 outputMatrix(1, 1) = inputTransformedPoint[0];
468 outputMatrix(2, 2) = -inputTransformedPoint[0];
469 outputMatrix(3, 3) = -inputTransformedPoint[0];
470 outputMatrix *= inputPoint[0];
474 outputMatrix(0, 1) = inputTransformedPoint[2];
475 outputMatrix(0, 3) = -inputTransformedPoint[0];
476 outputMatrix(1, 2) = inputTransformedPoint[0];
477 outputMatrix(2, 3) = inputTransformedPoint[2];
478 outputMatrix += outputMatrix.transpose();
479 outputMatrix(0, 0) = inputTransformedPoint[1];
480 outputMatrix(1, 1) = -inputTransformedPoint[1];
481 outputMatrix(2, 2) = inputTransformedPoint[1];
482 outputMatrix(3, 3) = -inputTransformedPoint[1];
483 outputMatrix *= inputPoint[1];
487 outputMatrix(0, 1) = -inputTransformedPoint[1];
488 outputMatrix(0, 2) = inputTransformedPoint[0];
489 outputMatrix(1, 3) = inputTransformedPoint[0];
490 outputMatrix(2, 3) = inputTransformedPoint[1];
491 outputMatrix += outputMatrix.transpose();
492 outputMatrix(0, 0) = inputTransformedPoint[2];
493 outputMatrix(1, 1) = -inputTransformedPoint[2];
494 outputMatrix(2, 2) = -inputTransformedPoint[2];
495 outputMatrix(3, 3) = inputTransformedPoint[2];
496 outputMatrix *= inputPoint[2];
void pairingToQuaternionScalingsDerivative(const vnl_vector_fixed< TInput, NDimensions > &inputPoint, const vnl_vector_fixed< TInput, NDimensions > &inputTransformedPoint, vnl_matrix< TOutput > &outputMatrix, const int &dimScal)
itk::Point< TInput, NDimensions > computeAffineLSWFromTranslations(std::vector< itk::Point< TInput, NDimensions > > &inputOrigins, std::vector< itk::Point< TInput, NDimensions > > &inputTransformed, std::vector< TInput > &weights, typename itk::AffineTransform< TScalarType, NDimensions >::Pointer &resultTransform)
void computeLogEuclideanAverage(std::vector< vnl_matrix< TInput > > &inputTransforms, std::vector< TInput > &weights, typename itk::AffineTransform< TScalarType, NDimensions >::Pointer &resultTransform)
void computeTranslationLSWFromTranslations(std::vector< itk::Point< TInput, NDimensions > > &inputOrigins, std::vector< itk::Point< TInput, NDimensions > > &inputTransformed, std::vector< TInput > &weights, typename itk::AffineTransform< TScalarType, NDimensions >::Pointer &resultTransform)
void pairingToQuaternion(const vnl_vector_fixed< TInput, NDimensions > &inputPoint, const vnl_vector_fixed< TInput, NDimensions > &inputTransformedPoint, vnl_matrix< TOutput > &outputMatrix)
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...
void computeRigidLSWFromTranslations(std::vector< itk::Point< TInput, NDimensions > > &inputOrigins, std::vector< itk::Point< TInput, NDimensions > > &inputTransformed, std::vector< TInput > &weights, typename itk::AffineTransform< TScalarType, NDimensions >::Pointer &resultTransform)
vnl_matrix< TOutput > computeRotationFromQuaternion(vnl_vector< TInput > eigenVector)
itk::Point< TInput, NDimensions > computeAnisotropSimLSWFromTranslations(std::vector< itk::Point< TInput, NDimensions > > &inputOrigins, std::vector< itk::Point< TInput, NDimensions > > &inputTransformed, std::vector< TInput > &weights, typename itk::AffineTransform< TScalarType, NDimensions >::Pointer &resultTransform, vnl_matrix< double > &UMatrix)