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();
}
}