Skip to content

Commit

Permalink
add get calib param function
Browse files Browse the repository at this point in the history
  • Loading branch information
Bvallon-sl committed Feb 15, 2022
1 parent 5dba171 commit b725e19
Show file tree
Hide file tree
Showing 3 changed files with 115 additions and 42 deletions.
24 changes: 24 additions & 0 deletions Source/Private/ZEDCamera.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,20 @@ bool ZEDCamera::ImportMethod_CreateCamera() {
return false;// Return an error.
}

bool ZEDCamera::ImportMethod_GetCalibParams() {
if (v_dllHandle != NULL)
{
m_funcGetCalibParams = NULL;
FString procName = "sl_get_calibration_parameters";// Needs to be the exact name of the DLL method.
m_funcGetCalibParams = (__CalibParams)FPlatformProcess::GetDllExport(v_dllHandle, *procName);
if (m_funcGetCalibParams != NULL)
{
return true;
}
}
return false;// Return an error.
}

bool ZEDCamera::ImportMethod_Open()
{
if (v_dllHandle != NULL)
Expand Down Expand Up @@ -193,6 +207,7 @@ bool ZEDCamera::LoadDll(FString DLLName)
{
ImportMethod_CreateCamera();
ImportMethod_Open();
ImportMethod_GetCalibParams();
ImportMethod_Close();
ImportMethod_GetSerialNumber();
ImportMethod_Grab();
Expand Down Expand Up @@ -237,6 +252,15 @@ sl::ERROR_CODE ZEDCamera::Open(SL_InitParameters& initParameters, const char* pa
return e;
}

SL_CalibrationParameters* ZEDCamera::GetCalibrationParameters(bool raw_params) {
if (m_funcGetCalibParams == NULL)
{
return new SL_CalibrationParameters();
}
SL_CalibrationParameters* e = (SL_CalibrationParameters*)m_funcGetCalibParams(camera_id, raw_params);
return e;
}

void ZEDCamera::Close() {
if (m_funcClose == NULL)
{
Expand Down
6 changes: 5 additions & 1 deletion Source/Public/ZEDCamera.h
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@

typedef int(*__ObjectData)(int id, SL_ObjectDetectionRuntimeParameters& od_params, SL_Objects& objs);
typedef int(*__Open)(int id, SL_InitParameters& initParameters, const char* pathSVO, const char* ip, int streamPort, const char* outputFile,
const char* opt_settings_path, const char* opencv_calib_path);
const char* opt_settings_path, const char* opencv_calib_path);
typedef void(*__Close)(int id);
typedef int(*__EnableTracking)(int id, SL_PositionalTrackingParameters tracking_param, const char* areaDBpath);
typedef void(*__DisableTracking)(int id, const char* path);
Expand All @@ -27,6 +27,7 @@ typedef int(*__GetPosition)(int id, PoseData& poseData, sl::REFERENCE_FRAME refe
typedef bool(*__CreateCamera)(int id, bool verbose);
typedef int(*__SN)(int sn);
typedef int(*__Grab)(int id, SL_RuntimeParameters& rt_params);
typedef SL_CalibrationParameters*(*__CalibParams)(int id, bool raw_params);


class ZEDCamera {
Expand All @@ -46,6 +47,7 @@ class ZEDCamera {
sl::POSITIONAL_TRACKING_STATE GetPosition(PoseData& poseData, sl::REFERENCE_FRAME reference_frame);
int GetSerialNumber();
sl::ERROR_CODE RetrieveObjects(SL_ObjectDetectionRuntimeParameters& od_params, SL_Objects& objs);
SL_CalibrationParameters* GetCalibrationParameters(bool raw_params = false);

private:
void *v_dllHandle;
Expand All @@ -60,6 +62,7 @@ class ZEDCamera {
__ObjectData m_funcRetrieveOD;
__Grab m_funcGrab;
__SN m_funcGetSN;
__CalibParams m_funcGetCalibParams;

int camera_id;

Expand All @@ -74,6 +77,7 @@ class ZEDCamera {
bool ImportMethod_GetPosition();
bool ImportMethod_GetSerialNumber();
bool ImportMethod_RetrieveObjects();
bool ImportMethod_GetCalibParams();

bool LoadDll(FString DLLName);
void UnloadDll();
Expand Down
127 changes: 86 additions & 41 deletions Source/Public/ZEDStructs.h
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,49 @@

#define MAX_NUMBER_OBJECT 75


struct SL_Quaternion {
float x;
float y;
float z;
float w;

SL_Quaternion() {
x = 0;
y = 0;
z = 0;
w = 1;
}
};

struct SL_Vector2
{
float x;
float y;
};

struct SL_Vector3
{
float x;
float y;
float z;

SL_Vector3() {
x = 0;
y = 0;
z = 0;
}
};

struct SL_Vector4
{
float x;
float y;
float z;
float w;
};


struct SL_InitParameters
{
sl::INPUT_TYPE input_type;
Expand Down Expand Up @@ -55,6 +98,49 @@ struct SL_InitParameters
}
};

/**
* \brief Resolution
*/
struct SL_Resolution {
long long width;
long long height;
};

struct SL_CameraParameters {
float fx; /**< Focal length in pixels along x axis. */
float fy; /**< Focal length in pixels along y axis. */
float cx; /**< Optical center along x axis, defined in pixels (usually close to width/2). */
float cy; /**< Optical center along y axis, defined in pixels (usually close to height/2). */
double disto[5]; /**< Distortion factor : [ k1, k2, p1, p2, k3 ]. Radial (k1,k2,k3) and Tangential (p1,p2) distortion.*/
float v_fov; /**< Vertical field of view, in degrees. */
float h_fov; /**< Horizontal field of view, in degrees.*/
float d_fov; /**< Diagonal field of view, in degrees.*/
SL_Resolution image_size; /** size in pixels of the images given by the camera.*/
};

/**
Holds calibration information about the current ZED's hardware, including per-sensor calibration and offsets between the two sensors.
*/
struct SL_CalibrationParameters
{
/**
Intrinsic parameters of the left camera
*/
struct SL_CameraParameters left_cam;
/**
Intrinsic parameters of the right camera
*/
struct SL_CameraParameters right_cam;
/**
Left to Right camera rotation, expressed in user coordinate system and unit (defined by InitParameters).
*/
SL_Vector4 rotation;
/**
Left to Right camera translation, expressed in user coordinate system and unit (defined by InitParameters).
*/
SL_Vector3 translation;
};

/*
Parameters that define the behavior of the grab.
*/
Expand Down Expand Up @@ -116,47 +202,6 @@ struct SL_ObjectDetectionRuntimeParameters

};

struct SL_Quaternion {
float x;
float y;
float z;
float w;

SL_Quaternion() {
x = 0;
y = 0;
z = 0;
w = 1;
}
};

struct SL_Vector2
{
float x;
float y;
};

struct SL_Vector3
{
float x;
float y;
float z;

SL_Vector3() {
x = 0;
y = 0;
z = 0;
}
};

struct SL_Vector4
{
float x;
float y;
float z;
float w;
};

struct SL_Pose {
sl::float3 position;
sl::float4 rotation;
Expand Down

0 comments on commit b725e19

Please sign in to comment.