品牌网站升级,网站建设方案视频教程,企业网站空间买虚拟主机,网页设计基本流程文章目录 0 引言1 单目初始化Initializer1.1 构造函数1.2 成员函数1.2.1 Initialize1.2.2 FindHomography1.2.3 FindFundamental1.2.4 ReconstructH1.2.5 ReconstructF 2 总结 0 引言
ORB-SLAM2算法7详细了解了System主类和多线程、ORB-SLAM2学习笔记8详细了解了图像特征点提取… 文章目录 0 引言1 单目初始化Initializer1.1 构造函数1.2 成员函数1.2.1 Initialize1.2.2 FindHomography1.2.3 FindFundamental1.2.4 ReconstructH1.2.5 ReconstructF 2 总结 0 引言
ORB-SLAM2算法7详细了解了System主类和多线程、ORB-SLAM2学习笔记8详细了解了图像特征点提取和描述子的生成、ORB-SLAM2算法9详细了解了图像帧、ORB-SLAM2算法10详细了解了图像关键帧及ORB-SLAM2算法11详细了解了地图点本文主要学习ORB-SLAM2中的单目初始化Initializer类。
本文用到了2D-2D对极约束并涉及到基本矩阵、本质矩阵和单应矩阵的定义及如何从本质矩阵或单应矩阵恢复相机运动的位姿Rt比如八点法、奇异值分解SVD等数学原理。详细参考如下 2D-2D对极约束中的基本矩阵、本质矩阵和单应矩阵
1 单目初始化Initializer
1.1 构造函数
Initializer类的构造函数主要用于初始化成员变量和参数设置。
构造函数接受三个参数 const Frame ReferenceFrame参考帧。它是一个Frame类的对象包含了参考帧的相关信息如图像、相机内参等。通过参考帧的相机内参构造函数将其克隆为成员变量mK以便在后续的初始化过程中使用。 float sigma描述子距离阈值。在特征点匹配阶段如果两个特征点的描述子之间的汉明距离小于阈值sigma则认为它们是匹配的特征点。 int iterationsRANSAC迭代的次数。在运动估计阶段为了估计相机的初始位姿和三维点云使用了RANSAC算法。iterations参数指定了RANSAC算法的迭代次数。
// 根据参考帧构造初始化器Initializer::Initializer(const Frame ReferenceFrame, float sigma, int iterations)
{//从参考帧中获取相机的内参数矩阵mK ReferenceFrame.mK.clone();// 从参考帧中获取去畸变后的特征点mvKeys1 ReferenceFrame.mvKeysUn;//获取估计误差mSigma sigma;mSigma2 sigma*sigma;//最大迭代次数mMaxIterations iterations;
}
1.2 成员函数
Initializer类中主要的成员函数一览表
成员函数类型定义bool Initializer::Initialize(const Frame CurrentFrame, const vectorint vMatches12, cv::Mat R21, cv::Mat t21,vectorcv::Point3f vP3D, vectorbool vbTriangulated)public计算基础矩阵和单应性矩阵选取最佳的来恢复出最开始两帧之间的相对姿态并进行三角化得到初始地图点void Initializer::FindHomography(vectorbool vbMatchesInliers, float score, cv::Mat H21)public计算单应矩阵假设场景为平面情况下通过前两帧求取Homography矩阵并得到该模型的评分void Initializer::FindFundamental(vectorbool vbMatchesInliers, float score, cv::Mat F21)public计算基础矩阵假设场景为非平面情况下通过前两帧求取Fundamental矩阵得到该模型的评分cv::Mat Initializer::ComputeH21(const vectorcv::Point2f vP1, const vectorcv::Point2f vP2)public用DLT方法求解单应矩阵Hcv::Mat Initializer::ComputeF21(const vectorcv::Point2f vP1, const vectorcv::Point2f vP2)public根据特征点匹配求fundamental matrixnormalized 8点法float Initializer::CheckHomography(const cv::Mat H21, const cv::Mat H12, vectorbool vbMatchesInliers, float sigma)public对给定的homography matrix打分,需要使用到卡方检验的知识float Initializer::CheckFundamental(const cv::Mat F21, vectorbool vbMatchesInliers, float sigma)public对给定的Fundamental matrix打分,需要使用到卡方检验的知识bool Initializer::ReconstructF(vectorbool vbMatchesInliers, cv::Mat F21, cv::Mat K,cv::Mat R21, cv::Mat t21, vectorcv::Point3f vP3D, vectorbool vbTriangulated, float minParallax, int minTriangulated)public从基础矩阵F中求解位姿Rt及三维点bool Initializer::ReconstructH(vectorbool vbMatchesInliers, cv::Mat H21, cv::Mat K,cv::Mat R21, cv::Mat t21, vectorcv::Point3f vP3D, vectorbool vbTriangulated, float minParallax, int minTriangulated)public用单应矩阵H矩阵恢复R, t和三维点void Initializer::Triangulate(const cv::KeyPoint kp1, const cv::KeyPoint kp2, const cv::Mat P1, const cv::Mat P2, cv::Mat x3D)public给定投影矩阵P1,P2和图像上的匹配特征点点kp1,kp2从而计算三维点坐标void Initializer::Normalize(const vectorcv::KeyPoint vKeys, vectorcv::Point2f vNormalizedPoints, cv::Mat T)public归一化特征点到同一尺度作为后续normalize DLT的输入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)public用位姿来对特征匹配点三角化从中筛选中合格的三维点void Initializer::DecomposeE(const cv::Mat E, cv::Mat R1, cv::Mat R2, cv::Mat t)public分解本质矩阵Essential得到R,t
1.2.1 Initialize
Initialize初始化主要用到了如下的成员函数它们的关系如下图 该函数包含了单目初始化类的所有流程主要步骤如下
重新记录特征点对的匹配关系在所有匹配特征点对中随机选择8对匹配特征点为一组用于估计H矩阵和F矩阵计算fundamental 矩阵和homography 矩阵为了加速分别开了线程计算计算得分比例来判断选取哪个模型来求位姿R,t并对匹配的特征点进行三角化。
输入参数
CurrentFrame当前帧vMatches12参考帧(1)与当前帧(2)图像中特征点的匹配关系R21相机从参考帧到当前帧的旋转t21相机从参考帧到当前帧的平移vP3D三角化测量之后的三维地图点vbTriangulated标记三角化点是否有效有效为true
输出参数
true该帧可以成功初始化返回truefalse该帧不满足初始化条件返回false
// 计算基础矩阵和单应矩阵选取最佳的来恢复出最开始两帧之间的相对姿态并进行三角化得到初始地图点bool Initializer::Initialize(const Frame CurrentFrame, const vectorint vMatches12, cv::Mat R21, cv::Mat t21,vectorcv::Point3f vP3D, vectorbool vbTriangulated)
{// Fill structures with current keypoints and matches with reference frame// Reference Frame: 1, Current Frame: 2//获取当前帧的去畸变之后的特征点mvKeys2 CurrentFrame.mvKeysUn;// mvMatches12记录匹配上的特征点对记录的是帧2在帧1的匹配索引mvMatches12.clear();// 预分配空间大小和关键点数目一致mvKeys2.size()mvMatches12.reserve(mvKeys2.size());// 记录参考帧1中的每个特征点是否有匹配的特征点// 这个成员变量后面没有用到后面只关心匹配上的特征点 mvbMatched1.resize(mvKeys1.size());// Step 1 重新记录特征点对的匹配关系存储在mvMatches12是否有匹配存储在mvbMatched1// 将vMatches12有冗余 转化为 mvMatches12只记录了匹配关系for(size_t i0, iendvMatches12.size();iiend; i){//vMatches12[i]解释i表示帧1中关键点的索引值vMatches12[i]的值为帧2的关键点索引值//没有匹配关系的话vMatches12[i]值为 -1if(vMatches12[i]0){//mvMatches12 中只记录有匹配关系的特征点对的索引值//i表示帧1中关键点的索引值vMatches12[i]的值为帧2的关键点索引值mvMatches12.push_back(make_pair(i,vMatches12[i]));//标记参考帧1中的这个特征点有匹配关系mvbMatched1[i]true;}else//标记参考帧1中的这个特征点没有匹配关系mvbMatched1[i]false;}// 有匹配的特征点的对数const int N mvMatches12.size();// Indices for minimum set selection// 新建一个容器vAllIndices存储特征点索引并预分配空间vectorsize_t vAllIndices;vAllIndices.reserve(N);//在RANSAC的某次迭代中还可以被抽取来作为数据样本的特征点对的索引所以这里起的名字叫做可用的索引vectorsize_t vAvailableIndices;//初始化所有特征点对的索引索引值0到N-1for(int i0; iN; i){vAllIndices.push_back(i);}// Generate sets of 8 points for each RANSAC iteration// Step 2 在所有匹配特征点对中随机选择8对匹配特征点为一组用于估计H矩阵和F矩阵// 共选择 mMaxIterations (默认200) 组//mvSets用于保存每次迭代时所使用的向量mvSets vector vectorsize_t (mMaxIterations, //最大的RANSAC迭代次数vectorsize_t(8,0)); //这个则是第二维元素的初始值也就是第一维。这里其实也是一个第一维的构造函数第一维vector有8项每项的初始值为0.//用于进行随机数据样本采样设置随机数种子DUtils::Random::SeedRandOnce(0);//开始每一次的迭代 for(int it0; itmMaxIterations; it){//迭代开始的时候所有的点都是可用的vAvailableIndices vAllIndices;// Select a minimum set//选择最小的数据样本集使用八点法求所以这里就循环了八次for(size_t j0; j8; j){// 随机产生一对点的id,范围从0到N-1int randi DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);// idx表示哪一个索引对应的特征点对被选中int idx vAvailableIndices[randi];//将本次迭代这个选中的第j个特征点对的索引添加到mvSets中mvSets[it][j] idx;// 由于这对点在本次迭代中已经被使用了,所以我们为了避免再次抽到这个点,就在点的可选列表中,// 将这个点原来所在的位置用vector最后一个元素的信息覆盖,并且删除尾部的元素// 这样就相当于将这个点的信息从点的可用列表中直接删除了vAvailableIndices[randi] vAvailableIndices.back();vAvailableIndices.pop_back();}//依次提取出8个特征点对}//迭代mMaxIterations次选取各自迭代时需要用到的最小数据集// Launch threads to compute in parallel a fundamental matrix and a homography// Step 3 计算fundamental 矩阵 和homography 矩阵为了加速分别开了线程计算 //这两个变量用于标记在H和F的计算中哪些特征点对被认为是Inliervectorbool vbMatchesInliersH, vbMatchesInliersF;//计算出来的单应矩阵和基础矩阵的RANSAC评分这里其实是采用重投影误差来计算的float SH, SF; //score for H and F//这两个是经过RANSAC算法后计算出来的单应矩阵和基础矩阵cv::Mat H, F; // 构造线程来计算H矩阵及其得分// thread方法比较特殊在传递引用的时候外层需要用ref来进行引用传递否则就是浅拷贝thread threadH(Initializer::FindHomography, //该线程的主函数this, //由于主函数为类的成员函数所以第一个参数就应该是当前对象的this指针ref(vbMatchesInliersH), //输出特征点对的Inlier标记ref(SH), //输出计算的单应矩阵的RANSAC评分ref(H)); //输出计算的单应矩阵结果// 计算fundamental matrix并打分参数定义和H是一样的这里不再赘述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// Step 4 计算得分比例来判断选取哪个模型来求位姿R,t//通过这个规则来判断谁的评分占比更多一些注意不是简单的比较绝对评分大小而是看评分的占比float RH SH/(SHSF); //RHRatio of Homography// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)// 注意这里更倾向于用H矩阵恢复位姿。如果单应矩阵的评分占比达到了0.4以上,则从单应矩阵恢复运动,否则从基础矩阵恢复运动if(RH0.40)//更偏向于平面此时从单应矩阵恢复函数ReconstructH返回bool型结果return ReconstructH(vbMatchesInliersH, //输入匹配成功的特征点对Inliers标记H, //输入前面RANSAC计算后的单应矩阵mK, //输入相机的内参数矩阵R21,t21, //输出计算出来的相机从参考帧1到当前帧2所发生的旋转和位移变换vP3D, //特征点对经过三角测量之后的空间坐标也就是地图点vbTriangulated, //特征点对是否成功三角化的标记1.0, //这个对应的形参为minParallax即认为某对特征点的三角化测量中认为其测量有效时//需要满足的最小视差角如果视差角过小则会引起非常大的观测误差,单位是角度50); //为了进行运动恢复所需要的最少的三角化测量成功的点个数else //if(pF_HF0.6)// 更偏向于非平面从基础矩阵恢复return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);//一般地程序不应该执行到这里如果执行到这里说明程序跑飞了return false;
}1.2.2 FindHomography
FindHomography主要用到了如下的成员函数它们的关系如下图 该成员函数主要计算单应矩阵
将当前帧和参考帧中的特征点坐标进行归一化选择8个归一化后的点对进行迭代八点法计算单应矩阵利用重投影误差为当次RANSAC的结果评分更新具有最优评分的单应矩阵计算结果并且保存所对应的特征点的内点标记。
// 计算单应矩阵假设场景为平面情况下通过前两帧求取Homography矩阵并得到该模型的评分void Initializer::FindHomography(vectorbool vbMatchesInliers, float score, cv::Mat H21)
{// Number of putative matches//匹配的特征点对总数const int N mvMatches12.size();// Normalize coordinates// Step 1 将当前帧和参考帧中的特征点坐标进行归一化主要是平移和尺度变换// 具体来说,就是将mvKeys1和mvKey2归一化到均值为0一阶绝对矩为1归一化矩阵分别为T1、T2// 这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值// 归一化矩阵就是把上述归一化的操作用矩阵来表示。这样特征点坐标乘归一化矩阵可以得到归一化后的坐标//归一化后的参考帧1和当前帧2中的特征点坐标vectorcv::Point2f vPn1, vPn2;// 记录各自的归一化矩阵cv::Mat T1, T2;Normalize(mvKeys1,vPn1, T1);Normalize(mvKeys2,vPn2, T2);//这里求的逆在后面的代码中要用到辅助进行原始尺度的恢复cv::Mat T2inv T2.inv();// Best Results variables// 记录最佳评分score 0.0;// 取得历史最佳评分时,特征点对的inliers标记vbMatchesInliers vectorbool(N,false);// Iteration variables//某次迭代中参考帧的特征点坐标vectorcv::Point2f vPn1i(8);//某次迭代中当前帧的特征点坐标vectorcv::Point2f vPn2i(8);//以及计算出来的单应矩阵、及其逆矩阵cv::Mat H21i, H12i;// 每次RANSAC记录Inliers与得分vectorbool vbCurrentInliers(N,false);float currentScore;// Perform all RANSAC iterations and save the solution with highest score//下面进行每次的RANSAC迭代for(int it0; itmMaxIterations; it){// Select a minimum set// Step 2 选择8个归一化之后的点对进行迭代for(size_t j0; j8; j){//从mvSets中获取当前次迭代的某个特征点对的索引信息int idx mvSets[it][j];// vPn1i和vPn2i为匹配的特征点对的归一化后的坐标// 首先根据这个特征点对的索引信息分别找到两个特征点在各自图像特征点向量中的索引然后读取其归一化之后的特征点坐标vPn1i[j] vPn1[mvMatches12[idx].first]; //first存储在参考帧1中的特征点索引vPn2i[j] vPn2[mvMatches12[idx].second]; //second存储在参考帧1中的特征点索引}//读取8对特征点的归一化之后的坐标// Step 3 八点法计算单应矩阵// 利用生成的8个归一化特征点对, 调用函数 Initializer::ComputeH21() 使用八点法计算单应矩阵 // 关于为什么计算之前要对特征点进行归一化后面又恢复这个矩阵的尺度// 可以在《计算机视觉中的多视图几何》这本书中P193页中找到答案// 书中这里说,8点算法成功的关键是在构造解的方称之前应对输入的数据认真进行适当的归一化cv::Mat Hn ComputeH21(vPn1i,vPn2i);// 单应矩阵原理X2H21*X1其中X1,X2 为归一化后的特征点 // 特征点归一化vPn1 T1 * mvKeys1, vPn2 T2 * mvKeys2 得到:T2 * mvKeys2 Hn * T1 * mvKeys1 // 进一步得到:mvKeys2 T2.inv * Hn * T1 * mvKeys1H21i T2inv*Hn*T1;//然后计算逆H12i H21i.inv();// Step 4 利用重投影误差为当次RANSAC的结果评分currentScore CheckHomography(H21i, H12i, //输入单应矩阵的计算结果vbCurrentInliers, //输出特征点对的Inliers标记mSigma); //TODO 测量误差在Initializer类对象构造的时候由外部给定的// Step 5 更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记if(currentScorescore){//如果当前的结果得分更高那么就更新最优计算结果H21 H21i.clone();//保存匹配好的特征点对的Inliers标记vbMatchesInliers vbCurrentInliers;//更新历史最优评分score currentScore;}}
}
1.2.3 FindFundamental
FindFundamental主要用到了如下的成员函数它们的关系如下图 该成员函数主要计算单应矩阵
将当前帧和参考帧中的特征点坐标进行归一化选择8个归一化之后的点对进行迭代八点法计算基础矩阵利用重投影误差为当次RANSAC的结果评分更新具有最优评分的基础矩阵计算结果,并且保存所对应的特征点对的内点标记。
// 计算基础矩阵假设场景为非平面情况下通过前两帧求取Fundamental矩阵得到该模型的评分void Initializer::FindFundamental(vectorbool vbMatchesInliers, float score, cv::Mat F21)
{// 计算基础矩阵,其过程和上面的计算单应矩阵的过程十分相似.// Number of putative matches// 匹配的特征点对总数// const int N vbMatchesInliers.size(); // !源代码出错请使用下面代替const int N mvMatches12.size();// Normalize coordinates// Step 1 将当前帧和参考帧中的特征点坐标进行归一化主要是平移和尺度变换// 具体来说,就是将mvKeys1和mvKey2归一化到均值为0一阶绝对矩为1归一化矩阵分别为T1、T2// 这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值// 归一化矩阵就是把上述归一化的操作用矩阵来表示。这样特征点坐标乘归一化矩阵可以得到归一化后的坐标vectorcv::Point2f vPn1, vPn2;cv::Mat T1, T2;Normalize(mvKeys1,vPn1, T1);Normalize(mvKeys2,vPn2, T2);// ! 注意这里取的是归一化矩阵T2的转置,因为基础矩阵的定义和单应矩阵不同两者去归一化的计算也不相同cv::Mat T2t T2.t();// Best Results variables//最优结果score 0.0;vbMatchesInliers vectorbool(N,false);// Iteration variables// 某次迭代中参考帧的特征点坐标vectorcv::Point2f vPn1i(8);// 某次迭代中当前帧的特征点坐标vectorcv::Point2f vPn2i(8);// 某次迭代中计算的基础矩阵cv::Mat F21i;// 每次RANSAC记录的Inliers与得分vectorbool vbCurrentInliers(N,false);float currentScore;// Perform all RANSAC iterations and save the solution with highest score// 下面进行每次的RANSAC迭代for(int it0; itmMaxIterations; it){// Select a minimum set// Step 2 选择8个归一化之后的点对进行迭代for(int j0; j8; j){int idx mvSets[it][j];// vPn1i和vPn2i为匹配的特征点对的归一化后的坐标// 首先根据这个特征点对的索引信息分别找到两个特征点在各自图像特征点向量中的索引然后读取其归一化之后的特征点坐标vPn1i[j] vPn1[mvMatches12[idx].first]; //first存储在参考帧1中的特征点索引vPn2i[j] vPn2[mvMatches12[idx].second]; //second存储在参考帧1中的特征点索引}// Step 3 八点法计算基础矩阵cv::Mat Fn ComputeF21(vPn1i,vPn2i);// 基础矩阵约束p2^t*F21*p1 0其中p1,p2 为齐次化特征点坐标 // 特征点归一化vPn1 T1 * mvKeys1, vPn2 T2 * mvKeys2 // 根据基础矩阵约束得到:(T2 * mvKeys2)^t* Hn * T1 * mvKeys1 0 // 进一步得到:mvKeys2^t * T2^t * Hn * T1 * mvKeys1 0F21i T2t*Fn*T1;// Step 4 利用重投影误差为当次RANSAC的结果评分currentScore CheckFundamental(F21i, vbCurrentInliers, mSigma);// Step 5 更新具有最优评分的基础矩阵计算结果,并且保存所对应的特征点对的内点标记if(currentScorescore){//如果当前的结果得分更高那么就更新最优计算结果F21 F21i.clone();vbMatchesInliers vbCurrentInliers;score currentScore;}}
}
1.2.4 ReconstructH
ReconstructH主要用到了如下的成员函数它们的关系如下图 该成员函数主要用单应矩阵恢复Rt和三维点常见的两种方法分别是Faugeras SVD-based decomposition 和 Zhang SVD-based decompositionORB-SLAM2中使用了前者直接调用的OpenCV库中的SVD函数。
输入/输出参数
vbMatchesInliers匹配点对的内点标记H21从参考帧到当前帧的单应矩阵K相机的内参数矩阵R21计算出来的相机旋转t21计算出来的相机平移vP3D世界坐标系下三角化测量特征点对之后得到的特征点的空间坐标vbTriangulated特征点是否成功三角化的标记minParallax对特征点的三角化测量中认为其测量有效时需要满足的最小视差角如果视差角过小则会引起非常大的观测误差,单位是角度minTriangulated为了进行运动恢复所需要的最少的三角化测量成功的点个数
返回值
true单应矩阵成功计算出位姿和三维点false初始化失败
// 用单应矩阵H矩阵恢复R, t和三维点bool Initializer::ReconstructH(vectorbool vbMatchesInliers, cv::Mat H21, cv::Mat K,cv::Mat R21, cv::Mat t21, vectorcv::Point3f vP3D, vectorbool vbTriangulated, float minParallax, int minTriangulated)
{// 目的 通过单应矩阵H恢复两帧图像之间的旋转矩阵R和平移向量T// 参考 Motion and structure from motion in a piecewise plannar environment.// International Journal of Pattern Recognition and Artificial Intelligence, 1988// https://www.researchgate.net/publication/243764888_Motion_and_Structure_from_Motion_in_a_Piecewise_Planar_Environment// 流程:// 1. 根据H矩阵的奇异值d d2 或者 d -d2 分别计算 H 矩阵分解的 8 组解// 1.1 讨论 d 0 时的 4 组解// 1.2 讨论 d 0 时的 4 组解// 2. 对 8 组解进行验证并选择产生相机前方最多3D点的解为最优解// 统计匹配的特征点对中属于内点(Inlier)或有效点个数int N0;for(size_t i0, iend vbMatchesInliers.size() ; iiend; i)if(vbMatchesInliers[i])N;// We recover 8 motion hypotheses using the method of Faugeras et al.// Motion and structure from motion in a piecewise planar environment.// International Journal of Pattern Recognition and Artificial Intelligence, 1988// 参考SLAM十四讲第二版p170-p171// H K * (R - t * n / d) * K_inv// 其中: K表示内参数矩阵// K_inv 表示内参数矩阵的逆// R 和 t 表示旋转和平移向量// n 表示平面法向量// 令 H K * A * K_inv// 则 A k_inv * H * kcv::Mat invK K.inv();cv::Mat A invK*H21*K;// 对矩阵A进行SVD分解// A 等待被进行奇异值分解的矩阵// w 奇异值矩阵// U 奇异值分解左矩阵// Vt 奇异值分解右矩阵注意函数返回的是转置// cv::SVD::FULL_UV 全部分解// A U * w * Vtcv::Mat U,w,Vt,V;cv::SVD::compute(A, w, U, Vt, cv::SVD::FULL_UV);// 根据文献eq(8)计算关联变量VVt.t();// 计算变量s det(U) * det(V)// 因为det(V)det(Vt), 所以 s det(U) * det(Vt)float s cv::determinant(U)*cv::determinant(Vt);// 取得矩阵的各个奇异值float d1 w.atfloat(0);float d2 w.atfloat(1);float d3 w.atfloat(2);// SVD分解正常情况下特征值di应该是正的且满足d1d2d3if(d1/d21.00001 || d2/d31.00001) {return false;}// 在ORBSLAM中没有对奇异值 d1 d2 d3按照论文中描述的关系进行分类讨论, 而是直接进行了计算// 定义8中情况下的旋转矩阵、平移向量和空间向量vectorcv::Mat vR, vt, vn;vR.reserve(8);vt.reserve(8);vn.reserve(8);// Step 1.1 讨论 d 0 时的 4 组解// 根据论文eq.(12)有// x1 e1 * sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3))// x2 0// x3 e3 * sqrt((d2 * d2 - d2 * d2) / (d1 * d1 - d3 * d3))// 令 aux1 sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3))// aux3 sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3))// 则// x1 e1 * aux1// x3 e3 * aux2// 因为 e1,e2,e3 1 or -1// 所以有x1和x3有四种组合// x1 {aux1,aux1,-aux1,-aux1}// x3 {aux3,-aux3,aux3,-aux3}float aux1 sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));float aux3 sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));float x1[] {aux1,aux1,-aux1,-aux1};float x3[] {aux3,-aux3,aux3,-aux3};// 根据论文eq.(13)有// sin(theta) e1 * e3 * sqrt(( d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) /(d1 d3)/d2// cos(theta) (d2* d2 d1 * d3) / (d1 d3) / d2 // 令 aux_stheta sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1d3)*d2)// 则 sin(theta) e1 * e3 * aux_stheta// cos(theta) (d2*d2d1*d3)/((d1d3)*d2)// 因为 e1 e2 e3 1 or -1// 所以 sin(theta) {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta}float aux_stheta sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1d3)*d2);float ctheta (d2*d2d1*d3)/((d1d3)*d2);float stheta[] {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};// 计算旋转矩阵 R//根据不同的e1 e3组合所得出来的四种R t的解// | ctheta 0 -aux_stheta| | aux1|// Rp | 0 1 0 | tp | 0 |// | aux_stheta 0 ctheta | |-aux3|// | ctheta 0 aux_stheta| | aux1|// Rp | 0 1 0 | tp | 0 |// |-aux_stheta 0 ctheta | | aux3|// | ctheta 0 aux_stheta| |-aux1|// Rp | 0 1 0 | tp | 0 |// |-aux_stheta 0 ctheta | |-aux3|// | ctheta 0 -aux_stheta| |-aux1|// Rp | 0 1 0 | tp | 0 |// | aux_stheta 0 ctheta | | aux3|// 开始遍历这四种情况中的每一种for(int i0; i4; i){//生成Rp就是eq.(8) 的 Rcv::Mat Rpcv::Mat::eye(3,3,CV_32F);Rp.atfloat(0,0)ctheta;Rp.atfloat(0,2)-stheta[i];Rp.atfloat(2,0)stheta[i]; Rp.atfloat(2,2)ctheta;// eq.(8) 计算Rcv::Mat R s*U*Rp*Vt;// 保存vR.push_back(R);// eq. (14) 生成tp cv::Mat tp(3,1,CV_32F);tp.atfloat(0)x1[i];tp.atfloat(1)0;tp.atfloat(2)-x3[i];tp*d1-d3;// 这里虽然对t有归一化并没有决定单目整个SLAM过程的尺度// 因为CreateInitialMapMonocular函数对3D点深度会缩放然后反过来对 t 有改变// eq.(8)恢复原始的tcv::Mat t U*tp;vt.push_back(t/cv::norm(t));// 构造法向量npcv::Mat np(3,1,CV_32F);np.atfloat(0)x1[i];np.atfloat(1)0;np.atfloat(2)x3[i];// eq.(8) 恢复原始的法向量cv::Mat n V*np;//看PPT 16页的图保持平面法向量向上if(n.atfloat(2)0)n-n;// 添加到vectorvn.push_back(n);}// Step 1.2 讨论 d 0 时的 4 组解float aux_sphi sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);// cos_theta项float cphi (d1*d3-d2*d2)/((d1-d3)*d2);// 考虑到e1,e2的取值这里的sin_theta有两种可能的解float sphi[] {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};// 对于每种由e1 e3取值的组合而形成的四种解的情况for(int i0; i4; i){// 计算旋转矩阵 Rcv::Mat Rpcv::Mat::eye(3,3,CV_32F);Rp.atfloat(0,0)cphi;Rp.atfloat(0,2)sphi[i];Rp.atfloat(1,1)-1;Rp.atfloat(2,0)sphi[i];Rp.atfloat(2,2)-cphi;// 恢复出原来的Rcv::Mat R s*U*Rp*Vt;// 然后添加到vector中vR.push_back(R);// 构造tpcv::Mat tp(3,1,CV_32F);tp.atfloat(0)x1[i];tp.atfloat(1)0;tp.atfloat(2)x3[i];tp*d1d3;// 恢复出原来的tcv::Mat t U*tp;// 归一化之后加入到vector中,要提供给上面的平移矩阵都是要进行过归一化的vt.push_back(t/cv::norm(t));// 构造法向量npcv::Mat np(3,1,CV_32F);np.atfloat(0)x1[i];np.atfloat(1)0;np.atfloat(2)x3[i];// 恢复出原来的法向量cv::Mat n V*np;// 保证法向量指向上方if(n.atfloat(2)0)n-n;// 添加到vector中vn.push_back(n);}// 最好的good点int bestGood 0;// 其次最好的good点int secondBestGood 0; // 最好的解的索引初始值为-1int bestSolutionIdx -1;// 最大的视差角float bestParallax -1;// 存储最好解对应的对特征点对进行三角化测量的结果vectorcv::Point3f bestP3D;// 最佳解所对应的那些可以被三角化测量的点的标记vectorbool bestTriangulated;// Instead of applying the visibility constraints proposed in the WFaugeras paper (which could fail for points seen with low parallax)// We reconstruct all hypotheses and check in terms of triangulated points and parallax// Step 2. 对 8 组解进行验证并选择产生相机前方最多3D点的解为最优解for(size_t i0; i8; i){// 第i组解对应的比较大的视差角float parallaxi;// 三角化测量之后的特征点的空间坐标vectorcv::Point3f vP3Di;// 特征点对是否被三角化的标记vectorbool vbTriangulatedi;// 调用 Initializer::CheckRT(), 计算good点的数目int nGood CheckRT(vR[i],vt[i], //当前组解的旋转矩阵和平移向量mvKeys1,mvKeys2, //特征点mvMatches12,vbMatchesInliers, //特征匹配关系以及Inlier标记K, //相机的内参数矩阵vP3Di, //存储三角化测量之后的特征点空间坐标的4.0*mSigma2, //三角化过程中允许的最大重投影误差vbTriangulatedi, //特征点是否被成功进行三角测量的标记parallaxi); // 这组解在三角化测量的时候的比较大的视差角// 更新历史最优和次优的解// 保留最优的和次优的解.保存次优解的目的是看看最优解是否突出if(nGoodbestGood){// 如果当前组解的good点数是历史最优那么之前的历史最优就变成了历史次优secondBestGood bestGood;// 更新历史最优点bestGood nGood;// 最优解的组索引为i就是当前次遍历bestSolutionIdx i;// 更新变量bestParallax parallaxi;bestP3D vP3Di;bestTriangulated vbTriangulatedi;}// 如果当前组的good计数小于历史最优但却大于历史次优else if(nGoodsecondBestGood){// 说明当前组解是历史次优点更新之secondBestGood nGood;}}// Step 3 选择最优解。要满足下面的四个条件// 1. good点数最优解明显大于次优解这里取0.75经验值// 2. 视角差大于规定的阈值// 3. good点数要大于规定的最小的被三角化的点数量// 4. good数要足够多达到总数的90%以上if(secondBestGood0.75*bestGood bestParallaxminParallax bestGoodminTriangulated bestGood0.9*N){// 从最佳的解的索引访问到RtvR[bestSolutionIdx].copyTo(R21);vt[bestSolutionIdx].copyTo(t21);// 获得最佳解时成功三角化的三维点以后作为初始地图点使用vP3D bestP3D;// 获取特征点的被成功进行三角化的标记vbTriangulated bestTriangulated;//返回真找到了最好的解return true;}return false;
}1.2.5 ReconstructF
ReconstructF主要用到了如下的成员函数它们的关系如下图 该成员函数主要用基础矩阵恢复Rt和三维点F分解出EE有四组解选择计算的有效三维点在摄像头前方、投影误差小于阈值、视差角大于阈值最多的作为最优的解。
输入/输出参数
vbMatchesInliers匹配好的特征点对的Inliers标记F21从参考帧到当前帧的基础矩阵K相机的内参数矩阵R21计算出来的相机旋转t21计算出来的相机平移vP3D世界坐标系下三角化测量特征点对之后得到的特征点的空间坐标vbTriangulated特征点是否成功三角化的标记minParallax对特征点的三角化测量中认为其测量有效时需要满足的最小视差角如果视差角过小则会引起非常大的观测误差,单位是角度minTriangulated为了进行运动恢复所需要的最少的三角化测量成功的点个数
返回值
true基础矩阵成功计算出位姿和三维点false初始化失败
// 从基础矩阵F中求解位姿Rt及三维点bool Initializer::ReconstructF(vectorbool vbMatchesInliers, cv::Mat F21, cv::Mat K,cv::Mat R21, cv::Mat t21, vectorcv::Point3f vP3D, vectorbool vbTriangulated, float minParallax, int minTriangulated)
{// Step 1 统计有效匹配点个数并用 N 表示// vbMatchesInliers 中存储匹配点对是否是有效int N0;for(size_t i0, iend vbMatchesInliers.size() ; iiend; i)if(vbMatchesInliers[i]) N;// Step 2 根据基础矩阵和相机的内参数矩阵计算本质矩阵cv::Mat E21 K.t()*F21*K;// 定义本质矩阵分解结果形成四组解,分别是// (R1, t) (R1, -t) (R2, t) (R2, -t)cv::Mat R1, R2, t;// Step 3 从本质矩阵求解两个R解和两个t解共四组解// 不过由于两个t解互为相反数因此这里先只获取一个// 虽然这个函数对t有归一化但并没有决定单目整个SLAM过程的尺度. // 因为 CreateInitialMapMonocular 函数对3D点深度会缩放然后反过来对 t 有改变.//注意下文中的符号“”表示矩阵的转置// |0 -1 0|// E U Sigma V let W |1 0 0|// |0 0 1|// 得到4个解 E [R|t]// R1 UWV R2 UWV t1 U3 t2 -U3DecomposeE(E21,R1,R2,t); cv::Mat t1t;cv::Mat t2-t;// Reconstruct with the 4 hyphoteses and check// Step 4 分别验证求解的4种R和t的组合选出最佳组合// 原理若某一组合使恢复得到的3D点位于相机正前方的数量最多那么该组合就是最佳组合// 实现根据计算的解组合成为四种情况,并依次调用 Initializer::CheckRT() 进行检查,得到可以进行三角化测量的点的数目// 定义四组解分别在对同一匹配点集进行三角化测量之后的特征点空间坐标vectorcv::Point3f vP3D1, vP3D2, vP3D3, vP3D4;// 定义四组解分别对同一匹配点集的有效三角化结果True or Falsevectorbool vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4;// 定义四种解对应的比较大的特征点对视差角float parallax1,parallax2, parallax3, parallax4;// Step 4.1 使用同样的匹配点分别检查四组解记录当前计算的3D点在摄像头前方且投影误差小于阈值的个数记为有效3D点个数int nGood1 CheckRT(R1,t1, //当前组解mvKeys1,mvKeys2, //参考帧和当前帧中的特征点mvMatches12, vbMatchesInliers, //特征点的匹配关系和Inliers标记K, //相机的内参数矩阵vP3D1, //存储三角化以后特征点的空间坐标4.0*mSigma2, //三角化测量过程中允许的最大重投影误差vbTriangulated1, //参考帧中被成功进行三角化测量的特征点的标记parallax1); //认为某对特征点三角化测量有效的比较大的视差角int nGood2 CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2);int nGood3 CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3);int nGood4 CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4);// Step 4.2 选取最大可三角化测量的点的数目int maxGood max(nGood1,max(nGood2,max(nGood3,nGood4)));// 重置变量并在后面赋值为最佳R和TR21 cv::Mat();t21 cv::Mat();// Step 4.3 确定最小的可以三角化的点数 // 在0.9倍的内点数 和 指定值minTriangulated 50 中取最大的也就是说至少50个int nMinGood max(static_castint(0.9*N), minTriangulated);// 统计四组解中重建的有效3D点个数 0.7 * maxGood 的解的数目// 如果有多个解同时满足该条件认为结果太接近nsimilarnsimilar1就认为有问题了后面返回falseint nsimilar 0;if(nGood10.7*maxGood)nsimilar;if(nGood20.7*maxGood)nsimilar;if(nGood30.7*maxGood)nsimilar;if(nGood40.7*maxGood)nsimilar;// Step 4.4 四个结果中如果没有明显的最优结果或者没有足够数量的三角化点则返回失败// 条件1: 如果四组解能够重建的最多3D点个数小于所要求的最少3D点个数mMinGood失败// 条件2: 如果存在两组及以上的解能三角化出 0.7*maxGood的点说明没有明显最优结果失败if(maxGoodnMinGood || nsimilar1) {return false;}// Step 4.5 选择最佳解记录结果// 条件1: 有效重建最多的3D点即maxGood nGoodx也即是位于相机前方的3D点个数最多// 条件2: 三角化视差角 parallax 必须大于最小视差角 minParallax角度越大3D点越稳定//看看最好的good点是在哪种解的条件下发生的if(maxGoodnGood1){//如果该种解下的parallax大于函数参数中给定的最小值if(parallax1minParallax){// 存储3D坐标vP3D vP3D1;// 获取特征点向量的三角化测量标记vbTriangulated vbTriangulated1;// 存储相机姿态R1.copyTo(R21);t1.copyTo(t21);// 结束return true;}}else if(maxGoodnGood2){if(parallax2minParallax){vP3D vP3D2;vbTriangulated vbTriangulated2;R2.copyTo(R21);t1.copyTo(t21);return true;}}else if(maxGoodnGood3){if(parallax3minParallax){vP3D vP3D3;vbTriangulated vbTriangulated3;R1.copyTo(R21);t2.copyTo(t21);return true;}}else if(maxGoodnGood4){if(parallax4minParallax){vP3D vP3D4;vbTriangulated vbTriangulated4;R2.copyTo(R21);t2.copyTo(t21);return true;}}// 如果有最优解但是不满足对应的parallaxminParallax那么返回false表示求解失败return false;
}2 总结
Initializer类是用于单目相机初始化的关键类之一。它的主要任务是通过对初始帧进行特征匹配和运动估计来估计相机的初始位姿和场景的初始三维点云。
以下是Initializer类的主要功能和流程 在构造函数中Initializer类接受ORB特征提取器和描述子、相机内参、视差阈值等参数并进行初始化 Initializer类的核心方法是Initialize()它接收两个帧作为输入第一帧帧1和第二帧帧2。初始化过程分为两个阶段特征匹配和运动估计 在特征匹配阶段Initializer使用ORB特征提取器对帧1和帧2提取特征点和描述子。然后通过基于描述子的光流法进行特征点匹配得到匹配的特征点对 在运动估计阶段Initializer使用匹配的特征点对进行运动估计。它首先计算两个相邻帧之间的基础矩阵和单应矩阵然后通过基础矩阵和单应矩阵恢复出帧2相对于帧1的运动旋转和平移 接下来Initializer使用三角化方法对匹配的特征点对进行三维重建得到初始的稀疏三维点云 最后Initializer通过对三维点云的筛选和剔除外点以及对相机运动的评估进一步优化初始位姿和三维点云。
总结来说Initializer类通过特征匹配和运动估计的过程估计相机的初始位姿和场景的初始三维点云为ORB-SLAM2的后续跟踪和建图阶段提供了重要的初始信息。 Reference:
https://github.com/raulmur/ORB_SLAM2https://github.com/electech6/ORB_SLAM2_detailed_comments/tree/master2D-2D对极约束中的基本矩阵、本质矩阵和单应矩阵 须知少时凌云志曾许人间第一流。 ⭐️ 文章转载自： http://www.morning.hpkr.cn.gov.cn.hpkr.cn http://www.morning.yxwrr.cn.gov.cn.yxwrr.cn http://www.morning.fhyhr.cn.gov.cn.fhyhr.cn http://www.morning.wrlxt.cn.gov.cn.wrlxt.cn http://www.morning.nwpnj.cn.gov.cn.nwpnj.cn http://www.morning.bkqdg.cn.gov.cn.bkqdg.cn http://www.morning.klzdy.cn.gov.cn.klzdy.cn http://www.morning.jtmrx.cn.gov.cn.jtmrx.cn http://www.morning.krdb.cn.gov.cn.krdb.cn http://www.morning.fnmtc.cn.gov.cn.fnmtc.cn http://www.morning.lqynj.cn.gov.cn.lqynj.cn http://www.morning.blzrj.cn.gov.cn.blzrj.cn http://www.morning.newfeiya.com.cn.gov.cn.newfeiya.com.cn http://www.morning.mhnr.cn.gov.cn.mhnr.cn http://www.morning.dnjwm.cn.gov.cn.dnjwm.cn http://www.morning.lwgrf.cn.gov.cn.lwgrf.cn http://www.morning.gzzxlp.com.gov.cn.gzzxlp.com http://www.morning.pwgzh.cn.gov.cn.pwgzh.cn http://www.morning.qhnmj.cn.gov.cn.qhnmj.cn http://www.morning.fwkq.cn.gov.cn.fwkq.cn http://www.morning.zdydj.cn.gov.cn.zdydj.cn http://www.morning.lmfmd.cn.gov.cn.lmfmd.cn http://www.morning.gppqf.cn.gov.cn.gppqf.cn http://www.morning.lzrpy.cn.gov.cn.lzrpy.cn http://www.morning.nqlx.cn.gov.cn.nqlx.cn http://www.morning.saletj.com.gov.cn.saletj.com http://www.morning.bpttm.cn.gov.cn.bpttm.cn http://www.morning.gcfrt.cn.gov.cn.gcfrt.cn http://www.morning.yjprj.cn.gov.cn.yjprj.cn http://www.morning.zrlwl.cn.gov.cn.zrlwl.cn http://www.morning.ghssm.cn.gov.cn.ghssm.cn http://www.morning.nldsd.cn.gov.cn.nldsd.cn http://www.morning.tlpsd.cn.gov.cn.tlpsd.cn http://www.morning.skqfx.cn.gov.cn.skqfx.cn http://www.morning.zcxjg.cn.gov.cn.zcxjg.cn http://www.morning.ktsth.cn.gov.cn.ktsth.cn http://www.morning.lgnrl.cn.gov.cn.lgnrl.cn http://www.morning.lskyz.cn.gov.cn.lskyz.cn http://www.morning.srzhm.cn.gov.cn.srzhm.cn http://www.morning.dfrenti.com.gov.cn.dfrenti.com http://www.morning.wknbc.cn.gov.cn.wknbc.cn http://www.morning.rhkgz.cn.gov.cn.rhkgz.cn http://www.morning.ykshx.cn.gov.cn.ykshx.cn http://www.morning.llqch.cn.gov.cn.llqch.cn http://www.morning.qxwwg.cn.gov.cn.qxwwg.cn http://www.morning.xhlpn.cn.gov.cn.xhlpn.cn http://www.morning.gkdqt.cn.gov.cn.gkdqt.cn http://www.morning.jprrh.cn.gov.cn.jprrh.cn http://www.morning.tklqs.cn.gov.cn.tklqs.cn http://www.morning.bpmtq.cn.gov.cn.bpmtq.cn http://www.morning.zbjfq.cn.gov.cn.zbjfq.cn http://www.morning.kldtf.cn.gov.cn.kldtf.cn http://www.morning.qbfwb.cn.gov.cn.qbfwb.cn http://www.morning.ggnfy.cn.gov.cn.ggnfy.cn http://www.morning.wtyqs.cn.gov.cn.wtyqs.cn http://www.morning.wdhlc.cn.gov.cn.wdhlc.cn http://www.morning.xlmgq.cn.gov.cn.xlmgq.cn http://www.morning.tztgq.cn.gov.cn.tztgq.cn http://www.morning.lsmnn.cn.gov.cn.lsmnn.cn http://www.morning.lqffg.cn.gov.cn.lqffg.cn http://www.morning.dybth.cn.gov.cn.dybth.cn http://www.morning.wqrdx.cn.gov.cn.wqrdx.cn http://www.morning.jsljr.cn.gov.cn.jsljr.cn http://www.morning.3dcb8231.cn.gov.cn.3dcb8231.cn http://www.morning.zdydj.cn.gov.cn.zdydj.cn http://www.morning.rrcxs.cn.gov.cn.rrcxs.cn http://www.morning.bqyb.cn.gov.cn.bqyb.cn http://www.morning.nhzxr.cn.gov.cn.nhzxr.cn http://www.morning.bloao.com.gov.cn.bloao.com http://www.morning.ryrgx.cn.gov.cn.ryrgx.cn http://www.morning.zrkws.cn.gov.cn.zrkws.cn http://www.morning.nrcbx.cn.gov.cn.nrcbx.cn http://www.morning.qtzqk.cn.gov.cn.qtzqk.cn http://www.morning.krrjb.cn.gov.cn.krrjb.cn http://www.morning.jwxnr.cn.gov.cn.jwxnr.cn http://www.morning.zjqwr.cn.gov.cn.zjqwr.cn http://www.morning.ldpjm.cn.gov.cn.ldpjm.cn http://www.morning.bfrsr.cn.gov.cn.bfrsr.cn http://www.morning.nrqtk.cn.gov.cn.nrqtk.cn http://www.morning.kdpal.cn.gov.cn.kdpal.cn
