参考高翔老师的示例代码
成都创新互联公司服务项目包括陈巴尔虎网站建设、
陈巴尔虎网站制作、陈巴尔虎网页制作以及陈巴尔虎网络营销策划等。多年来，我们专注于互联网行业，利用自身积累的技术优势、行业经验、深度合作伙伴关系等，向广大中小型企业、政府机构等提供互联网行业的解决方案，
陈巴尔虎网站推广取得了明显的社会效益与经济效益。目前，我们服务的客户以成都为中心已经辐射到陈巴尔虎省份的部分城市，未来相信会继续扩大服务区域并继续获得客户的支持与信任！
在FeatureExtraction的基础上计算本质矩阵E 并计算对极约束。
FeatureExtraction部分：
SlamBook14讲-FeatureExtraction_就差一点点_的博客-博客
本质矩阵E 包含了相机运动的旋转矩阵和平移矩阵。
本质矩阵E 的求解需要匹配好的n对点和内参，这里的points类型是InputArray。后者是opencv中一种输入的数组（不可更改 const），有很多重载版本，可以用vector储存。
cv::findEssentialMat(points1,points2,K);
基础矩阵是
基础矩阵的求解不需要相机内参K 其中FM_8POINT代表8点法求解
cv::findFundamentalMat(points1,points2,cv::FM_8POINT);
求解出本质矩阵E 后 便可计算出R 和T
cv::recoverPose(EssentialMat,points1,points2,K,R,t);
可以通过验证对极约束是否接近0来判断求解是否正确，即
其中p1和p2是P分别在相机两个位置归一化平面的投影（是需要通过像素坐标换算到世界坐标的，变换方法是通过相机的内参进行换算。归一化坐标为[X/Z,Y/Z,1]），只要大多数对极约束接近0，则说明计算的本质矩阵E 和 是正确的。也可以用计算出的R和T，用代替E ，验证T 和R的正确性。其中是李代数的部分内容，可以理解为矩阵的叉积。 是通过t生成的反对称矩阵。
源码有些地方写的有些麻烦，主要是为了锻炼自己多用vector的迭代器和引用传递，算是复习C++
附上源码（包含了特征点提取与匹配的部分）：
#include#include#includevoid PoseEstimation2d2d(std::vector,std::vector,cv::Mat&,cv::Mat&);
void EpipolarConstraint(cv::Mat,const std::vector&, const std::vector& , cv::Mat,std::vector&,std::vector&);
int main(){
cv::Mat img1 = cv::imread("../1.png",1);
cv::Mat img2 = cv::imread("../2.png",1);
std::vectorKeyPoint1,KeyPoint2;
cv::Mat descriptors1,descriptors2;
cv::Ptrdetector = cv::ORB::create();//represented as vectors in a multidimensional space
cv::Ptrdescriptor = cv::ORB::create();//初始化描述子 descriptor为指针
cv::Ptrmatcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
detector->detect(img1,KeyPoint1);
detector->detect(img2,KeyPoint2);
descriptor->compute(img1,KeyPoint1,descriptors1);
descriptor->compute(img2,KeyPoint2,descriptors2);
// cv::Mat outimg1;
// cv::drawKeypoints(img1,KeyPoint1,outimg1);
// cv::imshow("img1特征点",outimg1);
//可以看出descriptor描述子每个元素都是0-255 意味着二进制的编码
//std::cout<< descriptors1<< std::endl;
std::vectormatches;
matcher->match(descriptors1,descriptors2,matches);
//可以看到结果不理想，很多误匹配的点，需要设置阈值筛选
// cv::Mat matchimg;
// cv::drawMatches(img1,KeyPoint1,img2,KeyPoint2,matches,matchimg);
// cv::imshow("特征匹配",matchimg);
//迭代器遍历 for遍历也行
double min = 10000,max = 0;
min = min_element( matches.begin(), matches.end(), [](const cv::DMatch& m1, const cv::DMatch& m2) {return m1.distancedistance;
max = max_element( matches.begin(), matches.end(), [](const cv::DMatch& m1, const cv::DMatch& m2) {return m1.distancedistance;
std::vectorGoodMatches;
for(auto i = matches.begin(); i< matches.end(); i++){
if(i->distance<= std::max( 2*min, 30.0 )){
GoodMatches.push_back(*i);
}
}
std::vectorGoodMatchPoint1,GoodMatchPoint2;
for(auto i = GoodMatches.begin(); i< GoodMatches.end(); i++){
GoodMatchPoint1.push_back(KeyPoint1[i->queryIdx]);
GoodMatchPoint2.push_back(KeyPoint2[i->trainIdx]);
}
cv::Mat R,t;
PoseEstimation2d2d(GoodMatchPoint1,GoodMatchPoint2,R,t);
cv::Mat t_x = ( cv::Mat_( 3,3 )<<
0, -t.at( 2,0 ), t.at( 1,0 ),
t.at( 2,0 ), 0, -t.at( 0,0 ),
-t.at( 1.0 ), t.at( 0,0 ), 0 );
cv::Mat t_r = t_x*R;
std::cout<< "t^R is "<< std::endl<< t_x*R<< std::endl;
// cv::Mat GoodMatchimg;
// cv::drawMatches(img1,KeyPoint1,img2,KeyPoint2,GoodMatches,GoodMatchimg);
// cv::imshow("特征匹配",GoodMatchimg);
// cv::imwrite("../GoodResult.png", GoodMatchimg);
// cv::waitKey(0);
cv::Mat K = ( cv::Mat_( 3,3 )<< 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
cv::Point2d cam1;//点在归一化平面的坐标
cv::Point2d cam2;
std::vectorx1;
std::vectorx2;
EpipolarConstraint(t_r,GoodMatchPoint1,GoodMatchPoint2,K,x1,x2);
return 0;
}
void EpipolarConstraint(cv::Mat E,const std::vector& PixelPoint1,const std::vector& PixelPoint2, cv::Mat K,std::vector& x1,std::vector& x2){
cv::Point2d p1,p2;
for(auto i = PixelPoint1.begin(); i< PixelPoint1.end(); i++){
p1.x=(i->pt.x-K.at( 0,2 ) ) / K.at( 0,0 );
p1.y=(i->pt.y-K.at( 1,2 ) ) / K.at( 1,1 );
x1.push_back((cv::Mat_( 3,1 )<< p1.x, p1.y, 1)) ;
}
for(auto i = PixelPoint2.begin(); i< PixelPoint2.end(); i++){
p2.x=(i->pt.x-K.at( 0,2 ) ) / K.at( 0,0 );
p2.y=(i->pt.y-K.at( 1,2 ) ) / K.at( 1,1 );
x2.push_back((cv::Mat_( 3,1 )<< p2.x, p2.y, 1)) ;
}
auto j = x2.begin();
for(auto i = x1.begin(); i< x1.end(); i++,j++){
cv::Mat EpipolarConstraint = (*j).t()* E * (*i);
std::cout<< "EpipolarConstraint is "<< EpipolarConstraint<< std::endl;
}
}
void PoseEstimation2d2d(std::vectorGoodMatchPoint1,std::vectorGoodMatchPoint2,cv::Mat& R,cv::Mat& t){
//初始化矩阵
cv::Mat K = (cv::Mat_(3,3)<< 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);//相机内参 fx fy cx cy
std::vectorpoints1, points2;
//深拷贝
for(auto i = GoodMatchPoint1.begin(); i< GoodMatchPoint1.end();i++){
points1.push_back(i->pt) ;
}
for(auto i = GoodMatchPoint2.begin(); i< GoodMatchPoint2.end();i++){
points2.push_back(i->pt) ;
}
cv::Mat FundamentalMat;
FundamentalMat = cv::findFundamentalMat(points1,points2,cv::FM_8POINT);
//std::cout<< "fundamental_matrix is "<< std::endl<< FundamentalMat<< std::endl;
cv::Mat EssentialMat;
EssentialMat = cv::findEssentialMat(points1,points2,K);
//EssentialMat = cv::findEssentialMat(points1,points2,521.0, cv::Point2d(325.1,249.7));
std::cout<< "EssentialMat is "<< std::endl<< EssentialMat<< std::endl;
cv::recoverPose(EssentialMat,points1,points2,K,R,t);
//cv::recoverPose(EssentialMat,points1,points2,R,t,521.0,cv::Point2d(325.1,249.7));
//std::cout<< "R is "<< std::endl<< R<< std::endl;
//std::cout<< "t is "<< std::endl<< t<< std::endl;
}
最终的运行结果如下：
EssentialMat is
[0.0109715107420577, 0.2483381379213602, 0.03170694792954255;
-0.2088597167616505, 0.029084710794909, -0.6744719771361425;
0.008249595749295274, 0.6614170563251771, 0.01676713455468244]
t^R is
[-0.01551605929362698, -0.3512031627022603, -0.04484039578292499;
0.2953722440781573, -0.04113199246326703, 0.9538474175082218;
-0.01166669019199051, -0.9353849714383189, -0.02371230909015907]
EpipolarConstraint is [-0.0005416579768821039]
EpipolarConstraint is [-0.002159398557490991]
EpipolarConstraint is [0.000329564098265081]
EpipolarConstraint is [-1.147072796144066e-05]
EpipolarConstraint is [0.000207988216714694]
EpipolarConstraint is [0.0001093677532205128]
EpipolarConstraint is [0.0004208746919946507]
EpipolarConstraint is [-0.003177997347355854]
EpipolarConstraint is [-4.07601554744362e-05]
EpipolarConstraint is [-0.00144829153347319]
EpipolarConstraint is [-0.0009615799940303968]
EpipolarConstraint is [-0.0008059848523957747]
EpipolarConstraint is [-0.001424066451101234]
EpipolarConstraint is [-0.0004308558850970052]
EpipolarConstraint is [-0.0004796564950435983]
EpipolarConstraint is [-0.0001958686247416352]
EpipolarConstraint is [0.001542044376943941]
EpipolarConstraint is [0.003106913282011514]
EpipolarConstraint is [0.0006736754289436708]
EpipolarConstraint is [-0.001173033197502501]
EpipolarConstraint is [-0.003987317225260016]
EpipolarConstraint is [-0.001256013301241379]
EpipolarConstraint is [-0.001550737161933333]
EpipolarConstraint is [0.00201042506321103]
EpipolarConstraint is [-0.0006111309111405394]
EpipolarConstraint is [-3.046189484311179e-05]
EpipolarConstraint is [0.001273590112099141]
EpipolarConstraint is [-0.004166758257779282]
EpipolarConstraint is [-0.001107404024207642]
EpipolarConstraint is [-0.0005148070022724105]
EpipolarConstraint is [-0.001776337642038897]
EpipolarConstraint is [-1.012294414959314e-12]
EpipolarConstraint is [-0.001856761858613207]
EpipolarConstraint is [-0.0004830391085845209]
EpipolarConstraint is [0.0004692803762148395]
EpipolarConstraint is [-0.002957773919832811]
EpipolarConstraint is [-0.003344265746093363]
EpipolarConstraint is [-0.0001976593466801457]
EpipolarConstraint is [-0.002823913457831666]
EpipolarConstraint is [-0.0004305282653053466]
EpipolarConstraint is [0.001181778913841636]
EpipolarConstraint is [-1.695033002846458e-12]
EpipolarConstraint is [-0.002141849714928215]
EpipolarConstraint is [0.001149598706735769]
EpipolarConstraint is [-0.002121385557104991]
EpipolarConstraint is [-1.019170858818086e-12]
EpipolarConstraint is [0.0009022844709607424]
EpipolarConstraint is [-0.002101173356852802]
EpipolarConstraint is [-0.0003542202672723524]
EpipolarConstraint is [-2.942426762718442e-05]
EpipolarConstraint is [-0.003786956691643206]
EpipolarConstraint is [-0.001587983371808432]
EpipolarConstraint is [-0.00292944464841105]
EpipolarConstraint is [-0.001016011288387075]
EpipolarConstraint is [0.0001215793621033301]
EpipolarConstraint is [-0.0009821639412199053]
EpipolarConstraint is [0.001458461868649605]
EpipolarConstraint is [-1.237312320791217e-05]
EpipolarConstraint is [-0.002735751536037237]
EpipolarConstraint is [0.001414873832845623]
EpipolarConstraint is [0.001789545990435903]
EpipolarConstraint is [-0.002550349147929159]
EpipolarConstraint is [-0.006256229913356795]
EpipolarConstraint is [-0.001875732926272605]
EpipolarConstraint is [-0.002105779810921735]
EpipolarConstraint is [-1.543598582287586e-12]
EpipolarConstraint is [-0.004014001023516178]
EpipolarConstraint is [-0.004726927213163874]
EpipolarConstraint is [-0.001328876731452905]
EpipolarConstraint is [5.292988269900434e-14]
EpipolarConstraint is [-0.005081445458945427]
EpipolarConstraint is [-0.001976431721038091]
EpipolarConstraint is [-0.001662302184417874]
EpipolarConstraint is [0.004283653601028871]
EpipolarConstraint is [0.004085015243819348]
EpipolarConstraint is [0.0001472315485530409]
EpipolarConstraint is [-0.0009652492684803365]
EpipolarConstraint is [-0.002341399165420976]
EpipolarConstraint is [-0.006138218209024097]
对于结果有几点注意
E 的 尺度是不确定的 这意味着E 乘以任何一个常数都是正确的 因为不会影响对极约束的结果，称为本质矩阵的尺度不确定性。可以看到结果中与E是差了一个常数项系数的。
如果相机的所有特征点几乎共面，本质矩阵会退化，应该使用单应矩阵，这里没有写。
你是否还在寻找稳定的海外服务器提供商？创新互联www.cdcxhl.cn海外机房具备T级流量清洗系统配攻击溯源，准确流量调度确保服务器高可用性，企业级服务器适合批量采购，新人活动首月15元起，快前往官网查看详情吧
本文题目：SlamBook14讲-PoseEstimation2d2d-创新互联
当前路径：
http://www.cdkjz.cn/article/ceisec.html
