LenzSDK_iOS/SDK/LenzCameraNativeModuleForRN/LenzCameraNativeModuleForRN/Classes/VideoStitch/lenz_stitcher.cpp

//
//  lenz_stitcher.cpp
//  LenzCameraNativeModuleForRN
//
//  Created by lr on 2023/2/8.
//

#include "lenz_stitcher.hpp"

Ptr<Feature2D> stitch_feature_finder = SiftFeatureDetector::create();
Ptr<FastFeatureDetector> detector = FastFeatureDetector::create(30);

const string ERROR_PLACE = "PANORAMA_ERROR_PLACE";

// 设定一些全局变量，用于记录后台拼接大图的数据，每次reset的时候需要把这些全局变量也一起重置
Mat big_stitch_image = Mat();
Mat neighbour_homo = Mat();
vector<KeyPoint> neighbour_keypoints;
Mat neighbour_descriptors = Mat();
int big_stitch_status = STITCH_STATUS_INIT;
int big_frame_num = 0;
float big_work_scale = 0;
string big_local_path = "";

inline LenzStitcher::LenzStitcher(string input_path)
{
    local_path = input_path;

    // 初始化拼接大图时用到的数据
    vector<KeyPoint>().swap(neighbour_keypoints);
    big_stitch_image = Mat();
    neighbour_homo = Mat();
    neighbour_descriptors = Mat();
    big_stitch_status = STITCH_STATUS_INIT;
    big_frame_num = 0;
    big_work_scale = 0;
    big_local_path = local_path;

    // 清理本地目录
    if (isFloderexit(local_path.c_str()))
        deleteFile(local_path.c_str()); // remove the files and dirs in local path
}

inline float image_resize(Mat &image, Mat &scaled_image, int target_height, int mode)
{
    float work_scale = 1.0f;
    // will scale down the image size if image rows bigger than max_width
    if (mode == 0)
    {
        if (image.rows > target_height)
            work_scale = float(target_height) / float(image.rows);
    }
    else
    {
        if (image.cols > target_height)
            work_scale = float(target_height) / float(image.cols);
    }
    resize(image, scaled_image, Size(), work_scale, work_scale, 5);
    return work_scale;
}

inline void to_gray(Mat &image, Mat &gray_image)
{
    int channels = image.channels();
    if (channels == 3) // BGR
        cvtColor(image, gray_image, COLOR_BGR2GRAY);
    else if (channels == 1) // gray
        image.copyTo(gray_image);
}

inline void LenzStitcher::remove_local_path()
{
    if (isFloderexit(local_path.c_str()))
    {
        deleteFile(local_path.c_str()); // remove the files and dirs in local path
        rmdir(local_path.c_str());      // remove the local path dir
    }
}

/**
 * @brief 视频拼接算法，
 *
 * 输出：
 *      int数组: [拼接状态，是否需要存图]
 *       拼接状态：
 *          -1: 拍摄完成后拼接大图失败
 *          0: 拍摄过程中产生拼接缩略图失败
 *          2: 特征点变少，放慢速度
 *          3: 特征点极少，即将失败
 *          4：当前拍摄固定了方向，请勿往反方向移动
 *       是否需要存图：
 *          0：不需要
 *          1：需要
 *
 */
inline int *LenzStitcher::ofcheck_stitch(Mat &input_frame, int const direction, int const is_last_one)
{
//    cout << "input_frame shape is " << input_frame.rows << "*" << input_frame.rows << ", type is " << input_frame.type();
    // 使用光流法来进行图片的匹配
    int static stitch_result[2]; // 返回结果有2个字段，第一个字段是拼接状态，第二个字段是是否需要存帧
    stitch_result[0] = 1;        // 默认当前帧会拼接成功
    stitch_result[1] = 0;

    if (big_stitch_status == STITCH_STATUS_FAILED)
    {
        stitch_result[0] = -1; // 拼接大图出现失败，则直接提示失败
        return stitch_result;
    }

    // 最后一帧不计算缩略图直接拼大图并返回大图拼接结果
    if (is_last_one)
    {
        // 在子线程进行拼接
        big_frame_index++;
        thread t(stitch_big_image, input_frame, last_angles, direction, big_frame_index);
        t.detach();

        bool stitch_success = get_big_image(big_frame_index);
        if (!stitch_success)
        {
            stitch_result[0] = -1; // 拍摄完成后拼接大图失败
        }
        return stitch_result;
    }

    if (stitcher_status == STITCH_STATUS_CHECKING)
    {
//        cout << "check: The stitcher is still running other function!" << stitcher_status << endl;
        stitch_result[0] = 0;
        return stitch_result;
    }

    bool save_this = false;
    stitcher_status = STITCH_STATUS_CHECKING;

    Mat frame;
    float resize_scale = image_resize(input_frame, frame, STITCH_HEIGHT, 1);
//    rotate(frame, frame, 0);
    // 图片压缩
    Mat scaled_frame, scaled_gray_frame;
    resize_scale = image_resize(input_frame, scaled_frame, FEATURE_HEIGHT, 1);
//    rotate(scaled_frame, scaled_frame, 0);
    to_gray(scaled_frame, scaled_gray_frame);
    scaled_frame.release();
    float work_scale = 1.0 * scaled_gray_frame.rows / frame.rows;

    vector<KeyPoint> frame_keypoints;
    detector->detect(scaled_gray_frame, frame_keypoints);

    if (frame_keypoints.size() < matches_thresh)
    {
        stitch_result[0] = 0;
        stitcher_status = STITCH_STATUS_FREE;
        return stitch_result;
    }

    frame_index++;

    // get the perspective corners
    vector<Point2f> frame_corners(4);
    frame_corners[0] = Point2f(0, 0);
    frame_corners[1] = Point2f(frame.cols, 0);
    frame_corners[2] = Point2f(frame.cols, frame.rows);
    frame_corners[3] = Point2f(0, frame.rows);
    vector<Point2f> transpose_corners(4);
    if (frame_index == 1)
    {
        last_stitch_image = frame;
        // update the last stitch image
        // string _stitched_img_path = local_path + "last_stitch.jpg";
        // bool status = imwrite(_stitched_img_path, frame);
        // last_stitch_image_path = _stitched_img_path;

        last_warp_points = frame_corners;
        last_check_data.scaled_gray_frame = scaled_gray_frame;
        last_check_data.keypoints = frame_keypoints;
        last_check_data.work_scale = work_scale;
        last_check_data.homo = (Mat_<double>(3, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);

        // 在子线程进行拼接
        big_frame_index++;
        thread t(stitch_big_image, input_frame, last_angles, direction, big_frame_index);
        t.detach();

        stitcher_status = STITCH_STATUS_FREE;
        stitch_result[1] = 1;
        return stitch_result;
    }

    vector<Point2f> frame_points, last_frame_points;
    randomSelectKeyPoints(last_check_data.keypoints, last_frame_points, 250);

    vector<uchar> vstatus;
    vector<float> verror;
    calcOpticalFlowPyrLK(last_check_data.scaled_gray_frame, scaled_gray_frame, last_frame_points, frame_points, vstatus, verror);
    vector<Point2f> matched_frame_points, neighbour_points;
    int good_match_count = 0;
    for (unsigned int i = 0; i < vstatus.size(); i++)
    {
        if (vstatus[i] == 0)
            continue;
        good_match_count++;
        neighbour_points.push_back(Point2f(last_frame_points[i].x / work_scale, last_frame_points[i].y / work_scale));
        matched_frame_points.push_back(Point2f(frame_points[i].x / work_scale, frame_points[i].y / work_scale));
    }
    //    LOGI("matched_frame_points size %d", matched_frame_points.size());
    if (matched_frame_points.size() < matches_thresh)
    {
        stitcher_status = STITCH_STATUS_FREE;
        failed_count++;
        if (failed_count >= failed_thresh)
        {
            stitch_result[0] = 0; // 特征点太少导致缩略图拼接失败了
        }
        return stitch_result;
    }
    Mat H = findHomography(matched_frame_points, neighbour_points, RANSAC); // find current frame perspective to neighbour image homography
    // get the initial perspective corners, to find the shift
    perspectiveTransform(frame_corners, transpose_corners, H);
    int move_direction = get_direction(frame_corners, transpose_corners); // 1从上到下，2从下到上，3从左到右，4从右到左
    if (last_direction == 0)
    {
        last_direction = move_direction; // 异形拼接的时候会拐弯，所以需要记录下它的上一个方向，当方向变化时，需要把那一帧给拼起来
    }
    if ((direction == 1 && move_direction == 2) ||
        (direction == 2 && move_direction == 1) ||
        (direction == 3 && move_direction == 4) ||
        (direction == 4 && move_direction == 3))
    {
        //        LOGI("请保持手机稳定向固定方向移动!");
        stitcher_status = STITCH_STATUS_FREE;
        stitch_result[0] = 4;
        return stitch_result;
    }
    H = last_check_data.homo * H;
    perspectiveTransform(frame_corners, transpose_corners, H);

    float left_x, right_x, top_y, bottom_y;
    left_x = min_axis(vector<float>{transpose_corners[0].x, transpose_corners[1].x, transpose_corners[2].x, transpose_corners[3].x});
    top_y = min_axis(vector<float>{transpose_corners[0].y, transpose_corners[1].y, transpose_corners[2].y, transpose_corners[3].y});

    float base_shift_x = (left_x < 0) ? -left_x : 0;
    total_shift_x += base_shift_x;
    float base_shift_y = (top_y < 0) ? -top_y : 0;
    total_shift_y += base_shift_y;
    Mat shift = (Mat_<double>(3, 3) << 1.0, 0.0, base_shift_x, 0.0, 1.0, base_shift_y, 0.0, 0.0, 1.0);
    H = shift * H;
    perspectiveTransform(last_warp_points, last_warp_points, shift); // 把之前的需要存下来的图的框映射到拼接图上

    perspectiveTransform(frame_corners, transpose_corners, H); // 把当前图片的四个顶点映射到拼接图上
    // 如果图片映射后的4个顶点非凸，直接返回失败
    if (!is_convex(transpose_corners))
    {
//        LOGE("error conners is not convex");
        stitcher_status = STITCH_STATUS_FREE;
        failed_count++;
        if (failed_count >= failed_thresh)
        {
            stitch_result[0] = 0;
        }
        return stitch_result;
    }

    // 计算映射后的四边形的四个内角
    float a0 = angle_of_two_vector(transpose_corners[1], transpose_corners[3], transpose_corners[0]);
    float a1 = angle_of_two_vector(transpose_corners[2], transpose_corners[0], transpose_corners[1]);
    float a2 = angle_of_two_vector(transpose_corners[3], transpose_corners[1], transpose_corners[2]);
    float a3 = angle_of_two_vector(transpose_corners[0], transpose_corners[2], transpose_corners[3]);
    // LOGI("calced angle a0 %f, a1 %f, a2 %f a3 %f", a0, a1, a2, a3);
    vector<float> angles = vector<float>{a0, a1, a2, a3};
    float min_angle = min_axis(angles);

    // 获取4个映射到拼接图外接顶点
    left_x = min_axis(vector<float>{transpose_corners[0].x, transpose_corners[3].x});
    right_x = max_axis(vector<float>{transpose_corners[1].x, transpose_corners[2].x});
    bottom_y = max_axis(vector<float>{transpose_corners[2].y, transpose_corners[3].y});
    top_y = min_axis(vector<float>{transpose_corners[0].y, transpose_corners[1].y});

    // 获取拼接图的尺寸
    float stitch_width = MAX(right_x, last_stitch_image.cols + total_shift_x);
    float stitch_height = MAX(bottom_y, last_stitch_image.rows + total_shift_y);
    float warp_width = right_x - left_x;
    float warp_height = bottom_y - top_y;

    // 判断一些异常情况要及时返回失败
    if (min_angle < angle_thresh || stitch_width > MAX_SIZE || stitch_height > MAX_SIZE || warp_width < 2 || warp_height < 2)
    {
        //        LOGE("error angle %f, stitch shape %f*%f, warp shape %f*%f", angle, stitch_width, stitch_height, warp_width, warp_height);
        stitcher_status = STITCH_STATUS_FREE;
        failed_count++;
        if (failed_count >= failed_thresh)
        {
            stitch_result[0] = 0;
        }
        return stitch_result;
    }
    last_angles = angles;

    // 计算当前图和上一个保存下来的图在拼接图上的重合度
    float iou = single_box_iou(last_warp_points, transpose_corners);

    // 如果当前图片与上一张存下来的图的iou低于阈值或者传入参数告诉我这是最后一帧
    // 或者手机移动的方向变化了(设定一下iou的阈值排除抖动的干扰)
    if (iou < iou_thresh || (last_direction != move_direction && iou < 0.9))
    {
        last_warp_points = transpose_corners;
        stitch_result[1] = 1; // 需要保存当前帧到本地

        // 在子线程进行拼接
        big_frame_index++;
        save_this = true;
        thread t(stitch_big_image, input_frame, angles, direction, big_frame_index);
        t.detach();
    }

    // 开始拼接，设定拼接频率
    if (save_this)
    {
        // merge the warped frame to the last stitched image
        Mat stitch_image(stitch_height, stitch_width, CV_8UC3, Scalar(0));
        // copy last stitch image to this new image
        last_stitch_image.copyTo(stitch_image(Rect(total_shift_x, total_shift_y, last_stitch_image.cols, last_stitch_image.rows)));
        total_shift_x = 0;
        total_shift_y = 0;

        Mat warpH = H.clone();
        Mat warp_shift = (Mat_<double>(3, 3) << 1.0, 0.0, -left_x, 0.0, 1.0, -top_y, 0.0, 0.0, 1.0);
        warpH = warp_shift * warpH;
        Mat scaled_image_warp;
        warpPerspective(frame, scaled_image_warp, warpH, Size(warp_width, warp_height));

        Mat last_shift_img = stitch_image.clone();
        scaled_image_warp.copyTo(stitch_image(Rect(left_x, top_y, warp_width, warp_height)));
        scaled_image_warp.release();

        optimizeSeam(last_shift_img, stitch_image, transpose_corners);
        last_shift_img.release();

        // string _stitched_img_path = local_path + "last_stitch.jpg";
        // bool status = imwrite(_stitched_img_path, stitch_image);
        // last_stitch_image_path = _stitched_img_path;
        last_stitch_image = stitch_image;
    }

    last_check_data.scaled_gray_frame = scaled_gray_frame;
    last_check_data.keypoints = frame_keypoints;
    last_check_data.work_scale = work_scale;
    last_check_data.homo = H;
    last_direction = move_direction;

    stitcher_status = STITCH_STATUS_FREE;
    int point_size = MIN(frame_keypoints.size(), matched_frame_points.size());
    if (point_size < 20)
    {
        stitch_result[0] = 3; // 特征点极少，即将失败
    }
    else if (point_size < 100)
    {
        stitch_result[0] = 2; // 特征点变少，放慢速度
    }
    failed_count = 0; // 当前帧拼接成功的话重置拼接失败的次数
    return stitch_result;
}

inline void stitch_big_image(Mat frame, vector<float> angles, int direction, int big_frame_index)
{
    double start, finish;
    // 如果前面某张图片拼接大图的时候失败了，后续也不用继续拼了
    if (big_stitch_status == STITCH_STATUS_FAILED)
    {
        //        LOGE("sub thread: stitch failed %d", big_frame_index);
        big_frame_num++;
        return;
    }
    try
    {
        float resize_scale = image_resize(frame, frame, BIG_STITCH_HEIGHT, 1);
        rotate(frame, frame, 0);
        // 只有在按照固定方向拼接的时候才进行柱面投影，否则接缝会太多不好看，
        // 角度变化太大的话对输入图片加入柱面投影以确保拼接图不会变形太大
        float angle1 = (angles[0] + angles[3]) / 2, angle2 = (angles[1] + angles[2]) / 2;
        if (angle1 < 80)
        {
            cylindrical_projection(frame, frame, angle1);
        }
        else if (angle2 < 80)
        {
            cylindrical_projection(frame, frame, angle2);
        }
        Mat scaled_frame;
        big_work_scale = image_resize(frame, scaled_frame, BIG_FEATURE_HEIGHT, 0);
        vector<KeyPoint> keypoints;
        Mat descriptors;
        stitch_feature_finder->detectAndCompute(scaled_frame, Mat(), keypoints, descriptors);

        // test
        string _img_path = big_local_path + "big_frame" + to_string(big_frame_index) + ".jpg";
        bool status = imwrite(_img_path, scaled_frame);
        test_image_path = _img_path;

        start = static_cast<double>(getTickCount());
        while (big_stitch_status == STITCH_STATUS_STITCHING)
        {
            finish = static_cast<double>(getTickCount());
            if ((finish - start) / getTickFrequency() > 10)
            {
                //                LOGE("sub thread: failed cause stitching long time %d", big_frame_index);
                big_frame_num++;
                big_stitch_status = STITCH_STATUS_FAILED;
                return;
            }
            usleep(10000); // 等待10ms, 其他线程正在拼接，等待其他线程结束
        }
        start = static_cast<double>(getTickCount());
        while (big_frame_index != big_frame_num + 1)
        {
            finish = static_cast<double>(getTickCount());
            if ((finish - start) / getTickFrequency() > 10)
            {
                //                LOGE("sub thread: failed cause wait long time %d", big_frame_index);
                big_frame_num++;
                big_stitch_status = STITCH_STATUS_FAILED;
                return;
            }
            usleep(10000); // 等待10ms, 排队，按顺序拼
        }

        if (big_stitch_status == STITCH_STATUS_INIT) // 第一张图
        {
            big_stitch_status = STITCH_STATUS_STITCHING;
            big_stitch_image = frame;
            neighbour_homo = (Mat_<double>(3, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
            neighbour_keypoints = keypoints;
            neighbour_descriptors = descriptors;
            big_stitch_status = STITCH_STATUS_FREE;
            //            LOGI("sub thread: stitch finished %d", big_frame_index);
            big_frame_num++;
            return;
        }

        // 正式开始拼接
        big_stitch_status = STITCH_STATUS_STITCHING;
        //        LOGI("sub thread: start stitching %d", big_frame_index);
        vector<DMatch> good_matches;
        single_match(neighbour_descriptors, descriptors, good_matches, 0.2);
        int good_match_count = good_matches.size();
        if (good_match_count < 20)
        {
            //            LOGE("sub thread: stitch %d image failed, too less good match point %d", big_frame_index, good_match_count);
            big_stitch_status = STITCH_STATUS_FAILED;
            big_frame_num++;
            return;
        }
        vector<Point2f> frame_points, neighbour_points;
        Point2f neighbour_point, frame_point;
        for (int i = 0; i < good_match_count; i++)
        {
            // Get the keypoints from the good matches
            frame_point = keypoints[good_matches[i].trainIdx].pt;
            frame_point.x = frame_point.x / big_work_scale; // 把点的位置映射回需要拼接的大图上
            frame_point.y = frame_point.y / big_work_scale;
            frame_points.push_back(frame_point);

            neighbour_point = neighbour_keypoints[good_matches[i].queryIdx].pt;
            neighbour_point.x = neighbour_point.x / big_work_scale;
            neighbour_point.y = neighbour_point.y / big_work_scale;
            neighbour_points.push_back(neighbour_point);
        }
        Mat H;
        H = findHomography(frame_points, neighbour_points, RANSAC); // find current frame perspective to neighbour image homography
        if (H.empty())
        {
            //            LOGE("sub thread: stitch %d image failed, H is empty", big_frame_index);
            big_stitch_status = STITCH_STATUS_FAILED;
            big_frame_num++;
            return;
        }
        H = neighbour_homo * H;
        vector<Point2f> frame_corners(4), transpose_corners(4);
        frame_corners[0] = Point2f(0, 0);
        frame_corners[1] = Point2f(frame.cols, 0);
        frame_corners[2] = Point2f(frame.cols, frame.rows);
        frame_corners[3] = Point2f(0, frame.rows);
        perspectiveTransform(frame_corners, transpose_corners, H);
        if (!is_convex(transpose_corners))
        {
            //            LOGE("sub thread: stitch %d image failed, calced corners is not convex", big_frame_index);
            big_stitch_status = STITCH_STATUS_FAILED;
            big_frame_num++;
            return;
        }

        float left_x, right_x, top_y, bottom_y;
        left_x = min_axis(vector<float>{transpose_corners[0].x, transpose_corners[1].x, transpose_corners[2].x, transpose_corners[3].x});
        top_y = min_axis(vector<float>{transpose_corners[0].y, transpose_corners[1].y, transpose_corners[2].y, transpose_corners[3].y});

        float base_shift_x = (left_x < 0) ? -left_x : 0;
        float base_shift_y = (top_y < 0) ? -top_y : 0;

        Mat shift = (Mat_<double>(3, 3) << 1.0, 0.0, base_shift_x, 0.0, 1.0, base_shift_y, 0.0, 0.0, 1.0);
        H = shift * H;
        perspectiveTransform(frame_corners, transpose_corners, H);

        left_x = min_axis(vector<float>{transpose_corners[0].x, transpose_corners[1].x, transpose_corners[2].x, transpose_corners[3].x});
        right_x = max_axis(vector<float>{transpose_corners[0].x, transpose_corners[1].x, transpose_corners[2].x, transpose_corners[3].x});
        bottom_y = max_axis(vector<float>{transpose_corners[0].y, transpose_corners[1].y, transpose_corners[2].y, transpose_corners[3].y});
        top_y = min_axis(vector<float>{transpose_corners[0].y, transpose_corners[1].y, transpose_corners[2].y, transpose_corners[3].y});

        float stitch_width = MAX(right_x, big_stitch_image.cols + base_shift_x);
        float stitch_height = MAX(bottom_y, big_stitch_image.rows + base_shift_y);

        if (stitch_width > BIG_MAX_SIZE || stitch_height > BIG_MAX_SIZE)
        {
            //            LOGE("sub thread: stitch %d image failed, TOO BIG", big_frame_index);
            big_stitch_status = STITCH_STATUS_FAILED;
            big_frame_num++;
            return;
        }

        float warp_width = right_x - left_x;
        float warp_height = bottom_y - top_y;

        Mat stitch_image(stitch_height, stitch_width, CV_8UC3, Scalar(0));
        // 把上一张拼接图贴到对应的区域
        big_stitch_image.copyTo(stitch_image(Rect(base_shift_x, base_shift_y, big_stitch_image.cols, big_stitch_image.rows)));

        // 截取出上一张拼接图在当前图片映射的区域的矩形，获取它在这个矩形内的轮廓
        Mat shift_stitch_image = stitch_image(Rect(left_x, top_y, warp_width, warp_height));
        Mat tmp_gray, tmp_binary;
        cvtColor(shift_stitch_image, tmp_gray, COLOR_BGR2GRAY);
        threshold(tmp_gray, tmp_binary, 1, 255, THRESH_BINARY);
        tmp_gray.release();
        vector<vector<Point>> contours;
        vector<Vec4i> hierarchy;
        findContours(tmp_binary, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_NONE, Point());
        tmp_binary.release();
        // 只留下最大的那个contours
        int _max_count = 0;
        int _max_idx = 0;
        for (int _i = 0; _i < contours.size(); _i++)
        {
            int point_num = contours[_i].size();
            if (point_num > _max_count)
            {
                _max_count = point_num;
                _max_idx = _i;
            }
        }
        contours = vector<vector<Point>>{contours[_max_idx]};

        // 对图片映射的区域扣掉上与一张图重合的部分后再直接相加融合，然后再处理接缝
        Mat shift_frame_warp;
        Mat warpH = H.clone();
        Mat warp_shift = (Mat_<double>(3, 3) << 1.0, 0.0, -left_x, 0.0, 1.0, -top_y, 0.0, 0.0, 1.0);
        warpH = warp_shift * warpH;
        warpPerspective(frame, shift_frame_warp, warpH, Size(warp_width, warp_height));
        Mat tmp_shifted_frame_warp = shift_frame_warp.clone();
        fillPoly(shift_frame_warp, contours, Scalar(0));
        shift_stitch_image = shift_stitch_image + shift_frame_warp;
        fill_contours(contours, tmp_shifted_frame_warp, shift_stitch_image);

        // 拼接好的图贴到对应的矩形区域
        shift_stitch_image.copyTo(stitch_image(Rect(left_x, top_y, warp_width, warp_height)));

        // 更新数据用于下次拼接
        big_stitch_image = stitch_image;
        neighbour_homo = H;
        neighbour_keypoints = keypoints;
        neighbour_descriptors = descriptors;
        big_stitch_status = STITCH_STATUS_FREE;
        big_frame_num++;
    }
    catch (const std::exception &e)
    {
        // 如果遇到了未捕获的异常，打印失败的信息之后直接返回失败。所有失败的情况尽可能在上面的代码里都处理掉，不要在这里处理
        //        LOGE("sub thread: when stitching %d big image encounter an error: %s", big_frame_index, e.what());
        big_stitch_status = STITCH_STATUS_FAILED;
        big_frame_num++;
    }
}

bool LenzStitcher::get_big_image(int big_frame_index)
{
    double start = static_cast<double>(getTickCount());
    if (big_stitch_status == STITCH_STATUS_FAILED)
    {
        return false;
    }
    double finish;
    while (big_frame_index > big_frame_num)
    {
        finish = static_cast<double>(getTickCount());
        if ((finish - start) / getTickFrequency() > 10)
        {
            break; // 等待了10秒都还没有拼完，则直接返回失败， 无论什么情况下，都不该等那么久
        }
        usleep(10000); // 还在拼接中，继续等待拼接结束
    }
    if (big_frame_index == big_frame_num && big_stitch_status == STITCH_STATUS_FREE) // 全部都拼接完成了，将最后的图片保存到本地并记录路径
    {
        string _img_path = big_local_path + "big_stitch.jpg";
        bool status = imwrite(_img_path, big_stitch_image);
        big_stitch_image_path = _img_path;
    }
    //    LOGI("finish stitching big image...");
    return true;
}

inline bool is_good_green_box(const vector<Point2f> &corners, const Mat &cpu_frame, float area_thresh, float angle_thresh)
{
    vector<Point> contour;
    for (int p_idx = 0; p_idx < corners.size(); p_idx++)
        contour.push_back(Point((int)corners[p_idx].x, (int)corners[p_idx].y));

    if (!isContourConvex(contour))
    {
        //        LOGE("check: the frame is not convex!");
        return false;
    }

    float angle = calc_hangle(corners);
    // LOGI("check: the perspective angle is %lf", angle);
    if (angle > angle_thresh)
    {
        //        LOGE("check: the perspective angle is %lf, too large!", angle);
        return false;
    }

    float frame_area = 1.0 * cpu_frame.cols * cpu_frame.rows;
    float green_box_area;
    Mat mask;
    mask = Mat::zeros(cpu_frame.size(), CV_8UC1);
    fillPoly(mask, vector<vector<Point>>{contour}, Scalar(1));
    green_box_area = countNonZero(mask);
    float area_p = green_box_area / frame_area;
    //    LOGI("check: the green box area p is %f", area_p);
    if (area_p < area_thresh)
    {
        //        LOGE("check: the frame is too small!");
        return false;
    }

    return true;
}

inline float calc_area(const vector<Point2f> &corners, const Mat &frame)
{
    vector<Point> contour;
    for (int p_idx = 0; p_idx < corners.size(); p_idx++)
        contour.push_back(Point(corners[p_idx].x, corners[p_idx].y));
    if (!isContourConvex(contour))
    {
        //        LOGE("check: the frame is not convex!");
        return 0;
    }
    float frame_area = 1.0 * frame.cols * frame.rows;
    float green_box_area;
    Mat mask;
    mask = Mat::zeros(frame.size(), CV_8UC1);
    fillPoly(mask, vector<vector<Point>>{contour}, Scalar(1));
    green_box_area = countNonZero(mask);
    float area_p = green_box_area / frame_area;
    return area_p;
}

inline bool is_convex(const vector<Point2f> &corners)
{
    vector<Point2f> contour;
    for (int p_idx = 0; p_idx < corners.size(); p_idx++)
        contour.push_back(Point2f(corners[p_idx].x, corners[p_idx].y));

    if (isContourConvex(contour))
        return true;
    return false;
}

inline float min_axis(vector<float> input_list)
{
    float min_value = input_list[0];
    for (int i = 1; i < input_list.size(); i++)
        if (min_value > input_list[i])
            min_value = input_list[i];
    return min_value;
}

inline float max_axis(vector<float> input_list)
{
    float max_value = input_list[0];
    for (int i = 1; i < input_list.size(); i++)
        if (max_value < input_list[i])
            max_value = input_list[i];
    return max_value;
}

// 计算四边形的四个角
void calc_angles(vector<Point2f> &conners)
{
    float a0 = angle_of_two_vector(conners[1], conners[3], conners[0]);
    float a1 = angle_of_two_vector(conners[2], conners[0], conners[1]);
    float a2 = angle_of_two_vector(conners[3], conners[1], conners[2]);
    float a3 = angle_of_two_vector(conners[0], conners[2], conners[3]);
//    LOGI("calced angle a0 %f, a1 %f, a2 %f a3 %f", a0, a1, a2, a3);
}

// 以pt1为M点 pt2为N点 c为O点
float angle_of_two_vector(Point2f &pt1, Point2f &pt2, Point2f &c)
{
    float theta = atan2(pt1.x - c.x, pt1.y - c.y) - atan2(pt2.x - c.x, pt2.y - c.y);
    if (theta > M_PI)
    {
        theta -= 2 * M_PI;
    }

    if (theta < -M_PI)
    {
        theta += 2 * M_PI;
    }

    theta = theta * 180.0 / M_PI;
    return theta;
}

inline float calc_hangle(const vector<cv::Point2f> &conners)
{
    float angle = 0.0f;
    float angle1 = abs(atan((conners[0].y - conners[1].y) / (conners[0].x - conners[1].x))) * 180.0 / M_PI;
    float angle2 = abs(atan((conners[2].y - conners[3].y) / (conners[2].x - conners[3].x))) * 180.0 / M_PI;
    angle = (angle2 > angle1) ? angle2 : angle1;
    return angle;
}

inline float calc_vangle(const vector<cv::Point2f> &conners)
{
    float angle = 0.0f;
    float angle1 = abs(atan((conners[0].y - conners[3].y) / (conners[0].x - conners[3].x))) * 180.0 / M_PI;
    float angle2 = abs(atan((conners[1].y - conners[2].y) / (conners[1].x - conners[2].x))) * 180.0 / M_PI;
    //    LOGI("ofchcek: vertical angle1 %f, angle2 %f", angle1, angle2);
    angle = (angle2 > angle1) ? angle2 : angle1;
    return 90 - angle;
}

float calc_angle(const vector<cv::Point2f> &conners)
{
    float angle = 0.0f;
    float angle1 = abs(atan((conners[0].y - conners[1].y) / (conners[0].x - conners[1].x))) * 180.0 / M_PI;
    float angle2 = abs(atan((conners[2].y - conners[3].y) / (conners[2].x - conners[3].x))) * 180.0 / M_PI;
    // LOGI("ofchcek: horizontal angle1 %f, angle2 %f", angle1, angle2);
    angle = (angle2 > angle1) ? angle2 : angle1;

    angle1 = abs(atan((conners[0].y - conners[3].y) / (conners[0].x - conners[3].x))) * 180.0 / M_PI;
    angle2 = abs(atan((conners[1].y - conners[2].y) / (conners[1].x - conners[2].x))) * 180.0 / M_PI;
    // LOGI("ofchcek: vertical angle1 %f, angle2 %f", angle1, angle2);
    return angle;
}

inline void double_match(const Mat &query_des, const Mat &train_des, vector<DMatch> &good_matches, float check_match_conf)
{
    // BFMatcher matcher(NORM_L2);
    FlannBasedMatcher matcher;
    set<pair<int, int>> match_indices;
    vector<vector<DMatch>> matches;
    matcher.knnMatch(query_des, train_des, matches, 2);
    for (size_t i = 0; i < matches.size(); ++i)
    {
        if (matches[i].size() < 2)
            continue;
        const DMatch &m0 = matches[i][0];
        const DMatch &m1 = matches[i][1];
        if (m0.distance < (1 - check_match_conf) * m1.distance)
        {
            good_matches.push_back(m0);
            match_indices.insert(make_pair(m0.queryIdx, m0.trainIdx));
        }
    }
    matches.clear();
    matcher.knnMatch(train_des, query_des, matches, 2);
    for (size_t i = 0; i < matches.size(); ++i)
    {
        if (matches[i].size() < 2)
            continue;
        const DMatch &m0 = matches[i][0];
        const DMatch &m1 = matches[i][1];
        if (m0.distance < (1 - check_match_conf) * m1.distance)
            if (match_indices.find(make_pair(m0.trainIdx, m0.queryIdx)) == match_indices.end())
                good_matches.push_back(DMatch(m0.trainIdx, m0.queryIdx, m0.distance));
    }
}

inline void single_match(const Mat &query_des, const Mat &train_des, vector<DMatch> &good_matches, float check_match_conf)
{
    // BFMatcher matcher(NORM_L2);
    FlannBasedMatcher matcher;
    vector<vector<DMatch>> matches;
    matcher.knnMatch(query_des, train_des, matches, 2);
    for (size_t i = 0; i < matches.size(); ++i)
    {
        if (matches[i].size() < 2)
            continue;
        const DMatch &m0 = matches[i][0];
        const DMatch &m1 = matches[i][1];
        if (m0.distance < (1 - check_match_conf) * m1.distance)
            good_matches.push_back(m0);
    }
}

inline void getFilePath(const char *path, const char *filename, char *filepath)
{
    strcpy(filepath, path);
    if (filepath[strlen(path) - 1] != '/')
        strcat(filepath, "/");
    strcat(filepath, filename);
}

inline void deleteFile(const char *path)
{
    DIR *dir;
    struct dirent *dirinfo;
    struct stat statbuf;
    char filepath[256] = {0};
    lstat(path, &statbuf);

    if (S_ISREG(statbuf.st_mode)) // 判断是否是常规文件
    {
        remove(path);
    }
    else if (S_ISDIR(statbuf.st_mode)) // 判断是否是目录
    {
        if ((dir = opendir(path)) == NULL)
            return;
        while ((dirinfo = readdir(dir)) != NULL)
        {
            getFilePath(path, dirinfo->d_name, filepath);
            if (strcmp(dirinfo->d_name, ".") == 0 || strcmp(dirinfo->d_name, "..") == 0) // 判断是否是特殊目录
                continue;
            deleteFile(filepath);
            rmdir(filepath);
            // LOGI("delete: remove local file %s successfully", filepath);
        }
        closedir(dir);
    }
    return;
}

inline bool isFloderexit(const char *path)
{
    DIR *dp;
    if ((dp = opendir(path)) == NULL)
    {
        return false;
    }

    closedir(dp);
    return true;
}

// 为了避免光流算法耗时太长，限定特征点的最大的个数
void randomSelectKeyPoints(vector<KeyPoint> kpts, vector<Point2f> &pts, int max_num)
{
    random_device rd;
    mt19937 rng(rd());
    shuffle(kpts.begin(), kpts.end(), rng);
    for (int i = 0; i < kpts.size(); i++)
    {
        if (i >= max_num)
            continue;
        pts.push_back(kpts[i].pt);
    }
    return;
}

// 优化两图的接缝处，使得拼接自然
inline void optimizeSeam(Mat &last_img, Mat &stitched_image, vector<Point2f> &transpose_corners)
{
    int thresh = 10;
    // 获取4个映射到大拼接图后的顶点
    int left_x = (int)min_axis(vector<float>{transpose_corners[0].x, transpose_corners[1].x, transpose_corners[2].x, transpose_corners[3].x});
    int right_x = (int)max_axis(vector<float>{transpose_corners[0].x, transpose_corners[1].x, transpose_corners[2].x, transpose_corners[3].x});
    int bottom_y = (int)max_axis(vector<float>{transpose_corners[0].y, transpose_corners[1].y, transpose_corners[2].y, transpose_corners[3].y});
    int top_y = (int)min_axis(vector<float>{transpose_corners[0].y, transpose_corners[1].y, transpose_corners[2].y, transpose_corners[3].y});

    LinePara lp0, lp1, lp2, lp3;
    float k20, k21, k22, k23;
    getLinePara(transpose_corners[0].x, transpose_corners[0].y, transpose_corners[1].x, transpose_corners[1].y, lp0);
    k20 = sqrt(lp0.k * lp0.k + 1);
    getLinePara(transpose_corners[1].x, transpose_corners[1].y, transpose_corners[2].x, transpose_corners[2].y, lp1);
    k21 = sqrt(lp1.k * lp1.k + 1);
    getLinePara(transpose_corners[2].x, transpose_corners[2].y, transpose_corners[3].x, transpose_corners[3].y, lp2);
    k22 = sqrt(lp2.k * lp2.k + 1);
    getLinePara(transpose_corners[3].x, transpose_corners[3].y, transpose_corners[0].x, transpose_corners[0].y, lp3);
    k23 = sqrt(lp3.k * lp3.k + 1);

    double processWidth = right_x - left_x;  // 重叠区域的宽度
    double processHeight = bottom_y - top_y; // 重叠区域的宽度
    double alpha = 1;                        // last_img中像素的权重
    for (int i = top_y; i < bottom_y; i++)
    {
        for (int j = left_x; j < right_x; j++)
        {
            if (last_img.at<Vec3b>(i, j)[0] == 0 && last_img.at<Vec3b>(i, j)[1] == 0 && last_img.at<Vec3b>(i, j)[2] == 0)
            {
                continue;
            }
            // 如果拼接图的该区域内遇到黑点，则完全拷贝上一张拼接图上的点
            else if (stitched_image.at<Vec3b>(i, j)[0] == 0 && stitched_image.at<Vec3b>(i, j)[1] == 0 && stitched_image.at<Vec3b>(i, j)[2] == 0)
            {
                stitched_image.at<Vec3b>(i, j) = last_img.at<Vec3b>(i, j);
            }
            else if (point_2_line(lp0.k, lp0.b, k20, j, i) < thresh) // 如果点在线的附近
            {
                stitched_image.at<Vec3b>(i, j) = last_img.at<Vec3b>(i, j);
            }
            else if (point_2_line(lp1.k, lp1.b, k21, j, i) < thresh) // 如果点在线的附近
            {
                stitched_image.at<Vec3b>(i, j) = last_img.at<Vec3b>(i, j);
            }
            else if (point_2_line(lp2.k, lp2.b, k22, j, i) < thresh) // 如果点在线的附近
            {
                stitched_image.at<Vec3b>(i, j) = last_img.at<Vec3b>(i, j);
            }
            else if (point_2_line(lp3.k, lp3.b, k23, j, i) < thresh) // 如果点在线的附近
            {
                stitched_image.at<Vec3b>(i, j) = last_img.at<Vec3b>(i, j);
            }
        }
    }
}

// 把边缘处的点给补全，免得有太多的接缝黑边
void fill_contours(vector<vector<Point>> target_contours, Mat &source_image, Mat &target_image)
{
    int h = source_image.rows, w = source_image.cols;
    // 对source_image求轮廓，并生成mask
    Mat source_gray, target_gray;
    cvtColor(source_image, source_gray, COLOR_BGR2GRAY);
    cvtColor(target_image, target_gray, COLOR_BGR2GRAY);
    Mat tmp_binary;
    threshold(source_gray, tmp_binary, 1, 255, THRESH_BINARY);
    vector<vector<Point>> source_contours;
    vector<Vec4i> hierarchy;
    findContours(tmp_binary, source_contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_NONE, Point());
    tmp_binary.release();
    // 只留下最大的那个contours
    int _max_count = 0;
    int _max_idx = 0;
    for (int _i = 0; _i < source_contours.size(); _i++)
    {
        int point_num = source_contours[_i].size();
        if (point_num > _max_count)
        {
            _max_count = point_num;
            _max_idx = _i;
        }
    }
    source_contours = vector<vector<Point>>{source_contours[_max_idx]};
    Mat mask = Mat(h, w, CV_8UC1, Scalar(0));
    fillPoly(mask, source_contours, Scalar(1));

    vector<Point> contour = target_contours[0];
    int range = 30;
    float weight;
    int total_num = contour.size();
    int line_direction = 0; // 判断当前点所在的直线的方向，0水平或者1垂直
    int direction = 0;      // 当前点和下一个点连成的线的方向，1从左向右,2从右向左,3从上到下,4从下到上

    for (int j = 0; j < total_num; j++)
    {
        int x = contour[j].x;
        int y = contour[j].y;

        if (x == 0 || y == 0 || x == w - 1 || y == h - 1 || mask.at<uchar>(y, x) == 0)
            continue;
        target_image.at<Vec3b>(y, x) = source_image.at<Vec3b>(y, x); // 填充边缘
        int d1 = 0, d2 = 0, d3 = 0, d4 = 0;                          // 当前点在4个方向上的差异值的总和
        // 先判断是水平还是垂直方向
        int dx = 0, dy = 0;
        for (int i = 0; i < 3; i++)
        {
            if (j - i >= 0)
            {
                dx += abs(x - contour[j - i].x);
                dy += abs(y - contour[j - i].y);
            }
            if (j + i < total_num)
            {
                dx += abs(x - contour[j + i].x);
                dy += abs(y - contour[j + i].y);
            }
        }
        if (dx < dy)
            line_direction = 0; // 水平
        if (dx > dy)
            line_direction = 1; // 垂直
        for (int i = 1; i < 3; i++)
        { // 各个方向找三个点，看差值是多少，来判断需要融合的方向
            if (line_direction == 0)
            {
                // 从左向右
                if (x + i < h)
                    d1 += abs(target_gray.at<uchar>(y, x + i) - source_gray.at<uchar>(y, x + i));

                // 从右向左
                if (x - i >= 0)
                    d2 += abs(target_gray.at<uchar>(y, x - i) - source_gray.at<uchar>(y, x - i));
            }
            else
            {
                // 从上到下
                if (y + i < h)
                    d3 += abs(target_gray.at<uchar>(y + i, x) - source_gray.at<uchar>(y + i, x));

                // 从下到上
                if (y - i >= 0)
                    d4 += abs(target_gray.at<uchar>(y - i, x) - source_gray.at<uchar>(y - i, x));
            }
        }

        if (d1 > d2 && d1 > d3 && d1 > d4)
            direction = 1;
        else if (d2 > d1 && d2 > d3 && d2 > d4)
            direction = 2;
        else if (d3 > d1 && d3 > d2 && d3 > d4)
            direction = 3;
        else if (d4 > d1 && d4 > d2 && d4 > d3)
            direction = 4;

        for (int i = 1; i < range; i++)
        {
            weight = 1.0 * i / range;
            if (direction == 1)
            { // 从左到右
                if (x + i < w && source_gray.at<uchar>(y, x + i) > 1)
                {
                    target_image.at<Vec3b>(y, x + i)[0] = source_image.at<Vec3b>(y, x + i)[0] * (1 - weight) + target_image.at<Vec3b>(y, x + i)[0] * weight;
                    target_image.at<Vec3b>(y, x + i)[1] = source_image.at<Vec3b>(y, x + i)[1] * (1 - weight) + target_image.at<Vec3b>(y, x + i)[1] * weight;
                    target_image.at<Vec3b>(y, x + i)[2] = source_image.at<Vec3b>(y, x + i)[2] * (1 - weight) + target_image.at<Vec3b>(y, x + i)[2] * weight;
                }
            }
            else if (direction == 2)
            { // 从右到左
                if (x - i >= 0 && source_gray.at<uchar>(y, x - i) > 1)
                {
                    target_image.at<Vec3b>(y, x - i)[0] = source_image.at<Vec3b>(y, x - i)[0] * (1 - weight) + target_image.at<Vec3b>(y, x - i)[0] * weight;
                    target_image.at<Vec3b>(y, x - i)[1] = source_image.at<Vec3b>(y, x - i)[1] * (1 - weight) + target_image.at<Vec3b>(y, x - i)[1] * weight;
                    target_image.at<Vec3b>(y, x - i)[2] = source_image.at<Vec3b>(y, x - i)[2] * (1 - weight) + target_image.at<Vec3b>(y, x - i)[2] * weight;
                }
            }
            else if (direction == 3)
            { // 从上到下
                if (y + i < h && source_gray.at<uchar>(y + i, x) > 1)
                {
                    target_image.at<Vec3b>(y + i, x)[0] = source_image.at<Vec3b>(y + i, x)[0] * (1 - weight) + target_image.at<Vec3b>(y + i, x)[0] * weight;
                    target_image.at<Vec3b>(y + i, x)[1] = source_image.at<Vec3b>(y + i, x)[1] * (1 - weight) + target_image.at<Vec3b>(y + i, x)[1] * weight;
                    target_image.at<Vec3b>(y + i, x)[2] = source_image.at<Vec3b>(y + i, x)[2] * (1 - weight) + target_image.at<Vec3b>(y + i, x)[2] * weight;
                }
            }
            else if (direction == 4)
            { // 从下到上
                if (y - i >= 0 && source_gray.at<uchar>(y - i, x) > 1)
                {
                    target_image.at<Vec3b>(y - i, x)[0] = source_image.at<Vec3b>(y - i, x)[0] * (1 - weight) + target_image.at<Vec3b>(y - i, x)[0] * weight;
                    target_image.at<Vec3b>(y - i, x)[1] = source_image.at<Vec3b>(y - i, x)[1] * (1 - weight) + target_image.at<Vec3b>(y - i, x)[1] * weight;
                    target_image.at<Vec3b>(y - i, x)[2] = source_image.at<Vec3b>(y - i, x)[2] * (1 - weight) + target_image.at<Vec3b>(y - i, x)[2] * weight;
                }
            }
        }
    }
}

inline float point_2_line(float k, float b, float k2, float x, float y)
{
    float distance = abs(k * x + b - y) / k2;
    return distance;
}

// 获取直线参数
inline void getLinePara(float x1, float y1, float x2, float y2, LinePara &LP)
{
    double m = 0;

    // 计算分子
    m = x2 - x1;

    if (0 == m)
    {
        LP.k = 10000.0;
        LP.b = y1 - LP.k * x1;
    }
    else
    {
        LP.k = (y2 - y1) / (x2 - x1);
        LP.b = y1 - LP.k * x1;
    }
}

// 1从上到下，2从下到上，3从左到右，4从右到左
int get_direction(vector<Point2f> &frame_corners, vector<Point2f> &transpose_corners)
{
    float dx = ((transpose_corners[0].x - frame_corners[0].x) +
                (transpose_corners[1].x - frame_corners[1].x) +
                (transpose_corners[2].x - frame_corners[2].x) +
                (transpose_corners[3].x - frame_corners[3].x)) /
               4;
    float dy = ((transpose_corners[0].y - frame_corners[0].y) +
                (transpose_corners[1].y - frame_corners[1].y) +
                (transpose_corners[2].y - frame_corners[2].y) +
                (transpose_corners[3].y - frame_corners[3].y)) /
               4;
    int direction;
    if (abs(dx) > abs(dy))
    {
        if (dx > 0)
            direction = 3;
        else
            direction = 4;
    }
    else
    {
        if (dy > 0)
            direction = 1;
        else
            direction = 2;
    }
    return direction;
}

inline int dcmp(float x)
{ // 精度误差比较
    if (x > 1e-14)
        return 1;
    return x < -1e-14 ? -1 : 0;
}

float cross(Point2f a, Point2f b, Point2f c) { return (a.x - c.x) * (b.y - c.y) - (b.x - c.x) * (a.y - c.y); } // 叉积

inline float CPIA(Point2f a[], Point2f b[], int na, int nb)
{                                   // 传入两个三角形，求相交部分的凸包
    vector<Point2f> p(20), tmp(20); // 复制点集与临时点集（P其实可以用B来做
    int tn, sflag, eflag;           // 每轮相交凸包的点，叉乘符号
    a[na] = a[0], b[nb] = b[0];     // 末点用初点复制方便首末点连边
    for (int k = 0; k <= nb; k++)
    {
        p[k] = b[k];
    }
    for (int i = 0; i < na && nb > 2; i++)
    {                                              // 扫一次A
        sflag = dcmp(cross(a[i + 1], p[0], a[i])); // 取A两点与B第一点求叉乘符号
        for (int j = tn = 0; j < nb; j++, sflag = eflag)
        { // 扫一次B，更新TMP，TN是点数
            if (sflag >= 0)
                tmp[tn++] = p[j];                          // 叉乘为正就是B数组的那个点压入
            eflag = dcmp(cross(a[i + 1], p[j + 1], a[i])); // 求叉乘符号
            if ((sflag ^ eflag) == -2)
                tmp[tn++] = get_intersection_point(a[i], a[i + 1], p[j], p[j + 1]); // 求交点
        }
        for (int k = 0; k < tn; k++)
        {
            p[k] = tmp[k];
        }
        nb = tn, p[nb] = p[0]; // TN即TMP点数记到NB
    }                          // 其实该是NP表示P数组个数，这里省了个变量就用NB表示，下面第二行做参数而已
    if (nb < 3)
        return 0.f;            // 相交部分凸包不够三个点，面积就是0
    return PolygonArea(p, nb); // 求出相交凸包部分的面积
}

inline float SPIA(vector<Point2f> a, vector<Point2f> b, int na, int nb)
{                                               // 传入两个多边形的点
    int i, j;                                   // 循环变量
    Point2f t1[4], t2[4];                       // 其实T13与T23没用上
    double num1, num2;                          // 叉乘符号
    float res = 0.f;                            // 答案初始化
    a[na] = t1[0] = a[0], b[nb] = t2[0] = b[0]; // 初始化T1,T2和ANA,BNB
    for (i = 2; i < na; i++)
    {                                            // 扫一次第一个多边形全部点
        t1[1] = a[i - 1], t1[2] = a[i];          // 每次在第一个多边形取两个点赋给T11,T12
        num1 = dcmp(cross(t1[1], t1[2], t1[0])); // 求出叉乘符号
        if (num1 < 0)
        {
            t1[1] = a[i], t1[2] = a[i - 1];
        }
        for (j = 2; j < nb; j++)
        {                                            // 扫一次第二个多边形全部点
            t2[1] = b[j - 1], t2[2] = b[j];          // 然后再在第二个多边形取两个点赋给T21,T22
            num2 = dcmp(cross(t2[1], t2[2], t2[0])); // 求出叉乘符号
            if (num2 < 0)
            {
                t2[1] = b[j], t2[2] = b[j - 1];
            }
            res += CPIA(t1, t2, 3, 3); // 累加相交部分面积
        }
    }
    return res;
}

Point2f get_intersection_point(Point2f a, Point2f b, Point2f c, Point2f d)
{ // 传入四点即两直线，输出交点
    Point2f p = a;
    float t = ((a.x - c.x) * (c.y - d.y) - (a.y - c.y) * (c.x - d.x)) / ((a.x - b.x) * (c.y - d.y) - (a.y - b.y) * (c.x - d.x));
    p.x += (b.x - a.x) * t;
    p.y += (b.y - a.y) * t;
    return p; // 输出交点，证明用数学方法易证
}

inline float PolygonArea(vector<Point2f> p, int n)
{ // 计算多边形面积，三角剖分
    if (n < 3)
        return 0.0;
    if (n >= 24)
        return 0.0;
    float s = p[0].y * (p[n - 1].x - p[1].x);
    p[n] = p[0];
    for (int i = 1; i < n; ++i)
        s += p[i].y * (p[i - 1].x - p[i + 1].x);
    return fabs(s * 0.5); // 叉乘出来是平行四边形面积故/2，且顺逆方向不定，故取ABS变正
}

inline float single_quadrangle_iou(vector<Point2f> pts1, vector<Point2f> pts2)
{
    const float area1 = PolygonArea(pts1, 4);
    const float area2 = PolygonArea(pts2, 4);

    if (area1 < 1e-14 || area2 < 1e-14)
    {
        return 0.f;
    }

    const float intersection = SPIA(pts1, pts2, 4, 4);

    const float iou = intersection / (area1 + area2 - intersection);
    return iou;
}

inline float single_box_iou(vector<Point2f> pts1, vector<Point2f> pts2)
{
    int xmin1 = (int)min_axis(vector<float>{pts1[0].x, pts1[1].x, pts1[2].x, pts1[3].x});
    int xmax1 = (int)max_axis(vector<float>{pts1[0].x, pts1[1].x, pts1[2].x, pts1[3].x});
    int ymax1 = (int)max_axis(vector<float>{pts1[0].y, pts1[1].y, pts1[2].y, pts1[3].y});
    int ymin1 = (int)min_axis(vector<float>{pts1[0].y, pts1[1].y, pts1[2].y, pts1[3].y});
    float area1 = (float)(xmax1 - xmin1) * (ymax1 - ymin1);

    int xmin2 = (int)min_axis(vector<float>{pts2[0].x, pts2[1].x, pts2[2].x, pts2[3].x});
    int xmax2 = (int)max_axis(vector<float>{pts2[0].x, pts2[1].x, pts2[2].x, pts2[3].x});
    int ymax2 = (int)max_axis(vector<float>{pts2[0].y, pts2[1].y, pts2[2].y, pts2[3].y});
    int ymin2 = (int)min_axis(vector<float>{pts2[0].y, pts2[1].y, pts2[2].y, pts2[3].y});
    float area2 = (float)(xmax2 - xmin2) * (ymax2 - ymin2);

    int inter_xmin = MAX(xmin1, xmin2);
    int inter_xmax = MIN(xmax1, xmax2);
    int inter_ymin = MAX(ymin1, ymin2);
    int inter_ymax = MIN(ymax1, ymax2);

    float inter_area = (float)(inter_xmax - inter_xmin) * (inter_ymax - inter_ymin);
    float union_area = (area1 + area2 - inter_area);
    if (union_area < 1e-14)
    {
        return 0.f;
    }

    float iou = 1.0 * inter_area / union_area;

    return iou;
}

// 柱面投影
void cylindrical_projection(Mat &img, Mat &output, float angle)
{
    float w = img.cols;
    float h = img.rows;
    Mat map_img = Mat(h, w, CV_8UC3, Scalar(0));
    float f = (w / (90 - angle) * 5) / atan(M_PI / 15);
    float cx = w / 2.0f;
    float cy = h / 2.0f;
    int max_col = 0;
    for (int i = 0; i < img.rows; i++)
    {
        for (int j = 0; j < img.cols; j++)
        {
            float x = j;
            float y = i;
            float x1 = f * atan((x - cx) / f) + f * atan(cx / f);
            float y1 = f * (y - cy) / sqrt((x - cx) * (x - cx) + f * f) + cy;

            int col = (int)(x1 + 0.5f); // 加0.5是为了四舍五入
            int row = (int)(y1 + 0.5f); // 加0.5是为了四舍五入
            max_col = col > max_col ? col : max_col;
            if (col < img.cols && row < img.rows)
            {
                map_img.at<Vec3b>(row, col)[0] = img.at<Vec3b>(i, j)[0];
                map_img.at<Vec3b>(row, col)[1] = img.at<Vec3b>(i, j)[1];
                map_img.at<Vec3b>(row, col)[2] = img.at<Vec3b>(i, j)[2];
            }
        }
    }
    output = map_img(Rect(0, 0, max_col, h));
}