add wust typr mpc and mutipule x
This commit is contained in:
cyy_mac
64
wust_vision-main/tasks/utils/ascii_banner.hpp
Normal file
64
wust_vision-main/tasks/utils/ascii_banner.hpp
Normal file
@@ -0,0 +1,64 @@
|
||||
#pragma once
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
namespace wust_vision {
|
||||
constexpr std::array ascii_banner = {
|
||||
R"( _ ____ _____________ _ ___________ ________ _ __ )",
|
||||
R"(| | / / / / / ___/_ __/ | | / / _/ ___// _/ __ \/ | / /)",
|
||||
R"(| | /| / / / / /\__ \ / / | | / // / \__ \ / // / / / |/ / )",
|
||||
R"(| |/ |/ / /_/ /___/ // / | |/ // / ___/ // // /_/ / /| / )",
|
||||
R"(|__/|__/\____//____//_/ |___/___//____/___/\____/_/ |_/ )",
|
||||
};
|
||||
namespace {
|
||||
struct RGB {
|
||||
int r, g, b;
|
||||
};
|
||||
|
||||
inline RGB hsv2rgb(float h, float s, float v) {
|
||||
float c = v * s;
|
||||
float x = c * (1 - std::fabs(std::fmod(h / 60.0f, 2) - 1));
|
||||
float m = v - c;
|
||||
|
||||
float r = 0, g = 0, b = 0;
|
||||
if (h < 60) {
|
||||
r = c;
|
||||
g = x;
|
||||
} else if (h < 120) {
|
||||
r = x;
|
||||
g = c;
|
||||
} else if (h < 180) {
|
||||
g = c;
|
||||
b = x;
|
||||
} else if (h < 240) {
|
||||
g = x;
|
||||
b = c;
|
||||
} else if (h < 300) {
|
||||
r = x;
|
||||
b = c;
|
||||
} else {
|
||||
r = c;
|
||||
b = x;
|
||||
}
|
||||
|
||||
return { int((r + m) * 255), int((g + m) * 255), int((b + m) * 255) };
|
||||
}
|
||||
} // namespace
|
||||
inline void printBanner() {
|
||||
constexpr const char* reset = "\033[0m";
|
||||
|
||||
for (const auto& line: ascii_banner) {
|
||||
const int n = static_cast<int>(std::string_view(line).size());
|
||||
|
||||
for (int i = 0; i < n; ++i) {
|
||||
// hue 从 0° → 360°
|
||||
float hue = 360.0f * i / std::max(1, n - 1);
|
||||
auto rgb = hsv2rgb(hue, 1.0f, 1.0f);
|
||||
|
||||
std::cout << "\033[38;2;" << rgb.r << ";" << rgb.g << ";" << rgb.b << "m" << line[i];
|
||||
}
|
||||
std::cout << reset << '\n';
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace wust_vision
|
||||
11
wust_vision-main/tasks/utils/config.hpp
Normal file
11
wust_vision-main/tasks/utils/config.hpp
Normal file
@@ -0,0 +1,11 @@
|
||||
#pragma once
|
||||
namespace wust_vision {
|
||||
constexpr const char* ML_CONFIG = "config/detect_ml.yaml";
|
||||
constexpr const char* OPENCV_CONFIG = "config/detect_opencv.yaml";
|
||||
constexpr const char* COMMON_CONFIG = "config/common.yaml";
|
||||
constexpr const char* CAMERA_CONFIG = "config/camera.yaml";
|
||||
constexpr const char* AUTO_AIM_CONFIG = "config/auto_aim.yaml";
|
||||
constexpr const char* AUTO_BUFF_CONFIG = "config/auto_buff.yaml";
|
||||
constexpr const char* AUTO_GUIDANCE_CONFIG = "config/auto_guidance.yaml";
|
||||
constexpr const char* AUTO_SNIPER_CONFIG = "config/auto_sniper.yaml";
|
||||
} // namespace wust_vision
|
||||
154
wust_vision-main/tasks/utils/debug_utils.hpp
Normal file
154
wust_vision-main/tasks/utils/debug_utils.hpp
Normal file
@@ -0,0 +1,154 @@
|
||||
#pragma once
|
||||
#include <fcntl.h>
|
||||
#include <fmt/core.h>
|
||||
#include <fstream>
|
||||
#include <nlohmann/json.hpp>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <sys/mman.h>
|
||||
#include <wust_vl/video/icamera.hpp>
|
||||
namespace wust_vision {
|
||||
|
||||
template<typename T, int MAX_N>
|
||||
class LogsStream {
|
||||
public:
|
||||
LogsStream(const std::string& n) {
|
||||
name = n;
|
||||
}
|
||||
void handleOnce(const T& t, nlohmann::json& j) {
|
||||
log_data.push_back(t);
|
||||
trim();
|
||||
insertData(j);
|
||||
}
|
||||
void push_back(const T& t) {
|
||||
log_data.push_back(t);
|
||||
}
|
||||
void trim() {
|
||||
while (log_data.size() > MAX_N) {
|
||||
log_data.erase(log_data.begin());
|
||||
}
|
||||
}
|
||||
void insertData(nlohmann::json& j) {
|
||||
j[name] = log_data;
|
||||
}
|
||||
void clear() {
|
||||
log_data.clear();
|
||||
}
|
||||
|
||||
private:
|
||||
std::string name;
|
||||
std::vector<T> log_data;
|
||||
};
|
||||
template<typename DebugT, typename DrawFn, typename OutputFn>
|
||||
void drawDebugOverlayImpl(
|
||||
const DebugT& dbg,
|
||||
std::pair<cv::Mat, cv::Mat> camera_info,
|
||||
bool auto_fps,
|
||||
DrawFn&& draw_fn,
|
||||
OutputFn&& output_fn
|
||||
) {
|
||||
static auto last_show_time = std::chrono::steady_clock::now();
|
||||
|
||||
if (dbg.img_frame.src_img.empty())
|
||||
return;
|
||||
|
||||
constexpr double min_interval_ms = 1000.0 / 30.0;
|
||||
const auto now = std::chrono::steady_clock::now();
|
||||
|
||||
if (auto_fps
|
||||
&& std::chrono::duration<double, std::milli>(now - last_show_time).count()
|
||||
< min_interval_ms)
|
||||
return;
|
||||
|
||||
last_show_time = now;
|
||||
|
||||
cv::Mat debug_img;
|
||||
if (dbg.img_frame.pixel_format == wust_vl::video::PixelFormat::GRAY) {
|
||||
cv::cvtColor(dbg.img_frame.src_img, debug_img, cv::COLOR_GRAY2RGB);
|
||||
} else if (dbg.img_frame.pixel_format == wust_vl::video::PixelFormat::BGR) {
|
||||
cv::cvtColor(dbg.img_frame.src_img, debug_img, cv::COLOR_BGR2RGB);
|
||||
} else {
|
||||
debug_img = dbg.img_frame.src_img;
|
||||
}
|
||||
|
||||
if (debug_img.empty())
|
||||
return;
|
||||
draw_fn(debug_img, dbg, camera_info);
|
||||
|
||||
output_fn(debug_img);
|
||||
}
|
||||
inline auto writeToFile = [](const cv::Mat& img) {
|
||||
cv::Mat bgr;
|
||||
cv::cvtColor(img, bgr, cv::COLOR_RGB2BGR);
|
||||
|
||||
std::vector<uchar> buf;
|
||||
cv::imencode(".jpg", bgr, buf);
|
||||
|
||||
std::ofstream ofs("/dev/shm/debug_frame.jpg.tmp", std::ios::binary);
|
||||
ofs.write(reinterpret_cast<const char*>(buf.data()), buf.size());
|
||||
ofs.close();
|
||||
|
||||
std::rename("/dev/shm/debug_frame.jpg.tmp", "/dev/shm/debug_frame.jpg");
|
||||
};
|
||||
class ShmWriter {
|
||||
public:
|
||||
static constexpr size_t shm_max_size = 2 * 1024 * 1024;
|
||||
|
||||
explicit ShmWriter(const char* name, mode_t mode = 0666) {
|
||||
fd_ = shm_open(name, O_CREAT | O_RDWR, mode);
|
||||
if (fd_ == -1) {
|
||||
std::cerr << "[SHM] shm_open failed\n";
|
||||
return;
|
||||
}
|
||||
|
||||
if (ftruncate(fd_, shm_max_size) == -1) {
|
||||
std::cerr << "[SHM] ftruncate failed\n";
|
||||
close(fd_);
|
||||
fd_ = -1;
|
||||
return;
|
||||
}
|
||||
|
||||
ptr_ = mmap(nullptr, shm_max_size, PROT_READ | PROT_WRITE, MAP_SHARED, fd_, 0);
|
||||
|
||||
if (ptr_ == MAP_FAILED) {
|
||||
std::cerr << "[SHM] mmap failed\n";
|
||||
close(fd_);
|
||||
fd_ = -1;
|
||||
ptr_ = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
~ShmWriter() {
|
||||
if (ptr_)
|
||||
munmap(ptr_, shm_max_size);
|
||||
if (fd_ != -1)
|
||||
close(fd_);
|
||||
}
|
||||
|
||||
void operator()(const cv::Mat& img) const {
|
||||
if (!ptr_)
|
||||
return;
|
||||
|
||||
static const std::vector<int> jpeg_params = { cv::IMWRITE_JPEG_QUALITY, 75 };
|
||||
|
||||
std::vector<uchar> buf;
|
||||
cv::imencode(".jpg", img, buf, jpeg_params);
|
||||
|
||||
if (buf.size() + 4 > shm_max_size)
|
||||
return;
|
||||
|
||||
uint32_t size = static_cast<uint32_t>(buf.size());
|
||||
std::memcpy(ptr_, &size, 4);
|
||||
std::memcpy(static_cast<char*>(ptr_) + 4, buf.data(), size);
|
||||
}
|
||||
|
||||
private:
|
||||
int fd_ { -1 };
|
||||
void* ptr_ { nullptr };
|
||||
};
|
||||
inline auto showWindow(const char* win_name) {
|
||||
return [win_name](const cv::Mat& img) {
|
||||
cv::imshow(win_name, img);
|
||||
cv::waitKey(1);
|
||||
};
|
||||
}
|
||||
} // namespace wust_vision
|
||||
99
wust_vision-main/tasks/utils/main_base.hpp
Normal file
99
wust_vision-main/tasks/utils/main_base.hpp
Normal file
@@ -0,0 +1,99 @@
|
||||
#pragma once
|
||||
#include "3rdparty/backward-cpp/backward.hpp"
|
||||
#include "tasks/utils/ascii_banner.hpp"
|
||||
#include "wust_vl/common/concurrency/monitored_thread.hpp"
|
||||
#include "wust_vl/common/utils/signal.hpp"
|
||||
|
||||
namespace wust_vision {
|
||||
template<typename T>
|
||||
concept VisionLike = std::default_initializable<T> && requires(T v, bool b) {
|
||||
{
|
||||
v.init(b)
|
||||
} -> std::same_as<bool>;
|
||||
{
|
||||
v.start()
|
||||
} -> std::same_as<void>;
|
||||
{
|
||||
v.checkStateMatchMode()
|
||||
} -> std::same_as<void>;
|
||||
};
|
||||
|
||||
template<VisionLike T>
|
||||
inline int runVisionMain(int argc, char** argv) {
|
||||
printBanner();
|
||||
bool debug = false;
|
||||
if (argc > 1) {
|
||||
const std::string firstArg = argv[1];
|
||||
debug = (firstArg == "true" || firstArg == "1");
|
||||
std::cout << "debug: " << firstArg << std::endl;
|
||||
}
|
||||
std::set_terminate([]() {
|
||||
std::cerr << "Uncaught exception, terminating program.\n";
|
||||
if (auto e = std::current_exception()) {
|
||||
try {
|
||||
std::rethrow_exception(e);
|
||||
} catch (const std::exception& ex) {
|
||||
std::cerr << "Exception: " << ex.what() << std::endl;
|
||||
} catch (...) {
|
||||
std::cerr << "Unknown exception" << std::endl;
|
||||
}
|
||||
}
|
||||
std::abort();
|
||||
});
|
||||
|
||||
try {
|
||||
int exit_code = 0;
|
||||
|
||||
{
|
||||
T v;
|
||||
v.init(debug);
|
||||
std::cout << "Starting program..." << std::endl;
|
||||
v.start();
|
||||
|
||||
wust_vl::common::utils::SignalHandler sig;
|
||||
sig.start([&] {});
|
||||
|
||||
bool exit_flag = false;
|
||||
|
||||
while (!sig.shouldExit() && !exit_flag) {
|
||||
wust_vl::common::concurrency::ThreadManager::instance().printStatus();
|
||||
const auto all_status =
|
||||
wust_vl::common::concurrency::ThreadManager::instance().getAllThreadStatuses();
|
||||
v.checkStateMatchMode();
|
||||
// for (auto& status: all_status) {
|
||||
// if (status.second
|
||||
// == wust_vl::common::concurrency::MonitoredThread::Status::Hung) {
|
||||
// std::cerr << status.first << " is Hunging! Exiting program..." << std::endl;
|
||||
// exit_code = -1;
|
||||
// std::exit(exit_code);
|
||||
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
||||
}
|
||||
}
|
||||
|
||||
std::cout << "Exiting program..." << std::endl;
|
||||
return exit_code;
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Caught exception in main: " << e.what() << "\n";
|
||||
throw;
|
||||
return -1;
|
||||
} catch (...) {
|
||||
std::cerr << "Unknown exception caught in main!\n";
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
} // namespace wust_vision
|
||||
|
||||
#define VISION_MAIN(VISION_TYPE) \
|
||||
int main(int argc, char** argv) { \
|
||||
return wust_vision::runVisionMain<VISION_TYPE>(argc, argv); \
|
||||
}
|
||||
|
||||
#define ENABLE_BACKWARD() \
|
||||
namespace backward { \
|
||||
static backward::SignalHandling sh; \
|
||||
}
|
||||
160
wust_vision-main/tasks/utils/sinple_img_rotate_saver.hpp
Normal file
160
wust_vision-main/tasks/utils/sinple_img_rotate_saver.hpp
Normal file
@@ -0,0 +1,160 @@
|
||||
#pragma once
|
||||
#include "Eigen/Dense"
|
||||
#include "opencv2/opencv.hpp"
|
||||
#include "wust_vl/common/utils/recorder.hpp"
|
||||
#include "wust_vl/common/utils/timer.hpp"
|
||||
namespace wust_vision {
|
||||
class ImgWriter: public wust_vl::common::utils::Writer<cv::Mat> {
|
||||
public:
|
||||
explicit ImgWriter(
|
||||
const std::filesystem::path& video_path,
|
||||
int fps = 30,
|
||||
int fourcc = cv::VideoWriter::fourcc('a', 'v', 'c', '1')
|
||||
):
|
||||
path_(video_path),
|
||||
fps_(fps),
|
||||
fourcc_(fourcc) {}
|
||||
|
||||
~ImgWriter() override {
|
||||
release();
|
||||
}
|
||||
|
||||
void write(std::ostream&, const cv::Mat& frame) override {
|
||||
if (frame.empty())
|
||||
return;
|
||||
|
||||
if (!initialized_) {
|
||||
initVideoWriter(frame.size());
|
||||
}
|
||||
|
||||
cv::Mat bgr;
|
||||
if (frame.channels() == 3)
|
||||
cv::cvtColor(frame, bgr, cv::COLOR_RGB2BGR);
|
||||
else
|
||||
cv::cvtColor(frame, bgr, cv::COLOR_GRAY2BGR);
|
||||
|
||||
writer_.write(bgr);
|
||||
}
|
||||
|
||||
private:
|
||||
void initVideoWriter(const cv::Size& frame_size) {
|
||||
if (std::filesystem::exists(path_.parent_path()) == false)
|
||||
std::filesystem::create_directories(path_.parent_path());
|
||||
|
||||
writer_.open(path_.string(), fourcc_, fps_, frame_size, true);
|
||||
if (!writer_.isOpened()) {
|
||||
throw std::runtime_error("Failed to open video writer for " + path_.string());
|
||||
}
|
||||
initialized_ = true;
|
||||
|
||||
std::cout << "[ImgWriter] Started writing video: " << path_ << std::endl;
|
||||
}
|
||||
|
||||
void release() {
|
||||
if (initialized_) {
|
||||
writer_.release();
|
||||
std::cout << "[ImgWriter] Video saved: " << path_ << std::endl;
|
||||
initialized_ = false;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
std::filesystem::path path_;
|
||||
cv::VideoWriter writer_;
|
||||
int fps_;
|
||||
int fourcc_;
|
||||
bool initialized_ = false;
|
||||
};
|
||||
|
||||
class RotateWriterCSV: public wust_vl::common::utils::Writer<Eigen::Vector3d> {
|
||||
public:
|
||||
using RotateWriterCSVPtr = std::shared_ptr<RotateWriterCSV>;
|
||||
inline RotateWriterCSVPtr makeShared(bool write_header = true) {
|
||||
return std::make_shared<RotateWriterCSV>(write_header);
|
||||
}
|
||||
explicit RotateWriterCSV(bool write_header = true): first_write_(write_header) {}
|
||||
|
||||
void write(std::ostream& os, const Eigen::Vector3d& data) override {
|
||||
double now = wust_vl::common::utils::time_utils::sinceProgramStartSec();
|
||||
if (first_write_) {
|
||||
os << "time,yaw,pitch,roll\n";
|
||||
first_write_ = false;
|
||||
}
|
||||
|
||||
os << std::fixed << std::setprecision(6) << now << "," << data[0] << "," << data[1] << ","
|
||||
<< data[2] << "\n";
|
||||
os.flush();
|
||||
}
|
||||
|
||||
private:
|
||||
bool first_write_;
|
||||
};
|
||||
class RotateReaderCSV {
|
||||
public:
|
||||
using RotateReaderCSVPtr = std::shared_ptr<RotateReaderCSV>;
|
||||
struct Record {
|
||||
double time;
|
||||
Eigen::Vector3d ypr;
|
||||
};
|
||||
|
||||
explicit RotateReaderCSV(const std::string& csv_path, double speed = 1.0):
|
||||
csv_path_(csv_path),
|
||||
speed_(speed) {
|
||||
loadCSV();
|
||||
}
|
||||
|
||||
/// 播放记录：每条数据 sleep 对齐真实时间间隔
|
||||
void replay(std::function<void(const Eigen::Vector3d&)> callback) const {
|
||||
if (records_.empty())
|
||||
return;
|
||||
|
||||
for (size_t i = 0; i < records_.size(); ++i) {
|
||||
const auto& rec = records_[i];
|
||||
callback(rec.ypr);
|
||||
|
||||
if (i + 1 < records_.size()) {
|
||||
double dt = (records_[i + 1].time - rec.time) / speed_;
|
||||
if (dt > 0.0)
|
||||
std::this_thread::sleep_for(std::chrono::duration<double>(dt));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
void loadCSV() {
|
||||
std::ifstream file(csv_path_);
|
||||
if (!file.is_open()) {
|
||||
throw std::runtime_error("Failed to open CSV: " + csv_path_);
|
||||
}
|
||||
|
||||
std::string line;
|
||||
bool header_skipped = false;
|
||||
while (std::getline(file, line)) {
|
||||
if (!header_skipped) { // 跳过表头
|
||||
header_skipped = true;
|
||||
continue;
|
||||
}
|
||||
std::istringstream ss(line);
|
||||
std::string token;
|
||||
Record rec;
|
||||
|
||||
std::getline(ss, token, ',');
|
||||
rec.time = std::stod(token);
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
std::getline(ss, token, ',');
|
||||
rec.ypr[i] = std::stod(token);
|
||||
}
|
||||
|
||||
records_.push_back(rec);
|
||||
}
|
||||
std::cout << "[RotateReaderCSV] Loaded " << records_.size() << " records from " << csv_path_
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
private:
|
||||
std::string csv_path_;
|
||||
std::vector<Record> records_;
|
||||
double speed_ = 1.0; // 1.0=原速, 2.0=2倍速, 0.5=半速
|
||||
};
|
||||
} // namespace wust_vision
|
||||
536
wust_vision-main/tasks/utils/utils.cpp
Normal file
536
wust_vision-main/tasks/utils/utils.cpp
Normal file
@@ -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
|
||||
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