#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <stdio.h>
#include <stdlib.h>
#include "opencv2/surface_matching.hpp"
#include <iostream>
#include "opencv2/surface_matching/ppf_helpers.hpp"
#include "opencv2/core/utility.hpp"

using namespace std;
using namespace cv;
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(int argc,char **argv)
{
// welcome message
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 the model and searches for it in the"
" input scene. The detected poses are further refined by ICP\n* and printed to the "
" standard output." << endl;
cout << "****************************************************" << endl;

/*if (argc < 3)
{
help("Not enough input arguments");
cout << argc<< endl;
exit(1);
}*/

#if (defined __x86_64__ || defined _M_X64)
cout << "Running on 64 bits" << endl;
//typedef uint64_t KeyType;
#else
cout << "Running on 32 bits" << endl;
//typedef unsigned int KeyType;
#endif

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

string modelFileName = "/home/mingchi/catkin_ws/src/opencv_test/table3.ply";
string sceneFileName = "/home/mingchi/catkin_ws/src/opencv_test/test.ply";

int64* p1;
int64* p2;
Mat* pc;
Mat* pcTest;


Mat pc_1 = loadPLYSimple(modelFileName.c_str(), 1);
//Mat pc = loadPLYSimple("/home/mingchi/catkin_ws/src/opencv_test/box2.ply", 1);
pc=&pc_1;
// Now train the model
cout << "Training..." << endl;
int64 tick1 = cv::getTickCount();
p1=&tick1;
ppf_match_3d::PPF3DDetector detector(0.025, 0.05);
detector.trainModel(*pc);
int64 tick2 = cv::getTickCount();
p2=&tick2;
cout << endl << "Training complete in "
<< (double)(*p2-*p1)/ cv::getTickFrequency()
<< " sec" << endl << "Loading model..." << endl;
// Read the scene
Mat pcTest_1 = loadPLYSimple(sceneFileName.c_str(), 1);
//Mat pcTest = loadPLYSimple("/home/mingchi/catkin_ws/src/opencv_test/test.ply", 1);
pcTest=&pcTest_1;
cout << endl << pcTest_1.size() << endl;

// Match the model to the scene and get the pose
cout << endl << "Starting matching..." << endl;
vector<Pose3DPtr> results;
int64 tick3 = cv::getTickCount();
*p1=tick3;
detector.match(*pcTest, results, 1.0/5.0, 0.03);
int64 tick4 = cv::getTickCount();
*p2=tick4;
cout << endl << "PPF Elapsed Time " <<
(*p2-*p1)/cv::getTickFrequency() << " sec" << endl;
//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
int N = 1;
/*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 (iterations,tolerance,rejection scale,numlevels)
ICP icp(100, 0.05f, 2.5f, 8);
int64 t1 = cv::getTickCount();
*p1=t1;
// Register for all selected poses
cout << endl << "Performing ICP on " << N << " poses..." << endl;
icp.registerModelToScene(*pc, *pcTest, resultsSub);
int64 t2 = cv::getTickCount();
*p2=t2;
cout << endl << "ICP Elapsed Time " <<
(*p2-*p1)/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(*pc, result->pose);
writePLY(pct, "para6700PCTrans.ply");
}
}

return 0;

}

這是我的原程式碼,並在ubuntu16.04的ros上進行編譯
輸入不同的場景點雲有時可以正常進行match並求出rt矩陣
困擾我很久的是,有時會產生Segmentation fault的問題
用gdb進行調試,發現程式跑到
detector.match(*pcTest, results, 1.0/5.0, 0.03);
後便會出現segfault並中斷程式
上網找很久發現可能是內存溢位等等的問題 （不確定）
但卻不知道如何分配內存

以下是gdb調試信息：
(gdb)
91 detector.match(*pcTest, results, 1.0/5.0, 0.03);
(gdb)

Program received signal SIGSEGV, Segmentation fault.
_int_malloc (av=av@entry=0x7ffff66acb20 <main_arena>, bytes=bytes@entry=191280)
at malloc.c:3809
3809 malloc.c: No such file or directory.
(gdb) bt
#0 _int_malloc (av=av@entry=0x7ffff66acb20 <main_arena>,
bytes=bytes@entry=191280) at malloc.c:3809
#1 0x00007ffff636ce3a in __libc_calloc (n=<optimized out>,
elem_size=<optimized out>) at malloc.c:3243
#2 0x00007ffff6c62aa3 in cv::ppf_match_3d::PPF3DDetector::match(cv::Mat const&, std::vector<cv::Ptr<cv::ppf_match_3d::Pose3D>, std::allocator<cv::Ptr<cv::ppf_match_3d::Pose3D> > >&, double, double) ()
from /opt/ros/kinetic/lib/x86_64-linux-gnu/libopencv_surface_matching3.so.3.3
#3 0x00000000004044fb in main (argc=1, argv=0x7fffffffdae8)
at /home/mingchi/catkin_ws/src/opencv_test/src/pose_estimation.cpp:91
(gdb) l
3804 in malloc.c
(gdb) c
Continuing.

Program terminated with signal SIGSEGV, Segmentation fault.
The program no longer exists.


請各位大神不吝賜教

有什麼理由要使用point, 範例都沒有這麼用了