Opencv2.4.9源码分析——Stitching（三）
2018-02-19 10:41
288 查看
3、相机参数评估3.1 原理相机参数的评估也称为相机定标。要想理解这部分内容，首先应该从成像原理开始讲起。
图6 小孔成像原理从图6可以看出，真实物体通过小孔映射到成像平面上，小孔到成像平面的距离称为焦距f。在成像平面上的图像是镜像倒立的，所以为了研究方便，在小孔和物体之间定义一个虚拟成像平面（在后面，我们把该平面也称为成像平面），它与小孔的距离也为焦距，则两个成像平面的图像大小是相同的，但虚拟成像平面上的图像与原物体的方向是一样的。
图7 成像的几何模型我们以小孔为坐标原点建立一个三维直角坐标系XYZ（如图7所示），坐标原点C称为相机的光心。成像平面xy平行于XY，并且距离光心C为f，其中Z轴定义为光轴，它与成像平面xy的交点为P，因此CP＝f。设空间中的任一点Q的坐标为（X, Y, Z），该点映射到成像平面的点q的坐标为(x, y,f)。由几何知识可得：
（31）式中，λ＝Z/f，则x＝fX/Z，y＝fY/Z。因此空间中Q点的三维坐标映射到成像平面的二维坐标q点在齐次坐标下的线性映射关系为：
（32）如图7所示，在成像平面（坐标系为xy）的坐标原点为P，但图像（设坐标系为uv）的坐标原点一般在左上角，所以这两个坐标系之间需要通过平移来进行转换。另外成像平面xy是长度单位，而相机图像传感器的单位是像素，因此像素与长度之间也是需要转换的，而且水平和垂直的像素往往是不相同的，所以横纵轴的转换系数是不一致。因此式32改写为：
（33）式中，fu和fv为焦距f在横纵轴的长度和像素的转换，它们之间的关系可以写为fv＝αfu，(cx, cy)为坐标平移。因为矩阵K的参数都是基于相机内部自身的参数，因此K称为相机内参数，K＝TV。相机除了具有内参数外，还有外参数。式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）由于相机只做旋转处理（或者我们认为物体离相机很远），则t0＝t1＝0，从而得到(u0, v0)和(u1, v1)的关系为：
（37）式中，R10为由图像1到图像0的相对旋转矩阵，R10＝R1R0-1。由式2可得：
（38）为简单起见，我们设两幅图像的相机内参数中的坐标平移都为0，即K＝V，因为抛弃参数T对图像拼接影响不大。我们在评估焦距时，还需定义f1u＝f1v＝f1，f0u＝f0v＝f0，即设图像的长宽等像素：
（39）则式38表示为
（40）式中，R10＝[rij]。由式40我们就可以得到焦距f0和f1：观察矩阵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、α、cx、cy、rx、ry和rz。前4个参数是内参数（见式33），后3个参数是外参数（即式55中的旋转向量的三个元素）。因此式25中的h为h(fu, α, cx, cy, rx, ry,rz)，由此得到J(h)为：
（59）式中，n表示待拼接的图像数量，也就是所有的相机。有时为了计算方便，导数可以用差分近似，例如我们要计算e对fu的偏导，则
（60）式中，∆表示一个很小的数。
图9 射线发散概念第二种误差指标函数是基于射线发散原理。如图9所示，不同的相机发出的射线透过相片后达到同一个物点，但由于误差，两条射线不会相交，或者称为两条射线发散了，我们就把这两条射线间的最短距离d定义为射线发散误差。下面，我们不加证明的给出射线发散误差的计算公式：设(u1, v1, 1)和(u2, v2, 1)为两幅不同图像的同一特征点的齐次坐标，则由单应矩阵H1和H2分别得到它们的物点坐标为：
（61）分别对坐标进行归一化处理：
（62）
（63）为了简化计算，射线间的最短距离可以表示为基于不同焦距f1和f2下的归一化后的物点坐标之差：
（64）式64说明在计算射线发散误差时，每一个内点有x轴、y轴和z轴三个误差值。我们假设射线就是相机的光轴，因此单应矩阵H中的参数α＝1，cx＝cy＝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函数：[cpp] view plain copyvoid 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");
}
评估相机焦距：[cpp] view plain copyvoid 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; //平均值
}
}
从单应矩阵中得到焦距：[cpp] view plain copyvoid 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
}
构建最大生成树：[cpp] view plain copyvoid 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);
}
计算旋转矩阵：[cpp] view plain copystruct 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，它的作用是细化相机参数：[cpp] view plain copyvoid 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算法迭代之前，要赋予相机参数初值：[cpp] view plain copyvoid 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函数表示得到最终的精确的相机参数：[cpp] view plain copyvoid 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：[cpp] view plain copyvoid 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：[cpp] view plain copyvoid 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类：[cpp] view plain copyvoid 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
}
}
[cpp] view plain copyvoid 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; //赋值
}
}
[cpp] view plain copyvoid 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++; //累计
}
}
}
[cpp] view plain copyvoid 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;
}
}
}
下面给出波形校正函数：[cpp] view plain copyvoid 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值的大小：[cpp] view plain copyvector<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 应用 下面给出相机参数评估部分的应用，得到的参数在终端内输出：[cpp] view plain copy#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]
转载http://blog.csdn.net/zhaocj/article/details/78809143
图6 小孔成像原理从图6可以看出，真实物体通过小孔映射到成像平面上，小孔到成像平面的距离称为焦距f。在成像平面上的图像是镜像倒立的，所以为了研究方便，在小孔和物体之间定义一个虚拟成像平面（在后面，我们把该平面也称为成像平面），它与小孔的距离也为焦距，则两个成像平面的图像大小是相同的，但虚拟成像平面上的图像与原物体的方向是一样的。
图7 成像的几何模型我们以小孔为坐标原点建立一个三维直角坐标系XYZ（如图7所示），坐标原点C称为相机的光心。成像平面xy平行于XY，并且距离光心C为f，其中Z轴定义为光轴，它与成像平面xy的交点为P，因此CP＝f。设空间中的任一点Q的坐标为（X, Y, Z），该点映射到成像平面的点q的坐标为(x, y,f)。由几何知识可得：
（31）式中，λ＝Z/f，则x＝fX/Z，y＝fY/Z。因此空间中Q点的三维坐标映射到成像平面的二维坐标q点在齐次坐标下的线性映射关系为：
（32）如图7所示，在成像平面（坐标系为xy）的坐标原点为P，但图像（设坐标系为uv）的坐标原点一般在左上角，所以这两个坐标系之间需要通过平移来进行转换。另外成像平面xy是长度单位，而相机图像传感器的单位是像素，因此像素与长度之间也是需要转换的，而且水平和垂直的像素往往是不相同的，所以横纵轴的转换系数是不一致。因此式32改写为：
（33）式中，fu和fv为焦距f在横纵轴的长度和像素的转换，它们之间的关系可以写为fv＝αfu，(cx, cy)为坐标平移。因为矩阵K的参数都是基于相机内部自身的参数，因此K称为相机内参数，K＝TV。相机除了具有内参数外，还有外参数。式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）由于相机只做旋转处理（或者我们认为物体离相机很远），则t0＝t1＝0，从而得到(u0, v0)和(u1, v1)的关系为：
（37）式中，R10为由图像1到图像0的相对旋转矩阵，R10＝R1R0-1。由式2可得：
（38）为简单起见，我们设两幅图像的相机内参数中的坐标平移都为0，即K＝V，因为抛弃参数T对图像拼接影响不大。我们在评估焦距时，还需定义f1u＝f1v＝f1，f0u＝f0v＝f0，即设图像的长宽等像素：
（39）则式38表示为
（40）式中，R10＝[rij]。由式40我们就可以得到焦距f0和f1：观察矩阵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、α、cx、cy、rx、ry和rz。前4个参数是内参数（见式33），后3个参数是外参数（即式55中的旋转向量的三个元素）。因此式25中的h为h(fu, α, cx, cy, rx, ry,rz)，由此得到J(h)为：
（59）式中，n表示待拼接的图像数量，也就是所有的相机。有时为了计算方便，导数可以用差分近似，例如我们要计算e对fu的偏导，则
（60）式中，∆表示一个很小的数。
图9 射线发散概念第二种误差指标函数是基于射线发散原理。如图9所示，不同的相机发出的射线透过相片后达到同一个物点，但由于误差，两条射线不会相交，或者称为两条射线发散了，我们就把这两条射线间的最短距离d定义为射线发散误差。下面，我们不加证明的给出射线发散误差的计算公式：设(u1, v1, 1)和(u2, v2, 1)为两幅不同图像的同一特征点的齐次坐标，则由单应矩阵H1和H2分别得到它们的物点坐标为：
（61）分别对坐标进行归一化处理：
（62）
（63）为了简化计算，射线间的最短距离可以表示为基于不同焦距f1和f2下的归一化后的物点坐标之差：
（64）式64说明在计算射线发散误差时，每一个内点有x轴、y轴和z轴三个误差值。我们假设射线就是相机的光轴，因此单应矩阵H中的参数α＝1，cx＝cy＝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函数：[cpp] view plain copyvoid 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");
}
评估相机焦距：[cpp] view plain copyvoid 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; //平均值
}
}
从单应矩阵中得到焦距：[cpp] view plain copyvoid 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
}
构建最大生成树：[cpp] view plain copyvoid 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);
}
计算旋转矩阵：[cpp] view plain copystruct 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，它的作用是细化相机参数：[cpp] view plain copyvoid 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算法迭代之前，要赋予相机参数初值：[cpp] view plain copyvoid 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函数表示得到最终的精确的相机参数：[cpp] view plain copyvoid 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：[cpp] view plain copyvoid 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：[cpp] view plain copyvoid 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类：[cpp] view plain copyvoid 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
}
}
[cpp] view plain copyvoid 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; //赋值
}
}
[cpp] view plain copyvoid 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++; //累计
}
}
}
[cpp] view plain copyvoid 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;
}
}
}
下面给出波形校正函数：[cpp] view plain copyvoid 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值的大小：[cpp] view plain copyvector<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 应用 下面给出相机参数评估部分的应用，得到的参数在终端内输出：[cpp] view plain copy#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]
转载http://blog.csdn.net/zhaocj/article/details/78809143
内容来自用户分享和网络整理，不保证内容的准确性，如有侵权内容，可联系管理员处理 
相关文章推荐
- Opencv2.4.9源码分析——Stitching（六）
- Opencv2.4.9源码分析——Stitching（八）
- Opencv2.4.9源码分析——Stitching（九）
- Opencv2.4.9源码分析——Stitching（四）
- Opencv2.4.9源码分析——Stitching（一）
- Opencv2.4.9源码分析——Stitching（三）
- Opencv2.4.9源码分析——Stitching（二）
- Opencv2.4.9源码分析——Stitching（八）
- Opencv2.4.9源码分析——Stitching（五）
- Opencv2.4.9源码分析——Stitching（七）
- Opencv2.4.9源码分析——HoughLinesP
- Opencv2.4.9源码分析——FAST
- Opencv2.4.9源码分析——Cascade Classification（三）
- Opencv2.4.9源码分析——adaptiveBilateralFilter
- Opencv2.4.9源码分析——DenseFeatureDetector
- Opencv2.4.9源码分析——MSER
- Opencv2.4.9源码分析——HoughLinesP
- Opencv2.4.9源码分析要点摘录
- Opencv2.4.9源码分析——SURF
- Opencv2.4.9源码分析——HoughLines