CustusX  22.04-rc5
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 "vtkForwardDeclarations.h"
14 #include "vtkPolyData.h"
15 #include "vtkCardinalSpline.h"
16 #include "vtkPoints.h"
17 #include "vtkCellArray.h"
18 #include "vtkCamera.h"
19 #include "vtkParametricSpline.h"
20 #include "vtkSpline.h"
21 
22 #include "cxVBcameraPath.h"
23 #include "cxMesh.h"
24 #include "cxTrackingService.h"
25 #include "cxPatientModelService.h"
26 #include "cxViewServiceProxy.h"
27 #include "cxView.h"
28 #include "cxLogger.h"
29 
30 namespace cx {
31 
33  : mTrackingService(tracker)
34  , mPatientModelService(patientModel)
35  , mViewService(visualization)
36  , mLastCameraViewAngle(0)
37  , mLastCameraRotAngle(0)
38  , mAutomaticRotation(true)
39 {
40  mManualTool = mTrackingService->getManualTool();
41  mSpline = vtkParametricSplinePtr::New();
42  mLastStoredViewVector.Identity();
43 
44 }
45 
47 {
48  if(mRoutePositions.size() > 0)
49  if(mRoutePositions.size() == mCameraRotations.size())
50  {
51  this->generateSplineCurve(mRoutePositions);
52  return;
53  }
54 
55  if(!mesh)
56  {
57  std::cout << "cameraRawPointsSlot is empty !" << std::endl;
58  return;
59  }
60 
61  this->generateSplineCurve(mesh);
62 }
63 
64 void CXVBcameraPath::generateSplineCurve(MeshPtr mesh)
65 {
66  vtkPolyDataPtr polyDataInput = mesh->getTransformedPolyDataCopy(mesh->get_rMd());
67  vtkPoints *vtkpoints = polyDataInput->GetPoints();
68 
69  mNumberOfInputPoints = polyDataInput->GetNumberOfPoints();
70 
71  mNumberOfControlPoints = mNumberOfInputPoints;
72 
73  // Setting the spline curve points
74  // First clean up previous stored data
75  mSpline->GetXSpline()->RemoveAllPoints();
76  mSpline->GetYSpline()->RemoveAllPoints();
77  mSpline->GetZSpline()->RemoveAllPoints();
78 
79  mSpline->SetPoints(vtkpoints);
80 }
81 
82 void CXVBcameraPath::generateSplineCurve(std::vector< Eigen::Vector3d > routePositions)
83 {
84  vtkPointsPtr vtkPoints = vtkPointsPtr::New();
85  for (int i = 0; i < routePositions.size(); i++)
86  vtkPoints->InsertNextPoint(routePositions[i](0),routePositions[i](1),routePositions[i](2));
87 
88  // Setting the spline curve points
89  // First clean up previous stored data
90  mSpline->GetXSpline()->RemoveAllPoints();
91  mSpline->GetYSpline()->RemoveAllPoints();
92  mSpline->GetZSpline()->RemoveAllPoints();
93 
94  mSpline->SetPoints(vtkPoints);
95 
96 }
97 
98 void CXVBcameraPath::cameraPathPositionSlot(int positionPercentage)
99 {
100  double splineParameter = positionPercentageAdjusted(positionPercentage) / 100.0;
101 
102  //Making shorter focus distance at last 20% of path, otherwise the camera might be outside of the smallest branches.
103  //Longer focus makes smoother turns at the first divisions.
104  double splineFocusDistance = 0.05;
105  if (splineParameter > 0.8)
106  splineFocusDistance = 0.02;
107 
108  double pos_r[3], focus_r[3], d_r[3];
109  double splineParameterArray[3];
110  splineParameterArray[0] = splineParameter;
111  splineParameterArray[1] = splineParameter;
112  splineParameterArray[2] = splineParameter;
113 
114  mSpline->Evaluate(splineParameterArray, pos_r, d_r);
115  splineParameterArray[0] = splineParameter + splineFocusDistance;
116  splineParameterArray[1] = splineParameter + splineFocusDistance;
117  splineParameterArray[2] = splineParameter + splineFocusDistance;
118  mSpline->Evaluate(splineParameterArray, focus_r, d_r);
119 
120  mLastCameraPos_r = Vector3D(pos_r[0], pos_r[1], pos_r[2]);
121  mLastCameraFocus_r = Vector3D(focus_r[0], focus_r[1], focus_r[2]);
122 
123  if(mAutomaticRotation)
124  if(mRoutePositions.size() > 0 && mRoutePositions.size() == mCameraRotationsSmoothed.size())
125  {
126  int index = (int) (splineParameter * (mRoutePositions.size() - 1));
127  mLastCameraRotAngle = mCameraRotationsSmoothed[index];
128  //CX_LOG_DEBUG() << "mLastCameraRotAngle: " << mLastCameraRotAngle*180/M_PI;
129  }
130 
131  this->updateManualToolPosition();
132 
133 }
134 
135 void CXVBcameraPath::updateManualToolPosition()
136 {
137  Vector3D viewDirection_r;
138  // New View direction
139  if(similar(mLastCameraFocus_r, mLastCameraPos_r, 0.01)) {
140  viewDirection_r = mLastStoredViewVector;
141  } else {
142  viewDirection_r = (mLastCameraFocus_r - mLastCameraPos_r).normalized();
143  mLastStoredViewVector = viewDirection_r;
144  }
145 
146 
147  Vector3D xVector = Vector3D(0,1,0);
148  Vector3D yVector = cross(viewDirection_r, xVector).normalized();
149 
150  // Construct tool transform
151  Transform3D rMt = Transform3D::Identity();
152  rMt.matrix().col(0).head(3) = xVector;
153  rMt.matrix().col(1).head(3) = yVector;
154  rMt.matrix().col(2).head(3) = viewDirection_r;
155  rMt.matrix().col(3).head(3) = mLastCameraPos_r;
156 
157  Transform3D rotateX = createTransformRotateX(mLastCameraViewAngle);
158  Transform3D rotateZ = createTransformRotateZ(mLastCameraRotAngle);
159 
160  Transform3D rMpr = mPatientModelService->get_rMpr();
161  Transform3D prMt = rMpr.inv() * rMt * rotateZ * rotateX;
162 
163  mManualTool->set_prMt(prMt);
164 
165  emit rotationChanged((int) (mLastCameraRotAngle * 180/M_PI));
166 }
167 
168 std::vector< double > CXVBcameraPath::smoothCameraRotations(std::vector< double > cameraRotations)
169 {
170  //Camera rotation is calculated as an average of rotation in the current position and positions ahead.
171  int numberOfElements = cameraRotations.size();
172  std::vector< double > cameraRotationsSmoothed = cameraRotations;
173 
174  //Checking that a second turn/bifurcation is not included in the average
175  int maxPositionsToSmooth = (int) (10 * numberOfElements/100);
176  int positionsToSmooth = maxPositionsToSmooth;
177  for(int i=0; i<numberOfElements; i++)
178  {
179  positionsToSmooth = std::min((int) (positionsToSmooth+.5*numberOfElements/100), maxPositionsToSmooth);
180  bool firstTurnPassed = false;
181  for(int j=i+1; j<std::min(i+positionsToSmooth, numberOfElements); j++)
182  if (cameraRotations[j] != cameraRotations[j-1])
183  {
184  if (firstTurnPassed)
185  {
186  positionsToSmooth = j-i;
187  break;
188  }
189  else
190  firstTurnPassed = true;
191  }
192 
193  std::vector< double > averageElements(cameraRotations.begin()+i, cameraRotations.begin()+std::min(i+positionsToSmooth,numberOfElements-1));
194  if(averageElements.size() > 0)
195  cameraRotationsSmoothed[i] = std::accumulate(averageElements.begin(), averageElements.end(), 0.0) / averageElements.size();
196 
197  }
198  return cameraRotationsSmoothed;
199 }
200 
201 
203 {
204  mLastCameraViewAngle = static_cast<double>(angle) * (M_PI / 180.0);
205  this->updateManualToolPosition();
206 }
207 
209 {
210  mLastCameraRotAngle = static_cast<double>(angle) * (M_PI / 180.0);
211  this->updateManualToolPosition();
212 }
213 
214 void CXVBcameraPath::setRoutePositions(std::vector< Eigen::Vector3d > routePositions)
215 {
216  mRoutePositions = routePositions;
217 }
218 
219 void CXVBcameraPath::setCameraRotations(std::vector< double > cameraRotations)
220 {
221  mCameraRotations = cameraRotations;
222  mCameraRotationsSmoothed = smoothCameraRotations(mCameraRotations);
223 }
224 
225 void CXVBcameraPath::setAutomaticRotation(bool automaticRotation)
226 {
227  mAutomaticRotation = automaticRotation;
228 }
229 
230 double positionPercentageAdjusted(double positionPercentage)
231 {
232  //Adjusting position to make smaller steps towards end of route
233  return 2*positionPercentage / (1 + positionPercentage/100.0);
234 }
235 } /* namespace cx */
void rotationChanged(int value)
void setAutomaticRotation(bool automaticRotation)
void cameraRawPointsSlot(MeshPtr mesh)
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 cameraPathPositionSlot(int positionPercentage)
void cameraRotateAngleSlot(int angle)
Vector3D cross(const Vector3D &a, const Vector3D &b)
compute cross product of a and b.
Definition: cxVector3D.cpp:41
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)
#define M_PI
Namespace for all CustusX production code.