Fraxinus  2023.01.05-dev+develop.0da12
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"
24 #include "cxTool.h"
25 #include "cxTrackingService.h"
26 #include <cxActiveToolWidget.h>
27 #include "cxDoubleWidgets.h"
28 #include "cxVisServices.h"
30 
31 namespace cx
32 {
33 
34 //------------------------------------------------------------------------------
36  BaseWidget(parent, "lap_frame_tool_calibration_widget", "LapFrame Calibrate"),
37  mServices(services),
38  mCalibrateButton(new QPushButton("Calibrate")),
39  mReferencePointLabel(new QLabel("Ref. point:")),
40  mTestButton(new QPushButton("Test calibration")),
41  mCalibrationLabel(new QLabel("Calibration: \n")),
42  mDeltaLabel(new QLabel("Delta:"))
43 {
44  QVBoxLayout* toplayout = new QVBoxLayout(this);
45 
46  mCameraAngleAdapter = DoubleProperty::initialize("Camera Angle", "",
47  "Additional tilt of calibration around tool y-axis,\nfor use with cameras tilted relative to tool direction",
48  0.0, DoubleRange(-M_PI/2, M_PI/2, M_PI/180), 0);
49  mCameraAngleAdapter->setInternal2Display(180.0/M_PI);
50 
51  mCalibRefTool = StringPropertySelectTool::New(mServices->tracking());
52  mCalibRefTool->setValueName("Calibration Frame");
53  mCalibRefTool->setHelp("Select Calibration Reference Frame");
54 
55  mCalibratingTool = StringPropertySelectTool::New(mServices->tracking());
56  mCalibratingTool->setValueName("Tool");
57  mCalibratingTool->setHelp("Select which Tool to calibrate");
58 
59  this->setToolTip("Calibrate tool matrix using a custom frame");
60 
61  // toplayout->addWidget(new QLabel("<b>Select a tool with a known reference point:</b>"));
62  toplayout->addWidget(new LabeledComboBoxWidget(this, mCalibRefTool));
63  toplayout->addWidget(mReferencePointLabel);
64  toplayout->addWidget(new LabeledComboBoxWidget(this, mCalibratingTool));
65  // toplayout->addWidget(new ActiveToolWidget(this));
66  toplayout->addWidget(new SpinBoxAndSliderGroupWidget(this, mCameraAngleAdapter));
67  toplayout->addWidget(mCalibrateButton);
68  toplayout->addWidget(mCalibrationLabel);
69  toplayout->addWidget(this->createHorizontalLine());
70  toplayout->addWidget(mTestButton);
71  toplayout->addWidget(mDeltaLabel);
72  toplayout->addStretch();
73 
74  mReferencePointLabel->setText("<i> Use only with the special Laparascopic <br>"
75  "calibration frame as reference. </i>");
76 
77  connect(mCalibrateButton, SIGNAL(clicked()), this, SLOT(calibrateSlot()));
78  connect(mTestButton, SIGNAL(clicked()), this, SLOT(testCalibrationSlot()));
79 
80  connect(mCalibRefTool.get(), SIGNAL(changed()), this, SLOT(toolSelectedSlot()));
81 
82  //setting default state
83  this->toolSelectedSlot();
84 
85  connect(mServices->tracking().get(), &cx::TrackingService::stateChanged, this, &LapFrameToolCalibrationWidget::trackingStartedSlot);
86 }
87 
89 {}
90 
91 void LapFrameToolCalibrationWidget::calibrateSlot()
92 {
93  ToolPtr refTool = mCalibRefTool->getTool();
94  ToolPtr tool = mCalibratingTool->getTool();
95  double cameraAngle = mCameraAngleAdapter->getValue();
96  if(!refTool || !tool)
97  {
98  reportError(QString("Calibration prerequisited not met: calref:%1, tool:%2").arg(refTool!=0).arg(tool!=0) );
99  return;
100  }
101  if(!refTool->getVisible() || !tool->getVisible() || !refTool->hasReferencePointWithId("1"))
102  {
103  reportError(QString("Calibration prerequisited not met: calref vis:%1, tool vis :%2, refpoint:%3")
104  .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