mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-17 04:15:44 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/dai: Add code to extract calibration of gray stereo cameras

Browse source
This commit is contained in:
Jakob Bornecrantz 2021-10-09 18:31:38 +01:00
parent 8c242c1bc1
commit b8e2cbd9ee
1 changed files with 91 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

91
src/xrt/drivers/depthai/depthai_driver.cpp
View file

@ -19,6 +19,8 @@
#include "util/u_logging.h"
#include "util/u_trace_marker.h"
#include "tracking/t_tracking.h"
#include "depthai_interface.h"
#include "depthai/depthai.hpp"
@ -118,6 +120,81 @@ struct depthai_fs
*
*/
static void
depthai_get_gray_cameras_calibration(struct depthai_fs *depthai, struct t_stereo_camera_calibration **c_ptr)
{
/*
* Read out values.
*/
std::vector<std::vector<float>> extrinsics = {};
struct
{
std::vector<std::vector<float>> intrinsics = {};
std::vector<float> distortion = {};
int width = -1, height = -1;
} left, right = {};
/*
* Get data.
*/
dai::CalibrationHandler calibData = depthai->device->readCalibration();
std::tie(left.intrinsics, left.width, left.height) =
calibData.getDefaultIntrinsics(dai::CameraBoardSocket::LEFT);
std::tie(right.intrinsics, right.width, right.height) =
calibData.getDefaultIntrinsics(dai::CameraBoardSocket::RIGHT);
left.distortion = calibData.getDistortionCoefficients(dai::CameraBoardSocket::LEFT);
right.distortion = calibData.getDistortionCoefficients(dai::CameraBoardSocket::RIGHT);
extrinsics = calibData.getCameraExtrinsics(dai::CameraBoardSocket::LEFT, dai::CameraBoardSocket::RIGHT);
/*
* Copy to the Monado calibration struct.
*/
const uint32_t num_dist = 14;
struct t_stereo_camera_calibration *c = NULL;
t_stereo_camera_calibration_alloc(&c, num_dist);
// Copy intrinsics
c->view[0].image_size_pixels.w = left.width;
c->view[0].image_size_pixels.h = left.height;
c->view[1].image_size_pixels.w = right.width;
c->view[1].image_size_pixels.h = right.height;
for (uint32_t row = 0; row < 3; row++) {
for (uint32_t col = 0; col < 3; col++) {
c->view[0].intrinsics[row][col] = left.intrinsics[row][col];
c->view[1].intrinsics[row][col] = right.intrinsics[row][col];
}
}
// Copy distortion
c->view[0].use_fisheye = false;
c->view[1].use_fisheye = false;
for (uint32_t i = 0; i < num_dist; i++) {
c->view[0].distortion[i] = left.distortion[i];
c->view[1].distortion[i] = right.distortion[i];
}
// Copy translation
for (uint32_t i = 0; i < 3; i++) {
c->camera_translation[i] = extrinsics[i][3];
}
// Copy rotation
for (uint32_t row = 0; row < 3; row++) {
for (uint32_t col = 0; col < 3; col++) {
c->camera_rotation[row][col] = extrinsics[row][col];
}
}
// To properly handle ref counting.
t_stereo_camera_calibration_reference(c_ptr, c);
t_stereo_camera_calibration_reference(&c, NULL);
}
static void
depthai_print_connected_cameras(struct depthai_fs *depthai)
{
@ -130,6 +207,19 @@ depthai_print_connected_cameras(struct depthai_fs *depthai)
DEPTHAI_DEBUG(depthai, "DepthAI: Connected cameras: %s", str.c_str());
}
static void
depthai_print_calib(struct depthai_fs *depthai)
{
struct t_stereo_camera_calibration *c = NULL;
depthai_get_gray_cameras_calibration(depthai, &c);
if (depthai->ll <= U_LOGGING_DEBUG) {
t_stereo_camera_calibration_dump(c);
}
t_stereo_camera_calibration_reference(&c, NULL);
}
static void
depthai_do_one_frame(struct depthai_fs *depthai)
{
@ -404,6 +494,7 @@ depthai_fs_single_rgb(struct xrt_frame_context *xfctx)
// Some debug printing.
depthai_print_connected_cameras(depthai);
depthai_print_calib(depthai);
// Make sure that the thread helper is initialised.
os_thread_helper_init(&depthai->play_thread);