PPF算法OpenCV---surface_matching模型的write和read源码实现（文末附完整代码工程）

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 deserialization
			std::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 serialization

			fs << "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;
#else
	cout << "Running on 32 bits" << endl;
#endif

#ifdef _OPENMP
	cout << "Running with OpenMP" << endl;
#else
	cout << "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 model

			cout << "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 model

			cout << "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 model

	ppf_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 scene

	Mat pcTest = loadPLYSimple(sceneFileName.c_str(), 1);

	// Match the model to the scene and get the pose

	cout << "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 above

	size_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 N

	size_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 ICP

	ICP icp(100, 0.8f, 2.5f, 4);
	int64 t1 = cv::getTickCount();

	// Register for all selected poses

	cout << "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 poses

	for (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. 实验结果

工程源码链接：https://download.csdn.net/download/u013546077/15647464

代码参考这位大佬https://github.com/agmangas