add wust typr mpc and mutipule x

wust_vision-main/tasks/utils/utils.cpp

@@ -0,0 +1,536 @@
+#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