#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
using namespace std;
using namespace cv;
//特征点匹配,传入两张图像,两张图像对应的特征点,最后生成的匹配存入matches数组
void find_feature_matches (
const Mat& img_1, const Mat& img_2,
std::vector<KeyPoint>& keypoints_1,
std::vector<KeyPoint>& keypoints_2,
std::vector< DMatch >& matches );
//像素坐标到相机归一化平面坐标,传如像素坐标值(Point2d类型const引用,快,又不改变原始值)和相机内参,返回归一化平面坐标(也是Point2d类型)
Point2d pixel2cam ( const Point2d& p, const Mat& K );
//BA优化,其实BA优化是单独的一块了。主函数中也就是一句调用这个BA函数,进行优化一下。
void bundleAdjustment (
const vector<Point3f> points_3d,
const vector<Point2f> points_2d,
const Mat& K,
Mat& R,
Mat& t
);
int main(int argc, char** argv)
{
if ( argc != 5 )
{
cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
return 1;
}
//-- 读取图像
Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );
vector<KeyPoint> keypoints_1, keypoints_2;
vector<DMatch> matches;
find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
cout<<"图一找到"<<keypoints_1.size() <<"个关键点"<<endl;
cout<<"图二找到"<<keypoints_2.size() <<"个关键点"<<endl;
cout<<"筛选后一共"<<matches.size() <<"组匹配点"<<endl;
// 建立3D点
Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED ); // 深度图为16位无符号数,单通道图像
Mat K = (Mat_<double
还是3D2D程序
最新推荐文章于 2021-11-20 16:17:23 发布
这篇博客深入探讨了3D和2D程序的设计与实现,涵盖了关键的技术概念和应用差异。
摘要由CSDN通过智能技术生成