I'm working on a D* Lite path planner in C . The program maintains a priority queue of cells (U), each cell have two cost values, and a key can be calculated for a cell which determine it's order on the priority queue.

using Cost = float;
using HeapKey = pair<Cost, Cost>;
using KeyCompare = std::greater<std::pair<HeapKey, unsigned int>>;
vector<pair<HeapKey, unsigned int>> U; 

When a cell is added it is done so by using:

U.push_back({ k, id });
push_heap(U.begin(), U.end(), KeyCompare());

As part of the path planning algorithm cells sometimes need to be removed, and here lies the current problem as far as I can see. I recently had help on this site to speed my program up quite a bit by using push_heap instead of make_heap, but now it seems that the part of the program that removes cells is the slowest part. Cells are removed from the priority queue by:

void DstarPlanner::updateVertex(unsigned int id) {
   ...
   ...
   auto it = find_if(U.begin(), U.end(), [=](auto p) { return p.second == id; });
   U.erase(it);
   ...
   ...
}

From my tests this seems to take roughly 80% of the time my program use for path planning. It was my hope coming here that a more time-saving method existed.

Thank you.

EDIT - Extra information.

void DstarPlanner::insertHeap(unsigned int id, HeapKey k) {
    U.push_back({ k, id });
    push_heap(U.begin(), U.end(), KeyCompare());
    in_U[id]  ;
  }

void DstarPlanner::updateVertex(unsigned int id) {
    Cell* u = graph.getCell(id);

    if (u->id != id_goal) {
      Cost mincost = infinity;
      for (auto s : u->neighbors) {
        mincost = min(mincost, graph.getEdgeCost(u->id, s->id)   s->g);
      }
      u->rhs = mincost;
    }
    if (in_U[id]) {
      auto it = find_if(U.begin(), U.end(), [=](auto p) { return p.second == id; });
      U.erase(it);
      in_U[id]--;
    } 
    if (u->g != u->rhs) {
      insertHeap(id, u->calculateKey());
    }
  }

vector<int> DstarPlanner::ComputeShortestPath() {

    vector<int> bestPath;
    vector<int> emptyPath;
    Cell* n = graph.getCell(id_start);    

    while (U.front().first < n->calculateKey() || n->rhs != n->g) {

      auto uid = U.front().second;
      Cell* u = graph.getCell(uid);
      auto kold = U.front().first;
      pop_heap(U.begin(), U.end(), KeyCompare());
      U.pop_back();
      in_U[u->id]--;

      if (kold < u->calculateKey()) {
        insertHeap(u->id, u->calculateKey());
      } else if (u->g > u->rhs) {
        u->g = u->rhs; 
        for (auto s : u->neighbors) {
          if (!occupied(s->id)) {
            updateVertex(s->id);
          }
        }
      } else {
        u->g = infinity;
        for (auto s : u->neighbors) {
          if (!occupied(s->id)) {
            updateVertex(s->id);
          }
        }
        updateVertex(u->id);
      }
    }

    bestPath=constructPath();
    return bestPath;
  }

CodePudding user response：

find_if does a linear search. It maybe faster to use:

• std::map/std::set -> Standard binary search tree implementations
• std::unordered_map/std::unordered_set -> Standard hash table implementations

These may use a lot of memory if your elements (key-value pairs) are small integers. To avoid that you can use 3rd party alternatives like boost::unordered_flat_map.

CodePudding user response：

How do you re-heapify after U.erase(it)? Do you ever delete multiple nodes at once?

If deletions need to be atomic between searches, then you can

1. swap it with end() - 1,
2. erase end() - 1, and
3. re-heapify.

Erasing end() - 1 is O(1) while erasing it is linear in std::distance(it, end).

void DstarPlanner::updateVertex(unsigned int id) {
   ...
   // take the id by reference since this is synchronous
   auto it = find_if(U.begin(), U.end(), [&](const auto& p) { return p.second == id; });
   *it = std::move(*(U.end() - 1));
   U.erase((U.end() - 1));
   std::make_heap(U.begin(), U.end()); // expensive!!! 3*distance(begin, end)
   ...
}

If you can delete multiple nodes between searches, then you can use a combination of erase remove_if to only perform one mass re-heapify. This is important be heapify is expensive.

1. it = remove_if(begin, end, [](){ lambda }
2. erase(it, end)
3. re-heapify

void DstarPlanner::updateVertex(const std::vector<unsigned int>& sorted_ids) {
   ...
   auto it = remove_if(U.begin(), U.end(), [&](const auto& p) { return std::binary_search(ids.begin(), ids.end(), p.second); });
   U.erase(it, U.end());
   std::make_heap(U.begin(), U.end()); // expensive!!! 3*distance(begin, end)
   ...
}

Doing better

You can possibly improve on this by replacing std::make_heap (which makes no assumptions about the heapiness of [begin(), end()) with a custom method that re-heapifies a former heap around "poison points" -- it only needs to initially inspect the elements around the elements that were swapped. This sounds like a pain to write and I'd only do it if the resulting program was still too slow.

Have you thought of...

Just not even removing elements from the heap? The fact you're using a heap tells me that the algorithm designers suggested a heap. If they suggested a heap, then they likely didn't envision random removals. This is speculation on my part. I'm otherwise not familiar with D* lite.