opencv---单应性矩阵分解纯旋转运动相机内外参

62 阅读 0 评论 41 点赞

我是靠谱客的博主美好大船，最近开发中收集的这篇文章主要介绍opencv---单应性矩阵分解纯旋转运动相机内外参，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

#include<opencv2/opencv.hpp>
#include <opencv2/core/hal/hal.hpp>
using namespace cv;
using namespace std;

static inline bool decomposeCholesky(double *A, size_t astep, int m) {
    if (!hal::Cholesky64f(A, astep, m, 0, 0, 0))
        return false;
    return true;
}

bool calibrateRotatingCamera(const std::vector<Mat> &Hs, Mat &K) {
    int m = static_cast<int>(Hs.size());
    CV_Assert(m >= 1);

    std::vector<Mat> Hs_(m);
    for (int i = 0; i < m; ++i) {
        CV_Assert(Hs[i].size() == Size(3, 3) && Hs[i].type() == CV_64F);
        Hs_[i] = Hs[i] / std::pow(determinant(Hs[i]), 1. / 3.);
    }

    const int idx_map[3][3] = {{0, 1, 2}, {1, 3, 4}, {2, 4, 5}};
    Mat_<double> A(6 * m, 6);
    A.setTo(0);

    int eq_idx = 0;
    for (int k = 0; k < m; ++k) {
        Mat_<double> H(Hs_[k]);
        for (int i = 0; i < 3; ++i) {
            for (int j = i; j < 3; ++j, ++eq_idx) {
                for (int l = 0; l < 3; ++l) {
                    for (int s = 0; s < 3; ++s) {
                        int idx = idx_map[l][s];
                        A(eq_idx, idx) += H(i, l) * H(j, s);
                    }
                }
                A(eq_idx, idx_map[i][j]) -= 1;
            }
        }
    }

    Mat_<double> wcoef;
    SVD::solveZ(A, wcoef);

    Mat_<double> W(3, 3);
    for (int i = 0; i < 3; ++i)
        for (int j = i; j < 3; ++j)
            W(i, j) = W(j, i) = wcoef(idx_map[i][j], 0) / wcoef(5, 0);
    if (!decomposeCholesky(W.ptr<double>(), W.step, 3))
        return false;
    W(0, 1) = W(0, 2) = W(1, 2) = 0;
    K = W.t();
    return true;
}