Очередь с приоритетом

    typedef std::vector<TreeNode::TreeNodePtr> NodeVector;
    NodeVector nodes_;

    typedef std::priority_queue<
        TreeNode::TreeNodePtr,
        std::vector<TreeNode::TreeNodePtr>,
        std::function<bool(TreeNode::TreeNodePtr, TreeNode::TreeNodePtr)> > NodeQueue;
 
    NodeQueue queue(
        [](TreeNode::TreeNodePtr f, TreeNode::TreeNodePtr s) -> bool { return (f->distance_ > s->distance_); });
 
    for (size_t i = 0; i != nodes_.size(); ++i)
    {
        queue.push(nodePtr);
    }
 
    while (!queue.empty())
    {
        TreeNode::TreeNodePtr nodePtr = queue.top();
            if (nodeFrom->distance_ > nodeTo->distance_ + weight)
            {
                (*nodeFrom).distance_ = (*nodeTo).distance_ + weight;
                nodeFrom->parentPtr_ = nodeTo;
            }
 
        queue.pop(); <--- Invalide Heap!!
    }