mirror of
https://github.com/storytold/artcraft.git
synced 2026-11-12 21:02:14 +00:00
working intrinsics/extrinsics !
This commit is contained in:
Brandon Thomas committed
1 parent
8016a4d24d
commit
68d3e4dc39
3 files changed
+202
-9
No files matched your search
@@ -2,22 +2,23 @@ use anyhow::Result as AnyhowResult;
|
||||
use kinect::capture::device_capturer::CaptureProvider;
|
||||
use point_cloud::debug::capture_proxy::CaptureProxy;
|
||||
use point_cloud::debug::image_proxy::ImageProxy;
|
||||
use kinect::k4a_sys_wrapper::ImageFormat;
|
||||
use kinect::k4a_sys_wrapper::{ImageFormat, Calibration};
|
||||
|
||||
pub struct FakeDeviceCaptureProvider {
|
||||
num_cameras: usize,
|
||||
captures: Vec<CaptureProxy>,
|
||||
calibration: Calibration,
|
||||
}
|
||||
|
||||
impl FakeDeviceCaptureProvider {
|
||||
pub fn new() -> AnyhowResult<Self> {
|
||||
// TODO: Color dimensions are wrong
|
||||
let color = ImageProxy::from_file("output/color_frame_0", 0, 0, 0, ImageFormat::ColorBgra32)?;
|
||||
// TODO: Color dimension stride is wrong (and maybe others)
|
||||
let color = ImageProxy::from_file("output/color_frame_0", 3840, 2160, 0, ImageFormat::ColorBgra32)?;
|
||||
let depth = ImageProxy::from_file("output/depth_frame_0", 640, 576, 1280, ImageFormat::Depth16)?;
|
||||
let frame_1 = CaptureProxy::from_image_proxy_pair(color, depth);
|
||||
|
||||
// TODO: Color dimensions are wrong
|
||||
let color = ImageProxy::from_file("output/color_frame_1", 0, 0, 0, ImageFormat::ColorBgra32)?;
|
||||
// TODO: Color dimension stride is wrong (and maybe others)
|
||||
let color = ImageProxy::from_file("output/color_frame_1", 3840, 2160, 0, ImageFormat::ColorBgra32)?;
|
||||
let depth = ImageProxy::from_file("output/depth_frame_1", 640, 576, 1280, ImageFormat::Depth16)?;
|
||||
let frame_2 = CaptureProxy::from_image_proxy_pair(color, depth);
|
||||
|
||||
@@ -25,13 +26,144 @@ impl FakeDeviceCaptureProvider {
|
||||
captures.push(frame_1);
|
||||
captures.push(frame_2);
|
||||
|
||||
let mut calibration = Calibration::default();
|
||||
|
||||
// NB: Experimentally determined "constants" from real devices
|
||||
calibration.0.color_resolution = 5;
|
||||
calibration.0.depth_mode = 2;
|
||||
calibration.0.depth_camera_calibration.resolution_width = 640;
|
||||
calibration.0.depth_camera_calibration.resolution_height = 576;
|
||||
calibration.0.depth_camera_calibration.metric_radius = 1.7399998;
|
||||
calibration.0.depth_camera_calibration.extrinsics.rotation = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0];
|
||||
calibration.0.depth_camera_calibration.extrinsics.translation = [0.0, 0.0, 0.0];
|
||||
calibration.0.depth_camera_calibration.intrinsics.type_ = 4;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameter_count = 14;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameters.param.cx = 325.4652;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameters.param.cy = 339.75952;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameters.param.fx = 503.72073;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameters.param.fy = 503.82776;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameters.param.k1 = 5.7966313;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameters.param.k2 = 4.1091304;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameters.param.k3 = 0.22066948;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameters.param.k4 = 6.1226273;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameters.param.k5 = 6.053665;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameters.param.k6 = 1.1403816;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameters.param.codx = 0.0;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameters.param.cody = 0.0;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameters.param.p2 = -0.00003910405;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameters.param.p1 = -0.00009726079;
|
||||
calibration.0.depth_camera_calibration.intrinsics.parameters.param.metric_radius = 0.0;
|
||||
|
||||
calibration.0.color_camera_calibration.resolution_width = 3840;
|
||||
calibration.0.color_camera_calibration.resolution_height = 2160;
|
||||
calibration.0.color_camera_calibration.metric_radius = 1.7;
|
||||
calibration.0.color_camera_calibration.extrinsics.rotation = [0.99998903, -0.0045318827, 0.001187322, 0.0044161947, 0.9964592, 0.08396203, -0.001563624, -0.08395587, 0.99646825];
|
||||
calibration.0.color_camera_calibration.extrinsics.translation = [-32.02683, -2.3786376, 3.9904723];
|
||||
calibration.0.color_camera_calibration.intrinsics.type_ = 4;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameter_count = 14;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameters.param.cx = 1919.9363;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameters.param.cy = 1105.1877;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameters.param.fx = 1832.5886;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameters.param.fy = 1832.6873;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameters.param.k1 = 0.4812507;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameters.param.k2 = -2.5299993;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameters.param.k3 = 1.4068391;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameters.param.k4 = 0.36012942;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameters.param.k5 = -2.355542;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameters.param.k6 = 1.3372931;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameters.param.codx = 0.0;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameters.param.cody = 0.0;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameters.param.p2 = -0.00037526494;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameters.param.p1 = 0.00054938474;
|
||||
calibration.0.color_camera_calibration.intrinsics.parameters.param.metric_radius = 0.0;
|
||||
calibration.0.extrinsics = [
|
||||
[
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
|
||||
translation: [0.0, 0.0, 0.0]
|
||||
},
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [0.99998903, -0.0045318827, 0.001187322, 0.0044161947, 0.9964592, 0.08396203, -0.001563624, -0.08395587, 0.99646825],
|
||||
translation: [-32.02683, -2.3786376, 3.9904723]
|
||||
},
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [0.00908543, 0.1157082, -0.99324167, -0.9999575, -0.0005106917, -0.009206355, -0.001572491, 0.9932831, 0.11569864],
|
||||
translation: [0.0, 0.0, 0.0]
|
||||
},
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [0.004534903, 0.112131886, -0.993683, -0.9999893, -0.00040909834, -0.004609848, -0.00092342496, 0.99369323, 0.11212883],
|
||||
translation: [-51.077, 3.2191863, 1.8031791]
|
||||
}
|
||||
],
|
||||
[
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [0.99998903, 0.0044161947, -0.001563624, -0.0045318827, 0.9964592, -0.08395587, 0.001187322, 0.08396203, 0.99646825],
|
||||
translation: [32.043224, 2.560097, -3.7386374]
|
||||
},
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [1.0, 0.00000000026193447, 0.0, 0.00000000026193447, 1.0, -0.000000007450581, 0.0, -0.000000007450581, 1.0],
|
||||
translation: [0.0000038146973, 0.0, 0.0]
|
||||
},
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [0.007381657, 0.031944036, -0.99946237, -0.9999551, -0.005697875, -0.007567407, -0.005936545, 0.9994734, 0.03190054],
|
||||
translation: [4.300721, -32.00875, 2.0599582]
|
||||
},
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [0.0028468627, 0.028323233, -0.99959475, -0.99998194, -0.005210849, -0.0029956135, -0.005293583, 0.9995853, 0.028307885],
|
||||
translation: [-46.929596, -28.807508, 3.8983316]
|
||||
}
|
||||
],
|
||||
[
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [0.00908543, -0.9999575, -0.001572491, 0.1157082, -0.0005106917, 0.9932831, -0.99324167, -0.009206355, 0.11569864],
|
||||
translation: [0.0, 0.0, 0.0]
|
||||
},
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [0.007381657, -0.9999551, -0.005936545, 0.031944036, -0.005697875, 0.9994734, -0.99946237, -0.007567407, 0.03190054],
|
||||
translation: [-32.02683, -2.3786376, 3.9904723]
|
||||
},
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [0.99999994, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.99999994],
|
||||
translation: [0.0, 0.0, 0.0]
|
||||
},
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [0.99998313, 0.0045562233, -0.003596194, -0.0045539755, 0.99998945, 0.00063277053, 0.0035990402, -0.0006163829, 0.99999326],
|
||||
translation: [-51.077, 3.2191863, 1.8031791]
|
||||
}
|
||||
],
|
||||
[
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [0.004534903, -0.9999893, -0.00092342496, 0.112131886, -0.00040909834, 0.99369323, -0.993683, -0.004609848, 0.11212883],
|
||||
translation: [3.452446, 3.9368703, -50.941692]
|
||||
},
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [0.0028468627, -0.99998194, -0.005293583, 0.028323233, -0.005210849, 0.9995853, -0.99959475, -0.0029956135, 0.028307885],
|
||||
translation: [-28.652746, -2.717628, -47.10723]
|
||||
},
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [0.99998313, -0.0045539755, 0.0035990402, 0.0045562233, 0.99998945, -0.0006163829, -0.003596194, 0.00063277053, 0.99999326],
|
||||
translation: [51.08431, -2.9853227, -1.9888868]
|
||||
},
|
||||
k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [0.99999994, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.99999994],
|
||||
translation: [-0.0000038146973, 0.00000023841858, 0.00000011920929] }
|
||||
]
|
||||
];
|
||||
|
||||
Ok(Self {
|
||||
num_cameras: 2,
|
||||
captures,
|
||||
calibration,
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
impl FakeDeviceCaptureProvider {
|
||||
pub fn get_calibration(&self) -> &Calibration {
|
||||
&self.calibration
|
||||
}
|
||||
}
|
||||
|
||||
impl CaptureProvider for FakeDeviceCaptureProvider {
|
||||
fn get_num_cameras(&self) -> usize {
|
||||
self.num_cameras
|
||||
|
||||
@@ -281,7 +281,6 @@ pub struct Calibration(pub k4a_sys::k4a_calibration_t);
|
||||
|
||||
impl Calibration {
|
||||
pub fn default() -> Self {
|
||||
// TODO: k4a_sys::k4a_calibration_type_t_K4A_CALIBRATION_TYPE_UNKNOWN
|
||||
let extrinsics = k4a_sys::_k4a_calibration_extrinsics_t {
|
||||
rotation: [0.0f32, 0.0f32, 0.0f32, 0.0f32, 0.0f32, 0.0f32, 0.0f32, 0.0f32, 0.0f32],
|
||||
translation: [0.0f32, 0.0f32, 0.0f32],
|
||||
@@ -338,8 +337,66 @@ impl Calibration {
|
||||
depth_mode: 0,
|
||||
extrinsics: extrinsics_4_4,
|
||||
};
|
||||
|
||||
Self(calibration)
|
||||
}
|
||||
|
||||
pub fn debug_print(&self) {
|
||||
println!("===== CALIBRATION =====");
|
||||
println!("\t Color resolution: {}", self.0.color_resolution);
|
||||
println!("\t Depth mode: {}", self.0.depth_mode);
|
||||
println!("\t Extrinsics: {:?}", self.0.extrinsics);
|
||||
|
||||
println!("\t depth.resolution_width: {}", self.0.depth_camera_calibration.resolution_width);
|
||||
println!("\t depth.resolution_height: {}", self.0.depth_camera_calibration.resolution_height);
|
||||
println!("\t depth.metric_radius: {}", self.0.depth_camera_calibration.metric_radius);
|
||||
println!("\t depth.extrinsics: {:?}", self.0.depth_camera_calibration.extrinsics);
|
||||
println!("\t depth.intrinsics.type: {}", self.0.depth_camera_calibration.intrinsics.type_);
|
||||
println!("\t depth.intrinsics.parameter_count: {}", self.0.depth_camera_calibration.intrinsics.parameter_count);
|
||||
unsafe {
|
||||
// NB: This is a union field, so we have to use unsafe access
|
||||
println!("\t depth.intrinsics.parameters.param.cx: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.cx);
|
||||
println!("\t depth.intrinsics.parameters.param.cy: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.cy);
|
||||
println!("\t depth.intrinsics.parameters.param.fx: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.fx);
|
||||
println!("\t depth.intrinsics.parameters.param.fy: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.fy);
|
||||
println!("\t depth.intrinsics.parameters.param.k1: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.k1);
|
||||
println!("\t depth.intrinsics.parameters.param.k2: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.k2);
|
||||
println!("\t depth.intrinsics.parameters.param.k3: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.k3);
|
||||
println!("\t depth.intrinsics.parameters.param.k4: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.k4);
|
||||
println!("\t depth.intrinsics.parameters.param.k5: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.k5);
|
||||
println!("\t depth.intrinsics.parameters.param.k6: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.k6);
|
||||
println!("\t depth.intrinsics.parameters.param.codx: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.codx);
|
||||
println!("\t depth.intrinsics.parameters.param.cody: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.cody);
|
||||
println!("\t depth.intrinsics.parameters.param.p2: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.p2);
|
||||
println!("\t depth.intrinsics.parameters.param.p1: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.p1);
|
||||
println!("\t depth.intrinsics.parameters.param.metric_radius: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.metric_radius);
|
||||
}
|
||||
println!("\t color.resolution_width: {}", self.0.color_camera_calibration.resolution_width);
|
||||
println!("\t color.resolution_height: {}", self.0.color_camera_calibration.resolution_height);
|
||||
println!("\t color.metric_radius: {}", self.0.color_camera_calibration.metric_radius);
|
||||
println!("\t color.extrinsics: {:?}", self.0.color_camera_calibration.extrinsics);
|
||||
println!("\t color.intrinsics.type: {}", self.0.color_camera_calibration.intrinsics.type_);
|
||||
println!("\t color.intrinsics.parameter_count: {}", self.0.color_camera_calibration.intrinsics.parameter_count);
|
||||
unsafe {
|
||||
// NB: This is a union field, so we have to use unsafe access
|
||||
println!("\t color.intrinsics.parameters.param.cx: {}", self.0.color_camera_calibration.intrinsics.parameters.param.cx);
|
||||
println!("\t color.intrinsics.parameters.param.cy: {}", self.0.color_camera_calibration.intrinsics.parameters.param.cy);
|
||||
println!("\t color.intrinsics.parameters.param.fx: {}", self.0.color_camera_calibration.intrinsics.parameters.param.fx);
|
||||
println!("\t color.intrinsics.parameters.param.fy: {}", self.0.color_camera_calibration.intrinsics.parameters.param.fy);
|
||||
println!("\t color.intrinsics.parameters.param.k1: {}", self.0.color_camera_calibration.intrinsics.parameters.param.k1);
|
||||
println!("\t color.intrinsics.parameters.param.k2: {}", self.0.color_camera_calibration.intrinsics.parameters.param.k2);
|
||||
println!("\t color.intrinsics.parameters.param.k3: {}", self.0.color_camera_calibration.intrinsics.parameters.param.k3);
|
||||
println!("\t color.intrinsics.parameters.param.k4: {}", self.0.color_camera_calibration.intrinsics.parameters.param.k4);
|
||||
println!("\t color.intrinsics.parameters.param.k5: {}", self.0.color_camera_calibration.intrinsics.parameters.param.k5);
|
||||
println!("\t color.intrinsics.parameters.param.k6: {}", self.0.color_camera_calibration.intrinsics.parameters.param.k6);
|
||||
println!("\t color.intrinsics.parameters.param.codx: {}", self.0.color_camera_calibration.intrinsics.parameters.param.codx);
|
||||
println!("\t color.intrinsics.parameters.param.cody: {}", self.0.color_camera_calibration.intrinsics.parameters.param.cody);
|
||||
println!("\t color.intrinsics.parameters.param.p2: {}", self.0.color_camera_calibration.intrinsics.parameters.param.p2);
|
||||
println!("\t color.intrinsics.parameters.param.p1: {}", self.0.color_camera_calibration.intrinsics.parameters.param.p1);
|
||||
println!("\t color.intrinsics.parameters.param.metric_radius: {}", self.0.color_camera_calibration.intrinsics.parameters.param.metric_radius);
|
||||
}
|
||||
println!("==========");
|
||||
}
|
||||
}
|
||||
|
||||
/// Adapted from k4a-sys. Represents a capture.
|
||||
|
||||
+7
-3
@@ -27,7 +27,7 @@ use std::thread;
|
||||
|
||||
use opencv::prelude::*;
|
||||
|
||||
use kinect::k4a_sys_wrapper::Device;
|
||||
use kinect::k4a_sys_wrapper::{Device, Calibration};
|
||||
use kinect::sensor_control::{capture_thread, CaptureProvider};
|
||||
use kinect::capture::multi_device_capturer::{MultiDeviceCapturer, start_capture_thread};
|
||||
use kinect::capture::fake_device_capturer::FakeDeviceCaptureProvider;
|
||||
@@ -63,11 +63,15 @@ pub fn main() {
|
||||
let primary_device = &multi_device.primary_device;
|
||||
let calibration = primary_device.get_calibration(depth_mode, color_format).unwrap();
|
||||
|
||||
let capture_provider = multi_device.get_sync_capture_provider();
|
||||
calibration.debug_print();
|
||||
|
||||
//let capture_provider = multi_device.get_sync_capture_provider();
|
||||
|
||||
thread::spawn(move || start_capture_thread(multi_device));
|
||||
|
||||
//let capture_provider = Arc::new(FakeDeviceCaptureProvider::new().unwrap());
|
||||
//let calibration = Calibration::default();
|
||||
let capture_provider = Arc::new(FakeDeviceCaptureProvider::new().unwrap());
|
||||
let calibration = capture_provider.get_calibration().clone();
|
||||
|
||||
graphics_imgui::run(capture_provider, calibration, ENABLE_WEBCAM);
|
||||
}
|
||||
Reference in new issue
Block a user