refactor ZED capture into modular camera class with env config
Extract camera lifecycle (open, warmup, grab, close) and
environment-based configuration into src/zed/camera.{h,cc}.
Thin main.cc now only handles the dora event loop.
Closes #2
This commit is contained in:
cristhian aguilera
+151
@@ -0,0 +1,151 @@
|
||||
#include "zed/camera.h"
|
||||
|
||||
#include <dora-node-api.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
#include <chrono>
|
||||
#include <initializer_list>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
|
||||
namespace {
|
||||
|
||||
rust::Vec<int64_t> make_shape(int height, int width, int channels) {
|
||||
rust::Vec<int64_t> shape;
|
||||
shape.push_back(height);
|
||||
shape.push_back(width);
|
||||
shape.push_back(channels);
|
||||
return shape;
|
||||
}
|
||||
|
||||
rust::Vec<double> make_distortion(const std::vector<double>& distortion) {
|
||||
rust::Vec<double> values;
|
||||
for (double v : distortion) {
|
||||
values.push_back(v);
|
||||
}
|
||||
return values;
|
||||
}
|
||||
|
||||
rust::Vec<int64_t> make_vector_int(std::initializer_list<int64_t> values) {
|
||||
rust::Vec<int64_t> out;
|
||||
for (int64_t v : values) {
|
||||
out.push_back(v);
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main() {
|
||||
auto dora_node = init_dora_node();
|
||||
|
||||
auto config = zed::Config::from_env();
|
||||
zed::Camera camera(config);
|
||||
camera.open();
|
||||
camera.warmup();
|
||||
auto calib = camera.calibration();
|
||||
|
||||
int64_t frame_id = 0;
|
||||
while (true) {
|
||||
auto event = dora_node.events->next();
|
||||
auto type = event_type(event);
|
||||
|
||||
if (type == DoraEventType::Stop || type == DoraEventType::AllInputsClosed) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (type != DoraEventType::Input) {
|
||||
continue;
|
||||
}
|
||||
|
||||
auto input = event_as_input(std::move(event));
|
||||
(void)input;
|
||||
|
||||
if (!camera.grab()) {
|
||||
std::cerr << "Grab failed; attempting to reconnect..." << std::endl;
|
||||
camera.close();
|
||||
camera.open();
|
||||
calib = camera.calibration();
|
||||
continue;
|
||||
}
|
||||
|
||||
sl::Mat& image_mat = camera.image();
|
||||
sl::Mat& point_cloud_mat = camera.point_cloud();
|
||||
|
||||
int width = image_mat.getWidth();
|
||||
int height = image_mat.getHeight();
|
||||
|
||||
cv::Mat image_rgba(height, width, CV_8UC4, image_mat.getPtr<sl::uchar1>());
|
||||
cv::Mat image_bgr;
|
||||
cv::cvtColor(image_rgba, image_bgr, cv::COLOR_BGRA2BGR);
|
||||
|
||||
const auto image_size = static_cast<size_t>(image_bgr.total() * image_bgr.elemSize());
|
||||
rust::Slice<const uint8_t> image_slice{
|
||||
reinterpret_cast<const uint8_t*>(image_bgr.data), image_size};
|
||||
|
||||
auto image_metadata = new_metadata();
|
||||
image_metadata->set_string("dtype", "uint8");
|
||||
image_metadata->set_string("encoding", "bgr8");
|
||||
image_metadata->set_string("layout", "HWC");
|
||||
image_metadata->set_list_int("shape", make_shape(height, width, 3));
|
||||
image_metadata->set_float("fx", calib.fx);
|
||||
image_metadata->set_float("fy", calib.fy);
|
||||
image_metadata->set_float("cx", calib.cx);
|
||||
image_metadata->set_float("cy", calib.cy);
|
||||
image_metadata->set_list_float("distortion", make_distortion(calib.distortion));
|
||||
image_metadata->set_int("frame_id", frame_id++);
|
||||
image_metadata->set_int("timestamp_ns", static_cast<int64_t>(
|
||||
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::steady_clock::now().time_since_epoch()
|
||||
).count()
|
||||
));
|
||||
|
||||
auto image_result = send_output_with_metadata(
|
||||
dora_node.send_output, "image_bgr", image_slice, std::move(image_metadata));
|
||||
if (!std::string(image_result.error).empty()) {
|
||||
std::cerr << "Error sending image_bgr: " << std::string(image_result.error) << std::endl;
|
||||
}
|
||||
|
||||
int pc_width = point_cloud_mat.getWidth();
|
||||
int pc_height = point_cloud_mat.getHeight();
|
||||
const size_t pc_size = static_cast<size_t>(pc_width * pc_height * sizeof(sl::float4));
|
||||
const auto* pc_ptr = reinterpret_cast<const uint8_t*>(point_cloud_mat.getPtr<sl::float4>());
|
||||
rust::Slice<const uint8_t> pc_slice{pc_ptr, pc_size};
|
||||
|
||||
auto pc_metadata = new_metadata();
|
||||
pc_metadata->set_string("dtype", "float32");
|
||||
pc_metadata->set_string("layout", "HWC");
|
||||
pc_metadata->set_string("channels", "XYZRGBA");
|
||||
pc_metadata->set_string("units", "mm");
|
||||
pc_metadata->set_list_int("shape", make_shape(pc_height, pc_width, 4));
|
||||
pc_metadata->set_float("fx", calib.fx);
|
||||
pc_metadata->set_float("fy", calib.fy);
|
||||
pc_metadata->set_float("cx", calib.cx);
|
||||
pc_metadata->set_float("cy", calib.cy);
|
||||
pc_metadata->set_list_float("distortion", make_distortion(calib.distortion));
|
||||
|
||||
auto pc_result = send_output_with_metadata(
|
||||
dora_node.send_output, "point_cloud", pc_slice, std::move(pc_metadata));
|
||||
if (!std::string(pc_result.error).empty()) {
|
||||
std::cerr << "Error sending point_cloud: " << std::string(pc_result.error) << std::endl;
|
||||
}
|
||||
|
||||
float camera_data[4] = {calib.fx, calib.fy, calib.cx, calib.cy};
|
||||
rust::Slice<const uint8_t> cam_slice{
|
||||
reinterpret_cast<const uint8_t*>(camera_data), sizeof(camera_data)};
|
||||
|
||||
auto cam_metadata = new_metadata();
|
||||
cam_metadata->set_string("dtype", "float32");
|
||||
cam_metadata->set_string("layout", "C");
|
||||
cam_metadata->set_list_int("shape", make_vector_int({4}));
|
||||
cam_metadata->set_list_float("distortion", make_distortion(calib.distortion));
|
||||
|
||||
auto cam_result = send_output_with_metadata(
|
||||
dora_node.send_output, "camera_info", cam_slice, std::move(cam_metadata));
|
||||
if (!std::string(cam_result.error).empty()) {
|
||||
std::cerr << "Error sending camera_info: " << std::string(cam_result.error) << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,175 @@
|
||||
#include "zed/camera.h"
|
||||
|
||||
#include <cctype>
|
||||
#include <chrono>
|
||||
#include <cstdlib>
|
||||
#include <iostream>
|
||||
#include <iterator>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
namespace {
|
||||
|
||||
int get_env_int(const char* name, int default_value) {
|
||||
const char* value = std::getenv(name);
|
||||
if (!value || std::string(value).empty()) {
|
||||
return default_value;
|
||||
}
|
||||
return std::stoi(value);
|
||||
}
|
||||
|
||||
bool get_env_bool(const char* name, bool default_value) {
|
||||
const char* value = std::getenv(name);
|
||||
if (!value || std::string(value).empty()) {
|
||||
return default_value;
|
||||
}
|
||||
std::string v(value);
|
||||
for (auto& c : v) {
|
||||
c = static_cast<char>(std::tolower(c));
|
||||
}
|
||||
return v == "1" || v == "true" || v == "yes" || v == "on";
|
||||
}
|
||||
|
||||
std::string get_env_str(const char* name, const std::string& default_value) {
|
||||
const char* value = std::getenv(name);
|
||||
if (!value || std::string(value).empty()) {
|
||||
return default_value;
|
||||
}
|
||||
return std::string(value);
|
||||
}
|
||||
|
||||
sl::RESOLUTION parse_resolution(int resolution) {
|
||||
switch (resolution) {
|
||||
case 1080:
|
||||
return sl::RESOLUTION::HD1080;
|
||||
case 2000:
|
||||
return sl::RESOLUTION::HD2K;
|
||||
case 720:
|
||||
default:
|
||||
return sl::RESOLUTION::HD720;
|
||||
}
|
||||
}
|
||||
|
||||
sl::DEPTH_MODE parse_depth_mode(const std::string& mode) {
|
||||
if (mode == "PERFORMANCE") {
|
||||
return sl::DEPTH_MODE::PERFORMANCE;
|
||||
}
|
||||
if (mode == "QUALITY") {
|
||||
return sl::DEPTH_MODE::QUALITY;
|
||||
}
|
||||
if (mode == "NONE") {
|
||||
return sl::DEPTH_MODE::NONE;
|
||||
}
|
||||
return sl::DEPTH_MODE::NEURAL;
|
||||
}
|
||||
|
||||
sl::FLIP_MODE parse_flip(const std::string& mode) {
|
||||
if (mode == "OFF") {
|
||||
return sl::FLIP_MODE::OFF;
|
||||
}
|
||||
return sl::FLIP_MODE::ON;
|
||||
}
|
||||
|
||||
void list_available_cameras() {
|
||||
auto devices = sl::Camera::getDeviceList();
|
||||
std::cout << "Detected ZED cameras: " << devices.size() << std::endl;
|
||||
for (std::size_t i = 0; i < devices.size(); i++) {
|
||||
const auto& dev = devices[i];
|
||||
std::cout << " - [" << i << "] model=" << sl::toString(dev.camera_model)
|
||||
<< ", serial=" << dev.serial_number << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
namespace zed {
|
||||
|
||||
Config Config::from_env() {
|
||||
Config c;
|
||||
c.resolution = get_env_int("ZED_RESOLUTION", c.resolution);
|
||||
c.fps = get_env_int("ZED_FPS", c.fps);
|
||||
c.depth_mode = get_env_str("ZED_DEPTH_MODE", c.depth_mode);
|
||||
c.depth_min_mm = get_env_int("ZED_DEPTH_MIN_MM", c.depth_min_mm);
|
||||
c.depth_max_mm = get_env_int("ZED_DEPTH_MAX_MM", c.depth_max_mm);
|
||||
c.depth_fill = get_env_bool("ZED_DEPTH_FILL", c.depth_fill);
|
||||
c.flip = get_env_str("ZED_FLIP", c.flip);
|
||||
c.warmup_frames = get_env_int("ZED_WARMUP_FRAMES", c.warmup_frames);
|
||||
int retry_seconds = get_env_int("ZED_RETRY_INTERVAL_SECONDS", 5);
|
||||
c.retry_interval_ms = retry_seconds * 1000;
|
||||
return c;
|
||||
}
|
||||
|
||||
Camera::Camera(const Config& config) : config_(config) {
|
||||
init_params_.camera_resolution = parse_resolution(config_.resolution);
|
||||
init_params_.camera_fps = config_.fps;
|
||||
init_params_.depth_mode = parse_depth_mode(config_.depth_mode);
|
||||
init_params_.coordinate_units = sl::UNIT::MILLIMETER;
|
||||
init_params_.camera_image_flip = parse_flip(config_.flip);
|
||||
init_params_.depth_minimum_distance = config_.depth_min_mm;
|
||||
init_params_.depth_maximum_distance = config_.depth_max_mm;
|
||||
|
||||
runtime_params_.enable_depth = true;
|
||||
runtime_params_.enable_fill_mode = config_.depth_fill;
|
||||
}
|
||||
|
||||
Camera::~Camera() {
|
||||
close();
|
||||
}
|
||||
|
||||
void Camera::open() {
|
||||
list_available_cameras();
|
||||
while (true) {
|
||||
sl::ERROR_CODE err = zed_.open(init_params_);
|
||||
if (err == sl::ERROR_CODE::SUCCESS) {
|
||||
return;
|
||||
}
|
||||
std::cerr << "Failed to open ZED camera: " << sl::toString(err) << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(config_.retry_interval_ms));
|
||||
}
|
||||
}
|
||||
|
||||
void Camera::close() {
|
||||
zed_.close();
|
||||
}
|
||||
|
||||
void Camera::warmup() {
|
||||
for (int i = 0; i < config_.warmup_frames; i++) {
|
||||
zed_.grab(runtime_params_);
|
||||
}
|
||||
}
|
||||
|
||||
bool Camera::grab() {
|
||||
if (zed_.grab(runtime_params_) != sl::ERROR_CODE::SUCCESS) {
|
||||
return false;
|
||||
}
|
||||
zed_.retrieveImage(image_mat_, sl::VIEW::LEFT);
|
||||
zed_.retrieveMeasure(point_cloud_mat_, sl::MEASURE::XYZRGBA);
|
||||
return true;
|
||||
}
|
||||
|
||||
Calibration Camera::calibration() const {
|
||||
sl::CameraInformation info = zed_.getCameraInformation();
|
||||
auto calib = info.camera_configuration.calibration_parameters;
|
||||
auto left = calib.left_cam;
|
||||
|
||||
Calibration result;
|
||||
result.fx = left.fx;
|
||||
result.fy = left.fy;
|
||||
result.cx = left.cx;
|
||||
result.cy = left.cy;
|
||||
result.distortion.reserve(std::size(left.disto));
|
||||
for (double v : left.disto) {
|
||||
result.distortion.push_back(v);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
sl::Mat& Camera::image() {
|
||||
return image_mat_;
|
||||
}
|
||||
|
||||
sl::Mat& Camera::point_cloud() {
|
||||
return point_cloud_mat_;
|
||||
}
|
||||
|
||||
} // namespace zed
|
||||
@@ -0,0 +1,50 @@
|
||||
#pragma once
|
||||
|
||||
#include <sl/Camera.hpp>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace zed {
|
||||
|
||||
struct Config {
|
||||
int resolution = 720;
|
||||
int fps = 15;
|
||||
std::string depth_mode = "NEURAL";
|
||||
int depth_min_mm = 10;
|
||||
int depth_max_mm = 500;
|
||||
bool depth_fill = false;
|
||||
std::string flip = "ON";
|
||||
int warmup_frames = 30;
|
||||
int retry_interval_ms = 5000;
|
||||
|
||||
static Config from_env();
|
||||
};
|
||||
|
||||
struct Calibration {
|
||||
float fx, fy, cx, cy;
|
||||
std::vector<double> distortion;
|
||||
};
|
||||
|
||||
class Camera {
|
||||
public:
|
||||
explicit Camera(const Config& config);
|
||||
~Camera();
|
||||
|
||||
void open();
|
||||
void close();
|
||||
void warmup();
|
||||
bool grab();
|
||||
Calibration calibration() const;
|
||||
sl::Mat& image();
|
||||
sl::Mat& point_cloud();
|
||||
|
||||
private:
|
||||
Config config_;
|
||||
sl::Camera zed_;
|
||||
sl::InitParameters init_params_;
|
||||
sl::RuntimeParameters runtime_params_;
|
||||
sl::Mat image_mat_;
|
||||
sl::Mat point_cloud_mat_;
|
||||
};
|
||||
|
||||
} // namespace zed
|
||||
Reference in New Issue
Block a user