536 lines
18 KiB
C++
536 lines
18 KiB
C++
#include "utils.hpp"
|
|
#include <iomanip>
|
|
#include <opencv2/core/eigen.hpp>
|
|
#include <pwd.h>
|
|
#include <regex>
|
|
// util functions
|
|
namespace wust_vision {
|
|
namespace utils {
|
|
double limit_rad(double angle) noexcept {
|
|
while (angle > M_PI)
|
|
angle -= 2.0 * M_PI;
|
|
while (angle < -M_PI)
|
|
angle += 2.0 * M_PI;
|
|
return angle;
|
|
}
|
|
|
|
Eigen::Vector3d
|
|
quatToEuler(const Eigen::Quaterniond& q, int axis0, int axis1, int axis2, bool extrinsic) {
|
|
if (!extrinsic)
|
|
std::swap(axis0, axis2);
|
|
|
|
auto i = axis0, j = axis1, k = axis2;
|
|
bool is_proper = (i == k);
|
|
if (is_proper)
|
|
k = 3 - i - j;
|
|
int sign = (i - j) * (j - k) * (k - i) / 2;
|
|
|
|
double a, b, c, d;
|
|
const Eigen::Vector4d xyzw = q.coeffs(); // [x,y,z,w]
|
|
if (is_proper) {
|
|
a = xyzw[3];
|
|
b = xyzw[i];
|
|
c = xyzw[j];
|
|
d = xyzw[k] * sign;
|
|
} else {
|
|
a = xyzw[3] - xyzw[j];
|
|
b = xyzw[i] + xyzw[k] * sign;
|
|
c = xyzw[j] + xyzw[3];
|
|
d = xyzw[k] * sign - xyzw[i];
|
|
}
|
|
|
|
Eigen::Vector3d eulers;
|
|
const double n2 = a * a + b * b + c * c + d * d;
|
|
eulers[1] = std::acos(2 * (a * a + b * b) / n2 - 1);
|
|
|
|
const double half_sum = std::atan2(b, a);
|
|
const double half_diff = std::atan2(-d, c);
|
|
|
|
const double eps = 1e-7;
|
|
const bool safe1 = std::abs(eulers[1]) >= eps;
|
|
const bool safe2 = std::abs(eulers[1] - M_PI) >= eps;
|
|
const bool safe = safe1 && safe2;
|
|
|
|
if (safe) {
|
|
eulers[0] = half_sum + half_diff;
|
|
eulers[2] = half_sum - half_diff;
|
|
} else {
|
|
if (!extrinsic) {
|
|
eulers[0] = 0;
|
|
eulers[2] = !safe1 ? 2 * half_sum : -2 * half_diff;
|
|
} else {
|
|
eulers[2] = 0;
|
|
eulers[0] = !safe1 ? 2 * half_sum : 2 * half_diff;
|
|
}
|
|
}
|
|
|
|
for (int idx = 0; idx < 3; idx++)
|
|
eulers[idx] = limit_rad(eulers[idx]);
|
|
|
|
if (!is_proper) {
|
|
eulers[2] *= sign;
|
|
eulers[1] -= M_PI / 2;
|
|
}
|
|
|
|
if (!extrinsic)
|
|
std::swap(eulers[0], eulers[2]);
|
|
|
|
return eulers;
|
|
}
|
|
|
|
Eigen::Quaterniond
|
|
eulerToQuat(const Eigen::Vector3d& euler, int axis0, int axis1, int axis2, bool extrinsic) {
|
|
const double rz = euler[0], ry = euler[1], rx = euler[2];
|
|
const Eigen::Quaterniond qx(Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()));
|
|
const Eigen::Quaterniond qy(Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()));
|
|
const Eigen::Quaterniond qz(Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()));
|
|
|
|
if (!extrinsic)
|
|
std::swap(axis0, axis2);
|
|
Eigen::Quaterniond q;
|
|
|
|
// 生成四元数
|
|
if (axis0 == 0 && axis1 == 1 && axis2 == 2)
|
|
q = qx * qy * qz;
|
|
else if (axis0 == 0 && axis1 == 2 && axis2 == 1)
|
|
q = qx * qz * qy;
|
|
else if (axis0 == 1 && axis1 == 0 && axis2 == 2)
|
|
q = qy * qx * qz;
|
|
else if (axis0 == 1 && axis1 == 2 && axis2 == 0)
|
|
q = qy * qz * qx;
|
|
else if (axis0 == 2 && axis1 == 0 && axis2 == 1)
|
|
q = qz * qx * qy;
|
|
else if (axis0 == 2 && axis1 == 1 && axis2 == 0)
|
|
q = qz * qy * qx;
|
|
else
|
|
throw std::invalid_argument("Unsupported axis order");
|
|
|
|
return q;
|
|
}
|
|
|
|
Eigen::Matrix3d
|
|
eulerToMatrix(const Eigen::Vector3d& euler, int axis0, int axis1, int axis2, bool extrinsic) {
|
|
return eulerToQuat(euler, axis0, axis1, axis2, extrinsic).toRotationMatrix();
|
|
}
|
|
|
|
Eigen::Vector3d
|
|
matrixToEuler(const Eigen::Matrix3d& R, int axis0, int axis1, int axis2, bool extrinsic) {
|
|
Eigen::Quaterniond q(R);
|
|
return quatToEuler(q, axis0, axis1, axis2, extrinsic);
|
|
}
|
|
|
|
Eigen::Vector3d quatToEuler(const Eigen::Quaterniond& q, EulerOrder order, bool extrinsic) {
|
|
switch (order) {
|
|
case EulerOrder::XYZ:
|
|
return quatToEuler(q, 0, 1, 2, extrinsic);
|
|
case EulerOrder::XZY:
|
|
return quatToEuler(q, 0, 2, 1, extrinsic);
|
|
case EulerOrder::YXZ:
|
|
return quatToEuler(q, 1, 0, 2, extrinsic);
|
|
case EulerOrder::YZX:
|
|
return quatToEuler(q, 1, 2, 0, extrinsic);
|
|
case EulerOrder::ZXY:
|
|
return quatToEuler(q, 2, 0, 1, extrinsic);
|
|
case EulerOrder::ZYX:
|
|
return quatToEuler(q, 2, 1, 0, extrinsic);
|
|
default:
|
|
throw std::invalid_argument("Unsupported EulerOrder");
|
|
}
|
|
}
|
|
|
|
Eigen::Quaterniond eulerToQuat(const Eigen::Vector3d& euler, EulerOrder order, bool extrinsic) {
|
|
switch (order) {
|
|
case EulerOrder::XYZ:
|
|
return eulerToQuat(euler, 0, 1, 2, extrinsic);
|
|
case EulerOrder::XZY:
|
|
return eulerToQuat(euler, 0, 2, 1, extrinsic);
|
|
case EulerOrder::YXZ:
|
|
return eulerToQuat(euler, 1, 0, 2, extrinsic);
|
|
case EulerOrder::YZX:
|
|
return eulerToQuat(euler, 1, 2, 0, extrinsic);
|
|
case EulerOrder::ZXY:
|
|
return eulerToQuat(euler, 2, 0, 1, extrinsic);
|
|
case EulerOrder::ZYX:
|
|
return eulerToQuat(euler, 2, 1, 0, extrinsic);
|
|
default:
|
|
throw std::invalid_argument("Unsupported EulerOrder");
|
|
}
|
|
}
|
|
|
|
Eigen::Matrix3d eulerToMatrix(const Eigen::Vector3d& euler, EulerOrder order, bool extrinsic) {
|
|
return eulerToQuat(euler, order, extrinsic).toRotationMatrix();
|
|
}
|
|
|
|
Eigen::Vector3d matrixToEuler(const Eigen::Matrix3d& R, EulerOrder order, bool extrinsic) {
|
|
return quatToEuler(Eigen::Quaterniond(R), order, extrinsic);
|
|
}
|
|
|
|
Eigen::MatrixXd cvToEigen(const cv::Mat& cv_mat) noexcept {
|
|
Eigen::MatrixXd eigen_mat = Eigen::MatrixXd::Zero(cv_mat.rows, cv_mat.cols);
|
|
cv::cv2eigen(cv_mat, eigen_mat);
|
|
return eigen_mat;
|
|
}
|
|
Eigen::Vector3d transformPosition(
|
|
const Eigen::Vector3d& pos_camera,
|
|
const Eigen::Matrix4d& T_camera_to_odom
|
|
) noexcept {
|
|
const Eigen::Vector4d pos_homo(pos_camera.x(), pos_camera.y(), pos_camera.z(), 1.0);
|
|
const Eigen::Vector4d pos_odom = T_camera_to_odom * pos_homo;
|
|
return pos_odom.head<3>();
|
|
}
|
|
|
|
Eigen::Quaterniond transformOrientation(
|
|
const Eigen::Quaterniond& q_camera,
|
|
const Eigen::Matrix4d& T_camera_to_odom
|
|
) noexcept {
|
|
const Eigen::Matrix3d R_camera_to_odom = T_camera_to_odom.block<3, 3>(0, 0);
|
|
const Eigen::Matrix3d R_ori_camera = q_camera.normalized().toRotationMatrix();
|
|
const Eigen::Matrix3d R_ori_odom = R_camera_to_odom * R_ori_camera;
|
|
return Eigen::Quaterniond(R_ori_odom).normalized();
|
|
}
|
|
void pnpToEigen(
|
|
const cv::Mat& rvec,
|
|
const cv::Mat& tvec,
|
|
Eigen::Vector3d& t_out,
|
|
Eigen::Quaterniond& q_out
|
|
) noexcept {
|
|
// 平移
|
|
cv::cv2eigen(tvec, t_out);
|
|
|
|
// 旋转
|
|
cv::Mat R_cv;
|
|
cv::Rodrigues(rvec, R_cv);
|
|
Eigen::Matrix3d R;
|
|
cv::cv2eigen(R_cv, R);
|
|
|
|
q_out = Eigen::Quaterniond(R).normalized();
|
|
}
|
|
void pnpToEigen(
|
|
const cv::Vec3d& rvec,
|
|
const cv::Vec3d& tvec,
|
|
Eigen::Vector3d& t_out,
|
|
Eigen::Quaterniond& q_out
|
|
) noexcept {
|
|
// 平移
|
|
t_out = Eigen::Vector3d(tvec[0], tvec[1], tvec[2]);
|
|
|
|
// Rodrigues 旋转向量转矩阵
|
|
cv::Mat rvec_mat(rvec); // cv::Vec3d -> 3x1 Mat
|
|
cv::Mat R_cv;
|
|
cv::Rodrigues(rvec_mat, R_cv);
|
|
|
|
Eigen::Matrix3d R;
|
|
cv::cv2eigen(R_cv, R);
|
|
|
|
q_out = Eigen::Quaterniond(R).normalized();
|
|
}
|
|
|
|
cv::Point2f computeCenter(const std::vector<cv::Point2f>& points) noexcept {
|
|
if (points.empty()) {
|
|
return cv::Point2f(0.f, 0.f);
|
|
}
|
|
|
|
float sum_x = 0.f;
|
|
float sum_y = 0.f;
|
|
for (const auto& pt: points) {
|
|
sum_x += pt.x;
|
|
sum_y += pt.y;
|
|
}
|
|
return cv::Point2f(sum_x / points.size(), sum_y / points.size());
|
|
}
|
|
bool isStateValid(const Eigen::VectorXd& state) noexcept {
|
|
return state.allFinite(); // 所有元素都不是 NaN 或 Inf
|
|
}
|
|
Eigen::Matrix4d computeCameraToOdomTransform(
|
|
const Eigen::Matrix3d& R_gimbal2odom,
|
|
const Eigen::Matrix3d& R_camera_to_gimbal,
|
|
const Eigen::Vector3d& t_camera_to_gimbal
|
|
) noexcept {
|
|
Eigen::Matrix4d T_gimbal_to_odom = Eigen::Matrix4d::Identity();
|
|
T_gimbal_to_odom.block<3, 3>(0, 0) = R_gimbal2odom;
|
|
|
|
Eigen::Matrix4d T_camera_to_gimbal = Eigen::Matrix4d::Identity();
|
|
T_camera_to_gimbal.block<3, 3>(0, 0) = R_camera_to_gimbal;
|
|
T_camera_to_gimbal.block<3, 1>(0, 3) = t_camera_to_gimbal;
|
|
|
|
const Eigen::Matrix4d T_camera_to_odom = T_gimbal_to_odom * T_camera_to_gimbal;
|
|
|
|
return T_camera_to_odom;
|
|
}
|
|
|
|
void addVelFromAccDt(Eigen::Vector3d& vel, const Eigen::Vector3d& acc, double dt) noexcept {
|
|
vel.x() += acc.x() * dt;
|
|
vel.y() += acc.y() * dt;
|
|
vel.z() += acc.z() * dt;
|
|
}
|
|
void addPosFromVelDt(Eigen::Vector3d& pos, const Eigen::Vector3d& vel, double dt) noexcept {
|
|
pos.x() += vel.x() * dt;
|
|
pos.y() += vel.y() * dt;
|
|
pos.z() += vel.z() * dt;
|
|
}
|
|
template<typename T>
|
|
bool tryGetValue(const YAML::Node& node, const char* key, T& out_val) {
|
|
if (!node[key]) {
|
|
return false;
|
|
}
|
|
try {
|
|
out_val = node[key].template as<T>();
|
|
return true;
|
|
} catch (...) {
|
|
return false;
|
|
}
|
|
}
|
|
void changeFileOwner(const std::string& filepath, const std::string& username) {
|
|
struct passwd* pwd = getpwnam(username.c_str());
|
|
if (pwd == nullptr) {
|
|
perror("getpwnam failed");
|
|
return;
|
|
}
|
|
const uid_t uid = pwd->pw_uid;
|
|
const gid_t gid = pwd->pw_gid;
|
|
|
|
if (chown(filepath.c_str(), uid, gid) != 0) {
|
|
perror("chown failed");
|
|
}
|
|
}
|
|
std::string getOriginalUsername() {
|
|
const char* sudo_user = std::getenv("SUDO_USER");
|
|
if (sudo_user) {
|
|
return std::string(sudo_user);
|
|
}
|
|
const uid_t uid = getuid();
|
|
struct passwd* pw = getpwuid(uid);
|
|
if (pw) {
|
|
return std::string(pw->pw_name);
|
|
}
|
|
return "";
|
|
}
|
|
bool setThreadAffinityAndPriority(
|
|
std::thread& thread,
|
|
int cpu_id,
|
|
int priority,
|
|
bool use_sched_fifo
|
|
) {
|
|
#ifdef __linux__
|
|
pthread_t native = thread.native_handle();
|
|
|
|
cpu_set_t cpuset;
|
|
CPU_ZERO(&cpuset);
|
|
CPU_SET(cpu_id, &cpuset);
|
|
if (pthread_setaffinity_np(native, sizeof(cpu_set_t), &cpuset) != 0) {
|
|
perror("pthread_setaffinity_np failed");
|
|
return false;
|
|
}
|
|
|
|
sched_param sch_params;
|
|
sch_params.sched_priority = priority;
|
|
int policy = use_sched_fifo ? SCHED_FIFO : SCHED_RR;
|
|
if (pthread_setschedparam(native, policy, &sch_params) != 0) {
|
|
perror("pthread_setschedparam failed");
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
|
|
#elif defined(_WIN32) || defined(_WIN64)
|
|
HANDLE native = (HANDLE)thread.native_handle();
|
|
|
|
DWORD_PTR affinityMask = 1ULL << cpu_id;
|
|
if (SetThreadAffinityMask(native, affinityMask) == 0) {
|
|
return false;
|
|
}
|
|
|
|
int win_priority = THREAD_PRIORITY_HIGHEST; // you can map `priority` if needed
|
|
if (!SetThreadPriority(native, win_priority)) {
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
|
|
#else
|
|
// Unsupported platform
|
|
(void)thread;
|
|
(void)cpu_id;
|
|
(void)priority;
|
|
(void)use_sched_fifo;
|
|
return false;
|
|
#endif
|
|
}
|
|
double rad2deg(double rad) noexcept {
|
|
return rad * 180.0 / M_PI;
|
|
}
|
|
double deg2rad(double deg) noexcept {
|
|
return deg * M_PI / 180.0;
|
|
}
|
|
|
|
std::tuple<double, double, double> xyz2ypd_rad(double x, double y, double z) noexcept {
|
|
const double distance = std::sqrt(x * x + y * y + z * z);
|
|
const double yaw = std::atan2(y, x);
|
|
const double pitch = std::atan2(z, std::sqrt(x * x + y * y));
|
|
return std::make_tuple(yaw, pitch, distance);
|
|
}
|
|
|
|
std::tuple<double, double, double>
|
|
ypd2xyz_rad(double yaw, double pitch, double distance) noexcept {
|
|
const double x = distance * std::cos(pitch) * std::cos(yaw);
|
|
const double y = distance * std::cos(pitch) * std::sin(yaw);
|
|
const double z = distance * std::sin(pitch);
|
|
return std::make_tuple(x, y, z);
|
|
}
|
|
|
|
std::tuple<double, double, double> xyz2ypd_deg(double x, double y, double z) noexcept {
|
|
const double distance = std::sqrt(x * x + y * y + z * z);
|
|
const double yaw = std::atan2(y, x);
|
|
const double pitch = std::atan2(z, std::sqrt(x * x + y * y));
|
|
return std::make_tuple(rad2deg(yaw), rad2deg(pitch), distance);
|
|
}
|
|
|
|
std::tuple<double, double, double>
|
|
ypd2xyz_deg(double yaw_deg, double pitch_deg, double distance) noexcept {
|
|
const double yaw = deg2rad(yaw_deg);
|
|
const double pitch = deg2rad(pitch_deg);
|
|
const double x = distance * std::cos(pitch) * std::cos(yaw);
|
|
const double y = distance * std::cos(pitch) * std::sin(yaw);
|
|
const double z = distance * std::sin(pitch);
|
|
return std::make_tuple(x, y, z);
|
|
}
|
|
Eigen::Vector3d xyz2ypd(const Eigen::Vector3d& xyz) noexcept {
|
|
const auto x = xyz[0], y = xyz[1], z = xyz[2];
|
|
const auto yaw = std::atan2(y, x);
|
|
const auto pitch = std::atan2(z, std::sqrt(x * x + y * y));
|
|
const auto distance = std::sqrt(x * x + y * y + z * z);
|
|
return { yaw, pitch, distance };
|
|
}
|
|
template<typename Point>
|
|
Point getCenter(const std::vector<Point>& points) noexcept {
|
|
if (points.empty())
|
|
return Point();
|
|
return std::accumulate(points.begin(), points.end(), Point())
|
|
/ static_cast<float>(points.size());
|
|
}
|
|
bool segmentIntersection(
|
|
const cv::Point2f& a1,
|
|
const cv::Point2f& a2,
|
|
const cv::Point2f& b1,
|
|
const cv::Point2f& b2,
|
|
cv::Point2f& intersection
|
|
) {
|
|
const cv::Point2f r = a2 - a1;
|
|
const cv::Point2f s = b2 - b1;
|
|
const float rxs = r.x * s.y - r.y * s.x;
|
|
const float qpxr = (b1 - a1).x * r.y - (b1 - a1).y * r.x;
|
|
|
|
if (fabs(rxs) < 1e-6)
|
|
return false; // 平行或重叠，无唯一交点
|
|
|
|
const float t = ((b1 - a1).x * s.y - (b1 - a1).y * s.x) / rxs;
|
|
const float u = qpxr / rxs;
|
|
|
|
if (t >= 0 && t <= 1 && u >= 0 && u <= 1) {
|
|
intersection = a1 + t * r;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
|
|
std::vector<cv::Point2f> intersectLineRotatedRect(
|
|
const cv::RotatedRect& rect,
|
|
const cv::Point2f& line_p1,
|
|
const cv::Point2f& line_p2
|
|
) {
|
|
std::vector<cv::Point2f> intersections;
|
|
|
|
cv::Point2f vertices[4];
|
|
rect.points(vertices);
|
|
|
|
for (int i = 0; i < 4; i++) {
|
|
const cv::Point2f p1 = vertices[i];
|
|
const cv::Point2f p2 = vertices[(i + 1) % 4];
|
|
cv::Point2f inter;
|
|
|
|
if (segmentIntersection(line_p1, line_p2, p1, p2, inter)) {
|
|
intersections.push_back(inter);
|
|
}
|
|
}
|
|
|
|
return intersections;
|
|
}
|
|
std::string makeTimestampedFileName() {
|
|
using namespace std::chrono;
|
|
const auto now = system_clock::now();
|
|
const auto time_t_now = system_clock::to_time_t(now);
|
|
const auto ms = duration_cast<milliseconds>(now.time_since_epoch()) % 1000;
|
|
|
|
std::tm tm_now {};
|
|
#if defined(_MSC_VER)
|
|
localtime_s(&tm_now, &time_t_now);
|
|
#else
|
|
localtime_r(&time_t_now, &tm_now);
|
|
#endif
|
|
|
|
std::ostringstream oss;
|
|
oss << std::put_time(&tm_now, "%Y%m%d_%H%M%S") << "_" << std::setfill('0') << std::setw(3)
|
|
<< ms.count();
|
|
return oss.str();
|
|
}
|
|
|
|
std::string expandEnv(const std::string& s) {
|
|
std::regex env_re(R"(\$\{([^}]+)\})");
|
|
std::smatch match;
|
|
std::string result = s;
|
|
while (std::regex_search(result, match, env_re)) {
|
|
const char* env = std::getenv(match[1].str().c_str());
|
|
std::string val = env ? env : "";
|
|
result.replace(match.position(0), match.length(0), val);
|
|
}
|
|
return result;
|
|
}
|
|
double computeBrightness(const cv::Mat& frame) {
|
|
cv::Mat gray;
|
|
cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
|
|
return cv::mean(gray)[0];
|
|
}
|
|
cv::Mat letterbox(
|
|
const cv::Mat& img,
|
|
Eigen::Matrix3f& transform_matrix,
|
|
const int new_shape_w,
|
|
const int new_shape_h
|
|
) noexcept {
|
|
const int img_h = img.rows;
|
|
const int img_w = img.cols;
|
|
|
|
const float scale = std::min((float)new_shape_h / img_h, (float)new_shape_w / img_w);
|
|
const int resize_h = int(img_h * scale + 0.5f);
|
|
const int resize_w = int(img_w * scale + 0.5f);
|
|
|
|
const int pad_h = new_shape_h - resize_h;
|
|
const int pad_w = new_shape_w - resize_w;
|
|
const int top = pad_h / 2;
|
|
const int left = pad_w / 2;
|
|
|
|
cv::Mat resized;
|
|
cv::resize(img, resized, cv::Size(resize_w, resize_h), 0, 0, cv::INTER_LINEAR);
|
|
|
|
cv::Mat out;
|
|
cv::copyMakeBorder(
|
|
resized,
|
|
out,
|
|
top,
|
|
pad_h - top,
|
|
left,
|
|
pad_w - left,
|
|
cv::BORDER_CONSTANT,
|
|
cv::Scalar(114, 114, 114)
|
|
);
|
|
|
|
const float inv_scale = 1.0f / scale;
|
|
|
|
transform_matrix << inv_scale, 0, -left * inv_scale, 0, inv_scale, -top * inv_scale, 0, 0,
|
|
1;
|
|
|
|
return out;
|
|
}
|
|
|
|
} // namespace utils
|
|
} // namespace wust_vision
|