From 0e46e2db7fede0b71ba988e5b734cec183a67dd0 Mon Sep 17 00:00:00 2001
From: Tom Panzarella <tpanzarella@loveparkrobotics.com>
Date: Thu, 3 Nov 2016 11:40:56 -0400
Subject: [PATCH] Compatibility fix for image chunk header v2

---
 ChangeLog.md                                  |  2 ++
 .../o3d3xx_framegrabber/byte_buffer.hpp       |  1 -
 .../libo3d3xx_framegrabber/byte_buffer.cpp    |  1 -
 .../libo3d3xx_framegrabber/frame_grabber.cpp  | 26 -------------------
 modules/image/src/libo3d3xx_image/image.cpp   | 23 +++++++++-------
 5 files changed, 16 insertions(+), 37 deletions(-)

diff --git a/ChangeLog.md b/ChangeLog.md
index 213287b..fe56baa 100644
--- a/ChangeLog.md
+++ b/ChangeLog.md
@@ -6,6 +6,8 @@
   is to allow for checking, visually, an off-board computed cloud vs. an
   on-board computed cloud.
 * Fixed memory access [bug](https://github.com/lovepark/libo3d3xx/issues/67)
+* Updated to support (in a backward compatible fashion) image chunk header
+  version 2.
 
 ## Changes between libo3d3xx 0.4.6 and 0.4.8
 
diff --git a/modules/framegrabber/include/o3d3xx_framegrabber/byte_buffer.hpp b/modules/framegrabber/include/o3d3xx_framegrabber/byte_buffer.hpp
index e6e8428..4c30a78 100644
--- a/modules/framegrabber/include/o3d3xx_framegrabber/byte_buffer.hpp
+++ b/modules/framegrabber/include/o3d3xx_framegrabber/byte_buffer.hpp
@@ -25,7 +25,6 @@
 namespace o3d3xx
 {
   extern const std::size_t IMG_TICKET_SZ; // bytes
-  extern const std::size_t IMG_CHUNK_HEADER_SZ; // bytes
 
   enum class pixel_format : std::uint8_t
   {
diff --git a/modules/framegrabber/src/libo3d3xx_framegrabber/byte_buffer.cpp b/modules/framegrabber/src/libo3d3xx_framegrabber/byte_buffer.cpp
index b65516f..0096afa 100644
--- a/modules/framegrabber/src/libo3d3xx_framegrabber/byte_buffer.cpp
+++ b/modules/framegrabber/src/libo3d3xx_framegrabber/byte_buffer.cpp
@@ -23,7 +23,6 @@
 #include <glog/logging.h>
 
 const std::size_t o3d3xx::IMG_TICKET_SZ = 16;
-const std::size_t o3d3xx::IMG_CHUNK_HEADER_SZ = 36;
 
 bool
 o3d3xx::verify_ticket_buffer(const std::vector<std::uint8_t>& buff)
diff --git a/modules/framegrabber/src/libo3d3xx_framegrabber/frame_grabber.cpp b/modules/framegrabber/src/libo3d3xx_framegrabber/frame_grabber.cpp
index 7eca867..51dbccb 100644
--- a/modules/framegrabber/src/libo3d3xx_framegrabber/frame_grabber.cpp
+++ b/modules/framegrabber/src/libo3d3xx_framegrabber/frame_grabber.cpp
@@ -375,32 +375,6 @@ o3d3xx::FrameGrabber::Run()
 {
   boost::asio::io_service::work work(this->io_service_);
 
-  // //
-  //
-  // XXX: TP I don't think this is necessary. I think it will more likely lead
-  // to error situations more often than helping out the situation. The user
-  // should take care to set the camera into "RUN" mode prior to starting the
-  // framegrabber.
-  //
-  // // setup the camera for image acquistion
-  // //
-  // try
-  //   {
-  //     this->cam_->RequestSession();
-  //     this->cam_->SetOperatingMode(o3d3xx::Camera::operating_mode::RUN);
-  //     this->cam_->CancelSession();
-  //   }
-  // catch (const o3d3xx::error_t& ex)
-  //   {
-  //     LOG(ERROR) << "Failed to setup camera for image acquisition: "
-  //                << ex.what();
-  //     return;
-  //   }
-
-  //
-  // After setting the schema, this gets called and kicks off our
-  // data processing "loop"
-  //
   auto result_schema_write_handler =
     [&, this]
     (const boost::system::error_code& ec, std::size_t bytes_transferred)
diff --git a/modules/image/src/libo3d3xx_image/image.cpp b/modules/image/src/libo3d3xx_image/image.cpp
index a1712c3..d89196f 100644
--- a/modules/image/src/libo3d3xx_image/image.cpp
+++ b/modules/image/src/libo3d3xx_image/image.cpp
@@ -209,17 +209,22 @@ o3d3xx::ImageBuffer::Organize()
   this->amp_.create(height, width, CV_16UC1);
   this->raw_amp_.create(height, width, CV_16UC1);
 
-  // move index pointers to where pixel data starts
-  cidx += o3d3xx::IMG_CHUNK_HEADER_SZ;
-  didx += RDIST_OK ? o3d3xx::IMG_CHUNK_HEADER_SZ : 0;
-  uidx += UVEC_OK ? o3d3xx::IMG_CHUNK_HEADER_SZ : 0;
-  aidx += AMP_OK ? o3d3xx::IMG_CHUNK_HEADER_SZ : 0;
-  raw_aidx += RAW_AMP_OK ? o3d3xx::IMG_CHUNK_HEADER_SZ : 0;
+  // move index pointers to where pixel data starts -- we assume
+  // (I think safely) that all header sizes will be uniform in the data stream,
+  // so, we use our invariant of the confidence image header
+  std::uint32_t pixel_data_offset =
+    o3d3xx::mkval<std::uint32_t>(this->bytes_.data()+cidx+8);
+
+  cidx += pixel_data_offset;
+  didx += RDIST_OK ? pixel_data_offset : 0;
+  uidx += UVEC_OK ? pixel_data_offset : 0;
+  aidx += AMP_OK ? pixel_data_offset : 0;
+  raw_aidx += RAW_AMP_OK ? pixel_data_offset : 0;
   if (CARTESIAN_OK)
     {
-      xidx += o3d3xx::IMG_CHUNK_HEADER_SZ;
-      yidx += o3d3xx::IMG_CHUNK_HEADER_SZ;
-      zidx += o3d3xx::IMG_CHUNK_HEADER_SZ;
+      xidx += pixel_data_offset;
+      yidx += pixel_data_offset;
+      zidx += pixel_data_offset;
     }
 
   float bad_point = std::numeric_limits<float>::quiet_NaN();