提交修正角度代码

BaseTracker/BaseTrack.h

@@ -5,6 +5,9 @@
 #include <vector>
 #include <memory>
 
+#define _PI_ 3.1415926
+
+
 class BaseTrack 
 {
 public:

BaseTracker/Track3D.cpp

@@ -217,5 +217,79 @@ int Track3D::GetKFDataOrder(std::vector<int>& order)
 }
 void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out)
 {
+    if (kf_ == nullptr)
+        return;
+    float x_angle = kf_->x_[3];
+    float angle = data[3];
+    float deta = angle - x_angle;
+    while (deta >= _PI_)
+    {
+        deta -= _PI_ * 2;
+    }
+    while (deta < -_PI_)
+    {
+        deta += _PI_ * 2;
+    }
+    angle = x_angle + deta;
+    out = data;
+    out[3] = angle;
+
+    /*
+            t = time.time()
+
+        points = self.last_x_y_list[:]
+        points = np.array(points)
+        x_list = points[:, 0]
+        y_list = points[:, 1]
+        if len(points) >= NUM_FRAME and abs(x_list[-1] - x_list[0]) > DIST_THRED or abs(y_list[-1] - y_list[0]) > DIST_THRED:
+            center_rot_y = center_rot_y_f(points)
+
+            rot_y = bbox3D[3]
+            if rot_y > np.pi: rot_y -= int((rot_y + np.pi)/(2*np.pi)) * np.pi * 2
+            if rot_y < -np.pi: rot_y += int((np.pi - rot_y)/(2*np.pi)) * np.pi * 2
+
+            if abs(center_rot_y - rot_y) > np.pi / 2.0 and abs(
+                    center_rot_y - rot_y) < np.pi * 3 / 2.0:  # if the angle of two theta is not acute angle
+                rot_y += np.pi
+                if rot_y > np.pi: rot_y -= np.pi * 2  # make the theta still in the range
+                if rot_y < -np.pi: rot_y += np.pi * 2
+
+            bbox3D[3] = rot_y
+
+        t3_0 = time.time() - t
+
+        t = time.time()
+
+
+        self.time_since_update = 0
+        self.history = []
+        self.hits += 1
+        if self.still_first:
+            self.first_continuing_hit += 1  # number of continuing hit in the fist time
+
+        ######################### orientation correction
+        if self.kf.x[3] >= np.pi: self.kf.x[3] -= np.pi * 2  # make the theta still in the range
+        if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi * 2
+
+        new_theta = bbox3D[3]
+        if new_theta >= np.pi: new_theta -= np.pi * 2  # make the theta still in the range
+        if new_theta < -np.pi: new_theta += np.pi * 2
+        bbox3D[3] = new_theta
+
+        predicted_theta = self.kf.x[3]
+        if abs(new_theta - predicted_theta) > np.pi / 2.0 and abs(
+                new_theta - predicted_theta) < np.pi * 3 / 2.0:  # if the angle of two theta is not acute angle
+            self.kf.x[3] += np.pi
+            if self.kf.x[3] > np.pi: self.kf.x[3] -= np.pi * 2  # make the theta still in the range
+            if self.kf.x[3] < -np.pi: self.kf.x[3] += np.pi * 2
+
+        # now the angle is acute: < 90 or > 270, convert the case of > 270 to < 90
+        if abs(new_theta - self.kf.x[3]) >= np.pi * 3 / 2.0:
+            if new_theta > 0:
+                self.kf.x[3] += np.pi * 2
+            else:
+                self.kf.x[3] -= np.pi * 2
 
+        t3_1 = time.time() - t
+    */
 }
\ No newline at end of file

TrackingRos.cpp

@@ -7,7 +7,6 @@
 #include "matrix.h"
 #include <eigen3/Eigen/Dense>
 
-#define _PI_ 3.1415926
 
 float to360(float rot_y,float lidar_x_angle)
 {