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:
@@ -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
|
||||
Reference in New Issue
Block a user