首页 > 基于SfM计算相机姿态

基于SfM计算相机姿态

具体过程为：

① 通过相机标定方法，预先计算相机的内参矩阵；

② 相邻帧特征点匹配，并结合内参矩阵计算本征矩阵；

③ 本征矩阵分解获得相机的外参矩阵[R | T]，最终的相机移动距离等于矩阵T的Frobenius范数。

#include 
#include 
#include 
#include 
#include using namespace cv;
using namespace std;void extract_features(vector<string>& image_names,vector<vector<KeyPoint>>& key_points_for_all,vector<Mat>& descriptor_for_all,vector<vector<Vec3b>>& colors_for_all)
{ key_points_for_all.clear();descriptor_for_all.clear();Mat image;//读取图像，获取图像特征点，并保存Ptr<Feature2D> sift = xfeatures2d::SIFT::create(0, 3, 0.04, 10);for (auto it = image_names.begin(); it != image_names.end(); ++it){ image = imread(*it);if (image.empty()) continue;vector<KeyPoint> key_points;Mat descriptor;//偶尔出现内存分配失败的错误sift->detectAndCompute(image, noArray(), key_points, descriptor);//特征点过少，则排除该图像if (key_points.size() <= 10) continue;key_points_for_all.push_back(key_points);descriptor_for_all.push_back(descriptor);vector<Vec3b> colors(key_points.size());for (int i = 0; i < key_points.size(); ++i){ Point2f& p = key_points[i].pt;colors[i] = image.at<Vec3b>(p.y, p.x);}colors_for_all.push_back(colors);}
}void match_features(Mat& query, Mat& train, vector<DMatch>& matches)
{ vector<vector<DMatch>> knn_matches;BFMatcher matcher(NORM_L2);matcher.knnMatch(query, train, knn_matches, 2);//获取满足Ratio Test的最小匹配的距离float min_dist = FLT_MAX;for (int r = 0; r < knn_matches.size(); ++r){ //Ratio Testif (knn_matches[r][0].distance > 0.6*knn_matches[r][1].distance)continue;float dist = knn_matches[r][0].distance;if (dist < min_dist) min_dist = dist;}matches.clear();for (size_t r = 0; r < knn_matches.size(); ++r){ //排除不满足Ratio Test的点和匹配距离过大的点if (knn_matches[r][0].distance > 0.6*knn_matches[r][1].distance ||knn_matches[r][0].distance > 5 * max(min_dist, 10.0f))continue;//保存匹配点matches.push_back(knn_matches[r][0]);}
}bool find_transform(Mat& K, vector<Point2f>& p1, vector<Point2f>& p2, Mat& R, Mat& T, Mat& mask)
{ //根据内参矩阵获取相机的焦距和光心坐标（主点坐标）double focal_length = 0.5*(K.at<double>(0) + K.at<double>(4));Point2d principle_point(K.at<double>(2), K.at<double>(5));//根据匹配点求取本征矩阵，使用RANSAC，进一步排除失配点Mat E = findEssentialMat(p1, p2, focal_length, principle_point, RANSAC, 0.999, 1.0, mask);if (E.empty()) return false;double feasible_count = countNonZero(mask);cout << (int)feasible_count << " -in- " << p1.size() << endl;//对于RANSAC而言，outlier数量大于50%时，结果是不可靠的if (feasible_count <= 15 || (feasible_count / p1.size()) < 0.6)return false;//分解本征矩阵，获取相对变换int pass_count = recoverPose(E, p1, p2, R, T, focal_length, principle_point, mask);//同时位于两个相机前方的点的数量要足够大if (((double)pass_count) / feasible_count < 0.7)return false;return true;
}void get_matched_points(vector<KeyPoint>& p1, vector<KeyPoint>& p2, vector<DMatch> matches, vector<Point2f>& out_p1, vector<Point2f>& out_p2)
{ out_p1.clear();out_p2.clear();for (int i = 0; i < matches.size(); ++i){ out_p1.push_back(p1[matches[i].queryIdx].pt);out_p2.push_back(p2[matches[i].trainIdx].pt);}
}void get_matched_colors(vector<Vec3b>& c1,vector<Vec3b>& c2,vector<DMatch> matches,vector<Vec3b>& out_c1,vector<Vec3b>& out_c2)
{ out_c1.clear();out_c2.clear();for (int i = 0; i < matches.size(); ++i){ out_c1.push_back(c1[matches[i].queryIdx]);out_c2.push_back(c2[matches[i].trainIdx]);}
}void reconstruct(Mat& K, Mat& R, Mat& T, vector<Point2f>& p1, vector<Point2f>& p2, Mat& structure)
{ //两个相机的投影矩阵[R T]，triangulatePoints只支持float型Mat proj1(3, 4, CV_32FC1);Mat proj2(3, 4, CV_32FC1);proj1(Range(0, 3), Range(0, 3)) = Mat::eye(3, 3, CV_32FC1);proj1.col(3) = Mat::zeros(3, 1, CV_32FC1);R.convertTo(proj2(Range(0, 3), Range(0, 3)), CV_32FC1);T.convertTo(proj2.col(3), CV_32FC1);Mat fK;K.convertTo(fK, CV_32FC1);proj1 = fK*proj1;proj2 = fK*proj2;//三角重建triangulatePoints(proj1, proj2, p1, p2, structure);
}void maskout_points(vector<Point2f>& p1, Mat& mask)
{ vector<Point2f> p1_copy = p1;p1.clear();for (int i = 0; i < mask.rows; ++i){ if (mask.at<uchar>(i) > 0)p1.push_back(p1_copy[i]);}
}void maskout_colors(vector<Vec3b>& p1, Mat& mask)
{ vector<Vec3b> p1_copy = p1;p1.clear();for (int i = 0; i < mask.rows; ++i){ if (mask.at<uchar>(i) > 0)p1.push_back(p1_copy[i]);}
}void save_structure(string file_name, vector<Mat>& rotations, vector<Mat>& motions, Mat& structure, vector<Vec3b>& colors)
{ int n = (int)rotations.size();FileStorage fs(file_name, FileStorage::WRITE);fs << "Camera Count" << n;fs << "Point Count" << structure.cols;fs << "Rotations" << "[";for (size_t i = 0; i < n; ++i){ fs << rotations[i];}fs << "]";fs << "Motions" << "[";for (size_t i = 0; i < n; 
                

                    
更多相关：

                    
    
                        
  • 
                    
    
                        
    剑指offer：面试题34. 二叉树中和为某一值的路径
    
                    
    
                    
    
                        
    
                            
    题目：二叉树中和为某一值的路径 
输入一棵二叉树和一个整数，打印出二叉树中节点值的和为输入整数的所有路径。从树的根节点开始往下一直到叶节点所经过的节点形成一条路径。 
示例: 给定如下二叉树，以及目标和 sum = 22， 
              5              /              4   8    ...
                            
    
                        
    
                    
    
				
  • 
                    
    
                        
    vector容器 begin()与end()函数、front()与back()的用法
    
                    
    
                    
    
                        
    
                            
    begin函数： 函数原型: iterator begin(); const_iterator begin(); 功能： 返回一个当前vector容器中起始元素的迭代器。 
end函数： 函数原型： iterator end(); const_iterator end(); 功能： 返回一个当前vector容器中末尾元素的迭代器。...
                            
    
                        
    
                    
    
				
  • 
                    
    
                        
    C++ STL: 容器vector源码分析
    
                    
    
                    
    
                        
    
                            
    文章目录前言vector的核心接口vector push_back实现vector 的 Allocatorvector 的 push_back总结...
                            
    
                        
    
                    
    
				
  • 
                    
    
                        
    leetcode-102 二叉树的层次遍历
    
                    
    
                    
    
                        
    
                            
    给定一个二叉树，返回其按层次遍历的节点值。 （即逐层地，从左到右访问所有节点）。 
例如: 给定二叉树: [3,9,20,null,null,15,7] 3 /  9 20 /  15 7 
返回其层次遍历结果： 
 [ [3], [9,20], [15,7] ] 
 
方法一（非递归） 二叉树的层次遍历即二叉树的广度遍历，可以使...
                            
    
                        
    
                    
    
				
  • 
                    
    
                        
    根据相机外参实现单应矩阵计算的理论与实践
    
                    
    
                    
    
                        
    
                            
    论文阅读模块将分享点云处理，SLAM，三维视觉，高精地图相关的文章。公众号致力于理解三维视觉领域相关内容的干货分享，欢迎各位加入我，我们一起每天一篇文章阅读，开启分享之旅,有兴趣的可联系微信[email protected]。单应矩阵介绍单应性在计算机视觉领域是一个非常重要的概念，它在图像校正、图像拼接、俯视图生成，相机位姿估计、...
                            
    
                        
    
                    
    
				
  • 
                    
    
                        
    python怎么编程乘法口诀表_用python编写乘法口诀表的方法
    
                    
    
                    
    
                        
    
                            
    用python编写乘法口诀表的方法
发布时间：2020-08-25 11:46:35
来源：亿速云
阅读：60
作者：小新
用python编写乘法口诀表的方法？这个问题可能是我们日常学习或工作经常见到的。希望通过这个问题能让你收获颇深。下面是小编给大家带来的参考内容，让我们一起来看看吧！
第一种：使用for遍历循环嵌套for x in...
                            
    
                        
    
                    
    
				
  • 
                    
    
                        
    这就是我为什么推荐使用var aa = for (var i = 0, l = aa.length； i ＜ l； i++) {var a = aa[i]；}循环的原因，每秒最快可以执行4000+次�
    
                    
    
                    
    
                        
    
                            
    //很长一段时间我都只使用以下方式做数组循环，具体原因看数据
var aa = 
for (var i = 0, l = aa.length; i < l; i++) {
var a = aa[i];}
 
数据采集图片来源于网友 
 
很明显，for循环第二种方式完胜！！！ 
至于for in、forEach什么的，不知道甩他们多少...
                            
    
                        
    
                    
    
				
  • 
                    
    
                        
    论文笔记03
    
                    
    
                    
    
                        
    
                            
    目录    1. Scene Graph Generation with External Knowledge and Image Reconstruction  2. Knowledge Acquisition for Visual Question Answering via Iterative Querying Author...
                            
    
                        
    
                    
    
				
  • 
                    
    
                        
    2019第四周作业(基础作业+挑战作业）
    
                    
    
                    
    
                        
    
                            
    基础题1： 输入一个正整数 n (1≤n≤10)和n 阶方阵a的元素，如果方阵a中的所有元素都沿主对角线对称，输出“Yes”, 否则，输出“No”。主对角线为从矩阵的左上角至右下角的连线，方阵a中的所有元素都沿主对角线对称指对所有i, k，a[i][k]和a[k][i]相等。输入输出示例如下： 输入： 3
1 2 3
4 5 6
7...
                            
    
                        
    
                    
    
				
  • 
                    
    
                        
    java听课笔记（四）
    
                    
    
                    
    
                        
    
                            
    程序流程控制 分支 顺序 循环 if switch&case 1 2 3 调整 break 1.6 前 switch(byte、short、char、int) 1.7 可放String 循环 while（次数不确定） do while for（确定次数） break（跳出本层循环） continue（跳出本次循环）     *   2...
                            
    
                        
    
                    
    
				
    •

©2024 11GX.COM