单目相机通过对极约束来求解相机运动的位姿。参考了ORBSLAM中单目实现的代码,这里用opencv来实现最简单的位姿估计.
mLeftImg = cv::imread(lImg, cv::IMREAD_GRAYSCALE);
mRightImg = cv::imread(rImg, cv::IMREAD_GRAYSCALE);
cv::Ptr<ORB> OrbLeftExtractor = cv::ORB::create();
cv::Ptr<ORB> OrbRightExtractor = cv::ORB::create();
OrbLeftExtractor->detectAndCompute(mLeftImg, noArray(), mLeftKps, mLeftDes);
OrbRightExtractor->detectAndCompute(mRightImg, noArray(), mRightKps, mRightDes);
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create(DescriptorMatcher::BRUTEFORCE_HAMMING);
matcher->match(mLeftDes, mRightDes, mMatches);
首先通过ORB特征提取,获取两幅图像的匹配度,我将其连线出来的效果:
RANSAC的算法原理可以google,很容易理解。先看看ORBSLAM中的实现:
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求解
// mMaxIter