第七讲部分课后习题

.gitignore

@@ -2,3 +2,4 @@ cmake-build-debug/
 .idea/
 .vscode/
 build/
+project

CMakeLists.txt

@@ -94,5 +94,11 @@ include_directories(ch3/useEigen)
 # target_link_libraries(direct_sparse ${OpenCV_LIBS} g2o_core g2o_stuff g2o_solver_csparse g2o_types_sba g2o_csparse_extension ${CSPARSE_LIBRARY})
 
 #半稠密直接法估计位姿
-add_executable(direct_semidense ch8/direct/direct_semidense.cpp)
-target_link_libraries(direct_semidense ${OpenCV_LIBS} g2o_core g2o_stuff g2o_solver_csparse g2o_types_sba g2o_csparse_extension ${CSPARSE_LIBRARY})
+# add_executable(direct_semidense ch8/direct/direct_semidense.cpp)
+# target_link_libraries(direct_semidense ${OpenCV_LIBS} g2o_core g2o_stuff g2o_solver_csparse g2o_types_sba g2o_csparse_extension ${CSPARSE_LIBRARY})
+
+# add_executable(p181 ch7/feature/exercise/p181.cpp)
+# target_link_libraries(p181 ${OpenCV_LIBS})
+
+add_executable(pose_estimation_3d2d ch7/feature/exercise/pose_estimation_3d2d.cpp ch7/feature/pose_estimation_2d2d.cpp)
+target_link_libraries(pose_estimation_3d2d ${OpenCV_LIBS} g2o_core g2o_stuff g2o_types_sba g2o_csparse_extension ${CSPARSE_LIBRARY})

ch7/feature/exercise/p181.cpp

@@ -0,0 +1,160 @@
+//
+// Created by bob on 19/04/10.
+//
+
+/**
+ * 2、设计程序调用OpenCV中的其他种类特征点。统计在提取1000个特征点时在你的机器上所用的时间
+ */
+
+
+
+#include <chrono>
+#include <opencv2/core/core.hpp>
+#include <opencv2/opencv.hpp>
+#include <opencv2/xfeatures2d/nonfree.hpp>
+#include <opencv2/xfeatures2d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#define MAX_POINTS 1000
+#define FILE_PATH "/home/bob/CLionProjects/slam14/ch7/feature/p1.jpg"
+using namespace std;
+
+cv::Mat img = cv::imread(FILE_PATH);
+
+inline chrono::steady_clock::time_point getNow(){
+    return chrono::steady_clock::now();
+}
+
+/**
+ * HOG特征的提取
+ */
+void getSIFT(){
+    vector<cv::KeyPoint> kp;
+    cv::Ptr<cv::xfeatures2d::SiftFeatureDetector> detector = cv::xfeatures2d::SiftFeatureDetector::create(1000);
+
+    auto t1 = getNow();
+    detector->detect(img, kp);
+    auto t2 = getNow();
+
+    std::cout << "SIFT总共找出来" << kp.size() << "个特征点" << std::endl;
+    chrono::duration<double> time_used = t2-t1;
+    std::cout << "耗时：" << time_used.count() << "秒" << std::endl;
+    cv::Mat c_image;
+    img.copyTo(c_image);
+
+    if (!kp.empty())
+    {
+        for (auto k : kp)
+        {
+            cv::circle(c_image, k.pt, 5, cv::Scalar(0, 240, 0), 1);
+        }
+
+        cv::imshow("SIFT", c_image);
+        cv::waitKey(0);
+    }
+}
+
+//TODO: 做题！
+/**
+ * SURF特征提取
+ */
+
+
+void getSURF(){
+    vector<cv::KeyPoint> kp;
+    cv::Ptr<cv::xfeatures2d::SURF> detector = cv::xfeatures2d::SURF::create(2800);
+
+    auto t1 = getNow();
+    detector->detect(img, kp);
+    auto t2 = getNow();
+
+    std::cout << "SURF总共找出来" << kp.size() << "个特征点" << std::endl;
+    chrono::duration<double> time_used = t2 - t1;
+    std::cout << "耗时：" << time_used.count() << "秒" << std::endl;
+    cv::Mat c_image;
+    img.copyTo(c_image);
+
+    if (!kp.empty())
+    {
+       for (auto k : kp){
+           cv::circle(c_image,k.pt,5,cv::Scalar(0,240,0),1);
+       } 
+
+       cv::imshow("SURF",c_image);
+       cv::waitKey(0);
+    }
+}
+
+void getBRISK(){
+    vector<cv::KeyPoint> kp;
+    cv::Ptr<cv::BRISK> detector = cv::BRISK::create();
+
+    auto t1 = getNow();
+    detector->detect(img, kp);
+    auto t2 = getNow();
+
+    std::cout << "BRISK总共找出来" << kp.size() << "个特征点" << std::endl;
+    chrono::duration<double> time_used = t2 - t1;
+    std::cout << "耗时：" << time_used.count() << "秒" << std::endl;
+    cv::Mat c_image;
+    img.copyTo(c_image);
+
+    if (!kp.empty())
+    {
+        for (auto k : kp)
+        {
+            cv::circle(c_image, k.pt, 5, cv::Scalar(0, 240, 0), 1);
+        }
+
+        cv::imshow("BRISK", c_image);
+        cv::waitKey(0);
+    }
+}
+
+void getAKAZE(){
+    vector<cv::KeyPoint> kp;
+    cv::Ptr<cv::AKAZE> detector = cv::AKAZE::create();
+
+    auto t1 = getNow();
+    detector->detect(img, kp);
+    auto t2 = getNow();
+
+    std::cout << "AKAZE总共找出来" << kp.size() << "个特征点" << std::endl;
+    chrono::duration<double> time_used = t2 - t1;
+    std::cout << "耗时：" << time_used.count() << "秒" << std::endl;
+    cv::Mat c_image;
+    img.copyTo(c_image);
+
+    if (!kp.empty())
+    {
+        for (auto k : kp)
+        {
+            cv::circle(c_image, k.pt, 5, cv::Scalar(0, 240, 0), 1);
+        }
+
+        cv::imshow("AKAZE", c_image);
+        cv::waitKey(0);
+    }
+}
+/**
+ * 输出：
+ * 
+ *SURF总共找出来779个特征点
+**耗时：0.153737秒
+**SIFT总共找出来1000个特征点
+**耗时：0.487359秒
+**BRISK总共找出来4331个特征点
+**耗时：0.081469秒
+**AKAZE总共找出来2829个特征点
+**耗时：0.277592秒
+ */
+void show(){
+    cv::imshow("show",img);
+    cv::waitKey(0);
+}
+int main(int argc, char** argv){
+    getSURF();
+    getSIFT();
+    getBRISK();
+    getAKAZE();
+}

ch7/feature/exercise/pose_estimation_3d2d.cpp

@@ -0,0 +1,178 @@
+//
+// Created by bob on 19/04/14.
+//
+
+/**
+ * 将第一个相机位姿也加入优化变量
+ */
+#include <iostream>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/calib3d.hpp>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <g2o/core/base_unary_edge.h>
+#include <g2o/core/block_solver.h>
+#include <g2o/core/base_vertex.h>
+#include <g2o/core/base_unary_edge.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>
+
+#include <chrono>
+
+using namespace std;
+
+void feature_extraction(cv::Mat &img_1, cv::Mat &img_2, vector<cv::KeyPoint> &kp_1, vector<cv::KeyPoint> &kp_2, vector<cv::DMatch> &matches);
+cv::Point2d pixel2cam(const cv::Point2f &p, cv::Mat &K);
+
+void bundleAdjustment(const vector<cv::Point3f> points_3d,
+                      const vector<cv::Point2f> points_2d_1,
+                      const vector<cv::Point2f> points_2d,
+                      const cv::Mat &K,
+                      cv::Mat &R,
+                      cv::Mat &t)
+{
+    //位姿李代数维度为6，地标landmark维度为3
+    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block;
+    //CSPARSE是一个求解线性问题的矩阵库
+    Block::LinearSolverType *lineasolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();
+    Block *solver_ptr = new Block(unique_ptr<Block::LinearSolverType>(lineasolver));
+    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(unique_ptr<Block>(solver_ptr));
+    g2o::SparseOptimizer optimizer;
+    optimizer.setAlgorithm(solver);
+
+    //顶点１
+    g2o::VertexSE3Expmap* pose_1 = new g2o::VertexSE3Expmap();
+    pose_1->setFixed(true);
+    pose_1->setId(1);
+    pose_1->setEstimate(g2o::SE3Quat());
+    optimizer.addVertex(pose_1);
+    //顶点2
+    g2o::VertexSE3Expmap *pose = new g2o::VertexSE3Expmap(); //相机位姿
+    Eigen::Matrix3d R_mat;
+    R_mat << R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
+        R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
+        R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2);
+
+    pose->setId(0);
+    pose->setEstimate(g2o::SE3Quat(R_mat, Eigen::Vector3d(t.at<double>(0, 0), t.at<double>(0, 1), t.at<double>(0, 2))));
+    optimizer.addVertex(pose);
+
+    //空间点坐标加入优化
+    int index = 2;
+    for (const cv::Point3f p : points_3d)
+    {
+        g2o::VertexSBAPointXYZ *point = new g2o::VertexSBAPointXYZ();
+        point->setId(index++);
+        point->setEstimate(Eigen::Vector3d(p.x, p.y, p.z));
+        //边缘化以稀疏求解
+        point->setMarginalized(true);
+        optimizer.addVertex(point);
+    }
+
+    //设定相机参数
+    g2o::CameraParameters *camera = new g2o::CameraParameters(K.at<double>(0, 0), Eigen::Vector2d(K.at<double>(0, 2), K.at<double>(1, 2)), 0);
+
+    camera->setId(0);
+    optimizer.addParameter(camera);
+
+    int count = 0;
+    //边,第一个相机加入优化
+    index = 2;
+    for (const cv::Point2f p : points_2d_1)
+    {
+        g2o::EdgeProjectXYZ2UV *edge = new g2o::EdgeProjectXYZ2UV();
+        //将边与点一一对应？
+        edge->setId(count++);
+        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(index)));
+        edge->setVertex(1, pose_1);
+        edge->setMeasurement(Eigen::Vector2d(p.x, p.y));
+        //以上面相机参数为准
+        edge->setParameterId(0, 0);
+        edge->setInformation(Eigen::Matrix2d::Identity());
+        optimizer.addEdge(edge);
+    }
+
+    //第二幅相机加入
+    for (const cv::Point2f p : points_2d)
+    {
+        g2o::EdgeProjectXYZ2UV *edge = new g2o::EdgeProjectXYZ2UV();
+        //将边与点一一对应？
+        edge->setId(count++);
+        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(index)));
+        edge->setVertex(1, pose);
+        edge->setMeasurement(Eigen::Vector2d(p.x, p.y));
+        //以上面相机参数为准
+        edge->setParameterId(0, 0);
+        edge->setInformation(Eigen::Matrix2d::Identity());
+        optimizer.addEdge(edge);
+    }
+    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
+    optimizer.setVerbose(true);
+    optimizer.initializeOptimization();
+    //最多100次迭代
+    optimizer.optimize(100);
+    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
+    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
+    cout << "g2o图优化耗时：" << time_used.count() << "秒." << endl;
+
+    cout << "经过g2o图优化之后的欧氏变换矩阵T1：" << endl;
+    cout << Eigen::Isometry3d(pose_1->estimate()).matrix() << endl;
+
+    cout << "经过g2o图优化之后的欧氏变换矩阵T2：" << endl;
+    cout << Eigen::Isometry3d(pose->estimate()).matrix() << endl;
+}
+
+int main(int argc,char** argv){
+   if (argc != 4)
+   {
+       cout << "请确保传入三张图像来做特征提取与匹配demo:feature_extraction img1_path img2_path depth_image_path" << endl;
+       return 1;
+   }
+
+   cv::Mat img_1 = cv::imread(argv[1],CV_LOAD_IMAGE_COLOR);
+   cv::Mat img_2 = cv::imread(argv[2],CV_LOAD_IMAGE_COLOR);
+   //深度图
+   cv::Mat d1 = cv::imread(argv[3],CV_LOAD_IMAGE_UNCHANGED);
+   cv::Mat K = (cv::Mat_<double>(3,3) << 520.9,0,325.1,0,521.0,249.7,0,0,1);
+
+   vector<cv::KeyPoint> kp_1,kp_2;
+   vector<cv::DMatch> matches;
+
+   feature_extraction(img_1,img_2,kp_1,kp_2,matches);
+
+   vector<cv::Point3f> points_3d;
+   vector<cv::Point2f> points_2d;
+   vector<cv::Point2f> points_2d_1;
+   for (auto m : matches){
+       //获取相应特征点对应的深度
+       ushort d = d1.ptr<unsigned short>(int(kp_1[m.queryIdx].pt.y))[int(kp_1[m.queryIdx].pt.x)];
+       if (d == 0)
+           continue;
+
+       float dd = d / 1000.0;
+       cv::Point2d p1 = pixel2cam(kp_1[m.queryIdx].pt,K);
+       points_2d.push_back(kp_2[m.trainIdx].pt);
+       points_2d_1.push_back(kp_1[m.queryIdx].pt);
+       points_3d.emplace_back(p1.x*dd,p1.y*dd,dd);
+   }
+
+   cout << "3D-2D匹配点对数：" << points_3d.size() << endl;
+
+   cv::Mat r,t;
+
+   cv::solvePnP(points_3d,points_2d,K,cv::Mat(),r,t,false,cv::SOLVEPNP_EPNP);
+
+   cv::Mat R;
+   cv::Rodrigues(r,R);
+
+
+   cout << "EPnP之后的R矩阵：" << endl << R << endl;
+   cout << "EPnP估测的t：" << endl << t << endl;
+
+   bundleAdjustment(points_3d,points_2d_1,points_2d,K,R,t);
+}