相机标定之棋盘格角点排序

OpenCV官方角点提取算法输出是直接排好序的，如果用其他角点提取算法输出角点需要自己排序，下面总结下自己排序方法

1. 找到棋盘格顶点（下图A,B,C,D）

    cv::Point2f LeftTop = *min_element(corner_points_buf.begin(), corner_points_buf.end(),
                                       [](const cv::Point2f& lhs, const cv::Point2f& rhs) {
                                           return lhs.x + lhs.y < rhs.x + rhs.y;
                                       });
    cv::Point2f RightTop = *max_element(corner_points_buf.begin(), corner_points_buf.end(),
                                        [](const cv::Point2f& lhs, const cv::Point2f& rhs) {
                                            return lhs.x - lhs.y < rhs.x - rhs.y;
                                        });
    cv::Point2f LeftBot = *min_element(corner_points_buf.begin(), corner_points_buf.end(),
                                      [](const cv::Point2f& lhs, const cv::Point2f& rhs) {
                                          return lhs.x - lhs.y < rhs.x - rhs.y;
                                      });
    cv::Point2f RightBot = *max_element(corner_points_buf.begin(), corner_points_buf.end(),
                                      [](const cv::Point2f& lhs, const cv::Point2f& rhs) {
                                          return lhs.x + lhs.y < rhs.x + rhs.y;
                                      });

2.仿射变换，映射图像角点到标准矩阵

        std::vector<cv::Point2f> imgPoints;
        std::vector<cv::Point2f> objPoints;

        //hardcode rate here, 11*8 chessboard write as 110*80, grid size 10
        imgPoints.push_back(LeftTop);
        objPoints.push_back(cv::Point2f(0.0,0.0));
        imgPoints.push_back(RightTop);
        objPoints.push_back(cv::Point2f((Config::CalibrationBoard.height - 1)*10,0.0));
        imgPoints.push_back(LeftBot);
        objPoints.push_back(cv::Point2f(0.0,(Config::CalibrationBoard.width - 1)*10));
        imgPoints.push_back(RightBot);
        objPoints.push_back(cv::Point2f((Config::CalibrationBoard.height - 1)*10,(Config::CalibrationBoard.width - 1)*10));
        if (imgPoints.size() != objPoints.size())
        {
            KUBOT_LOG(error) << "Affine transformation FAIL! Number of object and image points must be equal! ";
            return false;
        }

        cv::Mat H = cv::findHomography( cv::Mat(imgPoints), cv::Mat(objPoints), cv::RANSAC);

        std::vector<cv::Point2f>    points_transform;
        perspectiveTransform(corner_points_buf, points_transform, H);

        std::vector<ImgObjSeq>   img_obj_seq;
        for(size_t i = 0; i < corner_points_buf.size() ; i++) {
            ImgObjSeq ios(corner_points_buf[i], points_transform[i], 0);
            img_obj_seq.push_back(ios);
        }

3. 图像角点根据映射后的矩阵排序

struct ImgObjSeq
{
public:
    cv::Point2f     img;
    cv::Point2f     obj;
    int             seq;
    ImgObjSeq(){}
    ImgObjSeq(cv::Point2f p1,cv::Point2f p2,int s){
        img = p1;
        obj = p2;
        seq = s;
    }
};

inline bool LessSort(ImgObjSeq p1, ImgObjSeq p2)
{
    return (p1.seq < p2.seq);
}


std::vector<cv::Point2f> CameraCalibrator::chessboard_corner_sorting(std::vector<cv::Point2f> dst_input)
{
    std::vector<cv::Point2f>              corner_sorted;
    std::vector<std::vector<cv::Point2f>> point_all;
    std::vector<cv::Point2f>              point_row;

    point_all.clear();
    point_row.clear();
    corner_sorted.clear();

    for(unsigned int i = 0; i < dst_input.size();i++)
    {
        for(unsigned int j =i+1;j<dst_input.size();j++)
        {
            if(abs( dst_input.at(i).y - dst_input.at(j).y)<= 8/*grid size 10*/)
            {
                point_row.push_back(dst_input.at(j));
                dst_input.erase(dst_input.begin()+j);
                j--;
            }
        }
        point_row.push_back(dst_input.at(i));
        sort(point_row.begin(),point_row.end(),[](const cv::Point2f& p1, const cv::Point2f& p2) {
            return p1.x  < p2.x;
        });
        point_all.push_back(point_row);
        point_row.clear();
    }
    sort(point_all.begin(),point_all.end(),[](const std::vector<cv::Point2f>& p1, const std::vector<cv::Point2f>& p2) {
        return p1.at(0).y  < p2.at(0).y;
    });
    //after_sorting
    for(unsigned int i=0; i < point_all.size();i++)
    {
        for(unsigned int j=0; j < point_all.at(i).size();j++)
        {
            corner_sorted.push_back(point_all.at(i).at(j));
        }
    }
    return corner_sorted;
}

std::vector<cv::Point2f> corner_after_sorting;

auto points_transform_sorted =  chessboard_corner_sorting(points_transform);
for(size_t i = 0; i < points_transform_sorted.size() ; i++)
{
    for(size_t j = 0; j < img_obj_seq.size() ; j++)
       {
            if (img_obj_seq[j].obj == points_transform_sorted[i]){
            img_obj_seq[j].seq = i;
            continue;
            }
        }
}
std::sort(img_obj_seq.begin(),img_obj_seq.end(),LessSort);

for(size_t i = 0; i < img_obj_seq.size() ; i++){
    corner_after_sorting.push_back(img_obj_seq[i].img);
}