Я пытаюсь реализовать алгоритм RRT , используя kdtree в c ++ 14. Я реализовал связанные методы, которые работают, когда я запускаю их по отдельности, за исключением метода planner ().
Iпопытался отладить код, но тщетно. Там нет ошибок, по крайней мере, когда я компилирую код. Я полагаю, что проблема заключается в функции ниже:
std::vector<std::pair<double,double>> planner()
{
std::vector<std::pair<double,double>> plan;
std::unique_ptr<Node> root = std::make_unique<Node>(start_, nullptr);
std::unique_ptr<kdTree> tree = std::make_unique<kdTree>(start_, 0, nullptr, nullptr, std::move(root));
std::pair<double, double> current = start_;
while (!check_goal(current) && (max_iterations_)) //checks if the current point is in the goal region or if the max iterations are zero
{
std::pair<double, double> random_ = random_point();
NNS res = tree->search(random_, nullptr);
std::pair<double, double> nearest = res.point_;
std::pair<double, double> new_point = move_to(nearest, random_);
std::unique_ptr<Node> temp = std::make_unique<Node>(new_point, std::move(res.node_));
tree->insert(new_point, std::move(temp));
current = new_point;
max_iterations_--;
}
NNS res = tree->search(current, nullptr);
std::unique_ptr<Node> node = std::move(res.node_);
while (node->parent_){
plan.push_back(node->point_); // supposed to give the path from the goal(the point within the goal region) to the start
node = std::move(node->parent_);
}
return plan;
}
Я подозреваю, что назначение родительского указателя не происходит должным образом, особенно в этих двух строках, где я создаю узел, назначающий его точечный атрибут и родительский узел, который являетсязатем вставляется в kdtree.
std::unique_ptr<Node> temp = std::make_unique<Node>(new_point, std::move(res.node_));
tree->insert(new_point, std::move(temp));
Я не уверен, как умный указатель управляет памятью, связанной с узлом выше.
Пожалуйста, найдите полный код ниже:
#include <iostream>
#include <vector>
#include <memory>
#include <limits>
#include <utility>
#include <cmath>
#include <ctime>
#include <random>
#include <string>
#include <algorithm>
double epsilon_ = 5; //step size
double goal_radius_ = 2; //goal radius
std::pair<double, double> start_ = std::make_pair(2, 2); //start point
std::pair<double, double> goal_ = std::make_pair(-48, -89); //goal point
int max_iterations_ = 100000; // max number of iterations
/**
* @brief A node used to store the point and it's parent
*
*/
struct Node
{
std::pair<double, double> point_ = std::make_pair(std::nan("1"), std::nan("1"));
std::unique_ptr<Node> parent_ = nullptr;
Node(std::pair<double, double> point, std::unique_ptr<Node> parent) : point_(point) { parent_ = std::move(parent); }
};
/**
* @brief A structure used as the return value in kdtree search method.
* stores the minimum distance point, distance value, node
*
*/
struct NNS
{
std::pair<double, double> point_;
double dist_;
std::unique_ptr<Node> node_;
NNS(std::pair<double, double> point, double dist, std::unique_ptr<Node> node);
NNS();
};
/**
* @brief implementation of the kd tree class
*
*/
class kdTree
{
public:
std::pair<double, double> point_;
int dim_, axis_;
std::unique_ptr<Node> node_;
std::unique_ptr<kdTree> left_, right_;
kdTree(std::pair<double, double> point, int axis, std::unique_ptr<kdTree> left, std::unique_ptr<kdTree> right, std::unique_ptr<Node> node);
void insert(std::pair<double, double> &point, std::unique_ptr<Node> node);
inline double distance(std::pair<double, double> point) { return sqrt((this->point_.first - point.first) * (this->point_.first - point.first) + (this->point_.second - point.second) * (this->point_.second - point.second)); }
void print_tree();
NNS search(std::pair<double, double> &point, std::unique_ptr<Node> node, std::pair<double, double> ref_point = std::make_pair(std::nan("1"), std::nan("1")), double distance = std::numeric_limits<double>::infinity());
};
/**
* @brief Construct a new NNS::NNS object
*
*/
NNS::NNS() { ; }
/**
* @brief Construct a new NNS::NNS object
*
* @param point
* @param dist
* @param node
*/
NNS::NNS(std::pair<double, double> point, double dist, std::unique_ptr<Node> node)
{
this->point_ = point;
this->dist_ = dist;
this->node_ = std::move(node);
}
/**
* @brief Construct a new kd Tree::kd Tree object
*
* @param point
* @param axis
* @param left
* @param right
* @param node
*/
kdTree::kdTree(std::pair<double, double> point, int axis, std::unique_ptr<kdTree> left, std::unique_ptr<kdTree> right, std::unique_ptr<Node> node)
{
this->point_ = point;
this->axis_ = axis;
this->left_ = std::move(left);
this->right_ = std::move(right);
this->node_ = std::move(node);
}
/**
* @brief inserts a point,node in the kdtree
*
* @param point
* @param node
*/
void kdTree::insert(std::pair<double, double> &point, std::unique_ptr<Node> node)
{
if (!this->axis_)
{
if (this->point_.first > point.first)
{
if (!this->left_)
{
this->left_ = std::make_unique<kdTree>(point, (this->axis_ + 1) % 2, nullptr, nullptr, std::move(node));
}
else
{
this->left_->insert(point, std::move(node));
}
}
else
{
if (!this->right_)
{
this->right_ = std::make_unique<kdTree>(point, (this->axis_ + 1) % 2, nullptr, nullptr, std::move(node));
}
else
{
this->right_->insert(point, std::move(node));
}
}
}
else
{
if (this->point_.second > point.second)
{
if (!this->left_)
{
this->left_ = std::make_unique<kdTree>(point, (this->axis_ + 1) % 2, nullptr, nullptr, std::move(node));
}
else
{
this->left_->insert(point, std::move(node));
}
}
else
{
if (!this->right_)
{
this->right_ = std::make_unique<kdTree>(point, (this->axis_ + 1) % 2, nullptr, nullptr, std::move(node));
}
else
{
this->right_->insert(point, std::move(node));
}
}
}
}
/**
* @brief method to print the tree
*
*/
void kdTree::print_tree()
{
if (this->left_)
{
this->left_->print_tree();
}
std::cout << this->point_.first << "\t" << this->point_.second << std::endl;
if (this->right_)
{
this->right_->print_tree();
}
}
/**
* @brief searches for the point in the kdtree which is at a minimum euclidean distance from the entered point
*
* @param point
* @param node
* @param ref_point
* @param dist
* @return NNS the structure used for recursive nearest neighbor search
*/
NNS kdTree::search(std::pair<double, double> &point, std::unique_ptr<Node> node, std::pair<double, double> ref_point, double dist)
{
if (!this->left_ && !this->right_)
{
double w = this->distance(point);
if (w < dist)
{
return NNS(this->point_, w, std::move(this->node_));
}
else
{
return NNS(ref_point, dist, std::move(node));
}
}
else
{
double d = this->distance(point);
if (d < dist)
{
dist = d;
ref_point = this->point_;
node = std::move(this->node_);
}
if (!this->axis_)
{
if (point.first <= this->point_.first)
{
if (point.first - dist <= this->point_.first)
{
if (this->left_)
{
NNS res = this->left_->search(point, std::move(node), ref_point, dist);
ref_point = res.point_;
node = std::move(res.node_);
dist = res.dist_;
}
}
if (point.first + dist > this->point_.first)
{
if (this->right_)
{
return this->right_->search(point, std::move(node), ref_point, dist);
}
}
}
else
{
if (point.first + dist > this->point_.first)
{
if (this->right_)
{
NNS res = this->right_->search(point, std::move(node), ref_point, dist);
ref_point = res.point_;
node = std::move(res.node_);
dist = res.dist_;
}
}
if (point.first - dist <= this->point_.first)
{
if (this->left_)
{
return this->left_->search(point, std::move(node), ref_point, dist);
}
}
}
}
else
{
if (point.second <= this->point_.second)
{
if (point.second - dist <= this->point_.second)
{
if (this->left_)
{
NNS res = this->left_->search(point, std::move(node), ref_point, dist);
ref_point = res.point_;
node = std::move(res.node_);
dist = res.dist_;
}
}
if (point.second + dist > this->point_.second)
{
if (this->right_)
{
return this->right_->search(point, std::move(node), ref_point, dist);
}
}
}
else
{
if (point.second + dist > this->point_.second)
{
if (this->right_)
{
NNS res = this->right_->search(point, std::move(node), ref_point, dist);
ref_point = res.point_;
node = std::move(res.node_);
dist = res.dist_;
}
}
if (point.second - dist <= this->point_.second)
{
if (this->left_)
{
return this->left_->search(point, std::move(node), ref_point, dist);
}
}
}
}
return NNS(ref_point, dist, std::move(node));
}
}
/**
* @brief generates a random point
*
* @return std::pair<double, double> the random point
*/
std::pair<double, double> random_point()
{
srand(time(NULL));
std::random_device rd;
std::mt19937 rand_x(rd());
std::mt19937 rand_y(rd());
std::uniform_int_distribution<> dist_x(-100.0, 100.0);
std::uniform_int_distribution<> dist_y(-100.0, 100.0);
return std::make_pair(dist_x(rand_x), dist_y(rand_y));
}
/**
* @brief calculates the euclidean distance
*
* @param point1
* @param point2
* @return double the euclidean distance
*/
inline double distance(std::pair<double, double> point1, std::pair<double, double> point2) { return sqrt((point1.first - point2.first) * (point1.first - point2.first) + (point1.second - point2.second) * (point1.second - point2.second)); }
/**
* @brief returns a point at an epsilon distance in the direction of the second point
*
* @param from_
* @param towards_
* @return std::pair<double, double>
*/
std::pair<double, double> move_to(std::pair<double, double> from_, std::pair<double, double> towards_)
{
double angle = atan2(towards_.second - from_.second,
towards_.first - from_.first);
std::pair<double, double> next_point = std::make_pair((from_.first + epsilon_ * cos(angle)), (from_.second + epsilon_ * sin(angle)));
return next_point;
}
/**
* @brief checks if the point is within the radius of the goal
*
* @param current
* @return true
* @return false
*/
bool check_goal(std::pair<double, double> current)
{
if (distance(current, goal_) <= goal_radius_)
{
return true;
}
return false;
}
/**
* @brief RRT planning algorithm
*
* @return std::vector<std::pair<double,double>>
*/
std::vector<std::pair<double, double>> planner()
{
std::vector<std::pair<double, double>> plan;
std::unique_ptr<Node> root = std::make_unique<Node>(start_, nullptr);
std::unique_ptr<kdTree> tree = std::make_unique<kdTree>(start_, 0, nullptr, nullptr, std::move(root));
std::pair<double, double> current = start_;
while (!check_goal(current) && (max_iterations_)) //checks if the current point is in the goal region or if the max iterations are zero
{
std::pair<double, double> random_ = random_point();
NNS res = tree->search(random_, nullptr);
std::pair<double, double> nearest = res.point_;
std::pair<double, double> new_point = move_to(nearest, random_);
std::unique_ptr<Node> temp = std::make_unique<Node>(new_point, std::move(res.node_));
tree->insert(new_point, std::move(temp));
current = new_point;
max_iterations_--;
}
NNS res = tree->search(current, nullptr);
std::unique_ptr<Node> node = std::move(res.node_);
while (node->parent_)
{
plan.push_back(node->point_); // supposed to give the path from the goal(the point within the goal region) to the start
node = std::move(node->parent_);
}
return plan;
}
/**
* @brief the driver method
*
* @return int
*/
int main()
{
std::vector<std::pair<double, double>> plan = planner();
std::cout << plan.size() << " size \n";
for (auto p : plan)
{
std::cout << p.first << "\t" << p.second << std::endl;
}
return 0;
}
Я не вижу никаких ошибок даже после выполнения программы, но размер пути, который должен содержать пары двойников, либоноль или одна цифра, которая не проходит весь путь от цели до начала. Буду признателен за любую помощь с кодом, и я буду вечно благодарен за ваше время и усилия. Любые улучшения в коде приветствуются.