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

Fix using calibration data for undistortion and printing framerate

Browse source 
Signed-off-by: Jacki <jacki@thejackimonster.de>
This commit is contained in:
Jacki 2025-06-05 04:01:51 +02:00
parent 6314c867ca
commit a609d6b945
No known key found for this signature in database
GPG key ID: B404184796354C5E
1 changed files with 82 additions and 79 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -83,33 +83,15 @@ struct CameraCalibration {
};
};
const float fx = 239.9401992353717F;
const float fy = 240.1440084390103F;
const float cx = 244.88379123391678F;
const float cy = 315.48779284276173F;
const float k1 = -0.00023167964822506118F;
const float k2 = +0.0936140967533583F;
const float k3 = -0.12407217981118918F;
const float k4 = +0.07253328684439207F;
const float k5 = -0.02173257609236286F;
const float k6 = +0.0026589892415407887F;
const float p1 = +0.002702726113704367F;
const float p2 = -0.003645482535483966F;
const float s1 = -0.00747665631898417F;
const float s2 = -0.0003866048077984273F;
const float s3 = +0.007856725692160133F;
const float s4 = +0.0003370697673688772F;
typedef cv::Point2f (*apply_variant_func)(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.y = cam.fy * yn + cam.cy;
p.x = cam.fx * xn + cam.cx;
p.y = cam.fy * yn + cam.cy;
return p;
return p;
}
cv::Point2f apply_radial_only(const CameraCalibration& cam, float xn, float yn) {
@ -133,18 +115,18 @@ cv::Point2f apply_radial_only(const CameraCalibration& cam, float xn, float yn)
const float th2 = th * th;
const float th3 = th * th * th;
th_dist += k1 * th3;
th_dist += k2 * th2 * th3;
th_dist += k3 * th2 * th2 * th3;
th_dist += k4 * th3 * th3 * th3;
th_dist += k5 * th3 * th3 * th2 * th3;
th_dist += k6 * th3 * th3 * th2 * th2 * th3;
th_dist += cam.k1 * th3;
th_dist += cam.k2 * th2 * th3;
th_dist += cam.k3 * th2 * th2 * th3;
th_dist += cam.k4 * th3 * th3 * th3;
th_dist += cam.k5 * th3 * th3 * th2 * th3;
th_dist += cam.k6 * th3 * th3 * th2 * th2 * th3;
xn = th_dist * cp;
yn = th_dist * sp;
p.x = fx * xn + cx;
p.y = fy * yn + cy;
p.x = cam.fx * xn + cam.cx;
p.y = cam.fy * yn + cam.cy;
return p;
}
@ -171,26 +153,26 @@ cv::Point2f apply_radial_tangential(const CameraCalibration& cam, float xn, floa
const float th2 = th * th;
const float th3 = th * th * th;
th_dist += k1 * th3;
th_dist += k2 * th2 * th3;
th_dist += k3 * th2 * th2 * th3;
th_dist += k4 * th3 * th3 * th3;
th_dist += k5 * th3 * th3 * th2 * th3;
th_dist += k6 * th3 * th3 * th2 * th2 * th3;
th_dist += cam.k1 * th3;
th_dist += cam.k2 * th2 * th3;
th_dist += cam.k3 * th2 * th2 * th3;
th_dist += cam.k4 * th3 * th3 * th3;
th_dist += cam.k5 * th3 * th3 * th2 * th3;
th_dist += cam.k6 * th3 * th3 * th2 * th2 * th3;
xn = th_dist * cp;
yn = th_dist * sp;
r = xn * xn + yn * yn;
dx = (2.0F * xn * xn + r) * p1 + 2.0F * xn * yn * p2;
dy = (2.0F * yn * yn + r) * p2 + 2.0F * xn * yn * p1;
dx = (2.0F * xn * xn + r) * cam.p1 + 2.0F * xn * yn * cam.p2;
dy = (2.0F * yn * yn + r) * cam.p2 + 2.0F * xn * yn * cam.p1;
xn += dx;
yn += dy;
p.x = fx * xn + cx;
p.y = fy * yn + cy;
p.x = cam.fx * xn + cam.cx;
p.y = cam.fy * yn + cam.cy;
return p;
}
@ -219,66 +201,72 @@ cv::Point2f apply_fisheye624(const CameraCalibration& cam, float xn, float yn) {
const float th5 = th3 * th2;
const float th6 = th3 * th3;
th_dist += k1 * th3;
th_dist += k2 * th5;
th_dist += k3 * th5 * th2;
th_dist += k4 * th6 * th3;
th_dist += k5 * th6 * th5;
th_dist += k6 * th6 * th5 * th2;
th_dist += cam.k1 * th3;
th_dist += cam.k2 * th2 * th3;
th_dist += cam.k3 * th2 * th2 * th3;
th_dist += cam.k4 * th3 * th3 * th3;
th_dist += cam.k5 * th3 * th3 * th2 * th3;
th_dist += cam.k6 * th3 * th3 * th2 * th2 * th3;
xn = th_dist * cp;
yn = th_dist * sp;
r = xn * xn + yn * yn;
dx = (2.0F * xn * xn + r) * p1 + 2.0F * xn * yn * p2;
dy = (2.0F * yn * yn + r) * p2 + 2.0F * xn * yn * p1;
dx = (2.0F * xn * xn + r) * cam.p1 + 2.0F * xn * yn * cam.p2;
dy = (2.0F * yn * yn + r) * cam.p2 + 2.0F * xn * yn * cam.p1;
dx += (s1 + s2 * r) * r;
dy += (s3 + s4 * r) * r;
dx += (cam.s1 + cam.s2 * r) * r;
dy += (cam.s3 + cam.s4 * r) * r;
xn += dx;
yn += dy;
p.x = fx * xn + cx;
p.y = fy * yn + cy;
p.x = cam.fx * xn + cam.cx;
p.y = cam.fy * yn + cam.cy;
return p;
}
cv::Mat build_maps_variant(apply_variant_func func, const CameraCalibration& cam) {
cv::Mat map = cv::Mat(cv::Size(cam.resolution.width, cam.resolution.height), CV_16SC2);
cv::Mat build_maps_variant(apply_variant_func func, const CameraCalibration& cam, cv::Mat& indices) {
const cv::Size map_size (cam.resolution.width, cam.resolution.height);
cv::Mat map_x (map_size, CV_32FC1);
cv::Mat map_y (map_size, CV_32FC1);
cv::Point2f p;
cv::Vec2s v;
size_t x, y;
cv::Mat map (map_size, CV_16SC2);
indices = cv::Mat(map_size, CV_16UC1);
for (y = 0; y < map.rows; y++) {
for (x = 0; x < map.cols; x++) {
const float xn = (x - cx) / fx;
const float yn = (y - cy) / fy;
const float xn = (x - cam.cx) / cam.fx;
const float yn = (y - cam.cy) / cam.fy;
p = func(cam, xn, yn);
v[0] = static_cast<short>(p.x);
v[1] = static_cast<short>(p.y);
map.at<cv::Vec2s>(y, x) = v;
map_x.at<float>(y, x) = p.x;
map_y.at<float>(y, x) = p.y;
}
}
cv::convertMaps(map_x, map_y, map, indices, CV_16SC2);
return map;
}
void remap(const cv::Mat &input, cv::Mat &output, const cv::Mat &map) {
cv::Vec2s v;
size_t x, y;
for (y = 0; y < input.rows; y++) {
for (x = 0; x < input.cols; x++) {
v = map.at<cv::Vec2s>(y, x);
output.at<uint8_t>(y, x) = input.at<uint8_t>(v[1], v[0]);
}
cv::Mat build_maps(const CameraCalibration& cam, cv::Mat& indices) {
if (cam.num_kc >= 12) {
return build_maps_variant(apply_fisheye624, cam, indices);
} else
if (cam.num_kc >= 8) {
return build_maps_variant(apply_radial_tangential, cam, indices);
} else
if (cam.num_kc >= 6) {
return build_maps_variant(apply_radial_only, cam, indices);
} else {
return build_maps_variant(apply_intrinsic_only, cam, indices);
}
}
@ -350,7 +338,7 @@ int main(int argc, const char **argv) {
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);
cv::Ptr<cv::StereoBM> stereo = nullptr; // cv::StereoBM::create(16, 15);
cv::Ptr<cv::StereoBM> stereo = nullptr; //*/ cv::StereoBM::create(16, 15);
cv::VideoCapture cap;
cv::Mat frame;
@ -367,18 +355,35 @@ int main(int argc, const char **argv) {
cv::Mat left = cv::Mat(camera_res, img.type());
cv::Mat right = cv::Mat(camera_res, img.type());
cv::Mat map0_fisheye624 = build_maps_variant(apply_fisheye624, cam[0]);
cv::Mat map1_fisheye624 = build_maps_variant(apply_fisheye624, cam[1]);
cv::Mat indices0;
cv::Mat indices1;
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 undist_left = cv::Mat(left.size(), left.type());
cv::Mat undist_right = cv::Mat(right.size(), right.type());
std::time_t time_start = std::time(nullptr);
std::time_t time_current;
uint32_t frames = 0;
while (true) {
cap.read(frame);
if (frame.empty()) {
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) {
continue;
}
@ -434,9 +439,8 @@ int main(int argc, const char **argv) {
continue;
}
// cv::remap(left, undist, map_intrinsic, cv::noArray(), cv::INTER_LINEAR);
remap(left, undist_left, map0_fisheye624);
remap(right, undist_right, map1_fisheye624);
cv::remap(left, undist_left, map0_fisheye624, indices0, cv::INTER_LINEAR);
cv::remap(right, undist_right, map1_fisheye624, indices1, cv::INTER_LINEAR);
if (stereo) {
cv::Mat disparity;
@ -444,13 +448,12 @@ int main(int argc, const char **argv) {
stereo->compute(undist_left, undist_right, disparity);
disparity.convertTo(disp8, CV_8U, 255.0f / (16.0f * 16.0f));
disparity.convertTo(disp8, CV_8U, 1.0f / stereo->getNumDisparities());
cv::imshow("Live", disp8);
} else {
cv::Mat row;
cv::hconcat(left, right, row);
cv::hconcat(undist_left, undist_right, row);
cv::imshow("Live", row);