hesuicong/openMVS
1
0 Fork 0
Code Issues Pull Requests Actions Projects Releases Wiki Activity
Files
07a4ade7128bf2a4ee7276c7ca4033a2a21cd5ee
openMVS/libs/Common/Types.h
T
hesuicong 07dabbd186 提交scripts
2025-07-21 10:09:12 +08:00

2826 lines
98 KiB
C++
Raw Blame History

////////////////////////////////////////////////////////////////////
// Types.h
//
// Copyright 2007 cDc@seacave
// Distributed under the Boost Software License, Version 1.0
// (See http://www.boost.org/LICENSE_1_0.txt)
#ifndef __SEACAVE_TYPES_H__
#define __SEACAVE_TYPES_H__
// I N C L U D E S /////////////////////////////////////////////////
#ifdef _MSC_VER
#include <windows.h>
#include <tchar.h>
#else
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <errno.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#endif
#ifdef _SUPPORT_CPP11
#ifdef __clang__
#include <stdint.h>
#else
#include <cstdint>
#endif
#include <cstddef>
#include <type_traits>
#include <initializer_list>
#else
#include <stdint.h>
#endif
#ifdef _SUPPORT_CPP17
#if !defined(__GNUC__) || (__GNUC__ > 7)
#include <filesystem>
#endif
#endif
#include <new>
#include <string>
#include <codecvt>
#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <functional>
#include <algorithm>
#include <numeric>
#include <limits>
#include <map>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include <list>
#include <queue>
#include <deque>
#include <iterator>
#include <cmath>
#include <ctime>
#include <random>
#ifdef _USE_OPENMP
#include <omp.h>
#endif
// Function delegate functionality
#ifdef _SUPPORT_CPP11
#include "FastDelegateCPP11.h"
#define DELEGATE fastdelegate::delegate
#define DELEGATEBIND(DLGT, FNC) DLGT::from< FNC >()
#define DELEGATEBINDCLASS(DLGT, FNC, OBJ) DLGT::from(*OBJ, FNC)
#else
#include "FastDelegate.h"
#include "FastDelegateBind.h"
#define DELEGATE fastdelegate::FastDelegate
#define DELEGATEBIND(DLGT, FNC) fastdelegate::bind(FNC)
#define DELEGATEBINDCLASS(DLGT, FNC, OBJ) fastdelegate::bind(FNC, OBJ)
#endif
// include usual boost libraries
#ifdef _USE_BOOST
#if 1
// disable exception support
#define BOOST_NO_UNREACHABLE_RETURN_DETECTION
#define BOOST_EXCEPTION_DISABLE
#define BOOST_NO_EXCEPTIONS
#endif
#ifdef BOOST_NO_EXCEPTIONS
#include <boost/throw_exception.hpp>
#endif
#define BOOST_NO_UNREACHABLE_RETURN_DETECTION
// include headers that implement serialization support
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/utility.hpp>
#include <boost/serialization/export.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/list.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/set.hpp>
#if (BOOST_VERSION / 100000) > 1 || (BOOST_VERSION / 100 % 1000) > 55
#include <boost/serialization/unordered_map.hpp>
#include <boost/serialization/unordered_set.hpp>
#endif
#include <boost/serialization/nvp.hpp>
// include headers that define an input and output archive
#include <boost/archive/detail/common_oarchive.hpp>
#include <boost/archive/detail/common_iarchive.hpp>
// include headers that define memory pools
#include <boost/pool/object_pool.hpp>
#include <boost/pool/singleton_pool.hpp>
#endif
#ifdef _USE_EIGEN
#if defined(_MSC_VER)
#pragma warning (push)
#pragma warning (disable : 4244) // 'argument': conversion from '__int64' to 'int', possible loss of data
#endif
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <Eigen/QR>
#include <Eigen/LU>
#if defined(_MSC_VER)
#pragma warning (pop)
#endif
#endif
#pragma push_macro("free")
#undef free
#pragma push_macro("DEBUG")
#undef DEBUG
#include <opencv2/core/version.hpp>
#if CV_MAJOR_VERSION > 2 || CV_MINOR_VERSION > 3
#include <opencv2/opencv_modules.hpp>
#endif
#include <opencv2/opencv.hpp>
#ifdef HAVE_OPENCV_GPU
#if CV_MAJOR_VERSION > 2
#include <opencv2/cudaarithm.hpp>
namespace cv { namespace gpu = cuda; }
#else
#include <opencv2/gpu/gpu.hpp>
#endif
#endif
#pragma pop_macro("DEBUG")
#pragma pop_macro("free")
#ifdef _USE_SSE
#include <xmmintrin.h>
#include <emmintrin.h>
#endif
// D E F I N E S ///////////////////////////////////////////////////
// In order to create a singleton class
// add this define in your class;
#define DECLARE_SINGLETON(SClass) \
protected: SClass(); SClass(const SClass&); \
public: static inline SClass& GetInstance() { static SClass instance; return instance; }
// or add this define and declare GetInstance() method in the .cpp
// so that the singleton is unique even across modules;
#define DEFINE_SINGLETON(SClass) \
protected: SClass(); SClass(const SClass&); \
public: static SClass& GetInstance();
// or add this declare ms_pInstance;
#define DECLARE_SINGLETONPTR(SClass) \
SClass* SClass::ms_pInstance(NULL); \
SClass*& SClass::GetInstanceRef() { return ms_pInstance; } \
SClass* SClass::GetInstancePtr() { ASSERT(ms_pInstance); return ms_pInstance; } \
SClass& SClass::GetInstance() { ASSERT(ms_pInstance); return *ms_pInstance; }
// and this define in the class definition.
#define DEFINE_SINGLETONPTR(SClass) \
protected: SClass(); SClass(const SClass&); static SClass* ms_pInstance; \
public: static SClass*& GetInstanceRef(); \
public: static SClass* GetInstancePtr(); \
public: static SClass& GetInstance();
#ifndef CALL_MEMBER_FN
#define CALL_MEMBER_FN(object,ptrToMember) ((object).*(ptrToMember))
#endif
#ifndef __PROCESS__
# ifdef _MSC_VER
# define __PROCESS__ ((unsigned)GetCurrentProcessId())
# else
# define __PROCESS__ ((unsigned)getpid())
# endif
#endif
#ifndef __THREAD__
# ifdef _MSC_VER
# define __THREAD__ ((unsigned)GetCurrentThreadId())
# elif defined(__APPLE__)
# include <pthread.h>
inline pid_t GetCurrentThreadId() { uint64_t tid64; pthread_threadid_np(NULL, &tid64); return (pid_t)tid64; }
# define __THREAD__ ((unsigned)GetCurrentThreadId())
# else
# include <sys/syscall.h>
# define __THREAD__ ((unsigned)((pid_t)syscall(SYS_gettid)))
# endif
#endif
#ifndef STCALL
# if defined(_MSC_VER)
# define STCALL __cdecl
# elif defined(__OS2__)
# if defined (__GNUC__) && __GNUC__ < 4
# define STCALL _cdecl
# else
# /* On other compilers on OS/2, we use the _System calling convention */
# /* to be compatible with every compiler */
# define STCALL _System
# endif
# elif defined(__GNUC__)
# define STCALL __attribute__((__cdecl__))
# else
# define STCALL
# endif
#endif // STCALL
// T Y P E D E F I N E S /////////////////////////////////////////
//
// Type defines
#ifndef _MSC_VER
typedef int32_t HRESULT;
typedef unsigned char BYTE;
typedef unsigned short WORD;
typedef unsigned int DWORD;
typedef uint64_t QWORD;
typedef char CHAR;
typedef CHAR* LPSTR;
typedef const CHAR* LPCSTR;
typedef CHAR TCHAR;
typedef LPSTR LPTSTR;
typedef LPCSTR LPCTSTR;
#define _tcslen strlen
#define _tcscpy strcpy
#define _tcsncpy strncpy
#define _tcschr strchr
#define _tcsrchr strrchr
#define _tcscmp strcmp
#define _tcsncmp strncmp
#define _tcsicmp strcasecmp
#define _tcsnicmp strncasecmp
#define _tcsncicmp(s1,s2,s) strncasecmp(s1, s2, (s)*sizeof(TCHAR))
#define _stprintf sprintf
#define _sntprintf snprintf
#define _vsntprintf vsnprintf
#define _vsctprintf _vscprintf
int _vscprintf(LPCSTR format, va_list pargs);
#define _T(s) s
#endif //_MSC_VER
#define DECLARE_NO_INDEX(...) std::numeric_limits<__VA_ARGS__>::max()
#ifndef MAKEWORD
#define MAKEWORD(a, b) ((WORD)(((BYTE)(((DWORD)(a)) & 0xff)) | ((WORD)((BYTE)(((DWORD)(b)) & 0xff))) << 8))
#endif
#ifndef MAKELONG
#define MAKELONG(a, b) ((DWORD)(((WORD)(((DWORD)(a)) & 0xffff)) | ((DWORD)((WORD)(((DWORD)(b)) & 0xffff))) << 16))
#endif
#ifndef LOWORD
#define LOWORD(l) ((WORD)(((DWORD)(l)) & 0xffff))
#endif
#ifndef HIWORD
#define HIWORD(l) ((WORD)((((DWORD)(l)) >> 16) & 0xffff))
#endif
#ifndef LOBYTE
#define LOBYTE(w) ((BYTE)(((WORD)(w)) & 0xff))
#endif
#ifndef HIBYTE
#define HIBYTE(w) ((BYTE)((((WORD)(w)) >> 8) & 0xff))
#endif
#ifndef MAX_PATH
#define MAX_PATH 260
#endif
#ifndef NULL
#define NULL 0
#endif
#ifdef max
#undef max
#endif
#ifdef min
#undef min
#endif
#ifndef MINF
#define MINF std::min
#endif
#ifndef MAXF
#define MAXF std::max
#endif
#ifndef RAND
#define RAND std::rand
#endif
namespace SEACAVE {
// signed and unsigned types of the size of the architecture
// (32 or 64 bit for x86 and respectively x64)
#ifdef _ENVIRONMENT64
typedef int64_t int_t;
typedef uint64_t uint_t;
#else
typedef int32_t int_t;
typedef uint32_t uint_t;
#endif
// type used for the size of the files
typedef int64_t size_f_t;
// type used as the default floating number precision
typedef double REAL;
// invalid index
constexpr uint32_t NO_ID = DECLARE_NO_INDEX(uint32_t);
template<typename TYPE, typename REALTYPE=REAL>
struct RealType { typedef typename std::conditional<std::is_floating_point<TYPE>::value, TYPE, REALTYPE>::type type; };
template<typename T>
inline T MINF3(const T& x1, const T& x2, const T& x3) {
return MINF(MINF(x1, x2), x3);
}
template<typename T>
inline T MAXF3(const T& x1, const T& x2, const T& x3) {
return MAXF(MAXF(x1, x2), x3);
}
template<typename T>
FORCEINLINE T RANDOM() { return T(RAND())/RAND_MAX; }
template<typename T1, typename T2>
union TAliasCast {
T1 f;
T2 i;
inline TAliasCast() {}
inline TAliasCast(T1 v) : f(v) {}
inline TAliasCast(T2 v) : i(v) {}
inline TAliasCast& operator = (T1 v) { f = v; return *this; }
inline TAliasCast& operator = (T2 v) { i = v; return *this; }
inline operator T1 () const { return f; }
};
typedef TAliasCast<float,int32_t> CastF2I;
typedef TAliasCast<double,int32_t> CastD2I;
} // namespace SEACAVE
#if defined(_MSC_VER)
# define __LITTLE_ENDIAN 0
# define __BIG_ENDIAN 1
# define __PDP_ENDIAN 2
# define __BYTE_ORDER __LITTLE_ENDIAN
#elif defined(__APPLE__)
# include <machine/endian.h>
#elif defined(__GNUC__)
# include <endian.h>
#endif
// I N C L U D E S /////////////////////////////////////////////////
#include "Strings.h"
#include "AutoPtr.h"
#include "List.h"
#include "Thread.h"
#include "SharedPtr.h"
#include "Queue.h"
#include "Hash.h"
#include "Timer.h"
#include "CriticalSection.h"
#include "Semaphore.h"
#include "Util.h"
#include "File.h"
#include "MemFile.h"
#include "LinkLib.h"
namespace SEACAVE {
typedef class GENERAL_API CSharedPtr<File> FilePtr;
typedef class GENERAL_API CSharedPtr<ISTREAM> ISTREAMPTR;
typedef ISTREAM* LPISTREAM;
typedef class GENERAL_API CSharedPtr<OSTREAM> OSTREAMPTR;
typedef OSTREAM* LPOSTREAM;
typedef class GENERAL_API CSharedPtr<IOSTREAM> IOSTREAMPTR;
typedef IOSTREAM* LPIOSTREAM;
typedef class GENERAL_API cList<void*, void*, 0> VoidArr;
typedef class GENERAL_API cList<LPCTSTR, LPCTSTR, 0> LPCTSTRArr;
typedef class GENERAL_API cList<String> StringArr;
typedef class GENERAL_API cList<IDX, IDX, 0> IDXArr;
typedef class GENERAL_API cList<uint8_t, uint8_t, 0> Unsigned8Arr;
typedef class GENERAL_API cList<unsigned, unsigned, 0> UnsignedArr;
typedef class GENERAL_API cList<uint32_t, uint32_t, 0> Unsigned32Arr;
typedef class GENERAL_API cList<uint64_t, uint64_t, 0> Unsigned64Arr;
typedef class GENERAL_API cList<size_t, size_t, 0> SizeArr;
typedef class GENERAL_API cList<int, int, 0> IntArr;
typedef class GENERAL_API cList<bool, bool, 0> BoolArr;
typedef class GENERAL_API cList<float, float, 0> FloatArr;
typedef class GENERAL_API cList<double, double, 0> DoubleArr;
} // namespace SEACAVE
#include "Log.h"
#include "EventQueue.h"
#include "SML.h"
#include "ConfigTable.h"
#include "HTMLDoc.h"
// D E F I N E S ///////////////////////////////////////////////////
//
// Constant defines
// everything went smooth
#define _OK ((HRESULT)0L)
// just reports no errors
#define _CANCEL 0x82000000
// general error message
#define _FAIL 0x82000001
// specific error messages
#define _CREATEAPI 0x82000002
#define _CREATEDEVICE 0x82000003
#define _CREATEBUFFER 0x82000004
#define _INVALIDPARAM 0x82000005
#define _INVALIDID 0x82000006
#define _BUFFERSIZE 0x82000007
#define _BUFFERLOCK 0x82000008
#define _NOTCOMPATIBLE 0x82000009
#define _OUTOFMEMORY 0x8200000a
#define _FILENOTFOUND 0x8200000b
#define _INVALIDFILE 0x8200000c
#define _NOSHADERSUPPORT 0x8200000d
#define _NOSERVERFOUND 0x8200000e
#define _WOULDBLOCK 0x8200000f
#ifndef SUCCEEDED
#define SUCCEEDED(hr) (((HRESULT)(hr)) >= 0)
#endif
#ifndef FAILED
#define FAILED(hr) (((HRESULT)(hr)) < 0)
#endif
// D E F I N E S ///////////////////////////////////////////////////
#define RGBA(r, g, b, a) ((DWORD)(((a) << 24) | ((r) << 16) | ((g) << 8) | (b)))
#define RGBC(clr) (RGBA((BYTE)((clr).fR*255), (BYTE)((clr).fG*255), (BYTE)((clr).fB*255), (BYTE)((clr).fA*255)))
#define RGB24TO8(r,g,b) ((BYTE)((((WORD)r)*30+((WORD)g)*59+((WORD)b)*11)/100))
#define RGB24TO16(r,g,b) ((((WORD)(((BYTE)(r))>>3))<<11) | (((WORD)(((BYTE)(g))>>2))<<5) | ((WORD)(((BYTE)(b))>>3)))
#define RGB16TOR(rgb) (((BYTE)(((WORD)(rgb))>>11))<<3)
#define RGB16TOG(rgb) (((BYTE)((((WORD)(rgb))&0x07E0)>>5))<<2)
#define RGB16TOB(rgb) (((BYTE)(((WORD)(rgb))&0x001F))<<3)
#define TIMER_START() SEACAVE::Timer::SysType timerStart = SEACAVE::Timer::GetSysTime()
#define TIMER_UPDATE() timerStart = SEACAVE::Timer::GetSysTime()
#define TIMER_GET() SEACAVE::Timer::SysTime2TimeMs(SEACAVE::Timer::GetSysTime() - timerStart)
#define TIMER_GET_INT() ((SEACAVE::Timer::SysType)TIMER_GET())
#define TIMER_GET_FORMAT() SEACAVE::Util::formatTime(TIMER_GET_INT())
// D E F I N E S ///////////////////////////////////////////////////
#ifndef CHECK
#define CHECK(exp) { if (!(exp)) { VERBOSE("Check failed: " #exp); abort(); } }
#endif
#ifndef ABORT
#define ABORT(msg) { VERBOSE("error: " #msg); exit(-1); }
#endif
#ifndef _USE_MATH_DEFINES
/** e */
#ifndef M_E
#define M_E 2.7182818284590452353602874713527
#endif
/** ln(2) */
#ifndef M_LN2
#define M_LN2 0.69314718055994530941723212145818
#endif
/** ln(10) */
#ifndef M_LN10
#define M_LN10 2.3025850929940456840179914546844
#endif
/** pi */
#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif
/** pi/2 */
#ifndef M_PI_2
#define M_PI_2 1.5707963267948966192313216916398
#endif
/** 1/pi */
#ifndef M_1_PI
#define M_1_PI 0.31830988618379067153776752674503
#endif
/** 2/pi */
#ifndef M_2_PI
#define M_2_PI 0.63661977236758134307553505349006
#endif
/** 2*sqrt(pi) */
#ifndef M_2_SQRTPI
#define M_2_SQRTPI 1.1283791670955125738961589031216
#endif
/** sqrt(2) */
#ifndef M_SQRT2
#define M_SQRT2 1.4142135623730950488016887242097
#endif
/** sqrt(1/2) */
#ifndef M_SQRT1_2
#define M_SQRT1_2 0.70710678118654752440084436210485
#endif
#endif
// constants
#define TWO_PI 6.283185307179586476925286766559
#define PI 3.1415926535897932384626433832795
#define HALF_PI 1.5707963267948966192313216916398
#define SQRT_2PI 2.506628274631000502415765284811
#define INV_TWO_PI 0.15915494309189533576888376337251
#define INV_PI 0.31830988618379067153776752674503
#define INV_HALF_PI 0.63661977236758134307553505349006
#define INV_SQRT_2PI 0.39894228040143267793994605993439
#define D2R(d) ((d)*(PI/180.0)) // degree to radian
#define R2D(r) ((r)*(180.0/PI)) // radian to degree
#define SQRT_2 1.4142135623730950488016887242097
#define SQRT_3 1.7320508075688772935274463415059
#define LOG_2 0.30102999566398119521373889472449
#define LN_2 0.69314718055994530941723212145818
#define ZERO_TOLERANCE (1e-7)
#define INV_ZERO (1e+14)
// float constants
#define FTWO_PI ((float)TWO_PI)
#define FPI ((float)PI)
#define FHALF_PI ((float)HALF_PI)
#define FSQRT_2PI ((float)SQRT_2PI)
#define FINV_TWO_PI ((float)INV_TWO_PI)
#define FINV_PI ((float)INV_PI)
#define FINV_HALF_PI ((float)INV_HALF_PI)
#define FINV_SQRT_2PI ((float)INV_SQRT_2PI)
#define FD2R(d) ((d)*(FPI/180.f)) // degree to radian
#define FR2D(r) ((r)*(180.f/FPI)) // radian to degree
#define FSQRT_2 ((float)SQRT_2)
#define FSQRT_3 ((float)SQRT_3)
#define FLOG_2 ((float)LOG_2)
#define FLN_2 ((float)LN_2)
#define FZERO_TOLERANCE 0.0001f
#define FINV_ZERO 1000000.f
#define GCLASS unsigned
#define FRONT 0
#define BACK 1
#define PLANAR 2
#define CLIPPED 3
#define CULLED 4
#define VISIBLE 5
// M A C R O S /////////////////////////////////////////////////////
#define FLOOR SEACAVE::Floor2Int
#define FLOOR2INT SEACAVE::Floor2Int
#define CEIL SEACAVE::Ceil2Int
#define CEIL2INT SEACAVE::Ceil2Int
#define ROUND SEACAVE::Round2Int
#define ROUND2INT SEACAVE::Round2Int
#define SIN std::sin
#define ASIN std::asin
#define COS std::cos
#define ACOS std::acos
#define TAN std::tan
#define ATAN std::atan
#define ATAN2 std::atan2
#define POW std::pow
#define POWI SEACAVE::powi
#define LOG2I SEACAVE::log2i
namespace SEACAVE {
// F U N C T I O N S ///////////////////////////////////////////////
template<typename T>
struct MakeIdentity { using type = T; };
template<typename T>
using MakeSigned = typename std::conditional<std::is_integral<T>::value,std::make_signed<T>,SEACAVE::MakeIdentity<T>>::type;
template<typename T1, typename T2>
constexpr T1 Cast(const T2& v) {
return static_cast<T1>(v);
}
template<typename T>
constexpr T& NEGATE(T& a) {
return (a = -a);
}
template<typename T>
constexpr T SQUARE(const T& a) {
return a * a;
}
template<typename T>
constexpr T CUBE(const T& a) {
return a * a * a;
}
template<typename T>
inline T SQRT(const T& a) {
return T(sqrt(a));
}
template<typename T>
inline T EXP(const T& a) {
return T(exp(a));
}
template<typename T>
inline T LOGN(const T& a) {
return T(log(a));
}
template<typename T>
inline T LOG10(const T& a) {
return T(log10(a));
}
template<typename T>
constexpr T powi(T base, unsigned exp) {
T result(1);
while (exp) {
if (exp & 1)
result *= base;
exp >>= 1;
base *= base;
}
return result;
}
constexpr int log2i(unsigned val) {
int ret = -1;
while (val) {
val >>= 1;
++ret;
}
return ret;
}
template <unsigned N> constexpr inline int log2i() { return 1+log2i<(N>>1)>(); }
template <> constexpr inline int log2i<0>() { return -1; }
template <> constexpr inline int log2i<1>() { return 0; }
template <> constexpr inline int log2i<2>() { return 1; }
template<typename T>
inline T arithmeticSeries(T n, T a1=1, T d=1) {
return (n*(a1*2+(n-1)*d))/2;
}
template<typename T>
constexpr T factorial(T n) {
T ret = 1;
while (n > 1)
ret *= n--;
return ret;
}
template<typename T>
constexpr T combinations(const T& n, const T& k) {
ASSERT(n >= k);
#if 1
T num = n;
const T den = factorial(k);
for (T i=n-k+1; i<n; ++i)
num *= i;
ASSERT(num%den == 0);
return num/den;
#else
return factorial(n) / (factorial(k)*factorial(n-k));
#endif
}
// adapted from https://github.com/whackashoe/fastapprox.git
// (set bSafe to true if the values might be smaller than -126)
template<bool bSafe>
inline float FPOW2(float p) {
if (bSafe && p < -126.f) {
return 0.f;
} else {
ASSERT(p >= -126.f);
CastF2I v;
v.i = static_cast<int32_t>((1 << 23) * (p + 126.94269504f));
return v.f;
}
}
template<bool bSafe>
inline float FEXP(float v) {
return FPOW2<bSafe>(1.44269504f * v);
}
// Inverse of the square root
// Compute a fast 1 / sqrtf(v) approximation
inline float RSQRT(float v) {
#ifdef _FAST_INVSQRT
// This code supposedly originates from Id-software
const float halfV = v * 0.5f;
(int32_t&)v = 0x5f3759df - (((int32_t&)v) >> 1);
// Iterations of the Newton's method
v = v * (1.5f - halfV * v * v);
v = v * (1.5f - halfV * v * v);
return v * (1.5f - halfV * v * v);
#else
return 1.f / SQRT(v);
#endif
}
inline double RSQRT(const double& x) {
#ifdef _FAST_INVSQRT
double v = x;
const double halfV = v * 0.5;
(int64_t&)v = 0x5fe6ec85e7de30daLL - (((int64_t&)v) >> 1);
// Iterations of the Newton's method
v = v * (1.5 - halfV * v * v);
v = v * (1.5 - halfV * v * v);
v = v * (1.5 - halfV * v * v);
return v * (1.5 - halfV * v * v);
#else
return 1.0 / SQRT(x);
#endif
}
// approximate tanh
template <typename T>
inline T TANH(const T& x) {
const T x2 = x*x;
#if 0
// Taylor series expansion (very inaccurate)
return x*(1.0 + x2*(-T(1)/T(3) + x2*(T(2)/T(15) + x2*(-T(17)/T(315) + x2*(T(62)/T(2835) - x2*(T(1382)/T(155925)))))));
#else
// Lambert's continued fraction
const T den = (((x2+T(378))*x2+T(17325))*x2+T(135135))*x;
const T div = ((x2*T(28)+T(3150))*x2+T(62370))*x2+T(135135);
return den/div;
#endif
}
/*----------------------------------------------------------------*/
// Cubic root functions
// cube root approximation using bit hack for 32-bit float (5 decimals)
// (exploits the properties of IEEE 754 floating point numbers
// by leveraging the fact that their binary representation is close to a log2 representation)
inline float cbrt5(float x) {
#if 0
CastF2I c(x);
c.i = ((c.i-(127<<23))/3+(127<<23));
#else
TAliasCast<float,uint32_t> c(x);
c.i = c.i/3 + 709921077u;
#endif
return c.f;
}
// cube root approximation using bit hack for 64-bit float
// adapted from Kahan's cbrt (5 decimals)
inline double cbrt5(double x) {
TAliasCast<double,uint32_t[2]> c(0.0), d(x);
c.i[1] = d.i[1]/3 + 715094163u;
return c.f;
}
// iterative cube root approximation using Halley's method
// faster convergence than Newton's method: (R/(a*a)+a*2)/3
template<typename T>
FORCEINLINE T cbrt_halley(const T& a, const T& R) {
const T a3 = a*a*a;
const T a3R = a3+R;
return a * (a3R + R) / (a3 + a3R);
}
// fast cubic root (variable precision)
template<typename T, int N>
FORCEINLINE T fast_cbrt(const T& x) {
return cbrt_halley(fast_cbrt<T,N-1>(x), x);
}
template<>
FORCEINLINE double fast_cbrt<double,1>(const double& x) {
return cbrt_halley((double)cbrt5((float)x), x);
}
template<>
FORCEINLINE float fast_cbrt<float,1>(const float& x) {
return cbrt_halley(cbrt5(x), x);
}
// default cubic root function
FORCEINLINE float CBRT(float x) {
#ifdef _FAST_CBRT
return fast_cbrt<float,1>(x);
#else
return POW(x, 1.0f/3.0f);
#endif
}
FORCEINLINE double CBRT(const double& x) {
#ifdef _FAST_CBRT
return fast_cbrt<double,2>(x);
#else
return POW(x, 1.0/3.0);
#endif
}
/*----------------------------------------------------------------*/
#if defined(__GNUC__)
FORCEINLINE int PopCnt(uint32_t bb) {
return __builtin_popcount(bb);
}
FORCEINLINE int PopCnt(uint64_t bb) {
return __builtin_popcountll(bb);
}
FORCEINLINE int PopCnt15(uint64_t bb) {
return __builtin_popcountll(bb);
}
FORCEINLINE int PopCntSparse(uint64_t bb) {
return __builtin_popcountll(bb);
}
#elif defined(_USE_SSE) && defined(_M_AMD64) // 64 bit windows
FORCEINLINE int PopCnt(uint32_t bb) {
return (int)_mm_popcnt_u32(bb);
}
FORCEINLINE int PopCnt(uint64_t bb) {
return (int)_mm_popcnt_u64(bb);
}
FORCEINLINE int PopCnt15(uint64_t bb) {
return (int)_mm_popcnt_u64(bb);
}
FORCEINLINE int PopCntSparse(uint64_t bb) {
return (int)_mm_popcnt_u64(bb);
}
#else
// general purpose population count
template<typename T>
constexpr int PopCnt(T bb)
{
STATIC_ASSERT(std::is_integral<T>::value && std::is_unsigned<T>::value);
return std::bitset<sizeof(T)*8>(bb).count();
}
template<>
inline int PopCnt(uint64_t bb) {
const uint64_t k1 = (uint64_t)0x5555555555555555;
const uint64_t k2 = (uint64_t)0x3333333333333333;
const uint64_t k3 = (uint64_t)0x0F0F0F0F0F0F0F0F;
const uint64_t k4 = (uint64_t)0x0101010101010101;
bb -= (bb >> 1) & k1;
bb = (bb & k2) + ((bb >> 2) & k2);
bb = (bb + (bb >> 4)) & k3;
return (bb * k4) >> 56;
}
// faster version assuming not more than 15 bits set, used in mobility
// eval, posted on CCC forum by Marco Costalba of Stockfish team
inline int PopCnt15(uint64_t bb) {
unsigned w = unsigned(bb >> 32), v = unsigned(bb);
v -= (v >> 1) & 0x55555555; // 0-2 in 2 bits
w -= (w >> 1) & 0x55555555;
v = ((v >> 2) & 0x33333333) + (v & 0x33333333); // 0-4 in 4 bits
w = ((w >> 2) & 0x33333333) + (w & 0x33333333);
v += w; // 0-8 in 4 bits
v *= 0x11111111;
return int(v >> 28);
}
// version faster on sparsely populated bitboards
inline int PopCntSparse(uint64_t bb) {
int count = 0;
while (bb) {
count++;
bb &= bb - 1;
}
return count;
}
#endif
/*----------------------------------------------------------------*/
#ifdef _FAST_FLOAT2INT
// fast float to int conversion
// (xs routines at stereopsis: http://www.stereopsis.com/sree/fpu2006.html by Sree Kotay)
const double _float2int_doublemagic = 6755399441055744.0; //2^52 * 1.5, uses limited precision to floor
const double _float2int_doublemagicdelta = (1.5e-8);
const double _float2int_doublemagicroundeps = (.5f-_float2int_doublemagicdelta); //almost .5f = .5f - 1e^(number of exp bit)
FORCEINLINE int CRound2Int(const double& x) {
const CastD2I c(x + _float2int_doublemagic);
ASSERT(int32_t(floor(x+.5)) == c.i);
return c.i;
}
#endif
template <typename INTTYPE=int>
FORCEINLINE INTTYPE Floor2Int(float x) {
#ifdef _FAST_FLOAT2INT
return CRound2Int(double(x)-_float2int_doublemagicroundeps);
#else
return static_cast<INTTYPE>(floor(x));
#endif
}
template <typename INTTYPE=int>
FORCEINLINE INTTYPE Floor2Int(double x) {
#ifdef _FAST_FLOAT2INT
return CRound2Int(x-_float2int_doublemagicroundeps);
#else
return static_cast<INTTYPE>(floor(x));
#endif
}
template <typename INTTYPE=int>
FORCEINLINE INTTYPE Ceil2Int(float x) {
#ifdef _FAST_FLOAT2INT
return CRound2Int(double(x)+_float2int_doublemagicroundeps);
#else
return static_cast<INTTYPE>(ceil(x));
#endif
}
template <typename INTTYPE=int>
FORCEINLINE INTTYPE Ceil2Int(double x) {
#ifdef _FAST_FLOAT2INT
return CRound2Int(x+_float2int_doublemagicroundeps);
#else
return static_cast<INTTYPE>(ceil(x));
#endif
}
template <typename INTTYPE=int>
FORCEINLINE INTTYPE Round2Int(float x) {
#ifdef _FAST_FLOAT2INT
return CRound2Int(double(x)+_float2int_doublemagicdelta);
#else
return static_cast<INTTYPE>(floor(x+.5f));
#endif
}
template <typename INTTYPE=int>
FORCEINLINE INTTYPE Round2Int(double x) {
#ifdef _FAST_FLOAT2INT
return CRound2Int(x+_float2int_doublemagicdelta);
#else
return static_cast<INTTYPE>(floor(x+.5));
#endif
}
/*----------------------------------------------------------------*/
// INTERPOLATION
// Linear interpolation
inline float lerp(float u, float v, float x)
{
return u + (v - u) * x;
}
template<typename Type>
inline Type lerp(const Type& u, const Type& v, float x)
{
return u + (v - u) * x;
}
// Cubic interpolation
inline float cerp(float u0, float u1, float u2, float u3, float x)
{
const float p((u3 - u2) - (u0 - u1));
const float q((u0 - u1) - p);
const float r(u2 - u0);
return x * (x * (x * p + q) + r) + u1;
}
template<typename Type>
inline Type cerp(const Type& u0, const Type& u1, const Type& u2, const Type& u3, float x)
{
const Type p((u3 - u2) - (u0 - u1));
const Type q((u0 - u1) - p);
const Type r(u2 - u0);
return x * (x * (x * p + q) + r) + u1;
}
/*----------------------------------------------------------------*/
// S T R U C T S ///////////////////////////////////////////////////
#ifdef _USE_SSE
// define utile functions to deal with SSE operations
struct ALIGN(16) sse_vec4f {
union {
float v[4];
struct {
float x;
float y;
float z;
float w;
};
};
inline sse_vec4f() {}
inline sse_vec4f(const float* p) : x(p[0]), y(p[1]), z(p[2]), w(p[3]) {}
inline sse_vec4f(float f0, float f1, float f2, float f3) : x(f0), y(f1), z(f2), w(f3) {}
inline operator const float*() const {return v;}
inline operator float*() {return v;}
};
struct ALIGN(16) sse_vec2d {
union {
double v[2];
struct {
double x;
double y;
};
};
inline sse_vec2d() {}
inline sse_vec2d(const double* p) : x(p[0]), y(p[1]) {}
inline sse_vec2d(const double& f0, const double& f1) : x(f0), y(f1) {}
inline operator const double*() const {return v;}
inline operator double*() {return v;}
};
struct sse_f_t {
typedef __m128 sse_t;
typedef const sse_t& arg_sse_t;
typedef float real_t;
inline sse_f_t() {}
inline sse_f_t(const sse_t& p) : v(p) {}
inline sse_f_t(real_t p) : v(load1(p)) {}
inline sse_f_t(const real_t* p) : v(load(p)) {}
inline sse_f_t(real_t f0, real_t f1, real_t f2, real_t f3) : v(set(f0,f1,f2,f3)) {}
inline operator sse_t() const {return v;}
inline operator sse_t&() {return v;}
inline sse_t operator ==(sse_t s) const {return cmpeq(v,s);}
inline sse_t operator =(sse_t s) {return v=s;}
inline sse_t operator +(sse_t s) const {return add(v,s);}
inline sse_t operator +=(sse_t s) {return v=add(v,s);}
inline sse_t operator -(sse_t s) const {return sub(v,s);}
inline sse_t operator -=(sse_t s) {return v=sub(v,s);}
inline sse_t operator *(sse_t s) const {return mul(v,s);}
inline sse_t operator *=(sse_t s) {return v=mul(v,s);}
inline sse_t operator /(sse_t s) const {return div(v,s);}
inline sse_t operator /=(sse_t s) {return v=div(v,s);}
inline void get(real_t* p) const {store(p,v);}
static inline sse_t zero() {return _mm_setzero_ps();}
static inline sse_t load1(real_t p) {return _mm_load1_ps(&p);}
static inline sse_t load(const real_t* p) {return _mm_load_ps(p);}
static inline sse_t loadu(const real_t* p) {return _mm_loadu_ps(p);}
static inline sse_t set(real_t f0, real_t f1, real_t f2, real_t f3) {return _mm_set_ps(f0,f1,f2,f3);}
static inline void store(real_t *p, sse_t s){_mm_store_ps(p,s);}
static inline void storeu(real_t *p, sse_t s){_mm_storeu_ps(p,s);}
static inline sse_t add(sse_t s1, sse_t s2) {return _mm_add_ps(s1,s2);}
static inline sse_t sub(sse_t s1, sse_t s2) {return _mm_sub_ps(s1,s2);}
static inline sse_t mul(sse_t s1, sse_t s2) {return _mm_mul_ps(s1,s2);}
static inline sse_t div(sse_t s1, sse_t s2) {return _mm_div_ps(s1,s2);}
static inline sse_t min(sse_t s1, sse_t s2) {return _mm_min_ps(s1,s2);}
static inline sse_t max(sse_t s1, sse_t s2) {return _mm_max_ps(s1,s2);}
static inline sse_t cmpeq(sse_t s1, sse_t s2){return _mm_cmpeq_ps(s1,s2);}
static inline sse_t sqrt(sse_t s) {return _mm_sqrt_ps(s);}
static inline sse_t rsqrt(sse_t s) {return _mm_rsqrt_ps(s);}
static inline int floor2int(real_t f) {return _mm_cvtt_ss2si(_mm_load_ss(&f));}
#ifdef _WIN32
static inline real_t sum(sse_t s) {return (s.m128_f32[0]+s.m128_f32[2])+(s.m128_f32[1]+s.m128_f32[3]);}
static inline real_t sum3(sse_t s) {return (s.m128_f32[0]+s.m128_f32[2])+s.m128_f32[1];}
#else
static inline real_t sum(sse_t s) {real_t *f = (real_t*)(&s); return (f[0]+f[2])+(f[1]+f[3]);}
static inline real_t sum3(sse_t s) {real_t *f = (real_t*)(&s); return (f[0]+f[2])+f[1];}
#endif
/*
static inline real_t dot(sse_t s1, sse_t s2) {
sse_t temp = _mm_dp_ps(s1, s2, 0xF1);
real_t* f = (real_t*)(&temp); return f[0];
}
*/
static real_t dot(const real_t* a, const real_t* b, size_t size) {
const real_t* const end = a+size;
const size_t iters = (size>>2);
real_t fres = 0.f;
if (iters) {
const real_t* const e = a+(iters<<2);
sse_t mres = zero();
do {
mres = _mm_add_ps(mres, _mm_mul_ps(_mm_loadu_ps(a), _mm_loadu_ps(b)));
a += 4; b += 4;
} while (a < e);
fres = sum(mres);
}
while (a<end)
fres += (*a++) * (*b++);
return fres;
}
sse_t v;
};
class sse_d_t {
public:
typedef __m128d sse_t;
typedef double real_t;
inline sse_d_t() {}
inline sse_d_t(const sse_t& p) : v(p) {}
inline sse_d_t(const real_t& p) : v(load1(p)){}
inline sse_d_t(const real_t* p) : v(load(p)) {}
inline sse_d_t(const real_t& f0, const real_t& f1) : v(set(f0,f1)) {}
inline operator sse_t() const {return v;}
inline operator sse_t&() {return v;}
inline sse_t operator ==(sse_t s) const {return cmpeq(v,s);}
inline sse_t operator =(sse_t s) {return v=s;}
inline sse_t operator +(sse_t s) const {return add(v,s);}
inline sse_t operator +=(sse_t s) {return v=add(v,s);}
inline sse_t operator -(sse_t s) const {return sub(v,s);}
inline sse_t operator -=(sse_t s) {return v=sub(v,s);}
inline sse_t operator *(sse_t s) const {return mul(v,s);}
inline sse_t operator *=(sse_t s) {return v=mul(v,s);}
inline sse_t operator /(sse_t s) const {return div(v,s);}
inline sse_t operator /=(sse_t s) {return v=div(v,s);}
inline void get(real_t* p) const {store(p,v);}
static inline sse_t zero() {return _mm_setzero_pd();}
static inline sse_t load1(const real_t& p) {return _mm_load1_pd(&p);}
static inline sse_t load(const real_t* p) {return _mm_load_pd(p);}
static inline sse_t loadu(const real_t* p) {return _mm_loadu_pd(p);}
static inline sse_t set(const real_t& f0, const real_t& f1) {return _mm_set_pd(f0,f1);}
static inline void store(real_t *p, sse_t s){_mm_store_pd(p,s);}
static inline void storeu(real_t *p, sse_t s){_mm_storeu_pd(p,s);}
static inline sse_t add(sse_t s1, sse_t s2) {return _mm_add_pd(s1,s2);}
static inline sse_t sub(sse_t s1, sse_t s2) {return _mm_sub_pd(s1,s2);}
static inline sse_t mul(sse_t s1, sse_t s2) {return _mm_mul_pd(s1,s2);}
static inline sse_t div(sse_t s1, sse_t s2) {return _mm_div_pd(s1,s2);}
static inline sse_t min(sse_t s1, sse_t s2) {return _mm_min_pd(s1,s2);}
static inline sse_t max(sse_t s1, sse_t s2) {return _mm_max_pd(s1,s2);}
static inline sse_t cmpeq(sse_t s1, sse_t s2){return _mm_cmpeq_pd(s1,s2);}
static inline sse_t sqrt(sse_t s) {return _mm_sqrt_pd(s);}
static inline int floor2int(const real_t& f) {return _mm_cvttsd_si32(_mm_load_sd(&f));}
#ifdef _WIN32
static inline real_t sum(sse_t s) {return s.m128d_f64[0]+s.m128d_f64[1];}
#else
static inline real_t sum(sse_t s) {real_t *d = (real_t*)(&s); return d[0]+d[1];}
#endif
/*
static inline real_t dot(sse_t s1, sse_t s2) {
sse_t temp = _mm_dp_pd(s1, s2, 0x31);
real_t* f = (real_t*)(&temp); return f[0] ;
}
*/
static real_t dot(const real_t* a, const real_t* b, size_t size) {
const real_t* const end = a+size;
const size_t iters = (size>>1);
real_t fres = 0.0;
if (iters) {
const real_t* const e = a+(iters<<1);
sse_t mres = zero();
do {
mres = _mm_add_pd(mres, _mm_mul_pd(_mm_loadu_pd(a), _mm_loadu_pd(b)));
a += 2; b += 2;
} while (a < e);
fres = sum(mres);
}
while (a<end)
fres += (*a++) * (*b++);
return fres;
}
sse_t v;
};
inline void sse_prefetch(const void* p) {_mm_prefetch((const char*)p, _MM_HINT_NTA);}
#endif
// C L A S S E S ///////////////////////////////////////////////////
inline bool ISINFORNAN(float x) { return std::isinf(x) || std::isnan(x); }
inline bool ISINFORNAN(double x) { return std::isinf(x) || std::isnan(x); }
inline bool ISFINITE(float x) { return std::isfinite(x); }
inline bool ISFINITE(double x) { return std::isfinite(x); }
template<typename _Tp>
inline bool ISFINITE(const _Tp* x, size_t n) { for (size_t i=0; i<n; ++i) if (ISINFORNAN(x[i])) return false; return true; }
template<typename _Tp>
inline bool ISINSIDE(_Tp v,_Tp l0,_Tp l1) { ASSERT(l0<l1); return l0 <= v && v < l1; }
template<typename _Tp>
inline bool ISINSIDES(_Tp v,_Tp l0,_Tp l1) { return l0 < l1 ? ISINSIDE(v, l0, l1) : ISINSIDE(v, l1, l0); }
template<typename _Tp>
inline _Tp CLAMP(_Tp v, _Tp l0, _Tp l1) { ASSERT(l0<=l1); return MINF(MAXF(v, l0), l1); }
template<typename _Tp>
inline _Tp CLAMPS(_Tp v, _Tp l0, _Tp l1) { return l0 <= l1 ? CLAMP(v, l0, l1) : CLAMP(v, l1, l0); }
template<typename _Tp>
inline _Tp SIGN(_Tp x) { if (x > _Tp(0)) return _Tp(1); if (x < _Tp(0)) return _Tp(-1); return _Tp(0); }
template<typename _Tp>
inline _Tp ABS(_Tp x) { return std::abs(x); }
template<typename _Tp>
constexpr _Tp ZEROTOLERANCE() { return _Tp(0); }
template<>
constexpr float ZEROTOLERANCE() { return FZERO_TOLERANCE; }
template<>
constexpr double ZEROTOLERANCE() { return ZERO_TOLERANCE; }
template<typename _Tp>
constexpr _Tp EPSILONTOLERANCE() { return std::numeric_limits<_Tp>::epsilon(); }
template<>
constexpr float EPSILONTOLERANCE() { return 0.00001f; }
template<>
constexpr double EPSILONTOLERANCE() { return 1e-10; }
inline bool ISZERO(float x) { return ABS(x) < FZERO_TOLERANCE; }
inline bool ISZERO(double x) { return ABS(x) < ZERO_TOLERANCE; }
inline bool ISEQUAL(float x, float v) { return ABS(x-v) < FZERO_TOLERANCE; }
inline bool ISEQUAL(double x, double v) { return ABS(x-v) < ZERO_TOLERANCE; }
inline float INVZERO(float) { return FINV_ZERO; }
inline double INVZERO(double) { return INV_ZERO; }
template<typename _Tp>
inline _Tp INVZERO(_Tp) { return std::numeric_limits<_Tp>::max(); }
template<typename _Tp>
inline _Tp INVERT(_Tp x) { return (x==_Tp(0) ? INVZERO(x) : _Tp(1)/x); }
template<typename _Tp>
inline _Tp SAFEDIVIDE(_Tp x, _Tp y) { return (y==_Tp(0) ? INVZERO(y) : x/y); }
/*----------------------------------------------------------------*/
} // namespace SEACAVE
#include "Random.h"
#include "HalfFloat.h"
namespace SEACAVE {
// P R O T O T Y P E S /////////////////////////////////////////////
template <typename TYPE, int m, int n> class TMatrix;
template <typename TYPE, int DIMS> class TAABB;
template <typename TYPE, int DIMS> class TRay;
template <typename TYPE, int DIMS> class TPlane;
// 2D point struct
template <typename TYPE>
class TPoint2 : public cv::Point_<TYPE>
{
public:
typedef TYPE Type;
typedef cv::Point_<TYPE> Base;
typedef cv::Size Size;
typedef cv::Vec<TYPE,2> cvVec;
typedef cv::Matx<TYPE,2,1> Vec;
typedef cv::Matx<TYPE,1,2> VecT;
#ifdef _USE_EIGEN
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(TYPE,2)
typedef Eigen::Matrix<TYPE,2,1> EVec;
typedef Eigen::Map<const EVec> CEVecMap;
typedef Eigen::Map<EVec> EVecMap;
#endif
using Base::x;
using Base::y;
static const TPoint2 ZERO;
static const TPoint2 INF;
public:
inline TPoint2() {}
inline TPoint2(const Size& rhs) : Base(rhs) {}
template <typename T> inline TPoint2(const cv::Point_<T>& rhs) : Base((Type)rhs.x,(Type)rhs.y) {}
template <typename T> inline TPoint2(const cv::Matx<T,2,1>& rhs) : Base(rhs(0),rhs(1)) {}
template <typename T> inline TPoint2(const cv::Matx<T,1,2>& rhs) : Base(rhs(0),rhs(1)) {}
#ifdef _USE_EIGEN
inline TPoint2(const EVec& rhs) { operator EVecMap () = rhs; }
#endif
explicit inline TPoint2(const TYPE& _x) : Base(_x,_x) {}
inline TPoint2(const TYPE& _x, const TYPE& _y) : Base(_x,_y) {}
explicit inline TPoint2(const cv::Point3_<TYPE>& pt) : Base(pt.x/pt.z,pt.y/pt.z) {}
template <typename T> inline TPoint2& operator = (const cv::Point_<T>& rhs) { Base::operator = (rhs); return *this; }
template <typename T> inline TPoint2& operator = (const cv::Matx<T,2,1>& rhs) { operator Vec& () = rhs; return *this; }
template <typename T> inline TPoint2& operator = (const cv::Matx<T,1,2>& rhs) { operator VecT& () = rhs; return *this; }
#ifdef _USE_EIGEN
inline TPoint2& operator = (const EVec& rhs) { operator EVecMap () = rhs; return *this; }
#endif
// conversion to another data type
template <typename T> inline operator TPoint2<T> () const { return TPoint2<T>((T)x,(T)y); }
// pointer to the first element access
inline const TYPE* ptr() const { return &x; }
inline TYPE* ptr() { return &x; }
// 1D element access
inline const TYPE& operator [](size_t i) const { ASSERT(i>=0 && i<2); return ptr()[i]; }
inline TYPE& operator [](size_t i) { ASSERT(i>=0 && i<2); return ptr()[i]; }
// Access point as Size equivalent
inline operator const Size& () const { return *((const Size*)this); }
inline operator Size& () { return *((Size*)this); }
// Access point as vector equivalent
inline operator const Vec& () const { return *((const Vec*)this); }
inline operator Vec& () { return *((Vec*)this); }
// Access point as transposed vector equivalent
inline operator const VecT& () const { return *((const VecT*)this); }
inline operator VecT& () { return *((VecT*)this); }
#ifdef _USE_EIGEN
// Access point as Eigen equivalent
inline operator EVec () const { return CEVecMap((const TYPE*)this); }
// Access point as Eigen::Map equivalent
inline operator const CEVecMap () const { return CEVecMap((const TYPE*)this); }
inline operator EVecMap () { return EVecMap((TYPE*)this); }
#endif
#ifdef _USE_BOOST
// serialize
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar & boost::serialization::base_object<Base>(*this);
}
#endif
};
template <typename TYPE> const TPoint2<TYPE> TPoint2<TYPE>::ZERO(0,0);
template <typename TYPE> const TPoint2<TYPE> TPoint2<TYPE>::INF(std::numeric_limits<TYPE>::infinity(),std::numeric_limits<TYPE>::infinity());
/*----------------------------------------------------------------*/
typedef TPoint2<int> Point2i;
typedef TPoint2<hfloat> Point2hf;
typedef TPoint2<float> Point2f;
typedef TPoint2<double> Point2d;
/*----------------------------------------------------------------*/
// 3D point struct
template <typename TYPE>
class TPoint3 : public cv::Point3_<TYPE>
{
public:
typedef TYPE Type;
typedef cv::Point3_<TYPE> Base;
typedef cv::Vec<TYPE,3> cvVec;
typedef cv::Matx<TYPE,3,1> Vec;
typedef cv::Matx<TYPE,1,3> VecT;
#ifdef _USE_EIGEN
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(TYPE,3)
typedef Eigen::Matrix<TYPE,3,1> EVec;
typedef Eigen::Map<const EVec> CEVecMap;
typedef Eigen::Map<EVec> EVecMap;
#endif
using Base::x;
using Base::y;
using Base::z;
static const TPoint3 ZERO;
static const TPoint3 INF;
public:
inline TPoint3() {}
template <typename T> inline TPoint3(const cv::Point3_<T>& rhs) : Base((Type)rhs.x,(Type)rhs.y,(Type)rhs.z) {}
template <typename T> inline TPoint3(const cv::Matx<T,3,1>& rhs) : Base(rhs(0),rhs(1),rhs(2)) {}
template <typename T> inline TPoint3(const cv::Matx<T,1,3>& rhs) : Base(rhs(0),rhs(1),rhs(2)) {}
#ifdef _USE_EIGEN
inline TPoint3(const EVec& rhs) { operator EVecMap () = rhs; }
#endif
explicit inline TPoint3(const TYPE& _x) : Base(_x,_x,_x) {}
inline TPoint3(const TYPE& _x, const TYPE& _y, const TYPE& _z) : Base(_x,_y,_z) {}
template <typename T> inline TPoint3(const cv::Point_<T>& pt, const T& _z=T(1)) : Base(pt.x,pt.y,_z) {}
template <typename T1, typename T2> inline TPoint3(const cv::Point_<T1>& pt, const T2& _z) : Base(pt.x,pt.y,_z) {}
template <typename T> inline TPoint3& operator = (const cv::Point3_<T>& rhs) { Base::operator = (rhs); return *this; }
template <typename T> inline TPoint3& operator = (const cv::Matx<T,3,1>& rhs) { operator Vec& () = rhs; return *this; }
template <typename T> inline TPoint3& operator = (const cv::Matx<T,1,3>& rhs) { operator VecT& () = rhs; return *this; }
#ifdef _USE_EIGEN
inline TPoint3& operator = (const EVec& rhs) { operator EVecMap () = rhs; return *this; }
#endif
// conversion to another data type
template <typename T> inline operator TPoint3<T> () const { return TPoint3<T>((T)x,(T)y,(T)z); }
// pointer to the first element access
inline const TYPE* ptr() const { return &x; }
inline TYPE* ptr() { return &x; }
// 1D element access
inline const TYPE& operator [](BYTE i) const { ASSERT(i<3); return ptr()[i]; }
inline TYPE& operator [](BYTE i) { ASSERT(i<3); return ptr()[i]; }
// Access point as vector equivalent
inline operator const Vec& () const { return *reinterpret_cast<const Vec*>(this); }
inline operator Vec& () { return *reinterpret_cast<Vec*>(this); }
// Access point as transposed vector equivalent
inline operator const VecT& () const { return *reinterpret_cast<const VecT*>(this); }
inline operator VecT& () { return *reinterpret_cast<VecT*>(this); }
#ifdef _USE_EIGEN
// Access point as Eigen equivalent
inline operator EVec () const { return CEVecMap((const TYPE*)this); }
// Access point as Eigen::Map equivalent
inline operator const EVecMap () const { return CEVecMap((const TYPE*)this); }
inline operator EVecMap () { return EVecMap((TYPE*)this); }
#endif
// rotate point using the given parametrized rotation (axis-angle)
inline void RotateAngleAxis(const TPoint3& rot) { return (*this) = RotateAngleAxis((*this), rot); }
static TPoint3 RotateAngleAxis(const TPoint3& X, const TPoint3& rot);
#ifdef _USE_BOOST
// serialize
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar & boost::serialization::base_object<Base>(*this);
}
#endif
};
template <typename TYPE> const TPoint3<TYPE> TPoint3<TYPE>::ZERO(0,0,0);
template <typename TYPE> const TPoint3<TYPE> TPoint3<TYPE>::INF(std::numeric_limits<TYPE>::infinity(),std::numeric_limits<TYPE>::infinity(),std::numeric_limits<TYPE>::infinity());
/*----------------------------------------------------------------*/
typedef TPoint3<int> Point3i;
typedef TPoint3<hfloat> Point3hf;
typedef TPoint3<float> Point3f;
typedef TPoint3<double> Point3d;
/*----------------------------------------------------------------*/
// matrix struct
template <typename TYPE, int m, int n>
class TMatrix : public cv::Matx<TYPE,m,n>
{
public:
typedef TYPE Type;
typedef cv::Matx<TYPE,m,n> Base;
typedef cv::Vec<TYPE,m> Vec;
#ifdef _USE_EIGEN
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(TYPE,m*n)
typedef Eigen::Matrix<TYPE,m,n,(n>1?Eigen::RowMajor:Eigen::Default)> EMat;
typedef Eigen::Map<const EMat> CEMatMap;
typedef Eigen::Map<EMat> EMatMap;
#endif
using Base::val;
using Base::channels;
enum { elems = m*n };
static const TMatrix ZERO;
static const TMatrix IDENTITY;
static const TMatrix INF;
public:
inline TMatrix() {}
template <typename T> inline TMatrix(const cv::Matx<T,m,n>& rhs) : Base(rhs) {}
template <typename T> inline TMatrix(const cv::Point_<T>& rhs) : Base(rhs.x, rhs.y) {}
template <typename T> inline TMatrix(const cv::Point3_<T>& rhs) : Base(rhs.x, rhs.y, rhs.z) {}
inline TMatrix(const cv::Mat& rhs) : Base(rhs) {}
#ifdef _USE_EIGEN
inline TMatrix(const EMat& rhs) { operator EMatMap () = rhs; }
#endif
TMatrix(TYPE v0); //!< 1x1 matrix
TMatrix(TYPE v0, TYPE v1); //!< 1x2 or 2x1 matrix
TMatrix(TYPE v0, TYPE v1, TYPE v2); //!< 1x3 or 3x1 matrix
TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3); //!< 1x4, 2x2 or 4x1 matrix
TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4); //!< 1x5 or 5x1 matrix
TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5); //!< 1x6, 2x3, 3x2 or 6x1 matrix
TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5, TYPE v6); //!< 1x7 or 7x1 matrix
TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5, TYPE v6, TYPE v7); //!< 1x8, 2x4, 4x2 or 8x1 matrix
TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5, TYPE v6, TYPE v7, TYPE v8); //!< 1x9, 3x3 or 9x1 matrix
TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3, TYPE v4, TYPE v5, TYPE v6, TYPE v7, TYPE v8, TYPE v9); //!< 1x10, 2x5 or 5x2 or 10x1 matrix
TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3,
TYPE v4, TYPE v5, TYPE v6, TYPE v7,
TYPE v8, TYPE v9, TYPE v10, TYPE v11); //!< 1x12, 2x6, 3x4, 4x3, 6x2 or 12x1 matrix
TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3,
TYPE v4, TYPE v5, TYPE v6, TYPE v7,
TYPE v8, TYPE v9, TYPE v10, TYPE v11,
TYPE v12, TYPE v13); //!< 1x14, 2x7, 7x2 or 14x1 matrix
TMatrix(TYPE v0, TYPE v1, TYPE v2, TYPE v3,
TYPE v4, TYPE v5, TYPE v6, TYPE v7,
TYPE v8, TYPE v9, TYPE v10, TYPE v11,
TYPE v12, TYPE v13, TYPE v14, TYPE v15); //!< 1x16, 4x4 or 16x1 matrix
explicit TMatrix(const TYPE* vals); //!< initialize from a plain array
TMatrix(const TMatrix<TYPE,m,n>& a, const TMatrix<TYPE,m,n>& b, cv::Matx_AddOp) : Base(a, b, cv::Matx_AddOp()) {}
TMatrix(const TMatrix<TYPE,m,n>& a, const TMatrix<TYPE,m,n>& b, cv::Matx_SubOp) : Base(a, b, cv::Matx_SubOp()) {}
template<typename TYPE2> TMatrix(const TMatrix<TYPE,m,n>& a, TYPE2 alpha, cv::Matx_ScaleOp) : Base(a, alpha, cv::Matx_ScaleOp()) {}
TMatrix(const TMatrix<TYPE,m,n>& a, const TMatrix<TYPE,m,n>& b, cv::Matx_MulOp) : Base(a, b, cv::Matx_MulOp()) {}
TMatrix(const TMatrix<TYPE,m,n>& a, const TMatrix<TYPE,m,n>& b, cv::Matx_DivOp) : Base(a, b, cv::Matx_DivOp()) {}
template<int l> TMatrix(const TMatrix<TYPE,m,l>& a, const TMatrix<TYPE,l,n>& b, cv::Matx_MatMulOp) : Base(a, b, cv::Matx_MatMulOp()) {}
TMatrix(const TMatrix<TYPE,n,m>& a, cv::Matx_TOp) : Base(a, cv::Matx_TOp()) {}
template <typename T> inline TMatrix& operator = (const cv::Matx<T,m,n>& rhs) { Base::operator = (rhs); return *this; }
inline TMatrix& operator = (const cv::Mat& rhs) { Base::operator = (rhs); return *this; }
#ifdef _USE_EIGEN
inline TMatrix& operator = (const EMat& rhs) { operator EMatMap () = rhs; return *this; }
#endif
inline bool IsEqual(const Base&) const;
inline bool IsEqual(const Base&, TYPE eps) const;
// 1D element access
inline const TYPE& operator [](size_t i) const { ASSERT(i<elems); return val[i]; }
inline TYPE& operator [](size_t i) { ASSERT(i<elems); return val[i]; }
// Access point as vector equivalent
inline operator const Vec& () const { return *reinterpret_cast<const Vec*>(this); }
inline operator Vec& () { return *reinterpret_cast<Vec*>(this); }
#ifdef _USE_EIGEN
// Access point as Eigen equivalent
inline operator EMat () const { return CEMatMap((const TYPE*)val); }
// Access point as Eigen::Map equivalent
inline operator CEMatMap() const { return CEMatMap((const TYPE*)val); }
inline operator EMatMap () { return EMatMap((TYPE*)val); }
#endif
// calculate right null-space of this matrix ([n,n-m])
inline TMatrix<TYPE,n,n-m> RightNullSpace(int flags = 0) const;
// calculate right/left null-vector of this matrix ([n/m,1])
inline TMatrix<TYPE,n,1> RightNullVector(int flags = 0) const;
inline TMatrix<TYPE,m,1> LeftNullVector(int flags = 0) const;
#ifdef _USE_BOOST
// serialize
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar & boost::serialization::base_object<Base>(*this);
}
#endif
};
template <typename TYPE, int m, int n> const TMatrix<TYPE,m,n> TMatrix<TYPE,m,n>::ZERO(TMatrix::zeros());
template <typename TYPE, int m, int n> const TMatrix<TYPE,m,n> TMatrix<TYPE,m,n>::IDENTITY(TMatrix::eye());
template <typename TYPE, int m, int n> const TMatrix<TYPE,m,n> TMatrix<TYPE,m,n>::INF(TMatrix::all(std::numeric_limits<TYPE>::infinity()));
/*----------------------------------------------------------------*/
// generic matrix struct
template <typename TYPE>
class TDMatrix : public cv::Mat_<TYPE>
{
public:
typedef TYPE Type;
typedef cv::Mat_<TYPE> Base;
typedef cv::Size Size;
#ifdef _USE_EIGEN
typedef Eigen::Matrix<TYPE,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> EMat;
typedef Eigen::Map<const EMat> CEMatMap;
typedef Eigen::Map<EMat> EMatMap;
#endif
using Base::rows;
using Base::cols;
using Base::data;
using Base::step;
using Base::dims;
public:
inline TDMatrix() {}
inline TDMatrix(const Base& rhs) : Base(rhs) {}
inline TDMatrix(const cv::Mat& rhs) : Base(rhs) {}
inline TDMatrix(const cv::MatExpr& rhs) : Base(rhs) {}
inline TDMatrix(int _rows, int _cols) : Base(_rows, _cols) {}
inline TDMatrix(int _rows, int _cols, TYPE* _data, size_t _step=Base::AUTO_STEP) : Base(_rows, _cols, _data, _step) {}
inline TDMatrix(const Size& sz) : Base(sz) {}
inline TDMatrix(const Size& sz, const TYPE& v) : Base(sz, v) {}
inline TDMatrix(const Size& sz, TYPE* _data, size_t _step=Base::AUTO_STEP) : Base(sz.height, sz.width, _data, _step) {}
#ifdef _USE_EIGEN
inline TDMatrix(const EMat& rhs) { operator EMatMap () = rhs; }
#endif
inline TDMatrix& operator = (const Base& rhs) { Base::operator=(rhs); return *this; }
inline TDMatrix& operator = (const cv::MatExpr& rhs) { Base::operator=(rhs); return *this; }
#ifdef _USE_EIGEN
inline TDMatrix& operator = (const EMat& rhs) { operator EMatMap () = rhs; return *this; }
#endif
/// Construct the 2D matrix with the desired size and init its elements
inline void construct(int _rows, int _cols) {
Base::create(_rows, _cols);
for (int i=0; i<rows; ++i)
for (int j=0; j<cols; ++j)
new(cv::Mat::ptr<TYPE>(i,j)) TYPE;
}
inline void construct(const Size& sz) { construct(sz.height, sz.width); }
inline void destroy() {
for (int i=0; i<rows; ++i)
for (int j=0; j<cols; ++j)
cv::Mat::ptr<TYPE>(i,j)->~TYPE();
}
/// Set all elements to the given value
inline void memset(uint8_t v) { ASSERT(dims == 2 && cv::Mat::isContinuous()); ::memset(data, v, row_stride()*rows); }
inline void fill(const TYPE& v) { ASSERT(dims == 2); for (int i=0; i<rows; ++i) for (int j=0; j<cols; ++j) cv::Mat::at<TYPE>(i,j) = v; }
/// What is the row stride of the matrix?
inline size_t row_stride() const { ASSERT(dims == 2); return step[0]; }
/// What is the elem stride of the matrix?
inline size_t elem_stride() const { ASSERT(dims == 2 && step[1] == sizeof(TYPE)); return step[1]; }
/// Compute the area of the 2D matrix
inline int area() const { ASSERT(dims == 2); return cols*rows; }
/// Is this coordinate inside the 2D matrix?
template <typename T>
static inline typename std::enable_if<std::is_integral<T>::value,bool>::type isInside(const cv::Point_<T>& pt, const cv::Size& size) {
return pt.x>=T(0) && pt.y>=T(0) && pt.x<T(size.width) && pt.y<T(size.height);
}
template <typename T>
static inline typename std::enable_if<std::is_floating_point<T>::value,bool>::type isInside(const cv::Point_<T>& pt, const cv::Size& size) {
return pt.x>=T(0) && pt.y>=T(0) && pt.x<=T(size.width) && pt.y<=T(size.height);
}
template <typename T>
inline bool isInside(const cv::Point_<T>& pt) const {
return isInside<T>(pt, Base::size());
}
/// Is this coordinate inside the 2D matrix, and not too close to the edges?
/// @param border: the size of the border
inline bool isInsideWithBorder(const Size& pt, int border) const {
return pt.width>=border && pt.height>=border && pt.width<Base::size().width-border && pt.height<Base::size().height-border;
}
template <typename T>
inline typename std::enable_if<std::is_integral<T>::value,bool>::type isInsideWithBorder(const cv::Point_<T>& pt, int border) const {
return pt.x>=border && pt.y>=border && pt.x<Base::size().width-border && pt.y<Base::size().height-border;
}
template <typename T>
inline typename std::enable_if<std::is_floating_point<T>::value,bool>::type isInsideWithBorder(const cv::Point_<T>& pt, int border) const {
return pt.x>=T(border) && pt.y>=T(border) && pt.x<=T(Base::size().width-(border+1)) && pt.y<=T(Base::size().height-(border+1));
}
template <typename T, int border>
inline typename std::enable_if<std::is_integral<T>::value,bool>::type isInsideWithBorder(const cv::Point_<T>& pt) const {
return pt.x>=border && pt.y>=border && pt.x<Base::size().width-border && pt.y<Base::size().height-border;
}
template <typename T, int border>
inline typename std::enable_if<std::is_floating_point<T>::value,bool>::type isInsideWithBorder(const cv::Point_<T>& pt) const {
return pt.x>=T(border) && pt.y>=T(border) && pt.x<=T(Base::size().width-(border+1)) && pt.y<=T(Base::size().height-(border+1));
}
template <typename T, int border=0>
static inline void clip(TPoint2<T>& ptMin, TPoint2<T>& ptMax, const cv::Size& size) {
if (ptMin.x < T(border))
ptMin.x = T(border);
if (ptMin.y < T(border))
ptMin.y = T(border);
if (ptMax.x >= T(size.width-border))
ptMax.x = T(size.width-(border+1));
if (ptMax.y >= T(size.height-border))
ptMax.y = T(size.height-(border+1));
}
template <typename T, int border=0>
inline void clip(TPoint2<T>& ptMin, TPoint2<T>& ptMax) const {
clip<T,border>(ptMin, ptMax, Base::size());
}
/// Remove the given element from the vector
inline void remove(int idx) {
// replace the removed element by the last one and decrease the size
ASSERT(rows == 1 || cols == 1);
const int last = area()-1;
Base::operator()(idx) = Base::operator()(last);
Base::resize((size_t)last);
}
/** @author koeser
@warning very slow, generic implementation (order "n!"), better to use
matrix decomposition (see BIAS/MathAlgo/Lapack.hh) */
inline TYPE getDetSquare() const;
/** @brief computes the adjoint matrix
@author Christian Beder */
inline TDMatrix getAdjoint() const;
/** @brief compute square system matrix dest = A^T * A
@param dest holds result of Transpose * this
If you want to solve A * x = b, where A has more rows than columns,
a common technique is to solve x = (A^T * A)^-1 * A^T * b.
This function provides a fast way to compute A^T*A from A.
@author grest/koeser */
inline void getSystemMatrix(TDMatrix& dest) const;
/** @brief componentwise: this = 0.5(this + this^T) yields symmetric matrix
only allowed for square shaped matrices
@author koeser 01/2007 */
inline void makeSymmetric();
/** Return the L1 norm: |a| + |b| + |c| + ...
@author Ingo Thomsen
@date 04/11/2002
@status untested **/
inline TYPE getNormL1() const;
/** Return the L2 norm: a^2 + b^2 + c^2 + ...
@author woelk 07/2004 */
inline double getNormL2() const;
/** Kronecker-product with matrix, result in dest */
void Kronecker(const TDMatrix& B, TDMatrix& dest) const;
/** @brief swaps two rows
@author woelk 05/2008 www.vision-n.de */
void SwapRows(int i, int r);
/** @brief use the Gauss Jordan Algorithm to transform the matrix to
reduced row echelon form.
@author woelk 05/2008 www.vision-n.de */
void GaussJordan();
//! more convenient forms of row and element access operators
const TYPE& operator [](size_t i) const { return cv::Mat::at<TYPE>((int)i); }
TYPE& operator [](size_t i) { return cv::Mat::at<TYPE>((int)i); }
/// Access an element from the matrix. Bounds checking is only performed in debug mode.
inline const TYPE& operator [] (const Size& pos) const {
ASSERT(isInside(pos) && elem_stride() == sizeof(TYPE));
return ((const TYPE*)data)[pos.height*row_stride() + pos.width];
}
inline TYPE& operator [] (const Size& pos) {
ASSERT(isInside(pos) && elem_stride() == sizeof(TYPE));
return ((TYPE*)data)[pos.height*row_stride() + pos.width];
}
/// pointer to the beginning of the matrix data
inline const TYPE* getData() const { ASSERT(cv::Mat::empty() || cv::Mat::isContinuous()); return (const TYPE*)data; }
inline TYPE* getData() { ASSERT(cv::Mat::empty() || cv::Mat::isContinuous()); return (TYPE*)data; }
#ifdef _USE_EIGEN
// Access point as Eigen equivalent
inline operator EMat () const { return CEMatMap(getData(), rows, cols); }
// Access point as Eigen::Map equivalent
inline operator const CEMatMap () const { return CEMatMap(getData(), rows, cols); }
inline operator EMatMap () { return EMatMap(getData(), rows, cols); }
#endif
#ifdef _USE_BOOST
// serialize
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar & boost::serialization::base_object<Base>(*this);
}
#endif
};
/*----------------------------------------------------------------*/
typedef TDMatrix<REAL> DMatrix;
typedef TDMatrix<int8_t> DMatrix8S;
typedef TDMatrix<uint8_t> DMatrix8U;
typedef TDMatrix<int32_t> DMatrix32S;
typedef TDMatrix<uint32_t> DMatrix32U;
typedef TDMatrix<float> DMatrix32F;
typedef TDMatrix<double> DMatrix64F;
typedef CLISTDEF2(DMatrix) DMatrixArr;
typedef CLISTDEF2(cv::Mat) MatArr;
/*----------------------------------------------------------------*/
// generic vector struct
template <typename TYPE>
class TDVector : public TDMatrix<TYPE>
{
public:
typedef TYPE Type;
typedef TDMatrix<TYPE> Base;
typedef typename Base::Base BaseBase;
typedef cv::Size Size;
using Base::rows;
using Base::cols;
using Base::data;
public:
inline TDVector() {}
inline TDVector(const Base& rhs) : Base(rhs) {}
inline TDVector(const cv::Mat& rhs) : Base(rhs) {}
inline TDVector(const cv::MatExpr& rhs) : Base(rhs) {}
inline TDVector(int _rows) : Base(_rows, 1) {}
inline TDVector(int _rows, const TYPE& v) : Base(Size(_rows,1), v) {}
inline TDVector(int _rows, TYPE* _data, size_t _step=Base::AUTO_STEP) : Base(_rows, 1, _data, _step) {}
inline TDVector& operator = (const Base& rhs) { BaseBase::operator=(rhs); return *this; }
inline TDVector& operator = (const BaseBase& rhs) { BaseBase::operator=(rhs); return *this; }
inline TDVector& operator = (const cv::MatExpr& rhs) { BaseBase::operator=(rhs); return *this; }
//! equivalent to Mat::create(_rows, 1, DataType<_Tp>::type)
inline void create(int _rows) { BaseBase::create(_rows, 1); }
/** outer product, constructs a matrix.
Often written as v * v^T for col vectors
@author Daniel Grest, Oct 2002
@status tested */
TDMatrix<TYPE> getOuterProduct(const TDVector<TYPE>& v) const;
/** kronecker product
@author woelk 08/2004 */
void getKroneckerProduct(const TDVector<TYPE>& arg, TDVector<TYPE>& dst) const;
};
/*----------------------------------------------------------------*/
typedef TDVector<REAL> DVector;
typedef TDVector<int8_t> DVector8S;
typedef TDVector<uint8_t> DVector8U;
typedef TDVector<int32_t> DVector32S;
typedef TDVector<uint32_t> DVector32U;
typedef TDVector<float> DVector32F;
typedef TDVector<double> DVector64F;
typedef CLISTDEF2(DVector) DVectorArr;
/*----------------------------------------------------------------*/
// generic color type
// Here the color order means the order the data is stored on this machine.
// For example BGR means that the blue byte is stored first and the red byte last,
// which correspond to the RGB format on a little-endian machine.
#define _COLORMODE_BGR 1 // little-endian
#define _COLORMODE_RGB 2 // big-endian
#ifndef _COLORMODE
#define _COLORMODE _COLORMODE_BGR
#endif
template<typename TYPE> class ColorType
{
public:
typedef TYPE value_type;
typedef value_type alt_type;
static const value_type ONE;
static const alt_type ALTONE;
};
template<> class ColorType<uint8_t>
{
public:
typedef uint8_t value_type;
typedef float alt_type;
static const value_type ONE;
static const alt_type ALTONE;
};
template<> class ColorType<uint32_t>
{
public:
typedef uint32_t value_type;
typedef float alt_type;
static const value_type ONE;
static const alt_type ALTONE;
};
template<> class ColorType<float>
{
public:
typedef float value_type;
typedef uint8_t alt_type;
static const value_type ONE;
static const alt_type ALTONE;
};
template<> class ColorType<double>
{
public:
typedef double value_type;
typedef uint8_t alt_type;
static const value_type ONE;
static const alt_type ALTONE;
};
/*----------------------------------------------------------------*/
template <typename TYPE>
struct TPixel {
union {
struct {
#if _COLORMODE == _COLORMODE_BGR
TYPE b;
TYPE g;
TYPE r;
#endif
#if _COLORMODE == _COLORMODE_RGB
TYPE r;
TYPE g;
TYPE b;
#endif
};
struct {
TYPE c0;
TYPE c1;
TYPE c2;
};
TYPE c[3];
};
typedef typename ColorType<TYPE>::alt_type ALT;
typedef TYPE Type;
typedef TPoint3<TYPE> Pnt;
static const TPixel BLACK;
static const TPixel WHITE;
static const TPixel GRAY;
static const TPixel RED;
static const TPixel GREEN;
static const TPixel BLUE;
static const TPixel YELLOW;
static const TPixel MAGENTA;
static const TPixel CYAN;
// init
inline TPixel() {}
template <typename T> inline TPixel(const TPixel<T>& p)
#if _COLORMODE == _COLORMODE_BGR
: b(TYPE(p.b)), g(TYPE(p.g)), r(TYPE(p.r)) {}
#endif
#if _COLORMODE == _COLORMODE_RGB
: r(TYPE(p.r)), g(TYPE(p.g)), b(TYPE(p.b)) {}
#endif
inline TPixel(TYPE _r, TYPE _g, TYPE _b)
#if _COLORMODE == _COLORMODE_BGR
: b(_b), g(_g), r(_r) {}
#endif
#if _COLORMODE == _COLORMODE_RGB
: r(_r), g(_g), b(_b) {}
#endif
inline TPixel(const Pnt& col) : c0(col.x), c1(col.y), c2(col.z) {}
explicit inline TPixel(uint32_t col)
#if _COLORMODE == _COLORMODE_BGR
: b(TYPE(col&0xFF)), g(TYPE((col>>8)&0xFF)), r(TYPE((col>>16)&0xFF)) {}
#endif
#if _COLORMODE == _COLORMODE_RGB
: r(TYPE((col>>16)&0xFF)), g(TYPE((col>>8)&0xFF)), b(TYPE(col&0xFF)) {}
#endif
// set/get from default type
inline TPixel& set(TYPE _r, TYPE _g, TYPE _b) { r = _r; g = _g; b = _b; return *this; }
inline TPixel& set(const TYPE* clr) { c[0] = clr[0]; c[1] = clr[1]; c[2] = clr[2]; return *this; }
inline void get(TYPE& _r, TYPE& _g, TYPE& _b) const { _r = r; _g = g; _b = b; }
inline void get(TYPE* clr) const { clr[0] = c[0]; clr[1] = c[1]; clr[2] = c[2]; }
// set/get from alternative type
inline TPixel& set(ALT _r, ALT _g, ALT _b) { r = TYPE(_r); g = TYPE(_g); b = TYPE(_b); return *this; }
inline TPixel& set(const ALT* clr) { c[0] = TYPE(clr[0]); c[1] = TYPE(clr[1]); c[2] = TYPE(clr[2]); return *this; }
inline void get(ALT& _r, ALT& _g, ALT& _b) const { _r = ALT(r); _g = ALT(g); _b = ALT(b); }
inline void get(ALT* clr) const { clr[0] = ALT(c[0]); clr[1] = ALT(c[1]); clr[2] = ALT(c[2]); }
template<typename T> inline TPixel<typename std::enable_if<!std::is_floating_point<TYPE>::value || !std::is_same<T,uint8_t>::value,T>::type> cast() const { return TPixel<T>(T(r), T(g), T(b)); }
template<typename T> inline TPixel<typename std::enable_if<std::is_floating_point<TYPE>::value && std::is_same<T,uint8_t>::value,T>::type> cast() const {
return TPixel<uint8_t>(
(uint8_t)CLAMP(ROUND2INT(r), 0, 255),
(uint8_t)CLAMP(ROUND2INT(g), 0, 255),
(uint8_t)CLAMP(ROUND2INT(b), 0, 255)
);
}
// set/get as vector
inline const TYPE& operator[](size_t i) const { ASSERT(i<3); return c[i]; }
inline TYPE& operator[](size_t i) { ASSERT(i<3); return c[i]; }
// access as point equivalent
template<typename T> inline operator TPoint3<T>() const { return TPoint3<T>(T(c[0]), T(c[1]), T(c[2])); }
// access as vector equivalent
inline operator const Pnt& () const { return *((const Pnt*)this); }
inline operator Pnt& () { return *((Pnt*)this); }
// access as cv::Scalar equivalent
inline operator cv::Scalar () const { return cv::Scalar(c[0], c[1], c[2], TYPE(0)); }
// compare
inline bool operator==(const TPixel& col) const { return (memcmp(c, col.c, sizeof(TPixel)) == 0); }
inline bool operator!=(const TPixel& col) const { return (memcmp(c, col.c, sizeof(TPixel)) != 0); }
// operators
inline TPixel operator*(const TPixel& v) const { return TPixel(r*v.r, g*v.g, b*v.b); }
template<typename T> inline TPixel operator*(T v) const { return TPixel((TYPE)(v*r), (TYPE)(v*g), (TYPE)(v*b)); }
template<typename T> inline TPixel& operator*=(T v) { return (*this = operator*(v)); }
inline TPixel operator/(const TPixel& v) const { return TPixel(r/v.r, g/v.g, b/v.b); }
template<typename T> inline TPixel operator/(T v) const { return operator*(T(1)/v); }
template<typename T> inline TPixel& operator/=(T v) { return (*this = operator/(v)); }
inline TPixel operator+(const TPixel& v) const { return TPixel(r+v.r, g+v.g, b+v.b); }
template<typename T> inline TPixel operator+(T v) const { return TPixel((TYPE)(r+v), (TYPE)(g+v), (TYPE)(b+v)); }
template<typename T> inline TPixel& operator+=(T v) { return (*this = operator+(v)); }
inline TPixel operator-(const TPixel& v) const { return TPixel(r-v.r, g-v.g, b-v.b); }
template<typename T> inline TPixel operator-(T v) const { return TPixel((TYPE)(r-v), (TYPE)(g-v), (TYPE)(b-v)); }
template<typename T> inline TPixel& operator-=(T v) { return (*this = operator-(v)); }
inline uint32_t toDWORD() const { return RGBA((uint8_t)r, (uint8_t)g, (uint8_t)b, (uint8_t)0); }
// tools
template <typename VT>
static TPixel colorRamp(VT v, VT vmin, VT vmax);
static TPixel gray2color(ALT v);
#ifdef _USE_BOOST
// serialize
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar & c;
}
#endif
};
template <> inline TPixel<float>& TPixel<float>::set(uint8_t _r, uint8_t _g, uint8_t _b) { r = float(_r)/255; g = float(_g)/255; b = float(_b)/255; return *this; }
template <> inline void TPixel<float>::get(uint8_t& _r, uint8_t& _g, uint8_t& _b) const { _r = (uint8_t)CLAMP(ROUND2INT(r*255), 0, 255); _g = (uint8_t)CLAMP(ROUND2INT(g*255), 0, 255); _b = (uint8_t)CLAMP(ROUND2INT(b*255), 0, 255); }
template <> inline TPixel<uint8_t>& TPixel<uint8_t>::set(float _r, float _g, float _b) { r = (uint8_t)CLAMP(ROUND2INT(_r*255), 0, 255); g = (uint8_t)CLAMP(ROUND2INT(_g*255), 0, 255); b = (uint8_t)CLAMP(ROUND2INT(_b*255), 0, 255); return *this; }
template <> inline void TPixel<uint8_t>::get(float& _r, float& _g, float& _b) const { _r = float(r)/255; _g = float(g)/255; _b = float(b)/255; }
/*----------------------------------------------------------------*/
typedef TPixel<uint8_t> Pixel8U;
typedef TPixel<float> Pixel32F;
typedef TPixel<double> Pixel64F;
/*----------------------------------------------------------------*/
template <typename TYPE>
struct TColor {
union {
struct {
#if _COLORMODE == _COLORMODE_BGR
TYPE b;
TYPE g;
TYPE r;
TYPE a;
#endif
#if _COLORMODE == _COLORMODE_RGB
TYPE a;
TYPE r;
TYPE g;
TYPE b;
#endif
};
struct {
TYPE c0;
TYPE c1;
TYPE c2;
TYPE c3;
};
TYPE c[4];
};
typedef typename ColorType<TYPE>::alt_type ALT;
typedef TYPE Type;
typedef TPixel<TYPE> Pxl;
typedef TPoint3<TYPE> Pnt;
static const TColor BLACK;
static const TColor WHITE;
static const TColor GRAY;
static const TColor RED;
static const TColor GREEN;
static const TColor BLUE;
static const TColor YELLOW;
static const TColor MAGENTA;
static const TColor CYAN;
// init
inline TColor() {}
template <typename T> inline TColor(const TColor<T>& p)
: r(TYPE(p.r)), g(TYPE(p.g)), b(TYPE(p.b)), a(TYPE(p.a)) {}
inline TColor(TYPE _r, TYPE _g, TYPE _b, TYPE _a=ColorType<TYPE>::ONE)
: r(_r), g(_g), b(_b), a(_a) {}
inline TColor(const Pxl& col, TYPE _a=ColorType<TYPE>::ONE)
: r(col.r), g(col.g), b(col.b), a(_a) {}
#if _COLORMODE == _COLORMODE_BGR
inline TColor(const Pnt& col, TYPE _a=ColorType<TYPE>::ONE)
: b(col.x), g(col.y), r(col.z), a(_a) {}
#endif
#if _COLORMODE == _COLORMODE_RGB
inline TColor(const Pnt& col, TYPE _a=ColorType<TYPE>::ONE)
: r(col.x), g(col.y), b(col.z), a(_a) {}
#endif
explicit inline TColor(uint32_t col)
: r(TYPE((col>>16)&0xFF)), g(TYPE((col>>8)&0xFF)), b(TYPE(col&0xFF)), a(TYPE((col>>24)&0xFF)) {}
// set/get from default type
inline TColor& set(TYPE _r, TYPE _g, TYPE _b, TYPE _a=ColorType<TYPE>::ONE) { r = _r; g = _g; b = _b; a = _a; return *this; }
inline TColor& set(const TYPE* clr) { c[0] = clr[0]; c[1] = clr[1]; c[2] = clr[2]; c[3] = clr[3]; return *this; }
inline void get(TYPE& _r, TYPE& _g, TYPE& _b, TYPE& _a) const { _r = r; _g = g; _b = b; _a = a; }
inline void get(TYPE* clr) const { clr[0] = c[0]; clr[1] = c[1]; clr[2] = c[2]; clr[3] = c[3]; }
// set/get from alternative type
inline TColor& set(ALT _r, ALT _g, ALT _b, ALT _a=ColorType<TYPE>::ALTONE) { r = TYPE(_r); g = TYPE(_g); b = TYPE(_b); a = TYPE(_a); return *this; }
inline TColor& set(const ALT* clr) { c[0] = TYPE(clr[0]); c[1] = TYPE(clr[1]); c[2] = TYPE(clr[2]); c[3] = TYPE(clr[3]); return *this; }
inline void get(ALT& _r, ALT& _g, ALT& _b, ALT& _a) const { _r = ALT(r); _g = ALT(g); _b = ALT(b); _a = ALT(a); }
inline void get(ALT* clr) const { clr[0] = ALT(c[0]); clr[1] = ALT(c[1]); clr[2] = ALT(c[2]); clr[3] = ALT(c[3]); }
template<typename T> inline TColor<typename std::enable_if<!std::is_floating_point<TYPE>::value || !std::is_same<T,uint8_t>::value,T>::type> cast() const { return TColor<T>(T(r), T(g), T(b), T(a)); }
template<typename T> inline TColor<typename std::enable_if<std::is_floating_point<TYPE>::value && std::is_same<T,uint8_t>::value,T>::type> cast() const {
return TColor<uint8_t>(
(uint8_t)CLAMP(ROUND2INT(r), 0, 255),
(uint8_t)CLAMP(ROUND2INT(g), 0, 255),
(uint8_t)CLAMP(ROUND2INT(b), 0, 255),
(uint8_t)CLAMP(ROUND2INT(a), 0, 255)
);
}
// set/get as vector
inline const TYPE& operator[](size_t i) const { ASSERT(i<4); return c[i]; }
inline TYPE& operator[](size_t i) { ASSERT(i<4); return c[i]; }
// access as pixel equivalent
inline operator const Pxl& () const { return *((const Pxl*)this); }
inline operator Pxl& () { return *((Pxl*)this); }
// access as point equivalent
inline operator const Pnt& () const { return *((const Pnt*)this); }
inline operator Pnt& () { return *((Pnt*)this); }
// access as cv::Scalar equivalent
inline operator cv::Scalar () const { return cv::Scalar(c[0], c[1], c[2], c[3]); }
// compare
inline bool operator==(const TColor& col) const { return (memcmp(c, col.c, sizeof(TColor)) == 0); }
inline bool operator!=(const TColor& col) const { return (memcmp(c, col.c, sizeof(TColor)) != 0); }
// operators
inline TColor operator*(const TColor& v) const { return TColor(r*v.r, g*v.g, b*v.b, a*v.a); }
template<typename T> inline TColor operator*(T v) const { return TColor((TYPE)(v*r), (TYPE)(v*g), (TYPE)(v*b), (TYPE)(v*a)); }
template<typename T> inline TColor& operator*=(T v) { return (*this = operator*(v)); }
inline TColor operator/(const TColor& v) const { return TColor(r/v.r, g/v.g, b/v.b, a/v.a); }
template<typename T> inline TColor operator/(T v) const { return operator*(T(1)/v); }
template<typename T> inline TColor& operator/=(T v) { return (*this = operator/(v)); }
inline TColor operator+(const TColor& v) const { return TColor(r+v.r, g+v.g, b+v.b, a+v.a); }
template<typename T> inline TColor operator+(T v) const { return TColor((TYPE)(r+v), (TYPE)(g+v), (TYPE)(b+v), (TYPE)(a+v)); }
template<typename T> inline TColor& operator+=(T v) { return (*this = operator+(v)); }
inline TColor operator-(const TColor& v) const { return TColor(r-v.r, g-v.g, b-v.b, a-v.a); }
template<typename T> inline TColor operator-(T v) const { return TColor((TYPE)(r-v), (TYPE)(g-v), (TYPE)(b-v), (TYPE)(a-v)); }
template<typename T> inline TColor& operator-=(T v) { return (*this = operator-(v)); }
inline uint32_t toDWORD() const { return RGBA((uint8_t)r, (uint8_t)g, (uint8_t)b, (uint8_t)a); }
#ifdef _USE_BOOST
// serialize
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar & c;
}
#endif
};
template <> inline TColor<float>& TColor<float>::set(uint8_t _r, uint8_t _g, uint8_t _b, uint8_t _a) { r = float(_r)/255; g = float(_g)/255; b = float(_b)/255; a = float(_a)/255; return *this; }
template <> inline void TColor<float>::get(uint8_t& _r, uint8_t& _g, uint8_t& _b, uint8_t& _a) const { _r = uint8_t(r*255); _g = uint8_t(g*255); _b = uint8_t(b*255); _a = uint8_t(a*255); }
template <> inline TColor<uint8_t>& TColor<uint8_t>::set(float _r, float _g, float _b, float _a) { r = uint8_t(_r*255); g = uint8_t(_g*255); b = uint8_t(_b*255); a = uint8_t(_a*255); return *this; }
template <> inline void TColor<uint8_t>::get(float& _r, float& _g, float& _b, float& _a) const { _r = float(r)/255; _g = float(g)/255; _b = float(b)/255; _a = float(a)/255; }
template <> inline bool TColor<uint8_t>::operator==(const TColor& col) const { return (*((const uint32_t*)c) == *((const uint32_t*)col.c)); }
template <> inline bool TColor<uint8_t>::operator!=(const TColor& col) const { return (*((const uint32_t*)c) != *((const uint32_t*)col.c)); }
template <> inline uint32_t TColor<uint8_t>::toDWORD() const { return *((const uint32_t*)c); }
/*----------------------------------------------------------------*/
typedef TColor<uint8_t> Color8U;
typedef TColor<float> Color32F;
typedef TColor<double> Color64F;
/*----------------------------------------------------------------*/
// structure containing the image pixels
typedef Point2i ImageRef;
template <typename TYPE>
class TImage : public TDMatrix<TYPE>
{
public:
typedef TYPE Type;
typedef TDMatrix<TYPE> Base;
typedef cv::Size Size;
typedef typename Base::Base BaseBase;
using Base::rows;
using Base::cols;
using Base::data;
public:
inline TImage() {}
inline TImage(const Base& rhs) : Base(rhs) {}
inline TImage(const cv::Mat& rhs) : Base(rhs) {}
inline TImage(const cv::MatExpr& rhs) : Base(rhs) {}
inline TImage(int _rows, int _cols) : Base(_rows, _cols) {}
inline TImage(const Size& sz) : Base(sz) {}
inline TImage(const Size& sz, const TYPE& v) : Base(sz, v) {}
inline TImage(const Size& sz, TYPE* _data, size_t _step=Base::AUTO_STEP) : Base(sz.height, sz.width, _data, _step) {}
#ifdef _SUPPORT_CPP11
inline TImage(cv::Mat&& rhs) : Base(std::forward<cv::Mat>(rhs)) {}
inline TImage& operator = (cv::Mat&& rhs) { BaseBase::operator=(std::forward<cv::Mat>(rhs)); return *this; }
#endif
inline TImage& operator = (const Base& rhs) { BaseBase::operator=(rhs); return *this; }
inline TImage& operator = (const BaseBase& rhs) { BaseBase::operator=(rhs); return *this; }
inline TImage& operator = (const cv::MatExpr& rhs) { BaseBase::operator=(rhs); return *this; }
/// What is the image size?
inline int width() const { return cols; }
inline int height() const { return rows; }
/// What is the pixel stride of the image?
inline size_t pixel_stride() const { return Base::elem_stride(); }
inline const TYPE& getPixel(int y, int x) const;
template <typename T>
TYPE sample(const TPoint2<T>& pt) const;
template <typename T>
TYPE sampleSafe(const TPoint2<T>& pt) const;
template <typename T, typename TV, typename Functor>
bool sample(TV& v, const TPoint2<T>& pt, const Functor& functor) const;
template <typename T, typename TV, typename Functor>
bool sampleSafe(TV& v, const TPoint2<T>& pt, const Functor& functor) const;
template <typename T, typename Functor>
TYPE sample(const TPoint2<T>& pt, const Functor& functor, const TYPE& dv) const;
template <typename T, typename Functor>
TYPE sampleSafe(const TPoint2<T>& pt, const Functor& functor, const TYPE& dv) const;
template <typename SAMPLER, typename INTERTYPE>
INTERTYPE sample(const SAMPLER& sampler, const TPoint2<typename SAMPLER::Type>& pt) const;
template <typename T>
void toGray(TImage<T>& out, int code, bool bNormalize=false, bool bSRGB=false) const;
static cv::Size computeResize(const cv::Size& size, REAL scale);
static cv::Size computeResize(const cv::Size& size, REAL scale, unsigned resizes);
unsigned computeMaxResolution(unsigned& level, unsigned minImageSize=320, unsigned maxImageSize=INT_MAX) const;
static unsigned computeMaxResolution(unsigned width, unsigned height, unsigned& level, unsigned minImageSize=320, unsigned maxImageSize=INT_MAX);
template <typename T, typename PARSER>
static void RasterizeTriangle(const TPoint2<T>& v1, const TPoint2<T>& v2, const TPoint2<T>& v3, PARSER& parser);
template <typename T, typename PARSER, bool CULL=true>
static void RasterizeTriangleBary(const TPoint2<T>& v1, const TPoint2<T>& v2, const TPoint2<T>& v3, PARSER& parser);
template <typename T, typename PARSER>
static void RasterizeTriangleDepth(TPoint3<T> p1, TPoint3<T> p2, TPoint3<T> p3, PARSER& parser);
template <typename T, typename PARSER>
static void DrawLine(const TPoint2<T>& p1, const TPoint2<T>& p2, PARSER& parser);
typedef void (STCALL *FncDrawPointAntialias) (const ImageRef&, const ImageRef&, float, float, void*);
static bool DrawLineAntialias(Point2f x1, Point2f x2, FncDrawPointAntialias fncDrawPoint, void* pData=NULL);
static bool DrawLineAntialias(const ImageRef& x1, const ImageRef& x2, FncDrawPointAntialias fncDrawPoint, void* pData=NULL);
template <int HalfSize>
void DilateMean(TImage<TYPE>& out, const TYPE& invalid) const;
bool Load(const String&);
bool Save(const String&) const;
#ifndef _RELEASE
void Show(const String& winname, int delay=0, bool bDestroy=true) const;
#endif
#ifdef _USE_BOOST
// serialize
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar & boost::serialization::base_object<Base>(*this);
}
#endif
};
/*----------------------------------------------------------------*/
typedef TImage<uint8_t> Image8U;
typedef TImage<uint16_t> Image16U;
typedef TImage<hfloat> Image16F;
typedef TImage<float> Image32F;
typedef TImage<double> Image64F;
typedef TImage<Pixel8U> Image8U3;
typedef TImage<Color8U> Image8U4;
typedef TImage<Pixel32F> Image32F3;
typedef TImage<Color32F> Image32F4;
/*----------------------------------------------------------------*/
template <typename TYPE>
class TBitMatrix
{
public:
typedef TYPE Type;
typedef cv::Size Size;
struct Index {
size_t idx;
Type flag;
inline Index() {}
inline Index(size_t _idx, Type _flag) : idx(_idx), flag(_flag) {}
};
enum { numBitsPerCell = sizeof(Type)*8 };
enum { numBitsShift = LOG2I<TBitMatrix::numBitsPerCell>() };
public:
inline TBitMatrix() : data(NULL) {}
inline TBitMatrix(int _rows, int _cols=1) : rows(_rows), cols(_cols), data(rows && cols ? new Type[computeLength(rows, cols)] : NULL) {}
inline TBitMatrix(int _rows, int _cols, uint8_t v) : rows(_rows), cols(_cols), data(rows && cols ? new Type[computeLength(rows, cols)] : NULL) { if (!empty()) memset(v); }
inline TBitMatrix(const Size& sz) : rows(sz.height), cols(sz.width), data(rows && cols ? new Type[computeLength(sz)] : NULL) {}
inline TBitMatrix(const Size& sz, uint8_t v) : rows(sz.height), cols(sz.width), data(rows && cols ? new Type[computeLength(sz)] : NULL) { if (!empty()) memset(v); }
inline ~TBitMatrix() { delete[] data; }
inline void create(int _rows, int _cols=1) {
if (!empty() && rows == _rows && cols == _cols)
return;
rows = _rows; cols = _cols;
if (rows <=0 || cols <= 0) {
release();
return;
}
delete[] data;
data = new Type[length()];
}
inline void create(const Size& sz) { create(sz.height, sz.width); }
inline void release() { delete[] data; data = NULL; }
inline void memset(uint8_t v) { ASSERT(!empty()); ::memset(data, v, sizeof(Type)*length()); }
inline void swap(TBitMatrix& m) {
union {int i; Type* d;} tmp;
tmp.i = rows; rows = m.rows; m.rows = tmp.i;
tmp.i = cols; cols = m.cols; m.cols = tmp.i;
tmp.d = data; data = m.data; m.data = tmp.d;
}
inline void And(const TBitMatrix& m) {
ASSERT(rows == m.rows && cols == m.cols);
int i = length();
while (i-- > 0)
data[i] &= m.data[i];
}
inline void Or(const TBitMatrix& m) {
ASSERT(rows == m.rows && cols == m.cols);
int i = length();
while (i-- > 0)
data[i] |= m.data[i];
}
inline void XOr(const TBitMatrix& m) {
ASSERT(rows == m.rows && cols == m.cols);
int i = length();
while (i-- > 0)
data[i] ^= m.data[i];
}
inline bool empty() const { return data == NULL; }
inline Size size() const { return Size(cols, rows); }
inline int area() const { return cols*rows; }
inline int length() const { return computeLength(rows, cols); }
inline bool operator() (int i) const { return isSet(i); }
inline bool operator() (int i, int j) const { return isSet(i,j); }
inline bool operator() (const ImageRef& ir) const { return isSet(ir); }
inline bool isSet(int i) const { ASSERT(!empty() && i<area()); const Index idx(computeIndex(i)); return (data[idx.idx] & idx.flag) != 0; }
inline bool isSet(int i, int j) const { ASSERT(!empty() && i<rows && j<cols); const Index idx(computeIndex(i, j, cols)); return (data[idx.idx] & idx.flag) != 0; }
inline bool isSet(const ImageRef& ir) const { ASSERT(!empty() && ir.y<rows && ir.x<cols); const Index idx(computeIndex(ir, cols)); return (data[idx.idx] & idx.flag) != 0; }
inline void set(int i, bool v) { if (v) set(i); else unset(i); }
inline void set(int i, int j, bool v) { if (v) set(i, j); else unset(i, j); }
inline void set(const ImageRef& ir, bool v) { if (v) set(ir); else unset(ir); }
inline void set(int i) { ASSERT(!empty() && i<area()); const Index idx(computeIndex(i)); data[idx.idx] |= idx.flag; }
inline void set(int i, int j) { ASSERT(!empty() && i<rows && j<cols); const Index idx(computeIndex(i, j, cols)); data[idx.idx] |= idx.flag; }
inline void set(const ImageRef& ir) { ASSERT(!empty() && ir.y<rows && ir.x<cols); const Index idx(computeIndex(ir, cols)); data[idx.idx] |= idx.flag; }
inline void unset(int i) { ASSERT(!empty() && i<area()); const Index idx(computeIndex(i)); data[idx.idx] &= ~idx.flag; }
inline void unset(int i, int j) { ASSERT(!empty() && i<rows && j<cols); const Index idx(computeIndex(i, j, cols)); data[idx.idx] &= ~idx.flag; }
inline void unset(const ImageRef& ir) { ASSERT(!empty() && ir.y<rows && ir.x<cols); const Index idx(computeIndex(ir, cols)); data[idx.idx] &= ~idx.flag; }
inline void flip(int i) { ASSERT(!empty() && i<area()); const Index idx(computeIndex(i)); data[idx.idx] ^= idx.flag; }
inline void flip(int i, int j) { ASSERT(!empty() && i<rows && j<cols); const Index idx(computeIndex(i, j, cols)); data[idx.idx] ^= idx.flag; }
inline void flip(const ImageRef& ir) { ASSERT(!empty() && ir.y<rows && ir.x<cols); const Index idx(computeIndex(ir, cols)); data[idx.idx] ^= idx.flag; }
/// Is this coordinate inside the 2D matrix?
inline bool isInside(const Size& pt) const {
return pt.width>=0 && pt.height>=0 && pt.width<size().width && pt.height<size().height;
}
template <typename T>
inline bool isInside(const cv::Point_<T>& pt) const {
return int(pt.x)>=0 && int(pt.y)>=0 && int(pt.x)<size().width && int(pt.y)<size().height;
}
/// Is this coordinate inside the 2D matrix, and not too close to the edges?
/// @param border: the size of the border
inline bool isInsideWithBorder(const Size& pt, int border) const {
return pt.width>=border && pt.height>=border && pt.width<size().width-border && pt.height<size().height-border;
}
template <typename T>
inline bool isInsideWithBorder(const cv::Point_<T>& pt, int border) const {
return int(pt.x)>=border && int(pt.y)>=border && int(pt.x)<size().width-border && int(pt.y)<size().height-border;
}
template <typename T, int border>
inline bool isInsideWithBorder(const cv::Point_<T>& pt) const {
return int(pt.x)>=border && int(pt.y)>=border && int(pt.x)<size().width-border && int(pt.y)<size().height-border;
}
static inline int computeLength(int size) { return (size+numBitsPerCell-1)>>numBitsShift; }
static inline int computeLength(int _rows, int _cols) { return computeLength(_rows*_cols); }
static inline int computeLength(const Size& sz) { return computeLength(sz.area()); }
static inline Index computeIndex(int i) { return Index(i>>numBitsShift, Type(1)<<(i&(numBitsPerCell-1))); }
static inline Index computeIndex(int i, int j, int stride) { return computeIndex(i*stride+j); }
static inline Index computeIndex(const ImageRef& ir, int stride) { return computeIndex(ir.y*stride+ir.x); }
//! the number of rows and columns
int rows, cols;
//! pointer to the data
Type* data;
#ifdef _USE_BOOST
protected:
// implement BOOST serialization
friend class boost::serialization::access;
template<class Archive>
void save(Archive& ar, const unsigned int /*version*/) const {
if (empty()) {
const int sz(0);
ar & sz;
return;
}
ar & cols;
ar & rows;
ar & boost::serialization::make_array(data, length());
}
template<class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
release();
ar & cols;
if (cols == 0)
return;
ar & rows;
create(rows, cols);
ar & boost::serialization::make_array(data, length());
}
BOOST_SERIALIZATION_SPLIT_MEMBER()
#endif
};
/*----------------------------------------------------------------*/
typedef TBitMatrix<size_t> BitMatrix;
/*----------------------------------------------------------------*/
// weighted accumulator class that operates on arbitrary types
template <typename TYPE, typename ACCUMTYPE=TYPE, typename WEIGHTTYPE=float>
struct TAccumulator {
typedef TYPE Type;
typedef ACCUMTYPE AccumType;
typedef WEIGHTTYPE WeightType;
AccumType value;
WeightType weight;
inline TAccumulator() : value(INITTO(static_cast<Type*>(NULL), 0)), weight(0) {}
inline TAccumulator(const Type& v, const WeightType& w) : value(v), weight(w) {}
inline bool IsEmpty() const { return weight <= 0; }
// adds the given weighted value to the internal value
inline void Add(const Type& v, const WeightType& w) {
value += v*w;
weight += w;
}
inline TAccumulator& operator +=(const TAccumulator& accum) {
value += accum.value;
weight += accum.weight;
return *this;
}
inline TAccumulator operator +(const TAccumulator& accum) const {
return TAccumulator(
value + accum.value,
weight + accum.weight
);
}
// subtracts the given weighted value to the internal value
inline void Sub(const Type& v, const WeightType& w) {
value -= v*w;
weight -= w;
}
inline TAccumulator& operator -=(const TAccumulator& accum) {
value -= accum.value;
weight -= accum.weight;
return *this;
}
inline TAccumulator operator -(const TAccumulator& accum) const {
return TAccumulator(
value - accum.value,
weight - accum.weight
);
}
// returns the normalized version of the internal value
inline AccumType NormalizedFull() const {
return value / weight;
}
inline Type Normalized() const {
return Type(NormalizedFull());
}
#ifdef _USE_BOOST
// implement BOOST serialization
template<class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar & value;
ar & weight;
}
#endif
};
/*----------------------------------------------------------------*/
// structure used for sorting some indices by their score (decreasing by default)
template <typename IndexType, typename ScoreType>
struct TIndexScore {
IndexType idx;
ScoreType score;
inline TIndexScore() {}
inline TIndexScore(IndexType _idx, ScoreType _score) : idx(_idx), score(_score) {}
// compare by index (increasing)
inline bool operator<(IndexType i) const { return (idx < i); }
inline bool operator==(IndexType i) const { return (idx == i); }
// compare by score (decreasing)
inline bool operator<(const TIndexScore& r) const { return (score > r.score); }
inline bool operator==(const TIndexScore& r) const { return (score == r.score); }
static bool STCALL CompareByIndex(const TIndexScore& l, const TIndexScore& r) { return (l.idx < r.idx); }
static bool STCALL CompareByScore(const TIndexScore& l, const TIndexScore& r) { return (r.score < l.score); }
#ifdef _USE_BOOST
// implement BOOST serialization
template<class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar & idx;
ar & score;
}
#endif
};
/*----------------------------------------------------------------*/
typedef TIndexScore<uint32_t, float> IndexScore;
typedef CLISTDEF0(IndexScore) IndexScoreArr;
/*----------------------------------------------------------------*/
// structure describing a pair of indices as one long index
struct PairIdx {
typedef uint64_t PairIndex;
typedef uint32_t Index;
union {
PairIndex idx;
Index indices[2];
struct {
#if __BYTE_ORDER == __LITTLE_ENDIAN
Index j;
Index i;
#else
Index i;
Index j;
#endif
};
};
inline PairIdx() {}
inline PairIdx(PairIndex _idx) : idx(_idx) {}
inline PairIdx(Index _i, Index _j)
#if __BYTE_ORDER == __LITTLE_ENDIAN
: j(_j), i(_i) {}
#else
: i(_i), j(_j) {}
#endif
// get index
inline operator PairIndex () const { return idx; }
inline Index operator[](unsigned n) const {
ASSERT(n < 2);
#if __BYTE_ORDER == __LITTLE_ENDIAN
return indices[(n+1)%2];
#else
return indices[n];
#endif
}
inline Index& operator[](unsigned n) {
ASSERT(n < 2);
#if __BYTE_ORDER == __LITTLE_ENDIAN
return indices[(n+1)%2];
#else
return indices[n];
#endif
}
// compare by index (increasing)
inline bool operator<(const PairIdx& r) const { return (idx < r.idx); }
inline bool operator==(const PairIdx& r) const { return (idx == r.idx); }
};
/*----------------------------------------------------------------*/
typedef CLISTDEF0(PairIdx) PairIdxArr;
inline PairIdx MakePairIdx(uint32_t idxImageA, uint32_t idxImageB) {
return (idxImageA<idxImageB ? PairIdx(idxImageA, idxImageB) : PairIdx(idxImageB, idxImageA));
}
/*----------------------------------------------------------------*/
struct cuint32_t {
typedef uint32_t Type;
union {
uint32_t idx;
uint8_t i;
};
inline cuint32_t() {}
inline cuint32_t(uint32_t _idx) : idx(_idx) {}
inline operator uint32_t () const { return idx; }
inline operator uint32_t& () { return idx; }
};
/*----------------------------------------------------------------*/
// tools
String cvMat2String(const cv::Mat&, LPCSTR format="% 10.4f ");
template<typename TYPE, int m, int n> inline String cvMat2String(const TMatrix<TYPE,m,n>& mat, LPCSTR format="% 10.4f ") { return cvMat2String(cv::Mat(mat), format); }
template<typename TYPE> inline String cvMat2String(const TPoint3<TYPE>& pt, LPCSTR format="% 10.4f ") { return cvMat2String(cv::Mat(pt), format); }
/*----------------------------------------------------------------*/
} // namespace SEACAVE
#ifdef _USE_EIGEN
// Implement SO3 and SO2 lie groups
// as in TooN library: https://github.com/edrosten/TooN
// Copyright (C) 2005,2009 Tom Drummond (twd20@cam.ac.uk)
namespace Eigen {
/// Class to represent a three-dimensional rotation matrix. Three-dimensional rotation
/// matrices are members of the Special Orthogonal Lie group SO3. This group can be parameterized
/// three numbers (a vector in the space of the Lie Algebra). In this class, the three parameters are the
/// finite rotation vector, i.e. a three-dimensional vector whose direction is the axis of rotation
/// and whose length is the angle of rotation in radians. Exponentiating this vector gives the matrix,
/// and the logarithm of the matrix gives this vector.
template <typename Precision>
class SO3
{
public:
template <typename P>
friend std::istream& operator>>(std::istream& is, SO3<P>& rhs);
typedef Matrix<Precision,3,3,Eigen::RowMajor> Mat3;
typedef Matrix<Precision,3,1> Vec3;
/// Default constructor. Initializes the matrix to the identity (no rotation)
inline SO3() : mat(Mat3::Identity()) {}
/// Construct from a rotation matrix.
inline SO3(const Mat3& rhs) : mat(rhs) {}
/// Construct from the axis of rotation (and angle given by the magnitude).
inline SO3(const Vec3& v) : mat(exp(v)) {}
/// creates an SO3 as a rotation that takes Vector a into the direction of Vector b
/// with the rotation axis along a ^ b. If |a ^ b| == 0, it creates the identity rotation.
/// An assertion will fail if Vector a and Vector b are in exactly opposite directions.
/// @param a source Vector
/// @param b target Vector
SO3(const Vec3& a, const Vec3& b) {
ASSERT(a.size() == 3);
ASSERT(b.size() == 3);
Vec3 n(a.cross(b));
const Precision nrmSq(n.squaredNorm());
if (nrmSq == Precision(0)) {
// check that the vectors are in the same direction if cross product is 0; if not,
// this means that the rotation is 180 degrees, which leads to an ambiguity in the rotation axis
ASSERT(a.dot(b) >= Precision(0));
mat = Mat3::Identity();
return;
}
n *= Precision(1)/sqrt(nrmSq);
Mat3 R1;
R1.col(0) = a.normalized();
R1.col(1) = n;
R1.col(2) = R1.col(0).cross(n);
mat.col(0) = b.normalized();
mat.col(1) = n;
mat.col(2) = mat.col(0).cross(n);
mat = mat * R1.transpose();
}
/// Assignment operator from a general matrix. This also calls coerce()
/// to make sure that the matrix is a valid rotation matrix.
inline SO3& operator=(const Mat3& rhs) {
mat = rhs;
coerce();
return *this;
}
/// Modifies the matrix to make sure it is a valid rotation matrix.
void coerce() {
mat.row(0).normalize();
const Precision d01(mat.row(0).dot(mat.row(1)));
mat.row(1) -= mat.row(0) * d01;
mat.row(1).normalize();
const Precision d02(mat.row(0).dot(mat.row(2)));
mat.row(2) -= mat.row(0) * d02;
const Precision d12(mat.row(1).dot(mat.row(2)));
mat.row(2) -= mat.row(1) * d12;
mat.row(2).normalize();
// check for positive determinant <=> right handed coordinate system of row vectors
ASSERT(mat.row(0).cross(mat.row(1)).dot(mat.row(2)) > 0);
}
/// Exponentiate a vector in the Lie algebra to generate a new SO3.
/// See the Detailed Description for details of this vector.
inline Mat3 exp(const Vec3& vect) const;
/// Take the logarithm of the matrix, generating the corresponding vector in the Lie Algebra.
/// See the Detailed Description for details of this vector.
inline Vec3 ln() const;
/// Right-multiply by another rotation matrix
template <typename P>
inline SO3& operator *=(const SO3<P>& rhs) {
*this = *this * rhs;
return *this;
}
/// Right-multiply by another rotation matrix
inline SO3 operator *(const SO3& rhs) const { return SO3(*this, rhs); }
/// Returns the SO3 as a Matrix<3>
inline const Mat3& get_matrix() const { return mat; }
/// Returns the i-th generator. The generators of a Lie group are the basis
/// for the space of the Lie algebra. For %SO3, the generators are three
/// \f$3\times3\f$ matrices representing the three possible (linearized)
/// rotations.
inline static Mat3 generator(int i) {
Mat3 result(Mat3::Zero());
result((i+1)%3,(i+2)%3) = Precision(-1);
result((i+2)%3,(i+1)%3) = Precision( 1);
return result;
}
/// Returns the i-th generator times pos
inline static Vec3 generator_field(int i, const Vec3& pos) {
Vec3 result;
result(i) = Precision(0);
result((i+1)%3) = -pos((i+2)%3);
result((i+2)%3) = pos((i+1)%3);
return result;
}
template <typename PA, typename PB>
inline SO3(const SO3<PA>& a, const SO3<PB>& b) : mat(a.get_matrix()*b.get_matrix()) {}
protected:
Mat3 mat;
#ifdef _USE_BOOST
// implement BOOST serialization
friend class boost::serialization::access;
template<class Archive>
void save(Archive& ar, const unsigned int /*version*/) const
{
Vec3 comp(ln());
ar & comp;
}
template<class Archive>
void load(Archive& ar, const unsigned int /*version*/)
{
Vec3 comp;
ar & comp;
mat = exp(comp);
}
BOOST_SERIALIZATION_SPLIT_MEMBER()
#endif
};
/*----------------------------------------------------------------*/
/// Class to represent a two-dimensional rotation matrix. Two-dimensional rotation
/// matrices are members of the Special Orthogonal Lie group SO2. This group can be parameterized
/// with one number (the rotation angle).
template<typename Precision>
class SO2
{
public:
template <typename P>
friend std::istream& operator>>(std::istream&, SO2<P>&);
typedef Matrix<Precision,2,2,Eigen::RowMajor> Mat2;
/// Default constructor. Initializes the matrix to the identity (no rotation)
inline SO2() : mat(Mat2::Identity()) {}
/// Construct from a rotation matrix.
inline SO2(const Mat2& rhs) : mat(rhs) {}
/// Construct from an angle.
inline SO2(const Precision l) : mat(exp(l)) {}
/// Assignment operator from a general matrix. This also calls coerce()
/// to make sure that the matrix is a valid rotation matrix.
inline SO2& operator=(const Mat2& rhs) {
mat = rhs;
coerce();
return *this;
}
/// Modifies the matrix to make sure it is a valid rotation matrix.
inline void coerce() {
mat.row(0).normalize();
mat.row(1) = (mat.row(1) - mat.row(0) * (mat.row(0).dot(mat.row(1)))).normalized();
}
/// Exponentiate an angle in the Lie algebra to generate a new SO2.
inline Mat2 exp(const Precision& d) const;
/// extracts the rotation angle from the SO2
inline Precision ln() const;
/// Self right-multiply by another rotation matrix
inline SO2& operator *=(const SO2& rhs) {
mat = mat*rhs.get_matrix();
return *this;
}
/// Right-multiply by another rotation matrix
inline SO2 operator *(const SO2& rhs) const { return SO2(*this, rhs); }
/// Returns the SO2 as a Matrix<2>
inline const Mat2& get_matrix() const { return mat; }
/// returns generator matrix
inline static Mat2 generator() {
Mat2 result;
result(0,0) = Precision(0); result(0,1) = Precision(-1);
result(1,0) = Precision(1); result(1,1) = Precision(0);
return result;
}
protected:
Mat2 mat;
#ifdef _USE_BOOST
// implement BOOST serialization
friend class boost::serialization::access;
template<class Archive>
void save(Archive& ar, const unsigned int /*version*/) const
{
Precision comp(ln());
ar & comp;
}
template<class Archive>
void load(Archive& ar, const unsigned int /*version*/)
{
Precision comp;
ar & comp;
mat = exp(comp);
}
BOOST_SERIALIZATION_SPLIT_MEMBER()
#endif
};
/*----------------------------------------------------------------*/
} // namespace Eigen
#endif // _USE_EIGEN
#include "../Math/LMFit/lmmin.h"
#include "Types.inl"
#include "Util.inl"
#include "Rotation.h"
#include "Sphere.h"
#include "AABB.h"
#include "OBB.h"
#include "Plane.h"
#include "Ray.h"
#include "Line.h"
#include "Octree.h"
#include "UtilCUDA.h"
#endif // __SEACAVE_TYPES_H__
Reference in new issue View Git Blame Copy Permalink