ceres优化单应性矩阵

#include<opencv2/opencv.hpp>
#include<iostream>
#include<opencv2/xfeatures2d.hpp>
#include<vector>
#include <ceres/ceres.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <glog/logging.h>
#include<opencv2/core/eigen.hpp>
using namespace std;
using namespace cv;
using namespace cv::xfeatures2d;
using namespace Eigen;

//全局变量
Mat src_img = imread("../Tulips.jpg", 1);
Mat frame,gray,prev_frame,mask;
vector<Point2f> one_frame_point4;//第一张图片四点
vector<Point> next_frame_point4;
Point point_one;
vector<uchar> status;
vector<float> errors;
Mat homography_H;
// sift特征变量
Mat description_one;
Mat description_two;
vector<KeyPoint> keyPoint_one;
vector<Point2f> fpts[2];
vector<Point2f> src_points_4;
//ceres优化单应性矩阵中全局变量的说明
//Eigen模板类名<类型，行，列> 实例化类名；
typedef Eigen::Matrix<double ,3,3> E_Mat33; //3*3 double类型的矩阵，用来存储单应性矩阵H
typedef Eigen::Matrix<double ,2,1> E_Mat21;  //2*1 double类型的矩阵，用来存储单个数据点
typedef Eigen::MatrixXd E_Matxx;  //多行，多列的double类型的矩阵，用来存储用到的所有点

//设置优化迭代次数和误差终止值
struct EstimateHomographyOptions{
    EstimateHomographyOptions():max_num_iterations(50),expected_average_symmetric_distance(1e-16){}
    int max_num_iterations;
    double expected_average_symmetric_distance;
};
//代价函数计算函数
template <typename T>
void SymmetricGeometricDistanceTerms(
        const Eigen::Matrix<T,3,3> &H, //单应性矩阵变量
        const Eigen::Matrix<T,2,1> &x1, //前一帧匹配点变量
        const Eigen::Matrix<T,2,1> &x2, //当前帧匹配点变量
        T &residual）//代价函数变量
{
    typedef Eigen::Matrix<T,3,1> E_Mat31_T;//定义齐次坐标类型
    T forward_error[2];
    E_Mat31_T x(x1(0),x1(1),T(1.0)); //将前一帧匹配点的像素坐标转为齐次坐标
    E_Mat31_T y(x2(0),x2(1),T(1.0)); //将当前帧匹配点的像素坐标转为齐次坐标
    E_Mat31_T H_x = H*x; //计算前一帧匹配点在当前帧中的重投影坐标
    H_x/=H_x(2);  //重投影坐标归一化
    forward_error[0]=H_x(0)-y(0);  //计算重投影的x方向误差
    forward_error[1]=H_x(1)-y(1);  //计算重投影的y方向误差
    //计算重投影误差的二范数作为代价函数误差
    residual=forward_error[0]*forward_error[0]+forward_error[1]*forward_error[1];
};

//构建代价函数结构
class HomographySymmetricGeometricCostFunctor{
public:
    HomographySymmetricGeometricCostFunctor(const E_Mat21 &x,const E_Mat21 &y)
            :_x(x),_y(y){} //x,y是传入代价函数的每对匹配点
    //重定义()运算符号并求代价函数
    template <typename T>  
    //homography_parameters是优化变量，此例子是单应性矩阵H,residuals是代价函数误差值都是一维向量
    bool operator()(const T*homography_parameters,T*residuals)const{
        typedef Eigen::Matrix<T,3,3> E_Mat33_T;//Eigen下的3*3double矩阵
        typedef Eigen::Matrix<T,2,1> E_Mat21_T;//Eigen下的2*1double矩阵
        E_Mat33_T cH(homography_parameters);//将data指针的一维向量转为矩阵类型．
        E_Mat21_T cx(T(_x(0)),T(_x(1)));//将opencv下的前一帧匹配点坐标转为结构中需要用的类型
        E_Mat21_T cy(T(_y(0)),T(_y(1)));//将opencv下的当前一帧匹配点坐标转为结构中需要用的类型
        SymmetricGeometricDistanceTerms<T>(cH,cx,cy,residuals[0]);//调用代价函数求解公式
        return true;
    }
    const E_Mat21 _x;
    const E_Mat21 _y;
};

//ｃｅｒｅｓ进行优化

//ceres添加数据和代价函数
bool EstimateHomography2DFromCorrespondences(
        const E_Matxx &x1,//前一帧匹配点集
        const E_Matxx &x2,//当前帧匹配点集
        E_Mat33 &H //单应性矩阵
){
    //数据信息判断
    assert(2==x1.rows());
    assert(4<=x1.cols());
    assert(x1.rows()==x2.rows());
    assert(x1.cols()==x2.cols());
    //初始化单应性矩阵H
    cv::cv2eigen(homography_H,H);//opencv中矩阵转为Eigen中矩阵
    ceres::Problem problem; //构建最小二乘问题
    //往构建的问题中添加匹配点、核函数、单应性矩阵
    for (int i = 0; i <x1.cols() ; ++i) {
    //调用构建的代价函数结构
        HomographySymmetricGeometricCostFunctor *homography_cost_functor=
                new HomographySymmetricGeometricCostFunctor(x1.col(i),x2.col(i));
        problem.AddResidualBlock(
        //ceres自动求导求增量方程<代价函数类型，代价函数维度，优化变量维度>（代价函数）
                new AutoDiffCostFunction<HomographySymmetricGeometricCostFunctor,1,9>
                (homography_cost_functor),
                new ceres::CauchyLoss(1),//添加柯西核函数
                H.data()//添加的优化变量（矩阵转为一维向量）
        );
    }
    //ceres求解器优化配置(很多设置默认值就可以了)
    //默认最大迭代50次，误差1e-32
    ceres::Solver::Options solver_options;//实例化求解器对象
    //线性求解器的类型，用于计算Levenberg-Marquardt算法每次迭代中线性最小二乘问题的解
    solver_options.linear_solver_type=ceres::DENSE_QR;
    //记录优化过程，输出到cout位置
    solver_options.minimizer_progress_to_stdout= true;
    //ceres求解运算
    //实例化求解对象
    ceres::Solver::Summary summary;
    //求解
    ceres::Solve(solver_options,&problem,&summary);
    //输出优化信息
    cout<<"summary:n"<<summary.BriefReport();
    
    return summary.IsSolutionUsable();
}

//获取贴图图片的坐标
void get_img_points()
{

    Size src_size = src_img.size();
    src_points_4.push_back(Point2f(0, 0));
    src_points_4.push_back(Point2f(src_size.width - 1, 0));
    src_points_4.push_back(Point2f(src_size.width - 1, src_size.height - 1));
    src_points_4.push_back(Point2f(0, src_size.height - 1));
}
//鼠标交互获取mask区域和第一张图片特征点
void on_Left_Mouse(int event,int x,int y,int flags,void* param)
{
    Mat& image = *(Mat*)param;
    switch (event)
    {
        case EVENT_LBUTTONDOWN:
        {
            point_one.x = x;
            point_one.y = y;
            one_frame_point4.push_back(point_one);
            circle(image, point_one, 5, Scalar(0, 0, 255), -1, 8);
        }
            break;
    }
}
//获取特征点
void detectFeature(Mat &ingray)
{

    Ptr<SIFT> sift = xfeatures2d::SIFT::create();
    sift->detectAndCompute(ingray, mask, keyPoint_one, description_one);
    vector<Point2f> feature_one;
    Point show_point;
    for (int i = 0; i < keyPoint_one.size(); i++)
    {
        feature_one.push_back(keyPoint_one[i].pt);
    }
    fpts[0] = feature_one;
    cout << "第一张图片特征点个数" << fpts[0].size() << endl;
}

void LKTrackFeature()
{
    //光流追踪
    calcOpticalFlowPyrLK(prev_frame, gray, fpts[0], fpts[1], status, errors);
    //提取sift特征点
    Ptr<SIFT> sift = xfeatures2d::SIFT::create();
    vector<KeyPoint> keyPoint_two;
    sift->detectAndCompute(gray, mask, keyPoint_two, description_two);
    vector<DMatch> matches;
    BFMatcher matcher(NORM_L2);
    matcher.match(description_one,description_two,matches);
    double min_dist = 10000, max_dist = 0;
    for (int i = 0; i < description_one.rows; i++)
    {
        double dist = matches[i].distance;
        if (dist < min_dist) { min_dist = dist; }
        if (dist > max_dist) { max_dist = dist; }
    }
    cout << "min:" << min_dist << "t" << "max:" << max_dist << endl;
    vector<DMatch> good_match;
    for (int j = 0; j < description_one.rows; j++)
    {
        float a_dist = max(4 * min_dist, 30.0);
        if (matches[j].distance<= a_dist)
        {
            good_match.push_back(matches[j]);
        }
    }
    cout<<"good_match:"<<good_match.size()<<endl;
    int k = 0;
    for (int j = 0; j < good_match.size(); j++)
    {
        int sift_one_index =good_match[j].queryIdx;
        int sift_two_index =good_match[j].trainIdx;

        float a = fpts[1][sift_one_index].x-keyPoint_two[sift_two_index].pt.x;
        float b = fpts[1][sift_one_index].y - keyPoint_two[sift_two_index].pt.y;
        float a_b =abs(a)+abs(b);
        cout << "a+b:"<<a_b << endl;
        if ((a_b<0.1)&&(status[sift_one_index]))
        {
            fpts[0][k] = fpts[0][sift_one_index];
            fpts[1][k++] = fpts[1][sift_one_index];

        }
    }
    fpts[0].resize(k);
    fpts[1].resize(k);
    cout << fpts[0].size() << endl;
    cout << fpts[1].size() << endl;
}
int main()
{
    VideoCapture capture("../43_output.mp4");
    if (!capture.isOpened())
    {
        cout << "could not load video file..." << endl;
        return -1;
    }
    int num_frames = capture.get(CAP_PROP_FRAME_COUNT);
    double num_fps = capture.get(CAP_PROP_FPS);
    Size num_size = Size(capture.get(CAP_PROP_FRAME_WIDTH), capture.get(CAP_PROP_FRAME_HEIGHT));
    cout << "frames:" << num_frames << endl;
    cout << "fps:" << num_fps << endl;
    cout << "size:" << num_size << endl;
    VideoWriter writer;
    writer.open("../43_sift_LK_ceres.avi", VideoWriter::fourcc('M', 'J', 'P', 'G'), num_fps, num_size);
    namedWindow("video_input", 0);
    namedWindow("video_output", 0);
    //获取贴图的四个点坐标集合
    get_img_points();
    //读取视频图片进行处理
    int n = 0;
    while (capture.read(frame))
    {
        n++;
        cvtColor(frame, gray, COLOR_BGR2GRAY);
        if (prev_frame.empty())
        {
            gray.copyTo(prev_frame);
        }
        if (n==1)
        {
            Mat frame_one;
            frame.copyTo(frame_one);
            setMouseCallback("video_input",on_Left_Mouse,&frame_one);
            Mat tempImage;
            frame.copyTo(tempImage);
            while (true)
            {
                circle(tempImage,point_one,5,Scalar(0,0,255),-1,8);
                imshow("video_input", tempImage);
                if (waitKey(10) == 27)break;
            }
            mask = Mat::zeros(gray.size(),gray.type());
            vector<Point> points_mask;
            for (int i = 0; i <4 ; ++i) {
                points_mask.push_back(one_frame_point4[i]);
            }
            fillConvexPoly(mask,points_mask,Scalar(255),LINE_AA);
            detectFeature(gray);
            //贴图
            Mat warp_Homo = getPerspectiveTransform(src_points_4, one_frame_point4);
            Mat frame_clone, frame_fill;
            frame.copyTo(frame_clone);
            frame.copyTo(frame_fill);
            warpPerspective(src_img,frame_clone,warp_Homo,frame_clone.size());
            fillConvexPoly(frame_fill,points_mask,Scalar(0,0,0),LINE_AA);
            frame_fill = frame_fill + frame_clone;
            writer << frame_fill;

        }
        if (n>=2)
        {
            imshow("video_output", frame);
            LKTrackFeature();
            homography_H = findHomography(fpts[0],fpts[1],RANSAC,3,noArray(),2000,0.995);

            //ceres优化单应性矩阵
            //1数据转换
            int num = fpts[0].size();
            E_Matxx x1(2, num);
            E_Matxx x2(2, num);
            for (int i = 0; i < num; ++i) {
                x1(0, i) = static_cast<double>(fpts[0][i].x);
                x1(1, i) = static_cast<double>(fpts[0][i].y);
                x2(0, i) = static_cast<double>(fpts[1][i].x);
                x2(1, i) = static_cast<double>(fpts[1][i].y);
            }
            cout<<x1.cols()<<endl;
            E_Mat33 estimated_matrix;
            EstimateHomographyOptions options;
            options.expected_average_symmetric_distance = 0.02;
            EstimateHomography2DFromCorrespondences(x1, x2, options, estimated_matrix);
            // Normalize the matrix for easier comparison.
            estimated_matrix /= estimated_matrix(2, 2);
            cout<<endl;
            cout << "homograyphy_H:" << homography_H << endl;
            std::cout << "Estimated matrix:n" << estimated_matrix << "n";
            //cv::eigen2cv(estimated_matrix,homography_H);
            cout<<"H:"<<homography_H<<endl;
            //贴图操作
            vector<Point2f> points_tran;
            perspectiveTransform(one_frame_point4,points_tran,homography_H);
            Mat warp_Homo = getPerspectiveTransform(src_points_4, points_tran);
            Mat frame_clone, frame_fill;
            frame.copyTo(frame_clone);
            frame.copyTo(frame_fill);
            warpPerspective(src_img,frame_clone,warp_Homo,frame_clone.size());
            vector<Point> points_mask;
            for (int i = 0; i <4 ; ++i) {
                points_mask.push_back(points_tran[i]);
            }
            fillConvexPoly(frame_fill,points_mask,Scalar(0,0,0),LINE_AA);
            frame_fill = frame_fill + frame_clone;
            writer << frame_fill;

            //把第二张图片看成第一张
            mask = Mat::zeros(gray.size(), gray.type());
            fillConvexPoly(mask, points_mask ,Scalar(255), LINE_AA);
            gray.copyTo(prev_frame);
            detectFeature(prev_frame);
            for (int i = 0; i < 4; i++)
            {
                one_frame_point4[i] = points_tran[i];
            }
        }
        char c = waitKey(1);
        if (c==27)
        {
            break;
        }

    }
    waitKey(0);
    return 0;
}

最后

以上就是激情唇膏为你收集整理的ceres优化单应性矩阵的全部内容，希望文章能够帮你解决ceres优化单应性矩阵所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。