diff --git a/autoaim/autoaim.cpp b/autoaim/autoaim.cpp index 543e58f..6718cd6 100644 --- a/autoaim/autoaim.cpp +++ b/autoaim/autoaim.cpp @@ -4,6 +4,12 @@ using namespace cv; using namespace std; +#ifdef ASSIST_LABEL +int cnt = 0; +const string path_prefix = "/home/tup/Desktop/TUP-Vision-Infantry-2022/data/"; +ofstream file; +#endif //ASSIST_LABEL + //使用海伦公式计算三角形面积 inline float calcTriangleArea(cv::Point2f pts[3]) { @@ -28,8 +34,8 @@ Autoaim::Autoaim() coordsolver.loadParam(camera_param_path,"Coord"); lost_cnt = 0; is_last_target_exist = false; - // input_size = {640,384}; - input_size = {416,416}; + input_size = {640,384}; + // input_size = {416,416}; } Autoaim::~Autoaim() @@ -99,6 +105,10 @@ bool Autoaim::run(Image &src,VisionData &data) auto time_crop=std::chrono::steady_clock::now(); if (!detector.detect(input, objects)) { +#ifdef SHOW_AIM_CROSS + line(src.img, Point2f(src.img.size().width / 2, 0), Point2f(src.img.size().width / 2, src.img.size().height), Scalar(0,255,0), 1); + line(src.img, Point2f(0, src.img.size().height / 2), Point2f(src.img.size().width, src.img.size().height / 2), Scalar(0,255,0), 1); +#endif //SHOW_AIM_CROSS #ifdef SHOW_IMG imshow("dst",src.img); waitKey(1); @@ -107,9 +117,14 @@ bool Autoaim::run(Image &src,VisionData &data) is_last_target_exist = false; return false; } +#ifdef ASSIST_LABEL + auto img_name = path_prefix + to_string(cnt) + ".png"; + imwrite(img_name,input); +#endif //ASSIST_LABEL auto time_infer = std::chrono::steady_clock::now(); for (auto object : objects) { + #ifdef DETECT_RED if (object.color != 1) continue; @@ -118,6 +133,7 @@ bool Autoaim::run(Image &src,VisionData &data) if (object.color != 0) continue; #endif //DETECT_BLUE + Armor armor; armor.id = object.cls; armor.color = object.color; @@ -144,7 +160,7 @@ bool Autoaim::run(Image &src,VisionData &data) line(src.img, armor.apex2d[i % 4], armor.apex2d[(i + 1) % 4], Scalar(0,255,0), 1); #endif //SHOW_ALL_ARMOR - auto center = coordsolver.pnp(armor.apex2d, SOLVEPNP_IPPE); + auto center = coordsolver.pnp(armor.apex2d, SOLVEPNP_IPPE).coord; #ifdef USING_IMU #endif //USING_IMU @@ -159,12 +175,11 @@ bool Autoaim::run(Image &src,VisionData &data) for (auto iter = predictors_map.begin(); iter != predictors_map.end();) { //删除元素后迭代器会失效,需先行获取下一元素 - auto next = ++iter; - iter--; + auto next = iter; if (src.timestamp - (*iter).second.last_target.timestamp >= max_delta_t) - { - predictors_map.erase(iter); - } + next = predictors_map.erase(iter); + else + ++next; iter = next; } } @@ -172,7 +187,6 @@ bool Autoaim::run(Image &src,VisionData &data) std::list> predict_tasks; //为装甲板分配或新建最佳预测器 - // cout<<"walking"<(time_predict - time_start).count(); + #ifdef SHOW_FPS putText(src.img, "FPS:" + to_string(int(1000 / dr_full_ms)), Point2i(20,20), FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 255, 0), 2); #endif //SHOW_FPS @@ -312,7 +352,6 @@ bool Autoaim::run(Image &src,VisionData &data) #ifdef SHOW_IMG imshow("dst",src.img); waitKey(1); - #endif //SHOW_IMG #ifdef PRINT_LATENCY diff --git a/autoaim/autoaim.h b/autoaim/autoaim.h index b9297bf..ec07705 100644 --- a/autoaim/autoaim.h +++ b/autoaim/autoaim.h @@ -8,7 +8,7 @@ #include "predictor/predictor.h" #include "../serial/serialport.h" -const string network_path = "/home/tup/Desktop/TUP-Vision-Infantry-2022/model/yolox.xml"; +const string network_path = "/home/tup/Desktop/TUP-Vision-Infantry-2022/model/nano.xml"; const string camera_param_path = "/home/tup/Desktop/TUP-Vision-Infantry-2022/params/coord_param.yaml"; const string predict_param_path = "/home/tup/Desktop/TUP-Vision-Infantry-2022/params/filter/filter_param.yaml"; diff --git a/autoaim/debug.h b/autoaim/debug.h index 60f9297..eac0e6e 100644 --- a/autoaim/debug.h +++ b/autoaim/debug.h @@ -8,7 +8,7 @@ // #define PRINT_TARGET_INFO //是否输出目标信息 // #define SHOW_FPS //是否显示FPS #define SHOW_PREDICT //是否显示预测 -// #define SHOW_AIM_CROSS//是否绘制十字瞄准线 +#define SHOW_AIM_CROSS//是否绘制十字瞄准线 #endif // SHOW_IMG @@ -16,4 +16,6 @@ // #define PRINT_PRED_MAP_STATUS //输出目前预测器情况 #define USING_PREDICT //是否启用预测 //TODO:陀螺仪 -// #define USING_IMU //是否使用陀螺仪数据 \ No newline at end of file +// #define USING_IMU //是否使用陀螺仪数据 + +// #define ASSIST_LABEL //辅助数据集标注 \ No newline at end of file diff --git a/autoaim/detector/inference.cpp b/autoaim/detector/inference.cpp index e204683..d427d18 100644 --- a/autoaim/detector/inference.cpp +++ b/autoaim/detector/inference.cpp @@ -4,9 +4,6 @@ #include "inference.h" - - - /** * @brief Define names based depends on Unicode path support */ @@ -14,12 +11,12 @@ #define file_name_t std::string #define imread_t cv::imread #define NMS_THRESH 0.1 -#define BBOX_CONF_THRESH 0.8 +#define BBOX_CONF_THRESH 0.7 -// static constexpr int INPUT_W = 640; // Width of input -// static constexpr int INPUT_H = 384; // Height of input -static constexpr int INPUT_W = 416; // Width of input -static constexpr int INPUT_H = 416; // Height of input +static constexpr int INPUT_W = 640; // Width of input +static constexpr int INPUT_H = 384; // Height of input +// static constexpr int INPUT_W = 416; // Width of input +// static constexpr int INPUT_H = 416; // Height of input static constexpr int NUM_CLASSES = 8; // Number of classes static constexpr int NUM_COLORS = 3; // Number of color static constexpr int TOPK = 128; // TopK @@ -221,6 +218,7 @@ static void qsort_descent_inplace(std::vector& objects) qsort_descent_inplace(objects, 0, objects.size() - 1); } + static void nms_sorted_bboxes(const std::vector& faceobjects, std::vector& picked, float nms_threshold) { diff --git a/autoaim/predictor/predictor.cpp b/autoaim/predictor/predictor.cpp index 95a1156..0fd3536 100644 --- a/autoaim/predictor/predictor.cpp +++ b/autoaim/predictor/predictor.cpp @@ -36,7 +36,6 @@ bool Predictor::initParam(string coord_path) return true; } -//TODO:融合粒子滤波与拟合 Eigen::Vector3d Predictor::predict(Eigen::Vector3d xyz, int timestamp) { auto t1=std::chrono::steady_clock::now(); @@ -49,8 +48,21 @@ Eigen::Vector3d Predictor::predict(Eigen::Vector3d xyz, int timestamp) last_target = target; return xyz; } + + auto d_xyz = target.xyz - last_target.xyz; + auto delta_t = timestamp - last_target.timestamp; + auto last_dist = history_info.back().dist; + auto delta_time_estimate = ((last_dist / 100.f) / bullet_speed) * 1000 + delay; + auto time_estimate = delta_time_estimate + history_info.back().timestamp - history_info.front().timestamp; + //如速度过大,可认为为噪声干扰,进行滑窗滤波滤除 + if (d_xyz.norm() / (delta_t * 10.f) >= max_v) + { + auto filtered_xyz = shiftWindowFilter(history_info.size() - window_size - 2); + target = {filtered_xyz, (int)filtered_xyz.norm(), timestamp}; + } + //当队列长度不足时不使用拟合 - else if (history_info.size() < history_deque_len) + if (history_info.size() < history_deque_len) { history_info.push_back(target); fitting_disabled = true; @@ -68,24 +80,12 @@ Eigen::Vector3d Predictor::predict(Eigen::Vector3d xyz, int timestamp) history_info.push_back(target); fitting_disabled = false; } - - auto d_xyz = target.xyz - last_target.xyz; - auto delta_t = timestamp - last_target.timestamp; - auto last_dist = history_info.back().dist; - auto delta_time_estimate = ((last_dist / 100.f) / bullet_speed) * 1000 + delay; - auto time_estimate = delta_time_estimate + history_info.back().timestamp - history_info.front().timestamp; - //如速度过大,可认为为噪声干扰,进行滑窗滤波滤除 - if (d_xyz.norm() / (delta_t * 10.f) >= max_v && history_info.size() == history_deque_len) - { - auto filtered_xyz = shiftWindowFilter(); - target = {filtered_xyz, (int)filtered_xyz.norm(), timestamp}; - } - Eigen::Vector3d result = {0,0,0}; - Eigen::Vector3d result_pf = {0,0,0}; - Eigen::Vector3d result_fitting = {0,0,0}; + Eigen::Vector3d result = {0, 0, 0}; + Eigen::Vector3d result_pf = {0, 0, 0}; + Eigen::Vector3d result_fitting = {0, 0, 0}; PredictStatus is_pf_available; PredictStatus is_fitting_available; - //需注意粒子滤波使用相对时间(自上一次检测时所经过ms数),拟合使用绝对时间(自程序启动时所经过ms数) + //需注意粒子滤波使用相对时间(自上一次检测时所经过ms数),拟合使用自首帧所经过时间 if (fitting_disabled) { auto is_pf_available = predict_pf_run(target, result_pf, delta_time_estimate); @@ -99,7 +99,7 @@ Eigen::Vector3d Predictor::predict(Eigen::Vector3d xyz, int timestamp) is_fitting_available = get_fitting_available.get(); } - // 进行融合 + //进行融合 if (is_fitting_available.xyz_status[0] && !fitting_disabled) result[0] = result_fitting[0]; else @@ -151,23 +151,27 @@ Eigen::Vector3d Predictor::predict(Eigen::Vector3d xyz, int timestamp) } - inline Eigen::Vector3d Predictor::shiftWindowFilter() +inline Eigen::Vector3d Predictor::shiftWindowFilter(int start_idx=0) { - auto max_iter = history_deque_len - window_size + 1; - Eigen::Vector3d total_sum = {0,0,0}; - for(int i = 0; i < max_iter; i++) + //计算最大迭代次数 + auto max_iter = int(history_info.size() - start_idx) - window_size + 1; + Eigen::Vector3d total_sum = {0, 0, 0}; + // cout< history_info; //目标队列 const int max_timespan = 1000; //最大时间跨度,大于该时间重置预测器(ms) - const int max_cost = 1e1; //回归函数最大Cost + const int max_cost = 5e1; //回归函数最大Cost const int max_v = 15; //设置最大速度,单位m/s const int history_deque_len = 10; //队列长度 const int bullet_speed = 30; //TODO:弹速可变 const int delay = 50; //发弹延迟 - const int window_size = 5; //滑动窗口大小 + const int window_size = 3; //滑动窗口大小 - Eigen::Vector3d shiftWindowFilter(); }; struct CURVE_FITTING_COST diff --git a/coordsolver/coordsolver.cpp b/coordsolver/coordsolver.cpp index 712d60a..fff63cc 100644 --- a/coordsolver/coordsolver.cpp +++ b/coordsolver/coordsolver.cpp @@ -48,7 +48,7 @@ bool CoordSolver::loadParam(string coord_path,string param_name) return true; } -Eigen::Vector3d CoordSolver::pnp(Point2f apex[4], int method) +PnPInfo CoordSolver::pnp(Point2f apex[4], int method) { std::vector points_world; std::vector points_pic(apex,apex + 4); @@ -72,12 +72,25 @@ Eigen::Vector3d CoordSolver::pnp(Point2f apex[4], int method) {6.6,2.7,0}}; Mat rvec; + Mat rmat; Mat tvec; + Eigen::Matrix3d rmat_eigen; + Eigen::Vector3d coord_world = {0, 0, 0}; + Eigen::Vector3d tvec_eigen; + Eigen::Vector3d coord_camera; solvePnP(points_world, points_pic, intrinsic, dis_coeff, rvec, tvec, false, method); - Eigen::Vector3d result; - cv2eigen(tvec, result); + PnPInfo result; + //Pc = R * Pw + T + Rodrigues(rvec,rmat); + cv2eigen(rmat, rmat_eigen); + cv2eigen(tvec, tvec_eigen); + //转换至相机坐标系(左手坐标系) + result.coord = (rmat_eigen * coord_world) + tvec_eigen; + result.euler = rotationMatrixToEulerAngles(rmat_eigen) * 180 / CV_PI; + if (result.euler[0] <= 0) + result.euler[0] += 360; return result; } diff --git a/coordsolver/coordsolver.h b/coordsolver/coordsolver.h index f085bf8..2590dd6 100644 --- a/coordsolver/coordsolver.h +++ b/coordsolver/coordsolver.h @@ -15,6 +15,12 @@ using namespace std; using namespace cv; +struct PnPInfo +{ + Eigen::Vector3d coord; + Eigen::Vector3d euler; +}; + class CoordSolver { public: @@ -22,7 +28,7 @@ class CoordSolver ~CoordSolver(); bool loadParam(string coord_path,string param_name); - Eigen::Vector3d pnp(Point2f apex[4],int id); + PnPInfo pnp(Point2f apex[4], int method); Eigen::Vector3d staticCoordOffset(Eigen::Vector3d &xyz); Eigen::Vector3d camToWorld(Eigen::Vector3d &point_camera, Eigen::Matrix3d &rmat); Eigen::Vector3d worldToCam(Eigen::Vector3d &point_world, Eigen::Matrix3d &rmat); diff --git a/general/general.cpp b/general/general.cpp index 5efb295..ee2574d 100644 --- a/general/general.cpp +++ b/general/general.cpp @@ -124,3 +124,38 @@ string getParent(string path) return path.substr(0,last_slash_idx - 1); } + + +bool isRotationMatirx(Eigen::Matrix3d R) +{ + double err=1e-6; + Eigen::Matrix3d shouldIdenity; + shouldIdenity=R*R.transpose(); + Eigen::Matrix3d I=Eigen::Matrix3d::Identity(); + return (shouldIdenity - I).norm() < err; +} + +/** + * @brief 将旋转矩阵转化为欧拉角 + * @param R 旋转矩阵 + * @return 欧拉角 +*/ +Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R) +{ + double sy = sqrt(R(0,0) * R(0,0) + R(1,0) * R(1,0)); + bool singular = sy < 1e-6; + double x, y, z; + if (!singular) + { + x = atan2( R(2,1), R(2,2)); + y = atan2(-R(2,0), sy); + z = atan2( R(1,0), R(0,0)); + } + else + { + x = atan2(-R(1,2), R(1,1)); + y = atan2(-R(2,0), sy); + z = 0; + } + return {z, y, x}; +} \ No newline at end of file diff --git a/general/general.h b/general/general.h index b7fce71..8ecc27d 100644 --- a/general/general.h +++ b/general/general.h @@ -39,3 +39,5 @@ string getParent(string path); std::vector readLines(string file_path); std::vector generatePathTree(string path); + +Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R); diff --git a/params/coord_param.yaml b/params/coord_param.yaml index ff640f9..c07ac2c 100644 --- a/params/coord_param.yaml +++ b/params/coord_param.yaml @@ -6,21 +6,21 @@ Coord: Intrinsic: [1.451e+03, 0.000000000000, 645.373, 0.000000000000, 1.4563e+03, 491.0741, 0.000000000000, 0.000000000000, 1.000000000000] - Coeff: [-0.2146, 0.3219] + Coeff: [-0.2146, 0.32190, 0, 0, 0] # xyz_offset: [0,5,0] #坐标偏移量(相机至枪管),单位cm xyz_offset: [0,0,0] #坐标偏移量(相机至枪管),单位cm # angle_offset: [-11,3] #角度偏移量,单位度 angle_offset: [0,0] #角度偏移量,单位度 -#陀螺仪至相机的转化矩阵(旋转矩阵 + 平移向量) P_c = T_ic * P_i +#陀螺仪至相机的旋转矩阵 R_c = T_ic * R_i T_ic: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 1] -#相机至陀螺仪的转化矩阵(旋转矩阵 + 平移向量) P_i = T_ci * P_c + 0, 0, 0, 1] +#相机至陀螺仪的旋转矩阵 R_i = T_ci * R_c T_ci: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 1] + 0, 0, 0, 1] diff --git a/params/filter/filter_param.yaml b/params/filter/filter_param.yaml index 8fb60fb..f5b178f 100644 --- a/params/filter/filter_param.yaml +++ b/params/filter/filter_param.yaml @@ -1,16 +1,16 @@ autoaim_x: vector_len: 1 - num_particle: 500 - process_noise: [2] + num_particle: 700 + process_noise: [1] autoaim_y: vector_len: 1 - num_particle: 500 - process_noise: [2] + num_particle: 700 + process_noise: [1] autoaim_z: vector_len: 1 - num_particle: 2000 - process_noise: [2] + num_particle: 2500 + process_noise: [1] diff --git a/thread/thread.cpp b/thread/thread.cpp index 13125a4..dc6b45d 100644 --- a/thread/thread.cpp +++ b/thread/thread.cpp @@ -15,10 +15,10 @@ bool producer(Factory &factory) // 开始采集帧 DaHeng.SetStreamOn(); // 设置曝光事件 - DaHeng.SetExposureTime(10000); + DaHeng.SetExposureTime(4000); // 设置 DaHeng.SetGAIN(3, 16); - // 是否启用自动曝光 + // 是否启用自动白平衡 // DaHeng.Set_BALANCE_AUTO(1); while(1) { @@ -32,7 +32,8 @@ bool producer(Factory &factory) src.img = img; src.timestamp = (int)(std::chrono::duration(time_cap - time_start).count()); - + //用于辅助标注 + // DaHeng.SetExposureTime(1000 + src.timestamp % 100 * 30); factory.produce(src); } return true;