c语言同名标靶点自动匹配算法实现实例代码

#include <iostream>#include <vector>#include <math.h>using namespace std;struct Point{ int id; float x,y,z;  Point(int _id,float _x,float _y,float _z):id(_id),x(_x),y(_y),z(_z){}};struct Triangle{ Point p1,p2,p3;};class PointCloud{public:  bool readFile(char *filename); //void PointMatch(const PointCloud& pc1,const PointCloud &pc2);   vector<Point> pData; int r,g,b;};bool PointCloud::readFile(char *filename){ if(filename==NULL)return false; FILE *fp=fopen(filename,"r"); if (!fp) {  printf("Load file %s failed/n", filename);  return false; } char buffer[100]; int count=0; while(!feof(fp)) {  fgets(buffer, 300, fp);  float x,y,z;   sscanf_s(buffer,"%f%f%f",&x,&y,&z);  Point p(count++,x,y,z);  pData.push_back(p); }  pData.pop_back();}float calDis(const Point &p1,const Point &p2){ return sqrt((p2.x-p1.x)*(p2.x-p1.x)+(p2.y-p1.y)*(p2.y-p1.y)+(p2.z-p1.z)*(p2.z-p1.z));}void PointMatch(const PointCloud& pc1,const PointCloud &pc2){  for (int i=0;i<pc1.pData.size()-2;++i)  for(int j=i+1;j<pc1.pData.size()-1;++j)   for (int k=j+1;k<pc1.pData.size();++k)   {    Point p1=pc1.pData[i];    Point p2=pc1.pData[j];    Point p3=pc1.pData[k];    float s12=calDis(p1,p2);    float s13=calDis(p1,p3);    float s23=calDis(p2,p3);    for (int m=0;m<pc2.pData.size();++m)     for (int n=0;n<pc2.pData.size();++n)      for (int l=0;l<pc2.pData.size();++l)      {       if (n!=m&&m!=l&&n!=l)       {              Point _p1=pc2.pData[m];        Point _p2=pc2.pData[n];        Point _p3=pc2.pData[l];        float _s12=calDis(_p1,_p2);        float _s13=calDis(_p1,_p3);        float _s23=calDis(_p2,_p3);        if (fabs(s12-_s12)<0.45&&fabs(s13-_s13)<0.45&&fabs(s23-_s23)<0.45)        {         cout<<"pc1....."<<p1.id<<"pc2......"<<_p1.id<<endl;         cout<<"pc1....."<<p2.id<<"pc2......"<<_p2.id<<endl;         cout<<"pc1....."<<p3.id<<"pc2......"<<_p3.id<<endl;        }       }      }   }}int main(){ char *file1="测试点云//点云Mark0.txt"; char *file2="测试点云//点云Mark1.txt"; PointCloud pc1,pc2; pc1.readFile(file1); pc2.readFile(file2); PointMatch(pc1,pc2);}