GImage.h

// GSLAM - A general SLAM framework and benchmark
// Copyright 2018 PILAB Inc. All rights reserved.
// https://github.com/zdzhaoyong/GSLAM
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
//   used to endorse or promote products derived from this software without
//   specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author:xulei   email:xlnwpu@gmail.com
//
// This is the GSLAM main API header

#ifndef GSLAM_GIMAGE_H  // NOLINT
#define GSLAM_GIMAGE_H
#include <assert.h>
#include <stdint.h>
#include <string.h>
#include <atomic>

#if defined(HAS_OPENCV) || defined(HAS_OPENCV3)
#include <opencv2/core/core.hpp>
#else
typedef unsigned char uchar;
#endif

#ifndef CV_XADD
#include "Mutex.h"
static inline int CV_XADD(int* addr, int delta) {
  static GSLAM::MutexRW mutex;
  GSLAM::WriteMutex lock(mutex);
  int tmp = *addr;
  *addr += delta;
  return tmp;
}
#endif

namespace GSLAM {

enum GElementType {
  GElementType_8U = 0,
  GElementType_8S = 1,
  GElementType_16U = 2,
  GElementType_16S = 3,
  GElementType_32S = 4,
  GElementType_32F = 5,
  GElementType_64F = 6,
  GElementType_UserType = 7
};

template <typename C>
class GElement {
 public:
  enum { Type = GElementType_UserType };
};

template <>
class GElement<uint8_t> {
 public:
  enum { Type = GElementType_8U };
};

template <>
class GElement<char> {
 public:
  enum { Type = GElementType_8S };
};

template <>
class GElement<int16_t> {
 public:
  enum { Type = GElementType_16S };
};

template <>
class GElement<uint16_t> {
 public:
  enum { Type = GElementType_16U };
};

template <>
class GElement<int32_t> {
 public:
  enum { Type = GElementType_32S };
};

template <>
class GElement<float> {
 public:
  enum { Type = GElementType_32F };
};

template <>
class GElement<double> {
 public:
  enum { Type = GElementType_64F };
};

template <typename EleType = uint8_t, int channelSize = 1>
struct GImageType {
  enum { Type = ((GElement<EleType>::Type & 0x7) + ((channelSize - 1) << 3)) };
};

struct UMatData {  // for OpenCV Version 3
  enum {
    COPY_ON_MAP = 1,
    HOST_COPY_OBSOLETE = 2,
    DEVICE_COPY_OBSOLETE = 4,
    TEMP_UMAT = 8,
    TEMP_COPIED_UMAT = 24,
    USER_ALLOCATED = 32,
    DEVICE_MEM_MAPPED = 64
  };
  const void* prevAllocator;
  const void* currAllocator;
  int urefcount;
  int refcount;
  uchar* data;
  uchar* origdata;
  size_t size;

  int flags;
  void* handle;
  void* userdata;
  int allocatorFlags_;
  int mapcount;
  UMatData* originalUMatData;
};

/**
 * @brief The GImage class is a tiny implementation of image for removing
 * dependency of opencv.
 * Most APIs are corrosponding to "cv::Mat".
 */
class GImage {
 public:
  GImage() : cols(0), rows(0), flags(0), data(NULL), refCount(NULL) {}

  GImage(int rows_, int cols_, int type = GImageType<>::Type, uchar* src = NULL,
         bool copy = false)
      : cols(cols_), rows(rows_), flags(type), data(NULL), refCount(NULL) {
    if (src && !copy) {
      data = src;
      return;
    }

    int byteNum = total() * elemSize();
    if (byteNum <= 0) return;
    int alignBytes = alignSize(byteNum, static_cast<int>(sizeof(*refCount)));
    data = reinterpret_cast<uchar*>(fastMalloc(alignBytes + sizeof(int*)));
    if (!data) {
      cols = 0;
      rows = 0;
      return;
    }
    refCount = reinterpret_cast<int*>(data + alignBytes);
    *refCount = 1;
    if (src) memcpy(data, src, byteNum);
  }

  GImage(const GImage& ref)
      : cols(ref.cols),
        rows(ref.rows),
        flags(ref.flags),
        data(ref.data),
        refCount(ref.refCount) {
    if (refCount) CV_XADD(refCount, 1);
  }

  ~GImage() {
    if (data && refCount) {
      if (CV_XADD(refCount, -1) == 1) {
        release();
      }
    }
  }

  GImage& operator=(const GImage& rhs) {
    this->~GImage();
    cols = rhs.cols;
    rows = rhs.rows;
    flags = rhs.flags;
    data = rhs.data;
    refCount = rhs.refCount;
    if (refCount) CV_XADD(refCount, 1);
    return *this;
  }

  static GImage create(int rows, int cols, int type = GImageType<>::Type,
                       uchar* src = NULL, bool copy = false) {
    return GImage(rows, cols, type, src, copy);
  }

  static GImage zeros(int rows, int cols, int type = GImageType<>::Type,
                      uchar* src = NULL, bool copy = false) {
    GImage result(rows, cols, type, src, copy);
    memset(result.data, 0, result.total() * result.elemSize());
    return result;
  }

  bool empty() const { return !data; }
  int elemSize() const { return channels() * elemSize1(); }
  int elemSize1() const { return (1 << ((type() & 0x7) >> 1)); }

  int channels() const { return (type() >> 3) + 1; }
  int type() const { return flags; }
  int total() const { return cols * rows; }

  GImage clone() const { return GImage(rows, cols, flags, data, true); }

  template <typename C>
  C& at(int idx) {
    return (reinterpret_cast<C*>(data))[idx];
  }

  template <typename C>
  C& at(int ix, int iy) {
    return (reinterpret_cast<C*>(data))[iy * cols + ix];
  }

#if defined(HAS_OPENCV) || defined(HAS_OPENCV3)
#if CV_VERSION_EPOCH == 2
  inline operator cv::Mat() const {
    if (empty()) return cv::Mat();
    cv::Mat result(rows, cols, type(), data);
    if (refCount) {
      result.refcount = refCount;
      CV_XADD(refCount, 1);
    }
    return result;
  }
  GImage(const cv::Mat& mat) // NOLINT
      : cols(mat.cols),
        rows(mat.rows),
        flags(mat.type()),
        data(mat.data),
        refCount(mat.refcount) {
    if (refCount) CV_XADD(refCount, 1);
  }
#elif CV_VERSION_MAJOR == 3
  inline operator cv::Mat() const {
    if (!data) return cv::Mat();
    cv::Mat result(rows, cols, type(), data);
    if ((reinterpret_cast<uchar*>(refCount)) ==
        data + total() * elemSize()) {  // OpenCV2 style => OpenCV3 style
      // WARNING: MAKE SURE THERE ARE NO OTHER HOLDERS
      // construct a UMat that ref to data
      cv::UMatData* u = new cv::UMatData(nullptr);
      u->origdata = u->data = data;
      u->userdata = refCount;
      CV_XADD(refCount, 1);
      u->refcount = 2;
      refCount = &u->refcount;
      result.u = u;
      return result;
    } else {  // OpenCV3 style => OpenCV3 style
      CV_XADD(refCount, 1);
      cv::UMatData* u =
          (cv::UMatData*)((reinterpret_cast<uchar*>(refCount)) - sizeof(int) -
                          sizeof(cv::MatAllocator*) * 2);
      result.u = u;
      return result;
    }
    return result;  // no copy but not safe either
  }

  GImage(const cv::Mat& mat) //NOLINT
      : cols(mat.cols),
        rows(mat.rows),
        flags(mat.type()),
        data(mat.data),
        refCount(NULL) {
    if (mat.u && !mat.allocator) {
      // try to maintain the refcount things, but this need the mat is allocated
      // by default StdAllocator
      refCount = (&mat.u->refcount);
      CV_XADD(refCount, 1);
    } else if (0) {  // copy the data: SAFE but SLOW
      int byteNum = total() * elemSize();
      data = reinterpret_cast<uchar*>(fastMalloc(byteNum + sizeof(int*)));
      if (!data) {
        cols = 0;
        rows = 0;
        return;
      }
      refCount = reinterpret_cast<int*>(data + byteNum);
      *refCount = 1;
      if (mat.data) memcpy(data, mat.data, byteNum);
    }
  }
#endif

#endif

  void release() {
    int totalBytes = total() * elemSize();
    int alignBytes = alignSize(totalBytes, (int)(sizeof(*refCount))); //NOLINT
    if (refCount == ((int*)(data + alignBytes))) {  // OpenCV2 style  //NOLINT
      cols = rows = 0;
      refCount = NULL;
      fastFree(data);
      data = NULL;
    } else {  // OpenCV3 style
#if false     // use opencv's default deallocate
            cv::UMatData* u = (cv::UMatData*) \
                    ((reinterpret_cast<uchar*>(refCount))\
                     -sizeof(int)-sizeof(void*)*2);
            u->refcount--;
            if (u) (u->currAllocator ? u->currAllocator :\
                                    cv::Mat::getDefaultAllocator())->unmap(u);
#else         // use buildin deallocate
      GSLAM::UMatData* u =
          (GSLAM::UMatData*)((reinterpret_cast<uchar*>(refCount)) -
                             sizeof(int) - sizeof(void*) * 2);
      if ((*refCount) == 1 &&
          u->userdata == data + totalBytes) {  // this is allocated by GImage
        (*(reinterpret_cast<int*>(u->userdata)))--;
      }
      assert(u->size == totalBytes);
      assert(u->currAllocator == NULL);
      u->refcount--;
      deallocate(u);
#endif
      cols = rows = 0;
      refCount = NULL;
      data = NULL;
    }
  }

  template <typename _Tp>
  _Tp* ptr(int i0 = 0) {
    return reinterpret_cast<_Tp*>(data + i0 * cols * elemSize());
  }

  template <typename _Tp>
  const _Tp* ptr(int i0 = 0) const {
    return reinterpret_cast<_Tp*>(data + i0 * cols * elemSize());
  }

  const GImage row(int idx = 0) const {
    return GImage(1, cols, type(), data + elemSize() * cols * idx);
  }

 private:
  template <typename _Tp>
  static inline _Tp* alignPtr(_Tp* ptr,
                              int n = reinterpret_cast<int>(sizeof(_Tp))) {
    return reinterpret_cast<_Tp*>(((size_t)ptr + n - 1) & -n);
  }

  void* fastMalloc(size_t size) const {
    uchar* udata = reinterpret_cast<uchar*>(malloc(size + sizeof(void*) + 16));
    if (!udata) return NULL;
    uchar** adata = alignPtr(reinterpret_cast<uchar**>(udata) + 1, 16);
    adata[-1] = udata;
    return adata;
  }

  void fastFree(void* ptr) const {
    if (ptr) {
      uchar* udata = (reinterpret_cast<uchar**>(ptr))[-1];
      free(udata);
    }
  }

  static inline size_t alignSize(size_t sz, int n) {
    assert((n & (n - 1)) == 0);  // n is a power of 2
    return (sz + n - 1) & -n;
  }

  void deallocate(GSLAM::UMatData* u) const {
    // for OpenCV Version 3
    if (!u) return;

    assert(u->urefcount == 0);
    assert(u->refcount == 0);
    if (!(u->flags & GSLAM::UMatData::USER_ALLOCATED)) {
      fastFree(u->origdata);
      u->origdata = 0;
    }
    delete u;
  }

 public:
  int cols, rows, flags;
  uchar* data;
  mutable int* refCount;
};
}  // namespace GSLAM
#endif  // GSLAM_GIMAGE_H  //NOLINT