3d2d代码中的一些解读

最新推荐文章于 2024-05-10 11:09:34 发布

robinhjwy

最新推荐文章于 2024-05-10 11:09:34 发布

阅读量4.4k

点赞数 5

分类专栏： 14讲程序注释文章标签： SLAM

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

版权

cmakelists:

cmake_minimum_required(VERSION 3.7)
project(pose_estimation_3d2d)

set(CMAKE_CXX_STANDARD 11)

list(APPEND CMAKE_MODULE_PATH /home/roibn/install/g2o-master/cmake_modules)

find_package( OpenCV 3.1 REQUIRED )
find_package( G2O REQUIRED )
find_package( CSparse REQUIRED )

include_directories(
        ${OpenCV_INCLUDE_DIRS}
        ${G2O_INCLUDE_DIRS}
        ${CSPARSE_INCLUDE_DIR}
        "/usr/include/eigen3/"
)

set(SOURCE_FILES main.cpp)
add_executable(pose_estimation_3d2d ${SOURCE_FILES})
target_link_libraries( pose_estimation_3d2d
        ${OpenCV_LIBS}
        ${CSPARSE_LIBRARY}
        g2o_core g2o_stuff g2o_types_sba g2o_csparse_extension
        )

代码：

#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 pixel2cam ( const Point2d& p, const Mat& K );

//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() <<