提交吉利研究院相对坐标的显示

TrackEx/Rel2AbsLoc.cpp

@@ -41,6 +41,11 @@ int Rel2AbsLoc::Init(float offset_x, float offset_y)
     //     -0.0892323, -0.0157082, 0.9958870,  0.6100000,
     //     0.0       ,  0.0    , 0.0      ,   1.0;	//激光雷达到cpt的4*4的外参
       
+    m_matrix_ni = Eigen::Matrix4d::Identity();
+    m_matrix_ni.matrix() << -9.36218528e-05, -9.99968230e-01,  7.98499399e-03, -1.34839479e-02,
+        9.99726853e-01,  9.36218528e-05,  2.33679968e-02, -3.94606248e-02,
+        -2.33679968e-02,  7.98499399e-03,  9.99695083e-01, -1.68814610e+00,
+        0.00000000e+00,  0.00000000e+00,  0.00000000e+00,  1.00000000e+00;      
     return 0;
 }
 
@@ -92,5 +97,26 @@ int Rel2AbsLoc::Transform(float x,float y, float z, float rot_y,float& utmX,floa
 
     return 0;
 }
+int Rel2AbsLoc::transform_verse(float x,float y, float z, float rot_y,float& utmX,float& utmY,float& utmZ,float& utm_yaw,double& lon, double& lat)
+{
+    Eigen::Vector4d pos(x, y, z, 1); 	//xyz为跟踪系下坐标
+    Eigen::Vector4d pos_ni = m_matrix_ni*  pos;
+    return Transform(pos_ni(0),pos_ni(1),pos_ni(2),rot_y,utmX,utmY,utmZ,utm_yaw,lon,lat);   
+}
 
+int Rel2AbsLoc::transform_inverse(float utmX,float utmY, float utmZ, float utm_yaw, float& x,float& y, float& z, float& rot_y)
+{
+    double x_ENU = utmX - m_easting;
+    double y_ENU = utmY - m_northing;
+    double z_ENU = utmZ - m_alt;
+    rot_y = utm_yaw - m_yaw_bias;
+
+    Eigen::Vector4d pos_ENU(x_ENU, y_ENU, z_ENU, 1);
+    Eigen::Vector4d pos_ni = (m_imu_enu_extrinsic * m_lidar_imu_extrinsic).inverse() * pos_ENU;
+    Eigen::Vector4d pos_lidar = m_matrix_ni.inverse() * pos_ni; 
+
+    x = pos_lidar(0);
+    y = pos_lidar(1);
+    z = pos_lidar(2);
+}
 

TrackEx/Rel2AbsLoc.hpp

@@ -44,6 +44,9 @@ public:
 
     int Transform(float x,float y, float z, float rot_y,float& utmX,float& utmY,float& utmZ,float& utm_yaw,double& lon, double& lat);
 
+    int transform_verse(float x,float y, float z, float rot_y,float& utmX,float& utmY,float& utmZ,float& utm_yaw,double& lon, double& lat);
+    int transform_inverse(float utmX,float utmY, float utmZ, float utm_yaw, float& x,float& y, float& z, float& rot_y);
+
     gps_common::offset_UTM_trans m_trans;
     Eigen::Isometry3d m_lidar_imu_extrinsic;
     Eigen::Isometry3d m_imu_enu_extrinsic;
@@ -53,6 +56,8 @@ public:
     std::string m_zone;
     double m_alt;
     double m_yaw_bias; //修正的朝向
+
+    Eigen::Matrix4d m_matrix_ni;
 };
 
 #endif  // MODULE_NAME__HEADER_FILE_NAME_HPP_

TrackingRosAbs.h

@@ -23,6 +23,12 @@
 using objTrackListPtr = std::shared_ptr<jfx_common_msgs::det_tracking_array>;
 using LocalizationPtr = std::shared_ptr<jfx_common_msgs::localization>;
 
+struct TrackLocation
+{
+    objTrackListPtr objs;
+    LocalizationPtr loc;
+};
+using TrackLocationPtr = std::shared_ptr<TrackLocation>;
 
 class TrackingRos
 {
@@ -46,7 +52,7 @@ public:
 
 public:
 
-    SafeQueue<objTrackListPtr> m_objsQueue;
+    SafeQueue<TrackLocationPtr> m_objsQueue;
     SafeQueue<objTrackListPtr> m_fusionRes;
 
     BaseTracker<Track3D> m_tracker;
@@ -91,4 +97,5 @@ public:
     std::vector<LocalizationPtr> m_absLocList;//记录定位信息
     std::mutex m_absLocMtx;
     Rel2AbsLoc m_rel2Abs;
+    Rel2AbsLoc m_abs2Rel;
 };