astatin3/SiPEED-A075V
1
0
Fork 0
mirror of https://github.com/Astatin3/SiPEED-A075V.git synced 2026-06-09 00:28:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

Add rust panes

Browse Source
This commit is contained in:
Michael Mikovsky
2025-05-07 11:54:13 -06:00
parent beb235c036
commit af31fe3a04
19 changed files with 6878 additions and 1678 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Cargo.lock
Generated
+4470 -462
View File
File diff suppressed because it is too large Load Diff
Cargo.toml
+28 -2
View File
@@ -5,7 +5,33 @@ edition = "2024"
[dependencies] [dependencies]
byteorder = "1.5.0" byteorder = "1.5.0"
crossbeam-channel = "0.5.15"
eframe = { version = "0.31.1", features = [
"accesskit", # Make egui compatible with screen readers. NOTE: adds a lot of dependencies.
"default_fonts", # Embed the default egui fonts.
"glow",
"persistence",
] }
epi = "0.17.0"
image = "0.25.6"
ndarray = "0.16.1" ndarray = "0.16.1"
once_cell = "1.21.3" once_cell = "1.21.3"
opencv = "0.94.4" # opencv = "0.94.4"
reqwest = { version = "0.12.15", features = ["blocking"] } ureq = "3.0.11"
egui = { version = "0.31.1", features = ["callstack", "default", "log"] }
egui-snarl = { version = "0.7.1", features = ["serde"] }
glam = "0.30.3"
rand = "0.9.1"
serde = { version = "1.0.219", features = ["derive"] }
serde_json = "1.0.140"
syn = "2.0.101"
typetag = "0.2.20"
bytemuck = "1.23.0"
# simple_logger = "5.0.0"
log = "0.4.27"
env_logger = "0.11.8"
ply-rs = "0.1.3"
wgpu = "25.0.0"
dialog = "0.3.0"
pasture-core = "0.5.0"
pasture-io = "0.5.0"
pane_layout.json
+31
View File
@@ -0,0 +1,31 @@
[
{
"pane": {
"type": "PointRendererPane"
},
"id": "Point Cloud",
"mode": "Hidden"
},
{
"pane": {
"type": "PipelinePane",
"snarl": {
"nodes": {},
"wires": []
},
"style": {
"select_rect_contained": null
},
"snarl_ui_id": null
},
"id": "Pipeline Pane",
"mode": "Hidden"
},
{
"pane": {
"type": "CameraPane"
},
"id": "Raw Camera",
"mode": "Hidden"
}
]
python/.gitignore
-160
View File
@@ -1,160 +0,0 @@
# Byte-compiled / optimized / DLL files
__pycache__/
*.py[cod]
*$py.class
# C extensions
*.so
# Distribution / packaging
.Python
build/
develop-eggs/
dist/
downloads/
eggs/
.eggs/
lib/
lib64/
parts/
sdist/
var/
wheels/
share/python-wheels/
*.egg-info/
.installed.cfg
*.egg
MANIFEST
# PyInstaller
# Usually these files are written by a python script from a template
# before PyInstaller builds the exe, so as to inject date/other infos into it.
*.manifest
*.spec
# Installer logs
pip-log.txt
pip-delete-this-directory.txt
# Unit test / coverage reports
htmlcov/
.tox/
.nox/
.coverage
.coverage.*
.cache
nosetests.xml
coverage.xml
*.cover
*.py,cover
.hypothesis/
.pytest_cache/
cover/
# Translations
*.mo
*.pot
# Django stuff:
*.log
local_settings.py
db.sqlite3
db.sqlite3-journal
# Flask stuff:
instance/
.webassets-cache
# Scrapy stuff:
.scrapy
# Sphinx documentation
docs/_build/
# PyBuilder
.pybuilder/
target/
# Jupyter Notebook
.ipynb_checkpoints
# IPython
profile_default/
ipython_config.py
# pyenv
# For a library or package, you might want to ignore these files since the code is
# intended to run in multiple environments; otherwise, check them in:
# .python-version
# pipenv
# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
# However, in case of collaboration, if having platform-specific dependencies or dependencies
# having no cross-platform support, pipenv may install dependencies that don't work, or not
# install all needed dependencies.
#Pipfile.lock
# poetry
# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
# This is especially recommended for binary packages to ensure reproducibility, and is more
# commonly ignored for libraries.
# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
#poetry.lock
# pdm
# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
#pdm.lock
# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
# in version control.
# https://pdm.fming.dev/#use-with-ide
.pdm.toml
# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
__pypackages__/
# Celery stuff
celerybeat-schedule
celerybeat.pid
# SageMath parsed files
*.sage.py
# Environments
.env
.venv
env/
venv/
ENV/
env.bak/
venv.bak/
# Spyder project settings
.spyderproject
.spyproject
# Rope project settings
.ropeproject
# mkdocs documentation
/site
# mypy
.mypy_cache/
.dmypy.json
dmypy.json
# Pyre type checker
.pyre/
# pytype static type analyzer
.pytype/
# Cython debug symbols
cython_debug/
# PyCharm
# JetBrains specific template is maintained in a separate JetBrains.gitignore that can
# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
# and can be added to the global gitignore or merged into this file. For a more nuclear
# option (not recommended) you can uncomment the following to ignore the entire idea folder.
#.idea/
python/tof.py
-392
View File
@@ -1,392 +0,0 @@
import struct
import numpy as np
import cv2
#import matplotlib.pyplot as plt
import requests
import open3d as o3d
HOST = '192.168.233.1'
PORT = 80
# def create_point_cloud_map(width, height, fx, fy, cx, cy):
# """
# Create mapping arrays for converting depth image to point cloud.
# Args:
# width, height: Image dimensions
# fx, fy: Focal lengths
# cx, cy: Principal point coordinates
# Returns:
# x_map, y_map: Arrays that when multiplied by depth give X,Y coordinates
# """
# # Create pixel coordinate grid
# v, u = np.meshgrid(np.arange(height), np.arange(width), indexing='ij')
# # Convert to normalized image coordinates
# x_map = (u - cx) / fx
# y_map = (v - cy) / fy
# return x_map, y_map
# def depth_to_points(depth_image, x_map, y_map):
# """
# Convert depth image to point cloud using pre-computed maps.
# Args:
# depth_image: 2D depth array
# x_map, y_map: Pre-computed coordinate maps
# Returns:
# points: Nx3 array of XYZ coordinates
# """
# # Calculate X and Y coordinates
# X = depth_image * x_map
# Y = depth_image * y_map
# # Stack coordinates into point cloud
# valid_points = depth_image > 0
# points = np.stack((
# X[valid_points],
# Y[valid_points],
# depth_image[valid_points]
# ), axis=-1)
# return points
def create_point_cloud_map(width, height, fx, fy, cx, cy):
"""
Create mapping arrays for converting depth image to point cloud.
Args:
width, height: Image dimensions
fx, fy: Focal lengths
cx, cy: Principal point coordinates
Returns:
x_map, y_map: Arrays that when multiplied by depth give X,Y coordinates
"""
# Create pixel coordinate grid
v, u = np.meshgrid(np.arange(height), np.arange(width), indexing='ij')
# Convert to normalized image coordinates
x_map = (u - cx) / fx
y_map = (v - cy) / fy
return x_map, y_map
def transform_points(points, translation, rotation=None):
"""
Apply rigid transformation to points.
Args:
points: Nx3 array of XYZ coordinates
translation: [tx, ty, tz] translation vector
rotation: 3x3 rotation matrix (optional)
Returns:
transformed_points: Nx3 array of transformed coordinates
"""
if rotation is not None:
points = points @ rotation.T
return points + translation
def depth_to_colored_points(depth_image, color_image, x_map, y_map,
color_intrinsics, depth_to_color_translation,
depth_to_color_rotation=None):
"""
Convert depth image to colored point cloud using pre-computed maps.
Args:
depth_image: 2D depth array
color_image: RGB image array (height, width, 3)
x_map, y_map: Pre-computed coordinate maps for depth camera
color_intrinsics: (fx, fy, cx, cy) for RGB camera
depth_to_color_translation: [tx, ty, tz] from depth to color camera
depth_to_color_rotation: 3x3 rotation matrix (optional)
Returns:
points: Nx3 array of XYZ coordinates
colors: Nx3 array of RGB values
"""
# Calculate initial point cloud from depth
valid_points = depth_image > 0
X = depth_image * x_map
Y = depth_image * y_map
points = np.stack((
X[valid_points],
Y[valid_points],
depth_image[valid_points]
), axis=-1)
# Transform points to color camera coordinate system
transformed_points = transform_points(
points,
depth_to_color_translation,
depth_to_color_rotation
)
# Project points into color image
fx, fy, cx, cy = color_intrinsics
u = (transformed_points[:, 0] * fx / transformed_points[:, 2] + cx).astype(int)
v = (transformed_points[:, 1] * fy / transformed_points[:, 2] + cy).astype(int)
# Filter points that project outside image bounds
height, width = color_image.shape[:2]
valid_uvs = (u >= 0) & (u < width) & (v >= 0) & (v < height)
# Sample colors from valid projections
colors = np.zeros((len(points), 3), dtype=np.uint8)
colors[valid_uvs] = color_image[v[valid_uvs], u[valid_uvs]]
return points[valid_uvs], colors[valid_uvs]
def depth_to_points(depth_image, x_map, y_map):
"""
Convert depth image to point cloud using pre-computed maps.
Args:
depth_image: 2D depth array
x_map, y_map: Pre-computed coordinate maps
Returns:
points: Nx3 array of XYZ coordinates
"""
# Calculate X and Y coordinates
X = depth_image * x_map
Y = depth_image * y_map
# Stack coordinates into point cloud
valid_points = depth_image > 0
points = np.stack((
X[valid_points],
Y[valid_points],
depth_image[valid_points]
), axis=-1)
return points
def get_frame_from_http(host=HOST, port=PORT):
r = requests.get('http://{}:{}/getdeep'.format(host, port))
if(r.status_code == requests.codes.ok):
print('Get deep image')
deepimg = r.content
print('Length={}'.format(len(deepimg)))
(frameid, stamp_msec) = struct.unpack('<QQ', deepimg[0:8+8])
print((frameid, stamp_msec/1000))
return deepimg
def post_encode_config(config, host=HOST, port=PORT):
r = requests.post('http://{}:{}/set_cfg'.format(host, port), config)
if(r.status_code == requests.codes.ok):
return True
return False
def frame_config_decode(frame_config):
'''
@frame_config bytes
@return fields, tuple (trigger_mode, deep_mode, deep_shift, ir_mode, status_mode, status_mask, rgb_mode, rgb_res, expose_time)
'''
return struct.unpack("<BBBBBBBBi", frame_config)
def frame_config_encode(trigger_mode=1, deep_mode=1, deep_shift=255, ir_mode=1, status_mode=2, status_mask=7, rgb_mode=1, rgb_res=0, expose_time=0):
'''
@trigger_mode, deep_mode, deep_shift, ir_mode, status_mode, status_mask, rgb_mode, rgb_res, expose_time
@return frame_config bytes
'''
return struct.pack("<BBBBBBBBi",
trigger_mode, deep_mode, deep_shift, ir_mode, status_mode, status_mask, rgb_mode, rgb_res, expose_time)
def frame_payload_decode(frame_data: bytes, with_config: tuple):
'''
@frame_data, bytes
@with_config, tuple (trigger_mode, deep_mode, deep_shift, ir_mode, status_mode, status_mask, rgb_mode, rgb_res, expose_time)
@return imgs, tuple (deepth_img, ir_img, status_img, rgb_img)
'''
deep_data_size, rgb_data_size = struct.unpack("<ii", frame_data[:8])
frame_payload = frame_data[8:]
# 0:16bit 1:8bit, resolution: 320*240
deepth_size = (320*240*2) >> with_config[1]
deepth_img = struct.unpack("<%us" % deepth_size, frame_payload[:deepth_size])[
0] if 0 != deepth_size else None
frame_payload = frame_payload[deepth_size:]
# 0:16bit 1:8bit, resolution: 320*240
ir_size = (320*240*2) >> with_config[3]
ir_img = struct.unpack("<%us" % ir_size, frame_payload[:ir_size])[
0] if 0 != ir_size else None
frame_payload = frame_payload[ir_size:]
status_size = (320*240//8) * (16 if 0 == with_config[4] else
2 if 1 == with_config[4] else 8 if 2 == with_config[4] else 1)
status_img = struct.unpack("<%us" % status_size, frame_payload[:status_size])[
0] if 0 != status_size else None
frame_payload = frame_payload[status_size:]
assert(deep_data_size == deepth_size+ir_size+status_size)
rgb_size = len(frame_payload)
assert(rgb_data_size == rgb_size)
rgb_img = struct.unpack("<%us" % rgb_size, frame_payload[:rgb_size])[
0] if 0 != rgb_size else None
if (not rgb_img is None):
if (1 == with_config[6]):
jpeg = cv2.imdecode(np.frombuffer(
rgb_img, 'uint8', rgb_size), cv2.IMREAD_COLOR)
if not jpeg is None:
rgb = cv2.cvtColor(jpeg, cv2.COLOR_BGR2RGB)
rgb_img = rgb.tobytes()
else:
rgb_img = None
# elif 0 == with_config[6]:
# yuv = np.frombuffer(rgb_img, 'uint8', rgb_size)
# print(len(yuv))
# if not yuv is None:
# rgb = cv2.cvtColor(yuv, cv2.COLOR_YUV420P2RGB)
# rgb_img = rgb.tobytes()
# else:
# rgb_img = None
return (deepth_img, ir_img, status_img, rgb_img)
prev_status = None
def show_frame(frame_data: bytes):
global prev_status
config = frame_config_decode(frame_data[16:16+12])
frame_bytes = frame_payload_decode(frame_data[16+12:], config)
depth = np.frombuffer(frame_bytes[0], 'uint16' if 0 == config[1] else 'uint8').reshape(
240, 320) if frame_bytes[0] else None
ir = np.frombuffer(frame_bytes[1], 'uint16' if 0 == config[3] else 'uint8').reshape(
240, 320) if frame_bytes[1] else None
status = np.frombuffer(frame_bytes[2], 'uint16' if 0 == config[4] else 'uint8').reshape(
240, 320) if frame_bytes[2] else None
rgb = np.frombuffer(frame_bytes[3], 'uint8').reshape(
(480, 640, 3) if config[6] == 1 else (600, 800, 3)) if frame_bytes[3] else None
if not (depth is None or status is None or rgb is None):
if prev_status is None:
mask = (status==0)
else:
mask = (status==0)*(prev_status==0)
linear_mask = mask.reshape((-1))
# delete = np.where(1-linear_mask))
# depth_frame = (depth*mask)/1000
depth_frame = (depth)/1000
blurred = cv2.GaussianBlur(depth_frame,(3,3),1.0)
# depth_frame = cv2.addWeighted(depth_frame, 2.5, blurred, -1, 0)
prev_status = status
points = depth_to_points(depth_frame, x_map, y_map)
points = points[linear_mask]
colors = cv2.resize(rgb, (320, 240))
# cv2.imshow("color", mask)
# colors = (np.stack((depth_frame,) * 3, axis=-1)).reshape((-1, 3)).astype(np.float64) / 255.0
colors = colors.reshape((-1, 3)).astype(np.float64) / 255.0
# colors *= linear_mask
# colors = colors[:-(colors.shape[0]-points.shape[0])]
# colors = np.delete(colors, delete, axis = 0)
colors = colors[linear_mask]
# print(np.where(points))
# print(colors_e.shape)
return depth_frame, points, colors
return None, None, None
# create visualizer and window.
vis = o3d.visualization.Visualizer()
vis.create_window(height=480, width=640)
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(np.random.rand(10, 3))
vis.add_geometry(pcd)
x_map, y_map = create_point_cloud_map(
width=320, height=240,
fx=231.8290, fy=232.7785, # focal lengths
cx=166.9372, cy=123.5151 # principal point
)
depth_to_color_translation = np.array([0, 0, 0]) # 5cm offset in x
depth_to_color_rotation = np.eye(3) # Identity matrix if cameras are parallel
color_intrinsics = (520, 520, 325, 245)
keep_running = True
while keep_running:
if post_encode_config(frame_config_encode(1,0,255,0,2,7,1,0,0)):
p = get_frame_from_http()
depth_image, points, colors = show_frame(p)
if depth_image is None or colors is None : continue
cv2.imshow("e", depth_image)
cv2.waitKey(1)
# points, colors = depth_to_colored_points(
# depth_image,
# color_image,
# x_map,
# y_map,
# color_intrinsics,
# depth_to_color_translation,
# depth_to_color_rotation
# )
# points = depth_image.reshape((-1, 3))
# colors = color_image.reshape((-1, 3)).astype(np.float64) / 255.0
# colors[:, [0, 2]] = colors[:, [2, 0]]
print(points.shape)
print(colors.shape)
pcd.points = o3d.utility.Vector3dVector(points)
pcd.colors = o3d.utility.Vector3dVector(colors)
vis.update_geometry(pcd)
keep_running = vis.poll_events()
vis.update_renderer()
# pcd.points.extend(np.random.rand(n_new, 3))
# cv2.waitKey(1)
# with open("rgbd.raw", 'wb') as f:
# f.write(p)
# f.flush()
src/app.rs
+65
View File
@@ -0,0 +1,65 @@
// use egui::{FontFamily, FontId, RichText, Visuals};
// use eframe::egui_glow;
// use std::{sync::Arc, time::Instant};
// use egui::{accesskit::TextAlign, mutex::Mutex, Align2, Color32, FontId, Pos2, Stroke};
// use egui_glow::glow;
use crate::pane_manager::PaneManager;
/// We derive Deserialize/Serialize so we can persist app state on shutdown.
// #[derive(serde::Deserialize, serde::Serialize)]
// #[serde(default)]
pub struct App {
pane_manager: PaneManager,
}
impl App {
/// Called once before the first frame.
pub fn new(cc: &eframe::CreationContext<'_>) -> Option<Self> {
// This is also where you can customize the look and feel of egui using
// `cc.egui_ctx.set_visuals` and `cc.egui_ctx.set_fonts`.
// Load previous app state (if any).
// Note that you must enable the `persistence` feature for this to work.
// if let Some(storage) = cc.storage {
// return eframe::get_value(storage, eframe::APP_KEY).unwrap_or_default();
// }
Some(Self {
pane_manager: PaneManager::new(cc),
})
}
}
// impl Default for App {
// fn default() -> Self {
// Self {
// pane_manager: PaneManager::new(None),
// }
// }
// }
impl eframe::App for App {
/// Called each time the UI needs repainting, which may be many times per second.
fn update(&mut self, ctx: &egui::Context, _frame: &mut eframe::Frame) {
egui::CentralPanel::default().show(ctx, |ui| {
self.pane_manager.render(ui);
// egui::scroll_area::ScrollArea::vertical().show(ui, |ui| {
// egui::Frame::canvas(ui.style()).show(ui, |ui| {
// self.custom_painting(ui.max_rect(), ui);
// });
// })
});
}
// fn on_exit(&mut self, gl: Option<&glow::Context>) {
// if let Some(gl) = gl {
// self.rotating_triangle.lock().destroy(gl);
// }
// }
// Called by the frame work to save state before shutdown.
// fn save(&mut self, storage: &mut dyn eframe::Storage) {
// eframe::set_value(storage, eframe::APP_KEY, self);
// }
}
src/fetch_frame.rs
+335
View File
@@ -0,0 +1,335 @@
use byteorder::{LittleEndian, ReadBytesExt};
use ndarray::{Array2, Array3};
use std::{io::Cursor, ops::DerefMut};
pub struct ProcessedFrames {
pub depth: Option<Array2<u16>>,
pub ir: Option<Array2<u16>>,
pub status: Option<Array2<u16>>,
pub rgb: Option<Array3<u8>>,
}
impl Default for ProcessedFrames {
fn default() -> Self {
Self {
depth: None,
ir: None,
status: None,
rgb: None,
}
}
}
// Messages between threads
pub enum FrameMessage {
RawFrame(Vec<u8>),
DecodedFrame(ProcessedFrames),
Shutdown,
}
// Frame data structures
#[allow(dead_code)]
pub struct FrameConfig {
trigger_mode: u8,
deep_mode: u8,
deep_shift: u8,
ir_mode: u8,
status_mode: u8,
status_mask: u8,
rgb_mode: u8,
rgb_res: u8,
expose_time: i32,
}
pub struct FramePayload {
depth_img: Option<Vec<u8>>,
ir_img: Option<Vec<u8>>,
status_img: Option<Vec<u8>>,
rgb_img: Option<Vec<u8>>,
}
// Helper functions
fn frame_config_decode(frame_config: &[u8]) -> Result<FrameConfig, Box<dyn std::error::Error>> {
if frame_config.len() < 12 {
return Err("Frame config data too short".into());
}
let mut cursor = Cursor::new(frame_config);
Ok(FrameConfig {
trigger_mode: cursor.read_u8()?,
deep_mode: cursor.read_u8()?,
deep_shift: cursor.read_u8()?,
ir_mode: cursor.read_u8()?,
status_mode: cursor.read_u8()?,
status_mask: cursor.read_u8()?,
rgb_mode: cursor.read_u8()?,
rgb_res: cursor.read_u8()?,
expose_time: cursor.read_i32::<LittleEndian>()?,
})
}
pub fn frame_config_encode(
trigger_mode: u8,
deep_mode: u8,
deep_shift: u8,
ir_mode: u8,
status_mode: u8,
status_mask: u8,
rgb_mode: u8,
rgb_res: u8,
expose_time: i32,
) -> Vec<u8> {
let mut result = Vec::with_capacity(12);
result.push(trigger_mode);
result.push(deep_mode);
result.push(deep_shift);
result.push(ir_mode);
result.push(status_mode);
result.push(status_mask);
result.push(rgb_mode);
result.push(rgb_res);
// Add expose_time as little endian
result.extend_from_slice(&expose_time.to_le_bytes());
result
}
fn frame_payload_decode(
frame_data: &[u8],
config: &FrameConfig,
) -> Result<FramePayload, Box<dyn std::error::Error>> {
if frame_data.len() < 8 {
return Err("Frame data too short".into());
}
let mut cursor = Cursor::new(&frame_data[0..8]);
let deep_data_size = cursor.read_i32::<LittleEndian>()?;
let rgb_data_size = cursor.read_i32::<LittleEndian>()?;
let mut payload = &frame_data[8..];
// Depth image
let depth_size = (320 * 240 * 2) >> config.deep_mode;
let depth_img = if depth_size > 0 && payload.len() >= depth_size {
let result = payload[..depth_size].to_vec();
payload = &payload[depth_size..];
Some(result)
} else {
None
};
// IR image
let ir_size = (320 * 240 * 2) >> config.ir_mode;
let ir_img = if ir_size > 0 && payload.len() >= ir_size {
let result = payload[..ir_size].to_vec();
payload = &payload[ir_size..];
Some(result)
} else {
None
};
// Status image
let status_size = (320 * 240 / 8)
* match config.status_mode {
0 => 16,
1 => 2,
2 => 8,
_ => 1,
};
let status_img = if status_size > 0 && payload.len() >= status_size {
let result = payload[..status_size].to_vec();
payload = &payload[status_size..];
Some(result)
} else {
None
};
// Verify deep data size
let calculated_deep_size = depth_size + ir_size + status_size;
if calculated_deep_size != deep_data_size as usize {
warn!(
"Warning: Deep data size mismatch: {} vs {}",
calculated_deep_size, deep_data_size
);
}
// RGB image
let rgb_size = payload.len();
if rgb_size != rgb_data_size as usize {
warn!(
"Warning: RGB data size mismatch: {} vs {}",
rgb_size, rgb_data_size
);
}
let rgb_img = if rgb_size > 0 {
// Process RGB image based on config
if config.rgb_mode == 1 {
// JPEG decode using OpenCV
let rgb_data = payload.to_vec();
let decoded = decode_jpeg(&rgb_data);
decoded
} else {
Some(payload.to_vec())
}
} else {
None
};
Ok(FramePayload {
depth_img,
ir_img,
status_img,
rgb_img,
})
}
fn decode_jpeg(jpeg_data: &[u8]) -> Option<Vec<u8>> {
// Use the image crate to decode JPEG and convert to RGB
let img = image::load_from_memory(jpeg_data).ok()?;
let rgb_img = img.to_rgb8();
// Convert to raw bytes
Some(rgb_img.into_raw())
}
pub fn decode_frame(frame_data: &[u8]) -> Result<ProcessedFrames, Box<dyn std::error::Error>> {
if frame_data.len() < 28 {
// 16 (header) + 12 (config)
return Err("Frame data too short".into());
}
// Extract config
let config = frame_config_decode(&frame_data[16..28])?;
// Decode payload
let payload = frame_payload_decode(&frame_data[28..], &config)?;
// Process depth image
let depth = if let Some(depth_data) = payload.depth_img {
if config.deep_mode == 0 {
let data = depth_data.as_slice();
let depth_array = Array2::from_shape_fn((240, 320), |(y, x)| {
let idx = (y * 320 + x) * 2;
if idx + 1 < data.len() {
u16::from_le_bytes([data[idx], data[idx + 1]])
} else {
0
}
});
Some(depth_array)
} else {
let data = depth_data.as_slice();
let depth_array = Array2::from_shape_fn((240, 320), |(y, x)| {
let idx = y * 320 + x;
if idx < data.len() {
u16::from(data[idx])
} else {
0
}
});
Some(depth_array)
}
} else {
None
};
// Process IR image
let ir = if let Some(ir_data) = payload.ir_img {
if config.ir_mode == 0 {
let data = ir_data.as_slice();
let ir_array = Array2::from_shape_fn((240, 320), |(y, x)| {
let idx = (y * 320 + x) * 2;
if idx + 1 < data.len() {
u16::from_le_bytes([data[idx], data[idx + 1]])
} else {
0
}
});
Some(ir_array)
} else {
let data = ir_data.as_slice();
let ir_array = Array2::from_shape_fn((240, 320), |(y, x)| {
let idx = y * 320 + x;
if idx < data.len() {
u16::from(data[idx])
} else {
0
}
});
Some(ir_array)
}
} else {
None
};
// Process status image
let status = if let Some(status_data) = payload.status_img {
// Process according to status_mode
let data = status_data.as_slice();
let status_array = Array2::from_shape_fn((240, 320), |(y, x)| {
// This is a simplified approach - actual processing depends on status_mode
let idx = y * 320 + x;
if idx < data.len() {
u16::from(data[idx])
} else {
0
}
});
Some(status_array)
} else {
None
};
// Process RGB image
let rgb = if let Some(rgb_data) = payload.rgb_img {
let shape = if config.rgb_mode == 1 {
match config.rgb_res {
0 => (480, 640, 3), // Default resolution
_ => (600, 800, 3), // Alternative resolution
}
} else {
(480, 640, 3) // Default for non-JPEG
};
if rgb_data.len() >= shape.0 * shape.1 * shape.2 {
let rgb_array = Array3::from_shape_vec((shape.0, shape.1, shape.2), rgb_data)?;
Some(rgb_array)
} else {
warn!(
"RGB data size ({}) doesn't match expected size ({})",
rgb_data.len(),
shape.0 * shape.1 * shape.2
);
None
}
} else {
None
};
Ok(ProcessedFrames {
depth,
ir,
status,
rgb,
})
}
pub fn normalize(data: &Array2<u16>) -> Vec<u8> {
let mut result = Vec::with_capacity(data.dim().0 * data.dim().1 * 3);
let max = 255. / (*data.iter().max().unwrap_or(&255u16) as f32);
for &value in data.iter() {
let num = (value as f32 * max) as u8;
result.push(num);
result.push(num);
result.push(num);
}
result
}
src/lib.rs
+8
View File
@@ -0,0 +1,8 @@
#[macro_use]
extern crate log;
pub mod app;
pub mod fetch_frame;
pub mod nodes;
pub mod pane_manager;
pub mod panes;
src/main.rs
+16 -359
View File
@@ -1,363 +1,20 @@
use byteorder::{LittleEndian, ReadBytesExt}; use SiPEED_A075V::app::App;
use opencv::boxed_ref::BoxedRef;
use opencv::{core, highgui, imgcodecs, imgproc, prelude::*};
use reqwest;
use std::error::Error;
use std::io::Cursor;
const HOST: &str = "192.168.233.1"; // Assuming default value similar to Python code // Main function
const PORT: u16 = 80; // Assuming default value similar to Python code fn main() -> Result<(), eframe::Error> {
env_logger::init();
fn get_frame_from_http() -> Result<Vec<u8>, Box<dyn Error>> { let options = eframe::NativeOptions {
let client = reqwest::blocking::Client::new(); // viewport: egui::ViewportBuilder::default()
let url = format!("http://{}:{}/getdeep", HOST, PORT); // .with_inner_size([400.0, 300.0])
let response = client.get(&url).send()?; // .with_min_inner_size([300.0, 220.0]),
depth_buffer: 24,
..Default::default()
};
if response.status().is_success() { eframe::run_native(
println!("Get deep image"); "Frame Viewer",
let deepimg = response.bytes()?.to_vec(); options,
println!("Length={}", deepimg.len()); Box::new(|cc| Ok(Box::new(App::new(cc).unwrap()))),
)
if deepimg.len() >= 16 {
let mut cursor = Cursor::new(&deepimg[0..16]);
let frameid = cursor.read_u64::<LittleEndian>()?;
let stamp_msec = cursor.read_u64::<LittleEndian>()?;
println!("({}, {})", frameid, stamp_msec as f64 / 1000.0);
Ok(deepimg)
} else {
Err("Image data too short".into())
}
} else {
Err(format!("HTTP request failed with status: {}", response.status()).into())
}
}
fn post_encode_config(config: &[u8]) -> Result<bool, Box<dyn Error>> {
let client = reqwest::blocking::Client::new();
let url = format!("http://{}:{}/set_cfg", HOST, PORT);
let response = client.post(&url).body(config.to_vec()).send()?;
Ok(response.status().is_success())
}
#[derive(Debug, Clone, Copy)]
struct FrameConfig {
trigger_mode: u8,
deep_mode: u8,
deep_shift: u8,
ir_mode: u8,
status_mode: u8,
status_mask: u8,
rgb_mode: u8,
rgb_res: u8,
expose_time: i32,
}
fn frame_config_decode(frame_config: &[u8]) -> Result<FrameConfig, Box<dyn Error>> {
if frame_config.len() < 12 {
return Err("Frame config data too short".into());
}
let mut cursor = Cursor::new(frame_config);
Ok(FrameConfig {
trigger_mode: cursor.read_u8()?,
deep_mode: cursor.read_u8()?,
deep_shift: cursor.read_u8()?,
ir_mode: cursor.read_u8()?,
status_mode: cursor.read_u8()?,
status_mask: cursor.read_u8()?,
rgb_mode: cursor.read_u8()?,
rgb_res: cursor.read_u8()?,
expose_time: cursor.read_i32::<LittleEndian>()?,
})
}
fn frame_config_encode(
trigger_mode: u8,
deep_mode: u8,
deep_shift: u8,
ir_mode: u8,
status_mode: u8,
status_mask: u8,
rgb_mode: u8,
rgb_res: u8,
expose_time: i32,
) -> Vec<u8> {
let mut buffer = Vec::with_capacity(12);
buffer.push(trigger_mode);
buffer.push(deep_mode);
buffer.push(deep_shift);
buffer.push(ir_mode);
buffer.push(status_mode);
buffer.push(status_mask);
buffer.push(rgb_mode);
buffer.push(rgb_res);
buffer.extend_from_slice(&expose_time.to_le_bytes());
buffer
}
struct FramePayload {
depth_img: Option<Vec<u8>>,
ir_img: Option<Vec<u8>>,
status_img: Option<Vec<u8>>,
rgb_img: Option<Vec<u8>>,
}
fn frame_payload_decode(
frame_data: &[u8],
config: &FrameConfig,
) -> Result<FramePayload, Box<dyn Error>> {
if frame_data.len() < 8 {
return Err("Frame data too short".into());
}
let mut cursor = Cursor::new(&frame_data[0..8]);
let deep_data_size = cursor.read_i32::<LittleEndian>()?;
let rgb_data_size = cursor.read_i32::<LittleEndian>()?;
let mut frame_payload = &frame_data[8..];
// Calculate sizes based on configuration
let depth_size = (320 * 240 * 2) >> config.deep_mode;
let depth_img = if depth_size > 0 && frame_payload.len() >= depth_size {
let depth_data = frame_payload[..depth_size].to_vec();
frame_payload = &frame_payload[depth_size..];
Some(depth_data)
} else {
None
};
let ir_size = (320 * 240 * 2) >> config.ir_mode;
let ir_img = if ir_size > 0 && frame_payload.len() >= ir_size {
let ir_data = frame_payload[..ir_size].to_vec();
frame_payload = &frame_payload[ir_size..];
Some(ir_data)
} else {
None
};
let status_size = (320 * 240 / 8)
* match config.status_mode {
0 => 16,
1 => 2,
2 => 8,
_ => 1,
};
let status_img = if status_size > 0 && frame_payload.len() >= status_size {
let status_data = frame_payload[..status_size].to_vec();
frame_payload = &frame_payload[status_size..];
Some(status_data)
} else {
None
};
// Verify that we've consumed the expected amount of data
if deep_data_size as usize != depth_size + ir_size + status_size {
return Err("Data size mismatch in frame payload".into());
}
let rgb_size = frame_payload.len();
if rgb_data_size as usize != rgb_size {
return Err("RGB data size mismatch".into());
}
let mut rgb_img = if rgb_size > 0 {
Some(frame_payload.to_vec())
} else {
None
};
// Process RGB image if present
if let Some(rgb_data) = rgb_img.as_ref() {
if config.rgb_mode == 1 {
let jpeg_data = Mat::from_slice(rgb_data)?;
let jpeg = imgcodecs::imdecode(&jpeg_data, imgcodecs::IMREAD_COLOR)?;
if !jpeg.empty() {
let mut rgb = Mat::default();
imgproc::cvt_color(&jpeg, &mut rgb, imgproc::COLOR_BGR2RGB, 0)?;
let mut rgb_vec = Vec::new();
let mat_size = rgb.total() as usize * rgb.elem_size().unwrap();
rgb_vec.resize(mat_size, 0);
// This extracts the raw bytes
let mat_data = rgb.data_bytes()?;
rgb_vec.copy_from_slice(mat_data);
rgb_img = Some(rgb_vec);
} else {
rgb_img = None;
}
}
}
Ok(FramePayload {
depth_img,
ir_img,
status_img,
rgb_img,
})
}
struct FrameOutputs {
depth: Option<BoxedRef<'static, Mat>>,
ir: Option<BoxedRef<'static, Mat>>,
status: Option<BoxedRef<'static, Mat>>,
rgb: Option<BoxedRef<'static, Mat>>,
}
fn show_frame(frame_data: &[u8]) -> Result<(), Box<dyn Error>> {
if frame_data.len() < 28 {
// 16 + 12 minimum size
return Err("Frame data too short".into());
}
let config = frame_config_decode(&frame_data[16..28])?;
let frame_bytes = frame_payload_decode(&frame_data[28..], &config)?;
let depth = if let Some(depth_data) = frame_bytes.depth_img {
let depth_type = if config.deep_mode == 0 {
core::CV_16U
} else {
core::CV_8U
};
// Create a Mat from raw depth data
let depth_mat = unsafe {
let mut mat = Mat::new_rows_cols(240, 320, depth_type)?;
let mat_data = mat.data_bytes_mut()?;
if mat_data.len() == depth_data.len() {
mat_data.copy_from_slice(&depth_data);
mat
} else {
return Err("Depth data size mismatch".into());
}
};
// highgui::imshow("depth_mat", &depth_mat)?;
Some(depth_mat)
} else {
None
};
let ir = if let Some(ir_data) = frame_bytes.ir_img {
let ir_type = if config.ir_mode == 0 {
core::CV_16U
} else {
core::CV_8U
};
// Create a Mat from raw IR data
let mut ir_mat = unsafe {
let mut mat = Mat::new_rows_cols(240, 320, ir_type)?;
let mat_data = mat.data_bytes_mut()?;
if mat_data.len() == ir_data.len() {
mat_data.copy_from_slice(&ir_data);
mat
} else {
return Err("IR data size mismatch".into());
}
};
// highgui::imshow("ir_mat", &ir_mat)?;
Some(ir_mat)
} else {
None
};
// Process status image
let status = if let Some(status_data) = frame_bytes.status_img {
let status_type = if config.status_mode == 0 {
core::CV_16U
} else {
core::CV_8U
};
// Create a Mat from raw status data
let mut status_mat = unsafe {
let mut mat = Mat::new_rows_cols(240, 320, status_type)?;
let mat_data = mat.data_bytes_mut()?;
if mat_data.len() == status_data.len() {
mat_data.copy_from_slice(&status_data);
mat
} else {
return Err("Status data size mismatch".into());
}
};
// highgui::imshow("status_mat", &status_mat)?;
Some(status_mat)
} else {
None
};
// Process IR image
// let ir = if let Some(ir_data) = &frame_bytes.ir_img {
// let ir_data = ir_data.as_slice();
// let ir_mat = Mat::new_rows_cols_with_data(240, 320, ir_data).unwrap();
// Some(ir_mat)
// } else {
// None
// };
// Process RGB image
let rgb = if let Some(rgb_data) = &frame_bytes.rgb_img {
let (height, width) = if config.rgb_mode == 1 {
(480, 640)
} else {
(600, 800)
};
let rgb_mat = unsafe {
let mut mat = Mat::new_rows_cols(height, width, core::CV_8UC3)?;
let mat_data = mat.data_bytes_mut()?;
if mat_data.len() == rgb_data.len() {
mat_data.copy_from_slice(&rgb_data);
mat
} else {
return Err("RGB data size mismatch".into());
}
};
highgui::imshow("rgb", &rgb_mat);
Some(rgb_mat)
} else {
None
};
Ok(())
// Ok(FrameOutputs {
// depth,
// ir,
// status,
// rgb,
// })
}
fn scanloop() -> Result<(), Box<dyn Error>> {
if post_encode_config(&frame_config_encode(1, 0, 255, 0, 2, 7, 1, 0, 0)).unwrap_or(false) {
let raw_frame = get_frame_from_http()?;
show_frame(&raw_frame)?;
highgui::wait_key(1)?;
}
Ok(())
}
fn main() {
loop {
let _ = scanloop();
}
} }
src/nodes/constants.rs
+115
View File
@@ -0,0 +1,115 @@
// use crate::pane_manager::{Pane, PaneMode, PaneState, PsudoCreationContext};
use crate::panes::pipeline_editor::Node;
use eframe::epaint::Color32;
use egui::{Id, Ui};
use egui_snarl::ui::{PinInfo, WireStyle};
use egui_snarl::{InPin, OutPin};
#[derive(Clone, serde::Serialize, serde::Deserialize)]
pub struct Constants {
vars: Vec<(String, String)>,
popup_open: bool,
uid: Id,
}
#[typetag::serde]
impl Node for Constants {
fn new() -> Self {
let mut s = Self {
vars: Vec::new(),
popup_open: false,
uid: Id::new(rand::random::<u64>()),
};
s.vars.push(("VAR".to_string(), "Change Me".to_string()));
s
}
fn get_name(&self) -> &str {
"Constants"
}
fn get_description(&self) -> &str {
"Test Node"
}
fn duplicate(&self) -> Box<dyn Node> {
Box::new(Self::new())
}
fn inputs(&self) -> usize {
0
}
fn outputs(&self) -> usize {
self.vars.len()
}
fn show_input(&mut self, _pin: &InPin, _ui: &mut Ui, _scale: f32) -> PinInfo {
PinInfo::square()
}
fn show_output(&mut self, pin: &OutPin, ui: &mut Ui, _scale: f32) -> PinInfo {
ui.label(self.vars.iter().nth(pin.id.output).unwrap().0.clone());
PinInfo::square()
.with_fill(Color32::RED)
.with_wire_style(WireStyle::Bezier3)
}
fn can_rx(&self, _other: &Box<dyn Node>) -> bool {
true
}
fn can_tx(&self, _other: &Box<dyn Node>) -> bool {
true
}
fn context_menu(&mut self, ui: &mut Ui) {
// egui::Window::new("TEST").show(ui.ctx(), |ui| {
// ui.heading("EEEEE");
// });
if ui.button("Edit").clicked() {
self.popup_open = !self.popup_open;
ui.close_menu();
}
}
fn update(&mut self, ui: &mut Ui) {
if self.popup_open {
egui::Window::new("Edit - ".to_owned() + self.get_name())
.id(self.uid)
.show(ui.ctx(), |ui| {
egui::Grid::new("my_grid")
.striped(true)
.max_col_width(9999.)
.show(ui, |ui| {
for (_i, (var1, _var2)) in self.vars.iter().enumerate() {
if var1.is_empty() {
// self.vars.remove()
}
// ui.add(egui::TextEdit::singleline(var1.as_mut()));
// ui.add(egui::TextEdit::singleline(var2.as_mut()));
ui.end_row();
}
});
if ui.button("ADD").clicked() {
self.vars.push(("VAR".to_string(), "Change Me".to_string()));
}
});
}
}
}
// #[derive(serde::Serialize, serde::Deserialize, Clone)]
// pub struct Constant_Edit_Popup {
// pub data: Vec<String>,
// pub has_changed: bool,
// }
// #[typetag::serde]
// impl Pane for Constant_Edit_Popup {
// fn new() -> PaneState where Self: Sized {
// let mut s = Self {
// node: None,
// has_changed: false,
// };
// PaneState {
// id: s.name().to_string(),
// mode: PaneMode::Popup,
// pane: Box::new(s),
// }
// }
// fn init(&mut self, _pcc: &PsudoCreationContext) {}
// fn name(&mut self) -> &str {"ERROR"}
// fn render(&mut self, _ui: &mut Ui){}
// fn context_menu(&mut self, _ui: &mut Ui) {}
// }
src/nodes/mod.rs
+1
View File
@@ -0,0 +1 @@
pub mod constants;
src/pane_manager.rs
+313
View File
@@ -0,0 +1,313 @@
use crate::panes::*;
use eframe::egui_glow::glow;
use egui::Ui;
use std::sync::Arc;
// use erased_serde::serialize_trait_object;
#[derive(serde::Deserialize, serde::Serialize, PartialEq)]
pub enum PaneMode {
Hidden,
Windowed,
Right,
Left,
Bottom,
Center,
Popup,
}
#[typetag::serde(tag = "type")]
pub trait Pane {
fn new() -> PaneState
where
Self: Sized;
fn init(&mut self, pcc: &PsudoCreationContext);
fn name(&mut self) -> &str;
fn render(&mut self, ui: &mut Ui);
fn context_menu(&mut self, ui: &mut Ui);
}
// impl Deserializer for Pane {
// }
#[derive(serde::Deserialize, serde::Serialize)]
pub struct PaneState {
// #[serde(skip)]
pub pane: Box<dyn Pane>,
pub id: String,
pub mode: PaneMode,
// pub window_location: Pos2,
}
#[derive(serde::Serialize, serde::Deserialize)]
pub struct NoPane {}
#[typetag::serde]
impl Pane for NoPane {
fn new() -> PaneState
where
Self: Sized,
{
let mut s = Self {};
PaneState {
id: s.name().to_string(),
mode: PaneMode::Left,
pane: Box::new(s),
}
}
fn init(&mut self, _pcc: &PsudoCreationContext) {}
fn name(&mut self) -> &str {
"ERROR"
}
fn render(&mut self, _ui: &mut Ui) {}
fn context_menu(&mut self, _ui: &mut Ui) {}
}
impl PaneState {
pub fn render(&mut self, ui: &mut Ui) {
self.pane.render(ui);
}
}
pub struct PsudoCreationContext {
pub gl: Option<Arc<glow::Context>>,
}
pub struct PaneManager {
pcc: PsudoCreationContext,
pub panes: Vec<PaneState>,
}
impl PaneManager {
pub fn new(cc: &eframe::CreationContext<'_>) -> Self {
// if let Some(cc) = cc {
let mut panes = vec![
PointRendererPane::new(),
PipelinePane::new(),
CameraPane::new(),
];
let pcc = PsudoCreationContext { gl: cc.gl.clone() };
for pane in &mut panes {
pane.pane.init(&pcc);
}
Self { pcc, panes }
}
pub fn render(&mut self, ui: &mut Ui) {
let len = self.panes.len();
egui::TopBottomPanel::top("top_panel").show(ui.ctx(), |ui| {
egui::menu::bar(ui, |ui| {
// NOTE: no File->Quit on web pages!
egui::widgets::global_theme_preference_switch(ui);
ui.menu_button("File", |ui| {
if ui.button("Save Layout").clicked() {
self.save_layout();
}
if ui.button("Load Layout").clicked() {
self.load_layout();
}
if ui.button("Quit").clicked() {
ui.ctx().send_viewport_cmd(egui::ViewportCommand::Close);
}
});
ui.menu_button("View", |ui| {
for i in 0..len {
if self.panes[i].mode == PaneMode::Popup {
continue;
}
ui.menu_button(self.panes[i].id.clone(), |ui| {
if ui
.button(
({
if self.panes[i].mode == PaneMode::Center {
"*"
} else {
" "
}
})
.to_owned()
+ "Center",
)
.clicked()
{
for a in 0..len {
let pane2: &mut PaneState = &mut self.panes[a];
if pane2.mode == PaneMode::Center {
pane2.mode = PaneMode::Windowed;
}
}
self.panes[i].mode = PaneMode::Center;
ui.close_menu();
}
if ui
.button(
({
if self.panes[i].mode == PaneMode::Windowed {
"*"
} else {
" "
}
})
.to_owned()
+ "Window",
)
.clicked()
{
self.panes[i].mode = PaneMode::Windowed;
ui.close_menu();
}
if ui
.button(
({
if self.panes[i].mode == PaneMode::Left {
"*"
} else {
" "
}
})
.to_owned()
+ "Left",
)
.clicked()
{
self.panes[i].mode = PaneMode::Left;
ui.close_menu();
}
if ui
.button(
({
if self.panes[i].mode == PaneMode::Right {
"*"
} else {
" "
}
})
.to_owned()
+ "Right",
)
.clicked()
{
self.panes[i].mode = PaneMode::Right;
ui.close_menu();
}
if ui
.button(
({
if self.panes[i].mode == PaneMode::Bottom {
"*"
} else {
" "
}
})
.to_owned()
+ "Bottom",
)
.clicked()
{
self.panes[i].mode = PaneMode::Bottom;
ui.close_menu();
}
if ui
.button(
({
if self.panes[i].mode == PaneMode::Hidden {
"*"
} else {
" "
}
})
.to_owned()
+ "Hidden",
)
.clicked()
{
self.panes[i].mode = PaneMode::Hidden;
ui.close_menu();
}
});
}
});
ui.separator();
for i in 0..len {
if self.panes[i].mode != PaneMode::Hidden
&& self.panes[i].mode != PaneMode::Popup
{
ui.menu_button(self.panes[i].id.clone(), |ui| {
let _ = &mut self.panes[i].pane.context_menu(ui);
});
}
}
});
});
for i in 0..len {
let pane: &mut PaneState = &mut self.panes[i];
match pane.mode {
PaneMode::Hidden => {}
PaneMode::Left => {
egui::panel::SidePanel::left(pane.id.clone())
.resizable(true)
.show(ui.ctx(), |ui| {
pane.render(ui);
});
}
PaneMode::Right => {
egui::panel::SidePanel::right(pane.id.clone())
.resizable(true)
.show(ui.ctx(), |ui| {
pane.render(ui);
});
}
PaneMode::Bottom => {
egui::panel::TopBottomPanel::bottom(pane.id.clone())
.resizable(true)
.show(ui.ctx(), |ui| {
pane.render(ui);
});
}
PaneMode::Windowed | PaneMode::Popup => {
egui::Window::new(pane.id.clone())
.resizable(true)
.max_width(ui.clip_rect().width())
.max_height(ui.clip_rect().height())
.show(ui.ctx(), |ui| {
pane.render(ui);
});
}
PaneMode::Center => {
egui::CentralPanel::default().show(ui.ctx(), |ui| {
pane.render(ui);
});
}
}
}
}
fn save_layout(&self) {
if let Ok(json) = serde_json::to_string_pretty(&self.panes) {
let _ = std::fs::write("pane_layout.json", json);
}
}
fn load_layout(&mut self) {
if let Ok(panes) = std::fs::read_to_string("pane_layout.json") {
if let Ok(panes) = serde_json::from_str(&panes) {
self.panes = panes;
for pane in &mut self.panes {
pane.pane.init(&self.pcc);
}
}
}
}
}
src/panes/camera.rs
+184
View File
@@ -0,0 +1,184 @@
use std::{
mem,
sync::{
Arc,
mpsc::{self, Receiver, Sender},
},
thread,
};
use std::io::Cursor;
use byteorder::{LittleEndian, ReadBytesExt};
use egui::{Ui, mutex::Mutex};
use crate::{
fetch_frame::{FrameMessage, ProcessedFrames, decode_frame, frame_config_encode, normalize},
pane_manager::{Pane, PaneMode, PaneState, PsudoCreationContext},
};
// Constants (replace with your actual values)
const HOST: &str = "192.168.233.1";
const PORT: u16 = 80;
#[derive(serde::Serialize, serde::Deserialize)]
pub struct CameraPane {
#[serde(skip)]
frames: Arc<Mutex<ProcessedFrames>>,
#[serde(skip)]
thread_handle: Option<thread::JoinHandle<()>>,
}
impl Default for CameraPane {
fn default() -> Self {
// Shared state for the latest processed frames
let frames = Arc::new(Mutex::new(ProcessedFrames::default()));
let frames_clone = Arc::clone(&frames);
let decoder_handle = thread::spawn(move || {
loop {
match fetch_frame() {
Ok(frame_data) => match decode_frame(&frame_data) {
Ok(processed) => {
mem::replace(&mut *frames_clone.lock(), processed);
}
Err(e) => warn!("Error decoding frame: {}", e),
},
Err(e) => warn!("Error fetching frame: {}", e),
}
}
});
Self {
frames,
thread_handle: Some(decoder_handle),
}
}
}
#[typetag::serde]
impl Pane for CameraPane {
fn new() -> PaneState {
let mut s = CameraPane::default();
PaneState {
id: s.name().to_string(),
mode: PaneMode::Hidden,
pane: Box::new(s),
}
}
fn init(&mut self, pcc: &PsudoCreationContext) {
// decoder_thread(rx, tx);
// let mut frames_lock = self.frames.lock();
}
fn name(&mut self) -> &str {
"Raw Camera"
}
fn render(&mut self, ui: &mut Ui) {
let frames_lock = self.frames.lock();
let ref frames = *frames_lock;
let mut processed = false;
// Display depth image
if let Some(ref depth) = frames.depth {
let depth_viz = normalize(depth);
ui.heading("Depth");
image_widget(ui, "depth_img", &depth_viz, [320.0, 240.0]);
processed = true;
}
// Display IR image
if let Some(ref ir) = frames.ir {
let ir_viz = normalize(ir);
ui.heading("IR");
image_widget(ui, "ir_img", &ir_viz, [320.0, 240.0]);
processed = true;
}
// Display status image if available
if let Some(ref status) = frames.status {
let status_viz = normalize(status);
ui.heading("Status");
image_widget(ui, "status_img", &status_viz, [320.0, 240.0]);
processed = true;
}
// Display RGB image if available
if let Some(ref rgb) = frames.rgb {
let rgb_viz = rgb.as_slice().unwrap();
let size = match rgb.dim().1 {
640 => [640.0, 480.0],
800 => [800.0, 600.0],
_ => [640.0, 480.0], // Default
};
ui.heading("RGB");
image_widget(ui, "rgb_img", rgb_viz, size);
processed = true;
}
if !processed {
ui.heading("Waiting for frames...");
}
}
fn context_menu(&mut self, ui: &mut Ui) {}
}
fn fetch_frame() -> Result<Vec<u8>, Box<dyn std::error::Error>> {
is_success(&frame_config_encode(1, 0, 255, 0, 2, 7, 1, 0, 0))?;
let url = format!("http://{}:{}/getdeep", HOST, PORT);
let response = ureq::get(url).call()?;
if response.status() != 200 {
return Err(format!("Failed to get frame: HTTP {}", response.status()).into());
}
warn!("Got deep image");
let deep_img = response.into_body().read_to_vec()?;
warn!("Length={}", deep_img.len());
// Parse frame ID and timestamp
if deep_img.len() >= 16 {
let mut cursor = Cursor::new(&deep_img[0..16]);
let frame_id = cursor.read_u64::<LittleEndian>()?;
let stamp_msec = cursor.read_u64::<LittleEndian>()?;
warn!(
"Frame ID: {}, Timestamp: {:.3}s",
frame_id,
stamp_msec as f64 / 1000.0
);
}
return Ok(deep_img);
}
fn is_success(data: &[u8]) -> Result<(), Box<dyn std::error::Error>> {
let url = format!("http://{}:{}/set_cfg", HOST, PORT);
let response = ureq::post(url).send(data.to_vec())?;
if response.status() == 200 {
return Ok(());
} else {
return Err(format!("Status code: {}", response.status().to_string()).into());
}
}
// Helper to display images in egui
fn image_widget(ui: &mut egui::Ui, id: &str, rgb_data: &[u8], size: [f32; 2]) {
let color_image = egui::ColorImage::from_rgb([size[0] as usize, size[1] as usize], rgb_data);
let handle = ui
.ctx()
.load_texture(id, color_image, egui::TextureOptions::LINEAR);
let sized_image =
egui::load::SizedTexture::new(handle.id(), egui::vec2(size[0] as f32, size[1] as f32));
ui.image(sized_image);
}
src/panes/mod.rs
+7
View File
@@ -0,0 +1,7 @@
pub mod camera;
pub mod pipeline_editor;
pub mod point_cloud_renderer;
pub use camera::CameraPane;
pub use pipeline_editor::PipelinePane;
pub use point_cloud_renderer::PointRendererPane;
src/panes/pipeline_editor.rs
+442
View File
@@ -0,0 +1,442 @@
use crate::pane_manager::{Pane, PaneMode, PaneState, PsudoCreationContext};
use egui::Ui;
use egui::{Color32, Id, Pos2};
use egui_snarl::ui::WireStyle;
use egui_snarl::{
InPin, NodeId, OutPin, OutPinId, Snarl,
ui::{PinInfo, SnarlStyle, SnarlViewer},
};
#[derive(serde::Serialize, serde::Deserialize)]
pub struct PipelinePane {
snarl: Option<Snarl<Box<dyn Node>>>,
style: Option<SnarlStyle>,
snarl_ui_id: Option<Id>,
}
#[typetag::serde]
impl Pane for PipelinePane {
fn new() -> PaneState
where
Self: Sized,
{
let mut s = Self {
snarl: Some(Snarl::new()),
style: Some(SnarlStyle::new()),
snarl_ui_id: None,
};
PaneState {
id: s.name().to_string(),
mode: PaneMode::Hidden,
pane: Box::new(s),
}
}
fn init(&mut self, _cc: &PsudoCreationContext) {}
fn name(&mut self) -> &str {
"Pipeline Pane"
}
fn render(&mut self, ui: &mut Ui) {
self.snarl_ui_id = Some(ui.id());
if let Some(snarl) = &mut self.snarl {
if let Some(style) = &self.style {
snarl.show(&mut NodeViewer, style, "snarl", ui);
}
}
}
fn context_menu(&mut self, ui: &mut Ui) {
ui.menu_button("Add Node", |ui| {
if let Some(snarl) = &mut self.snarl {
NodeViewer::add_node_menu(Pos2 { x: 0., y: 0. }, ui, snarl);
}
});
if ui.button("Run").clicked() {
ui.close_menu();
self.run();
}
// if !self.snarl.is_none() {
// self.snarl.unwrap().add_node_menu(ui, ui.clip_rect().min.clone(), )
// }
}
}
impl PipelinePane {
fn run(&mut self) {
if let Some(snarl) = &mut self.snarl {
fn remove_duplicates(nodes: Vec<NodeId>) -> Vec<NodeId> {
let mut new_vec: Vec<NodeId> = Vec::new();
for node in nodes {
if !new_vec.contains(&node) {
new_vec.push(node);
}
}
new_vec
}
fn has_input_wire(snarl: &Snarl<Box<dyn Node>>, nodeid: NodeId) -> bool {
for wire in snarl.wires() {
if wire.1.node == nodeid {
return true;
}
}
false
}
fn get_output_wires(snarl: &Snarl<Box<dyn Node>>, nodeid: &NodeId) -> Vec<OutPinId> {
let mut arr: Vec<OutPinId> = Vec::new();
for wire in snarl.wires() {
if &wire.0.node == nodeid {
arr.push(wire.0)
}
}
arr
}
// let wires = snarl.wires().map(|| {})
let mut nodes: Vec<Vec<NodeId>> = Vec::new();
let mut starting_nodes: Vec<NodeId> = Vec::new();
for node in snarl.nodes_ids_data() {
if !has_input_wire(snarl, node.0) {
starting_nodes.push(node.0.clone())
}
}
starting_nodes = remove_duplicates(starting_nodes);
nodes.push(starting_nodes);
for i in 1..50 {
if nodes.get(i - 1).is_none() {
break;
}
let prevarr = nodes.get(i - 1).unwrap();
if prevarr.len() == 0 {
break;
}
let mut newarr: Vec<NodeId> = Vec::new();
for node in prevarr {
for wire in get_output_wires(snarl, node) {
newarr.push(wire.node);
}
}
newarr = remove_duplicates(newarr);
nodes.push(newarr);
}
for nodearr in nodes {
info!("Nodes: ");
for node in nodearr {
info!("{}", snarl.get_node(node).unwrap().get_name());
}
}
}
}
}
fn _format_float(v: f64) -> String {
let v = (v * 1000.0).round() / 1000.0;
format!("{}", v)
}
#[typetag::serde(tag = "type")]
pub trait Node {
fn new() -> Self
where
Self: Sized;
fn get_name(&self) -> &str;
fn get_description(&self) -> &str;
fn duplicate(&self) -> Box<dyn Node>;
fn inputs(&self) -> usize;
fn outputs(&self) -> usize;
fn show_input(&mut self, pin: &InPin, ui: &mut Ui, scale: f32) -> PinInfo;
fn show_output(&mut self, pin: &OutPin, ui: &mut Ui, scale: f32) -> PinInfo;
fn can_rx(&self, other: &Box<dyn Node>) -> bool;
fn can_tx(&self, other: &Box<dyn Node>) -> bool;
fn context_menu(&mut self, ui: &mut Ui);
fn update(&mut self, ui: &mut Ui);
}
#[derive(Clone, serde::Serialize, serde::Deserialize)]
struct Node1;
#[typetag::serde]
impl Node for Node1 {
fn new() -> Self {
Self
}
fn get_name(&self) -> &str {
"Test"
}
fn get_description(&self) -> &str {
"Test Node"
}
fn duplicate(&self) -> Box<dyn Node> {
Box::new(Self::new())
}
fn inputs(&self) -> usize {
1
}
fn outputs(&self) -> usize {
1
}
fn show_input(&mut self, _pin: &InPin, _ui: &mut Ui, _scale: f32) -> PinInfo {
PinInfo::square()
}
fn show_output(&mut self, _pin: &OutPin, _ui: &mut Ui, _scale: f32) -> PinInfo {
PinInfo::square()
.with_fill(Color32::RED)
.with_wire_style(WireStyle::Bezier3)
}
fn can_rx(&self, _other: &Box<dyn Node>) -> bool {
true
}
fn can_tx(&self, _other: &Box<dyn Node>) -> bool {
true
}
fn context_menu(&mut self, ui: &mut Ui) {
ui.label("Test!");
}
fn update(&mut self, _ui: &mut Ui) {}
}
#[derive(Clone, serde::Serialize, serde::Deserialize)]
struct Node2;
#[typetag::serde]
impl Node for Node2 {
fn new() -> Self {
Self
}
fn get_name(&self) -> &str {
"Test 2-1"
}
fn get_description(&self) -> &str {
"Test Node"
}
fn duplicate(&self) -> Box<dyn Node> {
Box::new(Self::new())
}
fn inputs(&self) -> usize {
2
}
fn outputs(&self) -> usize {
1
}
fn show_input(&mut self, _pin: &InPin, _ui: &mut Ui, _scale: f32) -> PinInfo {
PinInfo::square()
}
fn show_output(&mut self, _pin: &OutPin, _ui: &mut Ui, _scale: f32) -> PinInfo {
PinInfo::square()
.with_fill(Color32::RED)
.with_wire_style(WireStyle::Bezier3)
}
fn can_rx(&self, _other: &Box<dyn Node>) -> bool {
true
}
fn can_tx(&self, _other: &Box<dyn Node>) -> bool {
true
}
fn context_menu(&mut self, ui: &mut Ui) {
ui.label("Test!");
}
fn update(&mut self, _ui: &mut Ui) {}
}
#[derive(Clone, serde::Serialize, serde::Deserialize)]
struct Node3;
#[typetag::serde]
impl Node for crate::panes::pipeline_editor::Node3 {
fn new() -> Self {
Self
}
fn get_name(&self) -> &str {
"Test 1-2"
}
fn get_description(&self) -> &str {
"Test Node"
}
fn duplicate(&self) -> Box<dyn Node> {
Box::new(Self::new())
}
fn inputs(&self) -> usize {
1
}
fn outputs(&self) -> usize {
2
}
fn show_input(&mut self, _pin: &InPin, _ui: &mut Ui, _scale: f32) -> PinInfo {
PinInfo::square()
}
fn show_output(&mut self, _pin: &OutPin, _ui: &mut Ui, _scale: f32) -> PinInfo {
PinInfo::square()
.with_fill(Color32::RED)
.with_wire_style(WireStyle::Bezier3)
}
fn can_rx(&self, _other: &Box<dyn Node>) -> bool {
true
}
fn can_tx(&self, _other: &Box<dyn Node>) -> bool {
true
}
fn context_menu(&mut self, ui: &mut Ui) {
ui.label("Test!");
}
fn update(&mut self, _ui: &mut Ui) {}
}
#[derive(Clone, serde::Serialize, serde::Deserialize)]
struct NodeViewer;
impl SnarlViewer<Box<dyn Node>> for NodeViewer {
fn connect(&mut self, from: &OutPin, to: &InPin, snarl: &mut Snarl<Box<dyn Node>>) {
// Validate connection
let rx = snarl.get_node(to.id.node).unwrap();
let tx = snarl.get_node(from.id.node).unwrap();
if rx.can_rx(tx) && tx.can_tx(rx) {
for &remote in &to.remotes {
snarl.disconnect(remote, to.id);
}
snarl.connect(from.id, to.id);
}
}
fn disconnect(&mut self, _from: &OutPin, to: &InPin, snarl: &mut Snarl<Box<dyn Node>>) {
for &remote in &to.remotes {
snarl.disconnect(remote, to.id);
}
}
fn title(&mut self, node: &Box<dyn Node>) -> String {
node.get_name().to_string()
}
fn outputs(&mut self, node: &Box<dyn Node>) -> usize {
node.outputs()
}
fn inputs(&mut self, node: &Box<dyn Node>) -> usize {
node.inputs()
}
fn show_input(
&mut self,
pin: &InPin,
ui: &mut Ui,
scale: f32,
snarl: &mut Snarl<Box<dyn Node>>,
) -> PinInfo {
snarl
.get_node_mut(pin.id.node)
.unwrap()
.show_input(pin, ui, scale)
}
fn show_output(
&mut self,
pin: &OutPin,
ui: &mut Ui,
scale: f32,
snarl: &mut Snarl<Box<dyn Node>>,
) -> PinInfo {
snarl
.get_node_mut(pin.id.node)
.unwrap()
.show_output(pin, ui, scale)
}
fn has_graph_menu(&mut self, _pos: Pos2, _snarl: &mut Snarl<Box<dyn Node>>) -> bool {
true
}
fn show_graph_menu(
&mut self,
pos: egui::Pos2,
ui: &mut Ui,
_scale: f32,
snarl: &mut Snarl<Box<dyn Node>>,
) {
NodeViewer::add_node_menu(pos, ui, snarl);
}
fn has_on_hover_popup(&mut self, _: &Box<dyn Node>) -> bool {
true
}
fn show_on_hover_popup(
&mut self,
node: NodeId,
_inputs: &[InPin],
_outputs: &[OutPin],
ui: &mut Ui,
_scale: f32,
snarl: &mut Snarl<Box<dyn Node>>,
) {
ui.label(snarl.get_node(node).unwrap().get_description());
}
fn has_node_menu(&mut self, _node: &Box<dyn Node>) -> bool {
true
}
fn show_node_menu(
&mut self,
nodeid: NodeId,
_inputs: &[InPin],
_outputs: &[OutPin],
ui: &mut Ui,
_scale: f32,
snarl: &mut Snarl<Box<dyn Node + 'static>>,
) {
ui.label("Node menu");
snarl.get_node_mut(nodeid).unwrap().context_menu(ui);
if ui.button("Remove").clicked() {
snarl.remove_node(nodeid);
ui.close_menu();
} else if ui.button("Duplicate").clicked() {
let node = snarl.get_node_mut(nodeid).unwrap().duplicate();
snarl.insert_node(egui::pos2(0., 0.), node);
ui.close_menu();
// }// else if ui.button("Remove All Connections").clicked() {
// ui.
// ui.close_menu();
}
}
fn has_body(&mut self, _node: &Box<dyn Node>) -> bool {
true
}
fn show_body(
&mut self,
node: NodeId,
_inputs: &[InPin],
_outputs: &[OutPin],
ui: &mut Ui,
_scale: f32,
snarl: &mut Snarl<Box<dyn Node>>,
) {
snarl.get_node_mut(node).unwrap().update(ui);
}
}
impl NodeViewer {
pub fn add_node_menu(pos: Pos2, ui: &mut Ui, snarl: &mut Snarl<Box<dyn Node>>) {
ui.label("Add node");
if ui.button("Test").clicked() {
snarl.insert_node(pos, Box::new(Node1::new()));
ui.close_menu();
} else if ui.button("Constants").clicked() {
snarl.insert_node(pos, Box::new(crate::nodes::constants::Constants::new()));
ui.close_menu();
} else if ui.button("Test 2-1").clicked() {
snarl.insert_node(pos, Box::new(Node2::new()));
ui.close_menu();
} else if ui.button("Test 1-2").clicked() {
snarl.insert_node(pos, Box::new(Node3::new()));
ui.close_menu();
}
}
}
src/panes/point_cloud_renderer/mod.rs
+3
View File
@@ -0,0 +1,3 @@
mod pane;
mod renderer;
pub use pane::PointRendererPane;
src/panes/point_cloud_renderer/pane.rs
+235
View File
@@ -0,0 +1,235 @@
use std::{sync::Arc, time::Instant};
use eframe::egui_glow;
use egui::{Align2, Color32, FontId, InputState, Stroke, Ui, mutex::Mutex};
use crate::pane_manager::{Pane, PaneMode, PaneState, PsudoCreationContext};
use super::renderer::{Camera, PointRenderer};
#[derive(serde::Serialize, serde::Deserialize)]
pub struct PointRendererPane {
#[serde(skip)]
renderer: Arc<Mutex<PointRenderer>>,
#[serde(skip)]
points: Vec<(i32, i32, i32, Color32)>,
#[serde(skip)]
file_dialog_open: bool,
#[serde(skip)]
cur_path: String,
}
#[typetag::serde]
impl Pane for PointRendererPane {
fn new() -> PaneState
where
Self: Sized,
{
let renderer = PointRenderer::default();
let mut s = Self {
renderer: Arc::new(Mutex::new(renderer)),
points: Vec::new(),
file_dialog_open: false,
cur_path: "./".to_string(),
};
PaneState {
id: s.name().to_string(),
mode: PaneMode::Hidden,
pane: Box::new(s),
}
}
fn init(&mut self, pcc: &PsudoCreationContext) {
self.renderer.lock().init(pcc.gl.clone(), 1_000_000);
}
fn name(&mut self) -> &str {
"Point Cloud"
}
fn render(&mut self, ui: &mut Ui) {
let max_rect = ui.max_rect();
let renderer = self.renderer.clone();
if renderer.lock().gl.is_none() {
return;
// renderer.lock().expect("Renderer Not Initialized").init(ui.ctx()., 1_000_000);
}
renderer.lock().clear();
if self.file_dialog_open {
egui::Window::new("Load PLY File").show(ui.ctx(), |ui| {
ui.label("Enter PLY file path:");
ui.text_edit_singleline(&mut self.cur_path); // Add proper path handling
ui.horizontal(|ui| {
if ui.button("Load").clicked() {
let renderer = &mut renderer.lock();
// Add proper path handling and error reporting
let ply = renderer.load_ply();
if let Err(e) = ply {
warn!("Failed to load PLY: {}", e);
} else {
// self.renderer.lock().camera.reset();
self.points = ply.unwrap();
}
self.file_dialog_open = false;
}
if ui.button("Cancel").clicked() {
self.file_dialog_open = false;
}
});
});
}
let start_time = Instant::now();
let (rect, response) = ui.allocate_exact_size(
egui::Vec2 {
x: max_rect.width(),
y: max_rect.height(),
},
egui::Sense::drag(),
);
let input_state: Option<InputState> = ui.input(|input_state| {
if response.hovered() {
//&& response.has_focus() {
Some(input_state.clone())
} else {
None
}
});
if self.points.is_empty() {
let radius = 1000i32;
for i in 0..100000 {
// let theta = (i as f32 * 0.1).sin() * std::f32::consts::PI;
// let phi = (i as f32 * 0.1).cos() * std::f32::consts::PI;
let x = (radius as f32 * (i as f32).cos()) as i32;
let y = (radius as f32 * (i as f32).sin()) as i32;
let z = (i as f32 * 0.05) as i32;
// let x = (i as f32 * 0.1) as u32;
// let y = (i as f32 * 0.1) as u32 ;
// let z = (i as f32 * 0.1) as u32;
// Color based on position
let color = Color32::from_rgba_premultiplied(
((x as f32 / radius as f32) * 255.0) as u8,
((y as f32 / radius as f32) * 255.0) as u8,
((z as f32 / radius as f32) * 255.0) as u8,
255,
);
self.points.push((x, y, z, color));
}
}
// let painter = ui.painter();
for &(x, y, z, color) in &self.points {
renderer.lock().add_point(x, y, z, color);
}
let o = <std::option::Option<Camera> as Clone>::clone(&renderer.lock().camera)
.unwrap()
.orientation
.clone();
let cb = egui_glow::CallbackFn::new(move |_info, _painter| {
renderer.lock().render(max_rect, input_state.clone());
});
let callback = egui::PaintCallback {
rect: max_rect,
callback: Arc::new(cb),
};
ui.painter().add(callback);
let line_length: f32 = 20.;
// if let Some(input_state) = input_state {
// if input_state.pointer.any_down() {
let pos1 = o.inverse() * glam::Vec3::X;
let pos2 = o.inverse() * glam::Vec3::Y;
let pos3 = o.inverse() * glam::Vec3::Z;
ui.painter().line_segment(
[
rect.center(),
rect.center()
+ egui::Vec2 {
x: line_length * pos1.x,
y: -line_length * pos1.y,
},
],
Stroke {
width: 1.5,
color: Color32::RED,
},
);
ui.painter().line_segment(
[
rect.center(),
rect.center()
+ egui::Vec2 {
x: line_length * pos2.x,
y: -line_length * pos2.y,
},
],
Stroke {
width: 1.5,
color: Color32::BLUE,
},
);
ui.painter().line_segment(
[
rect.center(),
rect.center()
+ egui::Vec2 {
x: line_length * pos3.x,
y: -line_length * pos3.y,
},
],
Stroke {
width: 1.5,
color: Color32::GREEN,
},
);
// }}
let end_time = Instant::now();
// println!("{}", end_time.duration_since(start_time).as_millis());
let text_size = 12.;
ui.painter().text(
max_rect.min,
Align2::LEFT_TOP,
format!("{} ms", end_time.duration_since(start_time).as_millis()),
FontId::monospace(text_size),
Color32::WHITE,
);
ui.painter().text(
max_rect.min
+ egui::Vec2 {
x: 0.,
y: text_size,
},
Align2::LEFT_TOP,
format!("{} points", self.points.len()),
FontId::monospace(text_size),
Color32::WHITE,
);
}
fn context_menu(&mut self, ui: &mut Ui) {
if ui.button("Load PLY").clicked() {
self.file_dialog_open = true;
}
}
}
src/panes/point_cloud_renderer/renderer.rs
+626
View File
@@ -0,0 +1,626 @@
use eframe::egui_glow;
use egui::{Color32, InputState, Rect};
use egui_glow::glow;
use glam::{Mat4, Quat, Vec3};
use pasture_core::containers::{
BorrowedBuffer, BorrowedBufferExt, InterleavedBufferMut, VectorBuffer,
};
use pasture_core::layout::attributes::{COLOR_RGB, POSITION_3D};
use pasture_core::nalgebra::Vector3;
use pasture_io::base::read_all;
use std::fs::File;
use std::io::{BufRead, BufReader};
use std::sync::Arc;
// Shader sources updated for 3D rendering with fixed-point positions
const VERTEX_SHADER: &str = r#"
#version 330 core
layout (location = 0) in ivec3 position; // Using unsigned ints for position
layout (location = 1) in ivec4 color; // Using unsigned ints for color
uniform mat4 u_view_projection;
uniform float u_position_scale; // Scale factor to convert from uint to world space
uniform float u_point_size_scale; // Added point size scaling
out vec4 v_color;
void main() {
// Convert uint positions to world space
vec3 worldPos = vec3(position) * u_position_scale;
gl_Position = u_view_projection * vec4(worldPos, 1.0);
gl_PointSize = max(u_point_size_scale * 10.0 * (1.0 - gl_Position.z / gl_Position.w), 1.0);
v_color = vec4(color) / 255.0; // Convert uint colors to float
}
"#;
const FRAGMENT_SHADER: &str = r#"
#version 330 core
in vec4 v_color;
out vec4 FragColor;
void main() {
// Create circular points
vec2 coord = gl_PointCoord * 2.0 - 1.0;
float r = dot(coord, coord);
if (r > 1.0) discard;
// if (coord.x > 1.0) discard;
// if (coord.y > 1.0) discard;
// Apply simple lighting based on depth
// float depth = gl_FragCoord.z;
FragColor = v_color;
}
"#;
// Camera controller for 3D navigation
#[derive(Clone)]
pub struct Camera {
position: Vec3,
pub orientation: Quat,
distance: f32,
pub point_size_scale: f32,
// last_pos: Option<Pos2>,
}
impl Camera {
pub fn new() -> Self {
Self {
position: Vec3::new(0.0, 0.0, 5.0),
orientation: Quat::IDENTITY,
distance: 5.0,
point_size_scale: 0.1,
// last_pos: None,
}
}
// pub fn reset(&mut self) {
// self.position = Vec3::new(0.0, 0.0, 5.0);
// self.orientation = Quat::IDENTITY;
// self.distance = 5.0;
// // self.point_size_scale = 0.1;
// self.update_view();
// }
pub fn update(&mut self, i: InputState) {
let mut changed = false;
if i.pointer.secondary_down() && !i.modifiers.shift {
let delta = i.pointer.delta();
let rotation_speed = 0.01;
let pitch = delta.y * rotation_speed;
let yaw = delta.x * rotation_speed;
let pitch_rotation = Quat::from_axis_angle(Vec3::X, -pitch);
let yaw_rotation = Quat::from_axis_angle(Vec3::Y, -yaw);
let roll_rotation = Quat::from_axis_angle(Vec3::Z, 0.);
self.orientation = self.orientation * pitch_rotation * yaw_rotation * roll_rotation;
self.orientation = self.orientation.normalize();
changed = true;
} // else if i.pointer.secondary_down() && i.modifiers.shift {
// let cur_pos = i.pointer.latest_pos();
// if let Some(last_pos) = self.last_pos {
// let last_angle = f32::atan2(last_pos.y, last_pos.x);
// if let Some(cur_pos) = cur_pos {
// let cur_angle = f32::atan2(cur_pos.y, cur_pos.x);
// println!("{}",cur_angle - last_angle);
// let pitch_rotation = Quat::from_axis_angle(Vec3::X, 0.);
// let yaw_rotation = Quat::from_axis_angle(Vec3::Y, 0.);
// let roll_rotation = Quat::from_axis_angle(Vec3::Z,cur_angle-last_angle);
// self.orientation = self.orientation * pitch_rotation * yaw_rotation * roll_rotation;
// self.orientation = self.orientation.normalize();
// changed = true;
// }
// }
// self.last_pos = cur_pos;
// }
let zoom_delta = i.smooth_scroll_delta.x + i.smooth_scroll_delta.y;
if zoom_delta != 0. {
if i.modifiers.shift {
// self.point_size_scale = (self.point_size_scale * (1. - zoom_delta * 0.001));
let scale_delta = zoom_delta * 0.01;
self.point_size_scale = (self.point_size_scale + scale_delta).clamp(0.1, 1000.0);
// println!("{}", self.point_size_scale);
} else {
self.distance *= (1.0 - zoom_delta * 0.001).max(0.1);
}
changed = true;
}
if i.pointer.primary_down() {
let delta = i.pointer.delta();
let pan_speed = self.distance * 0.001;
// Get camera-relative right and up vectors
let right = self.get_right();
let up = self.get_up();
// Move camera in the camera plane
let pan = right * (-delta.x * pan_speed) + up * (delta.y * pan_speed);
self.position += pan;
changed = true;
}
if changed {
self.update_view();
}
}
fn get_right(&self) -> Vec3 {
self.orientation * Vec3::X
}
fn get_up(&self) -> Vec3 {
self.orientation * Vec3::Y
}
fn get_forward(&self) -> Vec3 {
self.orientation * -Vec3::Z
}
fn update_view(&mut self) {
// Ensure orientation stays normalized
self.orientation = self.orientation.normalize();
}
pub fn get_view_matrix(&self) -> Mat4 {
// Calculate view position by moving back from target along view direction
let forward = self.get_forward();
let view_pos = self.position - forward * self.distance;
Mat4::look_at_rh(view_pos, self.position, self.get_up())
}
// pub fn set_point_size_scale(&mut self, scale: f32) {
// self.point_size_scale = scale.clamp(0.1, 10.0);
// }
}
// PLY parsing structures
#[derive(Debug)]
struct PlyHeader {
vertex_count: usize,
has_colors: bool,
is_binary: bool,
}
// #[derive(Debug)]
// pub struct PlyPoint {
// position: (i32, i32, i32),
// color: Color32,
// }
#[derive(Default)]
pub struct PointRenderer {
pub gl: Option<Arc<glow::Context>>,
program: Option<glow::Program>,
vao: Option<glow::VertexArray>,
vbo: Option<glow::Buffer>,
points: Option<Vec<i32>>,
// capacity: usize,
pub camera: Option<Camera>,
}
// impl Defalt for PointRenderer {
// fn default() -> Self {
// Self {
// gl: Option<Arc<glow::Context>>,
// program: Option<glow::Program>,
// vao: Option<glow::VertexArray>,
// vbo: Option<glow::Buffer>,
// points: Option<Vec<i32>>,
// // capacity: usize,
// camera: Option<Camera>,
// }
// }
// }
// }
impl PointRenderer {
pub fn init(
&mut self,
gl: Option<Arc<glow::Context>>,
initial_capacity: usize,
) -> Result<(), Box<dyn std::error::Error>> {
use glow::HasContext;
let gl = if let Some(gl) = gl {
gl
} else {
return Err("GL Not initilized!".into());
};
let program = unsafe {
let program = gl.create_program().expect("Cannot create program");
let vertex_shader = gl
.create_shader(glow::VERTEX_SHADER)
.expect("Cannot create vertex shader");
gl.shader_source(vertex_shader, VERTEX_SHADER);
gl.compile_shader(vertex_shader);
let fragment_shader = gl
.create_shader(glow::FRAGMENT_SHADER)
.expect("Cannot create fragment shader");
gl.shader_source(fragment_shader, FRAGMENT_SHADER);
gl.compile_shader(fragment_shader);
gl.attach_shader(program, vertex_shader);
gl.attach_shader(program, fragment_shader);
gl.link_program(program);
gl.delete_shader(vertex_shader);
gl.delete_shader(fragment_shader);
program
};
let vao = unsafe {
let vao = gl
.create_vertex_array()
.expect("Cannot create vertex array");
gl.bind_vertex_array(Some(vao));
vao
};
let vbo = unsafe {
let vbo = gl.create_buffer().expect("Cannot create vertex buffer");
gl.bind_buffer(glow::ARRAY_BUFFER, Some(vbo));
// Position (3) + Color (4) = 7 u32s per vertex
let buffer_size = initial_capacity * 7 * std::mem::size_of::<i32>();
gl.buffer_data_size(glow::ARRAY_BUFFER, buffer_size as i32, glow::DYNAMIC_DRAW);
// Position attribute (uvec3)
gl.vertex_attrib_pointer_i32(0, 3, glow::INT, 28, 0);
gl.enable_vertex_attrib_array(0);
// Color attribute (uvec4)
gl.vertex_attrib_pointer_i32(1, 4, glow::INT, 28, 12);
gl.enable_vertex_attrib_array(1);
vbo
};
self.gl = Some(gl);
self.program = Some(program);
self.vao = Some(vao);
self.vbo = Some(vbo);
self.points = Some(Vec::with_capacity(initial_capacity * 7));
// capacity: initial_capacity,
self.camera = Some(Camera::new());
Ok(())
}
pub fn add_point(&mut self, x: i32, y: i32, z: i32, color: Color32) {
let [r, g, b, a] = color.to_array();
self.points
.as_mut()
.as_mut()
.expect("Not Initialised")
.extend_from_slice(&[x, y, z, r as i32, g as i32, b as i32, a as i32]);
}
pub fn clear(&mut self) {
self.points
.as_mut()
.as_mut()
.expect("Not Initialised")
.clear();
}
pub fn render(&mut self, rect: Rect, input_state: Option<InputState>) {
use glow::HasContext;
// Update camera
if let Some(i) = input_state {
self.camera.as_mut().expect("Not Initialised").update(i);
}
unsafe {
self.gl
.as_mut()
.expect("Not Initialised")
.use_program(self.program);
// Set up view-projection matrix
let aspect = rect.width() / rect.height();
let projection = Mat4::perspective_rh(45.0f32.to_radians(), aspect, 0.1, 1000.0);
let view = self
.camera
.as_mut()
.expect("Not Initialised")
.get_view_matrix();
let view_projection = projection * view;
let location = self
.gl
.as_mut()
.expect("Not Initialised")
.get_uniform_location(
*self.program.as_mut().expect("Not Initialised"),
"u_view_projection",
)
.expect("Cannot get uniform location");
self.gl
.as_mut()
.expect("Not Initialised")
.uniform_matrix_4_f32_slice(
Some(&location),
false,
&view_projection.to_cols_array(),
);
// Set position scale factor (converts uint positions to world space)
let scale_location = self
.gl
.as_mut()
.expect("Not Initialised")
.get_uniform_location(
*self.program.as_mut().expect("Not Initialised"),
"u_position_scale",
)
.expect("Cannot get scale uniform location");
self.gl
.as_mut()
.expect("Not Initialised")
.uniform_1_f32(Some(&scale_location), 0.001); // Adjust this value to scale your point cloud
let point_size_location = self
.gl
.as_mut()
.expect("Not Initialised")
.get_uniform_location(
*self.program.as_mut().expect("Not Initialised"),
"u_point_size_scale",
)
.expect("Cannot get point size scale location");
self.gl.as_mut().expect("Not Initialised").uniform_1_f32(
Some(&point_size_location),
self.camera
.as_mut()
.expect("Not Initialised")
.point_size_scale,
);
self.gl
.as_mut()
.expect("Not Initialised")
.bind_vertex_array(self.vao);
self.gl.as_mut().expect("Not Initialised").bind_buffer(
glow::ARRAY_BUFFER,
Some(*self.vbo.as_mut().expect("Not Initialised")),
);
self.gl
.as_mut()
.expect("Not Initialised")
.buffer_sub_data_u8_slice(
glow::ARRAY_BUFFER,
0,
bytemuck::cast_slice(&self.points.as_mut().expect("Not Initialised")),
);
self.gl
.as_mut()
.expect("Not Initialised")
.enable(glow::PROGRAM_POINT_SIZE);
self.gl
.as_mut()
.expect("Not Initialised")
.enable(glow::DEPTH_TEST);
self.gl
.as_mut()
.expect("Not Initialised")
.clear_depth_f32(1.0);
self.gl
.as_mut()
.expect("Not Initialised")
.depth_func(glow::LESS);
self.gl.as_mut().expect("Not Initialised").depth_mask(true);
// self.gl.clear_color(0.3, 0.3, 0.3, 1.0);
self.gl
.as_mut()
.expect("Not Initialised")
.clear(glow::COLOR_BUFFER_BIT | glow::DEPTH_BUFFER_BIT);
self.gl.as_mut().expect("Not Initialised").draw_arrays(
glow::POINTS,
0,
(self
.points
.as_mut()
.as_mut()
.expect("Not Initialised")
.len()
/ 7) as i32,
);
self.gl
.as_mut()
.expect("Not Initialised")
.disable(glow::DEPTH_TEST);
self.gl
.as_mut()
.expect("Not Initialised")
.disable(glow::PROGRAM_POINT_SIZE);
}
}
// Add method to load points from PLY file
pub fn load_ply(
&mut self,
) -> Result<Vec<(i32, i32, i32, Color32)>, Box<dyn std::error::Error>> {
use dialog::DialogBox;
let choice = dialog::FileSelection::new("Please select a file")
.title("File Selection")
// .path("/home/user/Downloads")
.show()
.expect("Could not display dialog box");
let mut points = read_all::<VectorBuffer, _>("pointcloud.las")?;
let loaded_points: Vec<(i32, i32, i32, Color32)> = Vec::with_capacity(points.len());
let has_pos = points.point_layout().has_attribute(&POSITION_3D);
let has_color = points.point_layout().has_attribute(&COLOR_RGB);
if has_pos {
for position in points
.view_attribute::<Vector3<f64>>(&POSITION_3D)
.into_iter()
.take(10)
{
info!("({};{};{})", position.x, position.y, position.z);
}
}
if has_color {
for i in 0..points.len() {
let point = points.get_point_mut(i);
warn!("{:?}", point);
// println!("({};{};{})", color.x, position.y, position.z);
}
}
Err("".into())
// let file = File::open(path).map_err(|e| format!("Failed to open file: {}", e))?;
// let reader = BufReader::new(file);
// let mut lines = reader.lines();
// // Parse header
// let header = Self::parse_ply_header(&mut lines)?;
// // Clear existing points
// self.clear();
// // Reserve capacity
// self.points
// .as_mut()
// .as_mut()
// .expect("Not Initialised")
// .reserve(header.vertex_count * 7);
// // Parse vertices based on format
// if header.is_binary {
// return Err("Binary PLY files not yet supported".to_string());
// } else {
// self.parse_ascii_ply_data(lines, header)
// }
}
fn parse_ply_header<B: BufRead>(lines: &mut std::io::Lines<B>) -> Result<PlyHeader, String> {
let mut vertex_count = 0;
let mut has_colors = false;
let mut is_binary = false;
let in_header = true;
while in_header {
let line = lines
.next()
.ok_or("Unexpected end of file")
.unwrap()
.unwrap()
.trim()
.to_string();
match line.as_str() {
"ply" => continue,
"format ascii 1.0" => is_binary = false,
"format binary_little_endian 1.0" => is_binary = true,
"end_header" => break,
_ => {
if line.starts_with("element vertex ") {
vertex_count = line
.split_whitespace()
.last()
.ok_or("Invalid vertex count")?
.parse()
.map_err(|_| "Invalid vertex count")?;
} else if line.starts_with("property") && line.contains("red") {
has_colors = true;
}
}
}
}
Ok(PlyHeader {
vertex_count,
has_colors,
is_binary,
})
}
// fn parse_ascii_ply_data<B: BufRead>(
// &mut self,
// lines: std::io::Lines<B>,
// header: PlyHeader,
// ) -> Result<Vec<(i32, i32, i32, Color32)>, String> {
// let mut vec: Vec<(i32, i32, i32, Color32)> = Vec::new();
// for line in lines.take(header.vertex_count) {
// let line = line.map_err(|e| format!("Failed to read line: {}", e))?;
// let parts: Vec<&str> = line.split_whitespace().collect();
// if parts.len() < 3 {
// return Err("Invalid vertex data".to_string());
// }
// // Parse position
// let x = parts[0]
// .parse::<f32>()
// .map_err(|_| "Invalid X coordinate")?;
// let y = parts[1]
// .parse::<f32>()
// .map_err(|_| "Invalid Y coordinate")?;
// let z = parts[2]
// .parse::<f32>()
// .map_err(|_| "Invalid Z coordinate")?;
// // Convert to fixed point (scale by 1000 for better precision)
// let x = (x * 1000.0) as i32;
// let y = (y * 1000.0) as i32;
// let z = (z * 1000.0) as i32;
// // Parse colors if present
// let color = if header.has_colors && parts.len() >= 6 {
// let r = parts[3].parse::<u8>().unwrap_or(255);
// let g = parts[4].parse::<u8>().unwrap_or(255);
// let b = parts[5].parse::<u8>().unwrap_or(255);
// Color32::from_rgb(r, g, b)
// } else {
// Color32::WHITE
// };
// vec.push((x, y, z, color));
// // self.add_point(x, y, z, color);
// }
// Ok(vec)
// }
}
impl Drop for PointRenderer {
fn drop(&mut self) {
// Clean up GPU resources
}
}
tof.py
-304
View File
@@ -1,304 +0,0 @@
import struct
import numpy as np
import cv2
from PIL import Image
#import matplotlib.pyplot as plt
import requests
from depth import img2depth
from matchdepth import align_depth_maps
# import open3d as o3d
HOST = '192.168.233.1'
PORT = 80
def get_frame_from_http(host=HOST, port=PORT):
r = requests.get('http://{}:{}/getdeep'.format(host, port))
if(r.status_code == requests.codes.ok):
# print('Get deep image')
deepimg = r.content
# print('Length={}'.format(len(deepimg)))
(frameid, stamp_msec) = struct.unpack('<QQ', deepimg[0:8+8])
# print((frameid, stamp_msec/1000))
return deepimg
def post_encode_config(config, host=HOST, port=PORT):
r = requests.post('http://{}:{}/set_cfg'.format(host, port), config)
if(r.status_code == requests.codes.ok):
return True
return False
def frame_config_decode(frame_config):
'''
@frame_config bytes
@return fields, tuple (trigger_mode, deep_mode, deep_shift, ir_mode, status_mode, status_mask, rgb_mode, rgb_res, expose_time)
'''
return struct.unpack("<BBBBBBBBi", frame_config)
def frame_config_encode(trigger_mode=1, deep_mode=1, deep_shift=255, ir_mode=1, status_mode=2, status_mask=7, rgb_mode=1, rgb_res=0, expose_time=0):
'''
@trigger_mode, deep_mode, deep_shift, ir_mode, status_mode, status_mask, rgb_mode, rgb_res, expose_time
@return frame_config bytes
'''
return struct.pack("<BBBBBBBBi",
trigger_mode, deep_mode, deep_shift, ir_mode, status_mode, status_mask, rgb_mode, rgb_res, expose_time)
def frame_payload_decode(frame_data: bytes, with_config: tuple):
'''
@frame_data, bytes
@with_config, tuple (trigger_mode, deep_mode, deep_shift, ir_mode, status_mode, status_mask, rgb_mode, rgb_res, expose_time)
@return imgs, tuple (deepth_img, ir_img, status_img, rgb_img)
'''
deep_data_size, rgb_data_size = struct.unpack("<ii", frame_data[:8])
frame_payload = frame_data[8:]
# 0:16bit 1:8bit, resolution: 320*240
deepth_size = (320*240*2) >> with_config[1]
deepth_img = struct.unpack("<%us" % deepth_size, frame_payload[:deepth_size])[
0] if 0 != deepth_size else None
frame_payload = frame_payload[deepth_size:]
# 0:16bit 1:8bit, resolution: 320*240
ir_size = (320*240*2) >> with_config[3]
ir_img = struct.unpack("<%us" % ir_size, frame_payload[:ir_size])[
0] if 0 != ir_size else None
frame_payload = frame_payload[ir_size:]
status_size = (320*240//8) * (16 if 0 == with_config[4] else
2 if 1 == with_config[4] else 8 if 2 == with_config[4] else 1)
status_img = struct.unpack("<%us" % status_size, frame_payload[:status_size])[
0] if 0 != status_size else None
frame_payload = frame_payload[status_size:]
assert(deep_data_size == deepth_size+ir_size+status_size)
rgb_size = len(frame_payload)
assert(rgb_data_size == rgb_size)
rgb_img = struct.unpack("<%us" % rgb_size, frame_payload[:rgb_size])[
0] if 0 != rgb_size else None
if (not rgb_img is None):
if (1 == with_config[6]):
jpeg = cv2.imdecode(np.frombuffer(
rgb_img, 'uint8', rgb_size), cv2.IMREAD_COLOR)
if not jpeg is None:
rgb = cv2.cvtColor(jpeg, cv2.COLOR_BGR2RGB)
rgb_img = rgb.tobytes()
else:
rgb_img = None
# elif 0 == with_config[6]:
# yuv = np.frombuffer(rgb_img, 'uint8', rgb_size)
# print(len(yuv))
# if not yuv is None:
# rgb = cv2.cvtColor(yuv, cv2.COLOR_YUV420P2RGB)
# rgb_img = rgb.tobytes()
# else:
# rgb_img = None
return (deepth_img, ir_img, status_img, rgb_img)
def scale_and_shift(image, scale_factor, shift_x, shift_y):
"""
Scale an RGB image and shift it by n pixels in x and y direction.
Parameters:
-----------
image : numpy.ndarray
Input RGB image with shape (height, width, 3)
scale_factor : float
Scale factor (e.g., 0.5 for half size, 2.0 for double size)
shift_x : int
Number of pixels to shift in x direction (positive: right, negative: left)
shift_y : int
Number of pixels to shift in y direction (positive: down, negative: up)
Returns:
--------
numpy.ndarray
Scaled and shifted image with the same shape as the input image
"""
# Get original image dimensions
height, width = image.shape[:2]
# Calculate new dimensions after scaling
new_height = int(height * scale_factor)
new_width = int(width * scale_factor)
# Scale the image
scaled_image = cv2.resize(image, (new_width, new_height), interpolation=cv2.INTER_LINEAR)
# Create a transformation matrix for the shift
M = np.float32([[1, 0, shift_x], [0, 1, shift_y]])
# Apply the shift to the scaled image
shifted_image = cv2.warpAffine(scaled_image, M, (new_width, new_height))
# Create a blank canvas with original dimensions
result = np.zeros_like(image)
# Calculate the region to copy from the shifted_scaled image
y_start = max(0, -shift_y)
y_end = min(new_height, height - shift_y)
x_start = max(0, -shift_x)
x_end = min(new_width, width - shift_x)
# Calculate the region to paste into the result image
result_y_start = max(0, shift_y)
result_y_end = min(height, new_height + shift_y)
result_x_start = max(0, shift_x)
result_x_end = min(width, new_width + shift_x)
# Copy the visible part of the shifted image to the result
if (y_end > y_start and x_end > x_start and
result_y_end > result_y_start and result_x_end > result_x_start):
result[result_y_start:result_y_end, result_x_start:result_x_end] = shifted_image[y_start:y_end, x_start:x_end]
return result
prev_status = None
prev_depth = None
def show_frame(frame_data: bytes):
global prev_status
global prev_depth
config = frame_config_decode(frame_data[16:16+12])
frame_bytes = frame_payload_decode(frame_data[16+12:], config)
depth = np.frombuffer(frame_bytes[0], 'uint16' if 0 == config[1] else 'uint8').reshape(
240, 320) if frame_bytes[0] else None
ir = np.frombuffer(frame_bytes[1], 'uint16' if 0 == config[3] else 'uint8').reshape(
240, 320) if frame_bytes[1] else None
status = np.frombuffer(frame_bytes[2], 'uint16' if 0 == config[4] else 'uint8').reshape(
240, 320) if frame_bytes[2] else None
rgb = np.frombuffer(frame_bytes[3], 'uint8').reshape(
(480, 640, 3) if config[6] == 1 else (600, 800, 3)) if frame_bytes[3] else None
if not (depth is None or status is None or rgb is None):
rgb = cv2.resize(rgb, dsize=(320, 240), interpolation=cv2.INTER_CUBIC) # Resize
rgb = scale_and_shift(rgb, 1.1, -10, -10)
status = 1-status
if prev_status is None:
mask = (status)
else:
mask = (status)*(prev_status)
prev_status = status
depth = depth*mask
if prev_depth is not None:
new_depth = (depth + prev_depth)/2
prev_depth = depth
depth = new_depth
else:
prev_depth = depth
img_depth = img2depth(rgb)
aligned_img_depth = align_depth_maps(depth, img_depth, mask)*(1-mask)
return (aligned_img_depth + depth), rgb, mask
return None
# create visualizer and window.
# vis = o3d.visualization.Visualizer()
# vis.create_window(height=480, width=640)
# pcd = o3d.geometry.PointCloud()
# pcd.points = o3d.utility.Vector3dVector(np.random.rand(10, 3))
# vis.add_geometry(pcd)
# depth_to_color_translation = np.array([0, 0, 0]) # 5cm offset in x
# depth_to_color_rotation = np.eye(3) # Identity matrix if cameras are parallel
# color_intrinsics = (520, 520, 325, 245)
keep_running = True
photocount = 0
while keep_running:
if post_encode_config(frame_config_encode(1,0,255,0,2,7,1,0,0)):
p = get_frame_from_http()
depth_image, rgb, mask = show_frame(p)
if depth_image is None: continue
depth_colored = cv2.applyColorMap((depth_image).astype(np.uint8), cv2.COLORMAP_JET)
# mask = (depth_image>1000)
# b = np.repeat((depth_image>10)[:, :, np.newaxis], 3, axis=2)
# b = np.repeat((mask==1)[:, :, np.newaxis], 3, axis=2)
cv2.imshow("depth", depth_colored)
cv2.imshow("rgb", rgb)
key = cv2.waitKey(1)
if key & 0xFF == 27:
break
elif key & 0xFF == 32:
photocount += 1
depth = Image.fromarray(depth_image)
rgb = Image.fromarray(rgb)
depth.save(f"./depth/depth-{photocount}.png")
rgb.save(f"./rgb/rgb-{photocount}.png")
print(f"Took photo {photocount}!")
# points, colors = depth_to_colored_points(
# depth_image,
# color_image,
# x_map,
# y_map,
# color_intrinsics,
# depth_to_color_translation,
# depth_to_color_rotation
# )
# points = depth_image.reshape((-1, 3))
# colors = color_image.reshape((-1, 3)).astype(np.float64) / 255.0
# colors[:, [0, 2]] = colors[:, [2, 0]]
# print(points.shape)
# print(colors.shape)
# pcd.points = o3d.utility.Vector3dVector(points)
# pcd.colors = o3d.utility.Vector3dVector(colors)
# vis.update_geometry(pcd)
# keep_running = vis.poll_events()
# vis.update_renderer()
# pcd.points.extend(np.random.rand(n_new, 3))
# cv2.waitKey(1)
# with open("rgbd.raw", 'wb') as f:
# f.write(p)
# f.flush()
cv2.destroyAllWindows()