admin管理员组文章数量:1613378
- 根据特征点匹配,随机生成每组包含8对匹配特征点的集合用于归一化八点算法
**
* @brief 并行地计算基础矩阵和单应性矩阵,选取其中一个模型,恢复出最开始两帧之间的相对姿态以及点云
*/
bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,
vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
{
// Fill structures with current keypoints and matches with reference frame
// Reference Frame: 1, Current Frame: 2
// Frame2 特征点
mvKeys2 = CurrentFrame.mvKeysUn;
// mvMatches12记录匹配上的特征点对
mvMatches12.clear();
mvMatches12.reserve(mvKeys2.size());
// mvbMatched1记录每个特征点是否有匹配的特征点,
// 这个变量后面没有用到,后面只关心匹配上的特征点
mvbMatched1.resize(mvKeys1.size());
// 步骤1:组织特征点对
for(size_t i=0, iend=vMatches12.size();i<iend; i++)
{
if(vMatches12[i]>=0)
{
mvMatches12.push_back(make_pair(i,vMatches12[i]));
mvbMatched1[i]=true;
}
else
mvbMatched1[i]=false;
}
// 匹配上的特征点的个数
const int N = mvMatches12.size();
// Indices for minimum set selection
// 新建一个容器vAllIndices,生成0到N-1的数作为特征点的索引
vector<size_t> vAllIndices;
vAllIndices.reserve(N);
vector<size_t> vAvailableIndices;
for(int i=0; i<N; i++)
{
vAllIndices.push_back(i);
}
// Generate sets of 8 points for each RANSAC iteration
// **步骤2:在所有匹配特征点对中随机选择8对匹配特征点为一组,共选择mMaxIterations组 **
// 用于FindHomography和FindFundamental求解
// mMaxIterations:200
mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));
DUtils::Random::SeedRandOnce(0);
for(int it=0; it<mMaxIterations; it++)
{
vAvailableIndices = vAllIndices;
// Select a minimum set
for(size_t j=0; j<8; j++)
{
// 产生0到N-1的随机数
int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
// idx表示哪一个索引对应的特征点被选中
int idx = vAvailableIndices[randi];
mvSets[it][j] = idx;
// randi对应的索引已经被选过了,从容器中删除
// randi对应的索引用最后一个元素替换,并删掉最后一个元素
vAvailableIndices[randi] = vAvailableIndices.back();
vAvailableIndices.pop_back();
}
}
// Launch threads to compute in parallel a fundamental matrix and a homography
// 步骤3:调用多线程分别用于计算fundamental matrix和homography
vector<bool> vbMatchesInliersH, vbMatchesInliersF;
float SH, SF; // score for H and F
cv::Mat H, F; // H and F
// ref是引用的功能:http://en.cppreference/w/cpp/utility/functional/ref
// 计算homograpy并打分
thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
// 计算fundamental matrix并打分
thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
// Wait until both threads have finished
threadH.join();
threadF.join();
// Compute ratio of scores
// 步骤4:计算得分比例,选取某个模型
float RH = SH/(SH+SF);
// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
// 步骤5:从H矩阵或F矩阵中恢复R,t
if(RH>0.40)
return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
else //if(pF_HF>0.6)
return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
return false;
}
- 计算基础矩阵
(1)归一化
(2)线性解
(3)利用重投影误差为当次RANSAC的结果评分,得到最佳结果
/**
* @brief 计算基础矩阵
*
* 假设场景为非平面情况下通过前两帧求取Fundamental矩阵(current frame 2 到 reference frame 1),并得到该模型的评分
*/
void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
{
// Number of putative matches
const int N = vbMatchesInliers.size();
// Normalize coordinates
vector<cv::Point2f> vPn1, vPn2;
cv::Mat T1, T2;
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
cv::Mat T2t = T2.t();
// Best Results variables
score = 0.0;
vbMatchesInliers = vector<bool>(N,false);
// Iteration variables
vector<cv::Point2f> vPn1i(8);
vector<cv::Point2f> vPn2i(8);
cv::Mat F21i;
vector<bool> vbCurrentInliers(N,false);
float currentScore;
// Perform all RANSAC iterations and save the solution with highest score
for(int it=0; it<mMaxIterations; it++)
{
// Select a minimum set
for(int j=0; j<8; j++)
{
int idx = mvSets[it][j];
vPn1i[j] = vPn1[mvMatches12[idx].first];
vPn2i[j] = vPn2[mvMatches12[idx].second];
}
cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
F21i = T2t*Fn*T1; //解除归一化
// 利用重投影误差为当次RANSAC的结果评分
currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);
if(currentScore>score)
{
F21 = F21i.clone();
vbMatchesInliers = vbCurrentInliers;
score = currentScore;
}
}
}
2.1 归一化
/**
* @brief 归一化特征点到同一尺度(作为normalize DLT的输入)
*
* [x' y' 1]' = T * [x y 1]' \n
* 归一化后x', y'的均值为0,sum(abs(x_i'-0))=1,sum(abs((y_i'-0))=1
*
* @param vKeys 特征点在图像上的坐标
* @param vNormalizedPoints 特征点归一化后的坐标
* @param T 将特征点归一化的矩阵
*/
void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T)
{
float meanX = 0;
float meanY = 0;
const int N = vKeys.size();
vNormalizedPoints.resize(N);
for(int i=0; i<N; i++)
{
meanX += vKeys[i].pt.x;
meanY += vKeys[i].pt.y;
}
meanX = meanX/N;
meanY = meanY/N;
float meanDevX = 0;
float meanDevY = 0;
// 将所有vKeys点减去中心坐标,使x坐标和y坐标均值分别为0
for(int i=0; i<N; 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;
// 将x坐标和y坐标分别进行尺度缩放,使得x坐标和y坐标的一阶绝对矩分别为1
for(int i=0; i<N; i++)
{
vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;
vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;
}
// |sX 0 -meanx*sX|
// |0 sY -meany*sY|
// |0 0 1 |
//归一化变化矩阵
T = cv::Mat::eye(3,3,CV_32F);
T.at<float>(0,0) = sX;
T.at<float>(1,1) = sY;
T.at<float>(0,2) = -meanX*sX;
T.at<float>(1,2) = -meanY*sY;
2.2 线性解
// x'Fx = 0 整理可得:Af = 0
// A = | x'x x'y x' y'x y'y y' x y 1 |, f = | f1 f2 f3 f4 f5 f6 f7 f8 f9 |
// 通过SVD求解Af = 0,A'A最小特征值对应的特征向量即为解
/**
* @brief 从特征点匹配求fundamental matrix(normalized 8点法)
* @param vP1 归一化后的点, in reference frame
* @param vP2 归一化后的点, in current frame
* @return 基础矩阵
* @see Multiple View Geometry in Computer Vision - Algorithm 11.1 p282 (中文版 p191)
*/
cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1,const vector<cv::Point2f> &vP2)
{
const int N = vP1.size();
cv::Mat A(N,9,CV_32F); // N*9
for(int i=0; i<N; 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.at<float>(i,0) = u2*u1;
A.at<float>(i,1) = u2*v1;
A.at<float>(i,2) = u2;
A.at<float>(i,3) = v2*u1;
A.at<float>(i,4) = v2*v1;
A.at<float>(i,5) = v2;
A.at<float>(i,6) = u1;
A.at<float>(i,7) = v1;
A.at<float>(i,8) = 1;
}
cv::Mat u,w,vt;
cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列
cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
w.at<float>(2)=0; // 秩2约束,将第3个奇异值设为0 //强迫约束
return u*cv::Mat::diag(w)*vt;
}
2.3 利用重投影误差为当次RANSAC的结果评分,得到最佳结果
/**
* @brief 对给定的fundamental matrix打分
*
* @see
* - Author's paper - IV. AUTOMATIC MAP INITIALIZATION (2)
* - Multiple View Geometry in Computer Vision - symmetric transfer errors: 4.2.2 Geometric distance
* - Multiple View Geometry in Computer Vision - model selection 4.7.1 RANSAC
*/
float Initializer::CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma)
{
const int N = mvMatches12.size();
const float f11 = F21.at<float>(0,0);
const float f12 = F21.at<float>(0,1);
const float f13 = F21.at<float>(0,2);
const float f21 = F21.at<float>(1,0);
const float f22 = F21.at<float>(1,1);
const float f23 = F21.at<float>(1,2);
const float f31 = F21.at<float>(2,0);
const float f32 = F21.at<float>(2,1);
const float f33 = F21.at<float>(2,2);
vbMatchesInliers.resize(N);
float score = 0;
// 基于卡方检验计算出的阈值(假设测量有一个像素的偏差)
const float th = 3.841; //置信度95%,自由度1
const float thScore = 5.991;//置信度95%,自由度2
const float invSigmaSquare = 1.0/(sigma*sigma);
for(int i=0; i<N; i++)
{
bool bIn = true;
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
// l2=F21x1=(a2,b2,c2)
// F21x1可以算出x1在图像中x2对应的线l
const float a2 = f11*u1+f12*v1+f13;
const float b2 = f21*u1+f22*v1+f23;
const float c2 = f31*u1+f32*v1+f33;
// x2应该在l这条线上:x2点乘l = 0
const float num2 = a2*u2+b2*v2+c2;
const float squareDist1 = num2*num2/(a2*a2+b2*b2); // 点到线的几何距离 的平方
const float chiSquare1 = squareDist1*invSigmaSquare;
if(chiSquare1>th)
bIn = false;
else
score += thScore - chiSquare1;
// Reprojection error in second image
// l1 =x2tF21=(a1,b1,c1)
const float a1 = f11*u2+f21*v2+f31;
const float b1 = f12*u2+f22*v2+f32;
const float c1 = f13*u2+f23*v2+f33;
const float num1 = a1*u1+b1*v1+c1;
const float squareDist2 = num1*num1/(a1*a1+b1*b1);
const float chiSquare2 = squareDist2*invSigmaSquare;
if(chiSquare2>th)
bIn = false;
else
score += thScore - chiSquare2;
if(bIn)
vbMatchesInliers[i]=true;
else
vbMatchesInliers[i]=false;
}
return score;
}
- 从F中恢复Rt
版权声明:本文标题:RANSAC求解基础矩阵----ORB-SLAM2 内容由热心网友自发贡献,该文观点仅代表作者本人, 转载请联系作者并注明出处:https://www.elefans.com/xitong/1728643990a1167452.html, 本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容,一经查实,本站将立刻删除。
发表评论