还是3D2D程序

最新推荐文章于 2021-08-22 15:44:47 发布

robinhjwy

最新推荐文章于 2021-08-22 15:44:47 发布

阅读量844

点赞数

分类专栏： 14讲程序注释文章标签： 3d 2d

本文链接：https://blog.csdn.net/robinhjwy/article/details/77803547

版权

这篇博客深入探讨了3D和2D程序的设计与实现，涵盖了关键的技术概念和应用差异。

摘要由CSDN通过智能技术生成

#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