修改绝对坐标跟踪逻辑修改

TrackEx/Rel2AbsLoc.cpp

+// Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+// Copyright ©2022 北京觉非科技有限公司 版权所有 保留一切权利
+
+// This software is furnished under a license and may be used and copied only in accordance with the
+// terms of such license and with the inclusion of the above copyright notice. This software or any
+// other copies thereof may not be provided or otherwise made available to any other person.
+
+// The information in this software is subject to change without notice and should not be
+// constructed as a commitment by Beijing JUEFX Technology Co.Ltd. JUEFX assumes no responsibility
+// for the use or reliability of its software on equipment which is not supported by JUEFX.
+
+/**
+ * @file file_name.cpp
+ * @author Jfx
+ * @brief Jfx cxx template file
+ * @version 0.1
+ * @date 2022-10-24
+ * @copyright Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+ **/
+
+
+#include "Rel2AbsLoc.hpp"
+
+
+
+/// @details 函数FunctionTemplate的详细注释
+/// 继续写详细注释
+/// 这样可以写很多行，都是详细注释
+int Rel2AbsLoc::Init(float offset_x, float offset_y)
+{
+    m_trans.set_offsets(offset_x,offset_y);
+    // 2. 激光雷达（跟踪）转到utm上相对于imu（定位原点）的偏移量
+    // 2.1. 激光雷达（跟踪）坐标转cpt外参
+    m_lidar_imu_extrinsic = Eigen::Isometry3d::Identity();
+    m_lidar_imu_extrinsic.matrix() << -0.9998077, -0.0196081, 0.0003459,0.0196496,
+       0.0196087, -0.9998060, 0.0018946,-0.0027971,
+       0.0003087, 0.0019010, 0.9999981,0.2077100,
+       0.0       ,  0.0    , 0.0      ,   1.0;
+    // m_lidar_imu_extrinsic.matrix() << 0.0060680, -0.9998656, -0.0152272,  -0.0148060,
+    //     0.9959924, 0.0046843,  0.0893156,   1.8629732,
+    //     -0.0892323, -0.0157082, 0.9958870,  0.6100000,
+    //     0.0       ,  0.0    , 0.0      ,   1.0;	//激光雷达到cpt的4*4的外参
+      
+    return 0;
+}
+
+int Rel2AbsLoc::SetLocalization(double lon, double lat, double alt,double roll, double pitch, double yaw)
+{
+    double yaw_arc = yaw /180*M_PI;
+    double roll_arc = roll/180*M_PI;
+    double pitch_arc = pitch/180*M_PI;
+    //1. 定位数据（经纬度）转带站心的utm
+    // double northing,  easting ;	//带站心的utm坐标（作为输出）
+    // std::string zone;		//（作为输出）
+    m_trans.LatLon2Map(lat, lon, m_northing, m_easting, m_zone);
+    m_alt = alt;
+
+    // 2.2 航向角修正
+    double lat_ = lat;	//定位提供的纬度
+    double lon_0 = lon;	//定位提供的经度
+    double lon_1 = lon + 0.001;
+    double northing_0, northing_1, easting_0, easting_1;
+    m_trans.LatLon2Map(lat_, lon_0, northing_0, easting_0, m_zone);
+    m_trans.LatLon2Map(lat_, lon_1, northing_1, easting_1, m_zone);
+    double bias_angle_z = atan2(northing_1 - northing_0, easting_1 - easting_0);
+    m_yaw_bias = yaw_arc + bias_angle_z; 		//由于utm地图的变形，修正定位提供的航向角
+
+    // 2.3 cpt转enu外参
+    m_imu_enu_extrinsic = Eigen::Isometry3d::Identity();
+    m_imu_enu_extrinsic.linear() = (Eigen::AngleAxisd(-m_yaw_bias, Eigen::Vector3d::UnitZ()) * //  pitch roll由定位提供
+                                                Eigen::AngleAxisd(-pitch_arc, Eigen::Vector3d::UnitY()) *
+                                                Eigen::AngleAxisd(roll_arc, Eigen::Vector3d::UnitX())).toRotationMatrix();
+    return 0;
+}
+
+int Rel2AbsLoc::Transform(float x,float y, float z, float rot_y,float& utmX,float& utmY,float& utmZ,float& utm_yaw,double& lon, double& lat)
+{
+    // 2.4 激光雷达（跟踪）转到utm上相对于imu（定位原点）的偏移量
+    Eigen::Vector4d pos_lidar(x, y, z, 1); 	//xyz为跟踪系下坐标
+    Eigen::Vector4d pos_ENU = m_imu_enu_extrinsic * m_lidar_imu_extrinsic * pos_lidar;
+    double x_ENU = pos_ENU(0);
+    double y_ENU = pos_ENU(1);
+    double z_ENU = pos_ENU(2);
+
+    // 3. 偏移量和定位的坐标想加
+    utmX = m_easting + x_ENU;
+    utmY = m_northing + y_ENU;
+    utmZ = m_alt + z_ENU;  		//alt为定位给的高度
+    utm_yaw = m_yaw_bias + rot_y;
+
+    m_trans.Map2LatLon(utmY,utmX,m_zone,lat,lon);
+    
+    return 0;
+}
+
+

TrackEx/Rel2AbsLoc.hpp

+// Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+// Copyright ©2022 北京觉非科技有限公司 版权所有 保留一切权利
+
+// This software is furnished under a license and may be used and copied only in accordance with the
+// terms of such license and with the inclusion of the above copyright notice. This software or any
+// other copies thereof may not be provided or otherwise made available to any other person.
+
+// The information in this software is subject to change without notice and should not be
+// constructed as a commitment by Beijing JUEFX Technology Co.Ltd. JUEFX assumes no responsibility
+// for the use or reliability of its software on equipment which is not supported by JUEFX.
+
+/**
+ * @file file_name.hpp
+ * @author Jfx
+ * @brief Jfx cxx template file
+ * @version 0.1
+ * @date 2022-10-24
+ * @copyright Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+ **/
+
+#ifndef _REL_TO_ABS_LOC_HPP_
+#define _REL_TO_ABS_LOC_HPP_
+
+#include "conversions.h"
+#include <eigen3/Eigen/Dense>
+
+
+/// 类ClassName的注释
+///
+/// 类ClassName的详细注释（如果没有详细注释可以连同上一行去除）
+class Rel2AbsLoc 
+{
+public:
+
+	/// 构造函数ClassName
+	Rel2AbsLoc() = default;
+	/// 析构函数~ClassName()
+	~Rel2AbsLoc() = default;
+
+
+	int Init(float offset_x, float offset_y);
+
+    int SetLocalization(double lon, double lat, double alt,double roll, double pitch, double yaw);
+
+    int Transform(float x,float y, float z, float rot_y,float& utmX,float& utmY,float& utmZ,float& utm_yaw,double& lon, double& lat);
+
+    gps_common::offset_UTM_trans m_trans;
+    Eigen::Isometry3d m_lidar_imu_extrinsic;
+    Eigen::Isometry3d m_imu_enu_extrinsic;
+
+    double m_northing;
+    double m_easting ;	//带站心的utm坐标（作为输出）
+    std::string m_zone;
+    double m_alt;
+    double m_yaw_bias; //修正的朝向
+};
+
+#endif  // MODULE_NAME__HEADER_FILE_NAME_HPP_

TrackingRosAbs.h

 #pragma once
 
 #include <vector>
+#include <mutex>
 #include "BaseTracker.h"
 #include "BaseTrackerEx.h"
+#include "Rel2AbsLoc.hpp"
 #include "Track3D.h"
 #include "TrackEx.hpp"
 #ifdef _QICHECHENG_
@@ -10,6 +12,7 @@
 #define jfx_common_msgs jfxrosperceiver
 #else
 #include "jfx_common_msgs/det_tracking_array.h"
+#include "jfx_common_msgs/localization.h"
 #endif
 #include "SafeQueue.hpp"
 //#include "TrackingMsg.h"
@@ -18,6 +21,7 @@
 #include <yaml-cpp/yaml.h>
 
 using objTrackListPtr = std::shared_ptr<jfx_common_msgs::det_tracking_array>;
+using LocalizationPtr = std::shared_ptr<jfx_common_msgs::localization>;
 
 
 class TrackingRos
@@ -30,28 +34,35 @@ public:
     ~TrackingRos();
 
     void TrackingCallBackFunc(const jfx_common_msgs::det_tracking_arrayConstPtr& msg);//接受ros发过来的消息
+    void LocalizationCallBackFunc(const jfx_common_msgs::localizationConstPtr& msg);//接受定位的消息
 
     void Init(ros::NodeHandle& nh);
 
-    void ThreadTrackingProcess();
+    void ThreadAbsLocProcess();
     void ThreadTrackingProcessEx();
 
+    int GetAbsLoc(uint64 timestamp,LocalizationPtr& locPtr);
+
 
 public:
 
     SafeQueue<objTrackListPtr> m_objsQueue;
+    SafeQueue<objTrackListPtr> m_fusionRes;
 
     BaseTracker<Track3D> m_tracker;
     BaseTrackerEx<TrackEx> m_trackerEx;
 
     bool m_isTrackingRun = false;//记录是否在运行
     std::thread m_trackingThread;//运行事件线程
+    bool m_isAbsLocRun = false;//记录是否在运行
+    std::thread m_absLocThread;//运行事件线程    
     ros::Publisher m_pub;//发送所有物体信息的publisher
     ros::Publisher m_pubCloud;//发送点云数据
     ros::Publisher m_pubBoundingBoxes;//发送3D框信息
     ros::Publisher m_pubMarkerArray;//发送marker框信息
     ros::Publisher m_pubMarkerArrow;//发送marker框信息
     ros::Subscriber m_subFusionRes;
+    ros::Subscriber m_subLocalization;//订阅定位信息
 #ifdef _SEND_ABUZHABI_
     ros::Publisher m_pubAbuzhabi;//阿布扎比发送的数据
 #endif
@@ -76,4 +87,8 @@ public:
     double m_cross_center_lon = 0;
     double m_cross_center_lat = 0;
     double m_clear_distance = 0;
+
+    std::vector<LocalizationPtr> m_absLocList;//记录定位信息
+    std::mutex m_absLocMtx;
+    Rel2AbsLoc m_rel2Abs;
 };