https://blog.csdn.net/cysisu/article/details/83411409
以上链接的代码实现的是基于有向图实现得到k条最短路径的算法,k条最短路径的算法是在上交大那篇论文中看到的思想,于是在网上找到了这个实现的代码,但是在利用此代码接入自己的地图时规划出来的路径有些是不对的,根本不通的,原因就是我们自己的地图是存在闭环的,而上述链接中的地图是不存在闭环的。于是就对其进行了相应的修改:
K-Shortest-Path算法的思想:
https://www.cnblogs.com/qq952693358/p/7354070.html
改写这个代码的过程就是:将其写成了一个类,在自己的架构中去调用。将他的代码中的函数写成类中的共有成员函数,在自己的架构中去调用接口传参就可以了。
一.首先讲一下如何对算法进行的修改
主要做了以下几方面的修改:
(1)在规划出一条最短路径后会基于此最短路径做迭代,迭代的过程中会将逐个将最短路径中的边设置为无穷大,然后去更新地图,此链接中给出的代码是(修改前的代码):
vector<vector<float> > K_Shortest_Path::cutEdge(const vector<vector<float> > &NW, vector<DijPath> kSPCost, unsigned int root)
{
vector<vector<float>>NWCopy = NW;
for (unsigned int i = 0; i < kSPCost.size(); i++)
{
//此for循环只是为了寻找和root相同的点
for (unsigned int j = 0; j < kSPCost[i].onePath.size(); j++)
{
if (kSPCost[i].onePath[j] == root)
{
unsigned int nextVertex = kSPCost[i].onePath[j + 1];
//1.此处是做出修改的第一步
if (j >= 1)
{
unsigned int beforeVertex = kSPCost[i].onePath[j - 1];
NWCopy[root][beforeVertex] = INF;
}
NWCopy[root][nextVertex] = INF; //设置为不可连接
break;
}
}
}
return NWCopy;
}
修改后的代码:也就是屏蔽掉了if的判断条件。
vector<vector<float> > K_Shortest_Path::cutEdge(const vector<vector<float> > &NW, vector<DijPath> kSPCost, unsigned int root)
{
vector<vector<float>>NWCopy = NW;
for (unsigned int i = 0; i < kSPCost.size(); i++)
{
//此for循环只是为了寻找和root相同的点
for (unsigned int j = 0; j < kSPCost[i].onePath.size(); j++)
{
if (kSPCost[i].onePath[j] == root)
{
unsigned int nextVertex = kSPCost[i].onePath[j + 1];
//1.此处是做出修改的第一步
// if (j >= 1)
// {
// unsigned int beforeVertex = kSPCost[i].onePath[j - 1];
// NWCopy[root][beforeVertex] = INF;
// }
NWCopy[root][nextVertex] = INF; //设置为不可连接
break;
}
}
}
return NWCopy;
}
(2)第二处做出的修改:在run()函数中,在基于第一条最短路径迭代规划接下来的最短路径的时候,if的判断条件进行了修改如下:
//前面的那个节点到终点的一条最短路径
DijPath secondPath = dij.dijkstra(NWCopy, kSPCost[k - 1].onePath[i], dst);
if (secondPath.cost > 100000 || secondPath.cost ==0 )//判断两点不可以到达
{
continue;
}
之前是:
DijPath secondPath = dij.dijkstra(NWCopy, kSPCost[k - 1].onePath[i], dst);
if (secondPath.cost > 100000)//判断两点不可以到达
{
continue;
}
cost变量之前是int类型的,在打印secondPath.cost这个值的时候发现有负无穷大的值也有0,所以将cost变量变成unsigned int 类型,这样就不会出现负无穷大了,另外再将判断条件加上一个==0,这样就可以解决了。
以后再换别的地图去用的时候,这个判断条件可以根据secondPath.cost的只去修改。
二.其次讲一下在这个将此链接中的代码用在自己的框架中时遇到的问题:
(1)系统总是出现崩溃死掉的情况,原因是定义对象指针的时候没有进行实例化;
vector<vector<unsigned int>> TaskManagerAGV::show_ksp(int startpoint, int endpoint)
{
// qDebug()<<"entre the show_ksp";
vector<vector<unsigned int>>pathlist,pathlistShow;
QVector<QStringList> shortPathMatrix = getshortpathmatrix();
pathlist = k_short_path->searchPath(startpoint, endpoint, shortPathMatrix, pathNodeMap, NodeHotValue);
return pathlist;
}
k_short_path在类中的定义如下:
K_Shortest_Path *k_short_path;
只是做了如上定义,没有进行实例化:
k_short_path = new K_Shortest_Path();
在类 K_Shortest_Path中的构造函数中加上此实例化的代码,系统每次运行的时候就不会崩溃了。