ros::opencv识别棋盘格角点

#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>

using namespace cv;

int main(int argc, char** argv)
{
    ros::init(argc, argv, "depth_point_filter_node");
    ros::NodeHandle nh;
    ros::NodeHandle pnh("~");

    // Read the image
    Mat image = imread("/home/lhh/Chessboard_image/4.jpg", IMREAD_GRAYSCALE);
    if (image.empty())
    {
        std::cerr << "Error: Unable to load image!" << std::endl;
        ros::shutdown();
    }

    imshow("Chessboard", image);
    waitKey(0);

    // 定义棋盘图案大小
    // 一般的M×N棋盘：
    // 每行的内角数量：每行上有 N-1 个内角
    // 每列的内角数量：每列上有 M-1 个内角
    Size patternSize(12, 14); // 棋盘每行和每列的内角数量

    // Detect chessboard corners
    std::vector<Point2f> corners;
    std::cout << "findChessboardCorners start" << std::endl;
    bool found = findChessboardCorners(image, patternSize, corners,
                 CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE);
    std::cout << "findChessboardCorners done" << std::endl;

    if (found)
    {
        // 提高角点精度
        cornerSubPix(image, corners, Size(11, 11), Size(-1, -1),
                     TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 0.001));

        // 绘制并显示角
        drawChessboardCorners(image, patternSize, corners, found);
        imshow("Chessboard", image);
        waitKey(0);
    }
    else
    {
        std::cerr << "Error: Chessboard corners not found!" << std::endl;
    }

    ros::spin();
}