data.h
Go to the documentation of this file.
1 
41 #include <endian.h>
42 
43 #if __BYTE_ORDER == __BIG_ENDIAN
44 uint64_t swapLong(uint64_t x) {
45  x = (x & 0x00000000FFFFFFFF) << 32 | (x & 0xFFFFFFFF00000000) >> 32;
46  x = (x & 0x0000FFFF0000FFFF) << 16 | (x & 0xFFFF0000FFFF0000) >> 16;
47  x = (x & 0x00FF00FF00FF00FF) << 8 | (x & 0xFF00FF00FF00FF00) >> 8;
48  return x;
49 }
50 
51 #define __Swap2Bytes(val) ( (((val) >> 8) & 0x00FF) | (((val) << 8) & 0xFF00) )
52 #define __Swap2BytesD(val) (val>7490?0:( (((val) >> 8) & 0x00FF) | (((val) << 8) & 0xFF00) ))
53 #define __Swap4Bytes(dword) ( ((dword>>24)&0x000000FF) | ((dword>>8)&0x0000FF00) | ((dword<<8)&0x00FF0000) | ((dword<<24)&0xFF000000) )
54 #define __Swap8Bytes(val) (swapLong(val))
55 #else
56 #define __Swap2Bytes(val) (val)
57 #define __Swap2BytesD(val) (val>7490?0:val)
58 #define __Swap4Bytes(dword) (dword)
59 #define __Swap8Bytes(dword) (dword)
60 #endif
61 
62 
63 /* This class gathers the main camera parameters. */
65  uint32_t width, height;
66  double cam2worldMatrix[4*4];
67  double fx, fy, cx, cy, k1, k2, f2rc;
68 
69  CameraParameters(uint32_t width=176, uint32_t height=144,
70  double *cam2worldMatrix=NULL,
71  double fx=146.5, double fy=146.5,double cx=84.4,double cy=71.2,
72  double k1=0.326442, double k2=0.219623,
73  double f2rc=0.0):
74  width(width), height(height),
75  fx(fx), fy(fy),
76  cx(cx), cy(cy),
77  k1(k1), k2(k2),
78  f2rc(f2rc)
79  {
80  if(cam2worldMatrix)
81  memcpy(this->cam2worldMatrix, cam2worldMatrix, sizeof(this->cam2worldMatrix));
82  }
83 };
84 
85 
86 /* Gathers methods to handle the raw data. */
87 class Data {
89  cv::Mat distance_, intensity_, confidence_;
90  int numBytesPerDistanceValue_, numBytesPerIntensityValue_, numBytesPerConfidenceValue_;
91  uint32_t framenumber_;
92 public:
93 
94  Data() : framenumber_(0) {
95  }
96 
97  const cv::Mat &get_distance() const {return distance_;}
98  const cv::Mat &get_intensity() const {return intensity_;}
99  const cv::Mat &get_confidence() const {return confidence_;}
100  uint32_t get_framenumber() const { return framenumber_; }
101 
102  const CameraParameters &getCameraParameters() const {return cameraParams_;}
103 
104  /* Get size of complete package. */
105  static size_t actual_size(const char *data, const size_t size) {
106  if(size<8) return 0;
107  // first 11 bytes contain some internal definitions
108  const uint32_t pkglength = ntohl( *(uint32_t*)(data+4) );
109 
110  return pkglength+9;
111  }
112 
113  /* Check if there are enough data to be parsed. */
114  static bool check_header(const char *data, const size_t size) {
115  if(size<11) return false;
116 
117  const uint32_t magicword = ntohl( *(uint32_t*)(data+0) );
118 
119  //if magic word in invalid we'll read the content anyway to skip data
120  return magicword != 0x02020202 || size>=actual_size(data, size);
121  }
122 
123  /* Extracts necessary data segments and triggers parsing of segments. */
124  bool read(const char *data, const size_t size) {
125  // first 11 bytes contain some internal definitions
126  const uint32_t magicword = ntohl( *(uint32_t*)(data+0) );
127  const uint32_t pkglength = ntohl( *(uint32_t*)(data+4) );
128  const uint16_t protocolVersion = ntohs( *(uint16_t*)(data+8) );
129  const uint8_t packetType = *(uint8_t*)(data+10);
130 
131  ROS_ASSERT( magicword == 0x02020202 );
132  ROS_ASSERT( protocolVersion == 1 );
133  ROS_ASSERT( packetType == 98 );
134 
135  if( magicword != 0x02020202 || protocolVersion != 1 || packetType != 98 )
136  return false;
137 
138  // next four bytes an id (should equal 1) and
139  // the number of segments (should be 3)
140  const uint16_t id = ntohs( *(uint16_t*)(data+11) );
141  const uint16_t numSegments = ntohs( *(uint16_t*)(data+13) );
142  ROS_ASSERT( id == 1 );
143  ROS_ASSERT( numSegments == 3 );
144 
145  // offset and changedCounter, 4 bytes each per segment
146  uint32_t offset[numSegments];
147  uint32_t changedCounter[numSegments];
148  for(size_t i=0; i<(size_t)numSegments; i++) {
149  offset[i] = 11 + ntohl( *(uint32_t*)(data+(i*8+15)) );
150  changedCounter[i] = ntohl( *(uint32_t*)(data+(i*8+19)) );
151  }
152 
153  // XML segment describes the data format
154  std::string xmlSegment(data+offset[0], offset[1]-offset[0]);
155 
156  // parsing the XML in order to extract necessary image information
157  parseXML(xmlSegment);
158 
159  /*self.cameraParams = \
160  CameraParameters(width=myXMLParser.imageWidth,
161  height=myXMLParser.imageHeight,
162  cam2worldMatrix=myXMLParser.cam2worldMatrix,
163  fx=myXMLParser.fx,fy=myXMLParser.fy,
164  cx=myXMLParser.cx,cy=myXMLParser.cy,
165  k1=myXMLParser.k1,k2=myXMLParser.k2,
166  f2rc=myXMLParser.f2rc)*/
167 
168  // extracting data from the binary segment (distance, intensity
169  // and confidence).
170  ROS_ASSERT(numBytesPerDistanceValue_==2);
171  ROS_ASSERT(numBytesPerIntensityValue_==2);
172  ROS_ASSERT(numBytesPerConfidenceValue_==2);
173 
174  distance_ = cv::Mat(cameraParams_.height, cameraParams_.width, CV_16UC1);
175  intensity_ = cv::Mat(cameraParams_.height, cameraParams_.width, CV_16UC1);
176  confidence_ = cv::Mat(cameraParams_.height, cameraParams_.width, CV_16UC1);
177 
178  getData(data+offset[1], size-offset[1]);
179 
180  return true;
181  }
182 
183 private:
184  /* Parse method needs the XML segment as string input. */
185  bool parseXML(const std::string &xmlString) {
186  numBytesPerConfidenceValue_ = numBytesPerIntensityValue_ = numBytesPerConfidenceValue_ = -1;
187 
188  boost::property_tree::ptree pt;
189  std::istringstream ss(xmlString);
190  try {
191  boost::property_tree::xml_parser::read_xml(ss, pt);
192  } catch(...) {
193  ROS_ERROR("failed to parse response (XML malformed)\ncontent: %s", xmlString.c_str());
194  return false;
195  }
196 
197  boost::property_tree::ptree ds = pt.get_child("SickRecord.DataSets.DataSetDepthMap.FormatDescriptionDepthMap.DataStream");
198  cameraParams_.width = ds.get<uint32_t>("Width");
199  cameraParams_.height = ds.get<uint32_t>("Height");
200 
201  int i=0;
202  BOOST_FOREACH(const boost::property_tree::ptree::value_type &item, ds.get_child("CameraToWorldTransform")) {
203  if(i<16)
204  cameraParams_.cam2worldMatrix[i] = item.second.get_value<double>();
205  ++i;
206  }
207  ROS_ASSERT(i==16);
208 
209  cameraParams_.fx = ds.get<double>("CameraMatrix.FX");
210  cameraParams_.fy = ds.get<double>("CameraMatrix.FY");
211  cameraParams_.cx = ds.get<double>("CameraMatrix.CX");
212  cameraParams_.cy = ds.get<double>("CameraMatrix.CY");
213 
214  cameraParams_.k1 = ds.get<double>("CameraDistortionParams.K1");
215  cameraParams_.k2 = ds.get<double>("CameraDistortionParams.K2");
216 
217  cameraParams_.f2rc = ds.get<double>("FocalToRayCross");
218 
219  // extract data format from XML (although it does not change)
220  std::string data_type;
221 
222  data_type=ds.get<std::string>("Distance", "");
223  boost::algorithm::to_lower(data_type);
224  if(data_type != "") {
225  ROS_ASSERT(data_type == "uint16");
226  numBytesPerDistanceValue_ = 2;
227  }
228 
229  data_type=ds.get<std::string>("Intensity", "");
230  boost::algorithm::to_lower(data_type);
231  if(data_type != "") {
232  ROS_ASSERT(data_type == "uint16");
233  numBytesPerIntensityValue_ = 2;
234  }
235 
236  data_type=ds.get<std::string>("Confidence", "");
237  boost::algorithm::to_lower(data_type);
238  if(data_type != "") {
239  ROS_ASSERT(data_type == "uint16");
240  numBytesPerConfidenceValue_ = 2;
241  }
242 
243  return true;
244  }
245 
246  void getData(const char *data, const size_t size) {
247  ROS_ASSERT(size>=14);
248  if(size<14) {
249  ROS_WARN("malformed data (1): %d<14", (int)size);
250  return;
251  }
252  const size_t numBytesDistance = (numBytesPerDistanceValue_ > 0) ? distance_.total()*numBytesPerDistanceValue_ : 0;
253  const size_t numBytesIntensity = (numBytesPerIntensityValue_ > 0) ? intensity_.total()*numBytesPerIntensityValue_ : 0;
254  const size_t numBytesConfidence = (numBytesPerConfidenceValue_ > 0) ? confidence_.total()*numBytesPerConfidenceValue_ : 0;
255 
256  // the binary part starts with entries for length, a timestamp
257  // and a version identifier
258  size_t offset = 0;
259  const uint32_t length = __Swap4Bytes( *(uint32_t*)(data+offset) );
260  const uint64_t timeStamp = __Swap8Bytes( *(uint64_t*)(data+offset+4) );
261  const uint16_t version = __Swap2Bytes( *(uint16_t*)(data+offset+12) );
262 
263  offset += 14; //calcsize('>IQH')
264 
265  if(length>size) {
266  ROS_WARN("malformed data (2): %d>%d", (int)length, (int)size);
267  return;
268  }
269 
270  if (version > 1) {
271  // more frame information follows in this case: frame number, data quality, device status ('<IBB')
272  framenumber_ = ntohl( *(uint32_t*)(data+offset) );
273  const uint8_t dataQuality = *(uint8_t*)(data+offset+4);
274  const uint8_t deviceStatus = *(uint8_t*)(data+offset+5);
275  offset += 6;
276  } else {
277  ++framenumber_;
278  }
279 
280  size_t end = offset + numBytesDistance + numBytesIntensity + numBytesConfidence; // calculating the end index
281  //wholeBinary = binarySegment[start:end] // whole data block
282  //distance = wholeBinary[0:numBytesDistance] // only the distance
283  // data (as string)
284 
285  ROS_ASSERT(end<=size);
286 
287  if (numBytesDistance > 0) {
288  for(size_t i=0; i<distance_.total(); i++)
289  distance_.at<uint16_t>(i) = __Swap2BytesD( *(uint16_t*)(data+offset+i*2) );
290  offset += numBytesDistance;
291  }
292  else {
293  distance_.setTo(0);
294  }
295  if (numBytesIntensity > 0) {
296  for(size_t i=0; i<intensity_.total(); i++)
297  intensity_.at<uint16_t>(i) = __Swap2Bytes( *(uint16_t*)(data+offset+i*2) );
298  offset += numBytesIntensity;
299  }
300  else {
301  intensity_.setTo(std::numeric_limits<uint16_t>::max()/2);
302  }
303  if (numBytesConfidence > 0) {
304  for(size_t i=0; i<confidence_.total(); i++)
305  confidence_.at<uint16_t>(i) = __Swap2Bytes( *(uint16_t*)(data+offset+i*2) );
306  offset += numBytesConfidence;
307  }
308  else {
309  confidence_.setTo(std::numeric_limits<uint16_t>::max());
310  }
311 
312  // data end with a 32byte (unused) CRC field and a copy of the length byte
313  const uint32_t unusedCrc = __Swap4Bytes( *(uint32_t*)(data+offset) );
314  const uint32_t lengthCopy = __Swap4Bytes( *(uint32_t*)(data+offset + 4) );
315 
316  if(length != lengthCopy)
317  ROS_WARN("check failed %d!=%d", (int)length, (int)lengthCopy);
318  ROS_ASSERT(length == lengthCopy);
319  }
320 
321 };
322 
323 #undef __Swap2Bytes
double f2rc
Definition: data.h:67
int numBytesPerIntensityValue_
Definition: data.h:90
static bool check_header(const char *data, const size_t size)
Definition: data.h:114
#define __Swap4Bytes(dword)
Definition: driver.h:54
uint32_t get_framenumber() const
Definition: data.h:100
bool read(const char *data, const size_t size)
Definition: data.h:124
Data()
Definition: data.h:94
uint32_t height
Definition: data.h:65
double cx
Definition: data.h:67
double cam2worldMatrix[4 *4]
Definition: data.h:66
const CameraParameters & getCameraParameters() const
Definition: data.h:102
double fy
Definition: data.h:67
uint32_t framenumber_
Definition: data.h:91
#define ROS_WARN(...)
#define __Swap2Bytes(val)
Definition: driver.h:52
CameraParameters(uint32_t width=176, uint32_t height=144, double *cam2worldMatrix=NULL, double fx=146.5, double fy=146.5, double cx=84.4, double cy=71.2, double k1=0.326442, double k2=0.219623, double f2rc=0.0)
Definition: data.h:69
void getData(const char *data, const size_t size)
Definition: data.h:246
CameraParameters cameraParams_
Definition: data.h:88
double k1
Definition: data.h:67
double cy
Definition: data.h:67
const cv::Mat & get_distance() const
Definition: data.h:97
uint64_t swapLong(uint64_t x)
Definition: data.h:44
#define __Swap8Bytes(val)
Definition: driver.h:55
#define __Swap2BytesD(val)
Definition: driver.h:53
static size_t actual_size(const char *data, const size_t size)
Definition: data.h:105
bool parseXML(const std::string &xmlString)
Definition: data.h:185
Definition: data.h:87
#define ROS_ASSERT(cond)
double fx
Definition: data.h:67
uint32_t width
Definition: data.h:65
cv::Mat intensity_
Definition: data.h:89
const cv::Mat & get_intensity() const
Definition: data.h:98
#define ROS_ERROR(...)
double k2
Definition: data.h:67
const cv::Mat & get_confidence() const
Definition: data.h:99


sick_visionary_t_driver
Author(s): Joshua Hampp
autogenerated on Mon Jun 10 2019 15:09:27