宝塔面板怎么做多个网站,邯郸网站制作多少钱,做网站多少钱PageAdmin,企业管理咨询是一种单目初始化
1.前提条件#xff08;640*480#xff09;
参与初始化的两帧各自的特征点数目都需要大于100.两帧特征点成功匹配的数目需要大于或等于100.两帧特征点三角化成功的三维点数目需要大于50.
2.针对条件三 流程如下
记录当前帧和参考帧#xff08;第一帧#xff…单目初始化
1.前提条件640*480
参与初始化的两帧各自的特征点数目都需要大于100.两帧特征点成功匹配的数目需要大于或等于100.两帧特征点三角化成功的三维点数目需要大于50.
2.针对条件三 流程如下
记录当前帧和参考帧第一帧之间的特征匹配关系。在特征匹配点对中随机选择8对匹配点作为一组。对这8对点分别计算基础矩阵F和单应矩阵H并得到得分。计算得分比例根据判断选择基础矩阵还是单应矩阵求位姿和三角化。
3. 求单应矩阵--平面
对特征匹配点坐标进行归一化。开始迭代每次选择8对点计算单应矩阵。根据当次计算的单应矩阵的重投影误差对结果进行评分。重复2、3步以得分最高的单应矩阵作为最优的单应矩阵。
3.1坐标归一化
目的防止坐标值量级差别较大。
好处能够提高运算结果的精度利用归一化处理后的图像坐标对尺度缩放和原点的选择不敏感。
归一化操作步骤如下
1.计算每个特征点坐标分别在两个坐标轴方向上的均值meanXmeanY
2.求出平均到每个点上其坐标偏离均值meanX、meanY的程度记为meanDevXmeanDevY并将其倒数作为一个尺度缩放因子sXsY。
3.用均值和尺度缩放因子对坐标进行归一化
4.用矩阵表示上述变换关系得到归一化矩阵T
归一化代码如下 * 为什么要归一化* 在相似变换之后(点在不同的坐标系下),他们的单应性矩阵是不相同的* 如果图像存在噪声,使得点的坐标发生了变化,那么它的单应性矩阵也会发生变化* 我们采取的方法是将点的坐标放到同一坐标系下,并将缩放尺度也进行统一 * 对同一幅图像的坐标进行相同的变换,不同图像进行不同变换* 缩放尺度是为了让噪声对于图像的影响在一个数量级上* * Step 1 计算特征点X,Y坐标的均值 * Step 2 计算特征点X,Y坐标离均值的平均偏离程度* Step 3 将x坐标和y坐标分别进行尺度归一化使得x坐标和y坐标的一阶绝对矩分别为1 * Step 4 计算归一化矩阵其实就是前面做的操作用矩阵变换来表示而已* * param[in] vKeys 待归一化的特征点* param[in out] vNormalizedPoints 特征点归一化后的坐标* param[in out] T 归一化特征点的变换矩阵*/
void Initializer::Normalize(const vectorcv::KeyPoint vKeys, vectorcv::Point2f vNormalizedPoints, cv::Mat T) //将特征点归一化的矩阵
{// 归一化的是这些点在x方向和在y方向上的一阶绝对矩随机变量的期望。// Step 1 计算特征点X,Y坐标的均值 meanX, meanYfloat meanX 0;float meanY 0;//获取特征点的数量const int N vKeys.size();//设置用来存储归一后特征点的向量大小和归一化前保持一致vNormalizedPoints.resize(N);//开始遍历所有的特征点for(int i0; iN; i){//分别累加特征点的X、Y坐标meanX vKeys[i].pt.x;meanY vKeys[i].pt.y;}//计算X、Y坐标的均值meanX meanX/N;meanY meanY/N;// Step 2 计算特征点X,Y坐标离均值的平均偏离程度 meanDevX, meanDevY注意不是标准差float meanDevX 0;float meanDevY 0;// 将原始特征点减去均值坐标使x坐标和y坐标均值分别为0for(int i0; iN; i){vNormalizedPoints[i].x vKeys[i].pt.x - meanX;vNormalizedPoints[i].y vKeys[i].pt.y - meanY;//累计这些特征点偏离横纵坐标均值的程度meanDevX fabs(vNormalizedPoints[i].x);meanDevY fabs(vNormalizedPoints[i].y);}// 求出平均到每个点上其坐标偏离横纵坐标均值的程度将其倒数作为一个尺度缩放因子meanDevX meanDevX/N;meanDevY meanDevY/N;float sX 1.0/meanDevX;float sY 1.0/meanDevY;// Step 3 将x坐标和y坐标分别进行尺度归一化使得x坐标和y坐标的一阶绝对矩分别为1 // 这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值期望for(int i0; iN; i){//对就是简单地对特征点的坐标进行进一步的缩放vNormalizedPoints[i].x vNormalizedPoints[i].x * sX;vNormalizedPoints[i].y vNormalizedPoints[i].y * sY;}// Step 4 计算归一化矩阵其实就是前面做的操作用矩阵变换来表示而已// |sX 0 -meanx*sX|// |0 sY -meany*sY|// |0 0 1 |T cv::Mat::eye(3,3,CV_32F);T.atfloat(0,0) sX;T.atfloat(1,1) sY;T.atfloat(0,2) -meanX*sX;T.atfloat(1,2) -meanY*sY;
}
3.2求解单应矩阵
单应矩阵H的自由度是8一对特征点可以提供两个约束方程所以只需要4对点提供8个约束方程就可以求解了。
为什么自由度是8因为Ah0矩阵H共有9个元素去掉一个自由度就是8个自由度。 对矩阵A进行SVD分解 代码
/*** brief 用DLT方法求解单应矩阵H* 这里最少用4对点就能够求出来不过这里为了统一还是使用了8对点求最小二乘解* * param[in] vP1 参考帧中归一化后的特征点* param[in] vP2 当前帧中归一化后的特征点* return cv::Mat 计算的单应矩阵H*/
cv::Mat Initializer::ComputeH21(const vectorcv::Point2f vP1, //归一化后的点, in reference frameconst vectorcv::Point2f vP2) //归一化后的点, in current frame
{// 基本原理见附件推导过程// |x| | h1 h2 h3 ||x|// |y| a | h4 h5 h6 ||y| 简写: x a H x, a为一个尺度因子// |1 | | h7 h8 h9 ||1|// 使用DLT(direct linear tranform)求解该模型// x a H x // --- (x) 叉乘 (H x) 0 (因为方向相同) (取前两行就可以推导出下面的了)// --- Ah 0 // A | 0 0 0 -x -y -1 xy yy y| h | h1 h2 h3 h4 h5 h6 h7 h8 h9 |// |-x -y -1 0 0 0 xx yx x|// 通过SVD求解Ah 0A^T*A最小特征值对应的特征向量即为解// 其实也就是右奇异值矩阵的最后一列//获取参与计算的特征点的数目const int N vP1.size();// 构造用于计算的矩阵 A cv::Mat A(2*N, //行注意每一个点的数据对应两行9, //列CV_32F); //float数据类型// 构造矩阵A将每个特征点添加到矩阵A中的元素for(int i0; iN; i){//获取特征点对的像素坐标const float u1 vP1[i].x;const float v1 vP1[i].y;const float u2 vP2[i].x;const float v2 vP2[i].y;//生成这个点的第一行A.atfloat(2*i,0) 0.0;A.atfloat(2*i,1) 0.0;A.atfloat(2*i,2) 0.0;A.atfloat(2*i,3) -u1;A.atfloat(2*i,4) -v1;A.atfloat(2*i,5) -1;A.atfloat(2*i,6) v2*u1;A.atfloat(2*i,7) v2*v1;A.atfloat(2*i,8) v2;//生成这个点的第二行A.atfloat(2*i1,0) u1;A.atfloat(2*i1,1) v1;A.atfloat(2*i1,2) 1;A.atfloat(2*i1,3) 0.0;A.atfloat(2*i1,4) 0.0;A.atfloat(2*i1,5) 0.0;A.atfloat(2*i1,6) -u2*u1;A.atfloat(2*i1,7) -u2*v1;A.atfloat(2*i1,8) -u2;}// 定义输出变量u是左边的正交矩阵U w为奇异矩阵vt中的t表示是右正交矩阵V的转置cv::Mat u,w,vt;//使用opencv提供的进行奇异值分解的函数cv::SVDecomp(A, //输入待进行奇异值分解的矩阵w, //输出奇异值矩阵u, //输出矩阵Uvt, //输出矩阵V^Tcv::SVD::MODIFY_A | //输入MODIFY_A是指允许计算函数可以修改待分解的矩阵官方文档上说这样可以加快计算速度、节省内存cv::SVD::FULL_UV); //FULL_UV把U和VT补充成单位正交方阵// 返回最小奇异值所对应的右奇异向量// 注意前面说的是右奇异值矩阵的最后一列但是在这里因为是vt转置后了所以是行由于A有9列数据故最后一列的下标为8return vt.row(8).reshape(0, //转换后的通道数这里设置为0表示是与前面相同3); //转换后的行数,对应V的最后一列
}
3.3检验单应矩阵并评分
通过单应矩阵对已经判断为内点的特征点进行双向投影计算加权重投影误差最终选择误差最小的单应矩阵作为最优解。
特征点对之间双向投影累计所有特征点对中的内点误差误差越大评分越低。
/*** brief 对给定的homography matrix打分,需要使用到卡方检验的知识* * param[in] H21 从参考帧到当前帧的单应矩阵* param[in] H12 从当前帧到参考帧的单应矩阵* param[in] vbMatchesInliers 匹配好的特征点对的Inliers标记* param[in] sigma 方差默认为1* return float 返回得分*/
float Initializer::CheckHomography(const cv::Mat H21, //从参考帧到当前帧的单应矩阵const cv::Mat H12, //从当前帧到参考帧的单应矩阵vectorbool vbMatchesInliers, //匹配好的特征点对的Inliers标记float sigma) //估计误差
{// 说明在已值n维观测数据误差服从N(0sigma的高斯分布时// 其误差加权最小二乘结果为 sum_error SUM(e(i)^T * Q^(-1) * e(i))// 其中e(i) [e_x,e_y,...]^T, Q维观测数据协方差矩阵即sigma * sigma组成的协方差矩阵// 误差加权最小二次结果越小说明观测数据精度越高// 那么score SUM((th - e(i)^T * Q^(-1) * e(i)))的分数就越高// 算法目标 检查单应变换矩阵// 检查方式通过H矩阵进行参考帧和当前帧之间的双向投影并计算起加权最小二乘投影误差// 算法流程// input: 单应性矩阵 H21, H12, 匹配点集 mvKeys1// do:// for p1(i), p2(i) in mvKeys:// error_i1 ||p2(i) - H21 * p1(i)||2// error_i2 ||p1(i) - H12 * p2(i)||2// // w1 1 / sigma / sigma// w2 1 / sigma / sigma// // if error1 th// score th - error_i1 * w1// if error2 th// score th - error_i2 * w2// // if error_1i th or error_2i th// p1(i), p2(i) are inner points// vbMatchesInliers(i) true// else // p1(i), p2(i) are outliers// vbMatchesInliers(i) false// end// end// output: score, inliers// 特点匹配个数const int N mvMatches12.size();// Step 1 获取从参考帧到当前帧的单应矩阵的各个元素const float h11 H21.atfloat(0,0);const float h12 H21.atfloat(0,1);const float h13 H21.atfloat(0,2);const float h21 H21.atfloat(1,0);const float h22 H21.atfloat(1,1);const float h23 H21.atfloat(1,2);const float h31 H21.atfloat(2,0);const float h32 H21.atfloat(2,1);const float h33 H21.atfloat(2,2);// 获取从当前帧到参考帧的单应矩阵的各个元素const float h11inv H12.atfloat(0,0);const float h12inv H12.atfloat(0,1);const float h13inv H12.atfloat(0,2);const float h21inv H12.atfloat(1,0);const float h22inv H12.atfloat(1,1);const float h23inv H12.atfloat(1,2);const float h31inv H12.atfloat(2,0);const float h32inv H12.atfloat(2,1);const float h33inv H12.atfloat(2,2);// 给特征点对的Inliers标记预分配空间vbMatchesInliers.resize(N);// 初始化score值float score 0;// 基于卡方检验计算出的阈值假设测量有一个像素的偏差// 自由度为2的卡方分布显著性水平为0.05对应的临界阈值const float th 5.991;//信息矩阵方差平方的倒数const float invSigmaSquare 1.0/(sigma * sigma);// Step 2 通过H矩阵进行参考帧和当前帧之间的双向投影并计算起加权重投影误差// H21 表示从img1 到 img2的变换矩阵// H12 表示从img2 到 img1的变换矩阵 for(int i 0; i N; i){// 一开始都默认为Inlierbool bIn true;// Step 2.1 提取参考帧和当前帧之间的特征匹配点对const cv::KeyPoint kp1 mvKeys1[mvMatches12[i].first];const cv::KeyPoint kp2 mvKeys2[mvMatches12[i].second];const float u1 kp1.pt.x;const float v1 kp1.pt.y;const float u2 kp2.pt.x;const float v2 kp2.pt.y;// Step 2.2 计算 img2 到 img1 的重投影误差// x1 H12*x2// 将图像2中的特征点通过单应变换投影到图像1中// |u1| |h11inv h12inv h13inv||u2| |u2in1|// |v1| |h21inv h22inv h23inv||v2| |v2in1| * w2in1inv// |1 | |h31inv h32inv h33inv||1 | | 1 |// 计算投影归一化坐标const float w2in1inv 1.0/(h31inv * u2 h32inv * v2 h33inv);const float u2in1 (h11inv * u2 h12inv * v2 h13inv) * w2in1inv;const float v2in1 (h21inv * u2 h22inv * v2 h23inv) * w2in1inv;// 计算重投影误差 ||p1(i) - H12 * p2(i)||2const float squareDist1 (u1 - u2in1) * (u1 - u2in1) (v1 - v2in1) * (v1 - v2in1);const float chiSquare1 squareDist1 * invSigmaSquare;// Step 2.3 用阈值标记离群点内点的话累加得分if(chiSquare1th)bIn false; else// 误差越大得分越低score th - chiSquare1;// 计算从img1 到 img2 的投影变换误差// x1in2 H21*x1// 将图像2中的特征点通过单应变换投影到图像1中// |u2| |h11 h12 h13||u1| |u1in2|// |v2| |h21 h22 h23||v1| |v1in2| * w1in2inv// |1 | |h31 h32 h33||1 | | 1 |// 计算投影归一化坐标const float w1in2inv 1.0/(h31*u1h32*v1h33);const float u1in2 (h11*u1h12*v1h13)*w1in2inv;const float v1in2 (h21*u1h22*v1h23)*w1in2inv;// 计算重投影误差 const float squareDist2 (u2-u1in2)*(u2-u1in2)(v2-v1in2)*(v2-v1in2);const float chiSquare2 squareDist2*invSigmaSquare;// 用阈值标记离群点内点的话累加得分if(chiSquare2th)bIn false;elsescore th - chiSquare2; // Step 2.4 如果从img2 到 img1 和 从img1 到img2的重投影误差均满足要求则说明是Inlier pointif(bIn)vbMatchesInliers[i]true;elsevbMatchesInliers[i]false;}return score;
}
4.求基础矩阵F--非平面
对特征匹配点坐标进行归一化选择8个点对来计算基础矩阵。根据当次计算的基础矩阵的重投影误差对结果进行评分。重复2、3步以得分最高的基础矩阵作为最优的基础矩阵。
4.1使用八点法求解基础矩阵 基础矩阵共有9个元素其中尺度等价性去掉1个自由度基础矩阵秩为2再去掉1个自由度所以自由度应该为7. SVD求解基础矩阵
/*** brief 根据特征点匹配求fundamental matrixnormalized 8点法* 注意F矩阵有秩为2的约束所以需要两次SVD分解* * param[in] vP1 参考帧中归一化后的特征点* param[in] vP2 当前帧中归一化后的特征点* return cv::Mat 最后计算得到的基础矩阵F*/
cv::Mat Initializer::ComputeF21(const vectorcv::Point2f vP1, //归一化后的点, in reference frameconst vectorcv::Point2f vP2) //归一化后的点, in current frame
{// 原理详见附件推导// xFx 0 整理可得Af 0// A | xx xy x yx yy y x y 1 |, f | f1 f2 f3 f4 f5 f6 f7 f8 f9 |// 通过SVD求解Af 0AA最小特征值对应的特征向量即为解//获取参与计算的特征点对数const int N vP1.size();//初始化A矩阵cv::Mat A(N,9,CV_32F); // N*9维// 构造矩阵A将每个特征点添加到矩阵A中的元素for(int i0; iN; i){const float u1 vP1[i].x;const float v1 vP1[i].y;const float u2 vP2[i].x;const float v2 vP2[i].y;A.atfloat(i,0) u2*u1;A.atfloat(i,1) u2*v1;A.atfloat(i,2) u2;A.atfloat(i,3) v2*u1;A.atfloat(i,4) v2*v1;A.atfloat(i,5) v2;A.atfloat(i,6) u1;A.atfloat(i,7) v1;A.atfloat(i,8) 1;}//存储奇异值分解结果的变量cv::Mat u,w,vt;// 定义输出变量u是左边的正交矩阵U w为奇异矩阵vt中的t表示是右正交矩阵V的转置cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 转换成基础矩阵的形式cv::Mat Fpre vt.row(8).reshape(0, 3); // v的最后一列//基础矩阵的秩为2,而我们不敢保证计算得到的这个结果的秩为2,所以需要通过第二次奇异值分解,来强制使其秩为2// 对初步得来的基础矩阵进行第2次奇异值分解cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 秩2约束强制将第3个奇异值设置为0w.atfloat(2)0; // 重新组合好满足秩约束的基础矩阵作为最终计算结果返回 return u*cv::Mat::diag(w)*vt;
}
4.2验证基础矩阵并评分
计算点到极线的距离并求和距离越大误差越大。p2到l2和p1到l1.
代码
/*** brief 对给定的Fundamental matrix打分* * param[in] F21 当前帧和参考帧之间的基础矩阵* param[in] vbMatchesInliers 匹配的特征点对属于inliers的标记* param[in] sigma 方差默认为1* return float 返回得分*/
float Initializer::CheckFundamental(const cv::Mat F21, //当前帧和参考帧之间的基础矩阵vectorbool vbMatchesInliers, //匹配的特征点对属于inliers的标记float sigma) //方差
{// 说明在已值n维观测数据误差服从N(0sigma的高斯分布时// 其误差加权最小二乘结果为 sum_error SUM(e(i)^T * Q^(-1) * e(i))// 其中e(i) [e_x,e_y,...]^T, Q维观测数据协方差矩阵即sigma * sigma组成的协方差矩阵// 误差加权最小二次结果越小说明观测数据精度越高// 那么score SUM((th - e(i)^T * Q^(-1) * e(i)))的分数就越高// 算法目标检查基础矩阵// 检查方式利用对极几何原理 p2^T * F * p1 0// 假设三维空间中的点 P 在 img1 和 img2 两图像上的投影分别为 p1 和 p2两个为同名点// 则p2 一定存在于极线 l2 上即 p2*l2 0. 而l2 F*p1 (a, b, c)^T// 所以这里的误差项 e 为 p2 到 极线 l2 的距离如果在直线上则 e 0// 根据点到直线的距离公式d (ax by c) / sqrt(a * a b * b)// 所以e (a * p2.x b * p2.y c) / sqrt(a * a b * b)// 算法流程// input: 基础矩阵 F 左右视图匹配点集 mvKeys1// do:// for p1(i), p2(i) in mvKeys:// l2 F * p1(i)// l1 p2(i) * F// error_i1 dist_point_to_line(x2,l2)// error_i2 dist_point_to_line(x1,l1)// // w1 1 / sigma / sigma// w2 1 / sigma / sigma// // if error1 th// score thScore - error_i1 * w1// if error2 th// score thScore - error_i2 * w2// // if error_1i th or error_2i th// p1(i), p2(i) are inner points// vbMatchesInliers(i) true// else // p1(i), p2(i) are outliers// vbMatchesInliers(i) false// end// end// output: score, inliers// 获取匹配的特征点对的总对数const int N mvMatches12.size();// Step 1 提取基础矩阵中的元素数据const float f11 F21.atfloat(0,0);const float f12 F21.atfloat(0,1);const float f13 F21.atfloat(0,2);const float f21 F21.atfloat(1,0);const float f22 F21.atfloat(1,1);const float f23 F21.atfloat(1,2);const float f31 F21.atfloat(2,0);const float f32 F21.atfloat(2,1);const float f33 F21.atfloat(2,2);// 预分配空间vbMatchesInliers.resize(N);// 设置评分初始值因为后面需要进行这个数值的累计float score 0;// 基于卡方检验计算出的阈值// 自由度为1的卡方分布显著性水平为0.05对应的临界阈值// ?是因为点到直线距离是一个自由度吗const float th 3.841;// 自由度为2的卡方分布显著性水平为0.05对应的临界阈值const float thScore 5.991;// 信息矩阵或 协方差矩阵的逆矩阵const float invSigmaSquare 1.0/(sigma*sigma);// Step 2 计算img1 和 img2 在估计 F 时的score值for(int i0; iN; i){//默认为这对特征点是Inliersbool bIn true;// Step 2.1 提取参考帧和当前帧之间的特征匹配点对const cv::KeyPoint kp1 mvKeys1[mvMatches12[i].first];const cv::KeyPoint kp2 mvKeys2[mvMatches12[i].second];// 提取出特征点的坐标const float u1 kp1.pt.x;const float v1 kp1.pt.y;const float u2 kp2.pt.x;const float v2 kp2.pt.y;// Reprojection error in second image// Step 2.2 计算 img1 上的点在 img2 上投影得到的极线 l2 F21 * p1 (a2,b2,c2)const float a2 f11*u1f12*v1f13;const float b2 f21*u1f22*v1f23;const float c2 f31*u1f32*v1f33;// Step 2.3 计算误差 e (a * p2.x b * p2.y c) / sqrt(a * a b * b)const float num2 a2*u2b2*v2c2;const float squareDist1 num2*num2/(a2*a2b2*b2);// 带权重误差const float chiSquare1 squareDist1*invSigmaSquare;// Step 2.4 误差大于阈值就说明这个点是Outlier // ? 为什么判断阈值用的 th1自由度计算得分用的thScore2自由度// ? 可能是为了和CheckHomography 得分统一if(chiSquare1th)bIn false;else// 误差越大得分越低score thScore - chiSquare1;// 计算img2上的点在 img1 上投影得到的极线 l1 p2 * F21 (a1,b1,c1)const float a1 f11*u2f21*v2f31;const float b1 f12*u2f22*v2f32;const float c1 f13*u2f23*v2f33;// 计算误差 e (a * p2.x b * p2.y c) / sqrt(a * a b * b)const float num1 a1*u1b1*v1c1;const float squareDist2 num1*num1/(a1*a1b1*b1);// 带权重误差const float chiSquare2 squareDist2*invSigmaSquare;// 误差大于阈值就说明这个点是Outlier if(chiSquare2th)bIn false;elsescore thScore - chiSquare2;// Step 2.5 保存结果if(bIn)vbMatchesInliers[i]true;elsevbMatchesInliers[i]false;}// 返回评分return score;
}
5.特征点对三角化 代码
/** 给定投影矩阵P1,P2和图像上的匹配特征点点kp1,kp2从而计算三维点坐标* brief * * param[in] kp1 特征点, in reference frame* param[in] kp2 特征点, in current frame* param[in] P1 投影矩阵P1* param[in] P2 投影矩阵P2* param[in out] x3D 计算的三维点*/
void Initializer::Triangulate(const cv::KeyPoint kp1, //特征点, in reference frameconst cv::KeyPoint kp2, //特征点, in current frameconst cv::Mat P1, //投影矩阵P1const cv::Mat P2, //投影矩阵P2cv::Mat x3D) //三维点
{// 原理// Trianularization: 已知匹配特征点对{x x} 和 各自相机矩阵{P P}, 估计三维点 X// x PX x PX// 它们都属于 x aPX模型// |X|// |x| |p1 p2 p3 p4 ||Y| |x| |--p0--||.|// |y| a |p5 p6 p7 p8 ||Z| |y| a|--p1--||X|// |z| |p9 p10 p11 p12||1| |z| |--p2--||.|// 采用DLT的方法x叉乘PX 0// |yp2 - p1| |0|// |p0 - xp2| X |0|// |xp1 - yp0| |0|// 两个点:// |yp2 - p1 | |0|// |p0 - xp2 | X |0| AX 0// |yp2 - p1 | |0|// |p0 - xp2| |0|// 变成程序中的形式// |xp2 - p0 | |0|// |yp2 - p1 | X |0| AX 0// |xp2- p0| |0|// |yp2- p1| |0|// 然后就组成了一个四元一次正定方程组SVD求解右奇异矩阵的最后一行就是最终的解.//这个就是上面注释中的矩阵Acv::Mat A(4,4,CV_32F);//构造参数矩阵AA.row(0) kp1.pt.x*P1.row(2)-P1.row(0);A.row(1) kp1.pt.y*P1.row(2)-P1.row(1);A.row(2) kp2.pt.x*P2.row(2)-P2.row(0);A.row(3) kp2.pt.y*P2.row(2)-P2.row(1);//奇异值分解的结果cv::Mat u,w,vt;//对系数矩阵A进行奇异值分解cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);//根据前面的结论奇异值分解右矩阵的最后一行其实就是解原理类似于前面的求最小二乘解四个未知数四个方程正好正定//别忘了我们更习惯用列向量来表示一个点的空间坐标x3D vt.row(3).t();//为了符合其次坐标的形式使最后一维为1x3D x3D.rowRange(0,3)/x3D.atfloat(3);
}
6.检验三角化结果 确定一个合格的三维点需要通过以下关卡
三维点的3个坐标必须是有限的实数三维点深度值必须为正三维点和两帧图像光轴中心夹角需要满足一定的条件。夹角越大视差越大三角化结果越准确。三维点的重投影误差小于设定的阈值。
我们会记录当前位姿对应的合格的三维点数目和视差。位姿可能有多组解根据如下条件选择最佳的解。
最优解成功三角化点数目明显大于次优解的点数目。最优解的视差大于设定点阈值。最优解成功三角化点数目大于设定的阈值。最优解成功三角化点数目占所有特征点数目的90%以上。
代码
/*** brief 用位姿来对特征匹配点三角化从中筛选中合格的三维点* * param[in] R 旋转矩阵R* param[in] t 平移矩阵t* param[in] vKeys1 参考帧特征点 * param[in] vKeys2 当前帧特征点* param[in] vMatches12 两帧特征点的匹配关系* param[in] vbMatchesInliers 特征点对内点标记* param[in] K 相机内参矩阵* param[in out] vP3D 三角化测量之后的特征点的空间坐标* param[in] th2 重投影误差的阈值* param[in out] vbGood 标记成功三角化点* param[in out] parallax 计算出来的比较大的视差角注意不是最大具体看后面代码* return int */
int Initializer::CheckRT(const cv::Mat R, const cv::Mat t, const vectorcv::KeyPoint vKeys1, const vectorcv::KeyPoint vKeys2,const vectorMatch vMatches12, vectorbool vbMatchesInliers,const cv::Mat K, vectorcv::Point3f vP3D, float th2, vectorbool vbGood, float parallax)
{ // 对给出的特征点对及其R t , 通过三角化检查解的有效性也称为 cheirality check// Calibration parameters//从相机内参数矩阵获取相机的校正参数const float fx K.atfloat(0,0);const float fy K.atfloat(1,1);const float cx K.atfloat(0,2);const float cy K.atfloat(1,2);//特征点是否是good点的标记这里的特征点指的是参考帧中的特征点vbGood vectorbool(vKeys1.size(),false);//重设存储空间坐标的点的大小vP3D.resize(vKeys1.size());//存储计算出来的每对特征点的视差vectorfloat vCosParallax;vCosParallax.reserve(vKeys1.size());// Camera 1 Projection Matrix K[I|0]// Step 1计算相机的投影矩阵 // 投影矩阵P是一个 3x4 的矩阵可以将空间中的一个点投影到平面上获得其平面坐标这里均指的是齐次坐标。// 对于第一个相机是 P1K*[I|0]// 以第一个相机的光心作为世界坐标系, 定义相机的投影矩阵cv::Mat P1(3,4, //矩阵的大小是3x4CV_32F, //数据类型是浮点数cv::Scalar(0)); //初始的数值是0//将整个K矩阵拷贝到P1矩阵的左侧3x3矩阵因为 K*I KK.copyTo(P1.rowRange(0,3).colRange(0,3));// 第一个相机的光心设置为世界坐标系下的原点cv::Mat O1 cv::Mat::zeros(3,1,CV_32F);// Camera 2 Projection Matrix K[R|t]// 计算第二个相机的投影矩阵 P2K*[R|t]cv::Mat P2(3,4,CV_32F);R.copyTo(P2.rowRange(0,3).colRange(0,3));t.copyTo(P2.rowRange(0,3).col(3));//最终结果是K*[R|t]P2 K*P2;// 第二个相机的光心在世界坐标系下的坐标cv::Mat O2 -R.t()*t;//在遍历开始前先将good点计数设置为0int nGood0;// 开始遍历所有的特征点对for(size_t i0, iendvMatches12.size();iiend;i){// 跳过outliersif(!vbMatchesInliers[i])continue;// Step 2 获取特征点对调用Triangulate() 函数进行三角化得到三角化测量之后的3D点坐标// kp1和kp2是匹配好的有效特征点const cv::KeyPoint kp1 vKeys1[vMatches12[i].first];const cv::KeyPoint kp2 vKeys2[vMatches12[i].second];//存储三维点的的坐标cv::Mat p3dC1;// 利用三角法恢复三维点p3dC1Triangulate(kp1,kp2, //特征点P1,P2, //投影矩阵p3dC1); //输出三角化测量之后特征点的空间坐标 // Step 3 第一关检查三角化的三维点坐标是否合法非无穷值// 只要三角测量的结果中有一个是无穷大的就说明三角化失败跳过对当前点的处理进行下一对特征点的遍历 if(!isfinite(p3dC1.atfloat(0)) || !isfinite(p3dC1.atfloat(1)) || !isfinite(p3dC1.atfloat(2))){//其实这里就算是不这样写也没问题因为默认的匹配点对就不是good点vbGood[vMatches12[i].first]false;//继续对下一对匹配点的处理continue;}// Check parallax// Step 4 第二关通过三维点深度值正负、两相机光心视差角大小来检查是否合法 //得到向量PO1cv::Mat normal1 p3dC1 - O1;//求取模长其实就是距离float dist1 cv::norm(normal1);//同理构造向量PO2cv::Mat normal2 p3dC1 - O2;//求模长float dist2 cv::norm(normal2);//根据公式a.*b|a||b|cos_theta 可以推导出来下面的式子float cosParallax normal1.dot(normal2)/(dist1*dist2);// Check depth in front of first camera (only if enough parallax, as infinite points can easily go to negative depth)// 如果深度值为负值为非法三维点跳过该匹配点对// ?视差比较小时重投影误差比较大。这里0.99998 对应的角度为0.36°,这里不应该是 cosParallax0.99998 吗// ?因为后面判断vbGood 点时的条件也是 cosParallax0.99998 // !可能导致初始化不稳定if(p3dC1.atfloat(2)0 cosParallax0.99998)continue;// Check depth in front of second camera (only if enough parallax, as infinite points can easily go to negative depth)// 讲空间点p3dC1变换到第2个相机坐标系下变为p3dC2cv::Mat p3dC2 R*p3dC1t; //判断过程和上面的相同if(p3dC2.atfloat(2)0 cosParallax0.99998)continue;// Step 5 第三关计算空间点在参考帧和当前帧上的重投影误差如果大于阈值则舍弃// Check reprojection error in first image// 计算3D点在第一个图像上的投影误差//投影到参考帧图像上的点的坐标x,yfloat im1x, im1y;//这个使能空间点的z坐标的倒数float invZ1 1.0/p3dC1.atfloat(2);//投影到参考帧图像上。因为参考帧下的相机坐标系和世界坐标系重合因此这里就直接进行投影就可以了im1x fx*p3dC1.atfloat(0)*invZ1cx;im1y fy*p3dC1.atfloat(1)*invZ1cy;//参考帧上的重投影误差这个的确就是按照定义来的float squareError1 (im1x-kp1.pt.x)*(im1x-kp1.pt.x)(im1y-kp1.pt.y)*(im1y-kp1.pt.y);// 重投影误差太大跳过淘汰if(squareError1th2)continue;// Check reprojection error in second image// 计算3D点在第二个图像上的投影误差计算过程和第一个图像类似float im2x, im2y;// 注意这里的p3dC2已经是第二个相机坐标系下的三维点了float invZ2 1.0/p3dC2.atfloat(2);im2x fx*p3dC2.atfloat(0)*invZ2cx;im2y fy*p3dC2.atfloat(1)*invZ2cy;// 计算重投影误差float squareError2 (im2x-kp2.pt.x)*(im2x-kp2.pt.x)(im2y-kp2.pt.y)*(im2y-kp2.pt.y);// 重投影误差太大跳过淘汰if(squareError2th2)continue;// Step 6 统计经过检验的3D点个数记录3D点视差角 // 如果运行到这里就说明当前遍历的这个特征点对靠谱经过了重重检验说明是一个合格的点称之为good点 vCosParallax.push_back(cosParallax);//存储这个三角化测量后的3D点在世界坐标系下的坐标vP3D[vMatches12[i].first] cv::Point3f(p3dC1.atfloat(0),p3dC1.atfloat(1),p3dC1.atfloat(2));//good点计数nGood;//判断视差角只有视差角稍稍大一丢丢的才会给打good点标记//? bug 我觉得这个写的位置不太对。你的good点计数都了然后才判断不是会让good点标志和good点计数不一样吗if(cosParallax0.99998)vbGood[vMatches12[i].first]true;}// Step 7 得到3D点中较小的视差角并且转换成为角度制表示if(nGood0){// 从小到大排序注意vCosParallax值越大视差越小sort(vCosParallax.begin(),vCosParallax.end());// !排序后并没有取最小的视差角而是取一个较小的视差角// 作者的做法如果经过检验过后的有效3D点小于50个那么就取最后那个最小的视差角(cos值最大)// 如果大于50个就取排名第50个的较小的视差角即可为了避免3D点太多时出现太小的视差角 size_t idx min(50,int(vCosParallax.size()-1));//将这个选中的角弧度制转换为角度制parallax acos(vCosParallax[idx])*180/CV_PI;}else//如果没有good点那么这个就直接设置为0了parallax0;//返回good点计数return nGood;
}
