#include "config.hpp" #include "transform_uv.h" #include "coordinate_convert.h" #include "vision_config.h" using namespace jfx_vision; std::unique_ptr<VisionConfig> jfx_vision::g_visionConfig; void initTransUV(VisionConfig& cvConfig, jfx::TransformUV& uv, int cameraIndex) { std::vector<double> f_c_x_y; std::vector<float> transform_M; std::vector<double> dist; f_c_x_y.emplace_back(cvConfig.cameraParamConfigs_map.at(cameraIndex).coordinate.intrinsics.fx); f_c_x_y.emplace_back(cvConfig.cameraParamConfigs_map.at(cameraIndex).coordinate.intrinsics.cx); f_c_x_y.emplace_back(cvConfig.cameraParamConfigs_map.at(cameraIndex).coordinate.intrinsics.fy); f_c_x_y.emplace_back(cvConfig.cameraParamConfigs_map.at(cameraIndex).coordinate.intrinsics.cy); transform_M = cvConfig.cameraParamConfigs_map.at(cameraIndex).coordinate.transform_M; dist = cvConfig.cameraParamConfigs_map.at(cameraIndex).coordinate.intrinsics.dist; printf("fx,cx,fy,cy:%.12f,%.12f,%.12f,%.12f \n",f_c_x_y[0],f_c_x_y[1],f_c_x_y[2],f_c_x_y[3] ); for(int i=0; i<dist.size(); i++) { printf("%.12f,",dist[i]); } uv.Init(f_c_x_y, transform_M, dist); } void initCoordCVT(VisionConfig& cvConfig, CoordinateConvert& cvt, int cameraIndex) { auto coord = cvConfig.cameraParamConfigs_map.at(cameraIndex).coordinate; printf("\nhorn_r,rx,ry,horn_v,vx,vy %.12f, %.8f,%.8f,%.12f,%.8f,%.8f\n",coord.horn_r, coord.rx, coord.ry, coord.horn_v, coord.vx, coord.vy); cvt.Init(coord.horn_r, coord.rx, coord.ry, coord.horn_v, coord.vx, coord.vy); } int main() { std::string config_yaml = "/data/catkin_ws_xishan/src/rosrv2x/jfx_rvconfig/param/vision_config.yaml"; g_visionConfig.reset(new VisionConfig(config_yaml)); // b_center=1566 380 rect=1515 298 1617 380 result=2.76558304,106.13797760 juefx_lat=31.58605464 juefx_lon=120.41351219, crossname=50272, taskid=xianfengchunfengn, Hit_Streak=9 Coast_Cycles=0 // cam:idx2 :center=1566 380 Trans X=2.80015530 Y=106.79021222 Lat=31.58604875,Lon=120.41351181 jfx::TransformUV transUV; CoordinateConvert coordinatCvt; //for(int i=0;i<4;i++) { initTransUV(*g_visionConfig, transUV, 2); initCoordCVT(*g_visionConfig, coordinatCvt, 2); vector<float> transf_M = g_visionConfig->cameraParamConfigs_map.at(2).coordinate.transform_M; for(int i=0; i< transf_M.size();i++) { printf("%.12f,", transf_M[i]); } vector<float> r; int lx = 1515; int ly = 298; int rx = 1617; int ry = 380; int bcenter_x = (lx + rx)/ 2; int bcenter_y = ry; int ret = transUV.Process(bcenter_x, bcenter_y, r); if (ret == 0) { float m_NowX = r[0]; float m_NowY = r[1]; jfx::Array result84; jfx::Array objPoint = { r[0], r[1] }; coordinatCvt.Dev2WGS84(objPoint, result84, false); double m_Lat = result84[0]; double m_Lon = result84[1]; printf("\ncenter=%d %d Trans X=%.8f Y=%.8f Lat=%.8f,Lon=%.8f\n", bcenter_x, bcenter_y, m_NowX, m_NowY, m_Lat, m_Lon); } //} }