UnrealXR/nrealAirLinuxDriver
Archived
2
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add code to rectify frames for stereo depth images

Browse source 
Signed-off-by: Jacki <jacki@thejackimonster.de>
This commit is contained in:
Jacki 2025-06-06 04:22:05 +02:00
parent 262df2523c
commit 682daf4385
No known key found for this signature in database
GPG key ID: B404184796354C5E
3 changed files with 160 additions and 64 deletions
Download patch file Download diff file Expand all files Collapse all files

179
examples/debug_cam/src/camera.cpp
View file

@ -22,6 +22,9 @@
// THE SOFTWARE. // THE SOFTWARE.
// //
#include <cstdint>
#include <cstring>
#include <opencv2/core/types.hpp>
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include <opencv2/videoio.hpp> #include <opencv2/videoio.hpp>
#include <stdio.h> #include <stdio.h>
@ -39,6 +42,8 @@ const uint8_t chunk_map[128] = {
92, 59, 104, 24, 15, 73, 65, 38, 58, 10, 23, 33, 55, 57, 107, 92, 59, 104, 24, 15, 73, 65, 38, 58, 10, 23, 33, 55, 57, 107,
100, 94, 27, 95, 45, 91, 4, 114}; 100, 94, 27, 95, 45, 91, 4, 114};
uint8_t reverse_chunk_map [128];
#define CHUNK_SIZE 2400 #define CHUNK_SIZE 2400
#define CHUNK_AMOUNT 128 #define CHUNK_AMOUNT 128
#define CHUNK_SUM_LEN 128 #define CHUNK_SUM_LEN 128
@ -51,6 +56,9 @@ const uint8_t chunk_map[128] = {
#define CAM_IMAGE_DATA_SIZE (CAM_HEIGHT * CAM_WIDTH) #define CAM_IMAGE_DATA_SIZE (CAM_HEIGHT * CAM_WIDTH)
struct CameraCalibration { struct CameraCalibration {
float position [3];
float rotation [9];
struct { struct {
size_t width; size_t width;
size_t height; size_t height;
@ -81,11 +89,18 @@ struct CameraCalibration {
float s4; float s4;
}; };
}; };
float fx_t;
float fy_t;
float cx_t;
float cy_t;
}; };
void calculate_camera_matrices(const CameraCalibration& cam, cv::Mat& K, cv::Mat& D) { void calculate_camera_matrices(const CameraCalibration& cam, cv::Mat& K, cv::Mat& D, cv::Mat& P, cv::Mat& R) {
K = cv::Mat::eye(3, 3, CV_32FC1); K = cv::Mat::eye(3, 3, CV_32FC1);
D = cv::Mat::zeros(1, 12, CV_32FC1); D = cv::Mat::zeros(1, 12, CV_32FC1);
P = cv::Mat::zeros(3, 1, CV_32FC1);
R = cv::Mat::eye(3, 3, CV_32FC1);
K.at<float>(0, 0) = cam.fx; K.at<float>(0, 0) = cam.fx;
K.at<float>(1, 1) = cam.fy; K.at<float>(1, 1) = cam.fy;
@ -112,6 +127,20 @@ void calculate_camera_matrices(const CameraCalibration& cam, cv::Mat& K, cv::Mat
D.at<float>(0, 10) = cam.s3; D.at<float>(0, 10) = cam.s3;
D.at<float>(0, 11) = cam.s4; D.at<float>(0, 11) = cam.s4;
} }
P.at<float>(0) = cam.position[0];
P.at<float>(1) = cam.position[1];
P.at<float>(2) = cam.position[2];
R.at<float>(0, 0) = cam.rotation[0];
R.at<float>(0, 1) = cam.rotation[1];
R.at<float>(0, 2) = cam.rotation[2];
R.at<float>(1, 0) = cam.rotation[3];
R.at<float>(1, 1) = cam.rotation[4];
R.at<float>(1, 2) = cam.rotation[5];
R.at<float>(2, 0) = cam.rotation[6];
R.at<float>(2, 1) = cam.rotation[7];
R.at<float>(2, 2) = cam.rotation[8];
} }
typedef cv::Point2f (*apply_variant_func)(const CameraCalibration& cam, float xn, float yn); typedef cv::Point2f (*apply_variant_func)(const CameraCalibration& cam, float xn, float yn);
@ -119,14 +148,13 @@ typedef cv::Point2f (*apply_variant_func)(const CameraCalibration& cam, float xn
cv::Point2f apply_intrinsic_only(const CameraCalibration& cam, float xn, float yn) { cv::Point2f apply_intrinsic_only(const CameraCalibration& cam, float xn, float yn) {
cv::Point2f p; cv::Point2f p;
p.x = cam.fx * xn + cam.cx; p.x = cam.fx_t * xn + cam.cx_t;
p.y = cam.fy * yn + cam.cy; p.y = cam.fy_t * yn + cam.cy_t;
return p; return p;
} }
cv::Point2f apply_radial_only(const CameraCalibration& cam, float xn, float yn) { cv::Point2f apply_radial_only(const CameraCalibration& cam, float xn, float yn) {
cv::Point2f p;
float r, sp, cp; float r, sp, cp;
float th, th_dist; float th, th_dist;
@ -156,14 +184,10 @@ cv::Point2f apply_radial_only(const CameraCalibration& cam, float xn, float yn)
xn = th_dist * cp; xn = th_dist * cp;
yn = th_dist * sp; yn = th_dist * sp;
p.x = cam.fx * xn + cam.cx; return apply_intrinsic_only(cam, xn, yn);
p.y = cam.fy * yn + cam.cy;
return p;
} }
cv::Point2f apply_radial_tangential(const CameraCalibration& cam, float xn, float yn) { cv::Point2f apply_radial_tangential(const CameraCalibration& cam, float xn, float yn) {
cv::Point2f p;
float r, sp, cp; float r, sp, cp;
float th, th_dist; float th, th_dist;
float dx, dy; float dx, dy;
@ -202,14 +226,10 @@ cv::Point2f apply_radial_tangential(const CameraCalibration& cam, float xn, floa
xn += dx; xn += dx;
yn += dy; yn += dy;
p.x = cam.fx * xn + cam.cx; return apply_intrinsic_only(cam, xn, yn);
p.y = cam.fy * yn + cam.cy;
return p;
} }
cv::Point2f apply_fisheye624(const CameraCalibration& cam, float xn, float yn) { cv::Point2f apply_fisheye624(const CameraCalibration& cam, float xn, float yn) {
cv::Point2f p;
float r, sp, cp; float r, sp, cp;
float th, th_dist; float th, th_dist;
float dx, dy; float dx, dy;
@ -253,10 +273,7 @@ cv::Point2f apply_fisheye624(const CameraCalibration& cam, float xn, float yn) {
xn += dx; xn += dx;
yn += dy; yn += dy;
p.x = cam.fx * xn + cam.cx; return apply_intrinsic_only(cam, xn, yn);
p.y = cam.fy * yn + cam.cy;
return p;
} }
cv::Mat build_maps_variant(apply_variant_func func, const CameraCalibration& cam, cv::Mat& indices) { cv::Mat build_maps_variant(apply_variant_func func, const CameraCalibration& cam, cv::Mat& indices) {
@ -305,6 +322,9 @@ int main(int argc, const char **argv) {
CameraCalibration cam [2]; CameraCalibration cam [2];
device_imu_type dev; device_imu_type dev;
memset(&(cam[0]), 0, sizeof(cam[0]));
memset(&(cam[1]), 0, sizeof(cam[1]));
const char *video_filename = "/dev/video0"; const char *video_filename = "/dev/video0";
if (argc > 1) { if (argc > 1) {
@ -330,6 +350,9 @@ int main(int argc, const char **argv) {
const device_imu_camera_sensor_type *sensor = device_imu_camera_get_sensor(camera, i); const device_imu_camera_sensor_type *sensor = device_imu_camera_get_sensor(camera, i);
const device_imu_vec3_type position = device_imu_sensor_get_position(sensor);
const device_imu_mat3x3_type rotation = device_imu_sensor_get_rotation(sensor);
const device_imu_size_type resolution = device_imu_sensor_get_resolution(sensor); const device_imu_size_type resolution = device_imu_sensor_get_resolution(sensor);
const device_imu_vec2_type cc = device_imu_sensor_get_cc(sensor); const device_imu_vec2_type cc = device_imu_sensor_get_cc(sensor);
@ -340,6 +363,9 @@ int main(int argc, const char **argv) {
continue; continue;
} }
memcpy(cam[i].position, (const float*) &(position), sizeof(float) * 3);
memcpy(cam[i].rotation, (const float*) &(rotation), sizeof(float) * 9);
cam[i].resolution.width = resolution.width; cam[i].resolution.width = resolution.width;
cam[i].resolution.height = resolution.height; cam[i].resolution.height = resolution.height;
@ -356,6 +382,12 @@ int main(int argc, const char **argv) {
} }
device_imu_sensor_get_kc(sensor, &num_kc, cam[i].kc); device_imu_sensor_get_kc(sensor, &num_kc, cam[i].kc);
cam[i].fx_t = cam[i].fx;
cam[i].fy_t = cam[i].fy;
cam[i].cx_t = cam[i].cx;
cam[i].cy_t = cam[i].cy;
} }
} }
@ -369,7 +401,14 @@ int main(int argc, const char **argv) {
const cv::Size sensor_res (cam[0].resolution.height, cam[0].resolution.width); const cv::Size sensor_res (cam[0].resolution.height, cam[0].resolution.width);
const cv::Size camera_res (cam[0].resolution.width, cam[0].resolution.height); const cv::Size camera_res (cam[0].resolution.width, cam[0].resolution.height);
cv::Ptr<cv::StereoBM> stereo = nullptr; //*/ cv::StereoBM::create(16, 15); const float sigmaColor = 50.0F;
const float sigmaSpace = 25.0F;
cv::Ptr<cv::StereoBM> stereo = nullptr;
if ((argc > 2) && (0 == strcmp(argv[2], "--depth"))) {
stereo = cv::StereoBM::create(16, 9);
}
cv::VideoCapture cap; cv::VideoCapture cap;
cv::Mat frame; cv::Mat frame;
@ -386,23 +425,41 @@ int main(int argc, const char **argv) {
cv::Mat left = cv::Mat(camera_res, img.type()); cv::Mat left = cv::Mat(camera_res, img.type());
cv::Mat right = cv::Mat(camera_res, img.type()); cv::Mat right = cv::Mat(camera_res, img.type());
for (size_t i = 0; i < CHUNK_AMOUNT; i++) {
reverse_chunk_map[chunk_map[i]] = i;
}
cv::Mat K [2]; cv::Mat K [2];
cv::Mat D [2]; cv::Mat D [2];
cv::Mat P [2];
cv::Mat R [2];
calculate_camera_matrices(cam[0], K[0], D[0]); calculate_camera_matrices(cam[0], K[0], D[0], P[0], R[0]);
calculate_camera_matrices(cam[1], K[1], D[1]); calculate_camera_matrices(cam[1], K[1], D[1], P[1], R[1]);
cv::Mat cR = R[0].inv() * R[1];
cv::Mat cT = P[1] - P[0];
cv::Mat Q;
cv::stereoRectify(K[0], D[0], K[1], D[1], camera_res, cR, cT, R[0], R[1], P[0], P[1], Q);
cv::Mat indices0; cv::Mat indices0;
cv::Mat indices1; cv::Mat indices1;
cam[0].cx_t -= P[0].at<double>(0, 3);
cam[0].cy_t -= P[0].at<double>(1, 3);
cam[1].cx_t -= P[1].at<double>(0, 3);
cam[1].cy_t -= P[1].at<double>(1, 3);
cv::Mat map0_fisheye624 = build_maps_variant(apply_fisheye624, cam[0], indices0); cv::Mat map0_fisheye624 = build_maps_variant(apply_fisheye624, cam[0], indices0);
cv::Mat map1_fisheye624 = build_maps_variant(apply_fisheye624, cam[1], indices1); cv::Mat map1_fisheye624 = build_maps_variant(apply_fisheye624, cam[1], indices1);
cv::Mat undist_left = cv::Mat(left.size(), left.type()); cv::Mat undist_left = cv::Mat(left.size(), left.type());
cv::Mat undist_right = cv::Mat(right.size(), right.type()); cv::Mat undist_right = cv::Mat(right.size(), right.type());
std::time_t time_start = std::time(nullptr); uint64_t t_last = 0;
std::time_t time_current;
uint32_t frames = 0; uint32_t frames = 0;
while (true) { while (true) {
@ -411,16 +468,6 @@ int main(int argc, const char **argv) {
break; break;
} }
time_current = std::time(nullptr);
frames++;
if (time_current > time_start) {
std::cout << (frames / 2) << " frames per second" << std::endl;
time_start = time_current;
frames = 0;
}
if (frame.cols < CAM_BUFFER_SIZE) { if (frame.cols < CAM_BUFFER_SIZE) {
continue; continue;
} }
@ -429,13 +476,26 @@ int main(int argc, const char **argv) {
uint8_t *data = img.ptr(); uint8_t *data = img.ptr();
const uint8_t *header = blocks + CAM_IMAGE_DATA_SIZE; const uint8_t *header = blocks + CAM_IMAGE_DATA_SIZE;
const size_t header_len = CAM_BUFFER_SIZE - CAM_IMAGE_DATA_SIZE;
uint64_t t_ns = *((const uint64_t *)(header + 0x00)); uint64_t t_eye = *((const uint64_t *)(header + 0x00));
uint64_t t_ms = *((const uint64_t *)(header + 0x3E)); uint64_t t_frame = *((const uint64_t *)(header + 0x3E));
uint16_t seq = *((const uint16_t *)(header + 0x12)); uint16_t seq = *((const uint16_t *)(header + 0x12));
uint32_t aperture = *((const uint32_t *)(header + 0x16));
uint16_t exposure = *((const uint16_t *)(header + 0x1A));
uint64_t number = *((const uint64_t *)(header + 0x33));
uint8_t is_right = *(header + 0x3B); uint8_t is_right = *(header + 0x3B);
frames++;
if ((t_frame - t_last) > 1000000000L) {
std::cout << (frames / 2) << " frames per second" << std::endl;
t_last = t_frame;
frames = 0;
}
size_t offset = 0; size_t offset = 0;
size_t sum = CHUNK_SUM_LEN * 0xFF + 1; size_t sum = CHUNK_SUM_LEN * 0xFF + 1;
@ -452,47 +512,52 @@ int main(int argc, const char **argv) {
} }
} }
for (size_t i = 0; i < CHUNK_AMOUNT; i++) { offset = reverse_chunk_map[offset];
if (chunk_map[i] == offset) {
offset = i;
break;
}
}
for (size_t i = 0; i < CHUNK_AMOUNT; i++) { for (size_t i = 0; i < CHUNK_AMOUNT; i++) {
size_t src_idx = CHUNK_SIZE * chunk_map[(offset + i) % CHUNK_AMOUNT]; const size_t src_idx = CHUNK_SIZE * chunk_map[(offset + i) % CHUNK_AMOUNT];
size_t dst_idx = CHUNK_SIZE * i; const size_t dst_idx = CHUNK_SIZE * i;
std::memcpy(data + dst_idx, blocks + src_idx, CHUNK_SIZE); std::memcpy(data + dst_idx, blocks + src_idx, CHUNK_SIZE);
} }
if (is_right) { if (is_right) {
cv::rotate(img, right, cv::ROTATE_90_CLOCKWISE); cv::Mat filtered;
} else {
cv::rotate(img, left, cv::ROTATE_90_COUNTERCLOCKWISE);
}
if ((left.empty()) || (right.empty())) { cv::bilateralFilter(img, filtered, 5, sigmaColor, sigmaSpace);
cv::rotate(filtered, right, cv::ROTATE_90_CLOCKWISE);
cv::remap(right, undist_right, map1_fisheye624, indices1, cv::INTER_LINEAR);
if (undist_left.empty()) {
continue; continue;
} }
} else {
cv::Mat filtered;
cv::bilateralFilter(img, filtered, 5, sigmaColor, sigmaSpace);
cv::rotate(filtered, left, cv::ROTATE_90_COUNTERCLOCKWISE);
cv::remap(left, undist_left, map0_fisheye624, indices0, cv::INTER_LINEAR); cv::remap(left, undist_left, map0_fisheye624, indices0, cv::INTER_LINEAR);
cv::remap(right, undist_right, map1_fisheye624, indices1, cv::INTER_LINEAR);
if (undist_right.empty()) {
continue;
}
}
{
cv::Mat row;
if (stereo) { if (stereo) {
cv::Mat disparity; cv::Mat disparity;
cv::Mat disp8;
stereo->compute(undist_left, undist_right, disparity); stereo->compute(undist_left, undist_right, disparity);
disparity.convertTo(row, CV_8U, 128.0f / (stereo->getNumDisparities() * 16.0f));
disparity.convertTo(disp8, CV_8U, 1.0f / (stereo->getNumDisparities() * 16.0f));
cv::imshow("Live", disp8);
} else { } else {
cv::Mat row;
cv::hconcat(undist_left, undist_right, row); cv::hconcat(undist_left, undist_right, row);
for (size_t i = 0; i < 640; i += 40) {
cv::line(row, cv::Point(0, i), cv::Point(960, i), cv::Scalar(255, 0, 0));
}
}
cv::imshow("Live", row); cv::imshow("Live", row);
} }

9
interface_lib/include/device_imu.h
View file

@ -124,6 +124,10 @@ struct device_imu_euler_t {
float yaw; float yaw;
}; };
struct device_imu_mat3x3_t {
float m [9];
};
struct device_imu_size_t { struct device_imu_size_t {
uint16_t width; uint16_t width;
uint16_t height; uint16_t height;
@ -144,6 +148,7 @@ typedef struct device_imu_vec2_t device_imu_vec2_type;
typedef struct device_imu_vec3_t device_imu_vec3_type; typedef struct device_imu_vec3_t device_imu_vec3_type;
typedef struct device_imu_quat_t device_imu_quat_type; typedef struct device_imu_quat_t device_imu_quat_type;
typedef struct device_imu_euler_t device_imu_euler_type; typedef struct device_imu_euler_t device_imu_euler_type;
typedef struct device_imu_mat3x3_t device_imu_mat3x3_type;
typedef struct device_imu_size_t device_imu_size_type; typedef struct device_imu_size_t device_imu_size_type;
typedef void (*device_imu_event_callback)( typedef void (*device_imu_event_callback)(
@ -203,6 +208,10 @@ uint32_t device_imu_camera_get_num_of_sensors(const device_imu_camera_type *came
const device_imu_camera_sensor_type* device_imu_camera_get_sensor(const device_imu_camera_type *camera, uint32_t index); const device_imu_camera_sensor_type* device_imu_camera_get_sensor(const device_imu_camera_type *camera, uint32_t index);
device_imu_mat3x3_type device_imu_sensor_get_rotation(const device_imu_camera_sensor_type *sensor);
device_imu_vec3_type device_imu_sensor_get_position(const device_imu_camera_sensor_type *sensor);
device_imu_size_type device_imu_sensor_get_resolution(const device_imu_camera_sensor_type *sensor); device_imu_size_type device_imu_sensor_get_resolution(const device_imu_camera_sensor_type *sensor);
device_imu_vec2_type device_imu_sensor_get_cc(const device_imu_camera_sensor_type *sensor); device_imu_vec2_type device_imu_sensor_get_cc(const device_imu_camera_sensor_type *sensor);

22
interface_lib/src/device_imu.c
View file

@ -1152,6 +1152,28 @@ const device_imu_camera_sensor_type* device_imu_camera_get_sensor(const device_i
return &(camera->sensors[index]); return &(camera->sensors[index]);
} }
device_imu_mat3x3_type device_imu_sensor_get_rotation(const device_imu_camera_sensor_type *sensor) {
device_imu_mat3x3_type rotation;
rotation.m[0] = sensor->cameraMisalignment.array[0][0];
rotation.m[1] = sensor->cameraMisalignment.array[0][1];
rotation.m[2] = sensor->cameraMisalignment.array[0][2];
rotation.m[3] = sensor->cameraMisalignment.array[1][0];
rotation.m[4] = sensor->cameraMisalignment.array[1][1];
rotation.m[5] = sensor->cameraMisalignment.array[1][2];
rotation.m[6] = sensor->cameraMisalignment.array[2][0];
rotation.m[7] = sensor->cameraMisalignment.array[2][1];
rotation.m[8] = sensor->cameraMisalignment.array[2][2];
return rotation;
}
device_imu_vec3_type device_imu_sensor_get_position(const device_imu_camera_sensor_type *sensor) {
device_imu_vec3_type position;
position.x = sensor->cameraOffset.axis.x;
position.y = sensor->cameraOffset.axis.y;
position.z = sensor->cameraOffset.axis.z;
return position;
}
device_imu_size_type device_imu_sensor_get_resolution(const device_imu_camera_sensor_type *sensor) { device_imu_size_type device_imu_sensor_get_resolution(const device_imu_camera_sensor_type *sensor) {
device_imu_size_type resolution; device_imu_size_type resolution;
resolution.width = sensor->resolution[0]; resolution.width = sensor->resolution[0];