TEXT

Posted 2021-03-09 xingkongcanghai

//类的初始化
    QRCodeDetector QRCode;//opencv4二维码
    MonocularRanging mono("../QR_test/cameraParams.xml");//单目测距类
    ChooseTarget Chooser(20, 10);//选择目标
    ChooseTarget::Target target;
    BallisticCalculation Ballistic(5, 25000, 100, 9806);//弹道计算
    mono.SetTargetSize(62, 62);
    //图像获取
    VideoCapture CAP;
    CAP.open("/dev/video0", CAP_V4L2);
    CAP.set(CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
    CAP.set(CAP_PROP_FRAME_HEIGHT, 480);
    CAP.set(CAP_PROP_FRAME_WIDTH, 640);
    CAP.set(CAP_PROP_AUTO_WB, 1);//白平衡
    //    CAP.set(CAP_PROP_AUTO_EXPOSURE, 0);
    //    CAP.set(CAP_PROP_EXPOSURE, 121);
    CAP.set(CAP_PROP_FPS, 120);

    if (!CAP.isOpened())
    {
        cout << "cap error" << endl;
        exit(0);
    }
    Mat Src;

    vector<Point2f> Points;
    SpatialLocation location;//6维空间坐标系类
    Sphe_COORD Spherical;//SphericalCoordiantes 球量
    Rect_COORD Trans;//SpaceRectangularCoordiantes 矩形空间量

    double time1 = cv::getTickCount();//时间
    int count = 0, _count = 0;

    while (true)
    {
        CAP >> Src;
        if (QRCode.detect(Src, Points))
        {
            line(Src, Points[0], Points[1], Scalar(0, 255, 0));
            line(Src, Points[1], Points[2], Scalar(0, 255, 0));
            line(Src, Points[2], Points[3], Scalar(0, 255, 0));
            line(Src, Points[3], Points[0], Scalar(0, 255, 0));

            mono.GetSpatialLocation(location, Points);//得到目标空间位置 6维空间坐标系类
            Spherical = location;//球量<<6维空间坐标系类
            Trans = Spherical;//矩形空间量<<球量

            /*getTickCount()：用于返回从操作系统启动到当前所经的计时周期数，看名字也很好理解，get Tick Count(s)。
            getTickFrequency()：用于返回CPU的频率。get Tick Frequency。这里的单位是秒，也就是一秒内重复的次数。
            */
            //将目标压入链表
            Chooser.PushTarget(ChooseTarget::Target(location, ((double)getTickCount() / getTickFrequency())));


            //printf("X: %+3.02lf	Y: %+3.02lf	Z: %+3.02lf
", target.x, target.y, target.z);
            Points.clear();

            //            printf("X: %+3.02lf/%+3.02lf	Y: %+3.02lf/%+3.02lf	Z: %+3.02lf/%+3.02lf
",
            //                   location.x, Trans.x,
            //                   location.y, Trans.y,
            //                   location.z, Trans.z);

                        //printf("phi: %+3.02lf theta: %+3.02lf r: %+3.02lf z: %+3.02lf
", Ran2Deg(Spherical.phi), Ran2Deg(Spherical.theta), Spherical.r, location.z);

                        //printf("P: %+3.02lf Y: %+3.02lf R: %+3.02lf
", location.Pitch, location.Yaw, location.Roll);

            putText(Src, to_string(location.z), Points[0], FONT_HERSHEY_TRIPLEX, 1, Scalar(0, 0, 255), 1);
        }


        if (Chooser.GetTarget(target))//如果得到目标
        {
            double vx, vy, vz;//x轴，y轴，z轴的速度引用
            target.GetSpeed(vx, vy, vz);//得到x，y，z速度
            //printf("X: %+3.02lf/%+3.02lf	Y: %+3.02lf/%+3.02lf	Z: %+3.02lf/%+3.02lf
", target.x, vx, target.y, vy, target.z, vz);
            Ballistic.GetAngle(target);//弹道的预测及计算
        }

        putText(Src, "FPS:" + to_string(_count), Point(0, 25), FONT_HERSHEY_TRIPLEX, 1, Scalar(0, 0, 255), 1);
        imshow("Src", Src);
        waitKey(1);

        //计算FPS
        count++;
        if ((cv::getTickCount() - time1) / cv::getTickFrequency() >= 1)
        {
            time1 = cv::getTickCount();
            _count = count;
            count = 0;
        }
    }
    return 0;