per-device calibration handling (no longer distorted)

We were only using the primary device's calibration
This commit is contained in:
Brandon Thomas committed 2020-09-11 11:56:19 -04:00
1 parent 66e535d96f
commit c8d875c95d
6 files changed
+81 -43

No files matched your search

+2 -2
View File
@@ -16,7 +16,7 @@ use std::path::Path;
use std::sync::{Arc, Mutex, PoisonError, MutexGuard};
use std::time::{Instant, Duration};
pub fn run(capture_provider: Arc<CaptureProvider>, calibration_data: Calibration, program_args: ProgramArgs) -> AnyhowResult<()> {
pub fn run(capture_provider: Arc<CaptureProvider>, program_args: ProgramArgs) -> AnyhowResult<()> {
let mut webcam_writer = None;
if program_args.enable_webcam_output_writing {
@@ -63,9 +63,9 @@ pub fn run(capture_provider: Arc<CaptureProvider>, calibration_data: Calibration
let mut visualizer = PointCloudVisualizer::new(
capture_provider.get_num_cameras(),
capture_provider.get_calibrations(),
true,
colorization_strategy,
calibration_data,
clear_color,
sdl_arcball.clone(),
);
+2 -2
View File
@@ -12,6 +12,6 @@ pub trait CaptureProvider {
*/
fn get_captures(&self) -> Option<Vec<CaptureProxy>>;
/** Get device calibration (from one of the cameras). */
fn get_calibration(&self) -> &Calibration;
/** Get device calibrations for all cameras. */
fn get_calibrations(&self) -> &Vec<Calibration>;
}
+9 -4
View File
@@ -7,7 +7,7 @@ use crate::point_cloud::debug::image_proxy::ImageProxy;
pub struct FakeDeviceCaptureProvider {
num_cameras: usize,
captures: Vec<CaptureProxy>,
calibration: Calibration,
all_device_calibrations: Vec<Calibration>,
}
impl FakeDeviceCaptureProvider {
@@ -151,10 +151,15 @@ impl FakeDeviceCaptureProvider {
]
];
let mut all_device_calibrations = Vec::new();
for _ in 0 .. 2 {
all_device_calibrations.push(calibration.clone());
}
Ok(Self {
num_cameras: 2,
captures,
calibration,
all_device_calibrations,
})
}
}
@@ -171,7 +176,7 @@ impl CaptureProvider for FakeDeviceCaptureProvider {
Some(capture_clones)
}
fn get_calibration(&self) -> &Calibration {
&self.calibration
fn get_calibrations(&self) -> &Vec<Calibration> {
&self.all_device_calibrations
}
}
+24 -13
View File
@@ -11,13 +11,15 @@ pub struct MultiDeviceCapturer {
num_cameras: usize,
pub primary_device: Device, // TODO: Temporary public viz
secondary_devices: Vec<Device>,
calibration: Calibration,
/// Calibrations for primary + all secondaries in order.
all_device_calibrations: Vec<Calibration>,
capture_provider: Arc<MultiDeviceCaptureProvider>,
}
pub struct MultiDeviceCaptureProvider {
num_cameras: usize,
calibration: Calibration,
/// Calibrations for primary + all secondaries in order.
all_device_calibrations: Vec<Calibration>,
captures: Arc<Mutex<Vec<Capture>>>, // TODO: Shouldn't need to wrap in mutex if we wrap struct instead.
}
@@ -28,6 +30,10 @@ impl MultiDeviceCapturer {
pub fn new(num_devices: u32, limit_secondary_devices: Option<usize>) -> Result<Self, KinectError> {
let mut primary_device = None;
let mut secondary_devices = Vec::new();
let mut calibrations = Vec::new();
let depth_mode : k4a_sys::k4a_depth_mode_t = 2; //k4a_sys::K4A_DEPTH_MODE_NFOV_UNBINNED;
let color_format: k4a_sys::k4a_color_resolution_t = k4a_sys::k4a_color_resolution_t_K4A_COLOR_RESOLUTION_2160P;
for i in 0..num_devices {
let mut device = Device::open(i)?;
@@ -38,7 +44,12 @@ impl MultiDeviceCapturer {
// NB: This assumes a daisy chain topology.
panic!("We already one primary device. We can't have two.");
}
let calibration = device.get_calibration(depth_mode, color_format).unwrap();
calibrations.push(calibration);
primary_device = Some(device);
continue;
}
@@ -46,7 +57,10 @@ impl MultiDeviceCapturer {
panic!("Secondary device does not have 'in' jack connected!");
}
let calibration = device.get_calibration(depth_mode, color_format).unwrap();
secondary_devices.push(device);
calibrations.push(calibration);
}
if primary_device.is_none() {
@@ -59,19 +73,16 @@ impl MultiDeviceCapturer {
let num_cameras = 1 + secondary_devices.len();
let depth_mode : k4a_sys::k4a_depth_mode_t = 2; //k4a_sys::K4A_DEPTH_MODE_NFOV_UNBINNED;
let color_format: k4a_sys::k4a_color_resolution_t = k4a_sys::k4a_color_resolution_t_K4A_COLOR_RESOLUTION_2160P;
let primary = primary_device.expect("There must be a primary device");
let calibration = primary.get_calibration(depth_mode, color_format).unwrap();
let calibration2 = calibration.clone();
let calibrations2 = calibrations.clone();
Ok(Self {
num_cameras,
primary_device: primary,
secondary_devices,
calibration,
capture_provider: Arc::new(MultiDeviceCaptureProvider::new(num_cameras, calibration2)),
all_device_calibrations: calibrations,
capture_provider: Arc::new(MultiDeviceCaptureProvider::new(num_cameras, calibrations2)),
})
}
@@ -128,10 +139,10 @@ impl MultiDeviceCapturer {
}
impl MultiDeviceCaptureProvider {
pub fn new(num_cameras: usize, calibration: Calibration) -> Self {
pub fn new(num_cameras: usize, all_device_calibrations: Vec<Calibration>) -> Self {
Self {
num_cameras,
calibration,
all_device_calibrations,
captures: Arc::new(Mutex::new(Vec::new())),
}
}
@@ -169,8 +180,8 @@ impl CaptureProvider for MultiDeviceCaptureProvider {
.collect())
}
fn get_calibration(&self) -> &Calibration {
&self.calibration
fn get_calibrations(&self) -> &Vec<Calibration> {
&self.all_device_calibrations
}
}
+2 -2
View File
@@ -127,10 +127,10 @@ pub fn main() -> AnyhowResult<()> {
Arc::new(FakeDeviceCaptureProvider::new().unwrap())
};
let calibration = capture_provider.get_calibration().clone();
//let calibration = capture_provider.get_calibration().clone();
//calibration.debug_print();
graphics_imgui::run(capture_provider, calibration, program_args)?;
graphics_imgui::run(capture_provider, program_args)?;
Ok(())
}
+42 -20
View File
@@ -23,6 +23,7 @@ use crate::point_cloud::pixel_structs::DepthPixel;
use crate::point_cloud::point_cloud_renderer::{PointCloudRenderer, PointCloudRendererError};
use crate::point_cloud::util::{colorize_depth_blue_to_red, get_depth_mode_range, ValueRange};
use crate::point_cloud::viewer_image::ViewerImage;
use crate::kinect::sensor_control::CaptureProvider;
pub type Result<T> = std::result::Result<T, PointCloudVisualizerError>;
@@ -108,8 +109,15 @@ pub struct PointCloudVisualizer {
pub point_cloud_renderer: PointCloudRenderer,
pub point_cloud_converters: Vec<GpuPointCloudConverter>,
calibration_data: Calibration,
transformation: Transformation, // TODO: WAT k4a_sys::k4a_transformation_t
/// Calibration data for all cameras
all_device_calibrations: Vec<Calibration>,
/// Transformations are constructed from camera calibrations
transformations: Vec<Transformation>,
/// Near and far minima/maxima for the current depth sensor mode.
/// These are constructed for each camera from the camera calibrations.
expected_depth_value_ranges: Vec<ValueRange>,
frame_buffer: Framebuffer,
depth_buffer: Renderbuffer,
@@ -131,9 +139,6 @@ pub struct PointCloudVisualizer {
color_xy_tables: Vec<ImageProxy>,
depth_xy_tables: Vec<ImageProxy>,
/// Near and far minima/maxima for the current depth sensor mode.
depth_value_range: ValueRange,
/// Background clear color for OpenGL.
clear_color: RgbaF32,
@@ -160,9 +165,9 @@ impl PointCloudVisualizer {
/// CTOR
///
pub fn new(num_cameras: usize,
all_device_calibrations: &Vec<Calibration>,
enable_color_point_cloud: bool,
initial_colorization_strategy: ColorizationStrategy,
calibration_data: Calibration,
clear_color: RgbaF32,
arcball_camera: Arc<Mutex<MouseCameraArcball>>) -> Self
{
@@ -178,8 +183,17 @@ impl PointCloudVisualizer {
gl::RenderbufferStorage(gl::RENDERBUFFER, gl::DEPTH_COMPONENT, width, height);
}
let expected_value_range = get_depth_mode_range(calibration_data.0.depth_mode)
.expect("Should be in correct depth sensor mode.");
let mut expected_depth_value_ranges = Vec::new();
let mut transformations = Vec::new();
for calibration in all_device_calibrations {
let value_range = get_depth_mode_range(calibration.0.depth_mode)
.expect("Should be in correct depth sensor mode.");
expected_depth_value_ranges.push(value_range);
let transformation = Transformation::from_calibration(&calibration);
transformations.push(transformation);
}
let mut point_cloud_converters = Vec::with_capacity(num_cameras);
let mut transformed_depth_images = Vec::with_capacity(num_cameras);
@@ -189,21 +203,23 @@ impl PointCloudVisualizer {
let mut color_xy_tables = Vec::with_capacity(num_cameras);
let mut depth_xy_tables = Vec::with_capacity(num_cameras);
for _ in 0 .. num_cameras {
for i in 0 .. num_cameras {
point_cloud_converters.push(GpuPointCloudConverter::new());
transformed_depth_images.push(None);
point_cloud_colorizations.push(None);
last_captures.push(None);
xyz_textures.push(Texture::new());
let calibration = all_device_calibrations.get(i).unwrap();
// TODO: generate color xytable only on `enable_color_point_cloud`.
let color_xy_table = GpuPointCloudConverter::generate_xy_table(
&calibration_data,
&calibration,
k4a_sys::k4a_calibration_type_t_K4A_CALIBRATION_TYPE_COLOR,
).unwrap();
let depth_xy_table = GpuPointCloudConverter::generate_xy_table(
&calibration_data,
&calibration,
k4a_sys::k4a_calibration_type_t_K4A_CALIBRATION_TYPE_DEPTH,
).unwrap();
@@ -232,16 +248,16 @@ impl PointCloudVisualizer {
color_xy_tables,
depth_xy_tables,
colorization_strategy: initial_colorization_strategy,
calibration_data: calibration_data.clone(),
transformation: Transformation::from_calibration(&calibration_data),
transformations,
last_captures,
transformed_depth_images,
point_cloud_colorizations,
xyz_textures,
depth_value_range: expected_value_range,
expected_depth_value_ranges,
clear_color,
debug_static_color_frames,
debug_static_depth_frames,
all_device_calibrations: all_device_calibrations.clone(),
};
visualizer.set_colorization_strategy(initial_colorization_strategy).expect("Should work");
@@ -379,6 +395,8 @@ impl PointCloudVisualizer {
// Depth Image extractor
//
let transformation = self.transformations.get(camera_index).unwrap();
let mut depth_image = match capture.get_depth_image() {
Ok(img) => img,
Err(_e) => {
@@ -401,7 +419,7 @@ impl PointCloudVisualizer {
unsafe {
let result = k4a_sys::k4a_transformation_depth_image_to_color_camera(
self.transformation.get_handle(),
transformation.get_handle(),
depth_image.get_handle(),
transformed_depth_image.get_handle(),
);
@@ -449,6 +467,8 @@ impl PointCloudVisualizer {
// This creates a color spectrum based on depth.
let dst_length = use_depth_image.get_width_pixels() * use_depth_image.get_height_pixels();
let depth_value_range = self.expected_depth_value_ranges.get(camera_index).unwrap();
unsafe {
// src: DepthPixel
let src_pixel_buffer = use_depth_image.get_buffer();
@@ -466,7 +486,7 @@ impl PointCloudVisualizer {
for i in 0 .. dst_length as isize {
let src_pixel = *typed_src_pixel_buffer.offset(i);
let pixel = colorize_depth_blue_to_red(src_pixel, &self.depth_value_range);
let pixel = colorize_depth_blue_to_red(src_pixel, depth_value_range);
(*typed_dst_pixel_buffer.offset(i)).red = pixel.red;
(*typed_dst_pixel_buffer.offset(i)).green = pixel.green;
(*typed_dst_pixel_buffer.offset(i)).blue = pixel.blue;
@@ -493,12 +513,14 @@ impl PointCloudVisualizer {
return Err(PointCloudVisualizerError::UnsupportedMode);
}
let calibration = self.all_device_calibrations.get(camera_index).unwrap();
self.colorization_strategy = strategy;
self.point_cloud_renderer.set_enable_shading(strategy == ColorizationStrategy::Shaded);
if strategy == ColorizationStrategy::Color {
let width = self.calibration_data.0.color_camera_calibration.resolution_width;
let height = self.calibration_data.0.color_camera_calibration.resolution_height;
let width = calibration.0.color_camera_calibration.resolution_width;
let height = calibration.0.color_camera_calibration.resolution_height;
let stride = width * size_of::<DepthPixel>() as i32;
// TODO: TEMP MULTI-CAMERA SUPPORT
@@ -516,8 +538,8 @@ impl PointCloudVisualizer {
}
} else {
let width = self.calibration_data.0.depth_camera_calibration.resolution_width as u32;
let height = self.calibration_data.0.depth_camera_calibration.resolution_height as u32;
let width = calibration.0.depth_camera_calibration.resolution_width as u32;
let height = calibration.0.depth_camera_calibration.resolution_height as u32;
let stride = width as i32 * size_of::<BgraPixel>() as i32;
// TODO: TEMP MULTI-CAMERA SUPPORT