提交更新

BaseTracker/Track3D.cpp

@@ -7,6 +7,7 @@
 #include <opencv2/opencv.hpp>
 #include <algorithm>
 
+#define DIST_THRED  0.5
 
 Track3D::Track3D():BaseTrack(10, 7)
 {
@@ -65,6 +66,31 @@ Track3D::Track3D():BaseTrack(10, 7)
 
     m_iou_threshold = 0.01;
 }
+void Track3D::Init(const std::vector<float>& data)
+{
+    std::vector<float> tmp = data;
+    //修正角度在-PI到PI
+    while (tmp[3] > _PI_)
+        tmp[3] -= 2 * _PI_;
+    while (tmp[3] < -_PI_)
+        tmp[3] += 2 * _PI_;
+    BaseTrack::Init(tmp);
+}
+
+void Track3D::Predict()
+{
+    if (kf_ == nullptr)
+        return;
+    point2d pos = {};
+    pos.x = kf_->x_[0];
+    pos.y = kf_->x_[1];
+    if (m_points.size() >= 5)
+    {
+        m_points.erase(m_points.begin());
+    }
+    m_points.push_back(pos);
+    BaseTrack::Predict();
+}
 
 
 float calcIOU(cv::RotatedRect rect1, cv::RotatedRect rect2) {
@@ -259,9 +285,22 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
 {
     if (kf_ == nullptr)
         return;
-    float x_angle = kf_->x_[3];
-    float angle = data[3];
-    float deta = angle - x_angle;
+    double rot_y = data[3];
+    if (m_points.size() >= 5 && (abs(m_points[4].x - m_points[0].x) > DIST_THRED || abs(m_points[4].y - m_points[0].y) > DIST_THRED))
+    {
+        double center_rot_y = correct_angle(m_points);
+        double rotate = abs(center_rot_y - rot_y);
+        while (rotate > _PI_)
+            rotate -= 2 * _PI_;
+        while (rotate < -_PI_)
+            rotate += 2 * _PI_;
+        if (rotate > _PI_ / 2.0f && rotate < _PI_ * 3 / 2.0f)//夹角在90到270度，认为角度计算错误，差180度
+        {
+            rot_y += _PI_;
+        }
+    }
+    double x_angle = kf_->x_[3];
+    double deta = rot_y - x_angle;
     while (deta >= _PI_)
     {
         deta -= _PI_ * 2;
@@ -270,70 +309,8 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
     {
         deta += _PI_ * 2;
     }
-    //if (abs(deta) > ANGLE_CHANGE_MAX)
-    //{
-    //    deta = deta > 0 ? ANGLE_CHANGE_MAX : -ANGLE_CHANGE_MAX;
-    //}
-    angle = x_angle + deta;
+    rot_y = 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
+    out[3] = rot_y;
 
-        t3_1 = time.time() - t
-    */
 }
\ No newline at end of file

BaseTracker/Track3D.h

@@ -26,6 +26,9 @@ public:
     Track3D();
     ~Track3D() {}
 
+    virtual void Init(const std::vector<float>& data);
+    virtual void Predict();
+
     virtual double CalculateIou(const std::vector<float>& data);
 
     virtual int GetIouDataOrder(std::vector<int>& order);
@@ -35,5 +38,5 @@ public:
 
     trackOjbPtr m_obj = nullptr;
 
-    std::queue<point2d> m_points;//保存最近的5个移动的点位
+    std::vector<point2d> m_points;//保存最近的5个移动的点位
 };