text:
#include "vrmusbcamcpp.h"

// c++ headers

#include <exception>
#include <memory>

// boost headers

#include <boost/format.hpp>
#include <boost/foreach.hpp>

// system-dependent includes

#ifdef WIN32
#include <windows.h>
#endif//WIN32

// API private functions (only consumed by this wrapper)

extern "C" VRmRetVal VRmUsbCamPrivateLockDevice(VRmUsbCamDevice f_device);
extern "C" VRmRetVal VRmUsbCamPrivateUnlockDevice(VRmUsbCamDevice f_device);

namespace VRmUsbCamCPP {

	// return-code to exception wrapper -----------------------------------------------------------------------

#define VRMERR(text) \
	{ throw Exception(text, VRM_ERROR_CODE_GENERIC_ERROR); }

#define VRMERR_WITHCODE(text, code) \
	{ throw Exception(text, code); }

	static std::string makeErrorDescription() {
		return std::string(VRmUsbCamGetLastError());
	}

	static ErrorNumber makeErrorNumber() {
		return static_cast<ErrorNumber>(VRmUsbCamGetLastErrorCode());
	};

#define VRMCALL(function) \
	{ \
	if (VRM_SUCCESS!=VRmUsbCam##function) \
	VRMERR_WITHCODE(makeErrorDescription(), makeErrorNumber());	\
	}

#define VRMDEVCALL(function)    /* VRMCALL() for device functions, checks VRmDeviceCallback exceptions. */ \
	{ \
	if (VRM_SUCCESS!=VRmUsbCam##function) \
	VRMERR_WITHCODE(makeErrorDescription(), makeErrorNumber());	\
	checkCallbackExceptions(); \
	}

	void Device::checkCallbackExceptions() const
	{
		std::string* lp_callback_exc= mp_last_callback_exception.get();
		if (lp_callback_exc)
		{
			std::string l_callback_exc= *lp_callback_exc;
			mp_last_callback_exception.reset();
			VRMERR(l_callback_exc);
		}
	}

#define COMCALL(function) \
	{ HRESULT res=(function); \
	if FAILED(res) \
	VRMERR_WITHCODE("function call \"" #function "\" failed", res); \
	}

#define VRMCHECK(condition) \
	{ if (!(condition)) \
	VRMERR("condition \"" #condition "\" is not met"); \
	}

#define null 0		// definitions for VRMCHECK
#ifdef WIN32
#define not  !		//
#endif
#define is_not !=	//

	// Helper functions / classes ---------------------------------------------------------------------

	/// helper class from auto lock/unlock-ing the VRmUsbCam global lock or some device lock
	template<typename t_handle, VRmRetVal(*t_lockfunc)(t_handle), VRmRetVal(*t_unlockfunc)(t_handle)>
	class VRmLockProxy
	{
	public:
		VRmLockProxy(t_handle fp_obj = 0) : mp_obj(fp_obj) {}
		void lock() { t_lockfunc(mp_obj); }
		void unlock() { t_unlockfunc(mp_obj); }
	private:
		t_handle mp_obj;
	};
	inline VRmRetVal globalLock(int) { return VRmUsbCamPrivateLock(); };
	inline VRmRetVal globalUnlock(int) { return VRmUsbCamPrivateUnlock(); };

	typedef VRmLockProxy<int, globalLock, globalUnlock> tVRmGlobalLockProxy;
	typedef VRmLockProxy<VRmUsbCamDevice, VRmUsbCamPrivateLockDevice, VRmUsbCamPrivateUnlockDevice> tVRmDeviceLockProxy;

	SizeI::SizeI(int width, int height)
	{
		m_width= width;
		m_height= height;
	}

	PointI::PointI(int x, int y)
	{
		m_x= x;
		m_y= y;
	}

	RectI::RectI(int left, int top, int width, int height)
	{
		m_left= left;
		m_top= top;
		m_width= width;
		m_height= height;
	}

	std::string PropInfo::ToString() const {
		return m_id_string;
	}

	// Exception --------------------------------------------------------------------------------------

#ifdef WIN32
	Exception::Exception(const std::string& message, ErrorNumber number)
		: std::exception(message.c_str()), m_number(number)
	{
	}
#else
	Exception::Exception(const std::string& message, ErrorNumber number)
		: std::exception(), m_number(number), m_message(message)
	{
	}

	Exception::~Exception() throw ()
	{}

	const char* Exception::what() const throw ()
	{
		return m_message.c_str();
	}

#endif

	std::string Exception::get_Description() const {
		return what();
	}

	ErrorNumber Exception::get_Number() const {
		return m_number;
	}

	// DeviceKey --------------------------------------------------------------------------------------

	std::string DeviceKey::ToString() const {
		return get_Product() + " #" + get_SerialString();
	}

	std::string DeviceKey::get_Manufacturer() const {
		return std::string(mp_key->mp_manufacturer_str);
	}

	std::string DeviceKey::get_Product() const {
		return std::string(mp_key->mp_product_str);
	}

	int DeviceKey::get_Serial() const {
		return mp_key->m_serial;
	}

	std::string DeviceKey::get_SerialString() const {
		VRmSTRING l_str=0;
		VRMCALL(GetSerialString(mp_key, &l_str));
		return std::string(l_str);
	}

	bool DeviceKey::get_Busy() const {
		return mp_key->m_busy?true:false;
	}

	int DeviceKey::get_VendorId() const {
		VRmWORD retval=0;
		VRMCALL(GetVendorId(mp_key, &retval));
		return retval;
	}

	int DeviceKey::get_ProductId() const {
		VRmWORD retval=0;
		VRMCALL(GetProductId(mp_key, &retval));
		return retval;
	}

	int DeviceKey::get_GroupId() const {
		VRmWORD retval=0;
		VRMCALL(GetGroupId(mp_key, &retval));
		return retval;
	}

	std::string DeviceKey::get_IpAddress() const {
		VRmSTRING l_str=0;
		VRMCALL(GetIpAddress(mp_key, &l_str));
		return std::string(l_str);
	}

	std::string DeviceKey::get_LocalIpAddress() const {
		VRmSTRING l_str=0;
		VRMCALL(GetLocalIpAddress(mp_key, &l_str));
		return std::string(l_str);
	}

	DeviceKey::DeviceKey(VRmDeviceKey* fp_key) { mp_key= fp_key; }

	bool DeviceKey::operator==(const DeviceKeyPtr& other) const
	{
		VRMCHECK(other is_not null);
		VRmBOOL l_tmp=1;
		VRMCALL(CompareDeviceKeys(mp_key, other->mp_key, &l_tmp));
		if (l_tmp)
			return false;
		return true;
	}

	bool DeviceKey::operator!=(const DeviceKeyPtr& other) const
	{
		VRMCHECK(other is_not null);
		return !operator==(other);
	}

	DeviceKey::~DeviceKey()
	{
		if (mp_key)
			VRmUsbCamFreeDeviceKey(&mp_key);
	}

	VRmDeviceKey* DeviceKey::get_NativeRepresentation() const
	{
		return mp_key;
	}

	// ImageFormat ---------------------------------------------------------------------------------------

	std::string ImageFormat::ToString() const {
		VRmSTRING lp_color_format= 0;
		VRMCALL(GetStringFromColorFormat(m_vrmformat.m_color_format, &lp_color_format));
		return (boost::format("%1%x%2% (%3%)") % m_vrmformat.m_width % m_vrmformat.m_height % lp_color_format).str();
	}

	std::string ImageFormat::get_Description() const {
		return m_description;
	}

	bool ImageFormat::operator==(const ImageFormat& other) const
	{
		VRmBOOL l_tmp=1;
		VRMCALL(CompareImageFormats(&m_vrmformat, &other.m_vrmformat, &l_tmp));
		if (l_tmp)
			return false;
		return true;
	}

	bool ImageFormat::operator!=(const ImageFormat& other) const
	{
		VRmBOOL l_tmp=1;
		VRMCALL(CompareImageFormats(&m_vrmformat, &other.m_vrmformat, &l_tmp));
		if (l_tmp)
			return true;
		return false;
	}

	ImageFormat::ImageFormat()
	{
		m_vrmformat.m_width= 0;
		m_vrmformat.m_height= 0;
		m_vrmformat.m_color_format= (VRmColorFormat)0;
		m_vrmformat.m_image_modifier= (VRmImageModifier)0;
		updateDescription();
	}

	ImageFormat::ImageFormat(const SizeI& size, ColorFormat colorformat, ImageModifier imagemodifier)
	{
		if (size.m_width<0 || size.m_height<0)
			VRMERR("Size must be non-negative");
		m_vrmformat.m_width= size.m_width;
		m_vrmformat.m_height= size.m_height;
		m_vrmformat.m_color_format= static_cast<VRmColorFormat>(colorformat);
		m_vrmformat.m_image_modifier= static_cast<VRmImageModifier>(imagemodifier);
		updateDescription();
	}

	ImageFormat::ImageFormat(const VRmImageFormat& format, const std::string& description)
	{
		m_vrmformat= format;
		if (description.empty())
			updateDescription();
		else
			m_description= description;
	}

	ImageFormat::ImageFormat( const VRmImageFormat& format )
	{
		m_vrmformat= format;
		updateDescription();
	}

	void ImageFormat::set_Size(const SizeI& size) {
		if (size.m_width<0 || size.m_height<0)
			VRMERR("Size must be non-negative");
		m_vrmformat.m_width= size.m_width;
		m_vrmformat.m_height= size.m_height;
		updateDescription();
	}

	SizeI ImageFormat::get_Size() const {
		return SizeI(m_vrmformat.m_width, m_vrmformat.m_height);
	}

	ColorFormat ImageFormat::get_ColorFormat() const {
		return m_vrmformat.m_color_format;
	}

	void ImageFormat::set_ColorFormat(ColorFormat colorformat) {
		m_vrmformat.m_color_format= colorformat;
		updateDescription();
	}

	ImageModifier ImageFormat::get_ImageModifier() const {
		return static_cast<ImageModifier>(m_vrmformat.m_image_modifier);
	}

	void ImageFormat::set_ImageModifier(ImageModifier imagemodifier) {
		m_vrmformat.m_image_modifier= static_cast<VRmImageModifier>(imagemodifier);
		updateDescription();
	}

	int ImageFormat::get_PixelDepth() const {
		VRmDWORD l_depth=0;
		VRMCALL(GetPixelDepthFromColorFormat(m_vrmformat.m_color_format, &l_depth));
		return l_depth;
	}

	std::vector<ImageFormat> ImageFormat::get_TargetFormatList() const {
		VRmDWORD size= 0;
		VRMCALL(GetTargetFormatListSize(&m_vrmformat, &size));
		std::vector<ImageFormat> l_target_format_list((size_t)size);
		for (VRmDWORD i=0; i<size; ++i) {
			VRmImageFormat l_tmp;
			VRMCALL(GetTargetFormatListEntry(&m_vrmformat, i, &l_tmp));
			l_target_format_list[i]= ImageFormat(l_tmp, std::string());
		}
		return l_target_format_list;
	}

	void ImageFormat::FlipHorizontal() {
		m_vrmformat.m_image_modifier ^= VRM_HORIZONTAL_MIRRORED;
		updateDescription();
	}

	void ImageFormat::FlipVertical() {
		m_vrmformat.m_image_modifier ^= VRM_VERTICAL_MIRRORED;
		updateDescription();
	}

	void ImageFormat::updateDescription() {
		m_description= std::string();
	}

	const VRmImageFormat& ImageFormat::get_NativeRepresentation() const
	{
		return m_vrmformat;
	}

	// Image ---------------------------------------------------------------------------------------------

	Image::Image(const ImageFormat& format)
		: mp_vrmimage(0), mp_owner(ImagePtr())
	{
		VRMCALL(NewImage(&mp_vrmimage, format.get_NativeRepresentation()));
	}

	Image::Image(const ImageFormat& format, void* buffer, int pitch)
		: mp_vrmimage(0), mp_owner(ImagePtr())
	{
		VRMCHECK(buffer is_not null);
		VRMCHECK(pitch>=0);
		VRMCALL(SetImage(&mp_vrmimage, format.get_NativeRepresentation(), (VRmBYTE*)buffer, pitch));
	}

	Image::Image(const ImageFormat& format, void* buffer, int pitch, int buffer_size)
		: mp_vrmimage(0), mp_owner(ImagePtr())
	{
		VRMCHECK(buffer is_not null);
		VRMCHECK(pitch>=0);
		VRMCALL(SetImageEx(&mp_vrmimage, format.get_NativeRepresentation(), (VRmBYTE*)buffer, pitch, buffer_size));
	}

	// used by Device::LockNextImage() and Image::Crop()
	Image::Image(VRmImage* fp_vrmimage, tOwnerPtr fp_owner)
		: mp_vrmimage(fp_vrmimage), mp_owner(fp_owner)
	{
	}

	ImagePtr Image::Clone() const {
		VRmImage* lp_new_image=0;
		VRMCALL(CopyImage(&lp_new_image, mp_vrmimage));
		return ImagePtr(new Image(lp_new_image, tOwnerPtr()));
	}

	Image::~Image() {
		if (mp_vrmimage) {
			DevicePtr* lpp_device= boost::get<DevicePtr>(&mp_owner);
			if ( lpp_device && *lpp_device )
				VRmUsbCamUnlockNextImage((*lpp_device)->get_NativeRepresentation(), &mp_vrmimage);
			else
				VRmUsbCamFreeImage(&mp_vrmimage);
		}
	}

	ImageFormat Image::get_ImageFormat() const {
		return ImageFormat(mp_vrmimage->m_image_format);
	}

	double Image::get_TimeStamp() const {
		return mp_vrmimage->m_time_stamp;
	}

	int Image::get_Pitch() const {
		return mp_vrmimage->m_pitch;
	}

	void* Image::get_Buffer() const {
		return mp_vrmimage->mp_buffer;
	}

	void Image::set_BufferSize(int size) {
		VRMCALL(SetImageBufferSize(mp_vrmimage, VRmDWORD(size)));
	}

	int Image::get_BufferSize() const {
		VRmDWORD l_tmp=0;
		VRMCALL(GetImageBufferSize(mp_vrmimage, &l_tmp));
		return l_tmp;
	}

	int Image::get_FrameCounter() const {
		VRmDWORD l_tmp=0;
		VRMCALL(GetFrameCounter(mp_vrmimage, &l_tmp));
		return l_tmp;
	}

	int Image::get_SensorPort() const {
		VRmDWORD l_tmp=0;
		VRMCALL(GetImageSensorPort(mp_vrmimage, &l_tmp));
		return l_tmp;
	}

	std::vector<unsigned char> Image::get_Footer() const {
		const void* lp_footer= 0;
		VRmDWORD l_length= 0;
		VRMCALL(GetImageFooter(mp_vrmimage, &lp_footer, &l_length));
		std::vector<unsigned char> lp_data((size_t)l_length);
		if (lp_data.size())
			memcpy(&lp_data[0], lp_footer, l_length);
		return lp_data;
	}

	ImagePtr Image::Crop(const RectI& rect) {
		VRmImage* lp_new_image=0;
		VRMCALL(CropImage(&lp_new_image, mp_vrmimage, &rect));
		return ImagePtr(new Image(lp_new_image, shared_from_this()));
	}

	VRmImage* Image::get_NativeRepresentation() const
	{
		return mp_vrmimage;
	}

	// DevicePropertyPage ---------------------------------------------------------------------------

#ifdef WIN32
	DevicePropertyPage::DevicePropertyPage(VRmDevicePropertyPage* fp_vrmpage)
		: mp_vrmpage(fp_vrmpage)
	{
	}

	void DevicePropertyPage::Move(const RectI& rect) {
		VRMCHECK(mp_vrmpage is_not null);
		if (!MoveWindow(mp_vrmpage->m_handle, rect.m_left, rect.m_top, rect.m_width, rect.m_height, TRUE))
			VRMERR_WITHCODE("MoveWindow() failed", GetLastError());
	}

	SizeI DevicePropertyPage::get_SizeHint() const {
		VRMCHECK(mp_vrmpage is_not null);
		return SizeI(mp_vrmpage->m_size_hint.m_width, mp_vrmpage->m_size_hint.m_height);
	}

	void DevicePropertyPage::Show() {
		VRMCHECK(mp_vrmpage is_not null);
		if (GetParent(mp_vrmpage->m_handle))
			VRMERR("only valid for top level pages (hWndParent is null)");
		ShowWindow(mp_vrmpage->m_handle, SW_SHOW); // NOTE: ShowWindow has no error return value
	}

	void DevicePropertyPage::Hide() {
		VRMCHECK(mp_vrmpage is_not null);
		if (GetParent(mp_vrmpage->m_handle))
			VRMERR("only valid for top level pages (hWndParent is null)");
		ShowWindow(mp_vrmpage->m_handle, SW_HIDE); // NOTE: ShowWindow has no error return value
	}

	void DevicePropertyPage::set_Size(const SizeI& size) {
		VRMCHECK(mp_vrmpage is_not null);
		RECT l_rect;
		if (!GetWindowRect(mp_vrmpage->m_handle, &l_rect))
			VRMERR_WITHCODE("GetWindowRect() failed", GetLastError());
		if (!MoveWindow(mp_vrmpage->m_handle, l_rect.left, l_rect.top, size.m_width, size.m_height, TRUE))
			VRMERR_WITHCODE("MoveWindow() failed", GetLastError());
	}

	SizeI DevicePropertyPage::get_Size() const {
		VRMCHECK(mp_vrmpage is_not null);
		RECT l_rect;
		if (!GetWindowRect(mp_vrmpage->m_handle, &l_rect))
			VRMERR_WITHCODE("GetWindowRect() failed", GetLastError());
		return SizeI(l_rect.right-l_rect.left, l_rect.bottom-l_rect.top);
	}

	void DevicePropertyPage::set_Location(const PointI& location) {
		VRMCHECK(mp_vrmpage is_not null);
		RECT l_rect;
		if (!GetWindowRect(mp_vrmpage->m_handle, &l_rect))
			VRMERR_WITHCODE("GetWindowRect() failed", GetLastError());
		if (!MoveWindow(mp_vrmpage->m_handle, location.m_x, location.m_y, l_rect.right-l_rect.left, l_rect.bottom-l_rect.top, TRUE))
			VRMERR_WITHCODE("MoveWindow() failed", GetLastError());
	}

	PointI DevicePropertyPage::get_Location() const {
		VRMCHECK(mp_vrmpage is_not null);
		HWND l_parent= GetParent(mp_vrmpage->m_handle);
		RECT l_rect;
		if (!l_parent)
		{
			if (!GetWindowRect(mp_vrmpage->m_handle, &l_rect))
				VRMERR_WITHCODE("GetWindowRect() failed", GetLastError());
			return PointI(l_rect.left, l_rect.top);
		}
		else
		{
			POINT l_pos;
			if (!ClientToScreen(mp_vrmpage->m_handle, &l_pos))
				VRMERR_WITHCODE("ClientToScreen() failed", GetLastError());
			POINT l_parentpos;
			if (!ClientToScreen(l_parent, &l_parentpos))
				VRMERR_WITHCODE("ClientToScreen() failed", GetLastError());
			l_pos.x-=l_parentpos.x;
			l_pos.y-=l_parentpos.y;
			return PointI(l_pos.x, l_pos.y);
		}
	}

	bool DevicePropertyPage::get_Visible() const {
		VRMCHECK(mp_vrmpage is_not null);
		return IsWindowVisible(mp_vrmpage->m_handle)?true:false;
	}

	HWND DevicePropertyPage::get_Handle() const {
		if (mp_vrmpage)
			return mp_vrmpage->m_handle;
		return 0;
	}

	DevicePropertyPage::~DevicePropertyPage() {
		if (mp_vrmpage)
		{
			VRmUsbCamDestroyDevicePropertyPage(&mp_vrmpage);
		}
	}

	VRmDevicePropertyPage* DevicePropertyPage::get_NativeRepresentation() const
	{
		return mp_vrmpage;
	}
#endif//WIN32

	// Device ---------------------------------------------------------------------------------------

	// methods

	Device::Device(const DeviceKeyPtr& key)
		: m_device(0), m_callbacks_registered(false)
	{
		VRMCHECK(key is_not null);
		VRMCALL(OpenDevice(key->get_NativeRepresentation(), &m_device));
		VRMCALL(RegisterDeviceCallbackEx(m_device, DeviceCallbackProxy, this));
		m_callbacks_registered= true;
	}

	void Device::Start() {
		VRMCHECK(m_device is_not null);
		VRMDEVCALL(Start(m_device));
	}

	void Device::Stop() {
		VRMCHECK(m_device is_not null);
		VRMDEVCALL(Stop(m_device));
	}

	bool Device::IsNextImageReady(int port) const {
		VRMCHECK(m_device is_not null);
		VRmBOOL tmp= false;
		VRMDEVCALL(IsNextImageReadyEx(m_device, port, &tmp));
		return tmp?true:false;
	}

	void Device::Close() {
		VRMCHECK(m_device is_not null);
		VRMDEVCALL(CloseDevice(m_device));
		m_device= 0;
	}

	ImagePtr Device::LockNextImage(int port, int* framesDropped, int timeoutMs) {
		VRMCHECK(m_device is_not null);
		VRmImage* lp_vrmimage= 0;
		VRmDWORD l_frames_dropped;
		VRMDEVCALL(LockNextImageEx2(m_device, port, &lp_vrmimage, &l_frames_dropped, timeoutMs));
		ImagePtr lp_image(new Image(lp_vrmimage, shared_from_this()/*, mp_com_device*/));
		if (framesDropped)
			*framesDropped= l_frames_dropped;
		return lp_image;
	}

	void Device::UnlockNextImage(ImagePtr& image) {
		VRMCHECK(m_device is_not null);
		VRMCHECK(image is_not null);
		DevicePtr* lpp_owner_device= boost::get<DevicePtr>(&image->mp_owner);
		if (!lpp_owner_device || *lpp_owner_device != shared_from_this())
			VRMERR("Image is not locked by this device");
		image.reset(); // this will unlock it when there are no other references!
	}

	void Device::SoftTrigger() {
		VRMCHECK(m_device is_not null);
		VRMDEVCALL(SoftTrigger(m_device));
	}

	std::vector<unsigned char> Device::LoadUserData() {
		VRMCHECK(m_device is_not null);
		VRmUserData* lp_vrmdata=0;
		VRMDEVCALL(LoadUserData(m_device, &lp_vrmdata));
		std::vector<unsigned char> lp_data((size_t)lp_vrmdata->m_length);
		if (lp_data.size())
			memcpy(&lp_data[0], lp_vrmdata->mp_data, lp_data.size());
		VRMCALL(FreeUserData(&lp_vrmdata));
		return lp_data;
	}

	void Device::SaveUserData(const std::vector<unsigned char>& data) {
		VRMCHECK(m_device is_not null);
		VRmUserData* lp_vrmdata=0;
		VRMCALL(NewUserData(&lp_vrmdata, VRmDWORD(data.size())));
		try
		{
			if (lp_vrmdata->m_length)
				memcpy(lp_vrmdata->mp_data, &data[0], lp_vrmdata->m_length);
			VRMDEVCALL(SaveUserData(m_device, lp_vrmdata));
		}
		catch (const std::exception&)
		{
			VRmUsbCamFreeUserData(&lp_vrmdata);
			throw;
		}
		VRMCALL(FreeUserData(&lp_vrmdata));
	}

	void Device::ResetFrameCounter() {
		VRMCHECK(m_device is_not null);
		VRMDEVCALL(ResetFrameCounter(m_device));
	}

#ifdef WIN32
	DevicePropertyPagePtr Device::CreatePropertyPage(HWND hWndParent, const RectI& rect) {
		VRMCHECK(m_device is_not null);

		VRmDevicePropertyPage* lpp_vrmpage= 0;
		VRMDEVCALL(CreateDevicePropertyPage(m_device, hWndParent, &rect, &lpp_vrmpage));

		return DevicePropertyPagePtr(new DevicePropertyPage(lpp_vrmpage));
	}

	DevicePropertyPagePtr Device::CreatePropertyPage() {
		VRMCHECK(m_device is_not null);

		VRmDevicePropertyPage* lpp_vrmpage= 0;
		VRMDEVCALL(CreateDevicePropertyPage(m_device, 0, 0, &lpp_vrmpage));

		return DevicePropertyPagePtr(new DevicePropertyPage(lpp_vrmpage));
	}
#endif//WIN32

	void Device::LoadConfig(int id) {
		VRMCHECK(m_device is_not null);
		VRMDEVCALL(LoadConfig(m_device, id));
	}

	void Device::SaveConfig(int id) {
		VRMCHECK(m_device is_not null);
		VRMDEVCALL(SaveConfig(m_device, id));
	}

	bool Device::SaveConfigRequiresFirmwareCompression(int id) {
		VRMCHECK(m_device is_not null);
		VRmBOOL tmp= false;
		VRMDEVCALL(SaveConfigRequiresFirmwareCompression(m_device, id, &tmp));
		return tmp?true:false;
	}

	void Device::DeleteConfig(int id) {
		VRMCHECK(m_device is_not null);
		VRMDEVCALL(DeleteConfig(m_device, id));
	}

	int Device::FindSensorPortListIndex(int port) const
	{
		VRMCHECK(m_device is_not null);
		VRmDWORD index;
		VRMDEVCALL(FindSensorPortListIndex(m_device, port, &index));
		return index;
	}

	// properties

	DeviceKeyPtr Device::get_DeviceKey() const {
		VRMCHECK(m_device is_not null);
		VRmDeviceKey* lp_vrmkey=0;
		VRMDEVCALL(GetDeviceKey(m_device, &lp_vrmkey));
		return DeviceKeyPtr(new DeviceKey(lp_vrmkey));
	}

	ImageFormat Device::get_SourceFormat(int port) const {
		VRMCHECK(m_device is_not null);
		tVRmDeviceLockProxy l_device_lock_proxy(m_device);
		boost::lock_guard<tVRmDeviceLockProxy> l_device_lock(l_device_lock_proxy);
		VRmImageFormat l_tmp;
		VRMDEVCALL(GetSourceFormatEx(m_device, port, &l_tmp));
		const char* lcp_description;
		VRMDEVCALL(GetSourceFormatDescription(m_device, port, &lcp_description));
		return ImageFormat(l_tmp, lcp_description?std::string(lcp_description):std::string());
	}

	std::vector<int> Device::get_SensorPortList() const {
		VRMCHECK(m_device is_not null);
		tVRmDeviceLockProxy l_device_lock_proxy(m_device);
		boost::lock_guard<tVRmDeviceLockProxy> l_device_lock(l_device_lock_proxy);
		VRmDWORD size= 0;
		VRMDEVCALL(GetSensorPortListSize(m_device, &size));
		std::vector<int> l_port_list((size_t)size);
		for (VRmDWORD i=0; i<size; ++i) {
			VRmDWORD l_tmp;
			VRMDEVCALL(GetSensorPortListEntry(m_device, i, &l_tmp));
			l_port_list[i]= l_tmp;
		}
		return l_port_list;
	}

	std::vector<ImageFormat> Device::get_TargetFormatList(int port) const {
		VRMCHECK(m_device is_not null);
		tVRmDeviceLockProxy l_device_lock_proxy(m_device);
		boost::lock_guard<tVRmDeviceLockProxy> l_device_lock(l_device_lock_proxy);
		VRmDWORD size= 0;
		VRMDEVCALL(GetTargetFormatListSizeEx2(m_device, port, &size));
		std::vector<ImageFormat> l_target_format_list((size_t)size);
		for (VRmDWORD i=0; i<size; ++i) {
			VRmImageFormat l_tmp;
			VRMDEVCALL(GetTargetFormatListEntryEx2(m_device, port, i, &l_tmp));
			l_target_format_list[i]= ImageFormat(l_tmp, std::string());
		}
		return l_target_format_list;
	}

	void Device::set_ConfigData(const std::vector<unsigned char>& data) {
		VRMCHECK(m_device is_not null);
		VRmUserData* lp_vrmdata=0;
		VRMCALL(NewUserData(&lp_vrmdata, (VRmDWORD)data.size()));
		try {
			if (lp_vrmdata->m_length)
				memcpy(lp_vrmdata->mp_data, &data[0], lp_vrmdata->m_length);
			VRMDEVCALL(SetConfigData(m_device, lp_vrmdata));
		}
		catch (const std::exception&)
		{
			VRmUsbCamFreeUserData(&lp_vrmdata);
			throw;
		}
		VRMCALL(FreeUserData(&lp_vrmdata));
	}

	std::vector<unsigned char> Device::get_ConfigData() const {
		VRMCHECK(m_device is_not null);
		VRmUserData* lp_vrmdata=0;
		VRMDEVCALL(GetConfigData(m_device, &lp_vrmdata));
		std::vector<unsigned char> data((size_t)lp_vrmdata->m_length);
		try {
			if (data.size())
				memcpy(&data[0], lp_vrmdata->mp_data, data.size());
		}
		catch (const std::exception&)
		{
			VRmUsbCamFreeUserData(&lp_vrmdata);
			throw;
		}
		VRMCALL(FreeUserData(&lp_vrmdata));
		return data;
	}

	bool Device::get_ConfigIncludesUnsupportedValues() const {
		VRMCHECK(m_device is_not null);
		VRmBOOL l_val=0;
		VRMDEVCALL(ConfigIncludesUnsupportedValues(m_device, &l_val));
		return l_val!=0;
	}

	bool Device::get_Running() const {
		VRMCHECK(m_device is_not null);
		VRmBOOL tmp= false;
		VRMDEVCALL(GetRunning(m_device, &tmp));
		return tmp?true:false;
	}

	std::vector<PropId> Device::get_PropertyList() const {
		VRMCHECK(m_device is_not null);
		tVRmDeviceLockProxy l_device_lock_proxy(m_device);
		boost::lock_guard<tVRmDeviceLockProxy> l_device_lock(l_device_lock_proxy);

		VRmDWORD size= 0;
		VRMDEVCALL(GetPropertyListSize(m_device, &size));

		std::vector<PropId> l_list((size_t)size);
		for (VRmDWORD i=0; i<size; ++i) {
			VRmPropId l_tmp;
			VRMDEVCALL(GetPropertyListEntry(m_device, i, &l_tmp));
			l_list[i]= static_cast<PropId>(l_tmp);
		}
		return l_list;
	}

	template<typename t> static Variant convert2variant(const t& fcr_val) { return fcr_val; }
	template<> Variant convert2variant(const VRmBOOL& fcr_val) { return bool(fcr_val?true:false); }
	template<> Variant convert2variant(const VRmSTRING& fcr_val) { return std::string(fcr_val); }
	template<> Variant convert2variant(const VRmPropId& fcr_val) { return static_cast<PropId>(fcr_val); }
	template<> Variant convert2variant(const VRmSizeI& fcr_val) { return SizeI(fcr_val.m_width, fcr_val.m_height); }
	template<> Variant convert2variant(const VRmPointI& fcr_val) { return PointI(fcr_val.m_x, fcr_val.m_y); }
	template<> Variant convert2variant(const VRmRectI& fcr_val) { return RectI(fcr_val.m_left, fcr_val.m_top, fcr_val.m_width, fcr_val.m_height); }

	PropInfo Device::get_PropertyInfo(PropId propid) const {
		VRMCHECK(m_device is_not null);
		PropInfo l_vrmpropinfo;
		VRMDEVCALL(GetPropertyInfo(m_device, static_cast<VRmPropId>(propid), &l_vrmpropinfo));
		return l_vrmpropinfo;
	}

	bool Device::get_PropertySupported(PropId propid) const {
		VRMCHECK(m_device is_not null);
		VRmBOOL l_supported;
		VRMDEVCALL(GetPropertySupported(m_device, static_cast<VRmPropId>(propid), &l_supported));
		return l_supported?true:false;
	}

#define CONVERTATTRIBS(vrm_suffix) \
	{ \
	VRmPropAttribs##vrm_suffix vrm_attribs; \
	VRMDEVCALL(GetPropertyAttribs##vrm_suffix(m_device, static_cast<VRmPropId>(propid), &vrm_attribs)); \
	l_attribs.m_default= convert2variant(vrm_attribs.m_default); \
	l_attribs.m_min= convert2variant(vrm_attribs.m_min); \
	l_attribs.m_max= convert2variant(vrm_attribs.m_max); \
	l_attribs.m_step= convert2variant(vrm_attribs.m_step); \
	} \
	break

	PropAttribs Device::get_PropertyAttribs(PropId propid) const {
		VRMCHECK(m_device is_not null);
		VRmPropInfo vrm_propinfo;
		VRMDEVCALL(GetPropertyInfo(m_device, static_cast<VRmPropId>(propid), &vrm_propinfo));
		PropAttribs l_attribs;
		switch (vrm_propinfo.m_type) {
	case VRM_PROP_TYPE_BOOL: CONVERTATTRIBS(B);
	case VRM_PROP_TYPE_INT: CONVERTATTRIBS(I);
	case VRM_PROP_TYPE_FLOAT: CONVERTATTRIBS(F);
	case VRM_PROP_TYPE_STRING: CONVERTATTRIBS(S);
	case VRM_PROP_TYPE_ENUM: CONVERTATTRIBS(E);
	case VRM_PROP_TYPE_SIZE_I: CONVERTATTRIBS(SizeI);
	case VRM_PROP_TYPE_POINT_I: CONVERTATTRIBS(PointI);
	case VRM_PROP_TYPE_RECT_I: CONVERTATTRIBS(RectI);
	case VRM_PROP_TYPE_DOUBLE: CONVERTATTRIBS(D);
	default:
		VRMERR(std::string("unsupported property type for property ") + vrm_propinfo.m_id_string);
		}
		return l_attribs;
	}

#define RETURNGETVALUE(vrm_type, vrm_suffix) \
	{ \
	vrm_type l_vrmval; \
	VRMDEVCALL(GetPropertyValue##vrm_suffix(m_device, static_cast<VRmPropId>(propid), &l_vrmval)); \
	return convert2variant(l_vrmval); \
	}

	Variant Device::get_PropertyValue(PropId propid) const {
		VRMCHECK(m_device is_not null);
		VRmPropInfo vrm_propinfo;
		VRMDEVCALL(GetPropertyInfo(m_device, static_cast<VRmPropId>(propid), &vrm_propinfo));
		switch (vrm_propinfo.m_type) {
	case VRM_PROP_TYPE_BOOL: RETURNGETVALUE(VRmBOOL, B);
	case VRM_PROP_TYPE_INT: RETURNGETVALUE(int, I);
	case VRM_PROP_TYPE_FLOAT: RETURNGETVALUE(float, F);
	case VRM_PROP_TYPE_STRING: RETURNGETVALUE(VRmSTRING, S);
	case VRM_PROP_TYPE_ENUM: RETURNGETVALUE(VRmPropId, E);
	case VRM_PROP_TYPE_SIZE_I: RETURNGETVALUE(VRmSizeI, SizeI);
	case VRM_PROP_TYPE_POINT_I: RETURNGETVALUE(VRmPointI, PointI);
	case VRM_PROP_TYPE_RECT_I: RETURNGETVALUE(VRmRectI, RectI);
	case VRM_PROP_TYPE_DOUBLE: RETURNGETVALUE(double, D);
	default:
		VRMERR(std::string("unsupported property type for property ") + vrm_propinfo.m_id_string);
		}
	}

	void Device::set_PropertyValue(PropId propid, const Variant& value) {
		VRMCHECK(m_device is_not null);
		VRmPropInfo vrm_propinfo;
		VRMDEVCALL(GetPropertyInfo(m_device, static_cast<VRmPropId>(propid), &vrm_propinfo));
		const int* p_val_i = boost::get<int>(&value);
		switch (vrm_propinfo.m_type) {
	case VRM_PROP_TYPE_BOOL:
		{
			VRmBOOL vrm_val;
			const bool* p_val_b = boost::get<bool>(&value);
			if (p_val_b)
				vrm_val= *p_val_b ? 1:0;
			else if (p_val_i && (*p_val_i==0 || *p_val_i==1))
				vrm_val= *p_val_i;
			else VRMERR(std::string("type mismatch. expected boolean type for property ")+vrm_propinfo.m_id_string);
			VRMCALL(SetPropertyValueB(m_device, static_cast<VRmPropId>(propid), &vrm_val));
		}
		break;
	case VRM_PROP_TYPE_INT:
		{
			if (!p_val_i) VRMERR(std::string("type mismatch. expected integer type for property ")+vrm_propinfo.m_id_string);
			VRMCALL(SetPropertyValueI(m_device, static_cast<VRmPropId>(propid), p_val_i));
		}
		break;
	case VRM_PROP_TYPE_FLOAT:
		{
			float vrm_val;
			const float* p_val_f = boost::get<float>(&value);
			if (p_val_f)
				vrm_val= *p_val_f;
			else if (p_val_i)
				vrm_val= static_cast<float>(*p_val_i);
			else VRMERR(std::string("type mismatch. expected floating point type for property ")+vrm_propinfo.m_id_string);
			VRMCALL(SetPropertyValueF(m_device, static_cast<VRmPropId>(propid), &vrm_val));
		}
		break;
	case VRM_PROP_TYPE_DOUBLE:
		{
			double vrm_val;
			const double* p_val_d = boost::get<double>(&value);
			const float* p_val_f = boost::get<float>(&value);
			if (p_val_d)
				vrm_val= *p_val_d;
			else if (p_val_f)
				vrm_val= static_cast<double>(*p_val_f);
			else if (p_val_i)
				vrm_val= static_cast<double>(*p_val_i);
			else VRMERR(std::string("type mismatch. expected floating point type for property ")+vrm_propinfo.m_id_string);
			VRMCALL(SetPropertyValueD(m_device, static_cast<VRmPropId>(propid), &vrm_val));
		}
		break;
	case VRM_PROP_TYPE_STRING:
		{
			const std::string* p_val_s = boost::get<std::string>(&value);
			if (!p_val_s)
				VRMERR(std::string("type mismatch. expected string type for property ")+vrm_propinfo.m_id_string);
			VRmSTRING vrm_val= p_val_s->c_str();
			VRMCALL(SetPropertyValueS(m_device, static_cast<VRmPropId>(propid), &vrm_val));
		}
		break;
	case VRM_PROP_TYPE_ENUM:
		{
			VRmPropId vrm_val;
			const PropId* p_val_e = boost::get<PropId>(&value);
			if (p_val_e)
				vrm_val= static_cast<VRmPropId>(*p_val_e);
			else if (p_val_i)
				vrm_val= static_cast<VRmPropId>(*p_val_i);
			else VRMERR(std::string("type mismatch. expected PropId (integer) type for property ")+vrm_propinfo.m_id_string);
			VRMCALL(SetPropertyValueE(m_device, static_cast<VRmPropId>(propid), &vrm_val));
		}
		break;
	case VRM_PROP_TYPE_SIZE_I:
		{
			const VRmSizeI* p_val_size_i = boost::get<SizeI>(&value);
			if (!p_val_size_i)
				VRMERR(std::string("type mismatch. expected Size type for property ")+vrm_propinfo.m_id_string);
			VRMCALL(SetPropertyValueSizeI(m_device, static_cast<VRmPropId>(propid), p_val_size_i));
		}
		break;
	case VRM_PROP_TYPE_POINT_I:
		{
			const VRmPointI* p_val_point_i = boost::get<PointI>(&value);
			if (!p_val_point_i)
				VRMERR(std::string("type mismatch. expected Point type for property ")+vrm_propinfo.m_id_string);
			VRMCALL(SetPropertyValuePointI(m_device, static_cast<VRmPropId>(propid), p_val_point_i));
		}
		break;
	case VRM_PROP_TYPE_RECT_I:
		{
			const VRmRectI* p_val_rect_i = boost::get<RectI>(&value);
			if (!p_val_rect_i)
				VRMERR(std::string("type mismatch. expected Rectangle type for property ")+vrm_propinfo.m_id_string);
			VRMCALL(SetPropertyValueRectI(m_device, static_cast<VRmPropId>(propid), p_val_rect_i));
		}
		break;
	default:
		VRMERR(std::string("unsupported property type for property ") + vrm_propinfo.m_id_string);
		}
	}

	Device::~Device()
	{
		if (m_callbacks_registered)
			VRmUsbCamUnregisterDeviceCallbackEx(m_device, DeviceCallbackProxy, this);
		m_callbacks_registered= false;

		VRmUsbCamCloseDevice(m_device);
		m_device=0;
	}

	// receives Device* as fp_user_data
	void Device::DeviceCallbackProxy(VRmUsbCamDevice f_device, VRmDeviceCallbackType f_type, void* fp_user_data, const void* fcp_callback_params)
	{
		if (!fp_user_data)
			return;
		Device* lp_device= (Device*)(fp_user_data);
		if (lp_device->m_device == f_device)
			lp_device->RaiseEvent(f_type, fcp_callback_params);
	}

	EventArgs::~EventArgs()
	{
		// NOTE: this d-tor allows for proper derivation and turns EventArgs into a polymorphic type
	}

	template class Device::Event<Device::EventHandler>;
	template class Device::Event<Device::PropertyValueChangedEventHandler>;

	void Device::RaiseEvent(VRmDeviceCallbackType f_type, const void* fcp_callback_params)
	{
		try
		{
			switch (f_type)
			{
			case VRM_DEVICE_CALLBACK_TYPE_LUT_CHANGED:
				if (fcp_callback_params)
				{
					int l_sensor_port= *reinterpret_cast<const int*>(fcp_callback_params);
					BOOST_FOREACH(const LutChangedEventHandler& handler, LutChanged.m_handlers)
						handler(shared_from_this(), l_sensor_port);
				}
				break;
			case VRM_DEVICE_CALLBACK_TYPE_SOURCE_FORMAT_CHANGED:
				BOOST_FOREACH(const EventHandler& handler, SourceFormatChanged.m_handlers)
					handler(shared_from_this(), EventArgs());
				break;
			case VRM_DEVICE_CALLBACK_TYPE_PROPERTY_VALUE_CHANGED:
				if (fcp_callback_params)
				{
					PropId l_id= *reinterpret_cast<const PropId*>(fcp_callback_params);
					BOOST_FOREACH(const PropertyValueChangedEventHandler& handler, PropertyValueChanged.m_handlers)
						handler(shared_from_this(), l_id);
				}
				break;
			case VRM_DEVICE_CALLBACK_TYPE_PROPERTY_LIST_CHANGED:
				BOOST_FOREACH(const EventHandler& handler, PropertyListChanged.m_handlers)
					handler(shared_from_this(), EventArgs());
				break;
			case VRM_DEVICE_CALLBACK_TYPE_TARGET_FORMAT_LIST_CHANGED:
				BOOST_FOREACH(const EventHandler& handler, TargetFormatListChanged.m_handlers)
					handler(shared_from_this(), EventArgs());
				break;
			case VRM_DEVICE_CALLBACK_TYPE_PROPERTY_INFO_CHANGED:
				if (fcp_callback_params)
				{
					PropId l_id= *reinterpret_cast<PropId*>(const_cast<void*>(fcp_callback_params));
					BOOST_FOREACH(const PropertyInfoChangedEventHandler& handler, PropertyInfoChanged.m_handlers)
						handler(shared_from_this(), l_id);
				}
				break;
			case VRM_DEVICE_CALLBACK_TYPE_PROPERTY_ATTRIBS_CHANGED:
				if (fcp_callback_params)
				{
					PropId l_id= *reinterpret_cast<PropId*>(const_cast<void*>(fcp_callback_params));
					BOOST_FOREACH(const PropertyAttribsChangedEventHandler& handler, PropertyAttribsChanged.m_handlers)
						handler(shared_from_this(), l_id);
				}
				break;
			default:
				break;
			}
		}
		catch (const std::exception& exc)
		{
			if (!mp_last_callback_exception.get())
				mp_last_callback_exception.reset(
				new std::string(std::string("An event handler raised an exception: ") + exc.what()));
		}
		catch (...)
		{
			if (!mp_last_callback_exception.get())
				mp_last_callback_exception.reset(
				new std::string("An event handler raised an exception"));
		}
	}

	VRmUsbCamDevice Device::get_NativeRepresentation() const
	{
		return m_device;
	}

	VRmUsbCamCPP::EventReturnType Device::WaitForStrobe( int f_timeout_us )
	{
		VRMCHECK(m_device is_not null);
		VRmUsbCamCPP::EventReturnType tmp;
		VRMDEVCALL(WaitForStrobe(m_device, f_timeout_us, &tmp));
		return tmp;
	}

	void Device::SetUserLut( VRmDWORD f_port, VRmDWORD f_channel, UserLutFormat f_format, const std::vector<VRmWORD> &fcr_lut )
	{
		VRMCHECK(m_device is_not null);
		VRMCHECK(fcr_lut.size() > 0);

		VRMDEVCALL(SetUserLut(m_device, f_port, f_channel, f_format, &fcr_lut[0]));
	}

	std::vector<VRmWORD> Device::GetUserLut( VRmDWORD f_port, VRmDWORD f_channel, UserLutFormat f_format )
	{
		VRMCHECK(m_device is_not null);

		VRmWORD *lp_user_lut_tmp = 0;
		VRMDEVCALL(GetUserLut(m_device, f_port, f_channel, f_format, &lp_user_lut_tmp));

		size_t l_user_lut_size = 0;
		switch (f_format)
		{
		case VRM_USER_LUT_8_TO_16:
			l_user_lut_size = 1 << 8;
			break;
		case VRM_USER_LUT_10_TO_16:
			l_user_lut_size = 1 << 10;
			break;
		case VRM_USER_LUT_16_TO_16:
			l_user_lut_size = 1 << 16;
			break;
		default:
			VRMERR("UserLutFormat is not valid");
		}

		std::vector<VRmWORD> ret;
		// copy lp_user_lut_tmp's contents into ret
		ret.insert(ret.end(), &lp_user_lut_tmp[0], &lp_user_lut_tmp[l_user_lut_size]);

		VRMCALL(FreeUserLut(&lp_user_lut_tmp));

		return ret;
	}

	void Device::UserLutCommit( VRmDWORD f_port )
	{
		VRMCHECK(m_device is_not null);
		VRMDEVCALL(UserLutCommit(m_device, f_port));
	}

	// Global objects -----------------------------------------------------------------------------------

	void VRmUsbCam::EnableLogging() { VRMCALL(EnableLogging()); }

	void VRmUsbCam::EnableLoggingEx( const std::string& logFileName ) {
		VRMCALL(EnableLoggingEx(logFileName.c_str()));
	}
	void VRmUsbCam::UpdateDeviceKeyList(bool local /* = false */, bool usb /* = false */, bool ethernet /* = false */) {
		if (local || usb || ethernet)
		{
			VRMCALL(UpdateDeviceKeyListEx(local, usb, ethernet));
		}
		else
		{
			VRMCALL(UpdateDeviceKeyList());
		}
	}

	void VRmUsbCam::ResetDeviceKeyList()
	{
		VRMCALL(ResetDeviceKeyList());
	}

	void VRmUsbCam::RestartTimer() { VRMCALL(RestartTimer()); }

	void VRmUsbCam::ConvertImage(const ImagePtr& source, ImagePtr& target) {
		VRMCHECK(source is_not null);
		VRMCHECK(target is_not null);
		if (source->get_NativeRepresentation()==0)
			VRMERR("Source image is not valid");
		if (target->get_NativeRepresentation()==0)
			VRMERR("Target image is not valid");
		VRMCALL(ConvertImage(source->get_NativeRepresentation(), target->get_NativeRepresentation()));
	}

	DevicePtr VRmUsbCam::OpenDevice(DeviceKeyPtr key) {
		VRMCHECK(key is_not null);
		return DevicePtr(new Device(key));
	}

	ImagePtr VRmUsbCam::NewImage(const ImageFormat& format) {
		return ImagePtr(new Image(format));
	}

	ImagePtr VRmUsbCam::SetImage(const ImageFormat& format, void* buffer, int pitch) {
		return ImagePtr(new Image(format, buffer, pitch));
	}

	ImagePtr VRmUsbCam::SetImage(const ImageFormat& format, void* buffer, int pitch, int buffer_size) {
		return ImagePtr(new Image(format, buffer, pitch, buffer_size));
	}

	void VRmUsbCam::SavePNG(const std::string& fileName, const ImagePtr& source, int z_compession_level /* = -1 */)
	{
		VRMCHECK(source is_not null);
		if (source->get_NativeRepresentation()==0)
			VRMERR("Source image is not valid");
		VRMCALL(SavePNG(fileName.c_str(), source->get_NativeRepresentation(), z_compession_level));
	}

	ImagePtr VRmUsbCam::LoadPNG(const std::string& fileName)
	{
		VRmImage* lp_vrmimage = 0;
		VRMCALL(LoadPNG(fileName.c_str(), &lp_vrmimage));
		ImagePtr lp_image = ImagePtr(new Image(lp_vrmimage, ImagePtr()));
		return lp_image;
	}

#ifdef WIN32
	void VRmUsbCam::ActivateDevice(int vendorId, int productId, int serial) {
		VRMCALL(ActivateDevice(VRmWORD(vendorId), VRmWORD(productId), serial));
	}

	void VRmUsbCam::ActivateAllDevices() {
		VRMCALL(ActivateAllDevices());
	}

	void VRmUsbCam::DeactivateDevice(int vendorId, int productId, int serial) {
		VRMCALL(DeactivateDevice(VRmWORD(vendorId), VRmWORD(productId), serial));
	}
#endif

	template <typename Delegate>
	VRmUsbCamCPP::VRmUsbCam::Event<Delegate>::~Event()
	{
		if (!m_handlers.empty())
			VRmUsbCamUnregisterStaticCallback(VRmUsbCamCallbackProxy);
	}

	template class VRmUsbCam::Event<VRmUsbCam::DeviceChangeEventHandler>;
	VRmUsbCam::Event<VRmUsbCam::DeviceChangeEventHandler> VRmUsbCam::DeviceChange;

	void VRmUsbCam::VRmUsbCamCallbackProxy(VRmStaticCallbackType f_type, void* /*fp_user_data*/, const void* fcp_callback_params)
	{
		VRmUsbCam::RaiseEvent(f_type, fcp_callback_params);
	}

	void VRmUsbCam::RaiseEvent(VRmStaticCallbackType f_type, const void* fcp_callback_params)
	{
		try
		{
			switch (f_type)
			{
			case VRM_STATIC_CALLBACK_TYPE_DEVICE_CHANGE:
				if (fcp_callback_params)
				{
					DeviceChangeType l_type= *reinterpret_cast<const DeviceChangeType*>(fcp_callback_params);
					BOOST_FOREACH(const DeviceChangeEventHandler& handler, DeviceChange.m_handlers)
						handler(l_type);
				}
				break;
			default:
				break;
			}
		}
		catch (...)
		{
			// don't propagate this back through VRmUsbCam. I'm sorry.
		}
	}

#undef GetCurrentTime // stupid windows...

	double VRmUsbCam::get_CurrentTime() {
		double l_time;
		VRMCALL(GetCurrentTime(&l_time));
		return l_time;
	}

	std::vector<DeviceKeyPtr> VRmUsbCam::get_DeviceKeyList() {
		tVRmGlobalLockProxy l_global_lock_proxy;
		boost::lock_guard<tVRmGlobalLockProxy> l_global_lock(l_global_lock_proxy);

		VRmDWORD size= 0;
		VRMCALL(GetDeviceKeyListSize(&size));
		std::vector<DeviceKeyPtr> l_array((size_t)size);
		for (VRmDWORD i=0; i<size; ++i) {
			VRmDeviceKey* lp_key= 0;
			VRMCALL(GetDeviceKeyListEntry(i, &lp_key));
			l_array[i]= DeviceKeyPtr(new DeviceKey(lp_key));
		}
		return l_array;
	}

	int VRmUsbCam::get_Version() {
		VRmDWORD version= 0;
		VRMCALL(GetVersion(&version));
		return (int)version;
	}

} // namespace VRmUsbCamCPP


text:
/******************************************************************************
Copyright (c) 2020, Farbod Farshidian. All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

 * Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

 * Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

 * Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/

#include "ocs2_mobile_manipulator/FactoryFunctions.h"

#include <pinocchio/fwd.hpp>  // forward declarations must be included first.
#include <pinocchio/multibody/joint/joint-composite.hpp>
#include <pinocchio/multibody/model.hpp>

#include <boost/property_tree/info_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <ocs2_core/misc/LoadData.h>
#include <ocs2_pinocchio_interface/urdf.h>

namespace ocs2 {
namespace mobile_manipulator {

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
PinocchioInterface createPinocchioInterface(const std::string& robotUrdfPath, const ManipulatorModelType& type) {
  switch (type) {
    case ManipulatorModelType::DefaultManipulator: {
      // return pinocchio interface
      return getPinocchioInterfaceFromUrdfFile(robotUrdfPath);
    }
    case ManipulatorModelType::FloatingArmManipulator: {
      // add 6 DoF for the floating base
      pinocchio::JointModelComposite jointComposite(2);
      jointComposite.addJoint(pinocchio::JointModelTranslation());
      jointComposite.addJoint(pinocchio::JointModelSphericalZYX());
      // return pinocchio interface
      return getPinocchioInterfaceFromUrdfFile(robotUrdfPath, jointComposite);
    }
    case ManipulatorModelType::FullyActuatedFloatingArmManipulator: {
      // add 6 DOF for the fully-actuated free-floating base
      pinocchio::JointModelComposite jointComposite(2);
      jointComposite.addJoint(pinocchio::JointModelTranslation());
      jointComposite.addJoint(pinocchio::JointModelSphericalZYX());
      // return pinocchio interface
      return getPinocchioInterfaceFromUrdfFile(robotUrdfPath, jointComposite);
    }
    case ManipulatorModelType::WheelBasedMobileManipulator: {
      // add XY-yaw joint for the wheel-base
      pinocchio::JointModelComposite jointComposite(3);
      jointComposite.addJoint(pinocchio::JointModelPX());
      jointComposite.addJoint(pinocchio::JointModelPY());
      jointComposite.addJoint(pinocchio::JointModelRZ());
      // return pinocchio interface
      return getPinocchioInterfaceFromUrdfFile(robotUrdfPath, jointComposite);
    }
    default:
      throw std::invalid_argument("Invalid manipulator model type provided.");
  }
}

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
PinocchioInterface createPinocchioInterface(const std::string& robotUrdfPath, const ManipulatorModelType& type,
                                            const std::vector<std::string>& jointNames) {
  using joint_pair_t = std::pair<const std::string, std::shared_ptr<::urdf::Joint>>;

  // parse the URDF
  const auto urdfTree = ::urdf::parseURDFFile(robotUrdfPath);
  // remove extraneous joints from urdf
  ::urdf::ModelInterfaceSharedPtr newModel = std::make_shared<::urdf::ModelInterface>(*urdfTree);
  for (joint_pair_t& jointPair : newModel->joints_) {
    if (std::find(jointNames.begin(), jointNames.end(), jointPair.first) != jointNames.end()) {
      jointPair.second->type = urdf::Joint::FIXED;
    }
  }
  // resolve for the robot type
  switch (type) {
    case ManipulatorModelType::DefaultManipulator: {
      // return pinocchio interface
      return getPinocchioInterfaceFromUrdfModel(newModel);
    }
    case ManipulatorModelType::FloatingArmManipulator: {
      // add 6 DoF for the floating base
      pinocchio::JointModelComposite jointComposite(2);
      jointComposite.addJoint(pinocchio::JointModelTranslation());
      jointComposite.addJoint(pinocchio::JointModelSphericalZYX());
      // return pinocchio interface
      return getPinocchioInterfaceFromUrdfModel(newModel, jointComposite);
    }
    case ManipulatorModelType::FullyActuatedFloatingArmManipulator: {
      // add 6 DOF for the free-floating base
      pinocchio::JointModelComposite jointComposite(2);
      jointComposite.addJoint(pinocchio::JointModelTranslation());
      jointComposite.addJoint(pinocchio::JointModelSphericalZYX());
      // return pinocchio interface
      return getPinocchioInterfaceFromUrdfFile(robotUrdfPath, jointComposite);
    }
    case ManipulatorModelType::WheelBasedMobileManipulator: {
      // add XY-yaw joint for the wheel-base
      pinocchio::JointModelComposite jointComposite(3);
      jointComposite.addJoint(pinocchio::JointModelPX());
      jointComposite.addJoint(pinocchio::JointModelPY());
      jointComposite.addJoint(pinocchio::JointModelRZ());
      // return pinocchio interface
      return getPinocchioInterfaceFromUrdfModel(newModel, jointComposite);
    }
    default:
      throw std::invalid_argument("Invalid manipulator model type provided.");
  }
}

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
ManipulatorModelInfo createManipulatorModelInfo(const PinocchioInterface& interface, const ManipulatorModelType& type,
                                                const std::string& baseFrame, const std::string& eeFrame) {
  const auto& model = interface.getModel();

  ManipulatorModelInfo info;
  info.manipulatorModelType = type;
  info.stateDim = model.nq;
  // resolve for actuated dof based on type of robot
  switch (type) {
    case ManipulatorModelType::DefaultManipulator: {
      // for default arm, the state dimension and input dimensions are same.
      info.inputDim = info.stateDim;
      info.armDim = info.inputDim;
      break;
    }
    case ManipulatorModelType::FloatingArmManipulator: {
      // remove the static 6-DOF base joints that are unactuated.
      info.inputDim = info.stateDim - 6;
      info.armDim = info.inputDim;
      break;
    }
    case ManipulatorModelType::FullyActuatedFloatingArmManipulator: {
      // all states are actuatable
      info.inputDim = info.stateDim;
      info.armDim = info.inputDim - 6;
      break;
    }
    case ManipulatorModelType::WheelBasedMobileManipulator: {
      // for wheel-based, the input dimension is (v, omega, dq_j) while state dimension is (x, y, psi, q_j).
      info.inputDim = info.stateDim - 1;
      info.armDim = info.inputDim - 2;
      break;
    }
    default:
      throw std::invalid_argument("Invalid manipulator model type provided.");
      break;
  }
  // store frame names for using later.
  info.eeFrame = eeFrame;
  info.baseFrame = baseFrame;
  // get name of arm joints.
  const auto& jointNames = model.names;
  info.dofNames = std::vector<std::string>(jointNames.end() - info.armDim, jointNames.end());

  return info;
}

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
ManipulatorModelType loadManipulatorType(const std::string& configFilePath, const std::string& fieldName) {
  boost::property_tree::ptree pt;
  boost::property_tree::read_info(configFilePath, pt);
  const size_t type = pt.template get<size_t>(fieldName);
  return static_cast<ManipulatorModelType>(type);
}

}  // namespace mobile_manipulator
}  // namespace ocs2

ext: cpp


text:
/**
* Connected Vision - https://github.com/ConnectedVision
* MIT License
*/

#include <DataHandling/Store_SQLite_BaseClass.h>

#include <CppUTest/TestHarness.h>
#include <boost/shared_ptr.hpp>

#include <DataHandling/Store_Cache.h>
#include "Store_Null.h"

#include "TestHelper_Store_SQLite.hpp"
#include "MixIn_IStore.h"

namespace ConnectedVision {
namespace DataHandling {

// activate this define to write DB to file (e.g. for debugging purpose)
// DO NOT commit with UseFileDB enabled
// #define UseFileDB "Test_Store_SQLite_table.db"

// test wrapper for SQLiteCache
class TestWrapper_Store_Cache : public Store_Cache<TestDataClass>
{
public:
	TestWrapper_Store_Cache(
		const id_t& configID,				///< [in] config ID
		const int64_t cacheSize,			///< [in] number of maximal elements in cache
		const shared_ptr< IStore_ReadWrite<TestDataClass> >& baseStore		///< [in] store to be cached (typically a DB store)
	) : Store_Cache(configID, cacheSize, baseStore ), spy_write_through(true)
	{
		baseStoreOrig = baseStore;
		nullStore = make_shared< Store_Null<TestDataClass> >();
	}

	// overwrite write through
	virtual bool write_through(
		const shared_ptr<const TestDataClass>& data	///< [in] shared pointer to read-only data object
	)
	{ return spy_write_through; }
	bool spy_write_through;

	// test helper functions
	void flush()
	{ this->cacheFlush(); }

	void useCacheOnly()
	{ this->baseStore = this->nullStore; }

	void useCacheAndBaseStore()
	{ this->baseStore = this->baseStoreOrig; }

protected:
	shared_ptr< IStore_ReadWrite<TestDataClass> > baseStoreOrig;
	shared_ptr< IStore_ReadWrite<TestDataClass> > nullStore;
};

static StoreEnvironment<TestDataClass> env;
TEST_GROUP(Store_SQLiteCache)
{
	void setup()
	{
		// Init stuff
#ifdef UseFileDB
		db.init(UseFileDB, SQLITE_OPEN_CREATE);

		// delete existing data
		{
		scoped_db_lock lock(db);
		std::string sql = std::string("DROP TABLE IF EXISTS ") + TestWrapper_Store_SQLite_Table::tableName;
		sqlite3_exec(lock.db, sql.c_str(), NULL, NULL, NULL);
		}
#else
		db.init(":memory:", SQLITE_OPEN_CREATE);
#endif

		// init SQLite store
		baseStorePtr = ConnectedVision::make_shared<TestWrapper_Store_SQLite>(env.configID, db);

		// init cache store
		cacheSize = 10;
		storePtr = ConnectedVision::make_shared<TestWrapper_Store_Cache>(env.configID, cacheSize, baseStorePtr);
	}

	void teardown()
	{
		// Uninit stuff
	}

	DBConnection db;
	shared_ptr< IStore_ReadWrite<TestDataClass> > baseStorePtr;
	shared_ptr<TestWrapper_Store_Cache> storePtr;
	int64_t cacheSize;
};

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//  general IStore tests
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

MIXIN_APPLY(Store_SQLiteCache, MixIn_IStore, IStore_tests)
{
	params.env = &env;
	params.store = boost::static_pointer_cast< IStore_ReadWrite<TestDataClass> >(storePtr);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//  specific Store_SQLiteCAche tests
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

IGNORE_TEST(Store_SQLiteCache, initStore)
{
	//////////////////////////////////////
	// test initialization

	//////////////////////////////////////
	// actual test
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	store.flush();

	FAIL("TODO LONGS_EQUAL( cacheSize, store.spy_cache().size() );");
}

IGNORE_TEST(Store_SQLiteCache, getByIndex_should_cache_the_object)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	//////////////////////////////////////
	// actual test

	// get data / fill cache
	size_t index = 0;
	auto data = store.getByIndex(index);
	CHECK_DATA_EQUAL( dataList[index], data );

	// disable DB access
	store.useCacheOnly();
	auto cachedData = store.getByIndex(index);
	CHECK_DATA_EQUAL(dataList[index], cachedData);
	CHECK_EQUAL(data, cachedData);
}

IGNORE_TEST(Store_SQLiteCache, getByIndexRange_should_cache_the_object)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	//////////////////////////////////////
	// actual test

	// get data / fill cache
	size_t start = 0;
	size_t end = (size_t)cacheSize -1;
	auto dataByRange = store.getByIndexRange(start, end);
	LONGS_EQUAL( (end + 1 - start), dataByRange.size() );
	for ( size_t i = 0; i < (end + 1 - start); ++i )
	{
		CHECK_DATA_EQUAL( dataList[start + i], dataByRange[i] );
	}

	// disable DB access
	store.useCacheOnly();
	auto cachedDataByRange = store.getByIndexRange(start, end);
	LONGS_EQUAL( (end + 1 - start), cachedDataByRange.size() );
	for ( size_t i = 0; i < (end + 1 - start); ++i )
	{
		CHECK_DATA_EQUAL( dataList[start + i], cachedDataByRange[i] );
		CHECK_EQUAL( dataByRange[i], cachedDataByRange[i] );
	}
}

TEST(Store_SQLiteCache, getByID_should_cache_the_object)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	//////////////////////////////////////
	// actual test

	// get data / fill cache
	size_t index = 0;
	id_t id = dataList[index]->getconst_id();
	auto data = store.getByID(id);
	CHECK_DATA_EQUAL( dataList[index], data );

	// disable DB access
	store.useCacheOnly();
	auto cachedData = store.getByID(id);
	CHECK_DATA_EQUAL( dataList[index], cachedData );
	CHECK_EQUAL( data, cachedData );
}

TEST(Store_SQLiteCache, getByID_cached_objects_have_priority_over_baseStore)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	//////////////////////////////////////
	// actual test

	// get data / fill cache
	size_t index = 0;
	id_t id = dataList[index]->getconst_id();
	auto data = store.getByID(id);
	CHECK_DATA_EQUAL( dataList[index], data );

	// by-pass cache and alter base store
	auto clone = data->copy();
	clone->set_timestamp( data->getconst_timestamp() + 5 );
	baseStorePtr->save_move(clone);

	// query cache again
	auto cachedData = store.getByID(id);
	CHECK_DATA_EQUAL( dataList[index], cachedData );
}

IGNORE_TEST(Store_SQLiteCache, getByTimestamp_should_cache_the_object)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	//////////////////////////////////////
	// actual test

	// get data / fill cache
	size_t index = 0;
	timestamp_t time = dataList[index]->getconst_timestamp();
	auto data = store.getByTimestamp(time);
	LONGS_EQUAL( 1, data.size() );
	CHECK_DATA_EQUAL( dataList[index], data[0] );

	// disable DB access
	store.useCacheOnly();
	auto cachedData = store.getByTimestamp(time);
	LONGS_EQUAL( 1, cachedData.size() );
	CHECK_DATA_EQUAL( dataList[index], cachedData[0] );
	CHECK_EQUAL( data[0], cachedData[0] );
}

IGNORE_TEST(Store_SQLiteCache, getBeforeTimestamp_should_cache_the_object)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	//////////////////////////////////////
	// actual test

	// get data / fill cache
	size_t index = 0;
	timestamp_t time = dataList[index]->getconst_timestamp() +1;
	auto data = store.getBeforeTimestamp(time);
	LONGS_EQUAL( 1, data.size() );
	CHECK_DATA_EQUAL( dataList[index], data[0] );

	// disable DB access
	store.useCacheOnly();
	auto cachedData = store.getBeforeTimestamp(time);
	LONGS_EQUAL( 1, cachedData.size() );
	CHECK_DATA_EQUAL( dataList[index], cachedData[0] );
	CHECK_EQUAL( data[0], cachedData[0] );
}

IGNORE_TEST(Store_SQLiteCache, getAfterTimestamp_should_cache_the_object)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	//////////////////////////////////////
	// actual test

	// get data / fill cache
	size_t index = 0;
	timestamp_t time = dataList[index]->getconst_timestamp() -1;
	auto data = store.getAfterTimestamp(time);
	LONGS_EQUAL( 1, data.size() );
	CHECK_DATA_EQUAL( dataList[index], data[0] );

	// disable DB access
	store.useCacheOnly();
	auto cachedData = store.getAfterTimestamp(time);
	LONGS_EQUAL( 1, cachedData.size() );
	CHECK_DATA_EQUAL( dataList[index], cachedData[0] );
	CHECK_EQUAL( data[0], cachedData[0] );
}

IGNORE_TEST(Store_SQLiteCache, getAllInTimespan_should_cache_the_object)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	//////////////////////////////////////
	// actual test

	// get data / fill cache
	size_t index = 0;
	timestamp_t time = dataList[index]->getconst_timestamp();
	auto data = store.getAllInTimespan(time-1, time);
	LONGS_EQUAL( 1, data.size() );
	CHECK_DATA_EQUAL( dataList[index], data[0] );

	// disable DB access
	store.useCacheOnly();
	auto cachedData = store.getAllInTimespan(time-1, time);
	LONGS_EQUAL( 1, cachedData.size() );
	CHECK_DATA_EQUAL( dataList[index], cachedData[0] );
	CHECK_EQUAL( data[0], cachedData[0] );
}

IGNORE_TEST(Store_SQLiteCache, getByTimestamp_should_return_all_objects_of_the_timestamp_not_just_the_cached_ones)
{
	//////////////////////////////////////
	// test initialization
	timestamp_t time = env.startTime;

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	std::vector< shared_ptr<const TestDataClass> > dataList;
	for (int i = 0; i < (cacheSize + 10); i++ )
	{
		auto tmp = env.makeConstData(i, time, env.configID);
		store.save_const(tmp);
		dataList.push_back(tmp);
	}
	store.flush();

	//////////////////////////////////////
	// actual test

	auto data = store.getByTimestamp(time);
	CHECK_DATAVECTOR_EQUAL( dataList, data );
}

IGNORE_TEST(Store_SQLiteCache, getBeforeTimestamp_should_return_all_objects_of_the_timestamp_not_just_the_cached_ones)
{
	//////////////////////////////////////
	// test initialization
	timestamp_t time = env.startTime;

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	std::vector< shared_ptr<const TestDataClass> > dataList;
	for (int i = 0; i < (cacheSize + 10); i++ )
	{
		auto tmp = env.makeConstData(i, time, env.configID);
		store.save_const(tmp);
		dataList.push_back(tmp);
	}
	store.flush();

	//////////////////////////////////////
	// actual test

	auto data = store.getBeforeTimestamp(time+1);
	CHECK_DATAVECTOR_EQUAL( dataList, data );
}

IGNORE_TEST(Store_SQLiteCache, getAfterTimestamp_should_return_all_objects_of_the_timestamp_not_just_the_cached_ones)
{
	//////////////////////////////////////
	// test initialization
	timestamp_t time = env.startTime;

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	std::vector< shared_ptr<const TestDataClass> > dataList;
	for (int i = 0; i < (cacheSize + 10); i++ )
	{
		auto tmp = env.makeConstData(i, time, env.configID);
		store.save_const(tmp);
		dataList.push_back(tmp);
	}
	store.flush();

	//////////////////////////////////////
	// actual test

	auto data = store.getAfterTimestamp(time-1);
	CHECK_DATAVECTOR_EQUAL( dataList, data );
}

IGNORE_TEST(Store_SQLiteCache, getByIndexRange_should_return_all_object_not_just_the_cached_ones)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	//////////////////////////////////////
	// actual test

	// get data / fill cache
	auto data = store.getByIndexRange( 0, dataList.size() );
	CHECK_DATAVECTOR_EQUAL( dataList, data );
}

IGNORE_TEST(Store_SQLiteCache, getAllInTimespan_should_return_all_object_not_just_the_cached_ones)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	//////////////////////////////////////
	// actual test

	// get data / fill cache
	auto data = store.getAllInTimespan( dataList.front()->getconst_timestamp(), dataList.back()->getconst_timestamp() );
	CHECK_DATAVECTOR_EQUAL( dataList, data );
}

TEST(Store_SQLiteCache, save_const_caches_objects)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	shared_ptr<const TestDataClass> saveDataConst = boost::static_pointer_cast<const TestDataClass>(env.makeData(100, 0, env.configID));

	//////////////////////////////////////
	// actual test

	// save
	store.save_const(saveDataConst);

	// check cache
	store.useCacheOnly();
	auto cachedData = store.getByID( saveDataConst->getconst_id() );
	CHECK_DATA_EQUAL( saveDataConst, cachedData );
}

TEST(Store_SQLiteCache, save_move_caches_objects)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	shared_ptr<const TestDataClass> saveDataConst = env.makeConstData(100, 0, env.configID);
	shared_ptr<TestDataClass> saveData = saveDataConst->copy();

	//////////////////////////////////////
	// actual test

	// save
	store.save_move(saveData);

	// check cache
	store.useCacheOnly();
	auto cachedData = store.getByID( saveDataConst->getconst_id() );
	CHECK_DATA_EQUAL( saveDataConst, cachedData );
}

TEST(Store_SQLiteCache, save_copy_caches_objects)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	shared_ptr<const TestDataClass> saveDataConst = env.makeConstData(100, 0, env.configID);
	shared_ptr<TestDataClass> saveData = saveDataConst->copy();

	//////////////////////////////////////
	// actual test

	// save
	store.save_copy(saveData);

	// check cache
	store.useCacheOnly();
	auto cachedData = store.getByID( saveDataConst->getconst_id() );
	CHECK_DATA_EQUAL( saveDataConst, cachedData );
}

TEST(Store_SQLiteCache, save_constVector_caches_objects)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	std::vector<shared_ptr<const TestDataClass> > saveList;
	saveList.push_back( dataList[0] );

	//////////////////////////////////////
	// actual test

	// save
	store.save_constVector(saveList);

	// check cache
	store.useCacheOnly();
	auto cachedData = store.getByID( saveList[0]->getconst_id() );
	CHECK_DATA_EQUAL( saveList[0], cachedData );
}

TEST(Store_SQLiteCache, save_const_write_through_to_base_store)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	shared_ptr<const TestDataClass> saveDataConst = env.makeConstData(100, 0, env.configID);

	//////////////////////////////////////
	// actual test

	// set write through
	store.spy_write_through = true;

	// save
	store.save_const(saveDataConst);

	// check base store
	auto data = baseStorePtr->getByID( saveDataConst->getconst_id() );
	CHECK_DATA_EQUAL( saveDataConst, data );

	// check cache
	auto cachedData = store.getByID( saveDataConst->getconst_id() );
	CHECK_DATA_EQUAL( saveDataConst, cachedData );
}

TEST(Store_SQLiteCache, save_const_do_not_write_through_to_base_store)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	shared_ptr<const TestDataClass> saveDataConst = env.makeConstData(100, 0, env.configID);

	//////////////////////////////////////
	// actual test

	// set write through
	store.spy_write_through = false;

	// save
	store.save_const(saveDataConst);

	// check base store
	auto data = baseStorePtr->getByID( saveDataConst->getconst_id() );
	CHECK_FALSE( data );

	// check cache
	auto cachedData = store.getByID( saveDataConst->getconst_id() );
	CHECK_DATA_EQUAL( saveDataConst, cachedData );
}

TEST(Store_SQLiteCache, save_constVector_write_through_to_base_store)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	shared_ptr<const TestDataClass> saveDataConst = env.makeConstData(100, 0, env.configID);
	std::vector< shared_ptr<const TestDataClass> > saveList;
	saveList.push_back( saveDataConst );

	//////////////////////////////////////
	// actual test

	// set write through
	store.spy_write_through = true;

	// save
	store.save_constVector(saveList);

	// check base store
	auto data = baseStorePtr->getByID( saveDataConst->getconst_id() );
	CHECK_DATA_EQUAL( saveDataConst, data );

	// check cache
	auto cachedData = store.getByID( saveDataConst->getconst_id() );
	CHECK_DATA_EQUAL( saveDataConst, cachedData );
}

TEST(Store_SQLiteCache, save_constVector_do_not_write_through_to_base_store)
{
	//////////////////////////////////////
	// test initialization

	// test store
	cacheSize = 2;
	TestWrapper_Store_Cache store(env.configID, cacheSize, baseStorePtr);
	auto dataList = env.fillStoreWithData(store, (size_t)cacheSize + 10, env.configID);
	store.flush();

	shared_ptr<const TestDataClass> saveDataConst = env.makeConstData(100, 0, env.configID);
	std::vector< shared_ptr<const TestDataClass> > saveList;
	saveList.push_back( saveDataConst );

	//////////////////////////////////////
	// actual test

	// set write through
	store.spy_write_through = false;

	// save
	store.save_constVector(saveList);

	// check base store
	auto data = baseStorePtr->getByID( saveDataConst->getconst_id() );
	CHECK_FALSE( data );

	// check cache
	auto cachedData = store.getByID( saveDataConst->getconst_id() );
	CHECK_DATA_EQUAL( saveDataConst, cachedData );
}

}} // namespace scope

max_stars_repo_name: ConnectedVision/ConnectedVision

max_issues_repo_name: ConnectedVision/ConnectedVision

max_forks_repo_name: ConnectedVision/ConnectedVision


text:
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>

#include <opencv2/opencv.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>

#include "imgProc.hpp"
#include "sfm.hpp"
#include "tapl/pte/ptEngine.hpp"

inline double deg2rad(double deg) { return deg * M_PI / 180.0; }
inline double rad2deg(double rad) { return rad * 180.0 / M_PI; }

/**
 * Estimate four possible R and T from the Essential Matrix
 */
std::vector<Eigen::MatrixXd> computeInitialRTfromE(const Eigen::MatrixXd &E) {

    // SVD decomposition
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(E, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // build 'Z' and 'W'
    Eigen::MatrixXd Z(3,3);
    Z << 0, 1, 0,
        -1, 0, 0,
         0, 0, 0;
    Eigen::MatrixXd W(3,3);
    W << 0, -1, 0,
         1,  0, 0,
         0,  0, 1;

    // E can be re-written as E=MQ; where M=U.Z.U^T and Q=U.W.V^T or Q=U.W^T.V^T
    Eigen::MatrixXd M = svd.matrixU() * Z * svd.matrixU().transpose();
    Eigen::MatrixXd Q1 = svd.matrixU() * W * svd.matrixV().transpose();
    Eigen::MatrixXd Q2 = svd.matrixU() * W.transpose() * svd.matrixV().transpose();

    // R can be computed as R=(det Q)Q
    Eigen::MatrixXd R1 = Q1.determinant() * Q1;
    Eigen::MatrixXd R2 = Q2.determinant() * Q2;

    // E = UΣV^T, T is simply either u3 or −u3, where u3 is the third column vector of U
    Eigen::MatrixXd T1 = svd.matrixU().col(2);
    Eigen::MatrixXd T2 = -svd.matrixU().col(2);

    // compose four possible RT
    std::vector<Eigen::MatrixXd> RT;
    // R1 and T1
    Eigen::MatrixXd R1T1(3,4);
    R1T1.block<3,3>(0,0) = R1;
    R1T1.block<3,1>(0,3) = T1;
    RT.push_back(R1T1);
    // R1 and T2
    Eigen::MatrixXd R1T2(3,4);
    R1T2.block<3,3>(0,0) = R1;
    R1T2.block<3,1>(0,3) = T2;
    RT.push_back(R1T2);
    // R2 and T1
    Eigen::MatrixXd R2T1(3,4);
    R2T1.block<3,3>(0,0) = R2;
    R2T1.block<3,1>(0,3) = T1;
    RT.push_back(R2T1);
    // R2 and T2
    Eigen::MatrixXd R2T2(3,4);
    R2T2.block<3,3>(0,0) = R2;
    R2T2.block<3,1>(0,3) = T2;
    RT.push_back(R2T2);

    // return
    return RT;
}

/**
 * Linear estimate of the 3d point
 */
tapl::Point3d tapl::cve::StructureFromMotion::linearEstimate3dPt(
                    const std::vector<tapl::Point2d> &point2d,
                    const std::vector<Eigen::MatrixXd> &projectionMatrices ) {
    // We can re-write [p x MP = 0] in the form [AP = 0] and solve for P
    // by decomposing A using SVD
    // let's formulate the A matrix
    Eigen::MatrixXd A(point2d.size()*2, 4);
    for (auto i=0; i<point2d.size(); ++i) {
        A.row(i*2) << (point2d.at(i).x * projectionMatrices.at(i).row(2)) -
                                    projectionMatrices.at(i).row(0);
        A.row((i*2)+1) << (point2d.at(i).y * projectionMatrices.at(i).row(2)) -
                                    projectionMatrices.at(i).row(1);
    }
    // SVD decomposition
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    // P can be obtained as the last column of v or last row of v_transpose
    Eigen::MatrixXd pt3d_homogeneous = svd.matrixV().col(3);
    // homogeneous to cartesian coordinate conversion
    tapl::Point3d pt3d( pt3d_homogeneous(0,0) / pt3d_homogeneous(3,0),
                        pt3d_homogeneous(1,0) / pt3d_homogeneous(3,0),
                        pt3d_homogeneous(2,0) / pt3d_homogeneous(3,0) );
    return pt3d;
}

/**
 * Non-linear estimate of the 3d point
 */
tapl::Point3d tapl::cve::StructureFromMotion::nonLinearEstimate3dPt(
                    const std::vector<tapl::Point2d> &point2d,
                    const std::vector<Eigen::MatrixXd> &projectionMatrices,
                    std::pair<std::vector<float>,std::vector<float>> &reprojectionErrors,
                    const uint16_t nIterations,
                    const float reprErrorThresh ) {
    // compute linear estimate of the 3d point
    tapl::Point3d pt3d_initial = linearEstimate3dPt(point2d, projectionMatrices);
    // perform non-linear least squares optimization
    tapl::Point3d pt3d_optimize;
    reprojectionErrors = \
        gnOptim.optimize(pt3d_initial, point2d, projectionMatrices, nIterations, reprErrorThresh, pt3d_optimize);
    return pt3d_optimize;
}

/**
 * Estimate R, T, and triengulated points
 */
std::pair<Eigen::MatrixXd, std::vector<tapl::Point3d>>
    tapl::cve::StructureFromMotion::computeSFM(
                    const Eigen::MatrixXd &E,
                    const std::vector<std::vector<tapl::Point2d>> &points2d,
                    const Eigen::MatrixXd &projectionMat1,
                    const float &maxReprojectionErr ) {
    // compute four possible RT
    std::vector<Eigen::MatrixXd> RT = computeInitialRTfromE(E);
    // projection matrices
    std::vector<Eigen::MatrixXd> projectionMatrices(2);
    projectionMatrices.at(0) = projectionMat1;
    // set of triangulated 3d points
    std::vector<std::vector<tapl::Point3d>> pointsTriangulatedRef(4, std::vector<tapl::Point3d>(points2d.size()));
    std::vector<std::vector<tapl::Point3d>> pointsTriangulated(4, std::vector<tapl::Point3d>(points2d.size()));
    // reprojection errors
    std::vector<std::vector<std::pair<std::vector<float>,std::vector<float>>>>
        reprojectionErrors(4, std::vector<std::pair<std::vector<float>,std::vector<float>>>(points2d.size()));
    // iterate over 4 possible R & T
    for (auto it1=RT.begin(); it1!= RT.end(); ++it1) {
        auto rtIdx = std::distance(RT.begin(), it1);
        // build projection matrix
        Eigen::MatrixXd projectionMat2 = projectionMat1 * (*it1);
        // TODO: Update the implementation so that we have 1 projection matrix per camera
        projectionMatrices.at(1) = projectionMat2;
        // go through each point and triangulate it through multiple-views
        for (auto it2=points2d.begin(); it2!=points2d.end(); ++it2) {
            auto ptIdx = std::distance(points2d.begin(), it2);
            // triangulate 3d point
            std::pair<std::vector<float>,std::vector<float>> reprojectionError;
            auto pt3d = this->nonLinearEstimate3dPt(*it2, projectionMatrices, reprojectionError);
            reprojectionErrors[rtIdx][ptIdx] = reprojectionError;
            pointsTriangulatedRef[rtIdx][ptIdx] = pt3d;

            // convert to homogeneous coordinate system
            Eigen::MatrixXd point3dHomogeneous(4,1);
            point3dHomogeneous << pt3d.x, pt3d.y, pt3d.z, 1.0;
            auto ptsTransformed = (*it1) * point3dHomogeneous;
            pointsTriangulated[rtIdx][ptIdx] = *(new tapl::Point3d(ptsTransformed(0), ptsTransformed(1), ptsTransformed(2)));
        }
    }

    // for each RT, count number of points that fall in front of all cameras.
    // the one with maximum number of points will correspond to the correct RT
    size_t correctIdx = 0;
    uint16_t maxCount = 0;
    for (auto i=0; i<RT.size(); ++i) {
        uint16_t count = 0;
        for (auto j=0; j<points2d.size(); ++j) {
            if ((pointsTriangulatedRef[i][j].z > 0.0) &&
                (pointsTriangulated[i][j].z > 0.0)) {
                   count ++;
               }
        }
        if (count > maxCount) {
            maxCount = count;
            correctIdx = i;
        }
    }

    Eigen::MatrixXd correctRT = RT[correctIdx];
    std::vector<tapl::Point3d> correctTriangulatedPtsRef = pointsTriangulatedRef[correctIdx];
    std::vector<tapl::Point3d> correctTriangulatedPts = pointsTriangulated[correctIdx];

    // use PnP to get refined pose
    // get pose using ransac
    cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
    cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);
    const int iterationsCount = 500;       // number of Ransac iterations. default 100
    const float reprojectionError = 1.0;    // maximum allowed distance to consider it an inlier. default 8.0
    const float confidence = 0.99;          // RANSAC successful confidence. default 0.99
    const bool useExtrinsicGuess = true;   // default false
    const int flags = cv::SOLVEPNP_ITERATIVE;
    cv::Mat inliers;
    std::vector<cv::Point2f> kpts2d_;
    std::vector<cv::Point3f> kpts3d_;
    std::vector<uint16_t> idxRemove;
    for (auto i=0; i<points2d.size(); ++i) {
        // filter out the points outside of region-of-interest and the points with high reprojection error
        if ((correctTriangulatedPts[i].z > 0.0) && (correctTriangulatedPtsRef[i].z > 0.0) &&
            (correctTriangulatedPtsRef[i].z > this->minXYZ.at(2)) && (correctTriangulatedPtsRef[i].z < this->maxXYZ.at(2)) &&
            (fabs(correctTriangulatedPtsRef[i].x) > this->minXYZ.at(0)) && (fabs(correctTriangulatedPtsRef[i].x) < this->maxXYZ.at(0)) &&
            (fabs(correctTriangulatedPtsRef[i].y) > this->minXYZ.at(1)) && (fabs(correctTriangulatedPtsRef[i].y) < this->maxXYZ.at(1)) &&
            reprojectionErrors[correctIdx][i].second.at(0) < maxReprojectionErr ) { // post-optimization reprojection error in the first camera frame

            // compute epipolar lines
            Eigen::MatrixXf pt2(3,1);
            pt2 << points2d[i][1].x, points2d[i][1].y, 1.0;
            Eigen::MatrixXf epipolarLine1 = (E.transpose().cast <float> ()) * pt2.cast <float> ();
            epipolarLine1 = epipolarLine1 / epipolarLine1(2,0); // normalize
            // compute distance between epipolar line to the point in the other image
            auto dist2ep = fabs(static_cast<float>(epipolarLine1(0,0)) * points2d[i][0].x +
                                static_cast<float>(epipolarLine1(1,0)) * points2d[i][0].y +
                                static_cast<float>(epipolarLine1(2,0))) /
                            sqrt(pow(static_cast<float>(epipolarLine1(0,0)), 2) +
                                 pow(static_cast<float>(epipolarLine1(1,0)), 2));
            // if (dist2ep < 50.0) {
            // points in the camera plane for which the pose needs to be computed
            kpts2d_.push_back(cv::Point2f(points2d[i][1].x, points2d[i][1].y));
            // 3d points in the coordinate of reference camera frame
            kpts3d_.push_back(cv::Point3f(correctTriangulatedPtsRef[i].x, correctTriangulatedPtsRef[i].y, correctTriangulatedPtsRef[i].z));
            // }
            // else idxRemove.push_back(i);
        }
        else idxRemove.push_back(i);
    }
    // remove outliers
    for (auto it_idx=idxRemove.end()-1; it_idx!=idxRemove.begin()-1; --it_idx) {
        correctTriangulatedPtsRef.erase(correctTriangulatedPtsRef.begin()+(*it_idx));
    }
    if(kpts3d_.size() >= 6) {
        cv::solvePnPRansac(kpts3d_, kpts2d_, this->K, this->dist_coeff, rvec, tvec,
                            useExtrinsicGuess, iterationsCount, reprojectionError, confidence,
                            inliers, flags );
        float inliers_ratio = static_cast<float>(inliers.size().height) / static_cast<float>(kpts3d_.size());
        // TLOG_DEBUG << "Inliers ratio [" << kpts3d_.size() << "/" << inliers.size().height << "] = [" << inliers_ratio << "]";

        // get global pose in world reference frame
        // check for NaNs
        rvec.convertTo(rvec, CV_32F);
        tvec.convertTo(tvec, CV_32F);
        cv::patchNaNs(rvec, 0.0); // replace NaN with 0.0
        cv::patchNaNs(tvec, 0.0); // replace NaN with 0.0
        // rotation matrix
        cv::Mat R;
        cv::Rodrigues(rvec, R);
        // translation matrix
        cv::Mat t = cv::Mat::eye(4,4, CV_32FC1);
        tvec.copyTo(t(cv::Rect(3,0,1,3)));
        // build an RT matrix
        Eigen::MatrixXd R_eigen(3,3);
        cv2eigen(R, R_eigen);
        Eigen::MatrixXd RT_pnp(3,4);
        RT_pnp.block<3,3>(0,0) << R_eigen;
        RT_pnp.block<3,1>(0,3) << tvec.at<float>(0,0), tvec.at<float>(0,1), tvec.at<float>(0,2);

        return std::pair<Eigen::MatrixXd, std::vector<tapl::Point3d>>(RT_pnp, correctTriangulatedPtsRef);
    }
    else {
        TLOG_WARN << "PnP failed";
        return std::pair<Eigen::MatrixXd, std::vector<tapl::Point3d>>(correctRT, correctTriangulatedPtsRef);
    }
}

/**
 * Structure-from-Motion constructor
 */
tapl::cve::StructureFromMotion::StructureFromMotion(
                                     const std::vector<cv::Mat> &images,
                                     const cv::Mat &K,
                                     const std::vector<float> &minXYZ,
                                     const std::vector<float> &maxXYZ,
                                     const bool verbose) {
    // copy inputs to private variable
    this->images = images;
    this->K = K;
    this->minXYZ = minXYZ;
    this->maxXYZ = maxXYZ;
    this->verbose = verbose;
    // make bundles of 'm' images
}

/**
 * Structure-from-Motion implementation
 */
tapl::ResultCode tapl::cve::StructureFromMotion::process(
                                std::vector<tapl::Point3dColor> &points,
                                std::vector<tapl::Pose6dof> &poses,
                                std::vector<tapl::CameraPairs> &framePairs) {

    // global pose
    Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
    // Go through each camera frame
    for (auto it=this->images.begin()+1; it!=this->images.end(); ++it) {
        if (this->verbose) TLOG_INFO << "processing frames [" <<
                                        std::distance(images.begin(), it) <<
                                        "] and [" <<
                                        std::distance(images.begin(), it+1) << "]";

        tapl::CameraPairs camPairs(*(it-1), *it, this->K);

        // compute fundamental matrix
        if (tapl::cve::computeFundamentalMatrix(camPairs) != tapl::SUCCESS) {
            TLOG_ERROR << "could not compute fundamental matrix";
            return tapl::FAILURE;
        }

        // retrieve fundamental matrix
        cv::Mat F;
        if (camPairs.getFundamentalMatrix(F) !=  tapl::SUCCESS) {
            TLOG_ERROR << "could not retrieve fundamental matrix";
            return tapl::FAILURE;
        }

        // compute essential matrix
        cv::Mat E = this->K.t() * F * this->K;

        // get keypoints
        std::vector<cv::KeyPoint> kpts1;
        if (camPairs.first->getKeypoints(kpts1) !=  tapl::SUCCESS) {
            TLOG_ERROR << "could not retrieve keypoints";
            return tapl::FAILURE;
        }
        std::vector<cv::KeyPoint> kpts2;
        if (camPairs.second->getKeypoints(kpts2) !=  tapl::SUCCESS) {
            TLOG_ERROR << "could not retrieve keypoints";
            return tapl::FAILURE;
        }

        // get keypoints matches
        std::vector<cv::DMatch> kptMatches;
        if (camPairs.getKptsMatches(kptMatches) !=  tapl::SUCCESS) {
            TLOG_ERROR << "could not retrieve keypoints matches";
            return tapl::FAILURE;
        }

        // build vector of 'n' points in 'm' camera frames (n x m - Point2d)
        std::vector<std::vector<tapl::Point2d>> kptSet;
        std::vector<tapl::Point2d> trackedKpts;
        for (auto it_match=kptMatches.begin(); it_match!=kptMatches.end(); ++it_match) {
            if (it_match->distance < 100.0) {
                tapl::Point2d pt1 = tapl::Point2d( kpts1.at((*it_match).queryIdx).pt.x,
                                                   kpts1.at((*it_match).queryIdx).pt.y );
                tapl::Point2d pt2 = tapl::Point2d( kpts2.at((*it_match).trainIdx).pt.x,
                                                   kpts2.at((*it_match).trainIdx).pt.y );
                trackedKpts.push_back(pt1);
                // TODO: compute corresponding lines, find and reject outliers

                // this point in all cameras
                std::vector<tapl::Point2d> ptCameras = {pt1, pt2};
                kptSet.push_back(ptCameras);
            }
        }

        // draw the matches
        cv::Mat matchImg;
        cv::drawMatches(*(it-1), kpts1, *(it), kpts2,
                        kptMatches, matchImg,
                        cv::Scalar::all(-1), cv::Scalar::all(-1),
                        std::vector<char>(), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

        // convert E to eigen format
        Eigen::MatrixXd eigenE(3,3);
        cv2eigen(E, eigenE);
        // convert K to eigen format
        Eigen::MatrixXd eigenK(3,3);
        Eigen::MatrixXd projectionMat1(3,4);
        cv2eigen(this->K, eigenK);
        projectionMat1.block<3,3>(0,0) = eigenK;
        projectionMat1(3,3) = 1.0;
        auto sfm = computeSFM(eigenE, kptSet, projectionMat1);
        Eigen::MatrixXd RT = sfm.first;
        std::vector<tapl::Point3d> triangulatedPts = sfm.second;
        // Get point color
        std::vector<tapl::Point3dColor> triangulatedPtsColor;
        cv::Mat imgProj = cv::Mat::zeros((it-1)->rows, (it-1)->cols, CV_8UC3);
        for ( auto &pt3d : triangulatedPts) {
            Eigen::MatrixXd eigenPt3d(3,1);
            eigenPt3d << pt3d.x, pt3d.y, pt3d.z;
            auto eigenPt2dHomogeneous = eigenK * eigenPt3d;
            float pxX, pxY;
            pxX = eigenPt2dHomogeneous(0,0) / eigenPt2dHomogeneous(2,0);
            pxY = eigenPt2dHomogeneous(1,0) / eigenPt2dHomogeneous(2,0);
            // check if the coordinate is within limits
            cv::Vec3b rgb;
            if ((pxX >= 0) && (pxX < (it-1)->cols) &&
                (pxY >= 0) && (pxY < (it-1)->rows)) {
                rgb = (it-1)->at<cv::Vec3b>(static_cast<int>(pxY),static_cast<int>(pxX));
                imgProj.at<cv::Vec3b>(static_cast<int>(pxY),static_cast<int>(pxX)) = rgb;
            }
            else rgb = cv::Vec3b(0, 0, 0);
            tapl::Point3dColor pt3dColor( pt3d.x, pt3d.y, pt3d.z, static_cast<uint8_t>(rgb[0]),
                                          static_cast<uint8_t>(rgb[1]), static_cast<uint8_t>(rgb[2]));
            triangulatedPtsColor.push_back(pt3dColor);
        }

        // plot
        Eigen::Matrix4f P = Eigen::Matrix4f::Identity();
        P.block<3,4>(0,0) = RT.cast<float>();
        Eigen::Matrix4f Pi = Eigen::Matrix4f::Identity();
        Pi.block<3,3>(0,0) = P.block<3,3>(0,0).transpose();
        Pi.block<3,1>(0,3) = -P.block<3,3>(0,0).transpose() * P.block<3,1>(0,3);
        globalPose = globalPose * Pi;

        // compute pose
        cv::Mat cvPose;
        eigen2cv(globalPose, cvPose);
        tapl::Pose6dof pose(cvPose);
        // add to camera
        camPairs.pushPose(pose);
        camPairs.pushTrackedKpts(trackedKpts);
        camPairs.pushTriangulatedPts(triangulatedPtsColor);
        // push to the output vector
        poses.push_back(pose);
        for (auto &point : triangulatedPtsColor) points.push_back(point);
        // push camera pairs
        framePairs.push_back(camPairs);
    }

    // return success
    return tapl::SUCCESS;
}

max_stars_repo_name: towardsautonomy/TAPL

max_issues_repo_name: towardsautonomy/TAPL

max_forks_repo_name: towardsautonomy/TAPL