【发布时间】:2020-11-19 21:05:43
【问题描述】:
我实现了一个基于 D*-Lite 的路径规划算法。当我不开启优化(-O0)时,程序可以正常运行。但是当我打开优化级别(-O1/2/3)时,程序无法终止。在Visual Studio中,调试模式和发布模式都可以正常运行。以上情况,代码都是相同的。不知道怎么找问题,谁能帮帮我?
class DstarLite {
public:
DstarLite() = delete;
DstarLite(GridStatus* a, GridStatus* b, FILE* fp)
: k_m_(0), start_(a), last_(start_), goal_(b), open_close_(fp) {}
void calculateKey(GridStatus* s);
void updateVertex(GridStatus* u);
void initialize();
void computeShortestPath();
void rePlanning(vector<pair<GridStatus*, int>>& node_change);
GridStatus* getStart();
void setStart(GridStatus* val);
GridStatus* getGoal();
private:
Fib frontier_;
double k_m_;
unordered_map<GridStatus*, handle_t>
heap_map_;
GridStatus* start_;
GridStatus* last_;
GridStatus* goal_;
FILE* open_close_;
};
void DstarLite::calculateKey(GridStatus* s) {
s->f = min(s->g, s->rhs) + heuristic(start_, s) + k_m_;
s->k2 = min(s->g, s->rhs);
}
void DstarLite::initialize() {
fprintf(open_close_, "%d %d\n", start_->x, start_->y);
fprintf(open_close_, "%d %d\n", goal_->x, goal_->y);
goal_->rhs = 0;
calculateKey(goal_);
handle_t hand = frontier_.push(goal_);
heap_map_[goal_] = hand;
}
void DstarLite::updateVertex(GridStatus* u) {
bool heap_in = heap_map_.find(u) != heap_map_.end();
if (u->g != u->rhs && heap_in) {
calculateKey(u);
frontier_.update(heap_map_[u]);
} else if (u->g != u->rhs && !heap_in) {
calculateKey(u);
handle_t hand = frontier_.push(u);
heap_map_[u] = hand;
} else if (u->g == u->rhs && heap_in) {
calculateKey(u);
frontier_.erase(heap_map_[u]);
heap_map_.erase(u);
}
}
void DstarLite::computeShortestPath() {
int count = 0;
while (smaller(frontier_.top(), start_) || !myEqual(start_->rhs, start_->g)) {
count++;
auto u = frontier_.top();
pair<double, double> k_old = {u->f, u->k2};
pair<double, double> k_new;
k_new.first = min(u->g, u->rhs) + heuristic(start_, u) + k_m_;
k_new.second = min(u->g, u->rhs);
if (k_old < k_new) {
calculateKey(u);
frontier_.update(heap_map_[u]);
} else if (myGreater(u->g, u->rhs)) {
u->g = u->rhs;
frontier_.pop();
heap_map_.erase(u);
for (auto s : neighbors(u)) {
if (s->rhs > u->g + cost(u, s)) {
s->next = u;
s->rhs = u->g + cost(u, s);
updateVertex(s);
}
}
} else {
double g_old = u->g;
u->g = kDoubleInfinity;
auto neighbor = neighbors(u);
neighbor.push_back(u);
for (auto s : neighbor) {
if (myEqual(s->rhs, cost(s, u) + g_old)) {
if (!equal(s, goal_)) {
double pp_s = kDoubleInfinity;
for (auto succ : neighbors(s)) {
double dis = succ->g + cost(succ, s);
if (dis < pp_s) {
pp_s = dis;
s->next = succ;
}
}
s->rhs = pp_s;
}
}
updateVertex(s);
}
}
}
cout << "Dstar visited nodes : " << count << endl;
}
void DstarLite::rePlanning(vector<pair<GridStatus*, int>>& node_change) {
k_m_ += heuristic(last_, start_);
last_ = start_;
for (auto change : node_change) {
GridStatus* u = change.first;
int old_threat = u->threat;
int new_threat = change.second;
double c_old;
double c_new;
u->threat = new_threat;
u->rhs += (new_threat - old_threat) * threat_factor;
updateVertex(u);
for (auto v : neighbors(u)) {
u->threat = old_threat;
c_old = cost(v, u);
u->threat = new_threat;
c_new = cost(v, u);
if (c_old > c_new) {
if (v != goal_) {
if (v->rhs > u->g + c_new) {
v->next = u;
v->rhs = u->g + c_new;
}
}
} else if (myEqual(v->rhs, c_old + u->g)) {
if (v != goal_) {
double pp_s = kDoubleInfinity;
for (auto pre : neighbors(v)) {
double dis = pre->g + cost(pre, v);
if (dis < pp_s) {
pp_s = dis;
v->next = pre;
}
}
v->rhs = pp_s;
}
}
updateVertex(v);
}
}
}
GridStatus* DstarLite::getStart() { return start_; }
void DstarLite::setStart(GridStatus* val) { start_ = val; }
GridStatus* DstarLite::getGoal() { return goal_; }
DstarLite dstar(start, goal, open_close);
dstar.initialize();
dstar.computeShortestPath();
对不起,我觉得很难在代码中定位问题,所以之前没有显示代码。现在我重新编辑了问题,但是代码很多,主要调用部分是computeShortest()。
【问题讨论】:
-
代码中很可能存在错误,您依赖未定义的行为 (UB)。由于编译器/优化器可能假设您的代码没有未定义的行为,因此它可能会破坏您的代码,例如通过完全优化产生 UB 的代码路径。一个经典的例子是
int x; ... if (x+1<x) { overflow (); }- 因为整数溢出是UB,整个if-block 可能会被删除。将代码减少到重现错误的最小部分并提供Minimal, Reproducible Example,以便我们帮助您调试。