ORB-SLAM2的源码阅读（九）：Initializer类

还是博客写的顺序有些问题，应该一开始就写初始化来着。。。

#ifndef INITIALIZER_H
#define INITIALIZER_H#include<opencv2/opencv.hpp>
#include "Frame.h"namespace ORB_SLAM2
{// THIS IS THE INITIALIZER FOR MONOCULAR SLAM. NOT USED IN THE STEREO OR RGBD CASE.
class Initializer
{typedef pair<int,int> Match;public:// Fix the reference frameInitializer(const Frame &ReferenceFrame, float sigma = 1.0, int iterations = 200);// Computes in parallel a fundamental matrix and a homography// Selects a model and tries to recover the motion and the structure from motionbool Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12,cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated);private:void FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21);void FindFundamental(vector<bool> &vbInliers, float &score, cv::Mat &F21);cv::Mat ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2);cv::Mat ComputeF21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2);float CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma);float CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma);bool ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated);bool ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated);void Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D);void Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T);int CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2,const vector<Match> &vMatches12, vector<bool> &vbInliers,const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float &parallax);void DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t);// Keypoints from Reference Frame (Frame 1)vector<cv::KeyPoint> mvKeys1;// Keypoints from Current Frame (Frame 2)vector<cv::KeyPoint> mvKeys2;// Current Matches from Reference to Currentvector<Match> mvMatches12;vector<bool> mvbMatched1;// Calibrationcv::Mat mK;// Standard Deviation and Variancefloat mSigma, mSigma2;// Ransac max iterationsint mMaxIterations;// Ransac setsvector<vector<size_t> > mvSets;   };} //namespace ORB_SLAM#endif // INITIALIZER_H

LZ知道代码很长，但是请耐心些O(∩_∩)O哈哈~

#include "Initializer.h"#include "Thirdparty/DBoW2/DUtils/Random.h"#include "Optimizer.h"
#include "ORBmatcher.h"#include<thread>namespace ORB_SLAM2
{//ORB-SLAM2的初始化仅仅用于单目，经过初始化，通过2D-2D的特征匹配估计出
//特征点的深度及位姿//固定参考帧
Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations)
{mK = ReferenceFrame.mK.clone();mvKeys1 = ReferenceFrame.mvKeysUn;mSigma = sigma;mSigma2 = sigma*sigma;mMaxIterations = iterations;
}//同时计算本质矩阵和单应矩阵
//选择一个模型，试着从运动中恢复运动和结构
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//初始化只能默认第一帧为参考帧mvKeys2 = CurrentFrame.mvKeysUn;mvMatches12.clear();mvMatches12.reserve(mvKeys2.size());mvbMatched1.resize(mvKeys1.size());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;}elsemvbMatched1[i]=false;}const int N = mvMatches12.size();// Indices for minimum set selectionvector<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//默认迭代次数为200，sigma=0.1//针对8个点进行RANSACmvSets = 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 setfor(size_t j=0; j<8; j++){int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);int idx = vAvailableIndices[randi];mvSets[it][j] = idx;vAvailableIndices[randi] = vAvailableIndices.back();vAvailableIndices.pop_back();}}// Launch threads to compute in parallel a fundamental matrix and a homography//引发线程同时计算基础矩阵和单应矩阵vector<bool> vbMatchesInliersH, vbMatchesInliersF;float SH, SF;cv::Mat H, F;thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));// Wait until both threads have finishedthreadH.join();threadF.join();// Compute ratio of scores//计算比例，看使用哪个具体模型(主要是区分初始化匹配的点是否共面)float RH = SH/(SH+SF);// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)//论文写的是0.45，感觉这就是一个经验值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;
}//寻找单应矩阵
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
{// Number of putative matchesconst int N = mvMatches12.size();// Normalize coordinatesvector<cv::Point2f> vPn1, vPn2;cv::Mat T1, T2;Normalize(mvKeys1,vPn1, T1);Normalize(mvKeys2,vPn2, T2);cv::Mat T2inv = T2.inv();// Best Results variablesscore = 0.0;vbMatchesInliers = vector<bool>(N,false);// Iteration variables//单应矩阵：DLTvector<cv::Point2f> vPn1i(8);vector<cv::Point2f> vPn2i(8);cv::Mat H21i, H12i;vector<bool> vbCurrentInliers(N,false);float currentScore;// Perform all RANSAC iterations and save the solution with highest score//进行RANSAC操作，求解最高得分对应的结果for(int it=0; it<mMaxIterations; it++){// Select a minimum setfor(size_t j=0; j<8; j++){int idx = mvSets[it][j];vPn1i[j] = vPn1[mvMatches12[idx].first];vPn2i[j] = vPn2[mvMatches12[idx].second];}cv::Mat Hn = ComputeH21(vPn1i,vPn2i);H21i = T2inv*Hn*T1;H12i = H21i.inv();currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);if(currentScore>score){H21 = H21i.clone();vbMatchesInliers = vbCurrentInliers;score = currentScore;}}
}//寻找基本矩阵
void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
{// Number of putative matchesconst int N = vbMatchesInliers.size();// Normalize coordinatesvector<cv::Point2f> vPn1, vPn2;cv::Mat T1, T2;Normalize(mvKeys1,vPn1, T1);Normalize(mvKeys2,vPn2, T2);cv::Mat T2t = T2.t();// Best Results variablesscore = 0.0;vbMatchesInliers = vector<bool>(N,false);// Iteration variablesvector<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 scorefor(int it=0; it<mMaxIterations; it++){// Select a minimum setfor(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;currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);if(currentScore>score){F21 = F21i.clone();vbMatchesInliers = vbCurrentInliers;score = currentScore;}}
}//计算单应矩阵（normalized DLT）
//Algorithm 4.1 in MVG
cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2)
{const int N = vP1.size();cv::Mat A(2*N,9,CV_32F);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>(2*i,0) = 0.0;A.at<float>(2*i,1) = 0.0;A.at<float>(2*i,2) = 0.0;A.at<float>(2*i,3) = -u1;A.at<float>(2*i,4) = -v1;A.at<float>(2*i,5) = -1;A.at<float>(2*i,6) = v2*u1;A.at<float>(2*i,7) = v2*v1;A.at<float>(2*i,8) = v2;A.at<float>(2*i+1,0) = u1;A.at<float>(2*i+1,1) = v1;A.at<float>(2*i+1,2) = 1;A.at<float>(2*i+1,3) = 0.0;A.at<float>(2*i+1,4) = 0.0;A.at<float>(2*i+1,5) = 0.0;A.at<float>(2*i+1,6) = -u2*u1;A.at<float>(2*i+1,7) = -u2*v1;A.at<float>(2*i+1,8) = -u2;}cv::Mat u,w,vt;cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);return vt.row(8).reshape(0, 3);
}//从特征点匹配求基本矩阵（8点法）
//Algorithm 11.1 in MVG
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);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);cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);w.at<float>(2)=0;return  u*cv::Mat::diag(w)*vt;
}//检查单应矩阵是否合理
//计算symmetric transfer errors: 4.2.2 Geometric distance in MVG
//4.7.1 RANSAC in MVG
float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma)
{   const int N = mvMatches12.size();const float h11 = H21.at<float>(0,0);const float h12 = H21.at<float>(0,1);const float h13 = H21.at<float>(0,2);const float h21 = H21.at<float>(1,0);const float h22 = H21.at<float>(1,1);const float h23 = H21.at<float>(1,2);const float h31 = H21.at<float>(2,0);const float h32 = H21.at<float>(2,1);const float h33 = H21.at<float>(2,2);const float h11inv = H12.at<float>(0,0);const float h12inv = H12.at<float>(0,1);const float h13inv = H12.at<float>(0,2);const float h21inv = H12.at<float>(1,0);const float h22inv = H12.at<float>(1,1);const float h23inv = H12.at<float>(1,2);const float h31inv = H12.at<float>(2,0);const float h32inv = H12.at<float>(2,1);const float h33inv = H12.at<float>(2,2);vbMatchesInliers.resize(N);float score = 0;//来源于卡方分布const float th = 5.991;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 first image// x2in1 = H12*x2const 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;const float squareDist1 = (u1-u2in1)*(u1-u2in1)+(v1-v2in1)*(v1-v2in1);const float chiSquare1 = squareDist1*invSigmaSquare;if(chiSquare1>th)bIn = false;elsescore += th - chiSquare1;// Reprojection error in second image// x1in2 = H21*x1const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);const float chiSquare2 = squareDist2*invSigmaSquare;if(chiSquare2>th)bIn = false;elsescore += th - chiSquare2;if(bIn)vbMatchesInliers[i]=true;elsevbMatchesInliers[i]=false;}return score;
}//基本原理同单应矩阵
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;const float thScore = 5.991;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)const float a2 = f11*u1+f12*v1+f13;const float b2 = f21*u1+f22*v1+f23;const float c2 = f31*u1+f32*v1+f33;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;elsescore += 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;elsescore += thScore - chiSquare2;if(bIn)vbMatchesInliers[i]=true;elsevbMatchesInliers[i]=false;}return score;
}//从F恢复P
//Result 9.19 in MVG
bool Initializer::ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{int N=0;for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)if(vbMatchesInliers[i])N++;// Compute Essential Matrix from Fundamental Matrixcv::Mat E21 = K.t()*F21*K;cv::Mat R1, R2, t;// Recover the 4 motion hypothesesDecomposeE(E21,R1,R2,t);  cv::Mat t1=t;cv::Mat t2=-t;// Reconstruct with the 4 hyphoteses and checkvector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;vector<bool> vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4;float parallax1,parallax2, parallax3, parallax4;int nGood1 = CheckRT(R1,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,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);int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4)));R21 = cv::Mat();t21 = cv::Mat();int nMinGood = max(static_cast<int>(0.9*N),minTriangulated);int nsimilar = 0;if(nGood1>0.7*maxGood)nsimilar++;if(nGood2>0.7*maxGood)nsimilar++;if(nGood3>0.7*maxGood)nsimilar++;if(nGood4>0.7*maxGood)nsimilar++;// If there is not a clear winner or not enough triangulated points reject initializationif(maxGood<nMinGood || nsimilar>1){return false;}// If best reconstruction has enough parallax initializeif(maxGood==nGood1){if(parallax1>minParallax){vP3D = vP3D1;vbTriangulated = vbTriangulated1;R1.copyTo(R21);t1.copyTo(t21);return true;}}else if(maxGood==nGood2){if(parallax2>minParallax){vP3D = vP3D2;vbTriangulated = vbTriangulated2;R2.copyTo(R21);t1.copyTo(t21);return true;}}else if(maxGood==nGood3){if(parallax3>minParallax){vP3D = vP3D3;vbTriangulated = vbTriangulated3;R1.copyTo(R21);t2.copyTo(t21);return true;}}else if(maxGood==nGood4){if(parallax4>minParallax){vP3D = vP3D4;vbTriangulated = vbTriangulated4;R2.copyTo(R21);t2.copyTo(t21);return true;}}return false;
}// 从H恢复P
bool Initializer::ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{int N=0;for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; 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, 1988cv::Mat invK = K.inv();cv::Mat A = invK*H21*K;cv::Mat U,w,Vt,V;cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);V=Vt.t();float s = cv::determinant(U)*cv::determinant(Vt);float d1 = w.at<float>(0);float d2 = w.at<float>(1);float d3 = w.at<float>(2);if(d1/d2<1.00001 || d2/d3<1.00001){return false;}vector<cv::Mat> vR, vt, vn;vR.reserve(8);vt.reserve(8);vn.reserve(8);//n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1float 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};//case d'=d2float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};for(int i=0; i<4; i++){cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);Rp.at<float>(0,0)=ctheta;Rp.at<float>(0,2)=-stheta[i];Rp.at<float>(2,0)=stheta[i];Rp.at<float>(2,2)=ctheta;cv::Mat R = s*U*Rp*Vt;vR.push_back(R);cv::Mat tp(3,1,CV_32F);tp.at<float>(0)=x1[i];tp.at<float>(1)=0;tp.at<float>(2)=-x3[i];tp*=d1-d3;cv::Mat t = U*tp;vt.push_back(t/cv::norm(t));cv::Mat np(3,1,CV_32F);np.at<float>(0)=x1[i];np.at<float>(1)=0;np.at<float>(2)=x3[i];cv::Mat n = V*np;if(n.at<float>(2)<0)n=-n;vn.push_back(n);}//case d'=-d2float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};for(int i=0; i<4; i++){cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);Rp.at<float>(0,0)=cphi;Rp.at<float>(0,2)=sphi[i];Rp.at<float>(1,1)=-1;Rp.at<float>(2,0)=sphi[i];Rp.at<float>(2,2)=-cphi;cv::Mat R = s*U*Rp*Vt;vR.push_back(R);cv::Mat tp(3,1,CV_32F);tp.at<float>(0)=x1[i];tp.at<float>(1)=0;tp.at<float>(2)=x3[i];tp*=d1+d3;cv::Mat t = U*tp;vt.push_back(t/cv::norm(t));cv::Mat np(3,1,CV_32F);np.at<float>(0)=x1[i];np.at<float>(1)=0;np.at<float>(2)=x3[i];cv::Mat n = V*np;if(n.at<float>(2)<0)n=-n;vn.push_back(n);}int bestGood = 0;int secondBestGood = 0;    int bestSolutionIdx = -1;float bestParallax = -1;vector<cv::Point3f> bestP3D;vector<bool> bestTriangulated;// Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax)// We reconstruct all hypotheses and check in terms of triangulated points and parallaxfor(size_t i=0; i<8; i++){float parallaxi;vector<cv::Point3f> vP3Di;vector<bool> vbTriangulatedi;int nGood = CheckRT(vR[i],vt[i],mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K,vP3Di, 4.0*mSigma2, vbTriangulatedi, parallaxi);if(nGood>bestGood){secondBestGood = bestGood;bestGood = nGood;bestSolutionIdx = i;bestParallax = parallaxi;bestP3D = vP3Di;bestTriangulated = vbTriangulatedi;}else if(nGood>secondBestGood){secondBestGood = nGood;}}if(secondBestGood<0.75*bestGood && bestParallax>=minParallax && bestGood>minTriangulated && bestGood>0.9*N){vR[bestSolutionIdx].copyTo(R21);vt[bestSolutionIdx].copyTo(t21);vP3D = bestP3D;vbTriangulated = bestTriangulated;return true;}return false;
}void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
{cv::Mat A(4,4,CV_32F);A.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;cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);x3D = vt.row(3).t();x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}//归一化
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;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;for(int i=0; i<N; i++){vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;}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;
}int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2,const vector<Match> &vMatches12, vector<bool> &vbMatchesInliers,const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float &parallax)
{// Calibration parametersconst float fx = K.at<float>(0,0);const float fy = K.at<float>(1,1);const float cx = K.at<float>(0,2);const float cy = K.at<float>(1,2);vbGood = vector<bool>(vKeys1.size(),false);vP3D.resize(vKeys1.size());vector<float> vCosParallax;vCosParallax.reserve(vKeys1.size());// Camera 1 Projection Matrix K[I|0]cv::Mat P1(3,4,CV_32F,cv::Scalar(0));K.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]cv::Mat P2(3,4,CV_32F);R.copyTo(P2.rowRange(0,3).colRange(0,3));t.copyTo(P2.rowRange(0,3).col(3));P2 = K*P2;cv::Mat O2 = -R.t()*t;int nGood=0;for(size_t i=0, iend=vMatches12.size();i<iend;i++){if(!vbMatchesInliers[i])continue;const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];cv::Mat p3dC1;Triangulate(kp1,kp2,P1,P2,p3dC1);if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2))){vbGood[vMatches12[i].first]=false;continue;}// Check parallaxcv::Mat normal1 = p3dC1 - O1;float dist1 = cv::norm(normal1);cv::Mat normal2 = p3dC1 - O2;float dist2 = cv::norm(normal2);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)if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)continue;// Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)cv::Mat p3dC2 = R*p3dC1+t;if(p3dC2.at<float>(2)<=0 && cosParallax<0.99998)continue;// Check reprojection error in first imagefloat im1x, im1y;float invZ1 = 1.0/p3dC1.at<float>(2);im1x = fx*p3dC1.at<float>(0)*invZ1+cx;im1y = fy*p3dC1.at<float>(1)*invZ1+cy;float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);if(squareError1>th2)continue;// Check reprojection error in second imagefloat im2x, im2y;float invZ2 = 1.0/p3dC2.at<float>(2);im2x = fx*p3dC2.at<float>(0)*invZ2+cx;im2y = fy*p3dC2.at<float>(1)*invZ2+cy;float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);if(squareError2>th2)continue;vCosParallax.push_back(cosParallax);vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));nGood++;if(cosParallax<0.99998)vbGood[vMatches12[i].first]=true;}if(nGood>0){sort(vCosParallax.begin(),vCosParallax.end());size_t idx = min(50,int(vCosParallax.size()-1));parallax = acos(vCosParallax[idx])*180/CV_PI;}elseparallax=0;return nGood;
}//分解E，E=t^R
void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
{cv::Mat u,w,vt;cv::SVD::compute(E,w,u,vt);u.col(2).copyTo(t);t=t/cv::norm(t);cv::Mat W(3,3,CV_32F,cv::Scalar(0));W.at<float>(0,1)=-1;W.at<float>(1,0)=1;W.at<float>(2,2)=1;R1 = u*W*vt;if(cv::determinant(R1)<0)R1=-R1;R2 = u*W.t()*vt;if(cv::determinant(R2)<0)R2=-R2;
}} //namespace ORB_SLAM

加油，加油，坚持就是胜利。尽人事，听天命！，要学习斯多葛学派O(∩_∩)O哈哈~

ORB-SLAM2的源码阅读（九）：Initializer类相关推荐

IDEA源码阅读利器 — UML类图插件Diagram
来源:https://www.cnblogs.com/deng-cc/p/6927447.html 最近正好也没什么可忙的,就回过头来鼓捣过去的知识点,到 Servlet 部分时,以前学习的时候硬是把 ...
SDWebImage源码阅读(九)SDWebImageDownloader
这篇学习 SDWebImageDownloader 这个类. 首先依然是看 SDWebImageDownloader.h: SDWebImageDownloaderOptions 1 typedef ...
源码阅读：SDWebImage（十九）——UIImage+ForceDecode/UIImage+GIF/UIImage+MultiFormat
该文章阅读的SDWebImage的版本为4.3.3. 由于这几个分类都是UIImage的分类,并且内容相对较少,就写在一篇文章中. 1.UIImage+ForceDecode 这个分类为UIImage ...
Soul网关源码阅读（九）插件配置加载初探
Soul网关源码阅读(九)插件配置加载初探简介今日来探索一下插件的初始化,及相关的配置的加载源码Debug 插件初始化首先来到我们非常熟悉的插件链调用的类: SoulWebHa ...
Go-Excelize API源码阅读（十九）——SetHeaderFooter
Go-Excelize API源码阅读(十九)--SetHeaderFooter 开源摘星计划(WeOpen Star) 是由腾源会 2022 年推出的全新项目,旨在为开源人提供成长激励,为开源项目提 ...
TiDB 源码阅读系列文章（十九）tikv-client（下）
上篇文章中,我们介绍了数据读写过程中 tikv-client 需要解决的几个具体问题,本文将继续介绍 tikv-client 里的两个主要的模块--负责处理分布式计算的 copIterator 和执 ...
应用监控CAT之cat-client源码阅读（一）
CAT 由大众点评开发的,基于 Java 的实时应用监控平台,包括实时应用监控,业务监控.对于及时发现线上问题非常有用.(不知道大家有没有在用) 应用自然是最初级的,用完之后,还想了解下其背后的原理, ...
源码阅读：AFNetworking（十六）——UIWebView+AFNetworking
该文章阅读的AFNetworking的版本为3.2.0. 这个分类提供了对请求周期进行控制的方法,包括进度监控.成功和失败的回调. 1.接口文件 1.1.属性 /**网络会话管理者对象*/ @prop ...
源码阅读：SDWebImage（六）——SDWebImageCoderHelper
该文章阅读的SDWebImage的版本为4.3.3. 这个类提供了四个方法,这四个方法可分为两类,一类是动图处理,一类是图像方向处理. 1.私有函数先来看一下这个类里的两个函数 /**这个函数是计算 ...
源码阅读：AFNetworking（八）——AFAutoPurgingImageCache
该文章阅读的AFNetworking的版本为3.2.0. AFAutoPurgingImageCache该类是用来管理内存中图片的缓存. 1.接口文件 1.1.AFImageCache协议这个协议定 ...

ORB-SLAM2的源码阅读（九）：Initializer类

ORB-SLAM2的源码阅读（九）：Initializer类相关推荐

最新文章

热门文章