NorMIT-nav  16.5
An IGT application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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) 2008-2014, SINTEF Department of Medical Technology
5 All rights reserved.
6 
7 Redistribution and use in source and binary forms, with or without
8 modification, are permitted provided that the following conditions are met:
9 
10 1. Redistributions of source code must retain the above copyright notice,
11  this list of conditions and the following disclaimer.
12 
13 2. Redistributions in binary form must reproduce the above copyright notice,
14  this list of conditions and the following disclaimer in the documentation
15  and/or other materials provided with the distribution.
16 
17 3. Neither the name of the copyright holder nor the names of its contributors
18  may be used to endorse or promote products derived from this software
19  without specific prior written permission.
20 
21 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
25 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
26 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 =========================================================================*/
32 
33 #include "cxPositionStorageFile.h"
34 #include <QDateTime>
35 #include <boost/cstdint.hpp>
36 #include "cxFrame3D.h"
37 #include "cxTime.h"
38 
39 
40 namespace cx
41 {
42 
43 
44 PositionStorageReader::PositionStorageReader(QString filename) : positions(filename)
45 {
46  mError = false;
47  positions.open(QIODevice::ReadOnly);
48  stream.setDevice(&positions);
49  stream.setByteOrder(QDataStream::LittleEndian);
50 
51  char header[7];
52  memset(header, 0, sizeof(header));
53  stream.readRawData(header, 6);
54  mVersion = 0;
55  stream >> mVersion;
56 
57  if (!positions.exists())
58  {
59  std::cout << "Error: File [" << filename.toStdString() << "] does not exist." << std::endl;
60  }
61 
62  if (QString(header)!="SNWPOS" || mVersion<1)
63  {
64  //std::cout << QString(header).toStdString() << "-" << int(mVersion) << std::endl;
65  std::cout << "Error in header for file [" << filename.toStdString() << "]" << std::endl;
66  positions.close();
67  }
68 }
69 
71 {
72 
73 }
74 
76 {
77  return mVersion;
78 }
79 
80 bool PositionStorageReader::read(Transform3D* matrix, double* timestamp, int* toolIndex)
81 {
82  if (atEnd())
83  return false;
84 
85  quint8 type;
86  quint8 size;
87  quint64 ts;
88  quint8 tool;
89 // Frame3D frame;
90 
91  stream >> type;
92  stream >> size;
93  stream >> ts;
94  stream >> tool;
95 // stream >> frame.mThetaXY >> frame.mThetaZ >> frame.mPhi;
96 // stream >> frame.mPos[0] >> frame.mPos[1] >> frame.mPos[2];
97  Frame3D frame = this->frameFromStream();
98 
99  *matrix = frame.transform();
100  *timestamp = ts;
101  *toolIndex = tool;
102 
103  return (type==1) && (size==15);
104 }
105 
106 bool PositionStorageReader::read(Transform3D* matrix, double* timestamp, QString* toolUid)
107 {
108  if (this->atEnd())
109  return false;
110 
111  quint8 type;
112  quint8 size;
113 
114  stream >> type; // read type and make ready for a new read below
115 
116  while (type==2) // change tool format
117  {
118  if (this->atEnd())
119  return false;
120  stream >> size;
121  char* data = NULL;
122  uint isize = 0;
123  stream.readBytes(data, isize);
124  mCurrentToolUid = QString(QByteArray(data, size));
125  delete[] data;
126 
127  stream >> type; // read type and make ready for a new read below
128  }
129 
130  if (type==1) // tool-on-line format
131  {
132  quint64 ts;
133  quint8 tool;
134 // Frame3D frame;
135 // double thetaXY;
136 // double thetaZ;
137 // double phi;
138 
139  stream >> size;
140  stream >> ts;
141  stream >> tool;
142 // stream >> thetaXY >> thetaZ >> phi;
144 // stream >> frame.mPos[0] >> frame.mPos[1] >> frame.mPos[2];
145 // frame.mAngleAxis = Eigen::AngleAxisd(phi, unitVector(thetaXY, thetaZ));
146  Frame3D frame = this->frameFromStream();
147 
148  *matrix = frame.transform();
149  *timestamp = ts;
150  *toolUid = QString::number(tool);
151  return (size==69);
152  }
153  else if (type==3) // position only format
154  {
155  quint64 ts;
156 // Frame3D frame;
157 // double thetaXY;
158 // double thetaZ;
159 // double phi;
160 
161  stream >> size;
162  stream >> ts;
163 // stream >> thetaXY >> thetaZ >> phi;
164  Frame3D frame = this->frameFromStream();
165 // stream >> frame.mThetaXY >> frame.mThetaZ >> frame.mPhi;
166 // stream >> frame.mPos[0] >> frame.mPos[1] >> frame.mPos[2];
167 // frame.mAngleAxis = Eigen::AngleAxisd(phi, unitVector(thetaXY, thetaZ));
168 
169  *matrix = frame.transform();
170  *timestamp = ts;
171  *toolUid = mCurrentToolUid;
172  return (size==68);
173  }
174 
175  mError = true;
176  return false;
177 }
178 
179 Frame3D PositionStorageReader::frameFromStream()
180 {
181  boost::array<double, 6> rep;
182  stream >> rep[0] >> rep[1] >> rep[2] >> rep[3] >> rep[4] >> rep[5];
184  return retval;
185 }
186 
188 {
189  return !positions.isReadable() || stream.atEnd() || mError;
190 }
191 
196 {
197  QDateTime retval;
198  boost::uint64_t ts = static_cast<boost::uint64_t>(timestamp);
199  retval.setTime_t(ts/1000);
200  retval = retval.addMSecs(ts%1000);
201  return retval.toString(timestampMilliSecondsFormatNice());
202 }
203 
204 
205 //---------------------------------------------------------
206 //---------------------------------------------------------
207 //---------------------------------------------------------
208 
209 
210 PositionStorageWriter::PositionStorageWriter(QString filename) : positions(filename)
211 {
212  positions.open(QIODevice::Append);
213  stream.setDevice(&positions);
214  stream.setByteOrder(QDataStream::LittleEndian);
215  if (positions.size() == 0)
216  {
217  stream.writeRawData("SNWPOS", 6);
218  stream << (quint8)2; // version 1 had only 32 bit timestamps
219  }
220 }
221 
223 {
224  positions.close();
225 }
226 
227 
228 void PositionStorageWriter::write(Transform3D matrix, uint64_t timestamp, int toolIndex)
229 {
230  Frame3D frame = Frame3D::create(matrix);
231 
232  stream << (quint8)1; // Type - there is only one
233  stream << (quint8)(8+1+6*10); // Size of data following this point
234  stream << (quint64)timestamp; // Milliseconds since Epoch
235  stream << (quint8)toolIndex; // tool index
236  stream << (double)getThetaXY(frame.mAngleAxis.axis());
237  stream << (double)getThetaZ(frame.mAngleAxis.axis());
238  stream << (double)frame.mAngleAxis.angle();
239 // stream << (double)frame.mThetaXY;
240 // stream << (double)frame.mThetaZ;
241 // stream << (double)frame.mPhi;
242  stream << (double)frame.mPos[0];
243  stream << (double)frame.mPos[1];
244  stream << (double)frame.mPos[2];
245 }
246 
247 void PositionStorageWriter::write(Transform3D matrix, uint64_t timestamp, QString toolUid)
248 {
249  if (toolUid!=mCurrentToolUid)
250  {
251  QByteArray name = toolUid.toLatin1();
252 
253  stream << (quint8)2; // Type - tool change
254  stream << (quint8)(name.size()+4); // Size of data following this point
255  stream.writeBytes(name.data(), name.size());
256  mCurrentToolUid = toolUid;
257  }
258  else
259  {
260  Frame3D frame = Frame3D::create(matrix);
261  boost::array<double, 6> rep = frame.getCompactAxisAngleRep();
262 
263  stream << (quint8)3; // Type -
264  stream << (quint8)(8+6*10); // Size of data following this point
265  stream << (quint64)timestamp; // Milliseconds since Epoch
266 // stream << (double)frame.mThetaXY;
267 // stream << (double)frame.mThetaZ;
268 // stream << (double)frame.mPhi;
269  stream << rep[0];
270  stream << rep[1];
271  stream << rep[2];
272  stream << rep[3];
273  stream << rep[4];
274  stream << rep[5];
275  }
276 
277 }
278 
279 
280 } // namespace cx
static QString timestampToString(double timestamp)
PositionStorageReader(QString filename)
static Frame3D fromCompactAxisAngleRep(const boost::array< double, 6 > &rep)
Definition: cxFrame3D.cpp:201
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
Transform3D transform() const
Definition: cxFrame3D.cpp:161
Defines an axis-angle representation of a position+orientation in 3D space.
Definition: cxFrame3D.h:90
double getThetaXY(Vector3D k)
get thetaXY, meaning the angle of v projected onto the xy plane
Definition: cxVector3D.cpp:86
boost::array< double, 6 > getCompactAxisAngleRep() const
Definition: cxFrame3D.cpp:189
double getThetaZ(Vector3D k)
get thetaZ, z meaning the elevation from the xy plane
Definition: cxVector3D.cpp:91
static Frame3D create(const Transform3D &transform)
Definition: cxFrame3D.cpp:149
bool read(Transform3D *matrix, double *timestamp, int *toolIndex)
Vector3D mPos
position
Definition: cxFrame3D.h:110
void write(Transform3D matrix, uint64_t timestamp, int toolIndex)
Eigen::AngleAxisd mAngleAxis
angle-axis representation of rotation
Definition: cxFrame3D.h:109
QString timestampMilliSecondsFormatNice()
Definition: cxTime.cpp:51