同名标靶点自动匹配算法实现-三点匹配

来源：互联网发布：淘宝买水果靠谱吗知乎编辑：程序博客网时间：2024/04/30 07:56
1.两个标靶数据
这里写图片描述

2.匹配结果

3.代码
#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);}
0 0