Opencv2.4.9源码分析——Stitching(三)

版权声明:本文为赵春江原创文章,未经本人允许不得转载。 https://blog.csdn.net/zhaocj/article/details/78809143

3、相机参数评估

3.1 原理

相机参数的评估也称为相机定标。要想理解这部分内容,首先应该从成像原理开始讲起。


图6 小孔成像原理

从图6可以看出,真实物体通过小孔映射到成像平面上,小孔到成像平面的距离称为焦距f。在成像平面上的图像是镜像倒立的,所以为了研究方便,在小孔和物体之间定义一个虚拟成像平面(在后面,我们把该平面也称为成像平面),它与小孔的距离也为焦距,则两个成像平面的图像大小是相同的,但虚拟成像平面上的图像与原物体的方向是一样的。


图7 成像的几何模型

我们以小孔为坐标原点建立一个三维直角坐标系XYZ(如图7所示),坐标原点C称为相机的光心。成像平面xy平行于XY,并且距离光心Cf,其中Z轴定义为光轴,它与成像平面xy的交点为P,因此CPf。设空间中的任一点Q的坐标为(X, Y, Z),该点映射到成像平面的点q的坐标为(x, y,f)。

由几何知识可得:

(31)

式中,λZ/f,则xfX/ZyfY/Z。因此空间中Q点的三维坐标映射到成像平面的二维坐标q点在齐次坐标下的线性映射关系为:

(32)

如图7所示,在成像平面(坐标系为xy)的坐标原点为P,但图像(设坐标系为uv)的坐标原点一般在左上角,所以这两个坐标系之间需要通过平移来进行转换。另外成像平面xy是长度单位,而相机图像传感器的单位是像素,因此像素与长度之间也是需要转换的,而且水平和垂直的像素往往是不相同的,所以横纵轴的转换系数是不一致。因此式32改写为:

(33)

式中,fufv为焦距f在横纵轴的长度和像素的转换,它们之间的关系可以写为fvαfu,(cx, cy)为坐标平移。因为矩阵K的参数都是基于相机内部自身的参数,因此K称为相机内参数,KTV

相机除了具有内参数外,还有外参数。式33中的(X, Y, Z)称为相机坐标,而它与真实的世界坐标(X’, Y’, Z’)还存在欧几里得变换,即:

(34)

式中,R为3×3的旋转矩阵,t为3×1的平移向量。式34代入式33,得

(35)

式中,[R|t]称为相机外参数,M称为投影矩阵。

如果得到的两幅图像如图3所示的那样,即相机在三角架上通过旋转得到的两幅图像,则对于同一个世界坐标上的点(X’, Y’, Z’),在两幅图像上的坐标点分别为(u0, v0)和(u1, v1),即:

(36)

由于相机只做旋转处理(或者我们认为物体离相机很远),则t0t1=0,从而得到(u0, v0)和(u1, v1)的关系为:

(37)

式中,R10为由图像1到图像0的相对旋转矩阵,R10R1R0-1。由式2可得

(38)

为简单起见,我们设两幅图像的相机内参数中的坐标平移都为0,即KV,因为抛弃参数T对图像拼接影响不大。

我们在评估焦距时,还需定义f1uf1vf1f0uf0vf0,即设图像的长宽等像素:

(39)

则式38表示为

(40)

式中,R10=[rij]。

由式40我们就可以得到焦距f0f1:观察矩阵R10可知,R10前两行一定有相同的范数,并且是正交的,因此

(41)

(42)

由式41可得:

(43)

由式42可得:

(44)

由式43和式44得到了两个f0,选取哪个呢?比较式43和式44中分母部分的绝对值的大小,如果式43的分母大,则选择分式大的值作为f0,否则如果式44的分母大,则选择值小的作为f0

同理,矩阵R10的前两列也一定有相同的范数,并且也是正交的,因此

(45)

(46)

由式45可得

(47)

由式46可得

(48)

比较式47和式48中分母部分的绝对值的大小,如果式48的分母大,则选择分式大的值作为f1,否则如果式47的分母大,则选择值小的作为f1

如果两幅图像的焦距相同,则最终这两幅图像的焦距f为:

(49)

当评估计算得到多个f时,可取这些f的中值作为所有相机的焦距。

焦距得到后,我们就可以由式38得到R10

(50)

R01(由图像0到图像1的相对旋转矩阵)为:

(51)

(52)

对于刚性物体,它的旋转都是沿笛卡尔坐标系的x轴、y轴和z轴旋转,则分别沿着这三个轴的旋转矩阵定义为:

(53)

则旋转矩阵R表示为:

(54)

三维旋转除了可以用旋转矩阵描述外,还可以用旋转向量r描述,即r=[rx, ry, rz]T。旋转向量的长度(模)表示绕轴逆时针旋转的角度θ。旋转向量和旋转矩阵可以通过Rodrigues算法进行转换。由旋转向量转换为旋转矩阵的Rodrigues算法描述如下:

(55)

(56)

(57)

式中,I为3×3的单位矩阵。而由旋转矩阵转换为旋转向量的Rodrigues算法公式为:

(58)

当有多幅图像需要拼接为一幅图像时,是要以其中一幅图像为基准,其他图像都要旋转到该基准图像平面上的,所以就需要找到基准平面。这里用到的算法为最大生成树算法。

待拼接图像的排列是无序的,而且我们事先是不知道它们之间的关系的,我们只知道它们之间的单应矩阵,而单应矩阵是由图像间的内点计算评估得到的。由此我们可以构造一个无向图G,G的节点为图像,G的边为内点数,然后利用并查集在该G中得到一棵最大生成树。


图8 最大生成树

图8为用于拼接的最大生成树的一个例子,图(a)为无向图,节点为图像(A、B、C、D、E),节点间的边为内点数。图(b)为最大生成树,由图像C到图像B要经过最大的边连接,所以要经过图像A,而图像C和图像B之间的连接就需要去掉了。

我们把树的中心节点作为基准图像。中心节点的确定方法为:计算每一个节点到所有叶节点的距离,把其中的最大值作为该节点的值;然后选择这些值中最小者作为中心节点。这里的距离指的是节点间的节点数。如图8(c)所示,节点A和C为中心节点。中心节点可能是1个,也可能是2个,如果是2个,则选择其中一个即可。

基于以上方法,我们得到了相机的内外参数,但这样得到的参数忽略了多个图像间的约束,而且会产生累计误差。这时,我们就需要用到光束平差法(Bundle Adjustment)来精确化相机参数。光束指的是相机光心“发射”出来的光束(或射线),它透过相片达到物点,因此相片中的点应该和物点处于一个光束线上,但当两者不共线时,我们就需要对结构和视角参数进行调节,以达到最优解甚至共线的目的。最优化一般采用前文介绍过的LM算法。

应用于光束平差法的LM算法,误差指标函数可以有两个,一个是重映射误差,另一个是射线发散误差。

重映射误差公式为式25(即一个内点要有x轴和y轴两个误差值),而单应矩阵H是由式38得到。也就是说H是由相机的内外参数得到。相机的内外参数一共有7个:fuαcxcyrxryrz。前4个参数是内参数(见式33),后3个参数是外参数(即式55中的旋转向量的三个元素)。因此式25中的hh(fu, α, cx, cy, rx, ry,rz),由此得到J(h)为:

(59)

式中,n表示待拼接的图像数量,也就是所有的相机。有时为了计算方便,导数可以用差分近似,例如我们要计算efu的偏导,则

(60)

式中,∆表示一个很小的数。


图9 射线发散概念

第二种误差指标函数是基于射线发散原理。如图9所示,不同的相机发出的射线透过相片后达到同一个物点,但由于误差,两条射线不会相交,或者称为两条射线发散了,我们就把这两条射线间的最短距离d定义为射线发散误差。

下面,我们不加证明的给出射线发散误差的计算公式:

设(u1, v1, 1)和(u2, v2, 1)为两幅不同图像的同一特征点的齐次坐标,则由单应矩阵H1H2分别得到它们的物点坐标为:

(61)

分别对坐标进行归一化处理:

(62)

(63)

为了简化计算,射线间的最短距离可以表示为基于不同焦距f1f2下的归一化后的物点坐标之差:

(64)

式64说明在计算射线发散误差时,每一个内点有x轴、y轴和z轴三个误差值。

我们假设射线就是相机的光轴,因此单应矩阵H中的参数α=1,cxcy=0,所以式25中h只是基于4个参数的变量,即h(fu, rx,ry, rz),由此得到射线发散误差的J(h)为:

(65)

前面介绍的光束平差法会引起波形效应,即拼接的图像会呈现蛇形分布,这是因为真实拍摄相片时不大可能都保持水平而不倾斜的,也就是重力轴没有垂直于图像。因此我们就需要引入一个全局校正矩阵Q,用于“拉直”拼接图像,该方法也成为波形校正。

校正的目的应该是,在第k个相机旋转矩阵Rk乘以Q后,全局y轴应该与图像的x轴垂直,这种约束条件可以表示为:

(66)

式中,i=(1, 0, 0),j=(0, 1, 0),该式的含义是Rk的第一行rk0垂直于Q的第二列q1。式66的这类约束问题可以看成是最小二乘问题:

(67)

因此,q1是矩量矩阵∑rTr的最小特征值所对应的特征向量。矩阵Q的其他列的经验公式为:

(68)

式中,×表示矩阵的点乘(即对应元素相乘,与MATLAB中的点乘相同),分母表示对分子取模,即q0是归一化的结果。

(69)

最终的全局校正矩阵Q=[q0q1q2],而用Q乘以各个相机的R即实现了波形校正。如果要进行垂直校正拉直,则需选择最大特征值对应的特征向量。

 

3.2 源码

Estimator类是相机参数评估器的基类,HomographyBasedEstimator和BundleAdjusterBase都是Estimator类的子类。HomographyBasedEstimator类主要负责相机参数的计算评估,BundleAdjusterBase类主要通过光束平差法使相机参数精确化。

 

我们先来看HomographyBasedEstimator类,它的主要内容就是estimate函数:

void HomographyBasedEstimator::estimate(const vector<ImageFeatures> &features, const vector<MatchesInfo> &pairwise_matches,
                                        vector<CameraParams> &cameras)
//features表示所有待拼接图像的特征
//pairwise_matches表示匹配点对
//cameras表示相机参数信息
{
    LOGLN("Estimating rotations...");
#if ENABLE_LOG
    int64 t = getTickCount();
#endif

    const int num_images = static_cast<int>(features.size());    //得到待拼接图像的数量

#if 0
    // Robustly estimate focal length from rotating cameras
    vector<Mat> Hs;
    for (int iter = 0; iter < 100; ++iter)
    {
        int len = 2 + rand()%(pairwise_matches.size() - 1);
        vector<int> subset;
        selectRandomSubset(len, pairwise_matches.size(), subset);
        Hs.clear();
        for (size_t i = 0; i < subset.size(); ++i)
            if (!pairwise_matches[subset[i]].H.empty())
                Hs.push_back(pairwise_matches[subset[i]].H);
        Mat_<double> K;
        if (Hs.size() >= 2)
        {
            if (calibrateRotatingCamera(Hs, K))
                cin.get();
        }
    }
#endif
    //is_focals_estimated_为HomographyBasedEstimator类的全局变量,表示是否需要进行焦距评估
    if (!is_focals_estimated_)    //需要焦距评估,因为焦距还没有被评估
    {
        // Estimate focal length and set it for all cameras
        vector<double> focals;    //表示所有图像的焦距
        //评估焦距,estimateFocal在后面给出详细介绍
        estimateFocal(features, pairwise_matches, focals);
        cameras.assign(num_images, CameraParams());    //为相机参数分配空间
        for (int i = 0; i < num_images; ++i)
            cameras[i].focal = focals[i];    //相机焦距赋值
    }
    else    //不需要焦距评估
    {
        //得到相机内参数的cx, cy
        for (int i = 0; i < num_images; ++i)
        {
            cameras[i].ppx -= 0.5 * features[i].img_size.width;
            cameras[i].ppy -= 0.5 * features[i].img_size.height;
        }
    }

    // Restore global motion
    Graph span_tree;    //定义最大生成树
    vector<int> span_tree_centers;    //表示最大生成树的中心节点
    //得到最大生成树,该函数在后面给出了详细的介绍
    findMaxSpanningTree(num_images, pairwise_matches, span_tree, span_tree_centers);
    //以中心节点的图像为基准,计算其他图像与该图像的旋转矩阵,CalcRotation结构体在后面给出了详细的介绍
    span_tree.walkBreadthFirst(span_tree_centers[0], CalcRotation(num_images, pairwise_matches, cameras));

    // As calculations were performed under assumption that p.p. is in image center
    for (int i = 0; i < num_images; ++i)    //得到相机参数中的坐标平移
    {
        cameras[i].ppx += 0.5 * features[i].img_size.width;
        cameras[i].ppy += 0.5 * features[i].img_size.height;
    }

    LOGLN("Estimating rotations, time: " << ((getTickCount() - t) / getTickFrequency()) << " sec");
}

评估相机焦距:

void estimateFocal(const vector<ImageFeatures> &features, const vector<MatchesInfo> &pairwise_matches,
                       vector<double> &focals)
//features表示图像的特征
//pairwise_matches表示匹配点对
//focals表示相机焦距
{
    const int num_images = static_cast<int>(features.size());    //待拼接的图像数量
    focals.resize(num_images);    //定义focals向量变量的长度

    vector<double> all_focals;    //表示焦距
    //嵌套循环,得到每两个图像之间的匹配信息
    for (int i = 0; i < num_images; ++i)
    {
        for (int j = 0; j < num_images; ++j)
        {
            //得到第i幅图像与第j幅图像之间的匹配信息
            const MatchesInfo &m = pairwise_matches[i*num_images + j];
            if (m.H.empty())    //没有单应矩阵,说明它们之间不能拼接
                continue;    //进入下一个循环
            double f0, f1;    //表示这两幅图像的焦距
            bool f0ok, f1ok;    //表示是否得到了这两幅图像的焦距
            //从单应矩阵中得到焦距,focalsFromHomography函数在后面给出介绍
            focalsFromHomography(m.H, f0, f1, f0ok, f1ok);
            if (f0ok && f1ok)    //得到了两幅图像的焦距
                all_focals.push_back(sqrt(f0 * f1));    //式49,把焦距存入all_focals内
        }
    }

    if (static_cast<int>(all_focals.size()) >= num_images - 1)
    {
        double median;    //表示焦距中值

        std::sort(all_focals.begin(), all_focals.end());    //所有焦距排序
        //得到所有焦距的中值,作为最终的焦距
        if (all_focals.size() % 2 == 1)
            median = all_focals[all_focals.size() / 2];
        else
            median = (all_focals[all_focals.size() / 2 - 1] + all_focals[all_focals.size() / 2]) * 0.5;

        for (int i = 0; i < num_images; ++i)    //为所有相机的焦距赋同样的值
            focals[i] = median;    //焦距赋值
    }
    //按照正常的方面没有得到焦距的处理方法:焦距为所有拼接图像的长宽之和的平均值
    else
    {
        LOGLN("Can't estimate focal length, will use naive approach");
        double focals_sum = 0;
        for (int i = 0; i < num_images; ++i)    //所有图像的长宽之和
            focals_sum += features[i].img_size.width + features[i].img_size.height;
        for (int i = 0; i < num_images; ++i)
            focals[i] = focals_sum / num_images;    //平均值
    }
}

从单应矩阵中得到焦距:

void focalsFromHomography(const Mat& H, double &f0, double &f1, bool &f0_ok, bool &f1_ok)
//H表示单应矩阵
//f0和f1分别表示单应矩阵H所转换的两幅图像的焦距
//f0_ok和f1_ok分别表示f0和f1是否评估正确
{
    //确保H的数据类型和格式正确
    CV_Assert(H.type() == CV_64F && H.size() == Size(3, 3));

    const double* h = reinterpret_cast<const double*>(H.data);    //赋值指针
    //分别表示式43和式44,或式47和式48的分母
    double d1, d2; // Denominators
    //分别表示式43和式44,或式47和式48
    double v1, v2; // Focal squares value candidates

    f1_ok = true;
    d1 = h[6] * h[7];    //式48的分母
    d2 = (h[7] - h[6]) * (h[7] + h[6]);    //式47的分母
    v1 = -(h[0] * h[1] + h[3] * h[4]) / d1;    //式48
    v2 = (h[0] * h[0] + h[3] * h[3] - h[1] * h[1] - h[4] * h[4]) / d2;    //式47
    if (v1 < v2) std::swap(v1, v2);    //使v1不小于v2
    //表示到底选取式47还是式48作为f1
    if (v1 > 0 && v2 > 0) f1 = sqrt(std::abs(d1) > std::abs(d2) ? v1 : v2);
    else if (v1 > 0) f1 = sqrt(v1);    //v2小于0,则f1一定是v1的平方根
    else f1_ok = false;    //v1和v2都小于0,则没有得到f1

    f0_ok = true;
    d1 = h[0] * h[3] + h[1] * h[4];    //式44的分母
    d2 = h[0] * h[0] + h[1] * h[1] - h[3] * h[3] - h[4] * h[4];    //式43的分母
    v1 = -h[2] * h[5] / d1;    //式44
    v2 = (h[5] * h[5] - h[2] * h[2]) / d2;    //式43
    if (v1 < v2) std::swap(v1, v2);    //使v1不小于v2
    //表示到底选取式44还是式43作为f0
    if (v1 > 0 && v2 > 0) f0 = sqrt(std::abs(d1) > std::abs(d2) ? v1 : v2);
    else if (v1 > 0) f0 = sqrt(v1);    //v2小于0,则f1一定是v1的开根号
    else f0_ok = false;    //v1和v2都小于0,则没有得到f1
}

构建最大生成树:

void findMaxSpanningTree(int num_images, const vector<MatchesInfo> &pairwise_matches,
                             Graph &span_tree, vector<int> ¢ers)
//num_images表示待拼接图像的数量,也是最大生成树的节点数
//pairwise_matches表示图像间的拼接信息
//span_tree表示最大生成树
//centers表示最大生成树的中心节点
{
    Graph graph(num_images);    //定义无向图G
    vector<GraphEdge> edges;    //定义无向图G的边

    // Construct images graph and remember its edges
    //遍历待拼接图像,得到无向图G的边,即内点数
    for (int i = 0; i < num_images; ++i)
    {
        for (int j = 0; j < num_images; ++j)
        {
            //如果图像i和j没有单应矩阵,则说明这两幅图像不重叠,不能拼接
            if (pairwise_matches[i * num_images + j].H.empty())
                continue;
            //得到图像i和j的内点数
            float conf = static_cast<float>(pairwise_matches[i * num_images + j].num_inliers);
            graph.addEdge(i, j, conf);    //为G添加边
            edges.push_back(GraphEdge(i, j, conf));    //添加到边队列中
        }
    }

    DisjointSets comps(num_images);    //实例化DisjointSets类,表示定义一个并查集
    span_tree.create(num_images);    //创建生成树
    //表示生成树的幂,即节点间的连接数,例如某节点的幂为3,说明该节点与其他3个节点相连接
    vector<int> span_tree_powers(num_images, 0);

    // Find maximum spanning tree
    //按无向图G的边的大小(内点数)从小到大排序
    sort(edges.begin(), edges.end(), greater<GraphEdge>());
    for (size_t i = 0; i < edges.size(); ++i)    //从小到大遍历无向图G的边
    {
        int comp1 = comps.findSetByElem(edges[i].from);    //得到该边的起始节点的集合
        int comp2 = comps.findSetByElem(edges[i].to);    //得到该边的终止节点的集合
        //两种不相等,说明是一个新的边,需要通过并查集添加到生成树中
        if (comp1 != comp2) 
        {
            comps.mergeSets(comp1, comp2);    //合并这两个节点
            //为生成树添加该边
            span_tree.addEdge(edges[i].from, edges[i].to, edges[i].weight);
            span_tree.addEdge(edges[i].to, edges[i].from, edges[i].weight);
            //节点幂的累加
            span_tree_powers[edges[i].from]++;
            span_tree_powers[edges[i].to]++;
        }
    }

    // Find spanning tree leafs
    vector<int> span_tree_leafs;    //表示生成树的叶节点
    //生成树的节点的幂为1,则为叶节点
    for (int i = 0; i < num_images; ++i)    //遍历图像
        if (span_tree_powers[i] == 1)    //表示该图像为叶节点
            span_tree_leafs.push_back(i);    //放入队列中

    // Find maximum distance from each spanning tree vertex
    vector<int> max_dists(num_images, 0);    //表示节点与叶节点的最大距离
    vector<int> cur_dists;    //表示节点与叶节点的当前距离
    for (size_t i = 0; i < span_tree_leafs.size(); ++i)    //遍历叶节点
    {
        cur_dists.assign(num_images, 0);    //初始化
        //得到该叶节点到其他节点的距离,IncDistance表示距离的累加,即节点的累计
        span_tree.walkBreadthFirst(span_tree_leafs[i], IncDistance(cur_dists));
        //遍历所有节点,更新节点到叶节点的最大距离
        for (int j = 0; j < num_images; ++j) 
            max_dists[j] = max(max_dists[j], cur_dists[j]);
    }

    // Find min-max distance
    int min_max_dist = max_dists[0];    //表示所有最大距离中的最小值
    for (int i = 1; i < num_images; ++i)    //遍历所有节点
        if (min_max_dist > max_dists[i])
            min_max_dist = max_dists[i];    //得到最大距离中的最小值

    // Find spanning tree centers
    centers.clear();    //表示中心节点,清零
    for (int i = 0; i < num_images; ++i)    //遍历所有节点
        if (max_dists[i] == min_max_dist)
            centers.push_back(i);    //保存最大距离中的最小值所对应的节点
    //确保中心节点的数量必须大于0并小于3
    CV_Assert(centers.size() > 0 && centers.size() <= 2);
}

计算旋转矩阵:

struct CalcRotation
{
    CalcRotation(int _num_images, const vector<MatchesInfo> &_pairwise_matches, vector<CameraParams> &_cameras)
        : num_images(_num_images), pairwise_matches(&_pairwise_matches[0]), cameras(&_cameras[0]) {}

    void operator ()(const GraphEdge &edge)
    {
        int pair_idx = edge.from * num_images + edge.to;    //表示匹配点对的索引
        //构造式51中的参数K0
        Mat_<double> K_from = Mat::eye(3, 3, CV_64F);    //初始化
        K_from(0,0) = cameras[edge.from].focal;    //表示式33的fu
        //表示式33的fv
        K_from(1,1) = cameras[edge.from].focal * cameras[edge.from].aspect;
        K_from(0,2) = cameras[edge.from].ppx;    //表示式33的cx
        K_from(1,2) = cameras[edge.from].ppy;    //表示式33的cy
        //构造式51中的参数K1
        Mat_<double> K_to = Mat::eye(3, 3, CV_64F);    //初始化
        K_to(0,0) = cameras[edge.to].focal;    //表示式33的fu
        K_to(1,1) = cameras[edge.to].focal * cameras[edge.to].aspect;    //表示式33的fv
        K_to(0,2) = cameras[edge.to].ppx;    //表示式33的cx
        K_to(1,2) = cameras[edge.to].ppy;    //表示式33的cy

        Mat R = K_from.inv() * pairwise_matches[pair_idx].H.inv() * K_to;    //式51
        //式52,可见CameraParams变量中R实际存储的是相机旋转矩阵变量的逆
        cameras[edge.to].R = cameras[edge.from].R * R;
    }

    int num_images;    //表示待拼接图像的数量
    const MatchesInfo* pairwise_matches;    //表示匹配图像的信息
    CameraParams* cameras;    //表示相机参数
};

下面我们给出BundleAdjusterBase类的讲解。BundleAdjusterBase类的构造函数的主要作用是为每个相机参数数量(num_params_per_cam,如果是重映射误差,则num_params_per_cam应为7,如果是射线发散误差,则num_params_per_cam应为4)和每个内点误差数量(num_errs_per_measurement,如果是重映射误差,则num_errs_per_measurement应为2,即x轴和y轴的误差,如果是射线发散误差,则num_errs_per_measurement应为3,即x轴、y轴和z轴的误差)赋值,并且初始化setRefinementMask(表示需要精确化的相机内参数矩阵K的掩码矩阵)、setConfThresh(表示内点阈值conf_thresh_,也称作置信度阈值),以及setTermCriteria(表示LM算法的迭代终止条件)。

BundleAdjusterBase类的一个重要函数是estimate,它的作用是细化相机参数:

void BundleAdjusterBase::estimate(const vector<ImageFeatures> &features,
                                  const vector<MatchesInfo> &pairwise_matches,
                                  vector<CameraParams> &cameras)
//features表示图像特征
//pairwise_matches表示图像匹配信息
//cameras表示相机参数
{
    LOG_CHAT("Bundle adjustment");
#if ENABLE_LOG
    int64 t = getTickCount();
#endif

    num_images_ = static_cast<int>(features.size());    //表示待拼接图像的数量
    features_ = &features[0];    //图像特征变量的首地址赋值
    pairwise_matches_ = &pairwise_matches[0];    //表示匹配信息变量的首地址指针
    //初始化LM算法所需的参数,setUpInitialCameraParams为虚函数,实际是调用BundleAdjusterBase类的子类
    setUpInitialCameraParams(cameras);

    // Leave only consistent image pairs
    edges_.clear();    //edges_表示图像间内点数,该变量先清零
    //只保留那些内点数大于置信度阈值的图像匹配对
    for (int i = 0; i < num_images_ - 1; ++i)
    {
        for (int j = i + 1; j < num_images_; ++j)
        {
            //得到图像i和图像j的匹配信息
            const MatchesInfo& matches_info = pairwise_matches_[i * num_images_ + j];
            if (matches_info.confidence > conf_thresh_)    //内点数大于阈值
                edges_.push_back(make_pair(i, j));    //保留这个图像匹配对
        }
    }

    // Compute number of correspondences
    total_num_matches_ = 0;    //total_num_matches_表示所有保留下来的内点数
    for (size_t i = 0; i < edges_.size(); ++i)    //遍历图像匹配对,计算total_num_matches_
        total_num_matches_ += static_cast<int>(pairwise_matches[edges_[i].first * num_images_ +
                                                                edges_[i].second].num_inliers);
    //实例化CvLevMarq类,表示LM算法,CvLevMarq类构造函数的第一个参数表示需要优化变量的数量;CvLevMarq类构造函数的第二个参数表示误差数量;CvLevMarq类构造函数的第三个参数表示LM算法的迭代终止条件
    CvLevMarq solver(num_images_ * num_params_per_cam_,
                     total_num_matches_ * num_errs_per_measurement_,
                     term_criteria_);

    Mat err, jac;    //err表示LM算法的误差,jac表示雅可比参数
    CvMat matParams = cam_params_;    //表示相机参数矩阵
    cvCopy(&matParams, solver.param);    //复制,初始化用于LM算法的相机参数

    int iter = 0;    //表示迭代次数
    for(;;)    //LM算法的迭代循环
    {
        const CvMat* _param = 0;    //表示LM算法计算得到的相机参数
        CvMat* _jac = 0;    //表示LM计算得到的雅可比参数
        CvMat* _err = 0;    //表示LM计算得到的误差
        //调用update函数,得到相机参数_param,即式27和式28
        bool proceed = solver.update(_param, _jac, _err);

        cvCopy(_param, &matParams);    //复制,得到相机参数

        if (!proceed || !_err)    //如果该次迭代不成功,或误差为0
            break;    //退出LM算法
        //如果_jac不为零,则说明下次循环时,solver.update函数需要雅可比参数
        if (_jac) 
        {
            calcJacobian(jac);    //计算雅可比参数,该函数为虚函数
            CvMat tmp = jac;
            cvCopy(&tmp, _jac);    //复制
        }
        //如果_err不为零,则说明下次循环时,solver.update函数需要误差参数
        if (_err)
        {
            calcError(err);    //计算误差,该函数为虚函数
            LOG_CHAT(".");
            iter++;    //累计迭代次数
            CvMat tmp = err;
            cvCopy(&tmp, _err);    //复制
        }
    }
    //终端输出信息
    LOGLN_CHAT("");
    LOGLN_CHAT("Bundle adjustment, final RMS error: " << sqrt(err.dot(err) / total_num_matches_));
    LOGLN_CHAT("Bundle adjustment, iterations done: " << iter);
    //LM算法结束后,得到最终的精确的相机参数,该函数为虚函数
    obtainRefinedCameraParams(cameras);

    // Normalize motion to center image
    //利用精确的相机参数,再次计算基于中心图像的其他图像的相对旋转矩阵
    Graph span_tree;
    vector<int> span_tree_centers;
    //利用最大生成树算法计算中心图像
    findMaxSpanningTree(num_images_, pairwise_matches, span_tree, span_tree_centers);
    //前面我们介绍过,相机旋转矩阵CameraParams.R在前面的程序中其实是旋转矩阵的逆矩阵,现在让CameraParams.R是基于中心图像的相对旋转矩阵
    Mat R_inv = cameras[span_tree_centers[0]].R.inv();    //中心图像的旋转矩阵
    for (int i = 0; i < num_images_; ++i)    //得到其他图像的相对旋转矩阵
        cameras[i].R = R_inv * cameras[i].R;
    //终端输出信息
    LOGLN_CHAT("Bundle adjustment, time: " << ((getTickCount() - t) / getTickFrequency()) << " sec");
}

BundleAdjusterBase类有两个子类——BundleAdjusterReproj和BundleAdjusterRay,它们分别表示光束平差法的两个实现方法——重映射方法和射线发散方法,也就是误差指标函数的两种形式。

下面我们先给出BundleAdjusterReproj类。setUpInitialCameraParams函数表示初始化相机参数,即在LM算法迭代之前,要赋予相机参数初值:

void BundleAdjusterReproj::setUpInitialCameraParams(const vector<CameraParams> &cameras)
//cameras表示相机参数的初始化值
{
    //定义表示待精确化的所有参数的矩阵大小
    cam_params_.create(num_images_ * 7, 1, CV_64F);
    SVD svd;
    //遍历所有的图像(即所有相机),初始化相机参数
    for (int i = 0; i < num_images_; ++i) 
    {
        cam_params_.at<double>(i * 7, 0) = cameras[i].focal;    //初始化fu
        cam_params_.at<double>(i * 7 + 1, 0) = cameras[i].ppx;    //初始化cx
        cam_params_.at<double>(i * 7 + 2, 0) = cameras[i].ppy;    //初始化cy
        cam_params_.at<double>(i * 7 + 3, 0) = cameras[i].aspect;    //初始化α
        //表示得到满足正交关系的旋转矩阵R
        svd(cameras[i].R, SVD::FULL_UV);
        Mat R = svd.u * svd.vt;
        if (determinant(R) < 0)
            R *= -1;

        Mat rvec;
        Rodrigues(R, rvec);    //旋转矩阵R由Rodrigues算法得到旋转向量rvec
        CV_Assert(rvec.type() == CV_32F);
        cam_params_.at<double>(i * 7 + 4, 0) = rvec.at<float>(0, 0);    //初始化rx
        cam_params_.at<double>(i * 7 + 5, 0) = rvec.at<float>(1, 0);    //初始化ry
        cam_params_.at<double>(i * 7 + 6, 0) = rvec.at<float>(2, 0);    //初始化rz
    }
}

obtainRefinedCameraParams函数表示得到最终的精确的相机参数:

void BundleAdjusterReproj::obtainRefinedCameraParams(vector<CameraParams> &cameras) const
//cameras表示最终得到的相机参数
{
    for (int i = 0; i < num_images_; ++i)    //遍历所有的图像(即所有相机),得到相机参数
    {
        cameras[i].focal = cam_params_.at<double>(i * 7, 0);    //得到fu
        cameras[i].ppx = cam_params_.at<double>(i * 7 + 1, 0);   //得到cx
        cameras[i].ppy = cam_params_.at<double>(i * 7 + 2, 0);    //得到cy
        cameras[i].aspect = cam_params_.at<double>(i * 7 + 3, 0);    //得到α

        Mat rvec(3, 1, CV_64F);
        rvec.at<double>(0, 0) = cam_params_.at<double>(i * 7 + 4, 0);    //得到rx
        rvec.at<double>(1, 0) = cam_params_.at<double>(i * 7 + 5, 0);    //得到ry
        rvec.at<double>(2, 0) = cam_params_.at<double>(i * 7 + 6, 0);    //得到rz
        Rodrigues(rvec, cameras[i].R);    //旋转向量rvec由Rodrigues算法得到旋转矩阵R

        Mat tmp;
        cameras[i].R.convertTo(tmp, CV_32F);    //变换数据类型
        cameras[i].R = tmp;    //赋值
    }
}

calcError函数用于计算误差,即式25:

void BundleAdjusterReproj::calcError(Mat &err)
//err表示计算得到的误差
{
    err.create(total_num_matches_ * 2, 1, CV_64F);    //定义误差矩阵

    int match_idx = 0;    //表示重映射误差的索引
    //遍历最大生成树的边
    for (size_t edge_idx = 0; edge_idx < edges_.size(); ++edge_idx)
    {
        int i = edges_[edge_idx].first;    //表示边的始端图像
        int j = edges_[edge_idx].second;    //表示边的终端图像
        double f1 = cam_params_.at<double>(i * 7, 0);    //始端图像的fu
        double f2 = cam_params_.at<double>(j * 7, 0);    //终端图像的fu
        double ppx1 = cam_params_.at<double>(i * 7 + 1, 0);    //始端图像的cx
        double ppx2 = cam_params_.at<double>(j * 7 + 1, 0);    //终端图像的cx
        double ppy1 = cam_params_.at<double>(i * 7 + 2, 0);    //始端图像的cy
        double ppy2 = cam_params_.at<double>(j * 7 + 2, 0);    //终端图像的cy
        double a1 = cam_params_.at<double>(i * 7 + 3, 0);    //始端图像的α
        double a2 = cam_params_.at<double>(j * 7 + 3, 0);    //终端图像的α

        double R1[9];
        Mat R1_(3, 3, CV_64F, R1);
        Mat rvec(3, 1, CV_64F);
        rvec.at<double>(0, 0) = cam_params_.at<double>(i * 7 + 4, 0);    //始端图像的rx
        rvec.at<double>(1, 0) = cam_params_.at<double>(i * 7 + 5, 0);    //始端图像的ry
        rvec.at<double>(2, 0) = cam_params_.at<double>(i * 7 + 6, 0);    //始端图像的rz
        //旋转向量rvec由Rodrigues算法得到始端图像的旋转矩阵R
        Rodrigues(rvec, R1_);

        double R2[9];
        Mat R2_(3, 3, CV_64F, R2);
        rvec.at<double>(0, 0) = cam_params_.at<double>(j * 7 + 4, 0);    //终端图像的rx
        rvec.at<double>(1, 0) = cam_params_.at<double>(j * 7 + 5, 0);    //终端图像的ry
        rvec.at<double>(2, 0) = cam_params_.at<double>(j * 7 + 6, 0);    //终端图像的rz
        //旋转向量rvec由Rodrigues算法得到终端图像的旋转矩阵R
        Rodrigues(rvec, R2_);

        const ImageFeatures& features1 = features_[i];    //得到始端图像的特征
        const ImageFeatures& features2 = features_[j];    //得到终端图像的特征
        //得到两者的图像匹配信息
        const MatchesInfo& matches_info = pairwise_matches_[i * num_images_ + j];
        //为始端图像的相机内参数矩阵K赋值
        Mat_<double> K1 = Mat::eye(3, 3, CV_64F);
        K1(0,0) = f1; K1(0,2) = ppx1;
        K1(1,1) = f1*a1; K1(1,2) = ppy1;
        //为终端图像的相机内参数矩阵K赋值
        Mat_<double> K2 = Mat::eye(3, 3, CV_64F);
        K2(0,0) = f2; K2(0,2) = ppx2;
        K2(1,1) = f2*a2; K2(1,2) = ppy2;
        //得到两者的相对单应矩阵H,式38
        Mat_<double> H = K2 * R2_.inv() * R1_ * K1.inv();

        for (size_t k = 0; k < matches_info.matches.size(); ++k)    //遍历匹配点对
        {
            if (!matches_info.inliers_mask[k])    //表示不是内点
                continue;

            const DMatch& m = matches_info.matches[k];    //表示内点信息
            //表示内点点对
            Point2f p1 = features1.keypoints[m.queryIdx].pt; 
            Point2f p2 = features2.keypoints[m.trainIdx].pt;
            //由单应矩阵得到p1点的三维空间坐标
            double x = H(0,0)*p1.x + H(0,1)*p1.y + H(0,2);
            double y = H(1,0)*p1.x + H(1,1)*p1.y + H(1,2);
            double z = H(2,0)*p1.x + H(2,1)*p1.y + H(2,2);
            //把p1点的三维空间坐标重新映射到p2点所在平面,并比较两者的差,即重映射误差
            err.at<double>(2 * match_idx, 0) = p2.x - x/z;
            err.at<double>(2 * match_idx + 1, 0) = p2.y - y/z;
            match_idx++;    //累加匹配索引值
        }
    }
}

calcJacobian函数用于计算雅可比矩阵,即式59:

void BundleAdjusterReproj::calcJacobian(Mat &jac)
//jac表示得到的雅可比矩阵
{
    //定义雅可比矩阵的大小
    jac.create(total_num_matches_ * 2, num_images_ * 7, CV_64F);
    jac.setTo(0);    //清零

    double val;
    const double step = 1e-4;    //表示步长,即式60中的∆

    for (int i = 0; i < num_images_; ++i)    //遍历所有的匹配图像,即式59的所有行
    {
        if (refinement_mask_.at<uchar>(0, 0))    //计算雅可比矩阵的fu项
        {
            val = cam_params_.at<double>(i * 7, 0);    //提取fu
            cam_params_.at<double>(i * 7, 0) = val - step;    //fu-∆
            calcError(err1_);    //计算因fu变换而引起的误差
            cam_params_.at<double>(i * 7, 0) = val + step;    //fu+∆
            calcError(err2_);    //计算因fu变换而引起的误差
            calcDeriv(err1_, err2_, 2 * step, jac.col(i * 7));    //式60
            cam_params_.at<double>(i * 7, 0) = val;    //重新赋值fu,以备其他参数计算
        }
        if (refinement_mask_.at<uchar>(0, 2))    //计算雅可比矩阵的cx项
        {
            val = cam_params_.at<double>(i * 7 + 1, 0);    //提取cx
            cam_params_.at<double>(i * 7 + 1, 0) = val - step;    //cx-∆
            calcError(err1_);    //计算因cx变换而引起的误差
            cam_params_.at<double>(i * 7 + 1, 0) = val + step;    //cx+∆
            calcError(err2_);    //计算因cx变换而引起的误差
            calcDeriv(err1_, err2_, 2 * step, jac.col(i * 7 + 1));    //式60
            cam_params_.at<double>(i * 7 + 1, 0) = val;    //重新赋值cx,以备其他参数计算
        }
        if (refinement_mask_.at<uchar>(1, 2))    //计算雅可比矩阵的cy项
        {
            val = cam_params_.at<double>(i * 7 + 2, 0);    //提取cy
            cam_params_.at<double>(i * 7 + 2, 0) = val - step;    //cy-∆
            calcError(err1_);    //计算因cy变换而引起的误差
            cam_params_.at<double>(i * 7 + 2, 0) = val + step;    //cy+∆
            calcError(err2_);    //计算因cy变换而引起的误差
            calcDeriv(err1_, err2_, 2 * step, jac.col(i * 7 + 2));    //式60
            cam_params_.at<double>(i * 7 + 2, 0) = val;    //重新赋值cy,以备其他参数计算
        }
        if (refinement_mask_.at<uchar>(1, 1))    //计算雅可比矩阵的α项
        {
            val = cam_params_.at<double>(i * 7 + 3, 0);    //提取α
            cam_params_.at<double>(i * 7 + 3, 0) = val - step;    //α-∆
            calcError(err1_);    //计算因α变换而引起的误差
            cam_params_.at<double>(i * 7 + 3, 0) = val + step;    //α+∆
            calcError(err2_);    //计算因α变换而引起的误差
            calcDeriv(err1_, err2_, 2 * step, jac.col(i * 7 + 3));    //式60
            cam_params_.at<double>(i * 7 + 3, 0) = val;    //重新赋值α,以备其他参数计算
        }
        for (int j = 4; j < 7; ++j)    //计算rx, ry, rz
        {
            val = cam_params_.at<double>(i * 7 + j, 0);
            cam_params_.at<double>(i * 7 + j, 0) = val - step;
            calcError(err1_);
            cam_params_.at<double>(i * 7 + j, 0) = val + step;
            calcError(err2_);
            calcDeriv(err1_, err2_, 2 * step, jac.col(i * 7 + j));
            cam_params_.at<double>(i * 7 + j, 0) = val;
        }
    }
}

下面我们先给出BundleAdjusterRay类:

void BundleAdjusterRay::setUpInitialCameraParams(const vector<CameraParams> &cameras)
{
    //定义表示待精确化的所有参数的矩阵大小
    cam_params_.create(num_images_ * 4, 1, CV_64F);
    SVD svd;
    //遍历所有的图像(即所有相机),初始化相机参数
    for (int i = 0; i < num_images_; ++i)
    {
        cam_params_.at<double>(i * 4, 0) = cameras[i].focal;    //初始化fu
        //表示得到满足正交关系的旋转矩阵R
        svd(cameras[i].R, SVD::FULL_UV);
        Mat R = svd.u * svd.vt;
        if (determinant(R) < 0)
            R *= -1;

        Mat rvec;
        Rodrigues(R, rvec);    //旋转矩阵R由Rodrigues算法得到旋转向量rvec
        CV_Assert(rvec.type() == CV_32F);
        cam_params_.at<double>(i * 4 + 1, 0) = rvec.at<float>(0, 0);    //初始化rx
        cam_params_.at<double>(i * 4 + 2, 0) = rvec.at<float>(1, 0);    //初始化ry
        cam_params_.at<double>(i * 4 + 3, 0) = rvec.at<float>(2, 0);    //初始化rz
    }
}


void BundleAdjusterRay::obtainRefinedCameraParams(vector<CameraParams> &cameras) const
{
    for (int i = 0; i < num_images_; ++i)    //遍历所有的图像(即所有相机),得到相机参数
    {
        cameras[i].focal = cam_params_.at<double>(i * 4, 0);    //得到fu

        Mat rvec(3, 1, CV_64F);
        rvec.at<double>(0, 0) = cam_params_.at<double>(i * 4 + 1, 0);    //得到rx
        rvec.at<double>(1, 0) = cam_params_.at<double>(i * 4 + 2, 0);    //得到ry
        rvec.at<double>(2, 0) = cam_params_.at<double>(i * 4 + 3, 0);    //得到rz
        Rodrigues(rvec, cameras[i].R);    //旋转向量rvec由Rodrigues算法得到旋转矩阵R

        Mat tmp;
        cameras[i].R.convertTo(tmp, CV_32F);    //变换数据类型
        cameras[i].R = tmp;    //赋值
    }
}

void BundleAdjusterRay::calcError(Mat &err)
{
    err.create(total_num_matches_ * 3, 1, CV_64F);    //定义误差矩阵

    int match_idx = 0;    //表示重映射误差的索引
    //遍历最大生成树的边
    for (size_t edge_idx = 0; edge_idx < edges_.size(); ++edge_idx)
    {
        int i = edges_[edge_idx].first;    //表示边的始端图像
        int j = edges_[edge_idx].second;    //表示边的终端图像
        double f1 = cam_params_.at<double>(i * 4, 0);    //始端图像的fu
        double f2 = cam_params_.at<double>(j * 4, 0);    //终端图像的fu

        double R1[9];
        Mat R1_(3, 3, CV_64F, R1);
        Mat rvec(3, 1, CV_64F);
        rvec.at<double>(0, 0) = cam_params_.at<double>(i * 4 + 1, 0);    //始端图像的rx
        rvec.at<double>(1, 0) = cam_params_.at<double>(i * 4 + 2, 0);    //始端图像的ry
        rvec.at<double>(2, 0) = cam_params_.at<double>(i * 4 + 3, 0);    //始端图像的rz
        //旋转向量rvec由Rodrigues算法得到始端图像的旋转矩阵R
        Rodrigues(rvec, R1_);

        double R2[9];
        Mat R2_(3, 3, CV_64F, R2);
        rvec.at<double>(0, 0) = cam_params_.at<double>(j * 4 + 1, 0);    //终端图像的rx
        rvec.at<double>(1, 0) = cam_params_.at<double>(j * 4 + 2, 0);    //终端图像的ry
        rvec.at<double>(2, 0) = cam_params_.at<double>(j * 4 + 3, 0);    //终端图像的rz
        //旋转向量rvec由Rodrigues算法得到终端图像的旋转矩阵R
        Rodrigues(rvec, R2_);

        const ImageFeatures& features1 = features_[i];    //得到始端图像的特征
        const ImageFeatures& features2 = features_[j];    //得到终端图像的特征
        //得到两者的图像匹配信息
        const MatchesInfo& matches_info = pairwise_matches_[i * num_images_ + j];
        //为始端图像的相机内参数矩阵K赋值
        Mat_<double> K1 = Mat::eye(3, 3, CV_64F);
        K1(0,0) = f1; K1(0,2) = features1.img_size.width * 0.5;
        K1(1,1) = f1; K1(1,2) = features1.img_size.height * 0.5;
        //为终端图像的相机内参数矩阵K赋值
        Mat_<double> K2 = Mat::eye(3, 3, CV_64F);
        K2(0,0) = f2; K2(0,2) = features2.img_size.width * 0.5;
        K2(1,1) = f2; K2(1,2) = features2.img_size.height * 0.5;
        //计算两个单应矩阵
        Mat_<double> H1 = R1_ * K1.inv();
        Mat_<double> H2 = R2_ * K2.inv();

        for (size_t k = 0; k < matches_info.matches.size(); ++k)    //遍历匹配点对
        {
            if (!matches_info.inliers_mask[k])    //表示不是内点
                continue;

            const DMatch& m = matches_info.matches[k];    //表示内点信息

            Point2f p1 = features1.keypoints[m.queryIdx].pt;    //表示内点的一个点
            //由单应矩阵得到p1点的三维空间坐标
            double x1 = H1(0,0)*p1.x + H1(0,1)*p1.y + H1(0,2);
            double y1 = H1(1,0)*p1.x + H1(1,1)*p1.y + H1(1,2);
            double z1 = H1(2,0)*p1.x + H1(2,1)*p1.y + H1(2,2);
            double len = sqrt(x1*x1 + y1*y1 + z1*z1);    //式62
            x1 /= len; y1 /= len; z1 /= len;    //式63

            Point2f p2 = features2.keypoints[m.trainIdx].pt;    //表示内点的另一个点
            //由单应矩阵得到p2点的三维空间坐标
            double x2 = H2(0,0)*p2.x + H2(0,1)*p2.y + H2(0,2);
            double y2 = H2(1,0)*p2.x + H2(1,1)*p2.y + H2(1,2);
            double z2 = H2(2,0)*p2.x + H2(2,1)*p2.y + H2(2,2);
            len = sqrt(x2*x2 + y2*y2 + z2*z2);    //式62
            x2 /= len; y2 /= len; z2 /= len;    //式63

            double mult = sqrt(f1 * f2);    //式64的根号内的部分
            //式64
            err.at<double>(3 * match_idx, 0) = mult * (x1 - x2);
            err.at<double>(3 * match_idx + 1, 0) = mult * (y1 - y2);
            err.at<double>(3 * match_idx + 2, 0) = mult * (z1 - z2);

            match_idx++;    //累计
        }
    }
}

void BundleAdjusterRay::calcJacobian(Mat &jac)
{
    //定义雅可比矩阵的大小,式65
    jac.create(total_num_matches_ * 3, num_images_ * 4, CV_64F);

    double val;
    const double step = 1e-3;    //表示步长,即式60中的∆

    for (int i = 0; i < num_images_; ++i)    //遍历所有的匹配图像,即式65的所有行
    {
        for (int j = 0; j < 4; ++j)    //遍历4个待精确的参数fu, rx, ry, rz
        {
            val = cam_params_.at<double>(i * 4 + j, 0);
            cam_params_.at<double>(i * 4 + j, 0) = val - step;
            calcError(err1_);
            cam_params_.at<double>(i * 4 + j, 0) = val + step;
            calcError(err2_);
            calcDeriv(err1_, err2_, 2 * step, jac.col(i * 4 + j));
            cam_params_.at<double>(i * 4 + j, 0) = val;
        }
    }
}

下面给出波形校正函数:

void waveCorrect(vector<Mat> &rmats, WaveCorrectKind kind)
//rmats表示所有相机的旋转矩阵
//kind表示波形校正的方式,是水平校正(WAVE_CORRECT_HORIZ)还是垂直校正(WAVE_CORRECT_VERT)
{
    LOGLN("Wave correcting...");
#if ENABLE_LOG
    int64 t = getTickCount();    //用于计时
#endif
    //在前面已经分析过,程序中的旋转矩阵其实是公式中旋转矩阵的逆
    Mat moment = Mat::zeros(3, 3, CV_32F);    //表示矩量矩阵
    for (size_t i = 0; i < rmats.size(); ++i)    //遍历所有的R,得到矩量矩阵
    {
        Mat col = rmats[i].col(0);    //提取旋转矩阵的第一列
        moment += col * col.t();    //得到式67的方括号内的部分
    }
    Mat eigen_vals, eigen_vecs;    //表示特征值和特征向量
    eigen(moment, eigen_vals, eigen_vecs);    //计算矩量矩阵的特征向量

    Mat rg1;    //表示校正矩阵的第二列,即式67的q1
    if (kind == WAVE_CORRECT_HORIZ)    //水平校正
        rg1 = eigen_vecs.row(2).t();    //最小特征值对应的特征向量
    else if (kind == WAVE_CORRECT_VERT)    //垂直校正
        rg1 = eigen_vecs.row(0).t();    //最大特征值对应的特征向量
    else    //其他情况
        CV_Error(CV_StsBadArg, "unsupported kind of wave correction");

    Mat img_k = Mat::zeros(3, 1, CV_32F);
    for (size_t i = 0; i < rmats.size(); ++i)
        img_k += rmats[i].col(2);    //计算∑r2
    Mat rg0 = rg1.cross(img_k);    //得到校正矩阵的第一列
    rg0 /= norm(rg0);    //归一化处理,式68

    Mat rg2 = rg0.cross(rg1);    //得到校正矩阵的第三列,式69

    double conf = 0;    //表示置信度
    //根据置信度,调整校正矩阵的第一列和第二列
    if (kind == WAVE_CORRECT_HORIZ)    //水平校正
    {
        for (size_t i = 0; i < rmats.size(); ++i)
            conf += rg0.dot(rmats[i].col(0));
        if (conf < 0)
        {
            rg0 *= -1;
            rg1 *= -1;
        }
    }
    else if (kind == WAVE_CORRECT_VERT)    //垂直校正
    {
        for (size_t i = 0; i < rmats.size(); ++i)
            conf -= rg1.dot(rmats[i].col(0));
        if (conf < 0)
        {
            rg0 *= -1;
            rg1 *= -1;
        }
    }

    Mat R = Mat::zeros(3, 3, CV_32F);    //表示校正矩阵
    //构造校正矩阵
    Mat tmp = R.row(0);
    Mat(rg0.t()).copyTo(tmp);
    tmp = R.row(1);
    Mat(rg1.t()).copyTo(tmp);
    tmp = R.row(2);
    Mat(rg2.t()).copyTo(tmp);

    for (size_t i = 0; i < rmats.size(); ++i)    //遍历所有的R
        rmats[i] = R * rmats[i];    //波形校正

    LOGLN("Wave correcting, time: " << ((getTickCount() - t) / getTickFrequency()) << " sec");
}

在图像拼接时,有时会出现输入的图像不属于全景图像的时候,因此我们还需要应用leaveBiggestComponent函数把这些图像剔除掉,只保留全景图像集合。应用并查集可以很方便的得到全景图像集合,而图像间能否拼接,依据的是匹配置信度c值的大小:

vector<int> leaveBiggestComponent(vector<ImageFeatures> &features,  vector<MatchesInfo> &pairwise_matches,
                                      float conf_threshold)
//features表示图像特征
//pairwise_matches表示匹配点对的信息
//conf_threshold表示匹配置信度c,即式23
//返回能够拼接在一起的全景图像集合的索引
{
    const int num_images = static_cast<int>(features.size());    //得到待拼接图像的数量

    DisjointSets comps(num_images);    //实例化DisjointSets类,表示定义一个并查集
    //遍历待拼接图像对,得到能够拼接在一起的所有图像
    for (int i = 0; i < num_images; ++i) 
    {
        for (int j = 0; j < num_images; ++j)
        {
            //如果匹配置信度过小,则继续下一次循环,即图像i和图像j无法拼接
            if (pairwise_matches[i*num_images + j].confidence < conf_threshold)
                continue;
            int comp1 = comps.findSetByElem(i);    //提取出第i幅图像的所在集合
            int comp2 = comps.findSetByElem(j);    //提取出第j幅图像的所在集合
            if (comp1 != comp2)    //表示图像i和图像j不属于同一集合
                comps.mergeSets(comp1, comp2);    //合并两个集合
        }
    }
    //得到元素最多的那个集合,即全景图像集合,max_comp表示该集合内元素(即图像)的数量
    int max_comp = static_cast<int>(max_element(comps.size.begin(), comps.size.end()) - comps.size.begin());
    //表示我们所需要的全景图像集合的元素索引,即max_comp所表示的那个集合
    vector<int> indices; 
    vector<int> indices_removed;    //表示不是全景图像集合的元素索引
    for (int i = 0; i < num_images; ++i)    //遍历所有图像
        if (comps.findSetByElem(i) == max_comp)    //得到全景图像集合
            indices.push_back(i);    //得到图像索引
        else
            indices_removed.push_back(i);    //不是全景图像集合的元素索引

    vector<ImageFeatures> features_subset;    //表示特征子集
    vector<MatchesInfo> pairwise_matches_subset;    //表示匹配子集
    //遍历全景图像集合,得到特征子集和匹配子集
    for (size_t i = 0; i < indices.size(); ++i)
    {
        features_subset.push_back(features[indices[i]]);    //得到特征子集
        for (size_t j = 0; j < indices.size(); ++j)    //得到匹配子集
        {
            pairwise_matches_subset.push_back(pairwise_matches[indices[i]*num_images + indices[j]]);
            pairwise_matches_subset.back().src_img_idx = static_cast<int>(i);
            pairwise_matches_subset.back().dst_img_idx = static_cast<int>(j);
        }
    }
    //如果子集包括了所有图像,则直接退出,无需再执行下去
    if (static_cast<int>(features_subset.size()) == num_images)
        return indices;    //返回索引值
    //终端输出
    LOG("Removed some images, because can't match them or there are too similar images: (");
    LOG(indices_removed[0] + 1);
    for (size_t i = 1; i < indices_removed.size(); ++i)
        LOG(", " << indices_removed[i]+1);
    LOGLN(").");
    LOGLN("Try to decrease the match confidence threshold and/or check if you're stitching duplicates.");
    //特征和匹配信息重新赋值
    features = features_subset;
    pairwise_matches = pairwise_matches_subset;

    return indices;    //返回索引值
}

3.3 应用

 

下面给出相机参数评估部分的应用,得到的参数在终端内输出:

#include "opencv2/core/core.hpp"
#include "highgui.h"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/nonfree/nonfree.hpp"
#include "opencv2/legacy/legacy.hpp"

#include "opencv2/stitching/detail/autocalib.hpp"
#include "opencv2/stitching/detail/blenders.hpp"
#include "opencv2/stitching/detail/camera.hpp"
#include "opencv2/stitching/detail/exposure_compensate.hpp"
#include "opencv2/stitching/detail/matchers.hpp"
#include "opencv2/stitching/detail/motion_estimators.hpp"
#include "opencv2/stitching/detail/seam_finders.hpp"
#include "opencv2/stitching/detail/util.hpp"
#include "opencv2/stitching/detail/warpers.hpp"
#include "opencv2/stitching/warpers.hpp"

#include <iostream>
#include <fstream> 
#include <string>
#include <iomanip> 
using namespace cv;
using namespace std;
using namespace detail;

int main(int argc, char** argv)
{
   
   vector<Mat> imgs;    //得到待拼接图像队列	
   Mat img = imread("1.jpg");
   imgs.push_back(img);
   img = imread("2.jpg");
   imgs.push_back(img);

   Ptr<FeaturesFinder> finder;    //特征检测
   finder = new SurfFeaturesFinder();
   vector<ImageFeatures> features(2);
    (*finder)(imgs[0], features[0]);
    (*finder)(imgs[1], features[1]);

   vector<MatchesInfo> pairwise_matches;    //特征匹配
   BestOf2NearestMatcher matcher(false, 0.3f, 6, 6);
   matcher(features, pairwise_matches);

   HomographyBasedEstimator estimator;    //定义参数评估器
   vector<CameraParams> cameras;    //表示相机参数矢量队列
   estimator(features, pairwise_matches, cameras);    //相机参数评估

   cout<<"相机参数的初次评估:"<<endl;    //终端输出
   for (size_t i = 0; i < cameras.size(); ++i)
   {
      Mat R;
      cameras[i].R.convertTo(R, CV_32F);    //数据类型转换
      cameras[i].R = R;
      cout<<"第"<<i+1<<"个相机的内参数"<< ":\n" << cameras[i].K()<<endl;
      cout<<"第"<<i+1<<"个相机的旋转参数"<< ":\n" << R<<endl;
      cout<<"第"<<i+1<<"个相机的焦距"<< ":\n" << cameras[i].focal<<endl;
   }

   Ptr<detail::BundleAdjusterBase> adjuster;    //光束平差法,精确相机参数
   adjuster = new detail::BundleAdjusterReproj();    //重映射误差方法
   //adjuster = new detail::BundleAdjusterRay();    //射线发散误差方法
    
   adjuster->setConfThresh(1);    //设置匹配置信度,该值为1
   (*adjuster)(features, pairwise_matches, cameras);    //相机参数的精确评估

   cout<<"相机参数的精确评估:"<<endl;    //终端输出
   for (size_t i = 0; i < cameras.size(); ++i)  
   {  
      cout<<"第"<<i+1<<"个相机的内参数"<< ":\n" << cameras[i].K()<<endl;
      cout<<"第"<<i+1<<"个相机的旋转参数"<< ":\n" << cameras[i].R<<endl;
   }  

   vector<Mat> rmats;
   for (size_t i = 0; i < cameras.size(); ++i)
      rmats.push_back(cameras[i].R.clone());    //复制相机的旋转参数
   waveCorrect(rmats, WAVE_CORRECT_HORIZ);    //进行波形校正
   for (size_t i = 0; i < cameras.size(); ++i)
      cameras[i].R = rmats[i];    //赋值

   cout<<"波形校正:"<<endl;    //终端输出
   for (size_t i = 0; i < cameras.size(); ++i)
   {
      cout<<"第"<<i+1<<"个相机的旋转参数"<< ":\n" << cameras[i].R<<endl;
   }
   return 0;
}

最后的终端输出为:

 

相机参数的初次评估:

第1个相机的内参数:

[8828.962705830743,  0,  1632;

  0,  8828.962705830743,  916;

  0,  0,  1]

第1个相机的旋转参数:

[1,  0, 0;

  0,  1,  0;

  0,  0,  1]

第1个相机的焦距:

8828.96

第2个相机的内参数:

[8828.962705830743,  0,  1632;

  0,  8828.962705830743,  916;

  0,  0,  1]

第2个相机的旋转参数:

[0.96937662,  -0.010653165,  0.2045967;

 0.014337449,  0.9847675,  0.016997401;

 -0.1385226,  -0.023688598,  0.97040582]

第2个相机的焦距:

8828.96

相机参数的精确评估:

第1个相机的内参数:

[9870.390441988358,  0,  1741.665818978845;

  0,  12324.5895921202,  916.6138494133298;

  0,  0,  1]

第1个相机的旋转参数:

[1,  -2.066372e-009, 0;

 1.6589183e-009,  1,  0;

  0,  0,  1]

第2个相机的内参数:

[9869.984266558649,  0,  1516.680944868967;

  0,  12323.20985076497,  862.0564899921652;

  0,  0,  1]

第2个相机的旋转参数:

[0.98646808,  -0.012547051,  0.16347259;

 0.011426224,  0.99990433,  0.0077948803;

 -0.16355476,  -0.0058215279,  0.98651713]

波形校正:

第1个相机的旋转参数:

[0.99661297,  0.0057310974,  -0.082034826;

  -1.0345258e-010, 0.99756861,  0.069691904;

 0.082234778,  -0.069455855,  0.9941898]

第2个相机的旋转参数:

[0.99660957,  -0.006296434,  0.082034826;

 -9.3132257e-010,  0.99706745,  0.076528184;

 -0.082276106,  -0.07626871,  0.99368697]

展开阅读全文

没有更多推荐了,返回首页