因此，我正在尝试在C ++中实现D star lite。该代码没有实现其含义，我相信G和RHS的更新没有以正确的方式进行。
我试图通过用范围为2的for循环替换主循环中的while循环来进行进一步探究，并注意到updateVertex（）方法中RHS和G值中的杂散-2.14748e + 09并非如此达到所有期望值。请帮助我解决这个奇怪的问题。
#include <vector>
#include <iostream>
#include <algorithm>
#include <string>
#include <random>
#include <limits>
#include <cstdlib>
#include <utility>
#include <queue>
class Node {
public:
/**
* @var _x of type int stores the x coordinate of the node
*/
int _x = 0;
/**
* @var _y of type int stores the y coordinate of the node
*/
int _y = 0;
/**
* @var _key of type pair<double,double> stores the key values of the node
*/
std::pair<double, double> _key = std::make_pair(0, 0);
/**
* default constructor
*/
Node() {
}
/**
* Constructor taking 2 parameters
* @param x of type int used to initialize the value of the member _x
* @param y of type int used to initialize the value of the member _y
*/
Node(int x, int y)
: _x(x),
_y(y) {
}
/**
* Constructor which takes a const Node reference as a parameter copies
* the values of n in a new Node object
* @param n is a reference of Node class object
*
*/
Node(const Node &n) {
_x = n._x;
_y = n._y;
_key = n._key;
}
/**
* @brief Sets the key value of the node
*
* @param k is a reference of a pair<double,double>
* @return void
*/
void setkey(std::pair<double, double> k) {
_key = k;
}
/**
* @brief Gets the value of the key
* @return the key value of type pair<double,double>
*/
std::pair<double, double> getkey() {
return _key;
}
/**
* @brief A friend to the class Node. Overloads the operator +,
* used to add two const Node obejcts
*
* @param r a reference of the Node class object
* @param l a reference of the Node class object
* @return a const Node created by adding the _x and _y values of the parameters
*/
const Node operator+(const Node &r) {
return Node(this->_x + r._x, this->_y + r._y);
}
/**
* @brief A friend to the class Node.Overloads the operator ==, used to check if
* the two const Node elements are the same
*
*
* @param r a reference of the const Node class object
* @param l a reference of the const Node class object
* @return a bool value, true if the elements are equal false if different
*/
bool operator==(const Node &r) {
return ((this->_x == r._x) && (this->_y == r._y));
}
/**
* @brief A friend to the class Node.Overloads the operator !=, used to check if
* the two const Node elements are different
*
* @param r a reference of the const Node class object
* @param l a reference of the const Node class object
* @return a bool value, true if the elements are not equal false if equal
*/
bool operator!=(const Node &r) {
return !(*this == r);
}
/**
* @brief A friend to the class Node.Overloads the operator <, used to compare the key value of the Node and key
*
* @param l a reference of the const Node class object
* @param r is a key of type pair<double,double>
* @return a bool value, true or false
*/
friend bool operator<(const Node& l, const std::pair<double, double> r) {
if (l._key.first < r.first) {
return true;
} else {
if ((l._key.first == r.first) && (l._key.second < r.second))
return true;
else
return false;
}
}
/**
* Destructor
*/
~Node() {
}
};
/**
* @brief Structure compare used as the comparator in the PriorityQ making it a min priority queue
*/
struct compare {
bool operator()(const Node &l, const Node &r) {
if (l._key.first < r._key.first) {
return false;
} else {
if ((l._key.first == r._key.first) && (l._key.second < r._key.second))
return false;
else
return true;
}
}
};
/**
* @brief Template Class inherits from priority queue
*/
template<typename T>
class PriorityQ : public std::priority_queue<T, std::vector<T>, compare> {
public:
/**
* @brief A method used to remove a particular value from the queue
*
* @param value a const reference to the template T
* @return bool true or false
*/
bool remove(const T& value) {
// finds if the desired value is present in the queue
auto it = std::find(this->c.begin(), this->c.end(), value);
if (it != this->c.end()) {
// used to erase the element
this->c.erase(it);
// reorders the queue
std::make_heap(this->c.begin(), this->c.end(), this->comp);
return true;
} else {
return false;
}
}
/**
* @brief A method to find if an element is present in the queue
*
* @param value a const reference to the template T
* @return the template T
*/
T find(const T& value) {
auto it = std::find(this->c.begin(), this->c.end(), value);
return *it;
}
};
class DSL {
private:
std::vector<std::vector<double>> g;
std::vector<std::vector<double>> rhs;
std::vector<std::vector<int>> cost;
std::vector<std::vector<int>> map;
int m, n;
float dropout = 0.9;
Node start;
Node goal;
int km = 0;
public:
PriorityQ<Node> U;
DSL(int rows, int columns, Node s, Node gol) {
m = rows;
n = columns;
start = s;
goal = gol;
g.resize(m,
std::vector<double>(n, std::numeric_limits<double>::infinity()));
rhs.resize(m,
std::vector<double>(n, std::numeric_limits<double>::infinity()));
cost.resize(m, std::vector<int>(n, 1));
//generateMap();
map = { {0, 0, 0, 0, 0, 0},
{ 0, 1, 0, 0, 0, 0},
{ 0, 1, 1, 0, 0, 0},
{ 0, 0, 1, 0, 0, 0},
{ 0, 1, 1, 0, 0, 0},
{ 0, 1, 1, 1, 0, 0}};
}
std::vector<std::vector<double>> getG() {
return g;
}
std::vector<std::vector<int>> getCost() {
return cost;
}
void setCost(Node node, double val) {
cost[node._x][node._y] = val;
}
void setMap(Node node, double val) {
map[node._x][node._y] = val;
}
void setkm(Node node) {
km += heuristic(node);
}
std::vector<std::vector<int>> getMap() {
return map;
}
std::vector<std::vector<int>> generateMap() {
map.resize(m, std::vector<int>(n, 0));
size_t size = m * n;
std::random_device rd;
std::mt19937 gen(rd());
std::bernoulli_distribution dist(1 - dropout); // bernoulli_distribution takes chance of true n constructor
std::vector<int> temp(size);
std::generate(temp.begin(), temp.end(), [&] {return dist(gen);});
int i = 0;
for (int x = 0; x < m; x++) {
for (int y = 0; y < n; y++) {
map[x][y] = temp[i];
i++;
}
}
return map;
}
void initialize() {
rhs[goal._x][goal._y] = 0;
auto key = calculateKey(goal);
goal.setkey(key);
U.push(goal);
}
std::pair<double, double> calculateKey(Node node) {
double minVal =
g[node._x][node._y] > rhs[node._x][node._y] ?
rhs[node._x][node._y] : g[node._x][node._y];
double key1 = minVal + heuristic(node) + km;
double key2 = minVal;
// make pair of key1 and key2
return std::make_pair(key1, key2);
}
double heuristic(Node node) {
return (abs(start._x - node._x) + abs(start._y - node._y));
}
std::vector<Node> getNeighbours(Node node) {
std::vector < Node > neighbours;
if (node._x - 1 >= 0) {
neighbours.push_back(Node((node._x - 1),node._y));
}
if (node._x + 1 < m) {
neighbours.push_back(Node((node._x + 1),node._y));
}
if (node._y - 1 >= 0) {
neighbours.push_back(Node(node._x ,(node._y - 1)));
}
if (node._y + 1 < n) {
neighbours.push_back(Node((node._x),(node._y + 1)));
}
return neighbours;
}
void updateVertex(Node node) {
std::vector < Node > neighbours;
if (node != goal) {
neighbours = getNeighbours(node);
std::vector<double> values;
for (auto neighbour : neighbours) {
values.push_back(
g[neighbour._x][neighbour._y]
+ cost[neighbour._x][neighbour._y]);
}
// for(auto val:values){
// std::cout<<val<<"\t";
// }
rhs[node._x][node._y] = *std::min_element(values.begin(),
values.end());
}
if (node == U.find(node)) {
U.remove(node); // if found removes the node u
}
if (g[node._y][node._x] != rhs[node._y][node._x]) {
auto key = calculateKey(node); // calculates the key of u
node.setkey(key);// sets the key of u
U.push(node);// pushes u to the priority queue
}
}
void computeShortestPath() {
while (U.top() < calculateKey(start)
|| (rhs[start._x][start._y] != g[start._x][start._y])) {
Node u = U.top(); // top Node
U.pop();// remove Node
if (u < calculateKey(u)) {
std::pair<double, double> knew = calculateKey(u);
u.setkey(knew);
U.push(u);
} else if (g[u._x][u._y] > rhs[u._x][u._y]) {
// sets the g value of the node equal to it's rhs value
g[u._x][u._y] = rhs[u._x][u._y];
std::vector < Node > neighbours = getNeighbours(u);
for (auto &n : neighbours) {
if (cost[n._x][n._y] == 1)
// updates the inconsitent nodes
updateVertex(n);
}
} else {
// sets the g value to "infinity"
g[u._x][u._y] = std::numeric_limits<double>::infinity();
updateVertex(u);
std::vector < Node > neighbours = getNeighbours(u);
for (auto &n : neighbours) {
// checks the cost of the node
if (cost[n._x][n._y] == 1)
// updates the neighbours of the node u
updateVertex(n);
}
}
//printG();
//printRhs();
}
}
void printMap() {
for (size_t i = 0; i < map.size(); i++)
{
for (size_t j = 0; j < map[i].size(); j++)
{
std::cout << map[i][j];
}
std::cout<<std::endl;
}
}
void printG() {
std::cout<<"printing G"<<std::endl;
for (size_t i = 0; i < g.size(); i++)
{
for (size_t j = 0; j < g[i].size(); j++)
{
std::cout << g[i][j]<<" ";
}
std::cout<<std::endl;
}
}
void printRhs() {
std::cout<<"printing RHS"<<std::endl;
for (size_t i = 0; i < rhs.size(); i++)
{
for (size_t j = 0; j < rhs[i].size(); j++)
{
std::cout << rhs[i][j]<< " ";
}
std::cout<<std::endl;
}
}
};
int main() {
std::vector<std::pair<double, double>> path;
int m, n;
m = 6;
n = 6;
Node start(0, 0); // sets the start Node
Node goal(5, 4); // sets the goal Node
DSL dsl(m, n, start, goal);
std::cout << "Initial map" << std::endl;
dsl.printMap();
Node last = start;
dsl.initialize();
dsl.computeShortestPath();
Node current;
//while (start != goal) {
for( auto i = 0 ; i < 2; i++){
if (dsl.getG()[start._x][start._y]
== std::numeric_limits<double>::infinity()) {
std::cout << "There's no solution";
break;
}
std::vector<Node> neighbours1 = dsl.getNeighbours(start);
std::vector<double> costRange; // used to store the costs of neighbours
for (auto n : neighbours1) {
costRange.push_back(dsl.getG()[n._x][n._y] + dsl.getCost()[n._x][n._y]);
}
size_t index = 0;
for (size_t i = 0; i < costRange.size(); i++) {
if (costRange[index] > costRange[i])
// index of costRange with minimum value
index = i;
}
// argmin of the neighbours of start
Node nextStep = neighbours1[index];
if (!dsl.getMap()[nextStep._x][nextStep._y]) {
start = nextStep;
current = start;
} else {
dsl.setkm(last);
last = start;
// set the value of nextStep to infinity
dsl.setCost(nextStep, std::numeric_limits<double>::infinity());
std::vector<Node> neighbours2 = dsl.getNeighbours(nextStep);
for (auto n : neighbours2) {
dsl.updateVertex(n);
}
dsl.computeShortestPath();
}
//
// path.push_back(std::make_pair(current._x, current._y));
// dsl.setMap(current,8);
// std::cout<<"Current position"<<std::endl;
// dsl.printMap();
//break;
}
//std::cout<<"Final map"<<std::endl;
//dsl.printMap();
return 0;
}
此外，由于未按预期进行更新，computeShortestPath（）中的while循环不会以某种方式终止。 请用我的代码帮助我，任何帮助将不胜感激。非常感谢您阅读我的文章。请在编译时使用c ++ 11，可以使用来完成 g ++ -std = c ++ 11 test.cpp -o测试 其次是 ./test
else if (g[u._x][u._y] > rhs[u._x][u._y]) {
// sets the g value of the node equal to it's rhs value
g[u._x][u._y] = rhs[u._x][u._y];
std::vector < Node > neighbours = getNeighbours(u);
for (auto &n : neighbours) {
if (cost[n._x][n._y] == 1)
// updates the inconsitent nodes
updateVertex(n);
}
更新：我相信上面的代码块是代码出现错误的地方