Fix backward dijkstra optimization

[hubacji1/rrts.git] / src / rrtext13.cc

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 5545c95f40ad84aa466a4b052818f4d572730401..f6dd77473d014baa673fe3042ad1ab4222bdd634 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -1,3 +1,4 @@
+#include <algorithm>
 #include <queue>
 #include "rrtext.hh"
 
@@ -24,8 +25,15 @@ 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::pick_interesting()
+RRTExt13::interesting_forward()
 {
        this->dn_.clear();
        this->dn_.reserve(this->path_.size());
@@ -62,6 +70,50 @@ RRTExt13::pick_interesting()
 #endif
 }
 
+void
+RRTExt13::interesting_backward()
+{
+       this->dn_.clear();
+       this->dn_.reserve(this->path_.size());
+#if 0 // all path poses are interesting
+       for (auto n: this->opath_) {
+               this->dn_.push_back(DijkstraNode(n));
+               this->dn_.back().i = this->dn_.size() - 1;
+       }
+       std::reverse(this->dn_.begin(), this->dn_.end());
+       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);
+               for (unsigned int j = i - 1; j > 0; j--) {
+                       RRTNode& nj = *this->opath_[j];
+                       RRTNode& njl = *this->opath_[j + 1];
+                       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
+}
+
 void
 RRTExt13::dijkstra_forward()
 {
@@ -73,19 +125,25 @@ RRTExt13::dijkstra_forward()
                DijkstraNode fd = pq.top();
                RRTNode& f = *fd.node;
                pq.pop();
+               this->bc_.set_pose(f);
                for (unsigned int i = fd.i + 1; i < this->dn_.size(); i++) {
                        RRTNode& t = *this->dn_[i].node;
+                       if (!this->bc_.drivable(t)) {
+                               continue;
+                       }
                        double cost = f.cc() + 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_;
+                       bool tn = this->bc_.edist(t) < 2.0 * this->eta_;
                        if (cost < t.cc() && td && tn) {
                                this->join_steered(&f);
                                t.p(this->nodes_.back());
@@ -103,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]);
                                }
@@ -156,14 +223,19 @@ RRTExt13::compute_path()
        if (measure_time) {
                this->otime_ = -this->ter_.scnt();
        }
-       this->pick_interesting();
-       this->dijkstra_forward();
-#if 0 // TODO Fix as the code does not always finish.
-       RRTS::compute_path();
-       this->pick_interesting();
-       this->dijkstra_backward();
-#endif
-       RRTS::compute_path();
+       double curr_cc = this->goal_.cc();
+       double last_cc = curr_cc + 1.0;
+       while (curr_cc < last_cc) {
+               last_cc = curr_cc;
+               RRTS::compute_path();
+               this->interesting_forward();
+               this->dijkstra_forward();
+               RRTS::compute_path();
+               this->interesting_backward();
+               this->dijkstra_backward();
+               RRTS::compute_path();
+               curr_cc = this->goal_.cc();
+       }
        if (measure_time) {
                this->otime_ += this->ter_.scnt();
        }