提交修正rot_y

TrackingRos.cpp

@@ -7,16 +7,27 @@
 #include "matrix.h"
 #include <eigen3/Eigen/Dense>
 
-void to360(float& rot_y)
+#define _PI_ 3.1415926
+
+void to360(float& rot_y,float lidar_x_angle)
 {
-    if (rot_y > 0)
+    while (rot_y >= _PI_)
+    {
+        rot_y -= _PI_ * 2;
+    }
+    while (rot_y < -_PI_)
     {
-        rot_y = 2 * 3.1415926 - rot_y;
+        rot_y += _PI_ * 2;
     }
-    else
+    float tmp_angle = 0.0f;
+    float car_angle = rot_y / _PI_ * 180.0f;
+    tmp_angle = lidar_x_angle - car_angle;
+    tmp_angle += 360;
+    while (tmp_angle > 360)
     {
-        rot_y = -rot_y;
+        tmp_angle -= 360;
     }
+    rot_y = tmp_angle;
 }
 
 void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>& trans, float& gx, float& gy)
@@ -59,6 +70,8 @@ void TrackingRos::Init(ros::NodeHandle& nh)
     SDK_LOG(SDK_INFO, "kf_gpu = %d",gpu);
     m_tracker.SetGPU(gpu);
 
+    nh.param<float>("lidar_x_angle", m_lidar_x_angle, 354.102412638);
+
     YAML::Node config = YAML::LoadFile(file.c_str());
 
 
@@ -258,6 +271,7 @@ void TrackingRos::ThreadTrackingProcess()
                     }
                 }
                 //修正rot_y值
+                to360(obj.rot_y, m_lidar_x_angle);
 
                 jsk_recognition_msgs::BoundingBox box = {};
                 box.label = obj.obj_id;

TrackingRos.h

@@ -44,4 +44,6 @@ public:
     ros::Publisher m_pubBoundingBoxes;//发送3D框信息
 
     std::vector<std::vector<float>> m_trans;
+
+    float m_lidar_x_angle = 0.0f;//修改rot_y的参数，从配置里读取
 };