Fraxinus  18.10
An IGT application
cxLapFrameToolCalibrationWidget.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 
14 
15 #include <QPushButton>
16 #include <QTextStream>
17 #include <QFileDialog>
18 #include <QMessageBox>
19 #include "cxTypeConversions.h"
20 #include "cxLogger.h"
21 #include "cxTrackingService.h"
22 #include "cxVector3D.h"
23 #include "cxDefinitionStrings.h"
25 #include "cxTool.h"
26 #include "cxTrackingService.h"
27 #include <cxActiveToolWidget.h>
28 #include "cxDoubleWidgets.h"
29 #include "cxVisServices.h"
31 
32 namespace cx
33 {
34 
35 //------------------------------------------------------------------------------
37  BaseWidget(parent, "lap_frame_tool_calibration_widget", "LapFrame Calibrate"),
38  mServices(services),
39  mCalibrateButton(new QPushButton("Calibrate")),
40  mReferencePointLabel(new QLabel("Ref. point:")),
41  mTestButton(new QPushButton("Test calibration")),
42  mCalibrationLabel(new QLabel("Calibration: \n")),
43  mDeltaLabel(new QLabel("Delta:"))
44 {
45  QVBoxLayout* toplayout = new QVBoxLayout(this);
46 
47  mCameraAngleAdapter = DoubleProperty::initialize("Camera Angle", "",
48  "Additional tilt of calibration around tool y-axis,\nfor use with cameras tilted relative to tool direction",
49  0.0, DoubleRange(-M_PI/2, M_PI/2, M_PI/180), 0);
50  mCameraAngleAdapter->setInternal2Display(180.0/M_PI);
51 
52  mCalibRefTool = StringPropertySelectTool::New(mServices->tracking());
53  mCalibRefTool->setValueName("Calibration Frame");
54  mCalibRefTool->setHelp("Select Calibration Reference Frame");
55 
56  mCalibratingTool = StringPropertySelectTool::New(mServices->tracking());
57  mCalibratingTool->setValueName("Tool");
58  mCalibratingTool->setHelp("Select which Tool to calibrate");
59 
60  this->setToolTip("Calibrate tool matrix using a custom frame");
61 
62 // toplayout->addWidget(new QLabel("<b>Select a tool with a known reference point:</b>"));
63  toplayout->addWidget(new LabeledComboBoxWidget(this, mCalibRefTool));
64  toplayout->addWidget(mReferencePointLabel);
65  toplayout->addWidget(new LabeledComboBoxWidget(this, mCalibratingTool));
66 // toplayout->addWidget(new ActiveToolWidget(this));
67  toplayout->addWidget(new SpinBoxAndSliderGroupWidget(this, mCameraAngleAdapter));
68  toplayout->addWidget(mCalibrateButton);
69  toplayout->addWidget(mCalibrationLabel);
70  toplayout->addWidget(this->createHorizontalLine());
71  toplayout->addWidget(mTestButton);
72  toplayout->addWidget(mDeltaLabel);
73  toplayout->addStretch();
74 
75  mReferencePointLabel->setText("<i> Use only with the special Laparascopic <br>"
76  "calibration frame as reference. </i>");
77 
78  connect(mCalibrateButton, SIGNAL(clicked()), this, SLOT(calibrateSlot()));
79  connect(mTestButton, SIGNAL(clicked()), this, SLOT(testCalibrationSlot()));
80 
81  connect(mCalibRefTool.get(), SIGNAL(changed()), this, SLOT(toolSelectedSlot()));
82 
83  //setting default state
84  this->toolSelectedSlot();
85 
86  connect(mServices->tracking().get(), &cx::TrackingService::stateChanged, this, &LapFrameToolCalibrationWidget::trackingStartedSlot);
87 }
88 
90 {}
91 
92 void LapFrameToolCalibrationWidget::calibrateSlot()
93 {
94  ToolPtr refTool = mCalibRefTool->getTool();
95  ToolPtr tool = mCalibratingTool->getTool();
96  double cameraAngle = mCameraAngleAdapter->getValue();
97  if(!refTool || !tool)
98  {
99  reportError(QString("Calibration prerequisited not met: calref:%1, tool:%2").arg(refTool!=0).arg(tool!=0) );
100  return;
101  }
102  if(!refTool->getVisible() || !tool->getVisible() || !refTool->hasReferencePointWithId(1))
103  {
104  reportError(QString("Calibration prerequisited not met: calref vis:%1, tool vis :%2, refpoint:%3").arg(refTool->getVisible()).arg(tool->getVisible()).arg(refTool->hasReferencePointWithId(1)) );
105  return;
106  }
107 
108  LapFrameToolCalibrationCalculator calc(tool, refTool, cameraAngle);
109  Transform3D calibration = calc.get_calibration_sMt();
110 
111  QMessageBox msgBox;
112  msgBox.setText("Do you want to overwrite "+tool->getName()+"'s calibration file?");
113  msgBox.setInformativeText("This cannot be undone.");
114  msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel);
115  msgBox.setDefaultButton(QMessageBox::Ok);
116  int ret = msgBox.exec();
117 
118  if(ret == QMessageBox::Ok)
119  {
120  try
121  {
122  tool->setCalibration_sMt(calibration);
123  }
124  catch(std::exception& e)
125  {
126  QMessageBox msgBox2;
127  msgBox2.setText("Unknown error, could not calibrate the tool: "+tool->getName()+".");
128  msgBox2.setInformativeText(QString(e.what()));
129  msgBox2.setStandardButtons(QMessageBox::Ok);
130  msgBox2.setDefaultButton(QMessageBox::Ok);
131  int ret2 = msgBox2.exec();
132  return;
133  }
134  mCalibrationLabel->setText(QString("Calibration matrix for %1:\n%2").arg(tool->getName(), qstring_cast(calibration)));
135  }
136 }
137 
138 void LapFrameToolCalibrationWidget::testCalibrationSlot()
139 {
140  ToolPtr refTool = mCalibRefTool->getTool();
141  ToolPtr tool = mCalibratingTool->getTool();
142  double cameraAngle = mCameraAngleAdapter->getValue();
143 
144  if(!refTool || !tool || !refTool->hasReferencePointWithId(1))
145  return;
146 
147  LapFrameToolCalibrationCalculator calc(tool, refTool, cameraAngle);
148  Vector3D delta_selectedTool = calc.get_delta_ref();
149 
150  QString delta = QString("%1 mm").arg(delta_selectedTool.length(), 6, 'g', 1);
151  mDeltaLabel->setText("<b>Delta "+tool->getName()+":</b> "+qstring_cast(delta_selectedTool)+" <br> <b>Accuracy:</b> " + delta);
152 
153  report("Delta "+tool->getName()+": "+qstring_cast(delta_selectedTool)+" Length: "+ delta);
154 
155 
156 }
157 
158 void LapFrameToolCalibrationWidget::toolSelectedSlot()
159 {
160  // QString text("Ref. point: <UNDEFINED POINT>");
161  mCalibrateButton->setEnabled(false);
162  mTestButton->setEnabled(false);
163 
164  if (mCalibRefTool->getTool())
165  {
166 // mCalibrationLabel->setText("Calibration:\n" + qstring_cast(mCalibratingTool->getTool()->getCalibration_sMt()));
167 // Transform3D calibration = mCalibratingTool->getTool()->getCalibration_sMt();
168 // mCalibrationLabel->setText(QString("Calibration matrix for %1:\n%2").arg(tool->getName(), qstring_cast(calibration)));
169  mCalibrateButton->setEnabled(true);
170  mTestButton->setEnabled(true);
171  }
172 
173  // mReferencePointLabel->setText(text);
174 }
175 //------------------------------------------------------------------------------
176 
177 void LapFrameToolCalibrationWidget::trackingStartedSlot()
178 {
179  ToolPtr ref = mServices->tracking()->getTool("calibration_tool");
180  if (ref)
181  mCalibRefTool->setValue(ref->getUid());
182 }
183 
184 //------------------------------------------------------------------------------
185 //------------------------------------------------------------------------------
186 //------------------------------------------------------------------------------
187 
188 
189 
190 
192  mTool(tool), mCalibrationRef(calRef), mCameraAngle(cameraAngle)
193 {
194  m_sMpr = mTool->getCalibration_sMt() * mTool->get_prMt().inv();
195 
196 // m_qMcr = Transform3D::fromString(" 0.0, 0.0, -1.0, -71.5,"
197 // " 0.0, -1.0, 0.0, -8.0,"
198 // "-1.0, 0.0, 0.0, -8.8,"
199 // " 0.0, 0.0, 0.0, 1.0");
200 
201  m_qMcr = Transform3D::Identity();
202  m_qMpr = m_qMcr * mCalibrationRef->get_prMt().inv();
203 }
204 
206 {
207  Vector3D p(0,0,0);
208  Transform3D qMpr = m_qMcr * mCalibrationRef->get_prMt().inv();
209 
210  Vector3D calibPoint_pr = qMpr.inv().coord(p);
211  Vector3D toolPoint_pr = mTool->get_prMt().coord(p);
212  return calibPoint_pr - toolPoint_pr;
213 }
214 
216 {
217  Transform3D calibration = m_sMpr * m_qMpr.inv() * createTransformRotateY(mCameraAngle);
218  this->useOnlyRotationalPart(&calibration);
219  return calibration;
220 }
221 
232 void LapFrameToolCalibrationCalculator::useOnlyRotationalPart(Transform3D* transform)
233 {
234  Transform3D::LinearMatrixType rotationalPart = transform->rotation();
235  transform->linear() = rotationalPart;
236 }
237 
238 }
QString qstring_cast(const T &val)
DoubleBoundingBox3D transform(const Transform3D &m, const DoubleBoundingBox3D &bb)
Transform3D createTransformRotateY(const double angle)
void reportError(QString msg)
Definition: cxLogger.cpp:71
boost::shared_ptr< class VisServices > VisServicesPtr
Definition: cxMainWindow.h:40
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
Utility class for describing a bounded numeric range.
Definition: cxDoubleRange.h:32
Transform3D get_calibration_sMt()
new calibration matrix for the input tool.
Vector3D get_delta_ref()
how far from the reference point the sampled point is, in pr&#39;s coord
Composite widget for string selection.
Composite widget for scalar data manipulation.
static QFrame * createHorizontalLine()
Creates a horizontal line which can be inserted into widgets.
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:42
Interface for QWidget which handles widgets uniformly for the system.
Definition: cxBaseWidget.h:88
static StringPropertySelectToolPtr New(TrackingServicePtr trackingService)
void report(QString msg)
Definition: cxLogger.cpp:69
static DoublePropertyPtr initialize(const QString &uid, QString name, QString help, double value, DoubleRange range, int decimals, QDomNode root=QDomNode())
LapFrameToolCalibrationWidget(VisServicesPtr services, QWidget *parent)
LapFrameToolCalibrationCalculator(ToolPtr tool, ToolPtr calRef, double cameraAngle)
#define M_PI
Namespace for all CustusX production code.
boost::shared_ptr< class Tool > ToolPtr