位姿估计 -- PPF算法的OpenCV实现

• 给出cpp代码，复制粘贴改一下文件路径记得。

1.pose.cpp

//
// Created by yaohua on 2020/7/3.
//

// 3.2.0
// https://docs.opencv.org/3.0-beta/modules/surface_matching/doc/surface_matching.html

#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/videoio.hpp"
#include <opencv2/opencv.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/surface_matching.hpp>
#include <opencv2/surface_matching/ppf_helpers.hpp>

#include <omp.h>
#include <tbb/tbb.h>

#include <iostream>

using namespace cv;
using namespace std;
using namespace ppf_match_3d;


static void help(const string& errorMessage)
{
    cout << "Program init error : "<< errorMessage << endl;
    cout << "\nUsage : ppf_matching [input model file] [input scene file]"<< endl;
    cout << "\nPlease start again with new parameters"<< endl;
}

int main()
{
    cout << "Built with OpenCV " << CV_VERSION << endl;

    #ifdef _OPENMP
        cout << "Running with OpenMP" << endl;
    #else
        cout << "Running without OpenMP and without TBB" << endl;
    #endif

    //omp_set_num_threads(16);
    //#pragma omp parallel for
    //    for (int i = 0; i < 10; ++i) {
    //        cout<<"hehe:"<<i<<endl;
    //    }

    // 快速测试：加载你的模型和场景试一试？
    Mat obj = loadPLYSimple("/home/哈哈/桌面/data/parasaurolophus_6700.ply", 1);
    Mat scene = loadPLYSimple("/home/哈哈/桌面/data/rs22_proc2.ply", 1);

    // Now train the model
    cout << "Training..." << endl;

    int64 tick1 = cv::getTickCount();
    //PPF3DDetector: （相对采样步长，相对离散步长（噪声多时，设大），离散角度数（30或噪声大时25、20））
    //cv::ppf_match_3d::PPF3DDetector::PPF3DDetector
    ppf_match_3d::PPF3DDetector detector(0.025, 0.05,30);

    //开始训练
    detector.trainModel(obj);

    int64 tick2 = cv::getTickCount();
    cout << endl << "Training complete in "
         << (double)(tick2-tick1)/ cv::getTickFrequency()
         << " sec" << endl << "Loading model..." << endl;

    //detector.write("123.yaml", );
    //detector.read();

    // 加载哈希表
    //hashtablePrint(hash_table);

    // 匹配
    cout<<"Start matching..."<<endl;
    vector<Pose3DPtr> results;  // 最终位姿列表
    tick1 = getTickCount();
    detector.match(scene, results, 1.0/40.0, 0.05);
    tick2 = getTickCount();
    cout<<"PPF Elapsed Time:"<<(tick2 - tick1)/getTickFrequency()<<endl;


    //check results size from match call above
    size_t results_size = results.size();
    cout << "Number of matching poses: " << results_size;
    if (results_size == 0) {
        cout << endl << "No matching poses found. Exiting." << endl;
        exit(0);
    }

    // Get only first N results - but adjust to results size if num of results are less than that specified by N
    size_t N = 2;
    if (results_size < N) {
        cout << endl << "Reducing matching poses to be reported (as specified in code): "
             << N << " to the number of matches found: " << results_size << endl;
        N = results_size;
    }

    vector<Pose3DPtr> resultsSub(results.begin(),results.begin()+N);

    // Create an instance of ICP
    ICP icp(100, 0.005f, 2.5f, 8);//（最大迭代次数，tolerence: 控制每次迭代的精度， '删除离群点(reject outliers)' 步骤中的scale系数，金字塔的层数：越深精度低速度快）
    int64 t1 = cv::getTickCount();

    // Register for all selected poses
    cout << endl << "Performing ICP on " << N << " poses..." << endl;
    icp.registerModelToScene(obj, scene, resultsSub);//srcPC,dstPC,double & 最终残差,Matx44d & pose )

    int64 t2 = cv::getTickCount();

    cout << endl << "ICP Elapsed Time " << (t2-t1)/cv::getTickFrequency() << " sec" << endl;

    cout << "Poses: " << endl;
    // debug first five poses
    for (size_t i=0; i<resultsSub.size(); i++)
    {
        Pose3DPtr result = resultsSub[i];
        cout << "Pose Result " << i << endl;
        result->printPose();
        if (i==0)
        {
            Mat pct = transformPCPose(obj, result->pose);
            writePLY(pct, "afterMatch.ply");
        }
    }

    return 0;
}

好吧我导入了很多库2.CmakeLists.txt

# 这是OpenCV的CMakeLists

cmake_minimum_required(VERSION 3.16)
project(example_cmake)

set(OpenCV_DIR /usr/local/opencv4/lib/cmake/opencv4)
set(OpenCV_INCLUDE_DIRS /usr/local/opencv4/include/opencv4)

include_directories(include ${OpenCV_INCLUDE_DIRS})

find_package(OpenCV REQUIRED)

FIND_PACKAGE( OpenMP REQUIRED)
if(OPENMP_FOUND)
    message("OPENMP FOUND")
    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
endif()


set(CMAKE_CXX_STANDARD 14)

add_executable(example_cmake
        CMakeLists.txt
#        example.cpp
        pose.cpp
#        open_camera.cpp
        )

target_link_libraries(example_cmake
        -I/usr/local/opencv4
        -I/usr/local/opencv4/include
        -L/usr/local/opencv4/lib
        -lopencv_dnn
        -lopencv_ml
        -lopencv_objdetect
        -lopencv_shape
        -lopencv_stitching
        -lopencv_superres
        -lopencv_videostab
        -lopencv_calib3d
        -lopencv_features2d
        -lopencv_highgui
        -lopencv_videoio
        -lopencv_imgcodecs
        -lopencv_video
        -lopencv_photo
        -lopencv_imgproc
        -lopencv_flann
        -lopencv_core
        -lpthread
        -lopencv_surface_matching
        )

3.运行结果：

Pose Result 1

-- Pose to Model Index 62507: NumVotes = 62508, Residual = 0.000000
0.000000 0.000000 0.000000 -3.769911 
0.000000 0.000000 0.995472 -0.062859 
0.071299 -64.149699 -0.004839 -0.782653 
-0.622440 -502.057083 0.094929 0.619277 

参考OpenCV的PPF demo