add wust typr mpc and mutipule x
This commit is contained in:
cyy_mac
213
wust_vision-main/tasks/utils/utils.hpp
Normal file
213
wust_vision-main/tasks/utils/utils.hpp
Normal file
@@ -0,0 +1,213 @@
|
||||
// Created by Chengfu Zou on 2024.1.19
|
||||
// Copyright(C) FYT Vision Group. All rights resevred.
|
||||
// Copyright 2025 XiaoJian Wu
|
||||
//
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "3rdparty/angles.h"
|
||||
#include <Eigen/Dense>
|
||||
#include <concepts>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <thread>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
namespace wust_vision {
|
||||
// util functions
|
||||
namespace utils {
|
||||
// Convert euler angles to rotation matrix
|
||||
enum class EulerOrder { XYZ, XZY, YXZ, YZX, ZXY, ZYX };
|
||||
|
||||
double limit_rad(double angle) noexcept;
|
||||
|
||||
Eigen::Vector3d quatToEuler(
|
||||
const Eigen::Quaterniond& q,
|
||||
int axis0,
|
||||
int axis1,
|
||||
int axis2,
|
||||
bool extrinsic = true
|
||||
);
|
||||
Eigen::Quaterniond eulerToQuat(
|
||||
const Eigen::Vector3d& euler,
|
||||
int axis0,
|
||||
int axis1,
|
||||
int axis2,
|
||||
bool extrinsic = true
|
||||
);
|
||||
|
||||
Eigen::Matrix3d eulerToMatrix(
|
||||
const Eigen::Vector3d& euler,
|
||||
int axis0,
|
||||
int axis1,
|
||||
int axis2,
|
||||
bool extrinsic = true
|
||||
);
|
||||
|
||||
Eigen::Vector3d
|
||||
matrixToEuler(const Eigen::Matrix3d& R, int axis0, int axis1, int axis2, bool extrinsic = true);
|
||||
|
||||
Eigen::Vector3d
|
||||
quatToEuler(const Eigen::Quaterniond& q, EulerOrder order, bool extrinsic = true);
|
||||
|
||||
Eigen::Quaterniond
|
||||
eulerToQuat(const Eigen::Vector3d& euler, EulerOrder order, bool extrinsic = true);
|
||||
Eigen::Matrix3d
|
||||
eulerToMatrix(const Eigen::Vector3d& euler, EulerOrder order, bool extrinsic = true);
|
||||
Eigen::Vector3d
|
||||
matrixToEuler(const Eigen::Matrix3d& R, EulerOrder order, bool extrinsic = true);
|
||||
|
||||
Eigen::MatrixXd cvToEigen(const cv::Mat& cv_mat) noexcept;
|
||||
Eigen::Vector3d transformPosition(
|
||||
const Eigen::Vector3d& pos_camera,
|
||||
const Eigen::Matrix4d& T_camera_to_odom
|
||||
) noexcept;
|
||||
|
||||
Eigen::Quaterniond transformOrientation(
|
||||
const Eigen::Quaterniond& q_camera,
|
||||
const Eigen::Matrix4d& T_camera_to_odom
|
||||
) noexcept;
|
||||
void pnpToEigen(
|
||||
const cv::Mat& rvec,
|
||||
const cv::Mat& tvec,
|
||||
Eigen::Vector3d& t_out,
|
||||
Eigen::Quaterniond& q_out
|
||||
) noexcept;
|
||||
void pnpToEigen(
|
||||
const cv::Vec3d& rvec,
|
||||
const cv::Vec3d& tvec,
|
||||
Eigen::Vector3d& t_out,
|
||||
Eigen::Quaterniond& q_out
|
||||
) noexcept;
|
||||
|
||||
cv::Point2f computeCenter(const std::vector<cv::Point2f>& points) noexcept;
|
||||
bool isStateValid(const Eigen::VectorXd& state) noexcept;
|
||||
Eigen::Matrix4d computeCameraToOdomTransform(
|
||||
const Eigen::Matrix3d& R_gimbal2odom,
|
||||
const Eigen::Matrix3d& R_camera_to_gimbal,
|
||||
const Eigen::Vector3d& t_camera_to_gimbal
|
||||
) noexcept;
|
||||
|
||||
void addVelFromAccDt(Eigen::Vector3d& vel, const Eigen::Vector3d& acc, double dt) noexcept;
|
||||
void addPosFromVelDt(Eigen::Vector3d& pos, const Eigen::Vector3d& vel, double dt) noexcept;
|
||||
template<typename T>
|
||||
bool tryGetValue(const YAML::Node& node, const char* key, T& out_val);
|
||||
void changeFileOwner(const std::string& filepath, const std::string& username);
|
||||
std::string getOriginalUsername();
|
||||
bool setThreadAffinityAndPriority(
|
||||
std::thread& thread,
|
||||
int cpu_id,
|
||||
int priority,
|
||||
bool use_sched_fifo
|
||||
);
|
||||
double rad2deg(double rad) noexcept;
|
||||
double deg2rad(double deg) noexcept;
|
||||
std::tuple<double, double, double> xyz2ypd_rad(double x, double y, double z) noexcept;
|
||||
|
||||
std::tuple<double, double, double>
|
||||
ypd2xyz_rad(double yaw, double pitch, double distance) noexcept;
|
||||
|
||||
std::tuple<double, double, double> xyz2ypd_deg(double x, double y, double z) noexcept;
|
||||
std::tuple<double, double, double>
|
||||
ypd2xyz_deg(double yaw_deg, double pitch_deg, double distance) noexcept;
|
||||
Eigen::Vector3d xyz2ypd(const Eigen::Vector3d& xyz) noexcept;
|
||||
template<typename Point>
|
||||
Point getCenter(const std::vector<Point>& points) noexcept;
|
||||
bool segmentIntersection(
|
||||
const cv::Point2f& a1,
|
||||
const cv::Point2f& a2,
|
||||
const cv::Point2f& b1,
|
||||
const cv::Point2f& b2,
|
||||
cv::Point2f& intersection
|
||||
);
|
||||
std::vector<cv::Point2f> intersectLineRotatedRect(
|
||||
const cv::RotatedRect& rect,
|
||||
const cv::Point2f& line_p1,
|
||||
const cv::Point2f& line_p2
|
||||
);
|
||||
std::string makeTimestampedFileName();
|
||||
std::string expandEnv(const std::string& s);
|
||||
double computeBrightness(const cv::Mat& frame);
|
||||
cv::Mat letterbox(
|
||||
const cv::Mat& img,
|
||||
Eigen::Matrix3f& transform_matrix,
|
||||
const int new_shape_w,
|
||||
const int new_shape_h
|
||||
) noexcept;
|
||||
template<typename Func>
|
||||
void XSecOnce(Func&& func, double dt) noexcept {
|
||||
static auto last_call = std::chrono::steady_clock::now();
|
||||
|
||||
const auto now = std::chrono::steady_clock::now();
|
||||
const double elapsed = std::chrono::duration<double>(now - last_call).count();
|
||||
|
||||
if (elapsed >= dt) {
|
||||
last_call = now;
|
||||
func();
|
||||
}
|
||||
}
|
||||
template<typename T>
|
||||
concept Point2DLike = requires(T p) {
|
||||
{
|
||||
p.x
|
||||
} -> std::convertible_to<float>;
|
||||
{
|
||||
p.y
|
||||
} -> std::convertible_to<float>;
|
||||
T { 0.f, 0.f };
|
||||
};
|
||||
template<Point2DLike T>
|
||||
[[nodiscard]] inline T transformPoint2D(const Eigen::Matrix3f& H, const T& p) noexcept {
|
||||
const Eigen::Vector3f hp { p.x, p.y, 1.f };
|
||||
const Eigen::Vector3f tp = H * hp;
|
||||
return { tp.x(), tp.y() };
|
||||
}
|
||||
inline cv::Rect2f transformRect(const Eigen::Matrix3f& H, const cv::Rect2f& rect) {
|
||||
cv::Point2f p1(rect.x, rect.y);
|
||||
cv::Point2f p2(rect.x + rect.width, rect.y);
|
||||
cv::Point2f p3(rect.x, rect.y + rect.height);
|
||||
cv::Point2f p4(rect.x + rect.width, rect.y + rect.height);
|
||||
|
||||
auto tp1 = utils::transformPoint2D(H, p1);
|
||||
auto tp2 = utils::transformPoint2D(H, p2);
|
||||
auto tp3 = utils::transformPoint2D(H, p3);
|
||||
auto tp4 = utils::transformPoint2D(H, p4);
|
||||
|
||||
float min_x = std::min({ tp1.x, tp2.x, tp3.x, tp4.x });
|
||||
float min_y = std::min({ tp1.y, tp2.y, tp3.y, tp4.y });
|
||||
float max_x = std::max({ tp1.x, tp2.x, tp3.x, tp4.x });
|
||||
float max_y = std::max({ tp1.y, tp2.y, tp3.y, tp4.y });
|
||||
|
||||
return cv::Rect2f(min_x, min_y, max_x - min_x, max_y - min_y);
|
||||
}
|
||||
template<class Tag>
|
||||
[[nodiscard]] double orientationToYaw(const Eigen::Quaterniond& q) noexcept {
|
||||
static double last_yaw = 0;
|
||||
double roll, pitch, yaw;
|
||||
const Eigen::Vector3d euler = utils::quatToEuler(q, utils::EulerOrder::ZYX, false);
|
||||
yaw = euler[0];
|
||||
yaw = last_yaw + angles::shortest_angular_distance(last_yaw, yaw);
|
||||
last_yaw = yaw;
|
||||
return yaw;
|
||||
}
|
||||
template<class Tag>
|
||||
[[nodiscard]] double orientationToRoll(const Eigen::Quaterniond& q) noexcept {
|
||||
static double last_roll = 0;
|
||||
double roll, pitch, yaw;
|
||||
Eigen::Vector3d euler = utils::quatToEuler(q, utils::EulerOrder::ZYX, false);
|
||||
roll = euler[2];
|
||||
roll = last_roll + angles::shortest_angular_distance(last_roll, roll);
|
||||
last_roll = roll;
|
||||
return roll;
|
||||
}
|
||||
} // namespace utils
|
||||
} // namespace wust_vision
|
||||
Reference in New Issue
Block a user