提交测试

BaseTracker/Track3D.cpp

@@ -38,9 +38,11 @@ double correct_angle(std::vector<point2d>& points)
     //cout << "X_copy: " << endl << X_copy
     Eigen::Vector2d dX(points[4].x - points[0].x, points[4].y - points[0].y);
     Eigen::Vector2d orientation(vec(0, 0), vec(0, 1));
+    SDK_LOG(SDK_INFO, "vec = [%f,%f,%f,%f]",vec(0,0), vec(0,1), vec(1,0), vec(1,1));
     double alpha = dX.transpose() * orientation;
     if (alpha < 0)
         orientation = -orientation;
+    SDK_LOG(SDK_INFO, "correct_angle orientation[0] = %f, orientation[1] = %f", orientation[0], orientation[1]);
     double center_rot_y = atan2(orientation[1], orientation[0]);
     return center_rot_y;
 }