提交加入8个顶点的经纬高逻辑

TrackingRos.cpp

@@ -84,7 +84,7 @@ void my_compute_box_3d(Eigen::Vector3d center, double heading, Eigen::Vector3d s
 }
 
 #ifdef _ABUZHABI_
-void CalculateUTMPos(jfx_common_msgs::det_tracking &obj, std::vector<double> &wgs84_station, std::vector<double> &UTM_station, std::vector<std::vector<double>> &trans2station, std::vector<std::vector<double>> &kitti2origin, double &lon, double &lat)
+void CalculateUTMPos(jfx_common_msgs::det_tracking &obj, std::vector<double> &wgs84_station, std::vector<double> &UTM_station, std::vector<std::vector<double>> &trans2station, std::vector<std::vector<double>> &kitti2origin, double &lon, double &lat, double (&loc_lon)[8],double (&loc_lat)[8],double (&loc_alt)[8])
 {
     Eigen::Vector3d center(obj.x, obj.y, obj.z);
     double heading = obj.rot_y;
@@ -98,17 +98,17 @@ void CalculateUTMPos(jfx_common_msgs::det_tracking &obj, std::vector<double> &wg
     Eigen::Vector3d res3(res_corners_3d(3, 0), res_corners_3d(3, 1), res_corners_3d(3, 2));
     Eigen::Vector3d res6(res_corners_3d(6, 0), res_corners_3d(6, 1), res_corners_3d(6, 2));
 
-    Eigen::Vector3d head_pnt = (res3 + res2) / 2;
-    Eigen::Vector3d tail_pnt = (res0 + res3) / 2;
+    // Eigen::Vector3d head_pnt = (res3 + res2) / 2;
+    // Eigen::Vector3d tail_pnt = (res0 + res3) / 2;
     Eigen::Vector3d center_pnt = (res0 + res6) / 2;
 
-    double inter_len = 5.0f;
-    double total_len = 6.0f;
-    Eigen::Vector3d lidar_loc = (tail_pnt - head_pnt) * inter_len / (1.0 * total_len) + head_pnt;
+    // double inter_len = 5.0f;
+    // double total_len = 6.0f;
+    // Eigen::Vector3d lidar_loc = (tail_pnt - head_pnt) * inter_len / (1.0 * total_len) + head_pnt;
 
     Eigen::Vector4d tranTmp;
-    //tranTmp << center_pnt[0], center_pnt[1], center_pnt[2], 1;
-    tranTmp << lidar_loc[0], lidar_loc[1], lidar_loc[2], 1;
+    tranTmp << center_pnt[0], center_pnt[1], center_pnt[2], 1;
+    // tranTmp << lidar_loc[0], lidar_loc[1], lidar_loc[2], 1;
 
     Eigen::Matrix4d Trans2station = Eigen::Matrix4d::Zero();
     for (int i = 0; i < 4; i++)
@@ -135,6 +135,22 @@ void CalculateUTMPos(jfx_common_msgs::det_tracking &obj, std::vector<double> &wg
     }
     UVUTMLLAPtr->UTM2WGS84(UTM_pos,lonlatH_pos);
 
+    for(int i = 0; i < 8; i++)
+    {
+        Eigen::Vector4d tmp;
+        tmp << res_corners_3d(i, 0), res_corners_3d(i, 1), res_corners_3d(i, 2), 1;
+        auto loc_pos = Trans2station * tmp;
+        std::vector<double> UTM_tmp = { loc_pos(0) + UTM_station[0],
+                                        loc_pos(1) + UTM_station[1],
+                                        loc_pos(2) + UTM_station[2] };
+
+        std::vector<double> lonlatalt_pos = {0,0,UTM_tmp[2]};
+        UVUTMLLAPtr->UTM2WGS84(UTM_tmp,lonlatalt_pos);
+        loc_lon[i] = lonlatalt_pos[0];
+        loc_lat[i] = lonlatalt_pos[1];
+        loc_alt[i] = lonlatalt_pos[2];
+    }
+
     lon = lonlatH_pos[0];
     lat = lonlatH_pos[1];
 
@@ -560,13 +576,24 @@ void TrackingRos::ThreadTrackingProcess()
 #ifdef _ABUZHABI_
                         double lon = 0.0f;
                         double lat = 0.0f;
+                        double loc_lon[8] = {};
+                        double loc_lat[8] = {};
+                        double loc_alt[8] = {};
                         // CalculateGuassPos(obj, m_trans, m_kitti2origin, gx, gy);
-                        CalculateUTMPos(obj, m_wgs84_station,m_UTM_station, m_trans2station,m_kitti2origin, lon, lat);
+                        CalculateUTMPos(obj, m_wgs84_station,m_UTM_station, m_trans2station,m_kitti2origin, lon, lat,loc_lon,loc_lat,loc_alt);
                         // SDK_LOG(SDK_INFO, "CalculateGuassPos x = %f,y = %f, gx = %f, gy = %f,prob = %f", obj.x, obj.y, gx, gy, iter.second->GetProb());
                         // jfx::Array gpos = {gx, gy};
                         // jfx::Array pos = jfx::Inverse(gpos);
                         obj.Lat = lat;
                         obj.Long = lon;
+                        for(int i = 0; i < 8; i++)
+                        {
+                            jfx_common_msgs::Point64 loc = {};
+                            loc.lon = loc_lon[i];
+                            loc.lat = loc_lat[i];
+                            loc.alt = loc_alt[i];
+                            obj.points.emplace_back(loc);
+                        }
 #else
                         double gx = 0.0f;
                         double gy = 0.0f;
@@ -630,13 +657,24 @@ void TrackingRos::ThreadTrackingProcess()
 #ifdef _ABUZHABI_
 						double lon = 0.0f;
                         double lat = 0.0f;
+                        double loc_lon[8] = {};
+                        double loc_lat[8] = {};
+                        double loc_alt[8] = {};                        
                         // CalculateGuassPos(obj, m_trans, m_kitti2origin, gx, gy);
-                        CalculateUTMPos(obj, m_wgs84_station,m_UTM_station, m_trans2station,m_kitti2origin, lon, lat);
+                        CalculateUTMPos(obj, m_wgs84_station,m_UTM_station, m_trans2station,m_kitti2origin, lon, lat,loc_lon,loc_lat,loc_alt);
                         // SDK_LOG(SDK_INFO, "CalculateGuassPos x = %f,y = %f, gx = %f, gy = %f,prob = %f", obj.x, obj.y, gx, gy, iter.second->GetProb());
                         // jfx::Array gpos = {gx, gy};
                         // jfx::Array pos = jfx::Inverse(gpos);
                         obj.Lat = lat;
                         obj.Long = lon;
+                        for(int i = 0; i < 8; i++)
+                        {
+                            jfx_common_msgs::Point64 loc = {};
+                            loc.lon = loc_lon[i];
+                            loc.lat = loc_lat[i];
+                            loc.alt = loc_alt[i];
+                            obj.points.emplace_back(loc);
+                        }                        
 #else
                         double gx = 0.0f;
                         double gy = 0.0f;