使用C++ OpenCV实现椭圆区域检测与Aruco码的生成与检测并估计位姿

原创

小锋学长生活大爆炸

发布于 2022-08-19 21:42:21

1K0

发布于 2022-08-19 21:42:21

文章被收录于专栏：小锋学长生活大爆炸小锋学长生活大爆炸

前景概要

很多机器视觉的定位与识别场景，如无人车、无人机，都会用Aruco码或特定的标志物来实现，Aruco码的优点在于，xxxx(自行搜索)。

?对于像在低成本轻量级的无人机这种嵌入式系统上，搭载深度学习的识别算法目前还有困难

所以大家现在采用较多的，就是识别特定的标志物。之前看小鹏汽车的宣传片，他们也是在用黑白同心圆环的目标板。

实现内容

0、打开摄像头或某张图片

1、先检测圆环(因视角变换可能是椭圆环)；

2、裁剪保留圆环区域

3、检测圆环中的Aruco码(单个或菱形或棋盘)

4、计算目标的位姿

大致效果

代码预览

完整代码请看github（测试性代码，写的很粗糙，仅供参考）

Github:?https://github.com/1061700625/OpenCV_Aruco

Mat testDetect(Mat &markerImage, bool diamond = true, bool aamed=false, bool show=false) {
    /*************************************检测椭圆*****************************************************/
    // FeaturePoint(img ,img2);
    // EDCircle(markerImage);
    float t1, t2, tdelt;
    vector<Mat> results;
    Mat fullSplitImage;
 
    if(aamed) {
        t1 = cv::getTickCount();
        AAMED_Fled(markerImage, fullSplitImage, results);
        t2 = cv::getTickCount();
        tdelt = 1000.0*(t2-t1) / cv::getTickFrequency();
        std::cout << "AAMED耗时(ms)：" << tdelt << std::endl;
    }else {
        results.emplace_back(markerImage);
        fullSplitImage = markerImage.clone();
    }
 
    /************************************************************************************************/
 
    // 对每个椭圆区域进行检测
    for (auto& cropSplitImage: results) {
        //　检测Aruco
        vector<vector<Point2f>> diamondCorners;
        vector<cv::Vec4i> diamondIds;
        vector<vector<Point2f>> markerCorners;
        vector<int> markerIds;
        vector<vector<Point2f>> rejectedCandidates;
        t1 = cv::getTickCount();
        // cv::copyMakeBorder(cropSplitImage, cropSplitImage, 5, 5, 5, 5, cv::BORDER_CONSTANT, Scalar(255,0,0));
        detectAruco(cropSplitImage, markerCorners, rejectedCandidates, markerIds);
        if (markerCorners.empty()) {
            cout<<"无可用Marker"<<endl;
            return fullSplitImage;
        }
        // 显示检测到的但是由于字典对不上被拒绝的Marker
        if(show) {
            if (!rejectedCandidates.empty()){
                cout<<"一共有 "<<rejectedCandidates.size()<<" 个被拒绝的 Marker "<<endl;
                for (auto & rejectedCandidate : rejectedCandidates) {
                    for (int i=0;i<4;i++) {
                        cv::circle(fullSplitImage,cv::Point(rejectedCandidate[i].x,rejectedCandidate[i].y),6,cv::Scalar(0,0,255));
                    }
                }
            }
        }
 
        if(diamond) {
            detectDiamon(cropSplitImage, markerCorners, markerIds, diamondCorners, diamondIds);
        }
        t2 = cv::getTickCount();
        tdelt = 1000.0*(t2-t1) / cv::getTickFrequency();
        std::cout << "检测耗时(ms)：" << tdelt << std::endl;
 
 
        std::vector<cv::Vec3d> rvecs, tvecs;
        cv::Vec3d rvec, tvec;
        t1 = cv::getTickCount();
        if(diamond) {
            if (diamondIds.empty()) {
                cout<<"无可用diamondIds"<<endl;
                continue;
            }
            // 绘制检测边框
            if(show) {
                cv::aruco::drawDetectedDiamonds(fullSplitImage, diamondCorners, diamondIds);
            }
            // 估计相机位姿(相对于每一个marker)  markerLength为什么是squareLength?
            cv::aruco::estimatePoseSingleMarkers(diamondCorners, markerLength, cameraMatrix, distCoeffs, rvecs, tvecs);
        }else {
            if (markerIds.empty()){
                cout<<"无可用markerIds"<<endl;
                continue;
            }
            // 绘制检测边框
            if(show) {
                cv::aruco::drawDetectedMarkers(fullSplitImage, markerCorners, markerIds);
            }
            // 估计相机位姿(相对于每一个marker)
            // cv::aruco::estimatePoseSingleMarkers(markerCorners, squareLength, cameraMatrix, distCoeffs, rvecs, tvecs);
            // 估计相机位姿(相对于 aruco 板)
            cv::aruco::estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs, rvec, tvec); rvecs.emplace_back(rvec); tvecs.emplace_back(tvec);
        }
        t2 = cv::getTickCount();
        tdelt = 1000.0*(t2-t1) / cv::getTickFrequency();
        std::cout << "相机位姿估计耗时(ms)：" << tdelt << std::endl;
        // 为每个标记画轴
        t1 = cv::getTickCount();
        for (int i = 0; i < rvecs.size(); ++i) {
            rvec = rvecs[i];
            tvec = tvecs[i];
            // 得到的位姿估计是：从board坐标系到相机坐标系的
            cv::Mat R;
            cv::Rodrigues(rvec,R);
            Eigen::Matrix3d R_eigen;
            cv::cv2eigen(R,R_eigen);
            // Eigen中使用右乘的顺序, 因此ZYX对应的是012, 实际上这个编号跟乘法的顺序一致就可以了(从左向右看的顺序)
            Eigen::Vector3d zyx_Euler_fromR = R_eigen.eulerAngles(0,1,2);
            if(show) {
                cout << "R_{camera<---marker} :" << R << endl;
                cout << "t_{camera<---marker} :" << tvec << endl;
                cout << "zyx旋转欧拉角[输出顺序为:x,y,z]: " << (180)/(M_PI)*zyx_Euler_fromR.transpose()<<endl;
                cv::aruco::drawAxis(fullSplitImage, cameraMatrix, distCoeffs, rvec, tvec, 0.1);
                cout << "--------------------------------------------" << endl;
            }
            //cv::aruco::drawAxis(markerImage, cameraMatrix, distCoeffs, rvec, tvec, 0.1);
        }
        t2 = cv::getTickCount();
        tdelt = 1000.0*(t2-t1) / cv::getTickFrequency();
        std::cout << "欧拉角耗时(ms)：" << tdelt << std::endl;
    }
 
    if (aamed && show) {
        imshow("markerImage", markerImage);
        //imshow("fullSplitImage", fullSplitImage);
        //imwrite("../fullSplitImage.png", fullSplitImage);
    }
    // waitKey(0);
    return fullSplitImage;
}
 
int main(int argc, char* argv[])
{
    Mat fullSplitImage;
    Mat boardImage;
    board->draw( cv::Size(200, 200),  // 整个board的大小
                 boardImage,                         // 返回的图像
                 10,                            // 整个board的边距
                 1 );                           // 每个码内的边距
    imwrite("../boardImage.png", boardImage);
    cv::aruco::drawCharucoDiamond(dictionary, cv::Vec4i(0,1,2,3), 200, 150, boardImage);
    imwrite("../diamondImage.png", boardImage);
    //vector<Mat> markerImages = generateAruco(5);
    //Mat markerImage = markerImages[0];
    cout<<">> 预处理完成!"<<endl;
 
    /************************************************************************************************/
 
    float t1, t2, tdelt;
 
    Mat markerImage = imread("../img/4.jpg", 1);
 
    t1 = cv::getTickCount();
    fullSplitImage = testDetect(markerImage, false, false, true);
    t2 = cv::getTickCount();
    tdelt = 1000.0*(t2-t1) / cv::getTickFrequency();
    std::cout << "一帧总耗时(ms)：" << tdelt << std::endl;
 
    imshow("fullSplitImage", fullSplitImage);
    waitKey(0);
    return 0;
 
 
 
    /************************************************************************************************/
    Mat frame;
    VideoCapture capture;
    capture.open(0);
    if (!capture.isOpened()) {
        cerr << "ERROR! Unable to open camera\n";
        return -1;
    }
 
    cv::namedWindow("fullSplitImage",0);
    cv::resizeWindow("fullSplitImage", 960, 540);
    for (;;) {
        capture.read(frame);
        cv::resize(frame, frame, cv::Size(752, 480), 0, 0, cv::INTER_AREA);
        cout << frame.size << endl;
        if (frame.empty()) {
            cerr << "ERROR! blank frame grabbed\n";
            break;
        }
        t1 = cv::getTickCount();
        fullSplitImage = testDetect(frame, false, false);
        t2 = cv::getTickCount();
        tdelt = 1000.0*(t2-t1) / cv::getTickFrequency();
        std::cout << "一帧耗时(ms)：" << tdelt << std::endl;
        imshow("fullSplitImage", fullSplitImage.empty() ? frame : fullSplitImage);
        if (waitKey(1) >= 0)
            break;
    }
 
    return 0;
}