Fix backward dijkstra optimization

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 038677f7638132fceb34d64216274bd34d410dd3..6842312a2a32a31338d47fbd66b172ac4659d39a 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -104,6 +104,7 @@ private:
                RRTNode* node = nullptr;
                unsigned int i = 0;
                bool v = false;
+               double d = 0.0;
                bool vi();
                DijkstraNode(RRTNode* n);
        };
@@ -111,6 +112,10 @@ private:
        public:
                int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
        };
+       class DijkstraNodeBackwardComparator {
+       public:
+               int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
+       };
        std::vector<RRTNode*> opath_;
        double ogoal_cc_ = 0.0;
        double otime_ = 0.0;

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index dada6a375ea1416c1c26f8a6d251af31ce22b518..f6dd77473d014baa673fe3042ad1ab4222bdd634 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -25,6 +25,13 @@ RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
        return n1.node->cc() > n2.node->cc();
 }
 
+int
+RRTExt13::DijkstraNodeBackwardComparator::operator() (DijkstraNode const& n1,
+               DijkstraNode const& n2)
+{
+       return n1.d > n2.d;
+}
+
 void
 RRTExt13::interesting_forward()
 {
@@ -77,8 +84,10 @@ RRTExt13::interesting_backward()
        return;
 #endif
 #if 1 // cusp and 1st non-drivable path poses are interesting
+       double goal_cc = this->goal_.cc();
        this->dn_.push_back(DijkstraNode(this->opath_.back()));
        this->dn_.back().i = this->dn_.size() - 1;
+       this->dn_.back().d = goal_cc - this->opath_.back()->cc();
        for (unsigned int i = this->opath_.size() - 1; i > 1; i--) {
                RRTNode& ni = *this->opath_[i];
                this->bc_.set_pose(ni);
@@ -88,17 +97,20 @@ RRTExt13::interesting_backward()
                        if (!this->bc_.drivable(nj)) {
                                this->dn_.push_back(DijkstraNode(&njl));
                                this->dn_.back().i = this->dn_.size() - 1;
+                               this->dn_.back().d = goal_cc - njl.cc();
                                i = j;
                                break;
                        }
                        if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
                                this->dn_.push_back(DijkstraNode(&njl));
                                this->dn_.back().i = this->dn_.size() - 1;
+                               this->dn_.back().d = goal_cc - njl.cc();
                        }
                }
        }
        this->dn_.push_back(DijkstraNode(this->opath_.front()));
        this->dn_.back().i = this->dn_.size() - 1;
+       this->dn_.back().d = goal_cc - this->opath_.front()->cc();
 #endif
 }
 
@@ -149,29 +161,38 @@ RRTExt13::dijkstra_backward()
 {
        std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
                DijkstraNodeComparator> pq;
-       this->dn_.back().vi();
-       pq.push(this->dn_.back());
+       this->dn_.front().vi();
+       pq.push(this->dn_.front());
        while (!pq.empty()) {
                DijkstraNode td = pq.top();
                RRTNode& t = *td.node;
                pq.pop();
-               for (unsigned int i = td.i - 1; i > 0; i--) {
+               this->bc_.set_pose(t);
+               for (unsigned int i = td.i; i > 0; i--) {
+                       DijkstraNode& fd = this->dn_[i];
                        RRTNode& f = *this->dn_[i].node;
-                       double cost = f.cc() + this->cost_build(f, t);
+                       if (!this->bc_.drivable(f)) {
+                               continue;
+                       }
+                       double cost = td.d + this->cost_build(f, t);
                        this->steer(f, t);
                        if (this->steered_.size() == 0) {
                                break;
                        }
-                       if (this->collide_steered()) {
+                       unsigned int ss = this->steered_.size();
+                       if (this->collide_steered()
+                                       || ss != this->steered_.size()) {
                                continue;
                        }
                        this->bc_.set_pose(this->steered_.back());
                        bool td = this->bc_.drivable(t);
-                       bool tn = this->bc_.edist(t) < this->eta_;
-                       if (cost < t.cc() && td && tn) {
+                       bool tn = this->bc_.edist(t) < 2.0 * this->eta_;
+                       if (cost < fd.d && td && tn) {
+                               fd.d = cost;
                                this->join_steered(&f);
                                t.p(this->nodes_.back());
                                t.c(this->cost_build(this->nodes_.back(), t));
+                               this->recompute_path_cc();
                                if (!this->dn_[i].vi()) {
                                        pq.push(this->dn_[i]);
                                }