其他分享
首页 > 其他分享> > 工作记录——6

工作记录——6

作者:互联网

Time : 2020.01.21

发现之前的标准欧式距离的公式理解错了,导致每次运行算出来的结果都是4,所以修改了程序,并记录在这。

listener.cpp

#include "ros/ros.h"
#include <std_msgs/String.h>
#include <package/talker1.h>                   //自定义头文件
#include <package/talker2.h>                   //自定义头文件
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>//相近对齐
#include <message_filters/time_synchronizer.h>//区别
#include <message_filters/synchronizer.h>//区别
#include <cstdio>
#include <stdlib.h>
#include <string>
#include <sstream>
#include <algorithm>

using namespace std;
using namespace message_filters;

typedef sync_policies::ApproximateTime<package::talker1,package::talker2> MySyncPolicy;  
//匈牙利算法用到的
	#define MAXN 990                  //MAXN表示X集合和Y集合顶点个数的最大值

class Listener{
	public:
	Listener(ros::NodeHandle _nh);//在构造函数中传入节点句柄,并调用 定义订阅节点函数
	~Listener(){};
	void callback(const package::talker1ConstPtr& msg1,const package::talker2ConstPtr& msg2);
	int path(int u);
	int maxMatch();
	
	private:
	ros::NodeHandle nh;
	int length,width,s,v,q,length1,width1,s1,v1,q1,s2,v2,q2,n1,n2;
	string type,type1,type2;
	double Result;
	int g[MAXN][MAXN];                //邻接矩阵,g[i][j]=1表示有连接 
	int cx[MAXN],cy[MAXN];            //cx[i],表示最终求得的最大匹配中,与x集合中元素Xi匹配的集合Y中顶点的索引//cy[i],表示最终求得的最大匹配中,与y集合中元素Yi匹配的集合X中顶点的索引
	int mk[MAXN];                     //DFS算法中记录顶点访问状态的数据mk[i]=0表示未访问过,为1表示访问过,标记的永远是二分图右边的顶点。


	//定义消息同步机制和成员变量
	message_filters::Subscriber<package::talker1>* sub1 ;  // topic1 输入
	message_filters::Subscriber<package::talker2>* sub2;   // topic2 输入
	message_filters::Synchronizer<MySyncPolicy>* sync;
};

Listener::Listener(ros::NodeHandle _nh)
{
//类构造函数中开辟空间new
	nh=_nh;
	sub1 = new message_filters::Subscriber<package::talker1>(nh, "message1", 1000);
    sub2 = new message_filters::Subscriber<package::talker2>(nh, "message2", 1000);
    sync = new  message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(10), *sub1, *sub2);
    sync->registerCallback(boost::bind(&Listener::callback,this, _1, _2));
}

void Listener::callback(const package::talker1ConstPtr& msg1,const package::talker2ConstPtr& msg2)
{
//类成员函数回调处理
//输出相机检测到几个障碍物:
	 n1=msg1->s;
//输出激光雷达检测到几个障碍物:
	 n2=msg2->s;	
	 cout<<n2<<endl;
//相机的检测到的障碍物的位置、角度、速度等依次输出(放到一个矩阵中(n1*6)):
	int m=0,n=0;
	double array[n2];

	vector<vector<double>> msg1vector(n1,vector<double>(6));
	for(int i=0;i<n1;i++){
		for(int j=0;j<6;j++){
			msg1vector[i][j]=msg1->array.data[m];//把相机检测到的每个障碍物的特征信息放到矩阵中:
			//cout<<msg1vector[i][j]<<endl;
			m=m+1;
		 }			
	}
	
	vector<vector<double>> msg2vector(n2,vector<double>(3));//定义存放激光雷达检测到的障碍物信息的矩阵(n2*3):
	for(int i=0;i<n2;i++){
		for(int j=0;j<3;j++){
			msg2vector[i][j]=msg2->array.data[n];//把激光雷达检测到的每个障碍物的特征信息放到矩阵中:
			//cout<<msg2vector[i][j]<<endl;
			n=n+1;
		 }	
	}

/*
//欧式距离
//定义一个矩阵存放欧式距离计算结果、msg1中有4个障碍物,msg2中有5个障碍物,那么矩阵的维数是4*5,即n1*n2(矩阵1的行数×矩阵2的行数);
 	vector<vector<double>> distanceresultV(n1,vector<double>(n2));	
	
	for(int i=0;i<n1;i++){
    	for(int j=0;j<n2;j++){
		double d=0.0;	     //需要注意d的位置
        	for(int k=0;k<3;k++){//数组a与数组b的列是一样的,因此k代表他们的列数。
        		d+=(msg1vector[i][k]-msg2vector[j][k])*(msg1vector[i][k]-msg2vector[j][k]);   //注意此处是msg1vector-msg2vector
				Result=sqrt(d);
        	}
        	distanceresultV[i][j]=Result;
        	cout<<"distanceresultV"<<i<<"."<<j<<"     "<<distanceresultV[i][j]<<endl; 
    	}
	}
 */


//标准欧式距离
//判断分母是否为零
//定义一个矩阵存放欧式距离计算结果、msg1中有4个障碍物,msg2中有5个障碍物,那么矩阵的维数是4*5,即n1*n2(矩阵1的行数×矩阵2的行数);
//////////////////////////////////////////////////////////////////////////////////////////
double U[3]={0,0,0};
double u1[3]={0,0,0};
double u2[3]={0,0,0};//定义存放均值的参数;u1、u2分别存放msg1vector、msg2vector;U存放最终的数据;
double S[3]={0,0,0};
double s1[3]={0,0,0};
double s2[3]={0,0,0};//定义存放均方根的参数;s1、s2分别存放msg1vector、msg2vector每列的数据;S[3]存放最终的数据;

	for(int i=0;i<3;i++){
		for(int j=0;j<n1;j++){
			//u1[i]+=msg1vector[j][i];//u1[0]存放的是第一组的 第一列,u1[1]存放的是第二列,u1[2]存放的是第三列;
		}
		cout<<"u1"<<i<<"     "<<u1[i]<<endl;
	}
	for(int i=0;i<3;i++){
		for(int j=0;j<n2;j++){
			u2[i]+=msg2vector[j][i];//u2[0]存放的是第二组的 第一列,u2[1]存放的是第二列,u2[2]存放的是第三列;
			//cout<<"u2"<<i<<"     "<<u2[i]<<endl;			
		}
	}
//求数组中每一列的均值
	for(int i=0;i<3;i++){
		U[i]=(u1[i]+u2[i])/(n1+n2);//U[0]存放的是第一列的均值,U[1]存放的是第二列的均值,U[2]存放的是第三列的均值;
		//cout<<"U"<<i<<"     "<<U[i]<<endl;
	}
//MSG1VECTOR第一列数据、第二列数据、第三列数据
	for(int i=0;i<3;i++){
		for(int j=0;j<n1;j++){
			s1[i]+=(msg1vector[j][i]-U[i])*(msg1vector[j][i]-U[i]);//s1[0]存放的是第一列,s1[1]存放的是第二列,s1[2]存放的是第三列;
		}
	}
//MSG2VECTOR第一列数据、第二列数据、第三列数据
	for(int i=0;i<3;i++){
		for(int j=0;j<n2;j++){
			s2[i]+=(msg2vector[j][i]-U[i])*(msg2vector[j][i]-U[i]);//s1[0]存放的是第一列,s1[1]存放的是第二列,s1[2]存放的是第三列;
		}
	}
//求数组中每一列的均方根
	for(int i=0;i<3;i++){
		S[i]=(s1[i]+s2[i])/(n1+n2);//U[0]存放的是第一列的均值,U[1]存放的是第二列的均值,U[2]存放的是第三列的均值;
		//cout<<"S"<<i<<"     "<<S[i]<<endl;
	}
vector<vector<double>> distanceresultV(n1,vector<double>(n2));
	for(int i=0;i<n1;i++){
    	for(int j=0;j<n2;j++){
        	double d=0.0;
			for(int k=0;k<3;k++){//数组a与数组b的列是一样的,因此k代表他们的列数。
				//判断分母是否为零:
				if(S[k]){
				d+=(msg1vector[i][k]-msg2vector[j][k])*(msg1vector[i][k]-msg2vector[j][k])/S[k];   //计算标准欧式距离的关键步骤
				Result=sqrt(d);
				}
    		}
        distanceresultV[i][j]=Result;
        cout<<"distanceresultV"<<i<<"."<<j<<"     "<<distanceresultV[i][j]<<endl; //输出第一组数据和第二组数据之间的距离
		}//distanceresultV[i][j]矩阵就是接下来要处理的数据
	}


  /*	vector<vector<double>> distanceresultV(n1,vector<double>(n2));
	for(int i=0;i<n1;i++){
    	for(int j=0;j<n2;j++){
        	double d=0.0;
        	for(int k=0;k<3;k++){//数组a与数组b的列是一样的,因此k代表他们的列数。
				double u[3],s[3];
				u[k]=(msg1vector[i][k]+msg2vector[j][k])/2;//计算平均值
				s[k]=(msg1vector[i][k]-u[k])*(msg1vector[i][k]-u[k]);//计算分母
				//判断分母是否为零:
				if(s[k]){
				d+=(msg1vector[i][k]-msg2vector[j][k])*(msg1vector[i][k]-msg2vector[j][k])/s[k];   //计算标准欧式距离的关键步骤
				Result=sqrt(d);
				}
        	}
        	distanceresultV[i][j]=Result;
        	cout<<"distanceresultV"<<i<<"."<<j<<"     "<<distanceresultV[i][j]<<endl; 
    	}
	}//distanceresultV[i][j]矩阵就是接下来要处理的数据
	*/

 // 找到每行的最小值及次小值,并将其置为1,其余值置为0
   for(int i=0;i<n1;i++){
        for(int j=0;j<n2;j++){
            array[j]=distanceresultV[i][j];//先取出来距离矩阵的第一行
        }
        double m1,m2;//存储两个最小值
        m1=9999;
        m2=9999;
        for(int k=0;k<n2;k++){
            if(array[k]<m1){
                m2=m1;
                m1=array[k];
            }
            else if(array[k]<m2){
                m2=array[k];
            }
        }
		cout<<m1<<" / "<<m2<<endl;	
       for(int l=0;l<n2;l++){
           if(array[l]==m1){
               //array[l]=1;
               g[i][l]=1;
           }
           else if(array[l]==m2){
               //array[l]=1;
               g[i][l]=1;
           }
           else{
               g[i][l]=0;
           }
       }
    }
 	
	for(int m=0;m<n1;m++){
        for(int n=0;n<n2;n++){
            cout<<"g["<<m<<"]"<<"["<<n<<"]"<<g[m][n]<<endl;
        }
    } 

//在回调函数中调用匈牙利匹配算法
	int num= Listener::maxMatch();//调用匈牙利算法最核心的步骤//输出了有多少组匹配的障碍物
    cout<<"num="<<num<<endl;
	//比较n1、n2大小;
	int t=n1>n2?n1:n2;

	cout<<t<<endl;
	//
	for(int i=0;i<t;++i){
        cout<<"cx["<<i+1<<"] --> "<<cx[i]+1<<endl;
		int q=cx[i];
		//cout<<"cx[i]+1: "<<cx[i]+1<<endl;
		//cout<<"q: "<<cx[i]<<endl;
		//相机的第p+1个目标与激光雷达的第cx[p]+1个对象相对应
	 	cout<<"相机数据1: "<<msg1vector[i][0]<<endl;
		cout<<"相机数据2: "<<msg1vector[i][1]<<endl;
		cout<<"相机数据3: "<<msg1vector[i][2]<<endl;

		cout<<"激光雷达数据1: "<<msg2vector[q][0]<<endl;
		cout<<"激光雷达数据2: "<<msg2vector[q][1]<<endl;
		cout<<"激光雷达数据3: "<<msg2vector[q][2]<<endl;
		//给相机和激光雷达的速度、方向以及距离的测量值分别附一个权重 输出(msg1和msg2均参与计算);输出障碍物的类别、长、宽(输出msg1相对应的. 就可以了)
	}

}
//开始处理
//从集合X中的定顶点u出发,用深度有限的策略寻找增广路
//这种增广路只能是当前的匹配数增加1
int Listener::path(int u){                    //传入的是左侧的顶点
    for(int v=0;v<n2;++v){          //考虑所有右侧顶点,从第一个开始扫描,依次向下
        if(g[u][v] && !mk[v]){      //如果左侧有右侧某顶点相连接,并且这个右侧顶点没有被此时的左侧顶点访问过
            mk[v]=1;                //标记右侧顶点,访问v
            	//接下来,判断这个右侧顶点。
            	//如果v没有匹配,则直接将v匹配给u。或者,如果v已经匹配了,但是从cy[v],也就是从v之前已经匹配的x出发,找到一条增广路,但是这里记住这里v已经记录访问过了
            	//如果第一个条件成立,则不会递归调用
            if(cy[v]==-1 || path(cy[v])){     //path()函数的返回值是1或者0;
            	//如果满足上述条件,更新cx cy
                cx[u]=v;            //把Y中v匹配给X中u
                cy[v]=u;            //把X中u匹配给Y中v
                return 1;           //找到左侧点的匹配值,那么将返回1
            }
        }
    }
    return 0;                        //如果不存在从u出发的增广路,则返回0
    //具体的说:如果左侧点找不到一个匹配点,那么将会返回0,此时path(cy[v])=0,就不会将右侧点与之前匹配的点拆散,而是左侧点重新依次向下寻找右侧链接顶点
}

int Listener::maxMatch(){                       //求二分图最大匹配的匈牙利算法;
    int res=0;
   	memset(cx,-1,sizeof(cx));         //从0匹配开始增广,将cx和xy各元素都初始化为-1;
    memset(cy,-1,sizeof(cy));
    for(int i=0;i<n1;++i){            //开始依次从左侧顶点匹配;
        if(cx[i]==-1){                //从X集合中每个没有匹配的点出发开始寻找增广路;
            memset(mk,0,sizeof(mk));  //每个左侧顶点匹配之前都要初始化;
            res+=Listener::path(i);             //输出匹配成功个数,或者if(path(i)) res++   
        }
    }
    	return res;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "listener");
	ros::NodeHandle nh;
	Listener listener(nh);
	//listener.result();
	ros::spin();
    return 0;
}

talker1.cpp

#include "ros/ros.h"
#include <std_msgs/String.h>
#include <package/talker1.h>                   //自定义头文件
//#include <vector>                   //自定义头文件
using namespace std;

class Talker{
public:
	Talker(ros::NodeHandle& _nh);
	~Talker(){};
	void registerPub(); //定义发布者
	void pub(); 

private:
   
	ros::Publisher publisher;//set the publisher as a member
	ros::NodeHandle nh;
};


int main(int argc, char **argv)
{
  	ros::init(argc, argv, "talker1");
  	ros::NodeHandle n;
  	Talker talker1(n);
  	talker1.registerPub();
 	talker1.pub();
 	return 0;
}

Talker::Talker(ros::NodeHandle& _nh)
{
	nh=_nh;
}

void Talker::registerPub()
{
        publisher = nh.advertise<package::talker1>("message1", 1000); 
}

void Talker::pub(){

	while (ros::ok()){
        ros::Rate loop_rate(100);//以10赫兹频率发布消息
	package::talker1 talker1msg;// 类型 example6package::talker1msg
	talker1msg.header.stamp=ros::Time::now();
	int n=4;//检测到的障碍物数量,应是动态可变的, 需要将其实时传递给接收节点。
	int m=6*n;//数组长度,6应是固定值,6-1是障碍物特征参数类别数量   对应类别采用数字表示 0是卡车 1是汽车 2是非机动车 3是人
	std::vector<double> carray={5.3,1.4,2.4,1.0,3.0,0.0,3.4,6.2,2.2,1.0,1.5,1.0,3.2,5.2,3.0,1.3,1.0,2.0,4.5,2.1,5.2,3.2,1.2,3.0};		
	//talker1msg.length=30;
	//talker1msg.width=30;
	//talker1msg.s=30;
	//talker1msg.v=30;
	//talker1msg.q=30;
	//talker1msg.type="car";
	talker1msg.array.data=carray;
	talker1msg.s=n;
        publisher.publish(talker1msg);	
	ros::spinOnce();//此处写循环
        loop_rate.sleep();
	}
}

talker2.cpp

#include "ros/ros.h"
#include <std_msgs/String.h>
#include <package/talker2.h>                   //自定义头文件
#include <vector>
using namespace std;

class Talker{
public:
	Talker(ros::NodeHandle& _nh);
	~Talker(){};
	void registerPub(); //定义发布者
	void pub(); 

private:
   
	ros::Publisher publisher;//set the publisher as a member
	ros::NodeHandle nh;
};


int main(int argc, char **argv)
{
  	ros::init(argc, argv, "talker2");
  	ros::NodeHandle n;
  	Talker talker2(n);
  	talker2.registerPub();
 	talker2.pub();
 	return 0;
}

Talker::Talker(ros::NodeHandle& _nh)
{
	nh=_nh;
}

void Talker::registerPub()
{
        publisher = nh.advertise<package::talker2>("message2", 1000); 
}

void Talker::pub(){

	while (ros::ok()){
        ros::Rate loop_rate(100);//以10赫兹频率发布消息
	package::talker2 talker2msg;
	talker2msg.header.stamp=ros::Time::now();
	int n=5;//检测到的障碍物数量,应是动态可变的
	int m=3*n;//数组长度,3是检测到的每个障碍无的特征数据
	vector<double> carray={3.2,1.5,1.7,2.3,2.5,3.3,1.8,1.4,4.4,1.1,3.2,2.1,3.1,1.5,2.6};
	talker2msg.array.data=carray;
	talker2msg.s=n;
	///talker2msg.s=33;
	///talker2msg.v=34;
	///talker2msg.q=32;
        publisher.publish(talker2msg);
	ros::spinOnce();//此处写循环
        loop_rate.sleep();
	}
}

罗宾酱 发布了10 篇原创文章 · 获赞 6 · 访问量 303 私信 关注

标签:nh,include,记录,int,talker1msg,工作,Talker,ros
来源: https://blog.csdn.net/weixin_39652282/article/details/104060964