00001
00054 #include <endian.h>
00055
00056 #if __BYTE_ORDER == __BIG_ENDIAN
00057 uint64_t swapLong(uint64_t x) {
00058 x = (x & 0x00000000FFFFFFFF) << 32 | (x & 0xFFFFFFFF00000000) >> 32;
00059 x = (x & 0x0000FFFF0000FFFF) << 16 | (x & 0xFFFF0000FFFF0000) >> 16;
00060 x = (x & 0x00FF00FF00FF00FF) << 8 | (x & 0xFF00FF00FF00FF00) >> 8;
00061 return x;
00062 }
00063
00064 #define __Swap2Bytes(val) ( (((val) >> 8) & 0x00FF) | (((val) << 8) & 0xFF00) )
00065 #define __Swap2BytesD(val) (val>7490?0:( (((val) >> 8) & 0x00FF) | (((val) << 8) & 0xFF00) ))
00066 #define __Swap4Bytes(dword) ( ((dword>>24)&0x000000FF) | ((dword>>8)&0x0000FF00) | ((dword<<8)&0x00FF0000) | ((dword<<24)&0xFF000000) )
00067 #define __Swap8Bytes(val) (swapLong(val))
00068 #else
00069 #define __Swap2Bytes(val) (val)
00070 #define __Swap2BytesD(val) (val>7490?0:val)
00071 #define __Swap4Bytes(dword) (dword)
00072 #define __Swap8Bytes(dword) (dword)
00073 #endif
00074
00075
00076
00077 struct CameraParameters {
00078 uint32_t width, height;
00079 double cam2worldMatrix[4*4];
00080 double fx, fy, cx, cy, k1, k2, f2rc;
00081
00082 CameraParameters(uint32_t width=176, uint32_t height=144,
00083 double *cam2worldMatrix=NULL,
00084 double fx=146.5, double fy=146.5,double cx=84.4,double cy=71.2,
00085 double k1=0.326442, double k2=0.219623,
00086 double f2rc=0.0):
00087 width(width), height(height),
00088 fx(fx), fy(fy),
00089 cx(cx), cy(cy),
00090 k1(k1), k2(k2),
00091 f2rc(f2rc)
00092 {
00093 if(cam2worldMatrix)
00094 memcpy(this->cam2worldMatrix, cam2worldMatrix, sizeof(this->cam2worldMatrix));
00095 }
00096 };
00097
00098
00099
00100 class Data {
00101 CameraParameters cameraParams_;
00102 cv::Mat distance_, intensity_, confidence_;
00103 int numBytesPerDistanceValue_, numBytesPerIntensityValue_, numBytesPerConfidenceValue_;
00104 uint32_t framenumber_;
00105 public:
00106
00107 Data() : framenumber_(0) {
00108 }
00109
00110 const cv::Mat &get_distance() const {return distance_;}
00111 const cv::Mat &get_intensity() const {return intensity_;}
00112 const cv::Mat &get_confidence() const {return confidence_;}
00113 uint32_t get_framenumber() const { return framenumber_; }
00114
00115 const CameraParameters &getCameraParameters() const {return cameraParams_;}
00116
00117
00118 static size_t actual_size(const char *data, const size_t size) {
00119 if(size<8) return 0;
00120
00121 const uint32_t pkglength = ntohl( *(uint32_t*)(data+4) );
00122
00123 return pkglength+9;
00124 }
00125
00126
00127 static bool check_header(const char *data, const size_t size) {
00128 if(size<11) return false;
00129
00130 const uint32_t magicword = ntohl( *(uint32_t*)(data+0) );
00131
00132
00133 return magicword != 0x02020202 || size>=actual_size(data, size);
00134 }
00135
00136
00137 bool read(const char *data, const size_t size) {
00138
00139 const uint32_t magicword = ntohl( *(uint32_t*)(data+0) );
00140 const uint32_t pkglength = ntohl( *(uint32_t*)(data+4) );
00141 const uint16_t protocolVersion = ntohs( *(uint16_t*)(data+8) );
00142 const uint8_t packetType = *(uint8_t*)(data+10);
00143
00144 ROS_ASSERT( magicword == 0x02020202 );
00145 ROS_ASSERT( protocolVersion == 1 );
00146 ROS_ASSERT( packetType == 98 );
00147
00148 if( magicword != 0x02020202 || protocolVersion != 1 || packetType != 98 )
00149 return false;
00150
00151
00152
00153 const uint16_t id = ntohs( *(uint16_t*)(data+11) );
00154 const uint16_t numSegments = ntohs( *(uint16_t*)(data+13) );
00155 ROS_ASSERT( id == 1 );
00156 ROS_ASSERT( numSegments == 3 );
00157
00158
00159 uint32_t offset[numSegments];
00160 uint32_t changedCounter[numSegments];
00161 for(size_t i=0; i<(size_t)numSegments; i++) {
00162 offset[i] = 11 + ntohl( *(uint32_t*)(data+(i*8+15)) );
00163 changedCounter[i] = ntohl( *(uint32_t*)(data+(i*8+19)) );
00164 }
00165
00166
00167 std::string xmlSegment(data+offset[0], offset[1]-offset[0]);
00168
00169
00170 parseXML(xmlSegment);
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183 ROS_ASSERT(numBytesPerDistanceValue_==2);
00184 ROS_ASSERT(numBytesPerIntensityValue_==2);
00185 ROS_ASSERT(numBytesPerConfidenceValue_==2);
00186
00187 distance_ = cv::Mat(cameraParams_.height, cameraParams_.width, CV_16UC1);
00188 intensity_ = cv::Mat(cameraParams_.height, cameraParams_.width, CV_16UC1);
00189 confidence_ = cv::Mat(cameraParams_.height, cameraParams_.width, CV_16UC1);
00190
00191 getData(data+offset[1], size-offset[1]);
00192
00193 return true;
00194 }
00195
00196 private:
00197
00198 bool parseXML(const std::string &xmlString) {
00199 numBytesPerConfidenceValue_ = numBytesPerIntensityValue_ = numBytesPerConfidenceValue_ = -1;
00200
00201 boost::property_tree::ptree pt;
00202 std::istringstream ss(xmlString);
00203 try {
00204 boost::property_tree::xml_parser::read_xml(ss, pt);
00205 } catch(...) {
00206 ROS_ERROR("failed to parse response (XML malformed)\ncontent: %s", xmlString.c_str());
00207 return false;
00208 }
00209
00210 boost::property_tree::ptree ds = pt.get_child("SickRecord.DataSets.DataSetDepthMap.FormatDescriptionDepthMap.DataStream");
00211 cameraParams_.width = ds.get<uint32_t>("Width");
00212 cameraParams_.height = ds.get<uint32_t>("Height");
00213
00214 int i=0;
00215 BOOST_FOREACH(const boost::property_tree::ptree::value_type &item, ds.get_child("CameraToWorldTransform")) {
00216 if(i<16)
00217 cameraParams_.cam2worldMatrix[i] = item.second.get_value<double>();
00218 ++i;
00219 }
00220 ROS_ASSERT(i==16);
00221
00222 cameraParams_.fx = ds.get<double>("CameraMatrix.FX");
00223 cameraParams_.fy = ds.get<double>("CameraMatrix.FY");
00224 cameraParams_.cx = ds.get<double>("CameraMatrix.CX");
00225 cameraParams_.cy = ds.get<double>("CameraMatrix.CY");
00226
00227 cameraParams_.k1 = ds.get<double>("CameraDistortionParams.K1");
00228 cameraParams_.k2 = ds.get<double>("CameraDistortionParams.K2");
00229
00230 cameraParams_.f2rc = ds.get<double>("FocalToRayCross");
00231
00232
00233 std::string data_type;
00234
00235 data_type=ds.get<std::string>("Distance", "");
00236 boost::algorithm::to_lower(data_type);
00237 if(data_type != "") {
00238 ROS_ASSERT(data_type == "uint16");
00239 numBytesPerDistanceValue_ = 2;
00240 }
00241
00242 data_type=ds.get<std::string>("Intensity", "");
00243 boost::algorithm::to_lower(data_type);
00244 if(data_type != "") {
00245 ROS_ASSERT(data_type == "uint16");
00246 numBytesPerIntensityValue_ = 2;
00247 }
00248
00249 data_type=ds.get<std::string>("Confidence", "");
00250 boost::algorithm::to_lower(data_type);
00251 if(data_type != "") {
00252 ROS_ASSERT(data_type == "uint16");
00253 numBytesPerConfidenceValue_ = 2;
00254 }
00255
00256 return true;
00257 }
00258
00259 void getData(const char *data, const size_t size) {
00260 ROS_ASSERT(size>=14);
00261 if(size<14) {
00262 ROS_WARN("malformed data (1): %d<14", (int)size);
00263 return;
00264 }
00265 const size_t numBytesDistance = (numBytesPerDistanceValue_ > 0) ? distance_.total()*numBytesPerDistanceValue_ : 0;
00266 const size_t numBytesIntensity = (numBytesPerIntensityValue_ > 0) ? intensity_.total()*numBytesPerIntensityValue_ : 0;
00267 const size_t numBytesConfidence = (numBytesPerConfidenceValue_ > 0) ? confidence_.total()*numBytesPerConfidenceValue_ : 0;
00268
00269
00270
00271 size_t offset = 0;
00272 const uint32_t length = __Swap4Bytes( *(uint32_t*)(data+offset) );
00273 const uint64_t timeStamp = __Swap8Bytes( *(uint64_t*)(data+offset+4) );
00274 const uint16_t version = __Swap2Bytes( *(uint16_t*)(data+offset+12) );
00275
00276 offset += 14;
00277
00278 if(length>size) {
00279 ROS_WARN("malformed data (2): %d>%d", (int)length, (int)size);
00280 return;
00281 }
00282
00283 if (version > 1) {
00284
00285 framenumber_ = ntohl( *(uint32_t*)(data+offset) );
00286 const uint8_t dataQuality = *(uint8_t*)(data+offset+4);
00287 const uint8_t deviceStatus = *(uint8_t*)(data+offset+5);
00288 offset += 6;
00289 } else {
00290 ++framenumber_;
00291 }
00292
00293 size_t end = offset + numBytesDistance + numBytesIntensity + numBytesConfidence;
00294
00295
00296
00297
00298 ROS_ASSERT(end<=size);
00299
00300 if (numBytesDistance > 0) {
00301 for(size_t i=0; i<distance_.total(); i++)
00302 distance_.at<uint16_t>(i) = __Swap2BytesD( *(uint16_t*)(data+offset+i*2) );
00303 offset += numBytesDistance;
00304 }
00305 else {
00306 distance_.setTo(0);
00307 }
00308 if (numBytesIntensity > 0) {
00309 for(size_t i=0; i<intensity_.total(); i++)
00310 intensity_.at<uint16_t>(i) = __Swap2Bytes( *(uint16_t*)(data+offset+i*2) );
00311 offset += numBytesIntensity;
00312 }
00313 else {
00314 intensity_.setTo(std::numeric_limits<uint16_t>::max()/2);
00315 }
00316 if (numBytesConfidence > 0) {
00317 for(size_t i=0; i<confidence_.total(); i++)
00318 confidence_.at<uint16_t>(i) = __Swap2Bytes( *(uint16_t*)(data+offset+i*2) );
00319 offset += numBytesConfidence;
00320 }
00321 else {
00322 confidence_.setTo(std::numeric_limits<uint16_t>::max());
00323 }
00324
00325
00326 const uint32_t unusedCrc = __Swap4Bytes( *(uint32_t*)(data+offset) );
00327 const uint32_t lengthCopy = __Swap4Bytes( *(uint32_t*)(data+offset + 4) );
00328
00329 if(length != lengthCopy)
00330 ROS_WARN("check failed %d!=%d", (int)length, (int)lengthCopy);
00331 ROS_ASSERT(length == lengthCopy);
00332 }
00333
00334 };
00335
00336 #undef __Swap2Bytes