NorMIT-nav  16.5
An IGT application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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) 2008-2014, SINTEF Department of Medical Technology
5 All rights reserved.
6 
7 Redistribution and use in source and binary forms, with or without
8 modification, are permitted provided that the following conditions are met:
9 
10 1. Redistributions of source code must retain the above copyright notice,
11  this list of conditions and the following disclaimer.
12 
13 2. Redistributions in binary form must reproduce the above copyright notice,
14  this list of conditions and the following disclaimer in the documentation
15  and/or other materials provided with the distribution.
16 
17 3. Neither the name of the copyright holder nor the names of its contributors
18  may be used to endorse or promote products derived from this software
19  without specific prior written permission.
20 
21 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
25 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
26 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 =========================================================================*/
32 
33 #include "matrixInterpolation.h"
34 //#include <vector>
35 //#include "itkArray.h"
36 //#include "itkArray2D.h"
37 #include <iostream>
38 //#include "vnl/vnl_matrix.h"
39 //#include "vnl/vnl_vector.h"
40 //typedef vnl_matrix<double> vnl_matrix_double;
41 
42 
43 std::vector<vnl_matrix_double> matrixInterpolation(vnl_vector<double> DataPoints,
44  std::vector<vnl_matrix_double> DataValues, vnl_vector<double> InterpolationPoints, std::string InterpolationMethod)
45 {
46  try
47  {
48  if (DataPoints.size() != DataValues.size())
49  {
50  std::cerr << "\n\n" << __FILE__ << "," << __LINE__ << "\n" << ">>>>>>>> In "
51  << "::Number of input data points differs from number of input data values!!! Throw up ...\n";
52  throw;
53  }
54 
55  std::vector<vnl_matrix_double> InterpolationData(InterpolationPoints.size());
56 
57  if (InterpolationMethod.compare("closest point") == 0) // Closest point "interpolation"
58  {
59  unsigned int i = 1;
60  for (unsigned int j = 0; j < InterpolationPoints.size(); j++)
61  {
62  while (DataPoints[i] < InterpolationPoints[j] && i < DataPoints.size() - 1)
63  {
64  i++;
65  }
66 
67  if (std::abs(DataPoints[i - 1] - InterpolationPoints[j]) < std::abs(DataPoints[i] - InterpolationPoints[j]))
68  {
69  InterpolationData[j] = DataValues[i - 1];
70  }
71  else
72  {
73  InterpolationData[j] = DataValues[i];
74  }
75  }
76 
77  }
78 
79  else if (InterpolationMethod.compare("linear") == 0) // Linear interpolation
80  {
81  unsigned int j = 0;
82  for (unsigned int i = 0; i < InterpolationPoints.size(); i++)
83  {
84  while (DataPoints.get(j + 1) < InterpolationPoints.get(i) && j + 1 < DataPoints.size() - 1)
85  {
86  j++;
87  }
88  double t = (InterpolationPoints.get(i) - DataPoints.get(j)) / (DataPoints.get(j + 1) - DataPoints.get(j));
89 // std::cout << "t= " << t << std::endl;
90 
91  // Translation component interpolation
92  // -----------------------------------------------
93 
94  vnl_vector<double> InterpolatedTranslationComponent(4);
95  for (int k = 0; k < 3; k++)
96  {
97  InterpolatedTranslationComponent.put(k, (1 - t) * DataValues.at(j).get(k, 3) + t * DataValues.at(j + 1).get(
98  k, 3));
99  }
100 // std::cout << DataValues.at(j).get(0, 3) << " " << DataValues.at(j).get(1, 3) << " " << DataValues.at(j).get(2, 3) << std::endl;
101  InterpolatedTranslationComponent.put(3, 1);
102 
103  // Rotation matrix interpolation
104  // -----------------------------------------------
105  // This procedure is taken from Eberly (2008), "Rotation
106  // Representations and Performance Issues" found at
107  // http://www.geometrictools.com/.
108 
109  // Step 1. Extract the rotational parts of the
110  // transformation matrix and compute a matrix R.
111 
112  vnl_matrix<double> P = DataValues.at(j).extract(3, 3);
113  vnl_matrix<double> Q = DataValues.at(j + 1).extract(3, 3);
114  vnl_matrix<double> R = P.transpose() * Q;
115 
116  // Step 2. Compute an axis-angle representation of R.
117 
118  vnl_vector<double> A(3, 0);
119  double Argument = (R.get(0, 0) + R.get(1, 1) + R.get(2, 2) - 1) / 2;
120  // Due to roundoff error, the argument can become
121  // slightly larger than 1, causing an invalid input to
122  // acos. In these cases, it is assumed that the rotation
123  // is negligable, and the argument is set to 1 (making
124  // theta 0).
125  if (Argument > 1)
126  {
127  Argument = 1;
128  }
129  double theta = acos(Argument);
130 
131  if (theta == 0)
132  {
133  // There is no rotation, and the vector is set to an
134  // arbitrary unit vector ([1 0 0]).
135  A.put(0, 1);
136  }
137  else if (theta < 3.14159265)
138  {
139  A.put(0, R.get(2, 1) - R.get(1, 2));
140  A.put(1, R.get(0, 2) - R.get(2, 0));
141  A.put(2, R.get(1, 0) - R.get(0, 1));
142  A.normalize();
143  }
144  else
145  {
146  if (R.get(0, 0) > R.get(1, 1) && R.get(0, 0) > R.get(2, 2))
147  {
148  A.put(0, sqrt(R.get(0, 0) - R.get(1, 1) - R.get(2, 2) + 1) / 2);
149  A.put(1, R.get(0, 1) / (2 * A.get(0)));
150  A.put(2, R.get(0, 2) / (2 * A.get(0)));
151  }
152  else if (R.get(1, 1) > R.get(0, 0) && R.get(0, 0) > R.get(2, 2))
153  {
154  A.put(1, sqrt(R.get(1, 1) - R.get(0, 0) - R.get(2, 2) + 1) / 2);
155  A.put(0, R.get(0, 1) / (2 * A.get(1)));
156  A.put(2, R.get(1, 2) / (2 * A.get(1)));
157  }
158  else
159  {
160  A.put(2, sqrt(R.get(2, 2) - R.get(1, 1) - R.get(0, 0) + 1) / 2);
161  A.put(1, R.get(1, 2) / (2 * A.get(2)));
162  A.put(0, R.get(0, 2) / (2 * A.get(2)));
163  }
164  }
165 
166  // Step 3. Multiply the angle theta by t and convert
167  // back to rotation matrix representation.
168 
169  vnl_matrix<double> S(3, 3, 0);
170  vnl_matrix<double> I(3, 3);
171  vnl_matrix<double> Rt(3, 3);
172 
173  S.put(0, 1, -A.get(2));
174  S.put(0, 2, A.get(1));
175  S.put(1, 0, A.get(2));
176  S.put(1, 2, -A.get(0));
177  S.put(2, 0, -A.get(1));
178  S.put(2, 1, A.get(0));
179 
180  I.set_identity();
181 
182  Rt = I + sin(t * theta) * S + (1 - cos(t * theta)) * S * S;
183 
184  // Step 4. Compute the interpolated matrix, known as the
185  // slerp (spherical linear interpolation).
186 
187  vnl_matrix<double> InterpolatedRotationComponent = P * Rt;
188 
189  // Transformation matrix composition
190  // -----------------------------------------------
191  // Compose a 4x4 transformation matrix from the
192  // interpolated translation and rotation components.
193 
194  InterpolationData.at(i).set_size(4, 4);
195  InterpolationData.at(i).fill(0);
196  for (int r = 0; r < 3; r++)
197  {
198  for (int c = 0; c < 3; c++)
199  {
200  InterpolationData.at(i).put(r, c, InterpolatedRotationComponent.get(r, c));
201  }
202  }
203  InterpolationData.at(i).set_column(3, InterpolatedTranslationComponent);
204 
205  }
206  }
207  return InterpolationData;
208 
209  } catch (...)
210  {
211  std::cerr << "\n\n" << __FILE__ << "," << __LINE__ << "\n" << ">>>>>>>> In "
212  << "::Failed to interpolate the given data!!! Throw up ...\n";
213  throw;
214  }
215 
216 }
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.