00001 #ifndef _ROS_rtabmap_InfoEx_h 00002 #define _ROS_rtabmap_InfoEx_h 00003 00004 #include <stdint.h> 00005 #include <string.h> 00006 #include <stdlib.h> 00007 #include "ros/msg.h" 00008 #include "std_msgs/Header.h" 00009 #include "geometry_msgs/Transform.h" 00010 #include "geometry_msgs/Pose.h" 00011 #include "rtabmap/KeyPoint.h" 00012 00013 namespace rtabmap 00014 { 00015 00016 class InfoEx : public ros::Msg 00017 { 00018 public: 00019 std_msgs::Header header; 00020 int32_t refId; 00021 int32_t loopClosureId; 00022 int32_t localLoopClosureId; 00023 geometry_msgs::Transform mapCorrection; 00024 geometry_msgs::Transform loopClosureTransform; 00025 geometry_msgs::Pose currentPose; 00026 uint8_t posteriorKeys_length; 00027 int32_t st_posteriorKeys; 00028 int32_t * posteriorKeys; 00029 uint8_t posteriorValues_length; 00030 float st_posteriorValues; 00031 float * posteriorValues; 00032 uint8_t likelihoodKeys_length; 00033 int32_t st_likelihoodKeys; 00034 int32_t * likelihoodKeys; 00035 uint8_t likelihoodValues_length; 00036 float st_likelihoodValues; 00037 float * likelihoodValues; 00038 uint8_t rawLikelihoodKeys_length; 00039 int32_t st_rawLikelihoodKeys; 00040 int32_t * rawLikelihoodKeys; 00041 uint8_t rawLikelihoodValues_length; 00042 float st_rawLikelihoodValues; 00043 float * rawLikelihoodValues; 00044 uint8_t weightsKeys_length; 00045 int32_t st_weightsKeys; 00046 int32_t * weightsKeys; 00047 uint8_t weightsValues_length; 00048 int32_t st_weightsValues; 00049 int32_t * weightsValues; 00050 uint8_t statsKeys_length; 00051 char* st_statsKeys; 00052 char* * statsKeys; 00053 uint8_t statsValues_length; 00054 float st_statsValues; 00055 float * statsValues; 00056 uint8_t refWordsKeys_length; 00057 int32_t st_refWordsKeys; 00058 int32_t * refWordsKeys; 00059 uint8_t refWordsValues_length; 00060 rtabmap::KeyPoint st_refWordsValues; 00061 rtabmap::KeyPoint * refWordsValues; 00062 uint8_t loopWordsKeys_length; 00063 int32_t st_loopWordsKeys; 00064 int32_t * loopWordsKeys; 00065 uint8_t loopWordsValues_length; 00066 rtabmap::KeyPoint st_loopWordsValues; 00067 rtabmap::KeyPoint * loopWordsValues; 00068 00069 virtual int serialize(unsigned char *outbuffer) const 00070 { 00071 int offset = 0; 00072 offset += this->header.serialize(outbuffer + offset); 00073 union { 00074 int32_t real; 00075 uint32_t base; 00076 } u_refId; 00077 u_refId.real = this->refId; 00078 *(outbuffer + offset + 0) = (u_refId.base >> (8 * 0)) & 0xFF; 00079 *(outbuffer + offset + 1) = (u_refId.base >> (8 * 1)) & 0xFF; 00080 *(outbuffer + offset + 2) = (u_refId.base >> (8 * 2)) & 0xFF; 00081 *(outbuffer + offset + 3) = (u_refId.base >> (8 * 3)) & 0xFF; 00082 offset += sizeof(this->refId); 00083 union { 00084 int32_t real; 00085 uint32_t base; 00086 } u_loopClosureId; 00087 u_loopClosureId.real = this->loopClosureId; 00088 *(outbuffer + offset + 0) = (u_loopClosureId.base >> (8 * 0)) & 0xFF; 00089 *(outbuffer + offset + 1) = (u_loopClosureId.base >> (8 * 1)) & 0xFF; 00090 *(outbuffer + offset + 2) = (u_loopClosureId.base >> (8 * 2)) & 0xFF; 00091 *(outbuffer + offset + 3) = (u_loopClosureId.base >> (8 * 3)) & 0xFF; 00092 offset += sizeof(this->loopClosureId); 00093 union { 00094 int32_t real; 00095 uint32_t base; 00096 } u_localLoopClosureId; 00097 u_localLoopClosureId.real = this->localLoopClosureId; 00098 *(outbuffer + offset + 0) = (u_localLoopClosureId.base >> (8 * 0)) & 0xFF; 00099 *(outbuffer + offset + 1) = (u_localLoopClosureId.base >> (8 * 1)) & 0xFF; 00100 *(outbuffer + offset + 2) = (u_localLoopClosureId.base >> (8 * 2)) & 0xFF; 00101 *(outbuffer + offset + 3) = (u_localLoopClosureId.base >> (8 * 3)) & 0xFF; 00102 offset += sizeof(this->localLoopClosureId); 00103 offset += this->mapCorrection.serialize(outbuffer + offset); 00104 offset += this->loopClosureTransform.serialize(outbuffer + offset); 00105 offset += this->currentPose.serialize(outbuffer + offset); 00106 *(outbuffer + offset++) = posteriorKeys_length; 00107 *(outbuffer + offset++) = 0; 00108 *(outbuffer + offset++) = 0; 00109 *(outbuffer + offset++) = 0; 00110 for( uint8_t i = 0; i < posteriorKeys_length; i++){ 00111 union { 00112 int32_t real; 00113 uint32_t base; 00114 } u_posteriorKeysi; 00115 u_posteriorKeysi.real = this->posteriorKeys[i]; 00116 *(outbuffer + offset + 0) = (u_posteriorKeysi.base >> (8 * 0)) & 0xFF; 00117 *(outbuffer + offset + 1) = (u_posteriorKeysi.base >> (8 * 1)) & 0xFF; 00118 *(outbuffer + offset + 2) = (u_posteriorKeysi.base >> (8 * 2)) & 0xFF; 00119 *(outbuffer + offset + 3) = (u_posteriorKeysi.base >> (8 * 3)) & 0xFF; 00120 offset += sizeof(this->posteriorKeys[i]); 00121 } 00122 *(outbuffer + offset++) = posteriorValues_length; 00123 *(outbuffer + offset++) = 0; 00124 *(outbuffer + offset++) = 0; 00125 *(outbuffer + offset++) = 0; 00126 for( uint8_t i = 0; i < posteriorValues_length; i++){ 00127 union { 00128 float real; 00129 uint32_t base; 00130 } u_posteriorValuesi; 00131 u_posteriorValuesi.real = this->posteriorValues[i]; 00132 *(outbuffer + offset + 0) = (u_posteriorValuesi.base >> (8 * 0)) & 0xFF; 00133 *(outbuffer + offset + 1) = (u_posteriorValuesi.base >> (8 * 1)) & 0xFF; 00134 *(outbuffer + offset + 2) = (u_posteriorValuesi.base >> (8 * 2)) & 0xFF; 00135 *(outbuffer + offset + 3) = (u_posteriorValuesi.base >> (8 * 3)) & 0xFF; 00136 offset += sizeof(this->posteriorValues[i]); 00137 } 00138 *(outbuffer + offset++) = likelihoodKeys_length; 00139 *(outbuffer + offset++) = 0; 00140 *(outbuffer + offset++) = 0; 00141 *(outbuffer + offset++) = 0; 00142 for( uint8_t i = 0; i < likelihoodKeys_length; i++){ 00143 union { 00144 int32_t real; 00145 uint32_t base; 00146 } u_likelihoodKeysi; 00147 u_likelihoodKeysi.real = this->likelihoodKeys[i]; 00148 *(outbuffer + offset + 0) = (u_likelihoodKeysi.base >> (8 * 0)) & 0xFF; 00149 *(outbuffer + offset + 1) = (u_likelihoodKeysi.base >> (8 * 1)) & 0xFF; 00150 *(outbuffer + offset + 2) = (u_likelihoodKeysi.base >> (8 * 2)) & 0xFF; 00151 *(outbuffer + offset + 3) = (u_likelihoodKeysi.base >> (8 * 3)) & 0xFF; 00152 offset += sizeof(this->likelihoodKeys[i]); 00153 } 00154 *(outbuffer + offset++) = likelihoodValues_length; 00155 *(outbuffer + offset++) = 0; 00156 *(outbuffer + offset++) = 0; 00157 *(outbuffer + offset++) = 0; 00158 for( uint8_t i = 0; i < likelihoodValues_length; i++){ 00159 union { 00160 float real; 00161 uint32_t base; 00162 } u_likelihoodValuesi; 00163 u_likelihoodValuesi.real = this->likelihoodValues[i]; 00164 *(outbuffer + offset + 0) = (u_likelihoodValuesi.base >> (8 * 0)) & 0xFF; 00165 *(outbuffer + offset + 1) = (u_likelihoodValuesi.base >> (8 * 1)) & 0xFF; 00166 *(outbuffer + offset + 2) = (u_likelihoodValuesi.base >> (8 * 2)) & 0xFF; 00167 *(outbuffer + offset + 3) = (u_likelihoodValuesi.base >> (8 * 3)) & 0xFF; 00168 offset += sizeof(this->likelihoodValues[i]); 00169 } 00170 *(outbuffer + offset++) = rawLikelihoodKeys_length; 00171 *(outbuffer + offset++) = 0; 00172 *(outbuffer + offset++) = 0; 00173 *(outbuffer + offset++) = 0; 00174 for( uint8_t i = 0; i < rawLikelihoodKeys_length; i++){ 00175 union { 00176 int32_t real; 00177 uint32_t base; 00178 } u_rawLikelihoodKeysi; 00179 u_rawLikelihoodKeysi.real = this->rawLikelihoodKeys[i]; 00180 *(outbuffer + offset + 0) = (u_rawLikelihoodKeysi.base >> (8 * 0)) & 0xFF; 00181 *(outbuffer + offset + 1) = (u_rawLikelihoodKeysi.base >> (8 * 1)) & 0xFF; 00182 *(outbuffer + offset + 2) = (u_rawLikelihoodKeysi.base >> (8 * 2)) & 0xFF; 00183 *(outbuffer + offset + 3) = (u_rawLikelihoodKeysi.base >> (8 * 3)) & 0xFF; 00184 offset += sizeof(this->rawLikelihoodKeys[i]); 00185 } 00186 *(outbuffer + offset++) = rawLikelihoodValues_length; 00187 *(outbuffer + offset++) = 0; 00188 *(outbuffer + offset++) = 0; 00189 *(outbuffer + offset++) = 0; 00190 for( uint8_t i = 0; i < rawLikelihoodValues_length; i++){ 00191 union { 00192 float real; 00193 uint32_t base; 00194 } u_rawLikelihoodValuesi; 00195 u_rawLikelihoodValuesi.real = this->rawLikelihoodValues[i]; 00196 *(outbuffer + offset + 0) = (u_rawLikelihoodValuesi.base >> (8 * 0)) & 0xFF; 00197 *(outbuffer + offset + 1) = (u_rawLikelihoodValuesi.base >> (8 * 1)) & 0xFF; 00198 *(outbuffer + offset + 2) = (u_rawLikelihoodValuesi.base >> (8 * 2)) & 0xFF; 00199 *(outbuffer + offset + 3) = (u_rawLikelihoodValuesi.base >> (8 * 3)) & 0xFF; 00200 offset += sizeof(this->rawLikelihoodValues[i]); 00201 } 00202 *(outbuffer + offset++) = weightsKeys_length; 00203 *(outbuffer + offset++) = 0; 00204 *(outbuffer + offset++) = 0; 00205 *(outbuffer + offset++) = 0; 00206 for( uint8_t i = 0; i < weightsKeys_length; i++){ 00207 union { 00208 int32_t real; 00209 uint32_t base; 00210 } u_weightsKeysi; 00211 u_weightsKeysi.real = this->weightsKeys[i]; 00212 *(outbuffer + offset + 0) = (u_weightsKeysi.base >> (8 * 0)) & 0xFF; 00213 *(outbuffer + offset + 1) = (u_weightsKeysi.base >> (8 * 1)) & 0xFF; 00214 *(outbuffer + offset + 2) = (u_weightsKeysi.base >> (8 * 2)) & 0xFF; 00215 *(outbuffer + offset + 3) = (u_weightsKeysi.base >> (8 * 3)) & 0xFF; 00216 offset += sizeof(this->weightsKeys[i]); 00217 } 00218 *(outbuffer + offset++) = weightsValues_length; 00219 *(outbuffer + offset++) = 0; 00220 *(outbuffer + offset++) = 0; 00221 *(outbuffer + offset++) = 0; 00222 for( uint8_t i = 0; i < weightsValues_length; i++){ 00223 union { 00224 int32_t real; 00225 uint32_t base; 00226 } u_weightsValuesi; 00227 u_weightsValuesi.real = this->weightsValues[i]; 00228 *(outbuffer + offset + 0) = (u_weightsValuesi.base >> (8 * 0)) & 0xFF; 00229 *(outbuffer + offset + 1) = (u_weightsValuesi.base >> (8 * 1)) & 0xFF; 00230 *(outbuffer + offset + 2) = (u_weightsValuesi.base >> (8 * 2)) & 0xFF; 00231 *(outbuffer + offset + 3) = (u_weightsValuesi.base >> (8 * 3)) & 0xFF; 00232 offset += sizeof(this->weightsValues[i]); 00233 } 00234 *(outbuffer + offset++) = statsKeys_length; 00235 *(outbuffer + offset++) = 0; 00236 *(outbuffer + offset++) = 0; 00237 *(outbuffer + offset++) = 0; 00238 for( uint8_t i = 0; i < statsKeys_length; i++){ 00239 uint32_t length_statsKeysi = strlen(this->statsKeys[i]); 00240 memcpy(outbuffer + offset, &length_statsKeysi, sizeof(uint32_t)); 00241 offset += 4; 00242 memcpy(outbuffer + offset, this->statsKeys[i], length_statsKeysi); 00243 offset += length_statsKeysi; 00244 } 00245 *(outbuffer + offset++) = statsValues_length; 00246 *(outbuffer + offset++) = 0; 00247 *(outbuffer + offset++) = 0; 00248 *(outbuffer + offset++) = 0; 00249 for( uint8_t i = 0; i < statsValues_length; i++){ 00250 union { 00251 float real; 00252 uint32_t base; 00253 } u_statsValuesi; 00254 u_statsValuesi.real = this->statsValues[i]; 00255 *(outbuffer + offset + 0) = (u_statsValuesi.base >> (8 * 0)) & 0xFF; 00256 *(outbuffer + offset + 1) = (u_statsValuesi.base >> (8 * 1)) & 0xFF; 00257 *(outbuffer + offset + 2) = (u_statsValuesi.base >> (8 * 2)) & 0xFF; 00258 *(outbuffer + offset + 3) = (u_statsValuesi.base >> (8 * 3)) & 0xFF; 00259 offset += sizeof(this->statsValues[i]); 00260 } 00261 *(outbuffer + offset++) = refWordsKeys_length; 00262 *(outbuffer + offset++) = 0; 00263 *(outbuffer + offset++) = 0; 00264 *(outbuffer + offset++) = 0; 00265 for( uint8_t i = 0; i < refWordsKeys_length; i++){ 00266 union { 00267 int32_t real; 00268 uint32_t base; 00269 } u_refWordsKeysi; 00270 u_refWordsKeysi.real = this->refWordsKeys[i]; 00271 *(outbuffer + offset + 0) = (u_refWordsKeysi.base >> (8 * 0)) & 0xFF; 00272 *(outbuffer + offset + 1) = (u_refWordsKeysi.base >> (8 * 1)) & 0xFF; 00273 *(outbuffer + offset + 2) = (u_refWordsKeysi.base >> (8 * 2)) & 0xFF; 00274 *(outbuffer + offset + 3) = (u_refWordsKeysi.base >> (8 * 3)) & 0xFF; 00275 offset += sizeof(this->refWordsKeys[i]); 00276 } 00277 *(outbuffer + offset++) = refWordsValues_length; 00278 *(outbuffer + offset++) = 0; 00279 *(outbuffer + offset++) = 0; 00280 *(outbuffer + offset++) = 0; 00281 for( uint8_t i = 0; i < refWordsValues_length; i++){ 00282 offset += this->refWordsValues[i].serialize(outbuffer + offset); 00283 } 00284 *(outbuffer + offset++) = loopWordsKeys_length; 00285 *(outbuffer + offset++) = 0; 00286 *(outbuffer + offset++) = 0; 00287 *(outbuffer + offset++) = 0; 00288 for( uint8_t i = 0; i < loopWordsKeys_length; i++){ 00289 union { 00290 int32_t real; 00291 uint32_t base; 00292 } u_loopWordsKeysi; 00293 u_loopWordsKeysi.real = this->loopWordsKeys[i]; 00294 *(outbuffer + offset + 0) = (u_loopWordsKeysi.base >> (8 * 0)) & 0xFF; 00295 *(outbuffer + offset + 1) = (u_loopWordsKeysi.base >> (8 * 1)) & 0xFF; 00296 *(outbuffer + offset + 2) = (u_loopWordsKeysi.base >> (8 * 2)) & 0xFF; 00297 *(outbuffer + offset + 3) = (u_loopWordsKeysi.base >> (8 * 3)) & 0xFF; 00298 offset += sizeof(this->loopWordsKeys[i]); 00299 } 00300 *(outbuffer + offset++) = loopWordsValues_length; 00301 *(outbuffer + offset++) = 0; 00302 *(outbuffer + offset++) = 0; 00303 *(outbuffer + offset++) = 0; 00304 for( uint8_t i = 0; i < loopWordsValues_length; i++){ 00305 offset += this->loopWordsValues[i].serialize(outbuffer + offset); 00306 } 00307 return offset; 00308 } 00309 00310 virtual int deserialize(unsigned char *inbuffer) 00311 { 00312 int offset = 0; 00313 offset += this->header.deserialize(inbuffer + offset); 00314 union { 00315 int32_t real; 00316 uint32_t base; 00317 } u_refId; 00318 u_refId.base = 0; 00319 u_refId.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00320 u_refId.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00321 u_refId.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00322 u_refId.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00323 this->refId = u_refId.real; 00324 offset += sizeof(this->refId); 00325 union { 00326 int32_t real; 00327 uint32_t base; 00328 } u_loopClosureId; 00329 u_loopClosureId.base = 0; 00330 u_loopClosureId.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00331 u_loopClosureId.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00332 u_loopClosureId.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00333 u_loopClosureId.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00334 this->loopClosureId = u_loopClosureId.real; 00335 offset += sizeof(this->loopClosureId); 00336 union { 00337 int32_t real; 00338 uint32_t base; 00339 } u_localLoopClosureId; 00340 u_localLoopClosureId.base = 0; 00341 u_localLoopClosureId.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00342 u_localLoopClosureId.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00343 u_localLoopClosureId.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00344 u_localLoopClosureId.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00345 this->localLoopClosureId = u_localLoopClosureId.real; 00346 offset += sizeof(this->localLoopClosureId); 00347 offset += this->mapCorrection.deserialize(inbuffer + offset); 00348 offset += this->loopClosureTransform.deserialize(inbuffer + offset); 00349 offset += this->currentPose.deserialize(inbuffer + offset); 00350 uint8_t posteriorKeys_lengthT = *(inbuffer + offset++); 00351 if(posteriorKeys_lengthT > posteriorKeys_length) 00352 this->posteriorKeys = (int32_t*)realloc(this->posteriorKeys, posteriorKeys_lengthT * sizeof(int32_t)); 00353 offset += 3; 00354 posteriorKeys_length = posteriorKeys_lengthT; 00355 for( uint8_t i = 0; i < posteriorKeys_length; i++){ 00356 union { 00357 int32_t real; 00358 uint32_t base; 00359 } u_st_posteriorKeys; 00360 u_st_posteriorKeys.base = 0; 00361 u_st_posteriorKeys.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00362 u_st_posteriorKeys.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00363 u_st_posteriorKeys.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00364 u_st_posteriorKeys.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00365 this->st_posteriorKeys = u_st_posteriorKeys.real; 00366 offset += sizeof(this->st_posteriorKeys); 00367 memcpy( &(this->posteriorKeys[i]), &(this->st_posteriorKeys), sizeof(int32_t)); 00368 } 00369 uint8_t posteriorValues_lengthT = *(inbuffer + offset++); 00370 if(posteriorValues_lengthT > posteriorValues_length) 00371 this->posteriorValues = (float*)realloc(this->posteriorValues, posteriorValues_lengthT * sizeof(float)); 00372 offset += 3; 00373 posteriorValues_length = posteriorValues_lengthT; 00374 for( uint8_t i = 0; i < posteriorValues_length; i++){ 00375 union { 00376 float real; 00377 uint32_t base; 00378 } u_st_posteriorValues; 00379 u_st_posteriorValues.base = 0; 00380 u_st_posteriorValues.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00381 u_st_posteriorValues.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00382 u_st_posteriorValues.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00383 u_st_posteriorValues.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00384 this->st_posteriorValues = u_st_posteriorValues.real; 00385 offset += sizeof(this->st_posteriorValues); 00386 memcpy( &(this->posteriorValues[i]), &(this->st_posteriorValues), sizeof(float)); 00387 } 00388 uint8_t likelihoodKeys_lengthT = *(inbuffer + offset++); 00389 if(likelihoodKeys_lengthT > likelihoodKeys_length) 00390 this->likelihoodKeys = (int32_t*)realloc(this->likelihoodKeys, likelihoodKeys_lengthT * sizeof(int32_t)); 00391 offset += 3; 00392 likelihoodKeys_length = likelihoodKeys_lengthT; 00393 for( uint8_t i = 0; i < likelihoodKeys_length; i++){ 00394 union { 00395 int32_t real; 00396 uint32_t base; 00397 } u_st_likelihoodKeys; 00398 u_st_likelihoodKeys.base = 0; 00399 u_st_likelihoodKeys.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00400 u_st_likelihoodKeys.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00401 u_st_likelihoodKeys.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00402 u_st_likelihoodKeys.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00403 this->st_likelihoodKeys = u_st_likelihoodKeys.real; 00404 offset += sizeof(this->st_likelihoodKeys); 00405 memcpy( &(this->likelihoodKeys[i]), &(this->st_likelihoodKeys), sizeof(int32_t)); 00406 } 00407 uint8_t likelihoodValues_lengthT = *(inbuffer + offset++); 00408 if(likelihoodValues_lengthT > likelihoodValues_length) 00409 this->likelihoodValues = (float*)realloc(this->likelihoodValues, likelihoodValues_lengthT * sizeof(float)); 00410 offset += 3; 00411 likelihoodValues_length = likelihoodValues_lengthT; 00412 for( uint8_t i = 0; i < likelihoodValues_length; i++){ 00413 union { 00414 float real; 00415 uint32_t base; 00416 } u_st_likelihoodValues; 00417 u_st_likelihoodValues.base = 0; 00418 u_st_likelihoodValues.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00419 u_st_likelihoodValues.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00420 u_st_likelihoodValues.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00421 u_st_likelihoodValues.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00422 this->st_likelihoodValues = u_st_likelihoodValues.real; 00423 offset += sizeof(this->st_likelihoodValues); 00424 memcpy( &(this->likelihoodValues[i]), &(this->st_likelihoodValues), sizeof(float)); 00425 } 00426 uint8_t rawLikelihoodKeys_lengthT = *(inbuffer + offset++); 00427 if(rawLikelihoodKeys_lengthT > rawLikelihoodKeys_length) 00428 this->rawLikelihoodKeys = (int32_t*)realloc(this->rawLikelihoodKeys, rawLikelihoodKeys_lengthT * sizeof(int32_t)); 00429 offset += 3; 00430 rawLikelihoodKeys_length = rawLikelihoodKeys_lengthT; 00431 for( uint8_t i = 0; i < rawLikelihoodKeys_length; i++){ 00432 union { 00433 int32_t real; 00434 uint32_t base; 00435 } u_st_rawLikelihoodKeys; 00436 u_st_rawLikelihoodKeys.base = 0; 00437 u_st_rawLikelihoodKeys.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00438 u_st_rawLikelihoodKeys.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00439 u_st_rawLikelihoodKeys.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00440 u_st_rawLikelihoodKeys.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00441 this->st_rawLikelihoodKeys = u_st_rawLikelihoodKeys.real; 00442 offset += sizeof(this->st_rawLikelihoodKeys); 00443 memcpy( &(this->rawLikelihoodKeys[i]), &(this->st_rawLikelihoodKeys), sizeof(int32_t)); 00444 } 00445 uint8_t rawLikelihoodValues_lengthT = *(inbuffer + offset++); 00446 if(rawLikelihoodValues_lengthT > rawLikelihoodValues_length) 00447 this->rawLikelihoodValues = (float*)realloc(this->rawLikelihoodValues, rawLikelihoodValues_lengthT * sizeof(float)); 00448 offset += 3; 00449 rawLikelihoodValues_length = rawLikelihoodValues_lengthT; 00450 for( uint8_t i = 0; i < rawLikelihoodValues_length; i++){ 00451 union { 00452 float real; 00453 uint32_t base; 00454 } u_st_rawLikelihoodValues; 00455 u_st_rawLikelihoodValues.base = 0; 00456 u_st_rawLikelihoodValues.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00457 u_st_rawLikelihoodValues.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00458 u_st_rawLikelihoodValues.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00459 u_st_rawLikelihoodValues.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00460 this->st_rawLikelihoodValues = u_st_rawLikelihoodValues.real; 00461 offset += sizeof(this->st_rawLikelihoodValues); 00462 memcpy( &(this->rawLikelihoodValues[i]), &(this->st_rawLikelihoodValues), sizeof(float)); 00463 } 00464 uint8_t weightsKeys_lengthT = *(inbuffer + offset++); 00465 if(weightsKeys_lengthT > weightsKeys_length) 00466 this->weightsKeys = (int32_t*)realloc(this->weightsKeys, weightsKeys_lengthT * sizeof(int32_t)); 00467 offset += 3; 00468 weightsKeys_length = weightsKeys_lengthT; 00469 for( uint8_t i = 0; i < weightsKeys_length; i++){ 00470 union { 00471 int32_t real; 00472 uint32_t base; 00473 } u_st_weightsKeys; 00474 u_st_weightsKeys.base = 0; 00475 u_st_weightsKeys.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00476 u_st_weightsKeys.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00477 u_st_weightsKeys.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00478 u_st_weightsKeys.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00479 this->st_weightsKeys = u_st_weightsKeys.real; 00480 offset += sizeof(this->st_weightsKeys); 00481 memcpy( &(this->weightsKeys[i]), &(this->st_weightsKeys), sizeof(int32_t)); 00482 } 00483 uint8_t weightsValues_lengthT = *(inbuffer + offset++); 00484 if(weightsValues_lengthT > weightsValues_length) 00485 this->weightsValues = (int32_t*)realloc(this->weightsValues, weightsValues_lengthT * sizeof(int32_t)); 00486 offset += 3; 00487 weightsValues_length = weightsValues_lengthT; 00488 for( uint8_t i = 0; i < weightsValues_length; i++){ 00489 union { 00490 int32_t real; 00491 uint32_t base; 00492 } u_st_weightsValues; 00493 u_st_weightsValues.base = 0; 00494 u_st_weightsValues.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00495 u_st_weightsValues.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00496 u_st_weightsValues.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00497 u_st_weightsValues.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00498 this->st_weightsValues = u_st_weightsValues.real; 00499 offset += sizeof(this->st_weightsValues); 00500 memcpy( &(this->weightsValues[i]), &(this->st_weightsValues), sizeof(int32_t)); 00501 } 00502 uint8_t statsKeys_lengthT = *(inbuffer + offset++); 00503 if(statsKeys_lengthT > statsKeys_length) 00504 this->statsKeys = (char**)realloc(this->statsKeys, statsKeys_lengthT * sizeof(char*)); 00505 offset += 3; 00506 statsKeys_length = statsKeys_lengthT; 00507 for( uint8_t i = 0; i < statsKeys_length; i++){ 00508 uint32_t length_st_statsKeys; 00509 memcpy(&length_st_statsKeys, (inbuffer + offset), sizeof(uint32_t)); 00510 offset += 4; 00511 for(unsigned int k= offset; k< offset+length_st_statsKeys; ++k){ 00512 inbuffer[k-1]=inbuffer[k]; 00513 } 00514 inbuffer[offset+length_st_statsKeys-1]=0; 00515 this->st_statsKeys = (char *)(inbuffer + offset-1); 00516 offset += length_st_statsKeys; 00517 memcpy( &(this->statsKeys[i]), &(this->st_statsKeys), sizeof(char*)); 00518 } 00519 uint8_t statsValues_lengthT = *(inbuffer + offset++); 00520 if(statsValues_lengthT > statsValues_length) 00521 this->statsValues = (float*)realloc(this->statsValues, statsValues_lengthT * sizeof(float)); 00522 offset += 3; 00523 statsValues_length = statsValues_lengthT; 00524 for( uint8_t i = 0; i < statsValues_length; i++){ 00525 union { 00526 float real; 00527 uint32_t base; 00528 } u_st_statsValues; 00529 u_st_statsValues.base = 0; 00530 u_st_statsValues.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00531 u_st_statsValues.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00532 u_st_statsValues.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00533 u_st_statsValues.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00534 this->st_statsValues = u_st_statsValues.real; 00535 offset += sizeof(this->st_statsValues); 00536 memcpy( &(this->statsValues[i]), &(this->st_statsValues), sizeof(float)); 00537 } 00538 uint8_t refWordsKeys_lengthT = *(inbuffer + offset++); 00539 if(refWordsKeys_lengthT > refWordsKeys_length) 00540 this->refWordsKeys = (int32_t*)realloc(this->refWordsKeys, refWordsKeys_lengthT * sizeof(int32_t)); 00541 offset += 3; 00542 refWordsKeys_length = refWordsKeys_lengthT; 00543 for( uint8_t i = 0; i < refWordsKeys_length; i++){ 00544 union { 00545 int32_t real; 00546 uint32_t base; 00547 } u_st_refWordsKeys; 00548 u_st_refWordsKeys.base = 0; 00549 u_st_refWordsKeys.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00550 u_st_refWordsKeys.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00551 u_st_refWordsKeys.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00552 u_st_refWordsKeys.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00553 this->st_refWordsKeys = u_st_refWordsKeys.real; 00554 offset += sizeof(this->st_refWordsKeys); 00555 memcpy( &(this->refWordsKeys[i]), &(this->st_refWordsKeys), sizeof(int32_t)); 00556 } 00557 uint8_t refWordsValues_lengthT = *(inbuffer + offset++); 00558 if(refWordsValues_lengthT > refWordsValues_length) 00559 this->refWordsValues = (rtabmap::KeyPoint*)realloc(this->refWordsValues, refWordsValues_lengthT * sizeof(rtabmap::KeyPoint)); 00560 offset += 3; 00561 refWordsValues_length = refWordsValues_lengthT; 00562 for( uint8_t i = 0; i < refWordsValues_length; i++){ 00563 offset += this->st_refWordsValues.deserialize(inbuffer + offset); 00564 memcpy( &(this->refWordsValues[i]), &(this->st_refWordsValues), sizeof(rtabmap::KeyPoint)); 00565 } 00566 uint8_t loopWordsKeys_lengthT = *(inbuffer + offset++); 00567 if(loopWordsKeys_lengthT > loopWordsKeys_length) 00568 this->loopWordsKeys = (int32_t*)realloc(this->loopWordsKeys, loopWordsKeys_lengthT * sizeof(int32_t)); 00569 offset += 3; 00570 loopWordsKeys_length = loopWordsKeys_lengthT; 00571 for( uint8_t i = 0; i < loopWordsKeys_length; i++){ 00572 union { 00573 int32_t real; 00574 uint32_t base; 00575 } u_st_loopWordsKeys; 00576 u_st_loopWordsKeys.base = 0; 00577 u_st_loopWordsKeys.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00578 u_st_loopWordsKeys.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00579 u_st_loopWordsKeys.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00580 u_st_loopWordsKeys.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00581 this->st_loopWordsKeys = u_st_loopWordsKeys.real; 00582 offset += sizeof(this->st_loopWordsKeys); 00583 memcpy( &(this->loopWordsKeys[i]), &(this->st_loopWordsKeys), sizeof(int32_t)); 00584 } 00585 uint8_t loopWordsValues_lengthT = *(inbuffer + offset++); 00586 if(loopWordsValues_lengthT > loopWordsValues_length) 00587 this->loopWordsValues = (rtabmap::KeyPoint*)realloc(this->loopWordsValues, loopWordsValues_lengthT * sizeof(rtabmap::KeyPoint)); 00588 offset += 3; 00589 loopWordsValues_length = loopWordsValues_lengthT; 00590 for( uint8_t i = 0; i < loopWordsValues_length; i++){ 00591 offset += this->st_loopWordsValues.deserialize(inbuffer + offset); 00592 memcpy( &(this->loopWordsValues[i]), &(this->st_loopWordsValues), sizeof(rtabmap::KeyPoint)); 00593 } 00594 return offset; 00595 } 00596 00597 const char * getType(){ return "rtabmap/InfoEx"; }; 00598 const char * getMD5(){ return "9134640b305c7a8fe3e5322120ad0f22"; }; 00599 00600 }; 00601 00602 } 00603 #endif