提价代码

TrackingRos.cpp

@@ -4,6 +4,22 @@
 #include "LogBase.h"
 #include <yaml-cpp/yaml.h>
 #include "matrix.h"
+#include <eigen3/Eigen/Dense>
+
+void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>& trans, float& gx, float& gy)
+{
+    Eigen::Vector4f svTmp;
+    svTmp << x, y, z, 1;
+    Eigen::Matrix4f Trans = Eigen::Matrix4f::Zero();
+    for (int i = 0; i < 3; i++) {
+        for (int j = 0; j < 3; j++) {
+            Trans(i, j) = trans[i][j];
+        }
+    }
+    auto world_corners_3d = Trans * svTmp;
+    gx = world_corners_3d[0];
+    gy = world_corners_3d[1];
+}
 
 TrackingRos::~TrackingRos()
 {
@@ -22,9 +38,9 @@ void TrackingRos::Init(ros::NodeHandle& nh)
 
 
     auto cfg = config["TRACKING"];
-    auto value = cfg["TRANS"].as<std::vector<std::vector<float>>>();
+    m_trans = cfg["TRANS"].as<std::vector<std::vector<float>>>();
 
-    m_coordinate.Init(value[0][0], value[0][1], value[0][2], value[0][3], value[1][0], value[1][1]);
+    //m_coordinate.Init(value[0][0], value[0][1], value[0][2], value[0][3], value[1][0], value[1][1]);
 
     m_pub = nh.advertise<jfx_common_msgs::det_tracking_array>("/tracking_res", 100);
     m_pubCloud = nh.advertise<sensor_msgs::PointCloud2>("/tracking_cloud", 100);
@@ -125,11 +141,14 @@ void TrackingRos::ThreadTrackingProcess()
                         obj.v_x = data[7];
                         obj.v_y = data[8];
                         obj.v_z = data[9];
-                        jfx::Array pos = { obj.x,obj.y };
-                        jfx::Array out;
-                        m_coordinate.Dev2WGS84(pos, out, false);
-                        obj.Lat = out[0];
-                        obj.Long = out[1];
+                        float gx = 0.0f;
+                        float gy = 0.0f;
+                        CalculateGuassPos(obj.x, obj.y, obj.z, m_trans, gx, gy);
+                        jfx::Array gpos = {gx,gy};
+                        jfx::Array pos = jfx::Inverse(gpos);
+                        obj.Lat = pos[0];
+                        obj.Long = pos[1];
+                        SDK_LOG(SDK_INFO, "lat = %.10f, lon = %.10f",obj.Lat,obj.Long);
                     }
                     iter.second->m_obj = std::make_shared<jfx_common_msgs::det_tracking>(obj);
                 }

TrackingRos.h

@@ -37,5 +37,5 @@ public:
     ros::Publisher m_pub;//发送所有物体信息的publisher
     ros::Publisher m_pubCloud;//发送点云数据
 
-    CoordinateConvert m_coordinate;
+    std::vector<std::vector<float>> m_trans;
 };