CustusX  22.04-rc2
An IGT application
matrixInterpolation.cpp
Go to the documentation of this file.
1 /*=========================================================================
2 This file is part of CustusX, an Image Guided Therapy Application.
3 
4 Copyright (c) SINTEF Department of Medical Technology.
5 All rights reserved.
6 
7 CustusX is released under a BSD 3-Clause license.
8 
9 See Lisence.txt (https://github.com/SINTEFMedtek/CustusX/blob/master/License.txt) for details.
10 =========================================================================*/
11 
12 #include "matrixInterpolation.h"
13 //#include <vector>
14 //#include "itkArray.h"
15 //#include "itkArray2D.h"
16 #include <iostream>
17 //#include "vnl/vnl_matrix.h"
18 //#include "vnl/vnl_vector.h"
19 //typedef vnl_matrix<double> vnl_matrix_double;
20 
21 
22 std::vector<vnl_matrix_double> matrixInterpolation(vnl_vector<double> DataPoints,
23  std::vector<vnl_matrix_double> DataValues, vnl_vector<double> InterpolationPoints, std::string InterpolationMethod)
24 {
25  try
26  {
27  if (DataPoints.size() != DataValues.size())
28  {
29  std::cerr << "\n\n" << __FILE__ << "," << __LINE__ << "\n" << ">>>>>>>> In "
30  << "::Number of input data points differs from number of input data values!!! Throw up ...\n";
31  throw;
32  }
33 
34  std::vector<vnl_matrix_double> InterpolationData(InterpolationPoints.size());
35 
36  if (InterpolationMethod.compare("closest point") == 0) // Closest point "interpolation"
37  {
38  unsigned int i = 1;
39  for (unsigned int j = 0; j < InterpolationPoints.size(); j++)
40  {
41  while (DataPoints[i] < InterpolationPoints[j] && i < DataPoints.size() - 1)
42  {
43  i++;
44  }
45 
46  if (std::abs(DataPoints[i - 1] - InterpolationPoints[j]) < std::abs(DataPoints[i] - InterpolationPoints[j]))
47  {
48  InterpolationData[j] = DataValues[i - 1];
49  }
50  else
51  {
52  InterpolationData[j] = DataValues[i];
53  }
54  }
55 
56  }
57 
58  else if (InterpolationMethod.compare("linear") == 0) // Linear interpolation
59  {
60  unsigned int j = 0;
61  for (unsigned int i = 0; i < InterpolationPoints.size(); i++)
62  {
63  while (DataPoints.get(j + 1) < InterpolationPoints.get(i) && j + 1 < DataPoints.size() - 1)
64  {
65  j++;
66  }
67  double t = (InterpolationPoints.get(i) - DataPoints.get(j)) / (DataPoints.get(j + 1) - DataPoints.get(j));
68 // std::cout << "t= " << t << std::endl;
69 
70  // Translation component interpolation
71  // -----------------------------------------------
72 
73  vnl_vector<double> InterpolatedTranslationComponent(4);
74  for (int k = 0; k < 3; k++)
75  {
76  InterpolatedTranslationComponent.put(k, (1 - t) * DataValues.at(j).get(k, 3) + t * DataValues.at(j + 1).get(
77  k, 3));
78  }
79 // std::cout << DataValues.at(j).get(0, 3) << " " << DataValues.at(j).get(1, 3) << " " << DataValues.at(j).get(2, 3) << std::endl;
80  InterpolatedTranslationComponent.put(3, 1);
81 
82  // Rotation matrix interpolation
83  // -----------------------------------------------
84  // This procedure is taken from Eberly (2008), "Rotation
85  // Representations and Performance Issues" found at
86  // http://www.geometrictools.com/.
87 
88  // Step 1. Extract the rotational parts of the
89  // transformation matrix and compute a matrix R.
90 
91  vnl_matrix<double> P = DataValues.at(j).extract(3, 3);
92  vnl_matrix<double> Q = DataValues.at(j + 1).extract(3, 3);
93  vnl_matrix<double> R = P.transpose() * Q;
94 
95  // Step 2. Compute an axis-angle representation of R.
96 
97  vnl_vector<double> A(3, 0);
98  double Argument = (R.get(0, 0) + R.get(1, 1) + R.get(2, 2) - 1) / 2;
99  // Due to roundoff error, the argument can become
100  // slightly larger than 1, causing an invalid input to
101  // acos. In these cases, it is assumed that the rotation
102  // is negligable, and the argument is set to 1 (making
103  // theta 0).
104  if (Argument > 1)
105  {
106  Argument = 1;
107  }
108  double theta = acos(Argument);
109 
110  if (theta == 0)
111  {
112  // There is no rotation, and the vector is set to an
113  // arbitrary unit vector ([1 0 0]).
114  A.put(0, 1);
115  }
116  else if (theta < 3.14159265)
117  {
118  A.put(0, R.get(2, 1) - R.get(1, 2));
119  A.put(1, R.get(0, 2) - R.get(2, 0));
120  A.put(2, R.get(1, 0) - R.get(0, 1));
121  A.normalize();
122  }
123  else
124  {
125  if (R.get(0, 0) > R.get(1, 1) && R.get(0, 0) > R.get(2, 2))
126  {
127  A.put(0, sqrt(R.get(0, 0) - R.get(1, 1) - R.get(2, 2) + 1) / 2);
128  A.put(1, R.get(0, 1) / (2 * A.get(0)));
129  A.put(2, R.get(0, 2) / (2 * A.get(0)));
130  }
131  else if (R.get(1, 1) > R.get(0, 0) && R.get(0, 0) > R.get(2, 2))
132  {
133  A.put(1, sqrt(R.get(1, 1) - R.get(0, 0) - R.get(2, 2) + 1) / 2);
134  A.put(0, R.get(0, 1) / (2 * A.get(1)));
135  A.put(2, R.get(1, 2) / (2 * A.get(1)));
136  }
137  else
138  {
139  A.put(2, sqrt(R.get(2, 2) - R.get(1, 1) - R.get(0, 0) + 1) / 2);
140  A.put(1, R.get(1, 2) / (2 * A.get(2)));
141  A.put(0, R.get(0, 2) / (2 * A.get(2)));
142  }
143  }
144 
145  // Step 3. Multiply the angle theta by t and convert
146  // back to rotation matrix representation.
147 
148  vnl_matrix<double> S(3, 3, 0);
149  vnl_matrix<double> I(3, 3);
150  vnl_matrix<double> Rt(3, 3);
151 
152  S.put(0, 1, -A.get(2));
153  S.put(0, 2, A.get(1));
154  S.put(1, 0, A.get(2));
155  S.put(1, 2, -A.get(0));
156  S.put(2, 0, -A.get(1));
157  S.put(2, 1, A.get(0));
158 
159  I.set_identity();
160 
161  Rt = I + sin(t * theta) * S + (1 - cos(t * theta)) * S * S;
162 
163  // Step 4. Compute the interpolated matrix, known as the
164  // slerp (spherical linear interpolation).
165 
166  vnl_matrix<double> InterpolatedRotationComponent = P * Rt;
167 
168  // Transformation matrix composition
169  // -----------------------------------------------
170  // Compose a 4x4 transformation matrix from the
171  // interpolated translation and rotation components.
172 
173  InterpolationData.at(i).set_size(4, 4);
174  InterpolationData.at(i).fill(0);
175  for (int r = 0; r < 3; r++)
176  {
177  for (int c = 0; c < 3; c++)
178  {
179  InterpolationData.at(i).put(r, c, InterpolatedRotationComponent.get(r, c));
180  }
181  }
182  InterpolationData.at(i).set_column(3, InterpolatedTranslationComponent);
183 
184  }
185  }
186  return InterpolationData;
187 
188  } catch (...)
189  {
190  std::cerr << "\n\n" << __FILE__ << "," << __LINE__ << "\n" << ">>>>>>>> In "
191  << "::Failed to interpolate the given data!!! Throw up ...\n";
192  throw;
193  }
194 
195 }
std::vector< vnl_matrix_double > matrixInterpolation(vnl_vector< double > DataPoints, std::vector< vnl_matrix_double > DataValues, vnl_vector< double > InterpolationPoints, std::string InterpolationMethod)
Operation: Interpolate transformation matrices.