Time ：2020.01.23
这是假期前修改的程序。记忆中修改的部分是将激光雷达和相机检测到的障碍物的位置、速度、角度做加权处理并输出，此外还输出了相机检测到障碍物的类别、长、宽。
#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;
/*************************************************************************************************************************************
定义二位数组，将多传感器的输出信息传入各自数组中，方便后续计算
*************************************************************************************************************************************/
//相机的检测到的障碍物的位置、角度、速度等依次输出（放到一个矩阵中(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
/*************************************************************************************************************************************
处理存储几何距离的数组（将最小的两个置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];
}
}
//2020.01.21
double m3=m2-m1; //给出一个阈值判断，防止最小值和次小值差距太大，但仍将次小值视为匹配对象；
//cout<<"m1: "<<m1<<"m2: "<<m2<<"m3: "<<m3<<endl;
if(m3<=0.8){ //更改阈值大小可以决定匹配准确程度，阈值越小，搜索范围越小；
for(int l=0;l<n2;l++){
if(array[l]==m1){
g[i][l]=1;
}
else if(array[l]==m2){
g[i][l]=1;
}
else{
g[i][l]=0;
}
}
}
else{
for(int l=0;l<n2;l++){
if(array[l]==m1){
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;
vector<vector<double>> Result1(n1,vector<double>(6));
vector<vector<double>> Result2(n1,vector<double>(3));
vector<vector<double>> Result(n1,vector<double>(6));
for(int i=0;i<n1;++i){ ////////此处的n1应该是num吧，即匹配的个数
cout<<"cx["<<i+1<<"] --> "<<cx[i]+1<<endl;
int q=cx[i]; //q也是与i相关的数据 //相机的第i个目标与激光雷达的第cx[i]（第q）个对象相对应
/***************************************************************************************************************
对相机中的第i个目标与激光雷达中的第q个目标的前三列做加权处理
（至于后面采用什么样的加权算法还需要确定）
输出相机检测到的目标的长、宽及类别信息
************************************************************************************************ ***************/
//给相机和激光雷达的速度、方向以及距离的测量值分别附一个权重 输出（msg1和msg2均参与计算）；输出障碍物的类别、长、宽（输出msg1相对应的. 就可以了）
//假设相机的检测结果赋权重是{0.3,0.45,0.5,1,1,1} 列向量p1
//假设激光雷达的检测结果赋权重是{0.7,0.55,0.5} 列向量p1
double p1[6]={0.3,0.45,0.5,1,1,1};
double p2[3]={0.7,0.55,0.5};
//存放msg1vector乘上权重后的数据
for(int k=0;k<6;k++){
Result1[i][k]=(msg1vector[i][k])*p1[k];
Result[i][k]=Result1[i][k];//把Result1直接赋值给Result，这样Result1后边的三列数据就会给Result，后边直接将这两组矩阵的前三列相加即可
//cout<<"计算结果1是 : "<<Result1[i][k]<<endl;
}
//计算msg2vector乘上权重后的数据
for(int k=0;k<3;k++){
Result2[i][k]=(msg2vector[q][k])*p2[k];
//cout<<"计算结果2是 : "<<Result2[i][k]<<endl;
}
for(int k=0;k<3;k++){
Result[i][k]=Result1[i][k]+Result2[i][k];
}
/*****************************************************************************************************************
输出类别
******************************************************************************************************************/
int ii = Result[i][5];//这个是5，不是6
/*stringstream stream; //这个是将int类型转换成string类型；switch类型的表达式需要整形或者枚举型，所以不需要该部分
string result;
int ii = Result[i][5];//这个是5，不是6
cout<<"Result[i][6]"<<Result[i][6]<<endl; //
stream<<ii; //将int输入流
stream>>result; //从stream中抽取前面插入的int值
cout<<result<<endl; //
stream.clear();*/
switch(ii){//switch语句中的expression必须是一个整型或枚举类型，或者是一个class类型，其中class有一个单一的转换函数将其转换为整型或枚举类型。
case 0://当是整型数据时，直接case int类型，不需要 case 'A' :
cout<<"障碍物"<<i<<"的类型是： 卡车" << endl;
break;
case 1:
cout<<"障碍物"<<i<<"的类型是： 汽车" << endl;
break;
case 2:
cout<<"障碍物"<<i<<"的类型是： 非机动车" << endl;
break;
case 3:
cout<<"障碍物"<<i<<"的类型是： 行人" << endl;
break;
default:
cout<<"障碍物"<<i<<"的类型是： 无效的目标" << endl;
}
/*****************************************************************************************************************
输出障碍物的位置、速度、方向、长和宽
******************************************************************************************************************/
cout<<"障碍物"<<i<<"的位置是 : "<<Result[i][0]<<"m"<<endl;
cout<<"障碍物"<<i<<"的速度是 : "<<Result[i][1]<<"km/h"<<endl;
cout<<"障碍物"<<i<<"的角度是 : "<<Result[i][2]<<"°"<<endl;
cout<<"障碍物"<<i<<"的长是 : "<<Result[i][3]<<"m"<<endl;
cout<<"障碍物"<<i<<"的宽是 : "<<Result[i][4]<<"m"<<endl;
}
}//callback的末尾括号
/*************************************************************************************************************************************
匈牙利算法类外实现
*************************************************************************************************************************************/
//从集合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);
ros::spin();
return 0;
}