Fraxinus  2023.01.05-dev+develop.0da12
An IGT application
cxVBcameraPath.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 <iostream>
13 #include <QFile>
14 #include "vtkForwardDeclarations.h"
15 #include "vtkPolyData.h"
16 #include "vtkCardinalSpline.h"
17 #include "vtkPoints.h"
18 #include "vtkCellArray.h"
19 #include "vtkCamera.h"
20 #include "vtkParametricSpline.h"
21 #include "vtkSpline.h"
22 
23 #include "cxVBcameraPath.h"
24 #include "cxMesh.h"
25 #include "cxTrackingService.h"
26 #include "cxPatientModelService.h"
27 #include "cxViewServiceProxy.h"
28 #include "cxView.h"
29 #include "cxLogger.h"
30 
31 namespace cx {
32 
34  mTrackingService(tracker),
35  mPatientModelService(patientModel),
36  mViewService(visualization),
37  mLastCameraViewAngle(0),
38  mLastCameraRotAngle(0),
39  mAutomaticRotation(true),
40  mWritePositionsToFile(false)
41 {
42  mManualTool = mTrackingService->getManualTool();
43  mSpline = vtkParametricSplinePtr::New();
44  mLastStoredViewVector.Identity();
45 
46 }
47 
49 {
50  if(mRoutePositions.size() > 0)
51  if(mRoutePositions.size() == mCameraRotations.size())
52  {
53  this->generateSplineCurve(mRoutePositions);
54  return;
55  }
56 
57  if(!mesh)
58  {
59  std::cout << "cameraRawPointsSlot is empty !" << std::endl;
60  return;
61  }
62 
63  this->generateSplineCurve(mesh);
64 }
65 
66 void CXVBcameraPath::generateSplineCurve(MeshPtr mesh)
67 {
68  vtkPolyDataPtr polyDataInput = mesh->getTransformedPolyDataCopy(mesh->get_rMd());
69  vtkPoints *vtkpoints = polyDataInput->GetPoints();
70 
71  mNumberOfInputPoints = polyDataInput->GetNumberOfPoints();
72 
73  mNumberOfControlPoints = mNumberOfInputPoints;
74 
75  // Setting the spline curve points
76  // First clean up previous stored data
77  mSpline->GetXSpline()->RemoveAllPoints();
78  mSpline->GetYSpline()->RemoveAllPoints();
79  mSpline->GetZSpline()->RemoveAllPoints();
80 
81  mSpline->SetPoints(vtkpoints);
82 }
83 
84 void CXVBcameraPath::generateSplineCurve(std::vector< Eigen::Vector3d > routePositions)
85 {
86  vtkPointsPtr vtkPoints = vtkPointsPtr::New();
87  for (int i = 0; i < routePositions.size(); i++)
88  vtkPoints->InsertNextPoint(routePositions[i](0),routePositions[i](1),routePositions[i](2));
89 
90  // Setting the spline curve points
91  // First clean up previous stored data
92  mSpline->GetXSpline()->RemoveAllPoints();
93  mSpline->GetYSpline()->RemoveAllPoints();
94  mSpline->GetZSpline()->RemoveAllPoints();
95 
96  mSpline->SetPoints(vtkPoints);
97 
98 }
99 
100 void CXVBcameraPath::cameraPathPositionSlot(int positionPermillage)
101 {
102  mPositionPercentage = positionPercentageAdjusted(positionPermillage/10.0) / 100.0;
103 
104  //Making shorter focus distance at last 20% of path, otherwise the camera might be outside of the smallest branches.
105  //Longer focus makes smoother turns at the first divisions.
106  double splineFocusDistance = 0.05;
107  if (mPositionPercentage > 0.8)
108  splineFocusDistance = 0.02;
109 
110  double pos_r[3], focus_r[3], d_r[3];
111  double splineParameterArray[3];
112  splineParameterArray[0] = mPositionPercentage;
113  splineParameterArray[1] = mPositionPercentage;
114  splineParameterArray[2] = mPositionPercentage;
115 
116  mSpline->Evaluate(splineParameterArray, pos_r, d_r);
117  splineParameterArray[0] = mPositionPercentage + splineFocusDistance;
118  splineParameterArray[1] = mPositionPercentage + splineFocusDistance;
119  splineParameterArray[2] = mPositionPercentage + splineFocusDistance;
120  mSpline->Evaluate(splineParameterArray, focus_r, d_r);
121 
122  mLastCameraPos_r = Vector3D(pos_r[0], pos_r[1], pos_r[2]);
123  mLastCameraFocus_r = Vector3D(focus_r[0], focus_r[1], focus_r[2]);
124 
125  if(mAutomaticRotation)
126  if(mRoutePositions.size() > 0 && mRoutePositions.size() == mCameraRotationsSmoothed.size())
127  {
128  int index = (int) (mPositionPercentage * (mRoutePositions.size() - 1));
129  mLastCameraRotAngle = mCameraRotationsSmoothed[index];
130  //CX_LOG_DEBUG() << "mLastCameraRotAngle: " << mLastCameraRotAngle*180/M_PI;
131  }
132 
133  this->updateManualToolPosition();
134 
135 }
136 
137 void CXVBcameraPath::updateManualToolPosition()
138 {
139  Vector3D viewDirection_r;
140  // New View direction
141  if(similar(mLastCameraFocus_r, mLastCameraPos_r, 0.01)) {
142  viewDirection_r = mLastStoredViewVector;
143  } else {
144  viewDirection_r = (mLastCameraFocus_r - mLastCameraPos_r).normalized();
145  mLastStoredViewVector = viewDirection_r;
146  }
147 
148 
149  Vector3D xVector = Vector3D(0,1,0);
150  Vector3D yVector = cross(viewDirection_r, xVector).normalized();
151 
152  // Construct tool transform
153  Transform3D rMt = Transform3D::Identity();
154  rMt.matrix().col(0).head(3) = xVector;
155  rMt.matrix().col(1).head(3) = yVector;
156  rMt.matrix().col(2).head(3) = viewDirection_r;
157  rMt.matrix().col(3).head(3) = mLastCameraPos_r;
158 
159  Transform3D rotateX = createTransformRotateX(mLastCameraViewAngle);
160  Transform3D rotateZ = createTransformRotateZ(mLastCameraRotAngle);
161 
162  Transform3D rMpr = mPatientModelService->get_rMpr();
163  Transform3D prMt = rMpr.inv() * rMt * rotateZ * rotateX;
164 
165  mManualTool->set_prMt(prMt);
166 
167  if(mWritePositionsToFile)
168  this->writePositionToFile(prMt);
169 
170 
171  emit rotationChanged((int) (mLastCameraRotAngle * 180/M_PI));
172 }
173 
175 {
176  mWritePositionsToFile = write;
177  if(mWritePositionsToFile)
178  mTimeSinceStartRecording.start();
179 }
180 
182 {
183  mPositionsFilePath = path;
184 }
185 
186 void CXVBcameraPath::writePositionToFile(Transform3D prMt)
187 {
188  QFile positionFile(mPositionsFilePath + "_positions.txt");
189  if (positionFile.open(QIODevice::Append))
190  {
191  QTextStream stream(&positionFile);
192  stream << prMt(0,0) << " " << prMt(0,1) << " " << prMt(0,2) << " " << prMt(0,3) << endl;
193  stream << prMt(1,0) << " " << prMt(1,1) << " " << prMt(1,2) << " " << prMt(1,3) << endl;
194  stream << prMt(2,0) << " " << prMt(2,1) << " " << prMt(2,2) << " " << prMt(2,3) << endl;
195  }
196 
197  QFile timestampFile(mPositionsFilePath + "_timestamps.txt");
198  if (timestampFile.open(QIODevice::Append))
199  {
200  QTextStream stream(&timestampFile);
201  stream << mTimeSinceStartRecording.elapsed() << endl;
202  }
203 
204  QFile branchingPositionFile(mPositionsFilePath + "_branching.txt");
205  if (branchingPositionFile.open(QIODevice::Append))
206  {
207  bool branchingPoint = 0;
208  int originalRouteIndex = (int) (mRoutePositions.size()-1) * (1-mPositionPercentage);
209  if (std::find(mBranchingIndex.begin(), mBranchingIndex.end(), originalRouteIndex) != mBranchingIndex.end())
210  branchingPoint = 1;
211  QTextStream stream(&branchingPositionFile);
212  stream << branchingPoint << endl;
213  }
214 }
215 
216 
217 
218 std::vector< double > CXVBcameraPath::smoothCameraRotations(std::vector< double > cameraRotations)
219 {
220  //Camera rotation is calculated as an average of rotation in the current position and positions ahead.
221  int numberOfElements = cameraRotations.size();
222  std::vector< double > cameraRotationsSmoothed = cameraRotations;
223 
224  //Checking that a second turn/bifurcation is not included in the average
225  int maxPositionsToSmooth = (int) (10 * numberOfElements/100);
226  int positionsToSmooth = maxPositionsToSmooth;
227  for(int i=0; i<numberOfElements; i++)
228  {
229  positionsToSmooth = std::min((int) (positionsToSmooth+.5*numberOfElements/100), maxPositionsToSmooth);
230  bool firstTurnPassed = false;
231  for(int j=i+1; j<std::min(i+positionsToSmooth, numberOfElements); j++)
232  if (cameraRotations[j] != cameraRotations[j-1])
233  {
234  if (firstTurnPassed)
235  {
236  positionsToSmooth = j-i;
237  break;
238  }
239  else
240  firstTurnPassed = true;
241  }
242 
243  std::vector< double > averageElements(cameraRotations.begin()+i, cameraRotations.begin()+std::min(i+positionsToSmooth,numberOfElements-1));
244  if(averageElements.size() > 0)
245  cameraRotationsSmoothed[i] = std::accumulate(averageElements.begin(), averageElements.end(), 0.0) / averageElements.size();
246 
247  }
248  return cameraRotationsSmoothed;
249 }
250 
251 
253 {
254  mLastCameraViewAngle = static_cast<double>(angle) * (M_PI / 180.0);
255  this->updateManualToolPosition();
256 }
257 
259 {
260  mLastCameraRotAngle = static_cast<double>(angle) * (M_PI / 180.0);
261  this->updateManualToolPosition();
262 }
263 
264 void CXVBcameraPath::setRoutePositions(std::vector< Eigen::Vector3d > routePositions)
265 {
266  mRoutePositions = routePositions;
267 }
268 
269 void CXVBcameraPath::setCameraRotations(std::vector< double > cameraRotations)
270 {
271  mCameraRotations = cameraRotations;
272  mCameraRotationsSmoothed = smoothCameraRotations(mCameraRotations);
273 }
274 
275 void CXVBcameraPath::setBranchingIndexAlongRoute(std::vector< int > branchingIndex)
276 {
277  mBranchingIndex = branchingIndex;
278 }
279 
280 void CXVBcameraPath::setAutomaticRotation(bool automaticRotation)
281 {
282  mAutomaticRotation = automaticRotation;
283 }
284 
285 double positionPercentageAdjusted(double positionPercentage)
286 {
287  //Adjusting position to make smaller steps towards end of route
288  return 2*positionPercentage / (1 + positionPercentage/100.0);
289 }
290 } /* namespace cx */
void setWritePositionsFilePath(QString path)
void rotationChanged(int value)
void setAutomaticRotation(bool automaticRotation)
void cameraRawPointsSlot(MeshPtr mesh)
void cameraPathPositionSlot(int positionPermillage)
double positionPercentageAdjusted(double positionPercentage)
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
boost::shared_ptr< class TrackingService > TrackingServicePtr
void setCameraRotations(std::vector< double > cameraRotations)
vtkSmartPointer< vtkPoints > vtkPointsPtr
boost::shared_ptr< class ViewService > ViewServicePtr
void cameraRotateAngleSlot(int angle)
Vector3D cross(const Vector3D &a, const Vector3D &b)
compute cross product of a and b.
Definition: cxVector3D.cpp:41
void setBranchingIndexAlongRoute(std::vector< int > branchingIndex)
boost::shared_ptr< class PatientModelService > PatientModelServicePtr
void cameraViewAngleSlot(int angle)
vtkSmartPointer< vtkPolyData > vtkPolyDataPtr
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:42
void setRoutePositions(std::vector< Eigen::Vector3d > routePositions)
bool similar(const CameraInfo &lhs, const CameraInfo &rhs, double tol)
Transform3D createTransformRotateZ(const double angle)
Transform3D createTransformRotateX(const double angle)
boost::shared_ptr< class Mesh > MeshPtr
CXVBcameraPath(TrackingServicePtr tracker, PatientModelServicePtr patientModel, ViewServicePtr visualization)
void setWritePositionsToFile(bool write)
#define M_PI
Namespace for all CustusX production code.