i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-05 20:47:46 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
29f0bcc4d02e24753d81db106395b9b1880751e9
OrbbecSDK_ROS2/orbbec_camera/src/utils.cpp
T

1204 lines
39 KiB
C++
Raw Blame History

/*******************************************************************************
* Copyright (c) 2023 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
#include <algorithm>
#include <chrono>
#include <cstdlib>
#include <filesystem>
#include <iomanip>
#include <mutex>
#include <regex>
#include <sstream>
#include <vector>
#include "orbbec_camera/utils.h"
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include "orbbec_camera/constants.h"
namespace orbbec_camera {
OBLogSeverity obLogSeverityFromString(const std::string_view &log_level) {
if (log_level == "debug") {
return OBLogSeverity::OB_LOG_SEVERITY_DEBUG;
}
if (log_level == "info") {
return OBLogSeverity::OB_LOG_SEVERITY_INFO;
}
if (log_level == "warn" || log_level == "warning") {
return OBLogSeverity::OB_LOG_SEVERITY_WARN;
}
if (log_level == "error") {
return OBLogSeverity::OB_LOG_SEVERITY_ERROR;
}
if (log_level == "fatal") {
return OBLogSeverity::OB_LOG_SEVERITY_FATAL;
}
return OBLogSeverity::OB_LOG_SEVERITY_OFF;
}
std::string getObSdkLogDirectory() {
const char *log_dir_override = std::getenv("ORBBEC_LOG_DIR");
if (log_dir_override && log_dir_override[0] != '\0') {
return (std::filesystem::path(log_dir_override) / "Log").string();
}
const char *home = std::getenv("HOME");
const std::filesystem::path home_dir = home != nullptr ? home : "";
return (home_dir / ".ros" / "Log").string();
}
std::string configureObSdkLoggerForTool(const std::string &tool_name,
const std::string &log_level) {
const auto severity = obLogSeverityFromString(log_level);
ob::Context::setLoggerSeverity(severity);
ob::Context::setLoggerToConsole(OBLogSeverity::OB_LOG_SEVERITY_OFF);
if (severity == OBLogSeverity::OB_LOG_SEVERITY_OFF) {
ob::Context::setLoggerToFile(OBLogSeverity::OB_LOG_SEVERITY_OFF, "");
return "";
}
const std::filesystem::path log_dir(getObSdkLogDirectory());
std::filesystem::create_directories(log_dir);
const auto now = std::chrono::system_clock::now();
const std::time_t now_time = std::chrono::system_clock::to_time_t(now);
std::tm tm{};
#if defined(_WIN32)
localtime_s(&tm, &now_time);
#else
localtime_r(&now_time, &tm);
#endif
std::ostringstream file_name;
file_name << tool_name << "_sdk_" << std::put_time(&tm, "%Y%m%d_%H%M%S") << ".log";
ob::Context::setLoggerFileName(file_name.str());
ob::Context::setLoggerToFile(severity, log_dir.string().c_str());
return (log_dir / file_name.str()).string();
}
sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
OBCameraDistortion distortion, int width) {
(void)width;
sensor_msgs::msg::CameraInfo info;
info.distortion_model = getDistortionModels(distortion);
info.width = intrinsic.width;
info.height = intrinsic.height;
info.d.resize(8, 0.0);
info.d[0] = distortion.k1;
info.d[1] = distortion.k2;
info.d[2] = distortion.p1;
info.d[3] = distortion.p2;
info.d[4] = distortion.k3;
info.d[5] = distortion.k4;
info.d[6] = distortion.k5;
info.d[7] = distortion.k6;
bool all_zero = std::all_of(info.d.begin(), info.d.end(), [](double val) { return val == 0.0; });
info.roi.do_rectify = all_zero;
info.k.fill(0.0);
info.k[0] = intrinsic.fx;
info.k[2] = intrinsic.cx;
info.k[4] = intrinsic.fy;
info.k[5] = intrinsic.cy;
info.k[8] = 1.0;
info.r.fill(0.0);
info.r[0] = 1;
info.r[4] = 1;
info.r[8] = 1;
info.p.fill(0.0);
info.p[0] = info.k[0];
info.p[2] = info.k[2];
info.p[5] = info.k[4];
info.p[6] = info.k[5];
info.p[10] = 1.0;
return info;
}
void saveRGBPointsToPly(const std::shared_ptr<ob::Frame> &frame, const std::string &fileName) {
size_t point_size = frame->getDataSize() / sizeof(OBColorPoint);
FILE *fp = fopen(fileName.c_str(), "wb+");
std::shared_ptr<int> fp_guard(nullptr, [&fp](int *) {
fflush(fp);
fclose(fp);
});
fprintf(fp, "ply\n");
fprintf(fp, "format ascii 1.0\n");
fprintf(fp, "element vertex %zu\n", point_size);
fprintf(fp, "property float x\n");
fprintf(fp, "property float y\n");
fprintf(fp, "property float z\n");
fprintf(fp, "property uchar red\n");
fprintf(fp, "property uchar green\n");
fprintf(fp, "property uchar blue\n");
fprintf(fp, "end_header\n");
const auto *points = (OBColorPoint *)frame->getData();
CHECK_NOTNULL(points);
for (size_t i = 0; i < point_size; i++) {
fprintf(fp, "%.3f %.3f %.3f %d %d %d\n", points[i].x, points[i].y, points[i].z,
(int)points[i].r, (int)points[i].g, (int)points[i].b);
}
}
void saveRGBPointCloudMsgToPly(const sensor_msgs::msg::PointCloud2::UniquePtr &msg,
const std::string &fileName) {
FILE *fp = fopen(fileName.c_str(), "wb+");
CHECK_NOTNULL(fp);
CHECK_NOTNULL(msg);
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*msg, "z");
// First, count the actual number of valid points
size_t valid_points = 0;
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
++valid_points;
}
}
// Reset the iterators
iter_x = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "x");
iter_y = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "y");
iter_z = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "z");
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_r(*msg, "r");
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_g(*msg, "g");
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_b(*msg, "b");
fprintf(fp, "ply\n");
fprintf(fp, "format ascii 1.0\n");
fprintf(fp, "element vertex %zu\n", valid_points);
fprintf(fp, "property float x\n");
fprintf(fp, "property float y\n");
fprintf(fp, "property float z\n");
fprintf(fp, "property uchar red\n");
fprintf(fp, "property uchar green\n");
fprintf(fp, "property uchar blue\n");
fprintf(fp, "end_header\n");
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b) {
if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
fprintf(fp, "%.3f %.3f %.3f %d %d %d\n", *iter_x, *iter_y, *iter_z, (int)*iter_r,
(int)*iter_g, (int)*iter_b);
}
}
fflush(fp);
fclose(fp);
}
void saveDepthPointsToPly(const sensor_msgs::msg::PointCloud2::UniquePtr &msg,
const std::string &fileName) {
FILE *fp = fopen(fileName.c_str(), "wb+");
CHECK_NOTNULL(fp);
CHECK_NOTNULL(msg);
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*msg, "z");
// First, count the actual number of valid points
size_t valid_points = 0;
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
++valid_points;
}
}
// Reset the iterators
iter_x = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "x");
iter_y = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "y");
iter_z = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "z");
fprintf(fp, "ply\n");
fprintf(fp, "format ascii 1.0\n");
fprintf(fp, "element vertex %zu\n", valid_points);
fprintf(fp, "property float x\n");
fprintf(fp, "property float y\n");
fprintf(fp, "property float z\n");
fprintf(fp, "end_header\n");
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
fprintf(fp, "%.3f %.3f %.3f\n", *iter_x, *iter_y, *iter_z);
}
}
fflush(fp);
fclose(fp);
}
void savePointsToPly(const std::shared_ptr<ob::Frame> &frame, const std::string &fileName) {
size_t point_size = frame->getDataSize() / sizeof(OBPoint);
FILE *fp = fopen(fileName.c_str(), "wb+");
std::shared_ptr<int> fp_guard(nullptr, [&fp](int *) {
fflush(fp);
fclose(fp);
});
CHECK_NOTNULL(fp);
CHECK_NOTNULL(frame);
fprintf(fp, "ply\n");
fprintf(fp, "format ascii 1.0\n");
fprintf(fp, "element vertex %zu\n", point_size);
fprintf(fp, "property float x\n");
fprintf(fp, "property float y\n");
fprintf(fp, "property float z\n");
fprintf(fp, "end_header\n");
const auto *points = (OBPoint *)frame->getData();
CHECK_NOTNULL(points);
for (size_t i = 0; i < point_size; i++) {
fprintf(fp, "%.3f %.3f %.3f\n", points[i].x, points[i].y, points[i].z);
}
}
tf2::Quaternion rotationMatrixToQuaternion(const float rotation[9]) {
Eigen::Matrix3f m;
m << rotation[0], rotation[1], rotation[2], rotation[3], rotation[4], rotation[5], rotation[6],
rotation[7], rotation[8];
Eigen::Quaternionf q(m);
return {q.x(), q.y(), q.z(), q.w()};
}
std::ostream &operator<<(std::ostream &os, const OBCameraParam &rhs) {
auto depth_intrinsic = rhs.depthIntrinsic;
auto rgb_intrinsic = rhs.rgbIntrinsic;
os << "=====depth intrinsic=====\n";
os << "fx : " << depth_intrinsic.fx << "\n";
os << "fy : " << depth_intrinsic.fy << "\n";
os << "cx : " << depth_intrinsic.cx << "\n";
os << "cy : " << depth_intrinsic.cy << "\n";
os << "width : " << depth_intrinsic.width << "\n";
os << "height : " << depth_intrinsic.height << "\n";
os << "=====rgb intrinsic=====\n";
os << "fx : " << rgb_intrinsic.fx << "\n";
os << "fy : " << rgb_intrinsic.fy << "\n";
os << "cx : " << rgb_intrinsic.cx << "\n";
os << "cy : " << rgb_intrinsic.cy << "\n";
os << "width : " << rgb_intrinsic.width << "\n";
os << "height : " << rgb_intrinsic.height << "\n";
return os;
}
orbbec_camera_msgs::msg::Extrinsics obExtrinsicsToMsg(const OBD2CTransform &extrinsics,
const std::string &frame_id) {
orbbec_camera_msgs::msg::Extrinsics msg;
for (int i = 0; i < 9; ++i) {
msg.rotation[i] = extrinsics.rot[i];
if (i < 3) {
msg.translation[i] = extrinsics.trans[i] / 1000.0;
}
}
msg.header.frame_id = frame_id;
return msg;
}
rclcpp::Time fromMsToROSTime(uint64_t ms) {
auto total = ms * 1000000ULL;
uint64_t sec = total / 1000000000;
uint64_t nano_sec = total % 1000000000;
rclcpp::Time stamp(sec, nano_sec);
return stamp;
}
rclcpp::Time fromUsToROSTime(uint64_t us) {
auto total = us * 1000ULL;
uint64_t sec = total / 1000000000;
uint64_t nano_sec = total % 1000000000;
rclcpp::Time stamp(sec, nano_sec);
return stamp;
}
std::string getObSDKVersion() {
std::string major = std::to_string(ob::Version::getMajor());
std::string minor = std::to_string(ob::Version::getMinor());
std::string patch = std::to_string(ob::Version::getPatch());
std::string version = major + "." + minor + "." + patch;
return version;
}
OBFormat OBFormatFromString(const std::string &format) {
if (format.empty()) {
return OB_FORMAT_UNKNOWN;
}
std::string fixed_format;
std::transform(format.begin(), format.end(), std::back_inserter(fixed_format),
[](const auto ch) { return std::isalpha(ch) ? toupper(ch) : ch; });
std::cout << "OBFormatFromString: " << fixed_format << std::endl;
if (fixed_format == "MJPG") {
return OB_FORMAT_MJPG;
} else if (fixed_format == "MJPEG") {
return OB_FORMAT_MJPEG;
} else if (fixed_format == "YUYV") {
return OB_FORMAT_YUYV;
} else if (fixed_format == "YUYV2") {
return OB_FORMAT_YUY2;
} else if (fixed_format == "UYVY") {
return OB_FORMAT_UYVY;
} else if (fixed_format == "NV12") {
return OB_FORMAT_NV12;
} else if (fixed_format == "NV21") {
return OB_FORMAT_NV21;
} else if (fixed_format == "H264") {
return OB_FORMAT_H264;
} else if (fixed_format == "H265") {
return OB_FORMAT_H265;
} else if (fixed_format == "Y16") {
return OB_FORMAT_Y16;
} else if (fixed_format == "Y8") {
return OB_FORMAT_Y8;
} else if (fixed_format == "Y10") {
return OB_FORMAT_Y10;
} else if (fixed_format == "Y11") {
return OB_FORMAT_Y11;
} else if (fixed_format == "Y12") {
return OB_FORMAT_Y12;
} else if (fixed_format == "GRAY") {
return OB_FORMAT_GRAY;
} else if (fixed_format == "HEVC") {
return OB_FORMAT_HEVC;
} else if (fixed_format == "I420") {
return OB_FORMAT_I420;
} else if (fixed_format == "ACCEL") {
return OB_FORMAT_ACCEL;
} else if (fixed_format == "GYRO") {
return OB_FORMAT_GYRO;
} else if (fixed_format == "POINT") {
return OB_FORMAT_POINT;
} else if (fixed_format == "RGB_POINT") {
return OB_FORMAT_RGB_POINT;
} else if (fixed_format == "REL") {
return OB_FORMAT_RLE;
} else if (fixed_format == "RGB888" || fixed_format == "RGB") {
return OB_FORMAT_RGB888;
} else if (fixed_format == "BGR") {
return OB_FORMAT_BGR;
} else if (fixed_format == "Y14") {
return OB_FORMAT_Y14;
} else if (fixed_format == "BGRA") {
return OB_FORMAT_BGRA;
} else if (fixed_format == "COMPRESSED") {
return OB_FORMAT_COMPRESSED;
} else if (fixed_format == "RVL") {
return OB_FORMAT_RVL;
} else if (fixed_format == "Z16") {
return OB_FORMAT_Z16;
} else if (fixed_format == "YV12") {
return OB_FORMAT_YV12;
} else if (fixed_format == "BA81") {
return OB_FORMAT_BA81;
} else if (fixed_format == "RGBA") {
return OB_FORMAT_RGBA;
} else if (fixed_format == "BYR2") {
return OB_FORMAT_BYR2;
} else if (fixed_format == "RW16") {
return OB_FORMAT_RW16;
} else if (fixed_format == "Y12C4") {
return OB_FORMAT_Y12C4;
} else if (fixed_format == "LIDAR_POINT") {
return OB_FORMAT_LIDAR_POINT;
} else if (fixed_format == "LIDAR_SPHERE_POINT") {
return OB_FORMAT_LIDAR_SPHERE_POINT;
} else if (fixed_format == "LIDAR_SCAN") {
return OB_FORMAT_LIDAR_SCAN;
} else if (fixed_format == "LIDAR_CALIBRATION") {
return OB_FORMAT_LIDAR_CALIBRATION;
}
// else if (fixed_format == "DISP16") {
// return OB_FORMAT_DISP16;
// }
else {
return OB_FORMAT_UNKNOWN;
}
}
OBLiDARScanRate OBScanRateFromInt(const int rate) {
std::cout << "OBScanRateFromInt: " << rate << std::endl;
if (rate == 5) {
return OB_LIDAR_SCAN_5HZ;
} else if (rate == 10) {
return OB_LIDAR_SCAN_10HZ;
} else if (rate == 15) {
return OB_LIDAR_SCAN_15HZ;
} else if (rate == 20) {
return OB_LIDAR_SCAN_20HZ;
} else if (rate == 25) {
return OB_LIDAR_SCAN_25HZ;
} else if (rate == 30) {
return OB_LIDAR_SCAN_30HZ;
} else if (rate == 40) {
return OB_LIDAR_SCAN_40HZ;
} else {
return OB_LIDAR_SCAN_UNKNOWN;
}
}
std::string OBFormatToString(const OBFormat &format) {
switch (format) {
case OB_FORMAT_MJPG:
return "MJPG";
case OB_FORMAT_YUYV:
return "YUYV";
case OB_FORMAT_YUY2:
return "YUYV2";
case OB_FORMAT_UYVY:
return "UYVY";
case OB_FORMAT_NV12:
return "NV12";
case OB_FORMAT_NV21:
return "NV21";
case OB_FORMAT_H264:
return "H264";
case OB_FORMAT_H265:
return "H265";
case OB_FORMAT_Y16:
return "Y16";
case OB_FORMAT_Y8:
return "Y8";
case OB_FORMAT_Y10:
return "Y10";
case OB_FORMAT_Y11:
return "Y11";
case OB_FORMAT_Y12:
return "Y12";
case OB_FORMAT_GRAY:
return "GRAY";
case OB_FORMAT_HEVC:
return "HEVC";
case OB_FORMAT_I420:
return "I420";
case OB_FORMAT_ACCEL:
return "ACCEL";
case OB_FORMAT_GYRO:
return "GYRO";
case OB_FORMAT_POINT:
return "POINT";
case OB_FORMAT_RGB_POINT:
return "RGB_POINT";
case OB_FORMAT_RLE:
return "REL";
case OB_FORMAT_RGB888:
return "RGB888";
case OB_FORMAT_BGR:
return "BGR";
case OB_FORMAT_Y14:
return "Y14";
case OB_FORMAT_BGRA:
return "BGRA";
case OB_FORMAT_COMPRESSED:
return "COMPRESSED";
case OB_FORMAT_RVL:
return "RVL";
case OB_FORMAT_Z16:
return "Z16";
case OB_FORMAT_YV12:
return "YV12";
case OB_FORMAT_BA81:
return "BA81";
case OB_FORMAT_RGBA:
return "RGBA";
case OB_FORMAT_BYR2:
return "BYR2";
case OB_FORMAT_RW16:
return "RW16";
case OB_FORMAT_Y12C4:
return "Y12C4";
// case OB_FORMAT_DISP16:
// return "DISP16";
default:
return "UNKNOWN";
}
}
std::ostream &operator<<(std::ostream &os, const OBFormat &rhs) {
os << OBFormatToString(rhs);
return os;
}
std::string ObDeviceTypeToString(const OBDeviceType &type) {
switch (type) {
case OBDeviceType::OB_STRUCTURED_LIGHT_BINOCULAR_CAMERA:
return "structured light binocular camera";
case OBDeviceType::OB_STRUCTURED_LIGHT_MONOCULAR_CAMERA:
return "structured light monocular camera";
case OBDeviceType::OB_TOF_CAMERA:
return "tof camera";
default:
// 处理其他未预见的情况
break;
}
return "unknown technology camera";
}
rmw_qos_profile_t getRMWQosProfileFromString(const std::string &str_qos) {
std::string upper_str_qos = str_qos;
std::transform(upper_str_qos.begin(), upper_str_qos.end(), upper_str_qos.begin(), ::toupper);
if (upper_str_qos == "SYSTEM_DEFAULT") {
return rmw_qos_profile_system_default;
} else if (upper_str_qos == "DEFAULT") {
return rmw_qos_profile_default;
} else if (upper_str_qos == "PARAMETER_EVENTS") {
return rmw_qos_profile_parameter_events;
} else if (upper_str_qos == "SERVICES_DEFAULT") {
return rmw_qos_profile_services_default;
} else if (upper_str_qos == "PARAMETERS") {
return rmw_qos_profile_parameters;
} else if (upper_str_qos == "SENSOR_DATA") {
return rmw_qos_profile_sensor_data;
} else {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("astra_camera"),
"Invalid QoS profile: " << upper_str_qos << ". Using default QoS profile.");
return rmw_qos_profile_default;
}
}
bool isOpenNIDevice(int pid) {
static const std::vector<int> OPENNI_DEVICE_PIDS = {
0x0300, 0x0301, 0x0400, 0x0401, 0x0402, 0x0403, 0x0404, 0x0407, 0x0601, 0x060b, 0x060e,
0x060f, 0x0610, 0x0613, 0x0614, 0x0616, 0x0617, 0x0618, 0x061b, 0x062b, 0x062c, 0x062d,
0x0632, 0x0633, 0x0634, 0x0635, 0x0636, 0x0637, 0x0638, 0x0639, 0x063a, 0x0650, 0x0651,
0x0654, 0x0655, 0x0656, 0x0657, 0x0658, 0x0659, 0x065a, 0x065b, 0x065c, 0x065d, 0x0698,
0x0699, 0x069a, 0x055c, 0x065e, 0x069a, 0x069f, 0x06a0, 0x069e};
return std::any_of(OPENNI_DEVICE_PIDS.begin(), OPENNI_DEVICE_PIDS.end(),
[pid](int pid_openni) { return pid == pid_openni; });
}
OB_DEPTH_PRECISION_LEVEL depthPrecisionLevelFromString(
const std::string &depth_precision_level_str) {
if (depth_precision_level_str == "1mm") {
return OB_PRECISION_1MM;
} else if (depth_precision_level_str == "0.8mm") {
return OB_PRECISION_0MM8;
} else if (depth_precision_level_str == "0.4mm") {
return OB_PRECISION_0MM4;
} else if (depth_precision_level_str == "0.2mm") {
return OB_PRECISION_0MM2;
} else if (depth_precision_level_str == "0.1mm") {
return OB_PRECISION_0MM1;
} else {
return OB_PRECISION_0MM8;
}
}
float depthPrecisionFromString(const std::string &depth_precision_level_str) {
// covert 0.8mm to 0.8
if (depth_precision_level_str.size() < 2) {
RCLCPP_WARN_STREAM(rclcpp::get_logger("utils"),
"Invalid depth precision level: " << depth_precision_level_str
<< ". Using default precision level 1mm");
return 1.0;
}
std::string depth_precision_level_str_num =
depth_precision_level_str.substr(0, depth_precision_level_str.size() - 2);
return std::stof(depth_precision_level_str_num);
}
std::string colorPowerLineFrequencyToString(int value) {
switch (value) {
case 0:
return "disable";
case 1:
return "50hz";
case 2:
return "60hz";
case 3:
return "auto";
default:
return "unknown";
}
}
std::string depthPrecisionLevelToString(int value) {
switch (static_cast<OB_DEPTH_PRECISION_LEVEL>(value)) {
case OB_PRECISION_1MM:
return "1mm";
case OB_PRECISION_0MM8:
return "0.8mm";
case OB_PRECISION_0MM4:
return "0.4mm";
case OB_PRECISION_0MM2:
return "0.2mm";
case OB_PRECISION_0MM1:
return "0.1mm";
default:
return "unknown";
}
}
OBMultiDeviceSyncMode OBSyncModeFromString(const std::string &mode) {
if (mode == "FREE_RUN") {
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN;
} else if (mode == "STANDALONE") {
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_STANDALONE;
} else if (mode == "PRIMARY") {
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_PRIMARY;
} else if (mode == "SECONDARY") {
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SECONDARY;
} else if (mode == "SECONDARY_SYNCED") {
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SECONDARY_SYNCED;
} else if (mode == "SOFTWARE_TRIGGERING") {
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING;
} else if (mode == "HARDWARE_TRIGGERING") {
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_HARDWARE_TRIGGERING;
} else {
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN;
}
}
std::string disparityRangeModeToString(int value) {
switch (value) {
case 0:
return "64";
case 1:
return "128";
case 2:
return "256";
default:
return "unknown";
}
}
std::string exposureRangeModeToString(int value) {
switch (value) {
case 0:
return "regular";
case 1:
return "ultimate";
default:
return "unknown";
}
}
std::string intraCameraSyncReferenceToString(int value) {
switch (value) {
case 0:
return "Start";
case 1:
return "Middle";
case 2:
return "End";
default:
return "unknown";
}
}
OB_SAMPLE_RATE sampleRateFromString(std::string &sample_rate) {
// covert to lower case
std::transform(sample_rate.begin(), sample_rate.end(), sample_rate.begin(), ::tolower);
if (sample_rate == "1.5625hz") {
return OB_SAMPLE_RATE_1_5625_HZ;
} else if (sample_rate == "3.125hz") {
return OB_SAMPLE_RATE_3_125_HZ;
} else if (sample_rate == "6.25hz") {
return OB_SAMPLE_RATE_6_25_HZ;
} else if (sample_rate == "12.5hz") {
return OB_SAMPLE_RATE_12_5_HZ;
} else if (sample_rate == "25hz") {
return OB_SAMPLE_RATE_25_HZ;
} else if (sample_rate == "50hz") {
return OB_SAMPLE_RATE_50_HZ;
} else if (sample_rate == "100hz") {
return OB_SAMPLE_RATE_100_HZ;
} else if (sample_rate == "200hz") {
return OB_SAMPLE_RATE_200_HZ;
} else if (sample_rate == "500hz") {
return OB_SAMPLE_RATE_500_HZ;
} else if (sample_rate == "1khz" || sample_rate == "1000hz") {
return OB_SAMPLE_RATE_1_KHZ;
} else if (sample_rate == "2khz" || sample_rate == "2000hz") {
return OB_SAMPLE_RATE_2_KHZ;
} else if (sample_rate == "4khz" || sample_rate == "4000hz") {
return OB_SAMPLE_RATE_4_KHZ;
} else if (sample_rate == "8khz" || sample_rate == "8000hz") {
return OB_SAMPLE_RATE_8_KHZ;
} else if (sample_rate == "16khz" || sample_rate == "16000hz") {
return OB_SAMPLE_RATE_16_KHZ;
} else if (sample_rate == "32khz" || sample_rate == "32000hz") {
return OB_SAMPLE_RATE_32_KHZ;
} else {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("utils"), "Unknown OB_SAMPLE_RATE: " << sample_rate);
return OB_SAMPLE_RATE_100_HZ;
}
}
std::string sampleRateToString(const OB_SAMPLE_RATE &sample_rate) {
switch (sample_rate) {
case OB_SAMPLE_RATE_1_5625_HZ:
return "1.5625hz";
case OB_SAMPLE_RATE_3_125_HZ:
return "3.125hz";
case OB_SAMPLE_RATE_6_25_HZ:
return "6.25hz";
case OB_SAMPLE_RATE_12_5_HZ:
return "12.5hz";
case OB_SAMPLE_RATE_25_HZ:
return "25hz";
case OB_SAMPLE_RATE_50_HZ:
return "50hz";
case OB_SAMPLE_RATE_100_HZ:
return "100hz";
case OB_SAMPLE_RATE_200_HZ:
return "200hz";
case OB_SAMPLE_RATE_500_HZ:
return "500hz";
case OB_SAMPLE_RATE_1_KHZ:
return "1khz";
case OB_SAMPLE_RATE_2_KHZ:
return "2khz";
case OB_SAMPLE_RATE_4_KHZ:
return "4khz";
case OB_SAMPLE_RATE_8_KHZ:
return "8khz";
case OB_SAMPLE_RATE_16_KHZ:
return "16khz";
case OB_SAMPLE_RATE_32_KHZ:
return "32khz";
default:
return "100hz";
}
}
std::ostream &operator<<(std::ostream &os, const OB_SAMPLE_RATE &rhs) {
os << sampleRateToString(rhs);
return os;
}
OB_GYRO_FULL_SCALE_RANGE fullGyroScaleRangeFromString(std::string &full_scale_range) {
std::transform(full_scale_range.begin(), full_scale_range.end(), full_scale_range.begin(),
::tolower);
if (full_scale_range == "16dps") {
return OB_GYRO_FS_16dps;
} else if (full_scale_range == "31dps") {
return OB_GYRO_FS_31dps;
} else if (full_scale_range == "62dps") {
return OB_GYRO_FS_62dps;
} else if (full_scale_range == "125dps") {
return OB_GYRO_FS_125dps;
} else if (full_scale_range == "250dps") {
return OB_GYRO_FS_250dps;
} else if (full_scale_range == "500dps") {
return OB_GYRO_FS_500dps;
} else if (full_scale_range == "1000dps") {
return OB_GYRO_FS_1000dps;
} else if (full_scale_range == "2000dps") {
return OB_GYRO_FS_2000dps;
} else {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("utils"),
"Unknown OB_GYRO_FULL_SCALE_RANGE: " << full_scale_range);
return OB_GYRO_FS_2000dps;
}
}
std::string fullGyroScaleRangeToString(const OB_GYRO_FULL_SCALE_RANGE &full_scale_range) {
switch (full_scale_range) {
case OB_GYRO_FS_16dps:
return "16dps";
case OB_GYRO_FS_31dps:
return "31dps";
case OB_GYRO_FS_62dps:
return "62dps";
case OB_GYRO_FS_125dps:
return "125dps";
case OB_GYRO_FS_250dps:
return "250dps";
case OB_GYRO_FS_500dps:
return "500dps";
case OB_GYRO_FS_1000dps:
return "1000dps";
case OB_GYRO_FS_2000dps:
return "2000dps";
default:
return "16dps";
}
}
std::ostream &operator<<(std::ostream &os, const OB_GYRO_FULL_SCALE_RANGE &rhs) {
os << fullGyroScaleRangeToString(rhs);
return os;
}
OBAccelFullScaleRange fullAccelScaleRangeFromString(std::string &full_scale_range) {
std::transform(full_scale_range.begin(), full_scale_range.end(), full_scale_range.begin(),
::tolower);
if (full_scale_range == "2g") {
return OB_ACCEL_FS_2g;
} else if (full_scale_range == "4g") {
return OB_ACCEL_FS_4g;
} else if (full_scale_range == "8g") {
return OB_ACCEL_FS_8g;
} else if (full_scale_range == "16g") {
return OB_ACCEL_FS_16g;
} else if (full_scale_range == "3g") {
return OB_ACCEL_FS_3g;
} else if (full_scale_range == "6g") {
return OB_ACCEL_FS_6g;
} else if (full_scale_range == "12g") {
return OB_ACCEL_FS_12g;
} else if (full_scale_range == "24g") {
return OB_ACCEL_FS_24g;
} else {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("utils"),
"Unknown OB_ACCEL_FULL_SCALE_RANGE: " << full_scale_range);
return OB_ACCEL_FS_16g;
}
}
std::string fullAccelScaleRangeToString(const OBAccelFullScaleRange &full_scale_range) {
switch (full_scale_range) {
case OB_ACCEL_FS_2g:
return "2g";
case OB_ACCEL_FS_4g:
return "4g";
case OB_ACCEL_FS_8g:
return "8g";
case OB_ACCEL_FS_16g:
return "16g";
case OB_ACCEL_FS_3g:
return "3g";
case OB_ACCEL_FS_6g:
return "6g";
case OB_ACCEL_FS_12g:
return "12g";
case OB_ACCEL_FS_24g:
return "24g";
default:
return "2g";
}
}
std::ostream &operator<<(std::ostream &os, const OBAccelFullScaleRange &rhs) {
os << fullAccelScaleRangeToString(rhs);
return os;
}
std::string parseUsbPort(const std::string &line) {
std::string port_id;
std::regex usb_regex("(?:[^ ]+/usb[0-9]+[0-9./-]*/){0,1}([0-9.-]+)(:){0,1}[^ ]*",
std::regex_constants::ECMAScript);
std::smatch base_match;
bool found_usb = std::regex_match(line, base_match, usb_regex);
if (found_usb) {
port_id = base_match[1].str();
if (base_match[2].str().empty()) {
std::regex end_regex(".+(-[0-9]+$)", std::regex_constants::ECMAScript);
bool found_end = std::regex_match(port_id, base_match, end_regex);
if (found_end) {
port_id = port_id.substr(0, port_id.size() - base_match[1].str().size());
}
}
return port_id;
}
std::regex gmsl_regex("(gmsl[0-9]+)(?:-[0-9]+)*(-[0-9]+)$", std::regex_constants::ECMAScript);
bool found_gmsl = std::regex_match(line, base_match, gmsl_regex);
if (found_gmsl) {
port_id = base_match[1].str() + base_match[2].str();
std::cout << "Parsed GMSL Port ID: " << port_id << std::endl;
return port_id;
}
return "";
}
bool isValidJPEG(const std::shared_ptr<ob::ColorFrame> &frame) {
if (frame->getDataSize() < 2) { // Checking both start and end markers, so minimal size is 4
return false;
}
const auto *data = static_cast<const uint8_t *>(frame->getData());
// Check for JPEG start marker
if (data[0] != 0xFF || data[1] != 0xD8) {
return false;
}
return true;
}
std::string metaDataTypeToString(const OBFrameMetadataType &meta_data_type) {
switch (meta_data_type) {
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_TIMESTAMP:
return "frame_timestamp";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_SENSOR_TIMESTAMP:
return "sensor_timestamp";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_FRAME_NUMBER:
return "frame_number";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AUTO_EXPOSURE:
return "auto_exposure";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_EXPOSURE:
return "exposure";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_GAIN:
return "gain";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AUTO_WHITE_BALANCE:
return "auto_white_balance";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_WHITE_BALANCE:
return "white_balance";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_BRIGHTNESS:
return "brightness";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_CONTRAST:
return "contrast";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_SATURATION:
return "saturation";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_SHARPNESS:
return "sharpness";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_BACKLIGHT_COMPENSATION:
return "backlight_compensation";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_HUE:
return "hue";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_GAMMA:
return "gamma";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_POWER_LINE_FREQUENCY:
return "power_line_frequency";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_LOW_LIGHT_COMPENSATION:
return "low_light_compensation";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_MANUAL_WHITE_BALANCE:
return "manual_white_balance";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_ACTUAL_FRAME_RATE:
return "actual_frame_rate";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_FRAME_RATE:
return "frame_rate";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AE_ROI_LEFT:
return "ae_roi_left";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AE_ROI_TOP:
return "ae_roi_top";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AE_ROI_RIGHT:
return "ae_roi_right";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AE_ROI_BOTTOM:
return "ae_roi_bottom";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_EXPOSURE_PRIORITY:
return "exposure_priority";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_NAME:
return "hdr_sequence_name";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_SIZE:
return "hdr_sequence_size";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX:
return "hdr_sequence_index";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_LASER_POWER:
return "frame_laser_power";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_LASER_POWER_MODE:
return "frame_laser_power_mode";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_EMITTER_MODE:
return "frame_emitter_mode";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_GPIO_INPUT_DATA:
return "gpio_input_data";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_DISPARITY_SEARCH_OFFSET:
return "disparity_search_offset";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_DISPARITY_SEARCH_RANGE:
return "disparity search range";
default:
return "unknown_field";
}
}
std::ostream &operator<<(std::ostream &os, const OBFrameMetadataType &rhs) {
os << metaDataTypeToString(rhs);
return os;
}
OBHoleFillingMode holeFillingModeFromString(const std::string &hole_filling_mode) {
if (hole_filling_mode == "FILL_TOP") {
return OB_HOLE_FILL_TOP;
} else if (hole_filling_mode == "FILL_NEAREST") {
return OB_HOLE_FILL_NEAREST;
} else if (hole_filling_mode == "FILL_FAREST") {
return OB_HOLE_FILL_FAREST;
} else {
return OB_HOLE_FILL_NEAREST;
}
}
bool isGemini2R(int pid) {
if (pid == GEMINI2R_PID || pid == GEMINI2RL_PID) {
return true;
}
if (pid == GEMINI2R_PID2 || pid == GEMINI2RL_PID2) {
return true;
}
return false;
}
OBStreamType obStreamTypeFromString(const std::string &stream_type) {
std::string upper_stream_type = stream_type;
std::transform(upper_stream_type.begin(), upper_stream_type.end(), upper_stream_type.begin(),
::toupper);
if (upper_stream_type == "VIDEO") {
return OB_STREAM_VIDEO;
} else if (upper_stream_type == "IR") {
return OB_STREAM_IR;
} else if (upper_stream_type == "COLOR") {
return OB_STREAM_COLOR;
} else if (upper_stream_type == "DEPTH") {
return OB_STREAM_DEPTH;
} else if (upper_stream_type == "ACCEL") {
return OB_STREAM_ACCEL;
} else if (upper_stream_type == "GYRO") {
return OB_STREAM_GYRO;
} else if (upper_stream_type == "IR_LEFT") {
return OB_STREAM_IR_LEFT;
} else if (upper_stream_type == "IR_RIGHT") {
return OB_STREAM_IR_RIGHT;
} else if (upper_stream_type == "RAW_PHASE") {
return OB_STREAM_RAW_PHASE;
} else {
return OB_STREAM_UNKNOWN;
}
}
namespace {
bool isSameIntrinsic(const OBCameraIntrinsic &lhs, const OBCameraIntrinsic &rhs) {
return lhs.fx == rhs.fx && lhs.fy == rhs.fy && lhs.cx == rhs.cx && lhs.cy == rhs.cy &&
lhs.width == rhs.width && lhs.height == rhs.height;
}
bool isSameDistortion(const OBCameraDistortion &lhs, const OBCameraDistortion &rhs) {
return lhs.model == rhs.model && lhs.k1 == rhs.k1 && lhs.k2 == rhs.k2 && lhs.k3 == rhs.k3 &&
lhs.k4 == rhs.k4 && lhs.k5 == rhs.k5 && lhs.k6 == rhs.k6 && lhs.p1 == rhs.p1 &&
lhs.p2 == rhs.p2;
}
struct UndistortMapCacheEntry {
int width = 0;
int height = 0;
int image_type = 0;
OBCameraIntrinsic intrinsic{};
OBCameraDistortion distortion{};
cv::Mat map1;
cv::Mat map2;
};
} // namespace
UndistortedImageResult undistortImage(const cv::Mat &image, const OBCameraIntrinsic &intrinsic,
const OBCameraDistortion &distortion) {
UndistortedImageResult result;
result.new_intrinsic = intrinsic;
if (image.empty()) {
return result;
}
cv::Mat map1;
cv::Mat map2;
{
static std::mutex cache_mutex;
static std::vector<UndistortMapCacheEntry> map_cache;
std::lock_guard<std::mutex> lock(cache_mutex);
auto cache_it =
std::find_if(map_cache.begin(), map_cache.end(), [&](const UndistortMapCacheEntry &entry) {
return entry.width == image.cols && entry.height == image.rows &&
entry.image_type == image.type() && isSameIntrinsic(entry.intrinsic, intrinsic) &&
isSameDistortion(entry.distortion, distortion);
});
if (cache_it == map_cache.end()) {
UndistortMapCacheEntry entry;
entry.width = image.cols;
entry.height = image.rows;
entry.image_type = image.type();
entry.intrinsic = intrinsic;
entry.distortion = distortion;
cv::Mat camera_matrix = (cv::Mat_<double>(3, 3) << intrinsic.fx, 0.0, intrinsic.cx, 0.0,
intrinsic.fy, intrinsic.cy, 0.0, 0.0, 1.0);
cv::Mat dist_coeffs =
(cv::Mat_<double>(8, 1) << distortion.k1, distortion.k2, distortion.p1, distortion.p2,
distortion.k3, distortion.k4, distortion.k5, distortion.k6);
cv::initUndistortRectifyMap(camera_matrix, dist_coeffs, cv::Mat(), camera_matrix,
cv::Size(image.cols, image.rows), CV_16SC2, entry.map1,
entry.map2);
map_cache.emplace_back(std::move(entry));
cache_it = std::prev(map_cache.end());
}
map1 = cache_it->map1;
map2 = cache_it->map2;
}
result.image.create(image.size(), image.type());
cv::remap(image, result.image, map1, map2, cv::INTER_LINEAR);
result.new_intrinsic.width = image.cols;
result.new_intrinsic.height = image.rows;
return result;
}
std::string getDistortionModels(OBCameraDistortion distortion) {
switch (distortion.model) {
case OB_DISTORTION_NONE:
return sensor_msgs::distortion_models::PLUMB_BOB;
case OB_DISTORTION_MODIFIED_BROWN_CONRADY:
return sensor_msgs::distortion_models::PLUMB_BOB;
case OB_DISTORTION_INVERSE_BROWN_CONRADY:
return sensor_msgs::distortion_models::PLUMB_BOB;
case OB_DISTORTION_BROWN_CONRADY:
return sensor_msgs::distortion_models::PLUMB_BOB;
case OB_DISTORTION_BROWN_CONRADY_K6:
return sensor_msgs::distortion_models::PLUMB_BOB;
case OB_DISTORTION_KANNALA_BRANDT4:
return sensor_msgs::distortion_models::EQUIDISTANT;
default:
return sensor_msgs::distortion_models::PLUMB_BOB;
}
}
std::string calcMD5(const std::string &data) {
unsigned char digest[EVP_MAX_MD_SIZE];
unsigned int digest_len = 0;
EVP_MD_CTX *ctx = EVP_MD_CTX_new();
EVP_DigestInit_ex(ctx, EVP_md5(), nullptr);
EVP_DigestUpdate(ctx, data.data(), data.size());
EVP_DigestFinal_ex(ctx, digest, &digest_len);
EVP_MD_CTX_free(ctx);
std::stringstream ss;
ss << std::hex << std::setfill('0');
for (unsigned int i = 0; i < digest_len; ++i) ss << std::setw(2) << (int)digest[i];
return ss.str();
}
double getScanAngleIncrement(OBLiDARScanRate fps) {
switch (fps) {
case OB_LIDAR_SCAN_15HZ:
return deg2rad(0.075);
case OB_LIDAR_SCAN_20HZ:
return deg2rad(0.1);
case OB_LIDAR_SCAN_25HZ:
return deg2rad(0.125);
case OB_LIDAR_SCAN_30HZ:
return deg2rad(0.15);
case OB_LIDAR_SCAN_40HZ:
return deg2rad(0.2);
default:
return deg2rad(0.1);
}
}
double deg2rad(double deg) { return deg * M_PI / 180.0; }
double rad2deg(double rad) {
double angle_degrees = rad * (180.0 / M_PI);
if (angle_degrees < 0) {
angle_degrees += 360.0;
}
return angle_degrees;
}
} // namespace orbbec_camera
Reference in New Issue View Git Blame Copy Permalink