PPF算法OpenCV

算法OpenCV"/>

PPF算法OpenCV

opencv实现的PPF算法（surface_matching）中关于模型的写和读留有接口，但是没有具体的实现

1.在opencv_contrib-3.4.2\opencv_contrib-3.4.2\modules\surface_matching\include\opencv2\surface_matching\ppf_match_3d.hpp

void match(const Mat& scene, std::vector<Pose3DPtr> &results, const double relativeSceneSampleStep=1.0/5.0, const double relativeSceneDistance=0.03);void read(const FileNode& fn);void write(FileStorage& fs) const;

2、补充以上read和write的实现

我们的目标是希望将model离线训练，真实测量位姿的时候不需要重新训练一遍，直接读取model及相关参数进行估计和后续icp

这里要注意的是，模型需要存储的信息包括：

hashtable,ppf及angle_step、distance_step等参数，降采样后的modelFile数据sampled_pc也要保存

2.1 read实现

void PPF3DDetector::read(const FileNode &fn){fn["angle_step"] >> this->angle_step;fn["angle_step_radians"] >> this->angle_step_radians;fn["distance_step"] >> this->distance_step;fn["sampling_step_relative"] >> this->sampling_step_relative;fn["angle_step_relative"] >> this->angle_step_relative;fn["distance_step_relative"] >> this->distance_step_relative;fn["sampled_pc"] >> this->sampled_pc;fn["ppf"] >> this->ppf;fn["num_ref_points"] >> this->num_ref_points;fn["position_threshold"] >> this->position_threshold;fn["rotation_threshold"] >> this->rotation_threshold;fn["use_weighted_avg"] >> this->use_weighted_avg;fn["scene_sample_step"] >> this->scene_sample_step;fn["trained"] >> this->trained;// Hashtable deserializationstd::cout << " Hashtable deserialization" << std::endl;this->clearTrainingModels();int hash_table_size;fn["hash_table_size"] >> hash_table_size;hashtable_int *_hash_table = hashtableCreate(hash_table_size, NULL);THash *_hash_nodes = (THash *)calloc(hash_table_size, sizeof(THash));FileNode fn_nodes = fn["hash_table_nodes"];uint counter = 0;int id, i, ppf_ind;FileNode item;THash *thash_item;for (FileNodeIterator it = fn_nodes.begin(); it != fn_nodes.end(); it++){item = *it;item["id"] >> id;item["i"] >> i;item["ppfInd"] >> ppf_ind;thash_item = &_hash_nodes[counter];thash_item->id = id;thash_item->i = i;thash_item->ppfInd = ppf_ind;hashtableInsertHashed(_hash_table, id, (void *)thash_item);counter++;}
this->hash_nodes = _hash_nodes;this->hash_table = _hash_table;}

2.2 write实现

void PPF3DDetector::write(FileStorage &fs) const{fs << "angle_step" << this->angle_step;fs << "angle_step_radians" << this->angle_step_radians;fs << "distance_step" << this->distance_step;fs << "sampling_step_relative" << this->sampling_step_relative;fs << "angle_step_relative" << this->angle_step_relative;fs << "distance_step_relative" << this->distance_step_relative;fs << "sampled_pc" << this->sampled_pc;fs << "ppf" << this->ppf;fs << "num_ref_points" << this->num_ref_points;fs << "position_threshold" << this->position_threshold;fs << "rotation_threshold" << this->rotation_threshold;fs << "use_weighted_avg" << this->use_weighted_avg;fs << "scene_sample_step" << this->scene_sample_step;fs << "trained" << this->trained;// Hashtable serializationfs << "hash_table_size" << (int)this->hash_table->size;size_t n;struct hashnode_i *node;THash *data;fs << "hash_table_nodes" << "[";for (n = 0; n < this->hash_table->size; ++n){node = this->hash_table->nodes[n];while (node){data = (THash *)node->data;fs << "{";fs << "id" << data->id;fs << "i" << data->i;fs << "ppfInd" << data->ppfInd;fs << "}";node = node->next;}}fs << "]";}

3.Demo

为加速ICP速度，icp.registerModelToScene这里使用降采样后的点云

#include <iostream>
#include "../ppf/surface_matching.hpp"
#include "../ppf//ppf_helpers.hpp"
#include "opencv2/core/utility.hpp"
#include "opencv2/opencv.hpp"
#include <fstream>using namespace std;
using namespace cv;
using namespace ppf_match_3d;const char *DETECTOR_FILENAME = "detector.xml";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(int argc, char **argv)
{cout << "****************************************************" << endl;cout << "* Surface Matching demonstration: demonstrates the use of surface matching"" using point pair features."<< endl;cout << "* The sample loads a model and a scene, where the model lies in a different"" pose than the training.\n""* It then trains, serializes and deserializes the model, and searches for it in the input scene.\n""* The detected poses are further refined by ICP and printed to the standard output."<< endl;cout << "****************************************************" << endl;if (argc < 3){help("Not enough input arguments");exit(1);}#if (defined __x86_64__ || defined _M_X64)cout << "Running on 64 bits" << endl;
#elsecout << "Running on 32 bits" << endl;
#endif#ifdef _OPENMPcout << "Running with OpenMP" << endl;
#elsecout << "Running without OpenMP and without TBB" << endl;
#endif//string modelFileName = (string)argv[1];//string sceneFileName = (string)argv[2];string modelFileName = (string)"RobotData/Model.ply";string sceneFileName = (string)"RobotData/Scene5.ply";Mat pc = loadPLYSimple(modelFileName.c_str(), 1);int64 tick1, tick2;ifstream detectorFile(DETECTOR_FILENAME);if (!detectorFile.good()){{// Train the modelcout << "Training..." << endl;tick1 = cv::getTickCount();ppf_match_3d::PPF3DDetector detector(0.05, 0.05);detector.trainModel(pc);tick2 = cv::getTickCount();cout << "Training complete in "<< (double)(tick2 - tick1) / cv::getTickFrequency()<< " sec" << endl;// Serialize the modelcout << "Serializing..." << endl;tick1 = cv::getTickCount();FileStorage fsOut(DETECTOR_FILENAME, FileStorage::WRITE);detector.write(fsOut);fsOut.release();tick2 = cv::getTickCount();cout << "Serialization complete in "<< (double)(tick2 - tick1) / cv::getTickFrequency()<< " sec" << endl;}}else{cout << "Found detector file: Skipping training phase" << endl;}detectorFile.close();// Read the serialized modelppf_match_3d::PPF3DDetector detectorDes;cout << "Deserializing..." << endl;tick1 = cv::getTickCount();FileStorage fsLoad(DETECTOR_FILENAME, FileStorage::READ);detectorDes.read(fsLoad.root());fsLoad.release();tick2 = cv::getTickCount();cout << "Deserialization complete in "<< (double)(tick2 - tick1) / cv::getTickFrequency()<< " sec" << endl;// Read the sceneMat pcTest = loadPLYSimple(sceneFileName.c_str(), 1);// Match the model to the scene and get the posecout << "Starting matching using the deserialized model..." << endl;vector<Pose3DPtr> results;tick1 = cv::getTickCount();Mat pcTest_sampled, m_sampled_pc;//detectorDes.match(pcTest, results, 1.0 / 10.0, 0.05);detectorDes.match(pcTest, results, pcTest_sampled, m_sampled_pc, 1.0 / 10.0, 0.05);tick2 = cv::getTickCount();cout << "PPF Elapsed Time " << (tick2 - tick1) / cv::getTickFrequency()<< " sec" << endl;// Check results size from match call abovesize_t results_size = results.size();cout << "Number of matching poses: " << results_size << endl;if (results_size == 0){cout << "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 Nsize_t N = 1;if (results_size < N){cout << "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 ICPICP icp(100, 0.8f, 2.5f, 4);int64 t1 = cv::getTickCount();// Register for all selected posescout << "Performing ICP on " << N << " poses..." << endl;//icp.registerModelToScene(pc, pcTest, resultsSub);cout << "pc_sampled.size()" << m_sampled_pc.size() << endl;cout << "pctest_sampled.size()" << pcTest_sampled.size() << endl;icp.registerModelToScene(m_sampled_pc, pcTest_sampled, resultsSub);int64 t2 = cv::getTickCount();cout << "ICP Elapsed Time " << (t2 - t1) / cv::getTickFrequency() << " sec" << endl;cout << "Poses: " << endl;// Debug first five posesfor (size_t i = 0; i < resultsSub.size(); i++){Pose3DPtr result = resultsSub[i];cout << endl<< "Pose Result " << i << endl;result->printPose();if (i == 0){Mat pct = transformPCPose(pc, result->pose);// writePLY(pct, "para6700PCTrans.ply");writePLY(pct, "RobotData/modelTransToScene5_1.ply");}}return 0;
}

4. 实验结果

工程源码链接：

代码参考这位大佬