提交文件

BaseTracker/Iou.cpp

+#include "Iou.h"
+
+
+void HungarianMatching(const std::vector<std::vector<float>>& iou_matrix,
+    size_t nrows, size_t ncols,
+    std::vector<std::vector<float>>& association)
+{
+    Matrix<float> matrix(nrows, ncols);
+    // Initialize matrix with IOU values
+    for (size_t i = 0; i < nrows; i++) {
+        for (size_t j = 0; j < ncols; j++) {
+            // Multiply by -1 to find max cost
+            if (iou_matrix[i][j] != 0) {
+                matrix(i, j) = -iou_matrix[i][j];
+            }
+            else {
+                // TODO: figure out why we have to assign value to get correct result
+                matrix(i, j) = 1.0f;
+            }
+        }
+    }
+
+    //    // Display begin matrix state.
+    //    for (size_t row = 0 ; row < nrows ; row++) {
+    //        for (size_t col = 0 ; col < ncols ; col++) {
+    //            std::cout.width(10);
+    //            std::cout << matrix(row,col) << ",";
+    //        }
+    //        std::cout << std::endl;
+    //    }
+    //    std::cout << std::endl;
+
+
+        // Apply Kuhn-Munkres algorithm to matrix.
+    Munkres<float> m;
+    m.solve(matrix);
+
+    //    // Display solved matrix.
+    //    for (size_t row = 0 ; row < nrows ; row++) {
+    //        for (size_t col = 0 ; col < ncols ; col++) {
+    //            std::cout.width(2);
+    //            std::cout << matrix(row,col) << ",";
+    //        }
+    //        std::cout << std::endl;
+    //    }
+    //    std::cout << std::endl;
+
+    for (size_t i = 0; i < nrows; i++) {
+        for (size_t j = 0; j < ncols; j++) {
+            association[i][j] = matrix(i, j);
+        }
+    }
+}
+
+
+struct CVPoint2f
+{
+    CVPoint2f(float _x, float _y):x(_x),y(_y)
+    {
+    }
+    CVPoint2f(const CVPoint2f& obj)
+    {
+        x = obj.x;
+        y = obj.y;
+    }
+    float x;
+    float y;
+};
+
+
+bool bInBox(const std::vector<CVPoint2f> &vpBoxA, const CVPoint2f &p)
+{
+    std::vector<CVPoint2f> corners = vpBoxA;
+    for(int i = 0; i<vpBoxA.size(); i++)   //01230123
+        corners.push_back(vpBoxA[i]);
+
+    std::vector< std::vector<double> > linesA;
+    for(int i = 0; i<vpBoxA.size(); i++)
+    {
+        CVPoint2f p1 = corners[i];
+        CVPoint2f p2 = corners[i+1];
+        CVPoint2f p3 = corners[i+2];
+        double a;
+        if(p1.x - p2.x == 0)
+            a = -(p1.y - p2.y)/0.0001;
+        else
+            a = -(p1.y - p2.y)/(p1.x - p2.x);
+
+        double b = 1;
+        double c = -a * p2.x - p2.y;
+        double d = a*p3.x + b*p3.y + c;
+
+        std::vector<double> line{a,b,c,d};
+        linesA.push_back(line);
+    }
+
+    for(int i=0; i<linesA.size(); i++)
+    {
+        std::vector<double > l = linesA[i];
+        double y = l[0] * p.x + l[1] * p.y +l[2];
+        if(y * l[3] < 0)
+            return false;
+    }
+    return true;
+}
+
+double InterSection_2D(const std::vector<CVPoint2f> &vpBoxA, const std::vector<CVPoint2f> &vpBoxB)
+{
+    double min_x, max_x, min_y, max_y;
+    min_x = vpBoxA[0].x;
+    max_x = vpBoxA[0].x;
+    min_y = vpBoxA[0].y;
+    max_y = vpBoxA[0].y;
+
+    for(int i=1; i<vpBoxA.size(); i++)
+    {
+        CVPoint2f p = vpBoxA[i];
+        if(p.x > max_x)
+            max_x = p.x;
+        if(p.x < min_x)
+            min_x = p.x;
+        if(p.y > max_y)
+            max_y = p.y;
+        if(p.y < min_y)
+            min_y = p.y;
+    }
+    for(int i=0; i<vpBoxB.size(); i++)
+    {
+        CVPoint2f p = vpBoxB[i];
+        if(p.x > max_x)
+            max_x = p.x;
+        if(p.x < min_x)
+            min_x = p.x;
+        if(p.y > max_y)
+            max_y = p.y;
+        if(p.y < min_y)
+            min_y = p.y;
+    }
+
+    //将两个BBox的定点坐标最小值设置为0, 以防止有负数的产生
+    std::vector<CVPoint2f> vpBoxAA = vpBoxA;
+    std::vector<CVPoint2f> vpBoxBB = vpBoxB;
+
+    //if(min_x < 0 && min_y < 0)
+    for(int i=0; i<vpBoxA.size(); i++)
+    {
+        vpBoxAA[i].x = vpBoxAA[i].x - min_x;
+        vpBoxAA[i].y = vpBoxAA[i].y - min_y;
+        vpBoxBB[i].x = vpBoxBB[i].x - min_x;
+        vpBoxBB[i].y = vpBoxBB[i].y - min_y;
+    }
+
+    int imax_x = (int)((max_x - min_x) * 10000);
+    int imax_y = (int)((max_y - min_y) * 10000);
+
+    double points_inA = 0, points_inB = 0, points_inAB = 0;
+    srand((int)time(0));
+    for(int i = 0; i<100000; i++)
+    {
+        int xx = rand()%(imax_x)+1;  //生成[1, imax_x]之间的整数
+        int yy = rand()%(imax_y)+1;  //生成[1, imax_y]之间的整数
+
+        CVPoint2f p((float)xx / 10000.0, (float)yy / 10000.0);
+        if( bInBox(vpBoxAA, p) )
+            ++points_inA;
+        if( bInBox(vpBoxBB, p) )
+            ++points_inB;
+        if( bInBox(vpBoxAA, p) && bInBox(vpBoxBB, p) )
+            ++points_inAB;
+    }
+
+    double iou = points_inAB / (points_inA + points_inB - points_inAB);
+    //cout<<"points_inA : "<<points_inA<<",  points_inB: "<<points_inB<<" ,points_inAB: "<<points_inAB<<endl;
+    return iou;
+}
+//double CuboidIoU(const Eigen::MatrixXd &truth_poses, const Eigen::MatrixXd &landmark_poses)
+double CuboidIoU(const std::vector<float> &truth_poses, const std::vector<float> &landmark_poses)
+{
+
+    std::vector<CVPoint2f>  vground_points;
+    std::vector<CVPoint2f>  vlandmark_points;
+    if(1)  //通过坐标旋转求取groundtruth立方体中 2D-Boundbox四个顶点的坐标
+    {
+        //double cen_x = truth_poses(0,0);
+        //double cen_y = truth_poses(0,1);
+        //double len = truth_poses(0,6);
+        //double wid = truth_poses(0,7);
+        //double yaw = truth_poses(0,5);
+        double cen_x = truth_poses[0];
+        double cen_y = truth_poses[1];
+        double len = truth_poses[6];
+        double wid = truth_poses[7];
+        double yaw = truth_poses[5];
+
+        double x, y, xx, yy;
+        x = cen_x - len;
+        y = cen_y - wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p0(xx, yy);
+
+        x = cen_x - len;
+        y = cen_y + wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p1(xx, yy);
+
+        x = cen_x + len;
+        y = cen_y + wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p2(xx, yy);
+
+        x = cen_x + len;
+        y = cen_y - wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p3(xx, yy);
+
+        vground_points = {p0, p1, p2, p3};
+    }
+
+    if(1)//通过坐标旋转求取landmark中 2D-Boundbox四个顶点的坐标
+    {
+        //double cen_x = landmark_poses(0,0);
+        //double cen_y = landmark_poses(0,1);
+        //double len = landmark_poses(0,6);
+        //double wid = landmark_poses(0,7);
+        //double yaw = landmark_poses(0,5);
+        double cen_x = landmark_poses[0];
+        double cen_y = landmark_poses[1];
+        double len = landmark_poses[6];
+        double wid = landmark_poses[7];
+        double yaw = landmark_poses[5];
+
+        double x, y, xx, yy;
+        x = cen_x - len;
+        y = cen_y - wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p0(xx, yy);
+
+        x = cen_x - len;
+        y = cen_y + wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p1(xx, yy);
+
+        x = cen_x + len;
+        y = cen_y + wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p2(xx, yy);
+
+        x = cen_x + len;
+        y = cen_y - wid;
+        xx = (x - cen_x)*cos(yaw) - (y - cen_y)*sin(yaw) + cen_x;
+        yy = (x - cen_x)*sin(yaw) + (y - cen_y)*cos(yaw) + cen_y;
+        CVPoint2f p3(xx, yy);
+
+        vlandmark_points = {p0, p1, p2, p3};
+    }
+
+    double iou_2d = InterSection_2D(vlandmark_points, vground_points);
+    std::cout << " iou_2d = " << iou_2d << std::endl;
+    double iou_3d = 0;
+    if (iou_2d > 0) {
+        //double tru_minz = truth_poses(0, 2) - truth_poses(0, 8);
+        //double tru_maxz = truth_poses(0, 2) + truth_poses(0, 8);
+
+        //double land_minz = landmark_poses(0, 2) - landmark_poses(0, 8);
+        //double land_maxz = landmark_poses(0, 2) + landmark_poses(0, 8);
+
+        double tru_minz = truth_poses[2] - truth_poses[8];
+        double tru_maxz = truth_poses[2] + truth_poses[8];
+
+        double land_minz = landmark_poses[2] - landmark_poses[8];
+        double land_maxz = landmark_poses[2] + landmark_poses[8];
+        if (land_maxz <= tru_maxz && land_maxz >= tru_minz) {
+            double height_iou = (land_maxz - tru_minz) / (tru_maxz - land_minz);
+            iou_3d = iou_2d * height_iou;
+        } else if (tru_maxz < land_maxz && tru_maxz > land_minz) {
+            double height_iou = (tru_maxz - land_minz) / (land_maxz - tru_minz);
+            iou_3d = iou_2d * height_iou;
+        }
+    }
+
+    return iou_3d;
+}
+
+
+//void main(int argc, char **argv)
+//{
+//    Eigen::MatrixXd truth_poses(1,9);
+//    truth_poses<<-0.4875, -0.798913, -1.125, 0, 0, -1.27409, 0.240477, 0.235315, 0.375;
+//    Eigen::MatrixXd landmark_poses(1,9);
+//    landmark_poses<<-0.506616, -0.796303, -1.20499, 0, 0, -0.345443, 0.22,  0.22, 0.4 ;
+//    double iou_3d = CuboidIoU_once(truth_poses, landmark_poses);
+//    cout<<"the iou of two cuboid is: "<<iou_3d<<endl;
+//    return;
+//}
\ No newline at end of file