基于SfM计算相机姿态

具体过程为：
① 通过相机标定方法，预先计算相机的内参矩阵；
② 相邻帧特征点匹配，并结合内参矩阵计算本征矩阵；
③ 本征矩阵分解获得相机的外参矩阵[R | T]，最终的相机移动距离等于矩阵T的Frobenius范数。

#include <opencv2\xfeatures2d\nonfree.hpp>
#include <opencv2\features2d\features2d.hpp>
#include <opencv2\highgui\highgui.hpp>
#include <opencv2\calib3d\calib3d.hpp>
#include <iostream>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; ++i){fs << motions[i];}fs << "]";fs << "Points" << "[";for (size_t i = 0; i < structure.cols; ++i){Mat_<float> c = structure.col(i);c /= c(3);   //齐次坐标，需要除以最后一个元素才是真正的坐标值fs << Point3f(c(0), c(1), c(2));}fs << "]";fs << "Colors" << "[";for (size_t i = 0; i < colors.size(); ++i){fs << colors[i];}fs << "]";fs.release();
}void main()
{string img1 = "0004.png";string img2 = "0006.png";vector<string> img_names = { img1, img2 };vector<vector<KeyPoint>> key_points_for_all;vector<Mat> descriptor_for_all;vector<vector<Vec3b>> colors_for_all;vector<DMatch> matches;//本征矩阵Mat K(Matx33d(2759.48, 0, 1520.69,0, 2764.16, 1006.81,0, 0, 1));//提取特征extract_features(img_names, key_points_for_all, descriptor_for_all, colors_for_all);//特征匹配match_features(descriptor_for_all[0], descriptor_for_all[1], matches);//计算变换矩阵vector<Point2f> p1, p2;vector<Vec3b> c1, c2;Mat R, T;  //旋转矩阵和平移向量Mat mask;    //mask中大于零的点代表匹配点，等于零代表失配点get_matched_points(key_points_for_all[0], key_points_for_all[1], matches, p1, p2);get_matched_colors(colors_for_all[0], colors_for_all[1], matches, c1, c2);find_transform(K, p1, p2, R, T, mask);//三维重建Mat structure; //4行N列的矩阵，每一列代表空间中的一个点（齐次坐标）maskout_points(p1, mask);maskout_points(p2, mask);reconstruct(K, R, T, p1, p2, structure);//保存并显示vector<Mat> rotations = { Mat::eye(3, 3, CV_64FC1), R };vector<Mat> motions = { Mat::zeros(3, 1, CV_64FC1), T };maskout_colors(c1, mask);save_structure(".\\Viewer\\structure.yml", rotations, motions, structure, c1);//system(".\\Viewer\\SfMViewer.exe");
}

基于SfM计算相机姿态相关推荐

关于OpenCV的那些事——相机姿态更新
上一节我们使用张正友相机标定法获得了相机内参,这一节我们使用 PnP (Perspective-n-Point)算法估计相机初始姿态并更新之.推荐3篇我学习的博客:[姿态估计]Pose estimat ...
4-基于ArUco相机姿态评估
1-简介基于ArUco评估相机姿态,可以使用OPENCV的外部库(opencv_contrib)中的aruco模块,可以参考安装目录(库目录): -\opencv_contrib-4.5.4\mod ...
投射式AR/AR眼镜的标定之相机姿态、Unity投影矩阵的计算
首先感谢这位大佬的系列文章 https://zzlzz.blog.csdn.net/article/details/53215105 https://blog.csdn.net/zzlyw/artic ...
基于单目视觉的平面目标定位和坐标测量 (下) - 相机姿态估计和目标测量
上回说到,我们回顾了机器视觉的基础知识,然后一顿操作之后得到了相机内参A和畸变系数dist. 现在我们开始定位目标的坐标. 这个过程包括几个子部分: 1. 标记检测 2. 指定世界坐标系, ...
基于图像的摄像机姿态估计方法评析
作者丨黄浴@知乎来源丨https://zhuanlan.zhihu.com/p/467776433 编辑丨3D视觉工坊 arXiv在2022年1月15日上传论文"A Critical An ...
基于消失点的相机自标定（2）
点云PCL免费知识星球,点云论文速读. 标题:Camera calibration using two or three vanishing points 作者:Radu Orghidan∗, Joa ...
相机位姿估计2：[应用]实时位姿估计与三维重建相机姿态
关键词:相机位姿估计 OpenCV::solvePnP labview三维图片文章类型:应用展示+Demo演示 @Author:VShawn(singlex@foxmail.com) @Date:2 ...
全空间视频融合---学习之路（一）--直线提取检测--投影纹理--相机姿态标定
在增强现实的实际应用环境中,人们会为全空间大规模场景构造三维模型.为了增强虚拟环境的逼真性和动态性,可以向环境中融合真实世界的视频.透过视频可以获知模型该区域发生的事件.这就有了摄像机的视频信息和三 ...
图像scale与相机参数_基于Ladybug全景相机图像的360度全方位测距算法
基于Ladybug全景相机图像的360度全方位测距算法 [专利摘要]本发明公开了一种基于Ladybug全景相机图像的360度全方位测距算法,它涉及计算机视觉领域中的图像测距[技术领域].其方法步骤:读 ...

基于SfM计算相机姿态

基于SfM计算相机姿态相关推荐

最新文章

热门文章