CustusX  22.04-rc5
An IGT application
cxUSReconstructInputDataAlgoritms.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 
13 
14 namespace cx
15 {
16 
18 {
19  // Transform from image coordinate syst with origin in upper left corner
20  // to t (tool) space. TODO check is u is ul corner or ll corner.
22 
23  //mPos is prMt
24  for (unsigned i = 0; i < data->mPositions.size(); i++)
25  {
26  Transform3D prMt = data->mPositions[i].mPos;
27  data->mPositions[i].mPos = prMt * tMu;
28  }
29  //mPos is prMu
30 }
31 
33 {
34  Transform3D rMpr = data->rMpr;
35  // Transform from image coordinate syst with origin in upper left corner to t (tool) space.
37 
38  //mFrames is prMt
39  for (unsigned i = 0; i < data->mFrames.size(); i++)
40  {
41  Transform3D prMt = data->mFrames[i].mPos;
42  data->mFrames[i].mPos = rMpr * prMt * tMv;
43  }
44  //mFrames is rMu
45 }
46 
48 {
49  std::vector<double> error(data->mFrames.size(), 1000);
50 
51  if (data->mPositions.empty())
52  return error;
53 
54  for (unsigned i_frame = 0; i_frame < data->mFrames.size(); i_frame++)
55  {
56  std::vector<TimedPosition>::iterator posIter;
57  posIter = lower_bound(data->mPositions.begin(), data->mPositions.end(), data->mFrames[i_frame]);
58 
59  unsigned i_pos = distance(data->mPositions.begin(), posIter);
60  if (i_pos != 0)
61  i_pos--;
62 
63  if (i_pos >= data->mPositions.size() - 1)
64  i_pos = data->mPositions.size() - 2;
65 
66  double diff1 = fabs(data->mFrames[i_frame].mTime - data->mPositions[i_pos].mTime);
67  double diff2 = fabs(data->mFrames[i_frame].mTime - data->mPositions[i_pos + 1].mTime);
68  double diff = std::max(diff1, diff2);
69 
70  double t_delta_tracking = data->mPositions[i_pos + 1].mTime - data->mPositions[i_pos].mTime;
71  double t = 0;
72  if (!similar(t_delta_tracking, 0))
73  t = (data->mFrames[i_frame].mTime - data->mPositions[i_pos].mTime) / t_delta_tracking;
74  data->mFrames[i_frame].mPos = slerpInterpolate(data->mPositions[i_pos].mPos, data->mPositions[i_pos + 1].mPos, t);
75  error[i_frame] = diff;
76  }
77 
78  return error;
79 }
80 
82 {
83  //Convert input transforms to Quaternions
84  Eigen::Quaterniond aq = Eigen::Quaterniond(a.matrix().block<3, 3>(0,0));
85  Eigen::Quaterniond bq = Eigen::Quaterniond(b.matrix().block<3, 3>(0,0));
86 
87  Eigen::Quaterniond cq = aq.slerp(t, bq);
88 
89  Transform3D c;
90  c.matrix().block<3, 3>(0, 0) = Eigen::Matrix3d(cq);
91 
92  for (int i = 0; i < 4; i++)
93  c(i, 3) = (1 - t) * a(i, 3) + t * b(i, 3);
94  return c;
95 }
96 
98 {
99  Transform3D c;
100  for (int i = 0; i < 4; i++)
101  for (int j = 0; j < 4; j++)
102  c(i, j) = (1 - t) * a(i, j) + t * b(i, j);
103  return c;
104 }
105 
106 
107 } // namespace cx
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
std::vector< TimedPosition > mFrames
static void transformTrackingPositionsTo_prMu(USReconstructInputData *data)
static std::vector< double > interpolateFramePositionsFromTracking(USReconstructInputData *data)
Transform3D interpolate(const Transform3D &a, const Transform3D &b, double t)
Transform3D rMpr
patient registration
Transform3D get_uMv() const
get transform from inverted image space v (origin in ul corner) to image space u. ...
Transform3D get_tMu() const
get transform from image space u to probe tool space t.
std::vector< TimedPosition > mPositions
bool similar(const CameraInfo &lhs, const CameraInfo &rhs, double tol)
static Transform3D slerpInterpolate(const Transform3D &a, const Transform3D &b, double t)
static void transformFramePositionsTo_rMu(USReconstructInputData *data)
Namespace for all CustusX production code.