Fraxinus  2023.01.05-dev+develop.0da12
An IGT application
cxPositionStorageFile.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 "cxPositionStorageFile.h"
13 #include <QDateTime>
14 #include <boost/cstdint.hpp>
15 #include "cxFrame3D.h"
16 #include "cxTime.h"
17 
18 
19 namespace cx
20 {
21 
22 
23 PositionStorageReader::PositionStorageReader(QString filename) : positions(filename)
24 {
25  mError = false;
26  positions.open(QIODevice::ReadOnly);
27  stream.setDevice(&positions);
28  stream.setByteOrder(QDataStream::LittleEndian);
29 
30  char header[7];
31  memset(header, 0, sizeof(header));
32  stream.readRawData(header, 6);
33  mVersion = 0;
34  stream >> mVersion;
35 
36  if (!positions.exists())
37  {
38  std::cout << "Error: File [" << filename.toStdString() << "] does not exist." << std::endl;
39  }
40 
41  if (QString(header)!="SNWPOS" || mVersion<1)
42  {
43  //std::cout << QString(header).toStdString() << "-" << int(mVersion) << std::endl;
44  std::cout << "Error in header for file [" << filename.toStdString() << "]" << std::endl;
45  positions.close();
46  }
47 }
48 
50 {
51 
52 }
53 
55 {
56  return mVersion;
57 }
58 
59 bool PositionStorageReader::read(Transform3D* matrix, double* timestamp, int* toolIndex)
60 {
61  if (atEnd())
62  return false;
63 
64  quint8 type;
65  quint8 size;
66  quint64 ts;
67  quint8 tool;
68 // Frame3D frame;
69 
70  stream >> type;
71  stream >> size;
72  stream >> ts;
73  stream >> tool;
74 // stream >> frame.mThetaXY >> frame.mThetaZ >> frame.mPhi;
75 // stream >> frame.mPos[0] >> frame.mPos[1] >> frame.mPos[2];
76  Frame3D frame = this->frameFromStream();
77 
78  *matrix = frame.transform();
79  *timestamp = ts;
80  *toolIndex = tool;
81 
82  return (type==1) && (size==15);
83 }
84 
85 bool PositionStorageReader::read(Transform3D* matrix, double* timestamp, QString* toolUid)
86 {
87  if (this->atEnd())
88  return false;
89 
90  quint8 type;
91  quint8 size;
92 
93  stream >> type; // read type and make ready for a new read below
94 
95  while (type==2) // change tool format
96  {
97  if (this->atEnd())
98  return false;
99  stream >> size;
100  char* data = NULL;
101  uint isize = 0;
102  stream.readBytes(data, isize);
103  mCurrentToolUid = QString(QByteArray(data, size));
104  delete[] data;
105 
106  stream >> type; // read type and make ready for a new read below
107  }
108 
109  if (type==1) // tool-on-line format
110  {
111  quint64 ts;
112  quint8 tool;
113 // Frame3D frame;
114 // double thetaXY;
115 // double thetaZ;
116 // double phi;
117 
118  stream >> size;
119  stream >> ts;
120  stream >> tool;
121 // stream >> thetaXY >> thetaZ >> phi;
123 // stream >> frame.mPos[0] >> frame.mPos[1] >> frame.mPos[2];
124 // frame.mAngleAxis = Eigen::AngleAxisd(phi, unitVector(thetaXY, thetaZ));
125  Frame3D frame = this->frameFromStream();
126 
127  *matrix = frame.transform();
128  *timestamp = ts;
129  *toolUid = QString::number(tool);
130  return (size==69);
131  }
132  else if (type==3) // position only format
133  {
134  quint64 ts;
135 // Frame3D frame;
136 // double thetaXY;
137 // double thetaZ;
138 // double phi;
139 
140  stream >> size;
141  stream >> ts;
142 // stream >> thetaXY >> thetaZ >> phi;
143  Frame3D frame = this->frameFromStream();
144 // stream >> frame.mThetaXY >> frame.mThetaZ >> frame.mPhi;
145 // stream >> frame.mPos[0] >> frame.mPos[1] >> frame.mPos[2];
146 // frame.mAngleAxis = Eigen::AngleAxisd(phi, unitVector(thetaXY, thetaZ));
147 
148  *matrix = frame.transform();
149  *timestamp = ts;
150  *toolUid = mCurrentToolUid;
151  return (size==68);
152  }
153 
154  mError = true;
155  return false;
156 }
157 
158 Frame3D PositionStorageReader::frameFromStream()
159 {
160  boost::array<double, 6> rep;
161  stream >> rep[0] >> rep[1] >> rep[2] >> rep[3] >> rep[4] >> rep[5];
163  return retval;
164 }
165 
167 {
168  return !positions.isReadable() || stream.atEnd() || mError;
169 }
170 
175 {
176  QDateTime retval;
177  boost::uint64_t ts = static_cast<boost::uint64_t>(timestamp);
178  retval.setTime_t(ts/1000);
179  retval = retval.addMSecs(ts%1000);
180  return retval.toString(timestampMilliSecondsFormatNice());
181 }
182 
183 
184 //---------------------------------------------------------
185 //---------------------------------------------------------
186 //---------------------------------------------------------
187 
188 
189 PositionStorageWriter::PositionStorageWriter(QString filename) : positions(filename)
190 {
191  positions.open(QIODevice::Append);
192  stream.setDevice(&positions);
193  stream.setByteOrder(QDataStream::LittleEndian);
194  if (positions.size() == 0)
195  {
196  stream.writeRawData("SNWPOS", 6);
197  stream << (quint8)2; // version 1 had only 32 bit timestamps
198  }
199 }
200 
202 {
203  positions.close();
204 }
205 
206 
207 void PositionStorageWriter::write(Transform3D matrix, uint64_t timestamp, int toolIndex)
208 {
209  Frame3D frame = Frame3D::create(matrix);
210 
211  stream << (quint8)1; // Type - there is only one
212  stream << (quint8)(8+1+6*10); // Size of data following this point
213  stream << (quint64)timestamp; // Milliseconds since Epoch
214  stream << (quint8)toolIndex; // tool index
215  stream << (double)getThetaXY(frame.mAngleAxis.axis());
216  stream << (double)getThetaZ(frame.mAngleAxis.axis());
217  stream << (double)frame.mAngleAxis.angle();
218 // stream << (double)frame.mThetaXY;
219 // stream << (double)frame.mThetaZ;
220 // stream << (double)frame.mPhi;
221  stream << (double)frame.mPos[0];
222  stream << (double)frame.mPos[1];
223  stream << (double)frame.mPos[2];
224 }
225 
226 void PositionStorageWriter::write(Transform3D matrix, uint64_t timestamp, QString toolUid)
227 {
228  if (toolUid!=mCurrentToolUid)
229  {
230  QByteArray name = toolUid.toLatin1();
231 
232  stream << (quint8)2; // Type - tool change
233  stream << (quint8)(name.size()+4); // Size of data following this point
234  stream.writeBytes(name.data(), name.size());
235  mCurrentToolUid = toolUid;
236  }
237  else
238  {
239  Frame3D frame = Frame3D::create(matrix);
240  boost::array<double, 6> rep = frame.getCompactAxisAngleRep();
241 
242  stream << (quint8)3; // Type -
243  stream << (quint8)(8+6*10); // Size of data following this point
244  stream << (quint64)timestamp; // Milliseconds since Epoch
245 // stream << (double)frame.mThetaXY;
246 // stream << (double)frame.mThetaZ;
247 // stream << (double)frame.mPhi;
248  stream << rep[0];
249  stream << rep[1];
250  stream << rep[2];
251  stream << rep[3];
252  stream << rep[4];
253  stream << rep[5];
254  }
255 
256 }
257 
258 
259 } // namespace cx
static QString timestampToString(double timestamp)
PositionStorageReader(QString filename)
static Frame3D fromCompactAxisAngleRep(const boost::array< double, 6 > &rep)
Definition: cxFrame3D.cpp:180
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
Transform3D transform() const
Definition: cxFrame3D.cpp:140
Defines an axis-angle representation of a position+orientation in 3D space.
Definition: cxFrame3D.h:69
double getThetaXY(Vector3D k)
get thetaXY, meaning the angle of v projected onto the xy plane
Definition: cxVector3D.cpp:65
boost::array< double, 6 > getCompactAxisAngleRep() const
Definition: cxFrame3D.cpp:168
double getThetaZ(Vector3D k)
get thetaZ, z meaning the elevation from the xy plane
Definition: cxVector3D.cpp:70
static Frame3D create(const Transform3D &transform)
Definition: cxFrame3D.cpp:128
bool read(Transform3D *matrix, double *timestamp, int *toolIndex)
Vector3D mPos
position
Definition: cxFrame3D.h:89
void write(Transform3D matrix, uint64_t timestamp, int toolIndex)
Eigen::AngleAxisd mAngleAxis
angle-axis representation of rotation
Definition: cxFrame3D.h:88
QString timestampMilliSecondsFormatNice()
Definition: cxTime.cpp:30
Namespace for all CustusX production code.