1 /* 2 =============================================================================== 3 4 FILE: field_gpstime.hpp 5 6 CONTENTS: 7 8 9 PROGRAMMERS: 10 11 martin.isenburg@rapidlasso.com - http://rapidlasso.com 12 uday.karan@gmail.com - Hobu, Inc. 13 14 COPYRIGHT: 15 16 (c) 2007-2014, martin isenburg, rapidlasso - tools to catch reality 17 (c) 2014, Uday Verma, Hobu, Inc. 18 19 This is free software; you can redistribute and/or modify it under the 20 terms of the GNU Lesser General Licence as published by the Free Software 21 Foundation. See the COPYING file for more information. 22 23 This software is distributed WITHOUT ANY WARRANTY and without even the 24 implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 25 26 CHANGE HISTORY: 27 28 =============================================================================== 29 */ 30 31 #ifndef __las_hpp__ 32 #error Cannot directly include this file, this is a part of las.hpp 33 #endif 34 35 #define LASZIP_GPSTIME_MULTI 500 36 #define LASZIP_GPSTIME_MULTI_MINUS -10 37 #define LASZIP_GPSTIME_MULTI_UNCHANGED (LASZIP_GPSTIME_MULTI - LASZIP_GPSTIME_MULTI_MINUS + 1) 38 #define LASZIP_GPSTIME_MULTI_CODE_FULL (LASZIP_GPSTIME_MULTI - LASZIP_GPSTIME_MULTI_MINUS + 2) 39 40 #define LASZIP_GPSTIME_MULTI_TOTAL (LASZIP_GPSTIME_MULTI - LASZIP_GPSTIME_MULTI_MINUS + 6) 41 42 namespace lazperf 43 { 44 // Teach packers how to pack and unpack gps time 45 // 46 template<> 47 struct packers<las::gpstime> { unpacklazperf::packers48 inline static las::gpstime unpack(const char *in) { 49 uint64_t lower = packers<unsigned int>::unpack(in), 50 upper = packers<unsigned int>::unpack(in + 4); 51 52 return las::gpstime((upper << 32) | lower); 53 } 54 packlazperf::packers55 inline static void pack(const las::gpstime& t, char *buffer) { 56 packers<unsigned int>::pack(t.value & 0xFFFFFFFF, buffer); 57 packers<unsigned int>::pack(t.value >> 32, buffer + 4); 58 } 59 }; 60 61 // Figure how to compress and decompress GPS time fields 62 // 63 template<> 64 struct field<las::gpstime> { 65 typedef las::gpstime type; 66 fieldlazperf::field67 field() : compressor_inited_(false), decompressor_inited_(false) {} 68 69 template< 70 typename TEncoder 71 > compressWithlazperf::field72 inline const char *compressWith(TEncoder& enc, const char *buf) 73 { 74 las::gpstime this_val = packers<las::gpstime>::unpack(buf); 75 76 if (!compressor_inited_) { 77 compressors_.init(); 78 compressor_inited_ = true; 79 } 80 81 if (!common_.have_last_) { 82 // don't have the first data yet, just push it to our have last stuff and move on 83 common_.have_last_ = true; 84 common_.last_gpstime[0] = this_val; 85 86 // write this out to the encoder as it is 87 enc.getOutStream().putBytes((const unsigned char*)buf, 88 sizeof(las::gpstime)); 89 buf += sizeof(las::gpstime); 90 91 // we are done here 92 return buf; 93 } 94 95 if (common_.last_gpstime_diff[common_.last] == 0) { // if last integer different was 0 96 if (this_val.value == common_.last_gpstime[common_.last].value) { 97 enc.encodeSymbol(common_.m_gpstime_0diff, 0); 98 } 99 else { 100 // calculate the difference between the two doubles as an integer 101 // 102 int64_t curr_gpstime_diff_64 = this_val.value - common_.last_gpstime[common_.last].value; 103 int curr_gpstime_diff = static_cast<int>(curr_gpstime_diff_64); 104 105 if (curr_gpstime_diff_64 == static_cast<int64_t>(curr_gpstime_diff)) { 106 // this difference is small enough to be represented with 32 bits 107 enc.encodeSymbol(common_.m_gpstime_0diff, 1); 108 compressors_.ic_gpstime.compress(enc, 0, curr_gpstime_diff, 0); 109 common_.last_gpstime_diff[common_.last] = curr_gpstime_diff; 110 common_.multi_extreme_counter[common_.last] = 0; 111 } 112 else { // the difference is huge 113 U32 i; 114 115 // maybe the double belongs to another time sequence 116 // 117 for (i = 1; i < 4; i++) { 118 int64_t other_gpstime_diff_64 = this_val.value - 119 common_.last_gpstime[(common_.last+i)&3].value; 120 int other_gpstime_diff = static_cast<int>(other_gpstime_diff_64); 121 122 if (other_gpstime_diff_64 == static_cast<int64_t>(other_gpstime_diff)) { 123 enc.encodeSymbol(common_.m_gpstime_0diff, i+2); // it belongs to another sequence 124 common_.last = (common_.last+i)&3; 125 return compressWith(enc, buf); 126 } 127 } 128 129 // no other sequence found. start new sequence. 130 enc.encodeSymbol(common_.m_gpstime_0diff, 2); 131 compressors_.ic_gpstime.compress(enc, 132 static_cast<int>(common_.last_gpstime[common_.last].value >> 32), 133 static_cast<int>(this_val.value >> 32), 8); 134 135 enc.writeInt(static_cast<unsigned int>(this_val.value)); 136 137 common_.next = (common_.next+1)&3; 138 common_.last = common_.next; 139 common_.last_gpstime_diff[common_.last] = 0; 140 common_.multi_extreme_counter[common_.last] = 0; 141 } 142 common_.last_gpstime[common_.last] = this_val; 143 } 144 } 145 else { // the last integer difference was *not* zero 146 if (this_val.value == common_.last_gpstime[common_.last].value) { 147 // if the doubles have not changed use a special symbol 148 enc.encodeSymbol(common_.m_gpstime_multi, LASZIP_GPSTIME_MULTI_UNCHANGED); 149 } 150 else 151 { 152 // calculate the difference between the two doubles as an integer 153 int64_t curr_gpstime_diff_64 = this_val.value - 154 common_.last_gpstime[common_.last].value; 155 int curr_gpstime_diff = static_cast<int>(curr_gpstime_diff_64); 156 157 // if the current gpstime difference can be represented with 32 bits 158 if (curr_gpstime_diff_64 == static_cast<int64_t>(curr_gpstime_diff)) { 159 // compute multiplier between current and last integer difference 160 float multi_f = (float)curr_gpstime_diff / 161 (float)(common_.last_gpstime_diff[common_.last]); 162 int multi = I32_QUANTIZE(multi_f); 163 164 // compress the residual curr_gpstime_diff in dependance on the multiplier 165 if (multi == 1) { 166 // this is the case we assume we get most often for regular spaced pulses 167 enc.encodeSymbol(common_.m_gpstime_multi, 1); 168 compressors_.ic_gpstime.compress(enc, 169 common_.last_gpstime_diff[common_.last], curr_gpstime_diff, 1); 170 common_.multi_extreme_counter[common_.last] = 0; 171 } 172 else if (multi > 0) { 173 if (multi < LASZIP_GPSTIME_MULTI) { 174 // positive multipliers up to LASZIP_GPSTIME_MULTI are compressed directly 175 enc.encodeSymbol(common_.m_gpstime_multi, multi); 176 if (multi < 10) 177 compressors_.ic_gpstime.compress(enc, 178 multi*common_.last_gpstime_diff[common_.last], 179 curr_gpstime_diff, 2); 180 else 181 compressors_.ic_gpstime.compress(enc, 182 multi*common_.last_gpstime_diff[common_.last], 183 curr_gpstime_diff, 3); 184 } 185 else { 186 enc.encodeSymbol(common_.m_gpstime_multi, LASZIP_GPSTIME_MULTI); 187 compressors_.ic_gpstime.compress(enc, 188 LASZIP_GPSTIME_MULTI*common_.last_gpstime_diff[common_.last], 189 curr_gpstime_diff, 4); 190 common_.multi_extreme_counter[common_.last]++; 191 192 if (common_.multi_extreme_counter[common_.last] > 3) { 193 common_.last_gpstime_diff[common_.last] = curr_gpstime_diff; 194 common_.multi_extreme_counter[common_.last] = 0; 195 } 196 } 197 } 198 else if (multi < 0) { 199 if (multi > LASZIP_GPSTIME_MULTI_MINUS) { 200 // negative multipliers larger than LASZIP_GPSTIME_MULTI_MINUS are compressed directly 201 enc.encodeSymbol(common_.m_gpstime_multi, LASZIP_GPSTIME_MULTI - multi); 202 compressors_.ic_gpstime.compress(enc, 203 multi*common_.last_gpstime_diff[common_.last], 204 curr_gpstime_diff, 5); 205 } 206 else { 207 enc.encodeSymbol(common_.m_gpstime_multi, LASZIP_GPSTIME_MULTI - LASZIP_GPSTIME_MULTI_MINUS); 208 compressors_.ic_gpstime.compress(enc, 209 LASZIP_GPSTIME_MULTI_MINUS*common_.last_gpstime_diff[common_.last], 210 curr_gpstime_diff, 6); 211 212 common_.multi_extreme_counter[common_.last]++; 213 if (common_.multi_extreme_counter[common_.last] > 3) 214 { 215 common_.last_gpstime_diff[common_.last] = curr_gpstime_diff; 216 common_.multi_extreme_counter[common_.last] = 0; 217 } 218 } 219 } 220 else { 221 enc.encodeSymbol(common_.m_gpstime_multi, 0); 222 compressors_.ic_gpstime.compress(enc, 0, curr_gpstime_diff, 7); 223 common_.multi_extreme_counter[common_.last]++; 224 if (common_.multi_extreme_counter[common_.last] > 3) 225 { 226 common_.last_gpstime_diff[common_.last] = curr_gpstime_diff; 227 common_.multi_extreme_counter[common_.last] = 0; 228 } 229 } 230 } 231 else { // the difference is huge 232 int i; 233 // maybe the double belongs to another time sequence 234 for (i = 1; i < 4; i++) 235 { 236 int64_t other_gpstime_diff_64 = this_val.value - common_.last_gpstime[(common_.last+i)&3].value; 237 int other_gpstime_diff = static_cast<int>(other_gpstime_diff_64); 238 239 if (other_gpstime_diff_64 == static_cast<int64_t>(other_gpstime_diff)) { 240 // it belongs to this sequence 241 enc.encodeSymbol(common_.m_gpstime_multi, LASZIP_GPSTIME_MULTI_CODE_FULL+i); 242 common_.last = (common_.last+i)&3; 243 return compressWith(enc, buf); 244 } 245 } 246 247 // no other sequence found. start new sequence. 248 enc.encodeSymbol(common_.m_gpstime_multi, LASZIP_GPSTIME_MULTI_CODE_FULL); 249 compressors_.ic_gpstime.compress( 250 enc, 251 static_cast<int>(common_.last_gpstime[common_.last].value >> 32), 252 static_cast<int>(this_val.value >> 32), 8); 253 enc.writeInt(static_cast<unsigned int>(this_val.value)); 254 common_.next = (common_.next+1)&3; 255 common_.last = common_.next; 256 common_.last_gpstime_diff[common_.last] = 0; 257 common_.multi_extreme_counter[common_.last] = 0; 258 } 259 260 common_.last_gpstime[common_.last] = this_val; 261 } 262 } 263 return buf + sizeof(las::gpstime); 264 } 265 266 template< 267 typename TDecoder 268 > decompressWithlazperf::field269 inline char *decompressWith(TDecoder& dec, char *buf) { 270 if (!decompressor_inited_) { 271 decompressors_.init(); 272 decompressor_inited_ = true; 273 } 274 275 if (!common_.have_last_) { 276 // don't have the first data yet, read the whole point out of the stream 277 common_.have_last_ = true; 278 279 dec.getInStream().getBytes((unsigned char*)buf, 280 sizeof(las::gpstime)); 281 // decode this value 282 common_.last_gpstime[0] = packers<las::gpstime>::unpack(buf); 283 284 // we are done here 285 return buf + sizeof(las::gpstime); 286 } 287 288 int multi; 289 if (common_.last_gpstime_diff[common_.last] == 0) { // if the last integer difference was zero 290 multi = dec.decodeSymbol(common_.m_gpstime_0diff); 291 292 if (multi == 1) { // the difference can be represented with 32 bits 293 common_.last_gpstime_diff[common_.last] = decompressors_.ic_gpstime.decompress(dec, 0, 0); 294 common_.last_gpstime[common_.last].value += common_.last_gpstime_diff[common_.last]; 295 common_.multi_extreme_counter[common_.last] = 0; 296 } 297 else if (multi == 2) { // the difference is huge 298 common_.next = (common_.next+1)&3; 299 common_.last_gpstime[common_.next].value = decompressors_.ic_gpstime.decompress( 300 dec, 301 (common_.last_gpstime[common_.last].value >> 32), 8); 302 common_.last_gpstime[common_.next].value = common_.last_gpstime[common_.next].value << 32; 303 common_.last_gpstime[common_.next].value |= dec.readInt(); 304 common_.last = common_.next; 305 common_.last_gpstime_diff[common_.last] = 0; 306 common_.multi_extreme_counter[common_.last] = 0; 307 } 308 else if (multi > 2) { // we switch to another sequence 309 common_.last = (common_.last+multi-2)&3; 310 311 decompressWith(dec, buf); 312 } 313 } 314 else { 315 multi = dec.decodeSymbol(common_.m_gpstime_multi); 316 if (multi == 1) { 317 common_.last_gpstime[common_.last].value += decompressors_.ic_gpstime.decompress( 318 dec, 319 common_.last_gpstime_diff[common_.last], 1); 320 common_.multi_extreme_counter[common_.last] = 0; 321 } 322 else if (multi < LASZIP_GPSTIME_MULTI_UNCHANGED) { 323 int gpstime_diff; 324 if (multi == 0) { 325 gpstime_diff = decompressors_.ic_gpstime.decompress(dec, 0, 7); 326 common_.multi_extreme_counter[common_.last]++; 327 if (common_.multi_extreme_counter[common_.last] > 3) { common_.last_gpstime_diff[common_.last] = gpstime_diff; 328 common_.multi_extreme_counter[common_.last] = 0; 329 } 330 } 331 else if (multi < LASZIP_GPSTIME_MULTI) { 332 if (multi < 10) 333 gpstime_diff = decompressors_.ic_gpstime.decompress(dec, 334 multi*common_.last_gpstime_diff[common_.last], 2); 335 else 336 gpstime_diff = decompressors_.ic_gpstime.decompress(dec, 337 multi*common_.last_gpstime_diff[common_.last], 3); 338 } 339 else if (multi == LASZIP_GPSTIME_MULTI) { 340 gpstime_diff = decompressors_.ic_gpstime.decompress( 341 dec, 342 LASZIP_GPSTIME_MULTI*common_.last_gpstime_diff[common_.last], 4); 343 common_.multi_extreme_counter[common_.last]++; 344 if (common_.multi_extreme_counter[common_.last] > 3) { 345 common_.last_gpstime_diff[common_.last] = gpstime_diff; 346 common_.multi_extreme_counter[common_.last] = 0; 347 } 348 } 349 else { 350 multi = LASZIP_GPSTIME_MULTI - multi; 351 if (multi > LASZIP_GPSTIME_MULTI_MINUS) { 352 gpstime_diff = decompressors_.ic_gpstime.decompress( 353 dec, 354 multi*common_.last_gpstime_diff[common_.last], 5); 355 } 356 else 357 { 358 gpstime_diff = decompressors_.ic_gpstime.decompress( 359 dec, 360 LASZIP_GPSTIME_MULTI_MINUS*common_.last_gpstime_diff[common_.last], 6); 361 common_.multi_extreme_counter[common_.last]++; 362 if (common_.multi_extreme_counter[common_.last] > 3) { 363 common_.last_gpstime_diff[common_.last] = gpstime_diff; 364 common_.multi_extreme_counter[common_.last] = 0; 365 } 366 } 367 } 368 common_.last_gpstime[common_.last].value += gpstime_diff; 369 } 370 else if (multi == LASZIP_GPSTIME_MULTI_CODE_FULL) { 371 common_.next = (common_.next+1)&3; 372 common_.last_gpstime[common_.next].value = decompressors_.ic_gpstime.decompress( 373 dec, static_cast<int>(common_.last_gpstime[common_.last].value >> 32), 8); 374 common_.last_gpstime[common_.next].value = common_.last_gpstime[common_.next].value << 32; 375 common_.last_gpstime[common_.next].value |= dec.readInt(); 376 common_.last = common_.next; 377 common_.last_gpstime_diff[common_.last] = 0; 378 common_.multi_extreme_counter[common_.last] = 0; 379 } 380 else if (multi >= LASZIP_GPSTIME_MULTI_CODE_FULL) { 381 common_.last = (common_.last+multi-LASZIP_GPSTIME_MULTI_CODE_FULL)&3; 382 383 decompressWith(dec, buf); 384 } 385 } 386 packers<las::gpstime>::pack(common_.last_gpstime[common_.last], 387 buf); 388 return buf + sizeof(las::gpstime); 389 } 390 391 // All the things we need to compress a point, group them into structs 392 // so we don't have too many names flying around 393 394 // Common parts for both a compressor and decompressor go here 395 struct __common { 396 bool have_last_; 397 models::arithmetic m_gpstime_multi, m_gpstime_0diff; 398 unsigned int last, next; 399 std::array<las::gpstime, 4> last_gpstime; 400 std::array<int, 4> last_gpstime_diff; 401 std::array<int, 4> multi_extreme_counter; 402 __commonlazperf::field::__common403 __common() : 404 have_last_(false), 405 m_gpstime_multi(LASZIP_GPSTIME_MULTI_TOTAL), 406 m_gpstime_0diff(6), 407 last(0), next(0) { 408 409 last_gpstime.fill(las::gpstime()); 410 last_gpstime_diff.fill(0); 411 multi_extreme_counter.fill(0); 412 } 413 414 ~__commonlazperf::field::__common415 ~__common() { 416 } 417 } common_; 418 419 // These compressors are specific to a compressor usage, so we keep them separate here 420 struct __compressors { 421 compressors::integer ic_gpstime; 422 __compressorslazperf::field::__compressors423 __compressors() : 424 ic_gpstime(32, 9) {} 425 initlazperf::field::__compressors426 void init() { 427 ic_gpstime.init(); 428 } 429 } compressors_; 430 431 struct __decompressors { 432 decompressors::integer ic_gpstime; 433 __decompressorslazperf::field::__decompressors434 __decompressors() : 435 ic_gpstime(32, 9) {} 436 initlazperf::field::__decompressors437 void init() { 438 ic_gpstime.init(); 439 } 440 } decompressors_; 441 442 bool compressor_inited_; 443 bool decompressor_inited_; 444 }; 445 } // namespace lazperf 446