├── C++ ├── Opencv.VC.db ├── Opencv.sln ├── Opencv │ ├── Images │ │ ├── img.bmp │ │ ├── img1.bmp │ │ ├── img2.bmp │ │ ├── img3.bmp │ │ ├── img4.bmp │ │ ├── img5.bmp │ │ ├── obstacle.png │ │ ├── obstacle1.png │ │ ├── test.jpg │ │ ├── test.png │ │ └── test1.jpg │ ├── Input │ │ └── obstacle.txt │ ├── KdTree.cpp │ ├── KdTree.h │ ├── Opencv.vcxproj │ ├── Opencv.vcxproj.filters │ ├── Opencv.vcxproj.user │ ├── Output │ │ ├── convert.cpp │ │ ├── convert.exe │ │ ├── convert.o │ │ ├── input1_type1_1.txt │ │ ├── input1_type1_2.txt │ │ ├── input1_type5_1.txt │ │ ├── input1_type5_2.txt │ │ ├── input2_type1_1.txt │ │ ├── input2_type1_2.txt │ │ ├── input2_type5_1.txt │ │ ├── input2_type5_2.txt │ │ ├── input3_type1_1.txt │ │ ├── input3_type1_2.txt │ │ ├── input3_type4_1.txt │ │ ├── input3_type4_2.txt │ │ ├── input3_type5_1.txt │ │ ├── input3_type5_2.txt │ │ ├── input4_type1_1.txt │ │ ├── input4_type1_2.txt │ │ ├── input4_type5_1.txt │ │ ├── input4_type5_2.txt │ │ ├── obstacle_type2_input1.txt │ │ ├── obstacle_type2_input2.txt │ │ ├── obstacle_type2_input3.txt │ │ ├── obstacle_type2_input4.txt │ │ ├── obstacle_type4_input3.txt │ │ ├── obstacle_type5_input1.txt │ │ └── obstacle_type5_input2.txt │ ├── RRT-star-Implementation.cpp │ ├── RRT-star-Implementation.h │ ├── RRT.cpp │ ├── RRT.h │ ├── Source.cpp │ ├── obstacle.png │ ├── test1.txt │ ├── test_opencv.cpp │ └── x64 │ │ └── Debug │ │ ├── KdTree.obj │ │ ├── Opencv.Build.CppClean.log │ │ ├── Opencv.log │ │ ├── Opencv.tlog │ │ ├── CL.command.1.tlog │ │ ├── CL.read.1.tlog │ │ ├── CL.write.1.tlog │ │ ├── Opencv.lastbuildstate │ │ ├── link.command.1.tlog │ │ ├── link.read.1.tlog │ │ └── link.write.1.tlog │ │ ├── RRT-star-Implementation.obj │ │ ├── RRT.obj │ │ ├── test_opencv.obj │ │ ├── vc140.idb │ │ └── vc140.pdb ├── README.md └── x64 │ └── Debug │ ├── Opencv.exe │ ├── Opencv.ilk │ └── Opencv.pdb ├── Goal_biased_RRT.py ├── Images ├── img.bmp ├── img1.bmp ├── img2.bmp ├── img3.bmp ├── obstacle.png └── obstacle1.png ├── README.md ├── RRTStar_Goal_Biased.py ├── RRTStar_Obstacles.py ├── RRT_KdTree.py ├── RRT_Scan.py ├── RRT_Scan_final.py ├── RRT_Star_Obstacles_final.py ├── RRT_bidirectional.py ├── RRT_with_obstacles.py ├── RRTstar_Scan.py ├── RRTstar_Scan1.py ├── input.txt ├── kdTree.py ├── rrt_brute_force.py ├── rrt_star_kdtree.py ├── rrtstar_brute_force.py └── stefankoegl_kdtree.py /C++/Opencv.VC.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv.VC.db -------------------------------------------------------------------------------- /C++/Opencv.sln: -------------------------------------------------------------------------------- 1 |  2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio 14 4 | VisualStudioVersion = 14.0.25420.1 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Opencv", "Opencv\Opencv.vcxproj", "{881F74BD-1044-4B65-AAD5-FEB7FABCCCF1}" 7 | EndProject 8 | Global 9 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 10 | Debug|x64 = Debug|x64 11 | Debug|x86 = Debug|x86 12 | Release|x64 = Release|x64 13 | Release|x86 = Release|x86 14 | EndGlobalSection 15 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 16 | {881F74BD-1044-4B65-AAD5-FEB7FABCCCF1}.Debug|x64.ActiveCfg = Debug|x64 17 | {881F74BD-1044-4B65-AAD5-FEB7FABCCCF1}.Debug|x64.Build.0 = Debug|x64 18 | {881F74BD-1044-4B65-AAD5-FEB7FABCCCF1}.Debug|x86.ActiveCfg = Debug|Win32 19 | {881F74BD-1044-4B65-AAD5-FEB7FABCCCF1}.Debug|x86.Build.0 = Debug|Win32 20 | {881F74BD-1044-4B65-AAD5-FEB7FABCCCF1}.Release|x64.ActiveCfg = Release|x64 21 | {881F74BD-1044-4B65-AAD5-FEB7FABCCCF1}.Release|x64.Build.0 = Release|x64 22 | {881F74BD-1044-4B65-AAD5-FEB7FABCCCF1}.Release|x86.ActiveCfg = Release|Win32 23 | {881F74BD-1044-4B65-AAD5-FEB7FABCCCF1}.Release|x86.Build.0 = Release|Win32 24 | EndGlobalSection 25 | GlobalSection(SolutionProperties) = preSolution 26 | HideSolutionNode = FALSE 27 | EndGlobalSection 28 | EndGlobal 29 | -------------------------------------------------------------------------------- /C++/Opencv/Images/img.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/Images/img.bmp -------------------------------------------------------------------------------- /C++/Opencv/Images/img1.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/Images/img1.bmp -------------------------------------------------------------------------------- /C++/Opencv/Images/img2.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/Images/img2.bmp -------------------------------------------------------------------------------- /C++/Opencv/Images/img3.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/Images/img3.bmp -------------------------------------------------------------------------------- /C++/Opencv/Images/img4.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/Images/img4.bmp -------------------------------------------------------------------------------- /C++/Opencv/Images/img5.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/Images/img5.bmp -------------------------------------------------------------------------------- /C++/Opencv/Images/obstacle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/Images/obstacle.png -------------------------------------------------------------------------------- /C++/Opencv/Images/obstacle1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/Images/obstacle1.png -------------------------------------------------------------------------------- /C++/Opencv/Images/test.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/Images/test.jpg -------------------------------------------------------------------------------- /C++/Opencv/Images/test.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/Images/test.png -------------------------------------------------------------------------------- /C++/Opencv/Images/test1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/Images/test1.jpg -------------------------------------------------------------------------------- /C++/Opencv/Input/obstacle.txt: -------------------------------------------------------------------------------- 1 | 55 530 1082 33 2 | 72 68 1212 578 3 | 24 355 586 295 4 | 586 295 1197 23 -------------------------------------------------------------------------------- /C++/Opencv/KdTree.cpp: -------------------------------------------------------------------------------- 1 | #include "KdTree.h" 2 | 3 | using namespace std; 4 | 5 | 6 | NNret::NNret(int &x, int &y, long double d, node *nde) { 7 | this->x = x; 8 | this->y = y; 9 | this->d = d; 10 | this->nde = nde; 11 | } 12 | 13 | NNret::NNret() { 14 | ; 15 | } 16 | 17 | KDTree::KDTree(int &x, int &y, int axis, KDTree* left, KDTree* right, node* nde) { 18 | this->x = x; 19 | this->y = y; 20 | this->axis = axis; 21 | this->left = left; 22 | this->right = right; 23 | this->nde = nde; 24 | } 25 | 26 | void KDTree::insert(int &x, int &y, node *nde) { 27 | int x1 = this->x; 28 | int y1 = this->y; 29 | int axis = this->axis; 30 | 31 | if (axis == 0) { 32 | if (x1 > x) { 33 | if (this->left == NULL) { 34 | this->left = new KDTree(x, y, (axis + 1) % 2, NULL, NULL, nde); 35 | } 36 | else { 37 | this->left->insert(x, y, nde); 38 | } 39 | } 40 | else { 41 | if (this->right == NULL) { 42 | this->right = new KDTree(x, y ,(axis + 1) % 2, NULL, NULL, nde); 43 | } 44 | else { 45 | this->right->insert(x, y, nde); 46 | } 47 | } 48 | } 49 | else { 50 | if (y1 > y) { 51 | if (this->left == NULL) { 52 | this->left = new KDTree(x, y, (axis + 1) % 2, NULL, NULL, nde); 53 | } 54 | else { 55 | this->left->insert(x, y, nde); 56 | } 57 | } 58 | else { 59 | if (this->right == NULL) { 60 | this->right = new KDTree(x, y, (axis + 1) % 2, NULL, NULL, nde); 61 | } 62 | else { 63 | this->right->insert(x, y, nde); 64 | } 65 | } 66 | } 67 | } 68 | 69 | void KDTree::printTree(void) { 70 | if (this->left != NULL) { 71 | this->left->printTree(); 72 | } 73 | cout << this->x << " " << this->y << endl; 74 | if (this->right != NULL) { 75 | this->right->printTree(); 76 | } 77 | } 78 | 79 | long double KDTree::dist(int &x, int &y) { 80 | long double ans = 0; 81 | ans = sqrt((this->x -x)*(this->x-x)*1.0 + (this->y-y)*(this->y-y)*1.0); 82 | return ans; 83 | } 84 | 85 | NNret KDTree::search(int &x, int &y,long double dist, int &refx, int &refy, node *refnde) { 86 | int axis = this->axis; 87 | if (this->left == NULL && this->right == NULL) 88 | { 89 | long double w = this->dist(x, y); 90 | if (w < dist) { 91 | NNret ret = NNret(this->x, this->y, w, this->nde); 92 | return ret; 93 | } 94 | else { 95 | NNret ret = NNret(refx, refy, dist, refnde); 96 | return ret; 97 | } 98 | } 99 | else { 100 | long double d = this->dist(x, y); 101 | if (d < dist) { 102 | dist = d; 103 | refx = this->x; 104 | refy = this->y; 105 | refnde = this->nde; 106 | } 107 | if (axis == 0) { 108 | if (x <= this->x) { 109 | if (x - dist <= this->x) { 110 | if (this->left != NULL) { 111 | NNret ret = this->left->search(x, y, dist, refx, refy, refnde); 112 | refx = ret.x; 113 | refy = ret.y; 114 | dist = ret.d; 115 | refnde = ret.nde; 116 | } 117 | } 118 | if (x + dist > this->x) { 119 | if (this->right != NULL) { 120 | return this->right->search(x, y, dist, refx, refy, refnde); 121 | } 122 | } 123 | } 124 | else { 125 | if (x + dist > this->x) { 126 | if (this->right != NULL) { 127 | NNret ret = this->right->search(x, y, dist, refx, refy, refnde); 128 | dist = ret.d; 129 | refx = ret.x; 130 | refy = ret.y; 131 | refnde = ret.nde; 132 | } 133 | } 134 | if (x - dist <= this->x) { 135 | if (this->left != NULL) { 136 | return this->left->search(x, y, dist, refx, refy, refnde); 137 | } 138 | } 139 | } 140 | } 141 | else { 142 | if (y <= this->y) { 143 | if (y - dist <= this->y) { 144 | if (this->left != NULL) { 145 | NNret ret = this->left->search(x, y, dist, refx, refy, refnde); 146 | refx = ret.x; 147 | refy = ret.y; 148 | dist = ret.d; 149 | refnde = ret.nde; 150 | } 151 | } 152 | if (y + dist > this->y) { 153 | if (this->right != NULL) { 154 | return this->right->search(x, y, dist, refx, refy, refnde); 155 | } 156 | } 157 | } 158 | else { 159 | if (y + dist > this->y) { 160 | if (this->right != NULL) { 161 | NNret ret = this->right->search(x, y, dist, refx, refy, refnde); 162 | dist = ret.d; 163 | refx = ret.x; 164 | refy = ret.y; 165 | refnde = ret.nde; 166 | } 167 | } 168 | if (y - dist <= this->y) { 169 | if (this->left != NULL) { 170 | return this->left->search(x, y, dist, refx, refy, refnde); 171 | } 172 | } 173 | } 174 | 175 | } 176 | return NNret(refx, refy, dist, refnde); 177 | } 178 | } 179 | 180 | void KDTree::searchRNN(int &x, int &y, long double d, vector &v) { 181 | int axis = this->axis; 182 | if (this->left == NULL && this->right == NULL) { 183 | long double w = this->dist(x, y); 184 | if (w < d) { 185 | v.push_back(this->nde); 186 | } 187 | return; 188 | } 189 | else { 190 | long double w = this->dist(x, y); 191 | if (w < d) { 192 | v.push_back(this->nde); 193 | } 194 | if (axis == 0) { 195 | if (x < this->x) { 196 | if (x - d <= this->x && this->left != NULL) { 197 | this->left->searchRNN(x, y, d, v); 198 | } 199 | if (x + d > this->x && this->right != NULL) { 200 | this->right->searchRNN(x, y, d, v); 201 | return; 202 | } 203 | } 204 | else { 205 | if (x + d > this->x && this->right != NULL) { 206 | this->right->searchRNN(x, y, d, v); 207 | } 208 | if (x - d <= this->x && this->left != NULL) { 209 | this->left->searchRNN(x, y, d, v); 210 | return; 211 | } 212 | } 213 | }else{ 214 | if (y < this->y) { 215 | if (y - d <= this->y && this->left != NULL) { 216 | this->left->searchRNN(x, y, d, v); 217 | } 218 | if (y + d > this->y && this->right != NULL) { 219 | this->right->searchRNN(x, y, d, v); 220 | return; 221 | } 222 | } 223 | else { 224 | if (y + d > this->y && this->right != NULL) { 225 | this->right->searchRNN(x, y, d, v); 226 | } 227 | if (y - d <= this->y && this->left != NULL) { 228 | this->left->searchRNN(x, y, d, v); 229 | return; 230 | } 231 | } 232 | } 233 | } 234 | } 235 | 236 | KDTree* minNode(KDTree* x, KDTree* y, KDTree* z, int &depth) { 237 | KDTree* res = x; 238 | if (depth == 0) { 239 | if (y != NULL && y->x < res->x) 240 | res = y; 241 | if (z != NULL && z->x < res->x) 242 | res = z; 243 | return res; 244 | } 245 | else { 246 | if (y != NULL && y->y < res->y) 247 | res = y; 248 | if (z != NULL && z->y < res->y) 249 | res = z; 250 | return res; 251 | } 252 | } 253 | 254 | KDTree* findMin(KDTree* root, int &d) { 255 | if (root == NULL) 256 | return root; 257 | int cd = root->axis; 258 | if (cd == d) { 259 | if (root->left == NULL) { 260 | return root; 261 | } 262 | return findMin(root->left, d); 263 | } 264 | 265 | return minNode(root, findMin(root->left, d), findMin(root->right, d), d); 266 | } 267 | 268 | KDTree* deleteRec(KDTree* root, int &x, int &y) { 269 | if (root == NULL) { 270 | return NULL; 271 | } 272 | int cd = root->axis; 273 | if (root->x == x and root->y == y) { 274 | if (root->right != NULL) { 275 | KDTree* nde = findMin(root->right, cd); 276 | root->x = nde->x; 277 | root->y = nde->y; 278 | root->right = deleteRec(root->right, nde->x, nde->y); 279 | } 280 | else if (root->left != NULL) { 281 | KDTree* nde = findMin(root->left, cd); 282 | root->x = nde->x; 283 | root->y = nde->y; 284 | root->right = deleteRec(root->left, nde->x, nde->y); 285 | root->left = NULL; 286 | } 287 | else { 288 | delete root; 289 | return NULL; 290 | } 291 | return root; 292 | } 293 | if (cd == 0) { 294 | if (x < root->x) { 295 | root->left = deleteRec(root->left, x, y); 296 | } 297 | else { 298 | root->right = deleteRec(root->right, x, y); 299 | } 300 | } 301 | else { 302 | if (y < root->y) { 303 | root->left = deleteRec(root->left, x, y); 304 | } 305 | else { 306 | root->right = deleteRec(root->right, x, y); 307 | } 308 | } 309 | return root; 310 | } 311 | 312 | KDTree* KDTree::deletePoint(int &x, int &y) { 313 | /*int refx = -100000000; 314 | int refy = -100000000; 315 | NNret ret = this->search(x, y, LDBL_MAX, refx, refy, NULL); 316 | if (ret.d >= 1) { 317 | return this; 318 | }*/ 319 | return deleteRec(this, x, y); 320 | 321 | } 322 | 323 | bool KDTree::ifPresent(int& x, int& y) { 324 | int axix = this->axis; 325 | if (this->x == x && this->y == y) { 326 | return true; 327 | } 328 | if (this->left == NULL && this->right == NULL) 329 | return false; 330 | bool ans = false; 331 | if (axis == 0) { 332 | if (this->x > x ) { 333 | if(this->left != NULL) 334 | ans = ans || this->left->ifPresent(x, y); 335 | } 336 | else { 337 | if (this->right != NULL) 338 | ans = ans || this->right->ifPresent(x, y); 339 | } 340 | } 341 | else { 342 | if (this->y > y) { 343 | if (this->left != NULL) 344 | ans = ans || this->left->ifPresent(x, y); 345 | } 346 | else { 347 | if (this->right != NULL) 348 | ans = ans ||this->right->ifPresent(x, y); 349 | } 350 | } 351 | return ans; 352 | } 353 | 354 | KDTree* deleteEverything(KDTree* Point) { 355 | if (Point->left != NULL) { 356 | Point->left = deleteEverything(Point->left); 357 | } 358 | if (Point->right != NULL) { 359 | Point->right = deleteEverything(Point->right); 360 | } 361 | delete(Point); 362 | return NULL; 363 | } -------------------------------------------------------------------------------- /C++/Opencv/KdTree.h: -------------------------------------------------------------------------------- 1 | #ifndef KDTREE 2 | #define KDTREE 3 | 4 | #include 5 | #include "RRT.h" 6 | #define vi vector 7 | using namespace std; 8 | 9 | class NNret { 10 | public: 11 | int x, y; 12 | long double d; 13 | node * nde; 14 | NNret(int &x, int &y, long double d, node *nde); 15 | NNret(); 16 | 17 | }; 18 | 19 | class KDTree { 20 | public: 21 | int x, y; 22 | int dim, axis; 23 | KDTree *left, *right; 24 | node * nde; 25 | KDTree(int &x, int &y, int axis, KDTree* left, KDTree* right, node* nde); 26 | void insert(int &x, int &y, node *nde); 27 | long double dist(int &x, int &y); 28 | void printTree(void); 29 | NNret search(int &x, int &y, long double dist, int &refx, int &refy, node* refnde); 30 | void searchRNN(int &x, int &y, long double dist, vector &v); 31 | KDTree* deletePoint(int &x, int &y); 32 | bool ifPresent(int&x, int&y); 33 | 34 | }; 35 | 36 | KDTree* deleteRec(KDTree* root, int &x, int &y); 37 | KDTree* findMin(KDTree* root, int &d); 38 | KDTree* minNode(KDTree* x, KDTree* y, KDTree* z, int &depth); 39 | KDTree* deleteEverything(KDTree* Point); 40 | #endif -------------------------------------------------------------------------------- /C++/Opencv/Opencv.vcxproj: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | Debug 14 | x64 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | {881F74BD-1044-4B65-AAD5-FEB7FABCCCF1} 23 | Win32Proj 24 | Opencv 25 | 8.1 26 | 27 | 28 | 29 | Application 30 | true 31 | v140 32 | Unicode 33 | 34 | 35 | Application 36 | false 37 | v140 38 | true 39 | Unicode 40 | 41 | 42 | Application 43 | true 44 | v140 45 | Unicode 46 | 47 | 48 | Application 49 | false 50 | v140 51 | true 52 | Unicode 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | true 74 | 75 | 76 | true 77 | 78 | 79 | false 80 | 81 | 82 | false 83 | 84 | 85 | 86 | 87 | 88 | Level3 89 | Disabled 90 | WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) 91 | true 92 | 93 | 94 | Console 95 | true 96 | 97 | 98 | 99 | 100 | 101 | 102 | Level3 103 | Disabled 104 | _DEBUG;_CONSOLE;%(PreprocessorDefinitions) 105 | true 106 | C:\Users\Bhaskar\Downloads\opencv\build\include 107 | 108 | 109 | Console 110 | true 111 | C:\Users\Bhaskar\Downloads\opencv\build\x64\vc14\lib 112 | opencv_world310d.lib;%(AdditionalDependencies) 113 | 114 | 115 | 116 | 117 | Level3 118 | 119 | 120 | MaxSpeed 121 | true 122 | true 123 | WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 124 | true 125 | 126 | 127 | Console 128 | true 129 | true 130 | true 131 | 132 | 133 | 134 | 135 | Level3 136 | 137 | 138 | MaxSpeed 139 | true 140 | true 141 | NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 142 | true 143 | 144 | 145 | Console 146 | true 147 | true 148 | true 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | -------------------------------------------------------------------------------- /C++/Opencv/Opencv.vcxproj.filters: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;hm;inl;inc;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Source Files 20 | 21 | 22 | Source Files 23 | 24 | 25 | Source Files 26 | 27 | 28 | Source Files 29 | 30 | 31 | 32 | 33 | Source Files 34 | 35 | 36 | Source Files 37 | 38 | 39 | Source Files 40 | 41 | 42 | 43 | 44 | Source Files 45 | 46 | 47 | Source Files 48 | 49 | 50 | Source Files 51 | 52 | 53 | Source Files 54 | 55 | 56 | Source Files 57 | 58 | 59 | Source Files 60 | 61 | 62 | Source Files 63 | 64 | 65 | Source Files 66 | 67 | 68 | Source Files 69 | 70 | 71 | Source Files 72 | 73 | 74 | Source Files 75 | 76 | 77 | -------------------------------------------------------------------------------- /C++/Opencv/Opencv.vcxproj.user: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | -------------------------------------------------------------------------------- /C++/Opencv/Output/convert.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace std; 4 | 5 | int main() 6 | { 7 | for(int i = 1; i < 2; i++){ 8 | ifstream file; 9 | stringstream s1, s2, s3; 10 | string str1 = "obstacle_type5_input"; 11 | string str2 = ".txt"; 12 | s1 << str1 << i << str2; 13 | //cout << s1.str() << endl; 14 | file.open(s1.str().c_str()); 15 | double x, y; 16 | ofstream file1, file2; 17 | string str3 = "input"; 18 | string str4 = "_type5_1.txt"; 19 | string str5 = "_type5_2.txt"; 20 | s2 << str3 <> x >> y){ 26 | file1 << x << endl; 27 | file2 << y << endl; 28 | } 29 | file.close(); 30 | file1.close(); 31 | file2.close(); 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /C++/Opencv/Output/convert.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/Output/convert.exe -------------------------------------------------------------------------------- /C++/Opencv/Output/convert.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/Output/convert.o -------------------------------------------------------------------------------- /C++/Opencv/Output/input1_type1_1.txt: -------------------------------------------------------------------------------- 1 | 15194 2 | 31638 3 | 42295 4 | 72822 5 | 12035 6 | 14487 7 | 15898 8 | 23631 9 | 41572 10 | 18930 11 | 15069 12 | 11432 13 | 18477 14 | 19553 15 | 31072 16 | 20361 17 | 38292 18 | 26536 19 | 15946 20 | 19343 21 | 18682 22 | 22759 23 | 18889 24 | 40293 25 | 25605 26 | 38090 27 | 24243 28 | 53310 29 | 47072 30 | 16775 31 | 23511 32 | 13855 33 | 21224 34 | 42222 35 | 16021 36 | 17684 37 | 56548 38 | 20255 39 | 27881 40 | 35269 41 | 14921 42 | 44377 43 | 45849 44 | 19450 45 | 37963 46 | 19623 47 | 14488 48 | 18106 49 | 20241 50 | 87145 51 | 16242 52 | 13629 53 | 12510 54 | 25371 55 | 13085 56 | 21886 57 | 24226 58 | 11477 59 | 40016 60 | 23143 61 | 29377 62 | 22751 63 | 20007 64 | 15228 65 | 16792 66 | 13349 67 | 19745 68 | 15118 69 | 27469 70 | 14563 71 | 20186 72 | 18645 73 | 19918 74 | 52308 75 | 57265 76 | 16433 77 | 66162 78 | 17715 79 | 54669 80 | 11469 81 | 16133 82 | 23104 83 | 28984 84 | 31621 85 | 18865 86 | 73566 87 | 20917 88 | 17079 89 | 26622 90 | 14590 91 | 18439 92 | 16571 93 | 55863 94 | 19515 95 | 47709 96 | 25350 97 | 13107 98 | 22508 99 | 28041 100 | 35049 101 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input1_type1_2.txt: -------------------------------------------------------------------------------- 1 | 1427.88 2 | 1955.67 3 | 2126.07 4 | 2970.09 5 | 1274.75 6 | 1418.2 7 | 1484.73 8 | 1773.26 9 | 2304.63 10 | 1596.72 11 | 1427.88 12 | 1289.81 13 | 1509.18 14 | 1705.93 15 | 1958.58 16 | 1513.1 17 | 2007.89 18 | 1767.55 19 | 1461.24 20 | 1623.46 21 | 1447.99 22 | 1657.75 23 | 1548.19 24 | 2260.65 25 | 1811.79 26 | 1986.48 27 | 1632.02 28 | 2361.67 29 | 2072.59 30 | 1386.59 31 | 1651.15 32 | 1371.99 33 | 1597.53 34 | 2203.82 35 | 1549.28 36 | 1569.64 37 | 2605.66 38 | 1584.73 39 | 1939.95 40 | 2064.12 41 | 1427.88 42 | 2568.32 43 | 2347.5 44 | 1587.06 45 | 2074.98 46 | 1618.26 47 | 1465.17 48 | 1513.81 49 | 1685.89 50 | 3315.42 51 | 1427.51 52 | 1380.42 53 | 1391.44 54 | 1881.83 55 | 1400.66 56 | 1656.44 57 | 1891.76 58 | 1357.41 59 | 2191.61 60 | 1846.18 61 | 1991.98 62 | 1818.15 63 | 1629.32 64 | 1490.41 65 | 1472.87 66 | 1406.78 67 | 1543.26 68 | 1425.04 69 | 1988.13 70 | 1468.15 71 | 1703.39 72 | 1555.05 73 | 1619.16 74 | 2572.76 75 | 2627.84 76 | 1565.67 77 | 2880.88 78 | 1606.73 79 | 2559.28 80 | 1274.63 81 | 1602.42 82 | 1818.79 83 | 1886.55 84 | 2023.43 85 | 1545.67 86 | 2970.67 87 | 1710.56 88 | 1470.74 89 | 1880.01 90 | 1377.29 91 | 1574.28 92 | 1471.5 93 | 2571.08 94 | 1586.67 95 | 2550.77 96 | 1848.36 97 | 1412.42 98 | 1680.07 99 | 1947.35 100 | 2060.63 101 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input1_type5_1.txt: -------------------------------------------------------------------------------- 1 | 8898 2 | 4790 3 | 7202 4 | 4784 5 | 5566 6 | 5330 7 | 7637 8 | 5814 9 | 5659 10 | 5611 11 | 6523 12 | 4456 13 | 4428 14 | 6032 15 | 9456 16 | 7478 17 | 4463 18 | 9868 19 | 8756 20 | 6469 21 | 4801 22 | 4357 23 | 8730 24 | 6508 25 | 7246 26 | 4379 27 | 5519 28 | 9230 29 | 5856 30 | 5941 31 | 4644 32 | 6610 33 | 3918 34 | 4908 35 | 4407 36 | 6708 37 | 9200 38 | 3952 39 | 3691 40 | 4332 41 | 6177 42 | 3565 43 | 5172 44 | 6963 45 | 9065 46 | 13719 47 | 8469 48 | 4360 49 | 5256 50 | 9992 51 | 8295 52 | 11475 53 | 6621 54 | 6727 55 | 7196 56 | 10290 57 | 5566 58 | 4839 59 | 8082 60 | 3538 61 | 5022 62 | 3714 63 | 7092 64 | 6462 65 | 5632 66 | 4831 67 | 4615 68 | 5348 69 | 5354 70 | 5819 71 | 5020 72 | 8028 73 | 5606 74 | 10717 75 | 4921 76 | 4459 77 | 11905 78 | 5623 79 | 5989 80 | 6583 81 | 4309 82 | 3842 83 | 6096 84 | 8088 85 | 5100 86 | 5114 87 | 7586 88 | 5040 89 | 5752 90 | 4333 91 | 9291 92 | 3456 93 | 6526 94 | 4679 95 | 5335 96 | 8179 97 | 7547 98 | 3851 99 | 6053 100 | 6103 101 | 4782 102 | 4378 103 | 19315 104 | 5815 105 | 4037 106 | 4881 107 | 6865 108 | 4855 109 | 9517 110 | 8368 111 | 3709 112 | 6570 113 | 3853 114 | 9217 115 | 6605 116 | 6642 117 | 8786 118 | 6169 119 | 12535 120 | 18474 121 | 17759 122 | 6255 123 | 6477 124 | 5794 125 | 5734 126 | 3930 127 | 8111 128 | 5441 129 | 3954 130 | 5015 131 | 6999 132 | 5816 133 | 5328 134 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input1_type5_2.txt: -------------------------------------------------------------------------------- 1 | 2912.39 2 | 1729.54 3 | 2449.11 4 | 1654.64 5 | 1862.47 6 | 1806.69 7 | 2549.75 8 | 2055.3 9 | 1919.38 10 | 1951.66 11 | 2367.34 12 | 1687.48 13 | 1596.19 14 | 2118.55 15 | 3236.48 16 | 2411.74 17 | 1620.39 18 | 3596.55 19 | 2944.75 20 | 2151.32 21 | 1771.02 22 | 1519.14 23 | 2828.04 24 | 2235.95 25 | 2416.08 26 | 1596.19 27 | 2037.28 28 | 3062.1 29 | 2086.63 30 | 2085.02 31 | 1642.53 32 | 2354.43 33 | 1461.4 34 | 1759.84 35 | 1573.21 36 | 2281.16 37 | 3054.78 38 | 1477.43 39 | 1373.89 40 | 1596.19 41 | 2066.04 42 | 1392.21 43 | 1979.48 44 | 2519.01 45 | 3026.53 46 | 4395.53 47 | 2553.15 48 | 1596.19 49 | 1894.76 50 | 3466.79 51 | 2918.9 52 | 3924.7 53 | 2343.3 54 | 2334.72 55 | 2324.15 56 | 3474.67 57 | 1907.47 58 | 1707.9 59 | 2658.51 60 | 1395.92 61 | 1841.5 62 | 1436.26 63 | 2491.84 64 | 2318.51 65 | 2046.11 66 | 1672.17 67 | 1758.85 68 | 1809.39 69 | 1875.5 70 | 2135.84 71 | 1738.08 72 | 2665.18 73 | 1936.37 74 | 3474.7 75 | 1802.11 76 | 1556.82 77 | 4007 78 | 1998.93 79 | 2085.24 80 | 2208.1 81 | 1516.57 82 | 1447.19 83 | 2165.5 84 | 2701.08 85 | 1815.79 86 | 1649.74 87 | 2665.96 88 | 1877.11 89 | 1941.85 90 | 1596.19 91 | 3113.23 92 | 1331.28 93 | 2217.13 94 | 1767.52 95 | 2011.17 96 | 2703.11 97 | 2630.46 98 | 1409.53 99 | 2091.83 100 | 2236.14 101 | 1700.56 102 | 1596.19 103 | 6258.12 104 | 1998.76 105 | 1462.2 106 | 1675.09 107 | 2326.53 108 | 1853.73 109 | 3172 110 | 2859.8 111 | 1393.52 112 | 2126.88 113 | 1395.83 114 | 2912.39 115 | 2094.44 116 | 2200.1 117 | 2985.38 118 | 1926.21 119 | 3457.46 120 | 5452.34 121 | 5160.37 122 | 1946.43 123 | 2122.94 124 | 1929.66 125 | 1747.93 126 | 1409.84 127 | 2123.21 128 | 1863.45 129 | 1469.66 130 | 1679.26 131 | 2466.64 132 | 1977.6 133 | 1682.45 134 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input2_type1_1.txt: -------------------------------------------------------------------------------- 1 | 11624 2 | 15122 3 | 11615 4 | 16095 5 | 13550 6 | 13648 7 | 11177 8 | 14399 9 | 11995 10 | 12413 11 | 12377 12 | 13297 13 | 11489 14 | 13269 15 | 13455 16 | 12789 17 | 20082 18 | 13822 19 | 12540 20 | 13224 21 | 11589 22 | 13401 23 | 11319 24 | 13719 25 | 13078 26 | 21386 27 | 12608 28 | 13849 29 | 13572 30 | 12813 31 | 12402 32 | 10894 33 | 12278 34 | 12072 35 | 13288 36 | 12338 37 | 12672 38 | 12555 39 | 11779 40 | 13427 41 | 11500 42 | 18678 43 | 10865 44 | 10972 45 | 13291 46 | 13159 47 | 13389 48 | 11499 49 | 11992 50 | 11743 51 | 12282 52 | 11836 53 | 12009 54 | 12839 55 | 11974 56 | 12985 57 | 13339 58 | 12902 59 | 12034 60 | 11596 61 | 11796 62 | 12739 63 | 13699 64 | 10659 65 | 13344 66 | 11975 67 | 11384 68 | 10945 69 | 12403 70 | 10952 71 | 13357 72 | 11289 73 | 13679 74 | 12231 75 | 12299 76 | 13987 77 | 12369 78 | 13089 79 | 15729 80 | 14912 81 | 12054 82 | 13300 83 | 12698 84 | 11642 85 | 19710 86 | 13188 87 | 10933 88 | 12145 89 | 15641 90 | 11951 91 | 17867 92 | 12106 93 | 12462 94 | 11835 95 | 13744 96 | 12767 97 | 11423 98 | 18178 99 | 12135 100 | 11016 101 | 11785 102 | 12180 103 | 14310 104 | 13233 105 | 14391 106 | 12471 107 | 12575 108 | 14651 109 | 16407 110 | 14520 111 | 11542 112 | 13252 113 | 14096 114 | 12286 115 | 11844 116 | 12092 117 | 18277 118 | 13113 119 | 14162 120 | 14674 121 | 13358 122 | 13070 123 | 13073 124 | 19583 125 | 13276 126 | 12552 127 | 15700 128 | 16268 129 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input2_type1_2.txt: -------------------------------------------------------------------------------- 1 | 1276.41 2 | 1296.6 3 | 1275.84 4 | 1382.35 5 | 1319 6 | 1294.31 7 | 1286.42 8 | 1382.58 9 | 1286 10 | 1287.29 11 | 1294.49 12 | 1390.25 13 | 1311.2 14 | 1299.55 15 | 1361.1 16 | 1356.77 17 | 1752.99 18 | 1330.91 19 | 1292.49 20 | 1345.14 21 | 1284.52 22 | 1340.89 23 | 1283.43 24 | 1311.12 25 | 1354.25 26 | 1534.01 27 | 1306.16 28 | 1365.16 29 | 1410.77 30 | 1287.57 31 | 1309.15 32 | 1287.4 33 | 1274.94 34 | 1283.32 35 | 1334.18 36 | 1284.7 37 | 1345.45 38 | 1299.36 39 | 1289.24 40 | 1380.8 41 | 1288.28 42 | 1694.51 43 | 1275.58 44 | 1294.05 45 | 1298.83 46 | 1300.73 47 | 1392.32 48 | 1328.26 49 | 1277.81 50 | 1279.95 51 | 1290.24 52 | 1299.55 53 | 1310.9 54 | 1304.42 55 | 1298.79 56 | 1400.39 57 | 1278.97 58 | 1302.86 59 | 1281.07 60 | 1295.9 61 | 1286.87 62 | 1291.71 63 | 1353.52 64 | 1283.18 65 | 1323.55 66 | 1289.61 67 | 1298.33 68 | 1281.03 69 | 1284.56 70 | 1284.17 71 | 1288.67 72 | 1286.79 73 | 1290.13 74 | 1288.31 75 | 1284.56 76 | 1374.11 77 | 1284.56 78 | 1283.94 79 | 1280.14 80 | 1279.65 81 | 1315.54 82 | 1300.36 83 | 1289.83 84 | 1289.31 85 | 1729.79 86 | 1336.95 87 | 1281.32 88 | 1334.04 89 | 1457.75 90 | 1283.03 91 | 1528.29 92 | 1307.39 93 | 1278.49 94 | 1281.68 95 | 1285.77 96 | 1293.94 97 | 1284.18 98 | 1618.03 99 | 1282.35 100 | 1275.58 101 | 1285.11 102 | 1279.29 103 | 1352.74 104 | 1335.16 105 | 1413.34 106 | 1316.04 107 | 1274.93 108 | 1376.41 109 | 1356.51 110 | 1446.19 111 | 1284.24 112 | 1280.42 113 | 1372.81 114 | 1292.1 115 | 1273.91 116 | 1293.18 117 | 1524.93 118 | 1284.56 119 | 1324.68 120 | 1379.09 121 | 1391.18 122 | 1288.55 123 | 1288.93 124 | 1597.93 125 | 1289.53 126 | 1277.88 127 | 1554.95 128 | 1358.94 129 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input2_type5_1.txt: -------------------------------------------------------------------------------- 1 | 4935 2 | 4015 3 | 4477 4 | 4085 5 | 4270 6 | 4194 7 | 4363 8 | 4128 9 | 4054 10 | 4691 11 | 3974 12 | 3939 13 | 4307 14 | 4404 15 | 4026 16 | 4095 17 | 3255 18 | 4403 19 | 4041 20 | 4404 21 | 3456 22 | 4588 23 | 9516 24 | 4694 25 | 3960 26 | 4178 27 | 4408 28 | 4334 29 | 4014 30 | 4352 31 | 4256 32 | 4278 33 | 4197 34 | 4104 35 | 5262 36 | 4454 37 | 4336 38 | 4493 39 | 4055 40 | 4570 41 | 3960 42 | 3916 43 | 4284 44 | 4172 45 | 4522 46 | 4632 47 | 4073 48 | 4487 49 | 4313 50 | 4779 51 | 5012 52 | 4007 53 | 4314 54 | 4539 55 | 4060 56 | 4318 57 | 4128 58 | 4208 59 | 4023 60 | 4274 61 | 4389 62 | 4980 63 | 4131 64 | 4305 65 | 5034 66 | 5219 67 | 4293 68 | 5439 69 | 4343 70 | 3991 71 | 4136 72 | 4248 73 | 5030 74 | 6653 75 | 4064 76 | 4307 77 | 6871 78 | 4386 79 | 4285 80 | 4250 81 | 4030 82 | 4359 83 | 4565 84 | 4298 85 | 4612 86 | 4313 87 | 4370 88 | 3934 89 | 3961 90 | 4270 91 | 4594 92 | 4542 93 | 4206 94 | 16397 95 | 4029 96 | 3954 97 | 4411 98 | 3988 99 | 4340 100 | 4462 101 | 4097 102 | 11618 103 | 4266 104 | 3958 105 | 5509 106 | 4238 107 | 4961 108 | 4169 109 | 4959 110 | 3881 111 | 3975 112 | 4093 113 | 4320 114 | 4447 115 | 4235 116 | 4119 117 | 3906 118 | 4345 119 | 4117 120 | 4063 121 | 4303 122 | 4230 123 | 4129 124 | 4056 125 | 5009 126 | 4422 127 | 3976 128 | 6331 129 | 4069 130 | 4336 131 | 4487 132 | 4371 133 | 3952 134 | 5092 135 | 4119 136 | 4928 137 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input2_type5_2.txt: -------------------------------------------------------------------------------- 1 | 1505.54 2 | 1393.12 3 | 1505.54 4 | 1393.08 5 | 1455.88 6 | 1424.3 7 | 1508.45 8 | 1448.42 9 | 1448.68 10 | 1634.71 11 | 1329.28 12 | 1437.79 13 | 1492.41 14 | 1621.82 15 | 1425.03 16 | 1393.97 17 | 1387.39 18 | 1505.54 19 | 1365.77 20 | 1525.51 21 | 1488.66 22 | 1580.31 23 | 2301.49 24 | 1705.28 25 | 1417.99 26 | 1486.97 27 | 1609.23 28 | 1438.36 29 | 1394.09 30 | 1503.07 31 | 1531.81 32 | 1354.32 33 | 1469.01 34 | 1370.09 35 | 1844.06 36 | 1505.54 37 | 1438.88 38 | 1517.48 39 | 1366.73 40 | 1841.12 41 | 1329.28 42 | 1330.48 43 | 1509.18 44 | 1489.71 45 | 1526.27 46 | 1647.2 47 | 1481.78 48 | 1562.32 49 | 1546.36 50 | 1656.68 51 | 1666.53 52 | 1329.28 53 | 1477.83 54 | 1579.08 55 | 1381.37 56 | 1542.03 57 | 1431.5 58 | 1411.24 59 | 1364.55 60 | 1526.85 61 | 1505.54 62 | 1680.25 63 | 1421.23 64 | 1329.28 65 | 1615.78 66 | 1589.74 67 | 1514.97 68 | 1709.86 69 | 1414.57 70 | 1346.81 71 | 1392.81 72 | 1475.74 73 | 1720.44 74 | 2276.58 75 | 1351.74 76 | 1526.64 77 | 1915.88 78 | 1515.6 79 | 1494.54 80 | 1471.98 81 | 1365.6 82 | 1519.58 83 | 1518.75 84 | 1473.35 85 | 1620.2 86 | 1544.7 87 | 1568.8 88 | 1354.93 89 | 1329.28 90 | 1510.96 91 | 1614.22 92 | 1538.55 93 | 1423.69 94 | 1570.48 95 | 1433.9 96 | 1329.28 97 | 1521.31 98 | 1340.56 99 | 1459.36 100 | 1558.98 101 | 1373.7 102 | 3786.76 103 | 1425.31 104 | 1362.75 105 | 1893.18 106 | 1421.53 107 | 1560.05 108 | 1466.42 109 | 1633.64 110 | 1355.54 111 | 1329.28 112 | 1351.78 113 | 1482.46 114 | 1505.54 115 | 1399.06 116 | 1364.01 117 | 1360.63 118 | 1515.1 119 | 1449.64 120 | 1386.5 121 | 1459.52 122 | 1378.63 123 | 1395.81 124 | 1497.81 125 | 1631.39 126 | 1505.54 127 | 1448.58 128 | 2211.59 129 | 1391.86 130 | 1510.39 131 | 1585.91 132 | 1544.52 133 | 1389.33 134 | 1713.9 135 | 1419.9 136 | 1638.31 137 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input3_type1_1.txt: -------------------------------------------------------------------------------- 1 | 3068 2 | 2959 3 | 2733 4 | 3742 5 | 2878 6 | 3237 7 | 3796 8 | 3296 9 | 3592 10 | 2866 11 | 2635 12 | 2829 13 | 3387 14 | 3377 15 | 2943 16 | 3068 17 | 3099 18 | 2750 19 | 2951 20 | 3016 21 | 2883 22 | 3112 23 | 2441 24 | 2924 25 | 2625 26 | 2908 27 | 3275 28 | 3460 29 | 2696 30 | 2880 31 | 3971 32 | 3869 33 | 3236 34 | 3189 35 | 3508 36 | 3475 37 | 2650 38 | 2695 39 | 3137 40 | 3136 41 | 2300 42 | 2903 43 | 2968 44 | 3179 45 | 2723 46 | 2502 47 | 2653 48 | 2480 49 | 2696 50 | 2948 51 | 2746 52 | 3821 53 | 3432 54 | 3151 55 | 2691 56 | 4279 57 | 2863 58 | 3467 59 | 2953 60 | 3244 61 | 4090 62 | 3335 63 | 3480 64 | 2913 65 | 3421 66 | 2661 67 | 3000 68 | 2627 69 | 3113 70 | 2922 71 | 3121 72 | 2372 73 | 2435 74 | 3127 75 | 2692 76 | 3150 77 | 3125 78 | 3068 79 | 3504 80 | 3088 81 | 3067 82 | 2931 83 | 3081 84 | 2573 85 | 2370 86 | 3075 87 | 2938 88 | 3157 89 | 3891 90 | 4343 91 | 2871 92 | 6904 93 | 2618 94 | 2702 95 | 3504 96 | 2472 97 | 2790 98 | 2935 99 | 3391 100 | 2010 101 | 3106 102 | 2918 103 | 3389 104 | 2421 105 | 2611 106 | 2847 107 | 2554 108 | 3076 109 | 3109 110 | 3038 111 | 2408 112 | 2778 113 | 2832 114 | 2821 115 | 2330 116 | 2871 117 | 3065 118 | 3540 119 | 3236 120 | 3043 121 | 3210 122 | 2818 123 | 3388 124 | 2354 125 | 2947 126 | 2838 127 | 3197 128 | 2438 129 | 2849 130 | 2571 131 | 3158 132 | 3062 133 | 3035 134 | 2872 135 | 3195 136 | 2736 137 | 2395 138 | 2905 139 | 3393 140 | 2728 141 | 2892 142 | 3284 143 | 3101 144 | 2938 145 | 2931 146 | 2841 147 | 2406 148 | 2632 149 | 2728 150 | 2823 151 | 2876 152 | 4481 153 | 3415 154 | 3127 155 | 2800 156 | 4218 157 | 3588 158 | 3757 159 | 3402 160 | 3137 161 | 3725 162 | 3035 163 | 3146 164 | 2842 165 | 2745 166 | 1967 167 | 2696 168 | 3315 169 | 2873 170 | 2751 171 | 3332 172 | 2973 173 | 3171 174 | 2559 175 | 3241 176 | 2769 177 | 3311 178 | 3035 179 | 3310 180 | 3305 181 | 2969 182 | 2659 183 | 2964 184 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input3_type1_2.txt: -------------------------------------------------------------------------------- 1 | 653.188 2 | 642.495 3 | 640.317 4 | 646.89 5 | 646.883 6 | 641.902 7 | 668.502 8 | 633.413 9 | 657.966 10 | 640.75 11 | 644.886 12 | 652.789 13 | 650.133 14 | 664.777 15 | 664.503 16 | 646.433 17 | 650.87 18 | 658.128 19 | 646.678 20 | 649.915 21 | 664.562 22 | 660.409 23 | 639.524 24 | 637.288 25 | 652.454 26 | 650.366 27 | 650.643 28 | 649.61 29 | 657.294 30 | 647.396 31 | 666.105 32 | 653.307 33 | 660.48 34 | 635.445 35 | 635.162 36 | 652.842 37 | 664.511 38 | 645.502 39 | 651.086 40 | 649.216 41 | 654.318 42 | 648.283 43 | 646.197 44 | 654.935 45 | 656.13 46 | 667.533 47 | 639.57 48 | 650.64 49 | 651.309 50 | 674.819 51 | 649.538 52 | 656.497 53 | 657.977 54 | 651.23 55 | 649.918 56 | 657.736 57 | 645.08 58 | 645.539 59 | 654.987 60 | 648.074 61 | 656.852 62 | 649.313 63 | 650.327 64 | 641.384 65 | 655.559 66 | 646.363 67 | 641.647 68 | 643.783 69 | 642.26 70 | 645.207 71 | 641.857 72 | 650.051 73 | 643.991 74 | 659.137 75 | 668.79 76 | 638.28 77 | 656.903 78 | 639.421 79 | 651.758 80 | 648.903 81 | 656.744 82 | 648.975 83 | 669.238 84 | 644.597 85 | 649.626 86 | 670.498 87 | 656.138 88 | 640.807 89 | 651.502 90 | 658.753 91 | 648.171 92 | 887.958 93 | 655.153 94 | 649.577 95 | 696.095 96 | 655.923 97 | 633.466 98 | 643.062 99 | 666.25 100 | 645.559 101 | 641.857 102 | 665.299 103 | 648.109 104 | 662.131 105 | 649.122 106 | 650.755 107 | 641.196 108 | 652.304 109 | 659.357 110 | 639.855 111 | 647.3 112 | 651.502 113 | 648.334 114 | 635.438 115 | 651.284 116 | 641.627 117 | 642.376 118 | 641.598 119 | 654.714 120 | 653.066 121 | 642.327 122 | 660.936 123 | 653.99 124 | 654.318 125 | 645.596 126 | 640.131 127 | 667.53 128 | 661.439 129 | 650.455 130 | 647.523 131 | 647.931 132 | 645.037 133 | 671.425 134 | 654.166 135 | 650.017 136 | 669.8 137 | 638.177 138 | 642.461 139 | 655.199 140 | 675.902 141 | 641.669 142 | 659.416 143 | 653.134 144 | 659.429 145 | 639.029 146 | 655.905 147 | 649.665 148 | 653.834 149 | 643.304 150 | 650.254 151 | 640.818 152 | 653.478 153 | 643.847 154 | 641.857 155 | 638.236 156 | 701.427 157 | 650.281 158 | 641.666 159 | 669.648 160 | 654.178 161 | 648.857 162 | 635.849 163 | 644.957 164 | 654.052 165 | 649.578 166 | 650.027 167 | 661.559 168 | 644.576 169 | 648.653 170 | 664.588 171 | 645.875 172 | 646.91 173 | 658.493 174 | 646.169 175 | 652.932 176 | 650.181 177 | 645.509 178 | 639.225 179 | 657.177 180 | 671.315 181 | 649.087 182 | 663.348 183 | 647.798 184 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input3_type4_1.txt: -------------------------------------------------------------------------------- 1 | 5928 2 | 6493 3 | 6970 4 | 5589 5 | 5615 6 | 6090 7 | 6167 8 | 7564 9 | 7201 10 | 5369 11 | 8118 12 | 6407 13 | 6098 14 | 6058 15 | 6348 16 | 5229 17 | 6398 18 | 8430 19 | 9693 20 | 7769 21 | 7181 22 | 4748 23 | 5960 24 | 7074 25 | 12112 26 | 7328 27 | 9503 28 | 7792 29 | 7358 30 | 8546 31 | 8440 32 | 7269 33 | 6462 34 | 7326 35 | 6479 36 | 6017 37 | 6635 38 | 7539 39 | 8440 40 | 7245 41 | 6820 42 | 6710 43 | 7652 44 | 5681 45 | 6197 46 | 6242 47 | 5638 48 | 6261 49 | 6655 50 | 6686 51 | 6544 52 | 7019 53 | 8848 54 | 5838 55 | 6815 56 | 6902 57 | 7149 58 | 6298 59 | 6034 60 | 6728 61 | 6974 62 | 6849 63 | 5663 64 | 7191 65 | 6824 66 | 6342 67 | 6320 68 | 6651 69 | 6468 70 | 6667 71 | 6192 72 | 6654 73 | 7398 74 | 6518 75 | 6195 76 | 6570 77 | 6062 78 | 5386 79 | 6864 80 | 6069 81 | 6421 82 | 6889 83 | 7014 84 | 6554 85 | 6024 86 | 5758 87 | 5927 88 | 6592 89 | 6520 90 | 6663 91 | 6502 92 | 6029 93 | 7407 94 | 6204 95 | 5819 96 | 6904 97 | 6253 98 | 8992 99 | 8371 100 | 7312 101 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input3_type4_2.txt: -------------------------------------------------------------------------------- 1 | 646.362 2 | 639.611 3 | 686.429 4 | 634.44 5 | 641.793 6 | 639.693 7 | 638.157 8 | 634.688 9 | 697.481 10 | 641.481 11 | 633.999 12 | 650.501 13 | 629.12 14 | 632.071 15 | 691.246 16 | 640.769 17 | 635.309 18 | 634.766 19 | 662.298 20 | 694.198 21 | 637.513 22 | 649.986 23 | 634.053 24 | 695.281 25 | 629.637 26 | 636.323 27 | 697.195 28 | 634.32 29 | 700.974 30 | 628.452 31 | 633.811 32 | 634.114 33 | 630.947 34 | 635.729 35 | 642.999 36 | 630.404 37 | 630.046 38 | 633.15 39 | 637.391 40 | 689.186 41 | 692.428 42 | 663.826 43 | 703.803 44 | 636.784 45 | 637.924 46 | 635.763 47 | 635.616 48 | 641.808 49 | 638.441 50 | 636.537 51 | 631.173 52 | 640.719 53 | 635.447 54 | 638.004 55 | 638.398 56 | 660.12 57 | 688.062 58 | 636.429 59 | 634.953 60 | 655.467 61 | 643.145 62 | 634.353 63 | 634.103 64 | 633.145 65 | 631.676 66 | 639.46 67 | 638.102 68 | 633.357 69 | 628.245 70 | 647.241 71 | 640.91 72 | 632.901 73 | 633.802 74 | 640.3 75 | 629.031 76 | 631.692 77 | 638.356 78 | 645.138 79 | 632.858 80 | 634.998 81 | 633.661 82 | 633.49 83 | 633.137 84 | 631.858 85 | 638.347 86 | 633.807 87 | 631.119 88 | 637.859 89 | 637.832 90 | 641.879 91 | 639.964 92 | 638.089 93 | 644.186 94 | 636.657 95 | 635.857 96 | 634.678 97 | 688.648 98 | 633.04 99 | 671.446 100 | 645.318 101 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input3_type5_1.txt: -------------------------------------------------------------------------------- 1 | 1286 2 | 1245 3 | 1182 4 | 1468 5 | 1238 6 | 2052 7 | 1188 8 | 1192 9 | 1236 10 | 1424 11 | 1313 12 | 1180 13 | 1276 14 | 1112 15 | 1228 16 | 1185 17 | 1216 18 | 1391 19 | 1267 20 | 1178 21 | 1136 22 | 1142 23 | 1325 24 | 1141 25 | 1329 26 | 1232 27 | 1146 28 | 1201 29 | 1185 30 | 2032 31 | 1181 32 | 1331 33 | 1186 34 | 1241 35 | 1204 36 | 1293 37 | 1314 38 | 1336 39 | 1212 40 | 1248 41 | 1180 42 | 1231 43 | 1230 44 | 1147 45 | 1088 46 | 1236 47 | 1189 48 | 1228 49 | 1313 50 | 1281 51 | 1445 52 | 2019 53 | 1319 54 | 1163 55 | 1231 56 | 1439 57 | 1328 58 | 1639 59 | 1478 60 | 1221 61 | 1242 62 | 1300 63 | 1369 64 | 1274 65 | 1302 66 | 1256 67 | 1264 68 | 1327 69 | 1203 70 | 1998 71 | 1191 72 | 1230 73 | 1316 74 | 1318 75 | 1329 76 | 1325 77 | 1137 78 | 1207 79 | 1314 80 | 1089 81 | 1245 82 | 1131 83 | 1212 84 | 1135 85 | 1172 86 | 1132 87 | 1118 88 | 1920 89 | 1333 90 | 1243 91 | 1171 92 | 1229 93 | 1143 94 | 1144 95 | 1194 96 | 1236 97 | 1255 98 | 1190 99 | 2069 100 | 2418 101 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input3_type5_2.txt: -------------------------------------------------------------------------------- 1 | 688.657 2 | 727.843 3 | 743.103 4 | 755.092 5 | 728.692 6 | 563.373 7 | 703.043 8 | 687.869 9 | 742.955 10 | 776.58 11 | 711.908 12 | 707.247 13 | 706.931 14 | 745.911 15 | 709.989 16 | 691.324 17 | 783.475 18 | 758.999 19 | 801.19 20 | 682.98 21 | 759.65 22 | 761.096 23 | 702.895 24 | 769.276 25 | 771.675 26 | 737.765 27 | 766.45 28 | 700.004 29 | 736.49 30 | 715.793 31 | 754.32 32 | 730.479 33 | 702.931 34 | 703.436 35 | 717.315 36 | 712.789 37 | 758.903 38 | 755.913 39 | 729.385 40 | 707.138 41 | 691.475 42 | 732.46 43 | 793.189 44 | 701.377 45 | 706.12 46 | 719.843 47 | 692.168 48 | 712.086 49 | 703.895 50 | 713.655 51 | 753.973 52 | 588.99 53 | 729.691 54 | 755.99 55 | 731.725 56 | 746.696 57 | 789.564 58 | 711.516 59 | 766.002 60 | 748.433 61 | 692.634 62 | 706.845 63 | 702.188 64 | 703.672 65 | 708.273 66 | 730.412 67 | 687.742 68 | 733.88 69 | 705.917 70 | 717.587 71 | 719.059 72 | 714.656 73 | 750.431 74 | 704.987 75 | 713.525 76 | 724.772 77 | 713.378 78 | 695.059 79 | 698.296 80 | 673.624 81 | 723.643 82 | 714.517 83 | 792.556 84 | 773.782 85 | 723.235 86 | 769.471 87 | 700.285 88 | 685.041 89 | 740.513 90 | 750.938 91 | 730.599 92 | 698.804 93 | 774.45 94 | 749.137 95 | 692.293 96 | 762.546 97 | 736.358 98 | 713.875 99 | 604.701 100 | 714.977 101 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input4_type1_1.txt: -------------------------------------------------------------------------------- 1 | 8616 2 | 8360 3 | 8662 4 | 6896 5 | 9057 6 | 7431 7 | 8811 8 | 9301 9 | 8054 10 | 7571 11 | 8549 12 | 9671 13 | 8197 14 | 7591 15 | 8838 16 | 7394 17 | 7768 18 | 7950 19 | 9507 20 | 8400 21 | 10349 22 | 11941 23 | 7115 24 | 8532 25 | 8667 26 | 9145 27 | 8120 28 | 9270 29 | 8670 30 | 7263 31 | 9468 32 | 9345 33 | 9519 34 | 8750 35 | 10464 36 | 8066 37 | 28244 38 | 10015 39 | 9291 40 | 6778 41 | 7073 42 | 9068 43 | 7835 44 | 9092 45 | 7946 46 | 8215 47 | 8661 48 | 8675 49 | 7626 50 | 8917 51 | 8984 52 | 7980 53 | 8188 54 | 9715 55 | 7798 56 | 8669 57 | 9554 58 | 9814 59 | 7975 60 | 10963 61 | 17472 62 | 11328 63 | 12429 64 | 11006 65 | 8684 66 | 8807 67 | 8830 68 | 8938 69 | 8820 70 | 9605 71 | 10740 72 | 8508 73 | 7352 74 | 6881 75 | 9866 76 | 8916 77 | 7600 78 | 8661 79 | 8010 80 | 7418 81 | 6911 82 | 9694 83 | 7289 84 | 7630 85 | 8438 86 | 8915 87 | 7043 88 | 7380 89 | 7404 90 | 7611 91 | 8174 92 | 7568 93 | 7799 94 | 7815 95 | 9738 96 | 7910 97 | 8869 98 | 8343 99 | 7683 100 | 9220 101 | 6867 102 | 7049 103 | 8004 104 | 7881 105 | 8218 106 | 8774 107 | 8229 108 | 7228 109 | 7656 110 | 8666 111 | 11144 112 | 7513 113 | 9341 114 | 8477 115 | 9357 116 | 8823 117 | 10257 118 | 9004 119 | 7970 120 | 8073 121 | 7390 122 | 7369 123 | 6955 124 | 9586 125 | 7001 126 | 7604 127 | 8842 128 | 8335 129 | 9605 130 | 8198 131 | 8530 132 | 7987 133 | 8367 134 | 8138 135 | 9082 136 | 9575 137 | 8938 138 | 7892 139 | 8062 140 | 8483 141 | 7576 142 | 8760 143 | 7899 144 | 7237 145 | 8633 146 | 8737 147 | 8180 148 | 9408 149 | 9589 150 | 8092 151 | 11845 152 | 8212 153 | 8750 154 | 7627 155 | 8189 156 | 6997 157 | 8412 158 | 8554 159 | 9005 160 | 8855 161 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input4_type1_2.txt: -------------------------------------------------------------------------------- 1 | 937.378 2 | 920.279 3 | 994.172 4 | 924.598 5 | 976.024 6 | 917.11 7 | 911.726 8 | 932.204 9 | 902.479 10 | 902.624 11 | 926.89 12 | 956.952 13 | 926.946 14 | 894.443 15 | 916.069 16 | 886.098 17 | 915.614 18 | 926.299 19 | 919.168 20 | 912.72 21 | 889.351 22 | 926.072 23 | 889.46 24 | 903.102 25 | 935.176 26 | 913.22 27 | 888.807 28 | 942.354 29 | 912.137 30 | 924.461 31 | 927.074 32 | 985.304 33 | 909.481 34 | 931.375 35 | 1076.05 36 | 905.329 37 | 1391.63 38 | 932.083 39 | 962.781 40 | 928.404 41 | 893.567 42 | 967.527 43 | 918.869 44 | 910.201 45 | 932.204 46 | 921.34 47 | 918.063 48 | 924.348 49 | 922.083 50 | 937.611 51 | 1039.19 52 | 938.826 53 | 887.918 54 | 934.198 55 | 896.656 56 | 962.959 57 | 962.123 58 | 961.12 59 | 906.938 60 | 880.61 61 | 1005.49 62 | 905.829 63 | 964.18 64 | 919.074 65 | 921.922 66 | 927.961 67 | 937.25 68 | 931.709 69 | 893.489 70 | 895.402 71 | 979.908 72 | 925.793 73 | 909.201 74 | 897.008 75 | 956.248 76 | 930.646 77 | 940.731 78 | 987.503 79 | 918.263 80 | 899.736 81 | 899.588 82 | 967.499 83 | 906.31 84 | 910.444 85 | 909.185 86 | 975.24 87 | 896.255 88 | 928.874 89 | 910.201 90 | 925.93 91 | 926.955 92 | 933.482 93 | 915.665 94 | 909.669 95 | 964.627 96 | 935.165 97 | 961.973 98 | 996.643 99 | 913.003 100 | 985.228 101 | 907.659 102 | 885.369 103 | 986.878 104 | 943.05 105 | 961.467 106 | 1003.46 107 | 935.834 108 | 898.659 109 | 909.919 110 | 928.509 111 | 1056.68 112 | 893.56 113 | 997.47 114 | 942.148 115 | 997.921 116 | 974.629 117 | 934.51 118 | 931.66 119 | 894.782 120 | 915.177 121 | 930.756 122 | 938.217 123 | 928.869 124 | 1001.83 125 | 913.864 126 | 891.157 127 | 918.842 128 | 920.193 129 | 966.803 130 | 961.58 131 | 959.361 132 | 925.578 133 | 954.563 134 | 898.173 135 | 939.311 136 | 906.356 137 | 925.38 138 | 887.475 139 | 925.125 140 | 927.002 141 | 899.321 142 | 992.027 143 | 914.531 144 | 912.165 145 | 927.044 146 | 951.739 147 | 908.952 148 | 1008.25 149 | 976.616 150 | 926.572 151 | 1035.17 152 | 971.175 153 | 944.835 154 | 890.993 155 | 940.182 156 | 906.197 157 | 947.245 158 | 932.236 159 | 968.974 160 | 906.991 161 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input4_type5_1.txt: -------------------------------------------------------------------------------- 1 | 3012 2 | 4485 3 | 2879 4 | 3464 5 | 2554 6 | 2866 7 | 4448 8 | 2970 9 | 2274 10 | 2974 11 | 2977 12 | 3254 13 | 2377 14 | 2616 15 | 2220 16 | 3029 17 | 2921 18 | 4971 19 | 2480 20 | 2218 21 | 2703 22 | 2948 23 | 3095 24 | 2735 25 | 2642 26 | 2272 27 | 2750 28 | 3318 29 | 3114 30 | 2905 31 | 3081 32 | 2916 33 | 3154 34 | 2271 35 | 4903 36 | 2279 37 | 2217 38 | 3223 39 | 2795 40 | 2317 41 | 2782 42 | 2532 43 | 5208 44 | 3177 45 | 3011 46 | 2461 47 | 2332 48 | 3166 49 | 2777 50 | 2264 51 | 2894 52 | 2311 53 | 2768 54 | 2476 55 | 2239 56 | 3053 57 | 3055 58 | 5384 59 | 4663 60 | 5231 61 | 2938 62 | 2994 63 | 2724 64 | 3234 65 | 3039 66 | 2964 67 | 2299 68 | 3159 69 | 2823 70 | 3320 71 | 3573 72 | 3528 73 | 3300 74 | 2670 75 | 3866 76 | 2450 77 | 3405 78 | 2626 79 | 3060 80 | 4373 81 | 2857 82 | 3054 83 | 4198 84 | 3140 85 | 3525 86 | 3593 87 | 3027 88 | 5514 89 | 2889 90 | 3156 91 | 3175 92 | 2840 93 | 3521 94 | 2651 95 | 3066 96 | 5723 97 | 2925 98 | 2769 99 | 4392 100 | 2544 101 | -------------------------------------------------------------------------------- /C++/Opencv/Output/input4_type5_2.txt: -------------------------------------------------------------------------------- 1 | 1272.23 2 | 1861.87 3 | 1102.77 4 | 1366.1 5 | 1168.29 6 | 1119.12 7 | 1853.97 8 | 1261.73 9 | 1146.64 10 | 1100.86 11 | 1048.33 12 | 1137.92 13 | 1122.53 14 | 1137.56 15 | 1160.66 16 | 1134.72 17 | 1079.63 18 | 1821.05 19 | 1154.46 20 | 1148.07 21 | 1181.77 22 | 1087.85 23 | 1314.45 24 | 1097.17 25 | 1001.58 26 | 1104.57 27 | 1234.3 28 | 1196.33 29 | 1136.94 30 | 1281.04 31 | 1164.76 32 | 1243.45 33 | 1286.32 34 | 1145.69 35 | 1765.33 36 | 1193.15 37 | 1154.92 38 | 1219.87 39 | 1252.62 40 | 1122.34 41 | 1105.96 42 | 1159.38 43 | 1950.13 44 | 1136.23 45 | 1140.9 46 | 1138.38 47 | 1131.4 48 | 1133.16 49 | 1065.24 50 | 1165.19 51 | 1255.2 52 | 1132.78 53 | 1030.69 54 | 1182.46 55 | 1111.88 56 | 1166.47 57 | 1180.31 58 | 1917.36 59 | 1699.53 60 | 1906.08 61 | 1305.55 62 | 1140.41 63 | 1199.44 64 | 1194.16 65 | 1177.68 66 | 1088.34 67 | 1094.79 68 | 1181.15 69 | 1108.64 70 | 1181.18 71 | 1132.18 72 | 1116.36 73 | 1170.12 74 | 1161.66 75 | 1117.25 76 | 1181.51 77 | 1172.09 78 | 1160.19 79 | 1134.86 80 | 1915.37 81 | 1128.38 82 | 1313.9 83 | 1575.47 84 | 1177.84 85 | 1378.93 86 | 1484.58 87 | 1134.86 88 | 1926.01 89 | 1207.03 90 | 1289.15 91 | 1150.42 92 | 1094.42 93 | 1113.52 94 | 1196.05 95 | 1273.66 96 | 2102.8 97 | 1108.26 98 | 1216.46 99 | 1875.03 100 | 1076.07 101 | -------------------------------------------------------------------------------- /C++/Opencv/Output/obstacle_type2_input1.txt: -------------------------------------------------------------------------------- 1 | 59167 3781.1 2 | -------------------------------------------------------------------------------- /C++/Opencv/Output/obstacle_type2_input2.txt: -------------------------------------------------------------------------------- 1 | 4141 1498.47 2 | -------------------------------------------------------------------------------- /C++/Opencv/Output/obstacle_type2_input3.txt: -------------------------------------------------------------------------------- 1 | 1955 921.975 2 | -------------------------------------------------------------------------------- /C++/Opencv/Output/obstacle_type2_input4.txt: -------------------------------------------------------------------------------- 1 | 2347 1150.48 2 | -------------------------------------------------------------------------------- /C++/Opencv/Output/obstacle_type4_input3.txt: -------------------------------------------------------------------------------- 1 | 8440 633.811 2 | 7269 634.114 3 | 6462 630.947 4 | 7326 635.729 5 | 6479 642.999 6 | 6017 630.404 7 | 6635 630.046 8 | 7539 633.15 9 | 8440 637.391 10 | 7245 689.186 11 | 6820 692.428 12 | 6710 663.826 13 | 7652 703.803 14 | 5681 636.784 15 | 6197 637.924 16 | 6242 635.763 17 | 5638 635.616 18 | 6261 641.808 19 | 6655 638.441 20 | 6686 636.537 21 | 6544 631.173 22 | 7019 640.719 23 | 8848 635.447 24 | 5838 638.004 25 | 6815 638.398 26 | 6902 660.12 27 | 7149 688.062 28 | 6298 636.429 29 | 6034 634.953 30 | 6728 655.467 31 | 6974 643.145 32 | 6849 634.353 33 | 5663 634.103 34 | 7191 633.145 35 | 6824 631.676 36 | 6342 639.46 37 | 6320 638.102 38 | 6651 633.357 39 | 6468 628.245 40 | 6667 647.241 41 | 6192 640.91 42 | 6654 632.901 43 | 7398 633.802 44 | 6518 640.3 45 | 6195 629.031 46 | 6570 631.692 47 | 6062 638.356 48 | 5386 645.138 49 | 6864 632.858 50 | 6069 634.998 51 | 6421 633.661 52 | 6889 633.49 53 | 7014 633.137 54 | 6554 631.858 55 | 6024 638.347 56 | 5758 633.807 57 | 5927 631.119 58 | 6592 637.859 59 | 6520 637.832 60 | 6663 641.879 61 | 6502 639.964 62 | 6029 638.089 63 | 7407 644.186 64 | 6204 636.657 65 | 5819 635.857 66 | 6904 634.678 67 | 6253 688.648 68 | 8992 633.04 69 | 8371 671.446 70 | 7312 645.318 71 | -------------------------------------------------------------------------------- /C++/Opencv/Output/obstacle_type5_input1.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/Output/obstacle_type5_input1.txt -------------------------------------------------------------------------------- /C++/Opencv/Output/obstacle_type5_input2.txt: -------------------------------------------------------------------------------- 1 | 4422 1505.54 2 | 3976 1448.58 3 | 6331 2211.59 4 | 4069 1391.86 5 | 4336 1510.39 6 | 4487 1585.91 7 | 4371 1544.52 8 | 3952 1389.33 9 | 5092 1713.9 10 | 4119 1419.9 11 | 4928 1638.31 12 | -------------------------------------------------------------------------------- /C++/Opencv/RRT-star-Implementation.h: -------------------------------------------------------------------------------- 1 | #ifndef RRTSTAR 2 | #define RRTSTAR 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "KdTree.h" 9 | #include "RRT.h" 10 | #define EPSILON 30 11 | #define RADIUS 35 12 | #define PI 3.14159265358979323846 13 | using namespace std; 14 | using namespace cv; 15 | 16 | class workspace { 17 | public: 18 | Mat image, visibleimage, treeimage, tempimage; 19 | int sx, sy, dx, dy, cx, cy; 20 | bool flag; 21 | node *RRTree , *TempRRT; 22 | KDTree *Points, *TempPoints; 23 | bool goalFound, tempGoalFound; 24 | node* goalNode, *tempGoalNode; 25 | vector tempPointsarr; 26 | vector archievedTree; 27 | vector > steps; 28 | vector > points; 29 | 30 | workspace(); 31 | void getSourceandGoal(); 32 | void start(); 33 | bool checkifgoalFound(int &x, int &y); 34 | bool checkBoundaries(int &x, int &y); 35 | void addConnections(int &x, int &y); 36 | void generatePoint(int &x, int &y); 37 | bool obstacleFree(int &x, int &y); 38 | void stepto(int &x1, int &y1, int &x2, int &y2); 39 | bool obstacleFreeEdge(int &x1, int &y1, int &x2, int &y2); 40 | bool check_same(int &x1, int &y1, int &x2, int &y2); 41 | long double dist(int &x1, int &y1, int &x2, int &y2); 42 | void createLink(node* childlink, node* parentlink); 43 | void growRRT(); 44 | void SimpleRRT(); 45 | void GoalBiasedBlackRRT(); 46 | void markVisible(); 47 | void findNearestObstacle(int &x, int &y, long double &angle); 48 | void printWholeTree(); 49 | bool check_for_black(int &x1, int &y1, int &x2, int &y2); 50 | bool check_for_gray(int &x, int &y); 51 | void generateGoalBiasedPoint(int &x, int &y); 52 | bool checkForTempPoints(int &x, int &y); 53 | void updateRoot(node* root); 54 | void removeGeneratedLeafNodes(); 55 | double printFinalPath(); 56 | bool obstacleFreeGoalBiased(int &x, int &y); 57 | bool obstacleFreeEdgeGoalBiased(int &x1, int &y1, int &x2, int &y2); 58 | void validateTree(); 59 | void goalRootedRRTStar(); 60 | node* deleteWholeSubtree(node* nde); 61 | void removeConnectionOfGeneratedNodes(); 62 | void useArchieveTree(); 63 | void printBlackSubtree(node* nde); 64 | void deleteBlackSubtree(node* nde); 65 | void takeNextStep(); 66 | void addConnectionGoalRooted(int &x, int &y); 67 | void createLinkGoalRooted(node *childlink, node * parentlink); 68 | bool nonObstacleFree(int &x, int &y); 69 | bool obstacleEdge(int &x, int &y, int &x1, int &y1); 70 | void generateGoalRootedPoint(int &x, int &y); 71 | bool checkTermination(int &x, int &y); 72 | node* validateTempTree(node* nde); 73 | node* deleteTempSubtree(node *nde); 74 | void findNewRootNode(); 75 | bool checkIfGoalVisible(); 76 | bool checkWithinRadius(int &x, int &y); 77 | void printTempTree(); 78 | void beforePrintTempTree(); 79 | void deleteTempPoints(); 80 | void getSnG(int sx, int sy, int dx, int dy); 81 | void cleanEveryThing(); 82 | void type1(); 83 | void type2(); 84 | void type3(); 85 | void type4(); 86 | void type5(); 87 | }; 88 | #endif // !RRTSTAR -------------------------------------------------------------------------------- /C++/Opencv/RRT.cpp: -------------------------------------------------------------------------------- 1 | #include "RRT.h" 2 | 3 | using namespace std; 4 | 5 | node::node(int &x, int &y, long double cost, node* parent, vector &children) { 6 | this->x = x; 7 | this->y = y; 8 | this->cost = cost; 9 | this->parent = parent; 10 | this->children = children; 11 | } 12 | 13 | void node::add_child(node* node1) { 14 | this->children.push_back(node1); 15 | } 16 | 17 | long double dist(int &x1, int &y1, int &x2, int &y2) { 18 | return sqrt((x1-x2)*(x1-x2)*1.0 + (y1-y2)*(y1-y2)*1.0); 19 | } 20 | 21 | void node::propogateCost(void) { 22 | queue q; 23 | q.push(this); 24 | node * cur= NULL; 25 | while (!q.empty()) { 26 | cur = q.front(); 27 | q.pop(); 28 | for (int i = 0; i < cur->children.size(); i++) { 29 | //if (cur->children[i]->cost > cur->cost + dist(cur->x, cur->y, cur->children[i]->x, cur->children[i]->y)) { 30 | cur->children[i]->cost = cur->cost + dist(cur->x, cur->y, cur->children[i]->x, cur->children[i]->y); 31 | q.push(cur->children[i]); 32 | //} 33 | } 34 | } 35 | } -------------------------------------------------------------------------------- /C++/Opencv/RRT.h: -------------------------------------------------------------------------------- 1 | #ifndef RRTHeader 2 | #define RRTHeader 3 | 4 | #include 5 | #define vi vector 6 | 7 | using namespace std; 8 | 9 | class node { 10 | public: 11 | int x, y; 12 | long double cost; 13 | node *parent; 14 | vector children; 15 | 16 | node(int &x, int &y, long double cost, node* parent, vector &children); 17 | void add_child(node* node1); 18 | void propogateCost(void); 19 | }; 20 | #endif // !RRTHeader 21 | -------------------------------------------------------------------------------- /C++/Opencv/Source.cpp: -------------------------------------------------------------------------------- 1 | #include "KdTree.h" 2 | 3 | 4 | int main() { 5 | int n, d, x; 6 | cin >> n >> d; 7 | KDTree *tree = NULL; 8 | for (int i = 0; i < n; i++) { 9 | vector pnt; 10 | for (int j = 0; j < d; j++) { 11 | cin >> x; 12 | pnt.push_back(x); 13 | } 14 | if (tree == NULL) { 15 | tree = new KDTree(pnt, d, 0, NULL, NULL); 16 | } 17 | else { 18 | tree->insert(pnt); 19 | } 20 | } 21 | tree->printTree(); 22 | } 23 | -------------------------------------------------------------------------------- /C++/Opencv/obstacle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/obstacle.png -------------------------------------------------------------------------------- /C++/Opencv/test1.txt: -------------------------------------------------------------------------------- 1 | 18355 1446.97 2 | -------------------------------------------------------------------------------- /C++/Opencv/test_opencv.cpp: -------------------------------------------------------------------------------- 1 | #include "RRT-star-Implementation.h" 2 | 3 | 4 | int main() { 5 | ifstream inp; 6 | inp.open("Input/obstacle.txt"); 7 | int x1, y1, x2, y2; 8 | string str = "Output/obstacle_type5_input"; 9 | int J = 1; 10 | while (inp >> x1 >> y1 >> x2 >> y2) { 11 | cout < C:\Users\Bhaskar\documents\visual studio 2015\Projects\Opencv\x64\Debug\Opencv.exe 16 | Opencv.vcxproj -> C:\Users\Bhaskar\documents\visual studio 2015\Projects\Opencv\x64\Debug\Opencv.pdb (Full PDB) 17 | -------------------------------------------------------------------------------- /C++/Opencv/x64/Debug/Opencv.tlog/CL.command.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/x64/Debug/Opencv.tlog/CL.command.1.tlog -------------------------------------------------------------------------------- /C++/Opencv/x64/Debug/Opencv.tlog/CL.read.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/x64/Debug/Opencv.tlog/CL.read.1.tlog -------------------------------------------------------------------------------- /C++/Opencv/x64/Debug/Opencv.tlog/CL.write.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/x64/Debug/Opencv.tlog/CL.write.1.tlog -------------------------------------------------------------------------------- /C++/Opencv/x64/Debug/Opencv.tlog/Opencv.lastbuildstate: -------------------------------------------------------------------------------- 1 | #TargetFrameworkVersion=v4.0:PlatformToolSet=v140:EnableManagedIncrementalBuild=false:VCToolArchitecture=Native32Bit:WindowsTargetPlatformVersion=8.1 2 | Debug|x64|C:\Users\Bhaskar\documents\visual studio 2015\Projects\Opencv\| 3 | -------------------------------------------------------------------------------- /C++/Opencv/x64/Debug/Opencv.tlog/link.command.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/x64/Debug/Opencv.tlog/link.command.1.tlog -------------------------------------------------------------------------------- /C++/Opencv/x64/Debug/Opencv.tlog/link.read.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/x64/Debug/Opencv.tlog/link.read.1.tlog -------------------------------------------------------------------------------- /C++/Opencv/x64/Debug/Opencv.tlog/link.write.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/x64/Debug/Opencv.tlog/link.write.1.tlog -------------------------------------------------------------------------------- /C++/Opencv/x64/Debug/RRT-star-Implementation.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/x64/Debug/RRT-star-Implementation.obj -------------------------------------------------------------------------------- /C++/Opencv/x64/Debug/RRT.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/x64/Debug/RRT.obj -------------------------------------------------------------------------------- /C++/Opencv/x64/Debug/test_opencv.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/x64/Debug/test_opencv.obj -------------------------------------------------------------------------------- /C++/Opencv/x64/Debug/vc140.idb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/x64/Debug/vc140.idb -------------------------------------------------------------------------------- /C++/Opencv/x64/Debug/vc140.pdb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/Opencv/x64/Debug/vc140.pdb -------------------------------------------------------------------------------- /C++/README.md: -------------------------------------------------------------------------------- 1 | This module is an adaptation of RRT/RRT* using KDTrees. 2 | This is C++ implementation of following type: 3 | 4 | 1) RRT\* with Simple RRT(Highly goal biased).
5 | 2) RRT\* with archived RRT(Highly goal biased).
6 | 3) RRT\* with RRT\*.
7 | 4) RRT\* with goal Rooted RRT\*.
8 | 9 | The name of main algorithm is the result of process. Looking for a new name. 10 | For any queries regarding the implementation or discussion on different things like complexity and scalability feel free to contact at iiita.coder@gmail.com 11 | -------------------------------------------------------------------------------- /C++/x64/Debug/Opencv.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/x64/Debug/Opencv.exe -------------------------------------------------------------------------------- /C++/x64/Debug/Opencv.ilk: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/x64/Debug/Opencv.ilk -------------------------------------------------------------------------------- /C++/x64/Debug/Opencv.pdb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/C++/x64/Debug/Opencv.pdb -------------------------------------------------------------------------------- /Goal_biased_RRT.py: -------------------------------------------------------------------------------- 1 | from kdTree import kdTree 2 | from kdTree import node 3 | import sys, random, math, pygame ,time 4 | from pygame.locals import * 5 | from math import sqrt,cos,sin,atan2 6 | 7 | XDIM = 640 8 | YDIM = 480 9 | WINSIZE = [XDIM, YDIM] 10 | EPSILON = 10.0 11 | dim = 2 12 | p = 10 13 | 14 | class RRTAlgorithm: 15 | def __init__(self, source, goal, nodes): #initial and destination coordinates and number of nodes 16 | print source, goal 17 | self.start(source, goal, nodes) 18 | 19 | def generateRandom(self): 20 | return [random.random() * 640.0, random.random() * 480.0] 21 | 22 | def getPoints(self, goal): 23 | if 100.0*random.random() < p: 24 | return goal 25 | else: 26 | return self.generateRandom() 27 | 28 | def start(self, source, goal, nodes): 29 | pygame.init() 30 | screen = pygame.display.set_mode(WINSIZE) 31 | pygame.display.set_caption('RRT brute force') 32 | white = 100, 100, 100 33 | black = 20, 20, 40 34 | bright = 255, 255, 255 35 | screen.fill(black) 36 | 37 | RRTree = node(source, [], None, True) #actual RRTree 38 | Points = kdTree(None, None, 0, source, RRTree) #for storing generated points to increase the search complexity 39 | current = source 40 | 41 | while not self.check(current, goal): 42 | rand = self.getPoints(goal) 43 | 44 | ret = Points.search(rand, 100000000000000, None, None, None, None, None) 45 | nearest_neighbour = ret[1] 46 | new_point = self.step_from_to(nearest_neighbour, rand) 47 | #print rand, nearest_neighbour, new_point 48 | #time.sleep(0.1) 49 | nde = node(new_point, [], ret[2], True) 50 | ret[2].add_child(nde) 51 | Points.insert(new_point, dim, nde) 52 | current = new_point 53 | pnt = [int(new_point[0]), int (new_point[1])] 54 | pygame.draw.line(screen, white, nearest_neighbour , new_point) 55 | #pygame.draw.circle(screen, (255, 0, 0), pnt, 1) 56 | pygame.display.update() 57 | for e in pygame.event.get(): 58 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 59 | sys.exit("Leaving .") 60 | 61 | ret = Points.search(current, 100000000000000000000, None, None, None, None, None) 62 | nde = ret[2] 63 | 64 | while nde.parent != None: 65 | pygame.draw.line(screen, bright, nde.point, nde.parent.point) 66 | pygame.display.update() 67 | nde = nde.parent 68 | time.sleep(0.1) 69 | for e in pygame.event.get(): 70 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 71 | sys.exit("Leaving.") 72 | 73 | def check(self, point , goal): # checking if currently added node is at goal or not 74 | if point[0] > goal[0]-5 and point[0] < goal[0]+5 and point[1] > goal[1]-5 and point[1] < goal[1]+5: 75 | return True 76 | return False 77 | 78 | def dist(self, p1, p2): #returns euclid's distance between points p1 and p2 79 | return sqrt((p1[0] - p2[0]) * (p1[0] - p2[0]) + (p1[1] - p2[1]) * (p1[1] - p2[1])) 80 | 81 | def step_from_to(self,p1, p2): #returns point with at most epsilon distance from nearest neighbour in the direction of randomly generated point 82 | if self.dist(p1, p2) < EPSILON: 83 | return p2 84 | else: 85 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 86 | return [p1[0] + EPSILON * cos(theta), p1[1] + EPSILON * sin(theta)] 87 | 88 | def main(): 89 | tree = RRTAlgorithm([320, 240], [5, 5], 2000) 90 | 91 | main() -------------------------------------------------------------------------------- /Images/img.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/Images/img.bmp -------------------------------------------------------------------------------- /Images/img1.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/Images/img1.bmp -------------------------------------------------------------------------------- /Images/img2.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/Images/img2.bmp -------------------------------------------------------------------------------- /Images/img3.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/Images/img3.bmp -------------------------------------------------------------------------------- /Images/obstacle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/Images/obstacle.png -------------------------------------------------------------------------------- /Images/obstacle1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Rapidly-Exploring-Random-Tree-Star-Scan/7c566bf2423e322bc473fecbef88c7f70ec4dc28/Images/obstacle1.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Rapidly-Exploring-Random-Tree 2 | 3 | Hello folks, 4 | this is an python implementation of Rapidly Exploring Random Trees (RRT) algorithm in motion planning. RRT is one of the robust and scalable algorithm which is fast enough to solve in given query in static search space.I have included multiple files to visualize different types of RRT, but main aim to create a robust and scalable RRT with the help of visible roadmaps. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /RRTStar_Goal_Biased.py: -------------------------------------------------------------------------------- 1 | from kdTree import kdTree 2 | from kdTree import node 3 | import sys, random, pygame ,time 4 | from pygame.locals import * 5 | from math import sqrt,cos,sin,atan2, hypot 6 | import kdtree 7 | import numpy as np 8 | 9 | XDIM = 640 10 | YDIM = 480 11 | WINSIZE = [XDIM, YDIM] 12 | EPSILON = 50 13 | NUMNODES = 5000 14 | dim = 2 15 | RADIUS = 20 16 | p = 2 17 | 18 | class RRTAlgorithm(object): 19 | def __init__(self, source, goal, nodes): #initial and destination coordinates and number of nodes 20 | print source, goal 21 | self.costMap = [[10000000000 for x in range(YDIM)] for x in range(XDIM)] 22 | self.path = None 23 | self.goalNode = None 24 | self.start(source, goal, nodes) 25 | 26 | def addConnection1(self, Points, rand, screen): 27 | 28 | ret = Points.search(rand, 100000000000000, None, None, None, None, None) 29 | nearest_neighbour = ret[1] 30 | new_point = self.step_from_to(nearest_neighbour, rand) 31 | nde = node(new_point, [], ret[2], True, ret[2].cost + hypot(new_point[0] - ret[1][0], new_point[1] - ret[1][1])) 32 | ret[2].add_child(nde) 33 | Points.insert(new_point, dim, nde) 34 | current = new_point 35 | new_point = [int(new_point[0]), int(new_point[1])] 36 | pygame.draw.line(screen, (100, 100, 100), nearest_neighbour, new_point) 37 | pygame.display.update() 38 | return new_point 39 | pass 40 | 41 | def generatePoints(self, Pointmap): 42 | prob = 100.0*random.random() 43 | if prob < p: 44 | return self.goal 45 | else: 46 | rand = [int(random.random() * 640.0), int(random.random() * 480.0)] 47 | while Pointmap[rand[0]][rand[1]] == 1: 48 | rand = [int(random.random() * 640.0), int(random.random() * 480.0)] 49 | return rand 50 | 51 | def sortdist(self, n): 52 | return n[1] 53 | 54 | def updateSubtreCost(self, nde): 55 | for i in nde.children: 56 | if self.costMap[i.point[0]][ i.point[1]] < self.costMap[nde.point[0]][ nde.point[1]] + hypot(i.point[0]-nde.point[0], i.point[1]-nde.point[1]): 57 | print "yes" 58 | self.costMap[i.point[0]][ i.point[1]] = self.costMap[nde.point[0]][ nde.point[1]] + hypot(i.point[0]-nde.point[0], i.point[1]-nde.point[1]) 59 | self.updateSubtreCost(i) 60 | 61 | def createNewLink(self, childlink, parentlink, screen): 62 | 63 | oldParent = childlink.parent 64 | oldParent.children.remove(childlink) 65 | childlink.parent = parentlink 66 | parentlink.children.append(childlink) 67 | childlink.propogateCost() 68 | #self.updateSubtreCost(childlink) 69 | 70 | pygame.draw.line(screen, (20, 20, 40), oldParent.point, childlink.point) 71 | pygame.draw.line(screen, (100, 100, 100), parentlink.point, childlink.point) 72 | 73 | def findKNN(self, point, Pointmap): 74 | start = [int(point[0]-RADIUS), int(point[1]-RADIUS)] 75 | end = [int(point[0] + RADIUS), int(point[1] + RADIUS)] 76 | if start[0] < 0: 77 | start[0] = 0 78 | if start[1] < 0: 79 | start[1] = 0 80 | if end[0] >= XDIM: 81 | end[0] = XDIM-1 82 | if end[1] >= YDIM: 83 | end[1] = YDIM-1 84 | ret = [] 85 | 86 | for i in range(start[0], end[0]): 87 | for j in range(start[1], end[1]): 88 | if Pointmap[i][j] == 1: 89 | ret.append(([i, j], hypot(point[0]-i, point[1]-j))) 90 | return ret 91 | pass 92 | 93 | def addConnections(self, Points, Pointmap, new_point, screen, source): 94 | ret = Points.search(new_point, 1000000000000000000, None, None, None, None, None) 95 | nodes = [] 96 | 97 | nodes = Points.searchNN(new_point, RADIUS) 98 | #print len(nodes) 99 | flag = False 100 | for i in nodes: 101 | if ret[1] == i[0]: 102 | flag = True 103 | break 104 | if not flag: 105 | nodes.append((ret[1], ret[0])) 106 | 107 | sorted(nodes, key = self.sortdist) 108 | 109 | nn = nodes[0][0] 110 | #print 'nn ', nn 111 | cost = [] 112 | for i in nodes: 113 | cost.append(Points.search(i[0], 100000000000000000000, None, None, None, None, None)[2].cost) 114 | mincost = 10000000000000000000000000 115 | for i in range(len(nodes)): 116 | nn1 = nodes[i][0] 117 | if cost[i] + nodes[i][1] < mincost: 118 | mincost = cost[i] + nodes[i][1] 119 | nn = nn1 120 | pnt = nn 121 | ret = Points.search(pnt, 100000000000000, None, None, None, None, None) 122 | nearest_neighbour = ret[1] 123 | new_point = self.step_from_to(nearest_neighbour, new_point) 124 | new_point = [int(new_point[0]), int(new_point[1])] 125 | Pointmap[new_point[0]][new_point[1]] = 1 126 | pygame.draw.line(screen, (100, 100, 100), nn, new_point) 127 | pygame.display.update() 128 | c = mincost 129 | nde = node(new_point, [], ret[2], True, c) 130 | ret[2].add_child(nde) 131 | Points.insert(new_point, 2, nde) 132 | 133 | ''' 134 | 135 | Update Other links 136 | 137 | ''' 138 | 139 | flag = False 140 | # if self.goalNode !=None: 141 | # flag = True 142 | if self.path != None: 143 | for i in nodes: 144 | pnt1 = i[0] 145 | if pnt != pnt1 and pnt1 in self.path: 146 | flag = True 147 | if flag: 148 | nde1 = self.goalNode 149 | while nde1.parent != None: 150 | pygame.draw.line(screen, (100, 100, 100), nde1.point, nde1.parent.point) 151 | nde1 = nde1.parent 152 | #print "rubbed" 153 | pygame.display.update() 154 | #time.sleep(10) 155 | 156 | for i in range(len(nodes)): 157 | pnt = nodes[i][0] 158 | if pnt != nearest_neighbour and pnt != source: 159 | if cost[i] > c + hypot(pnt[0]-new_point[0], pnt[1]-new_point[1]) and hypot(pnt[0]-new_point[0], pnt[1]-new_point[1]) < EPSILON: 160 | child = Points.search(pnt, 10000000000000000000000000, None, None, None, None, None)[2] 161 | child.cost = c + hypot(pnt[0] - new_point[0], pnt[1] - new_point[1]) 162 | self.createNewLink(child, nde, screen) 163 | 164 | if flag: 165 | nodes = Points.searchNN(self.goal, 10) 166 | sorted(nodes, key=self.sortdist) 167 | pnt = nodes[0][0] 168 | self.path = [] 169 | nde = self.goalNode = Points.search(pnt, 1000000000000000000, None, None, None, None, None)[2] 170 | while nde.parent != None: 171 | pygame.draw.line(screen, (255, 255, 255), nde.point, nde.parent.point) 172 | self.path.append(nde.point) 173 | #print "point ", nde.point 174 | nde = nde.parent 175 | pygame.display.update() 176 | return new_point 177 | 178 | def start(self, source, goal, nodes): 179 | self.source = source 180 | self.goal = goal 181 | pygame.init() 182 | screen = pygame.display.set_mode(WINSIZE) 183 | pygame.display.set_caption('RRT star using KdTrees') 184 | white = 100, 100, 100 185 | black = 20, 20, 40 186 | bright = 255, 255, 255 187 | screen.fill(black) 188 | 189 | RRTree = node(source, [], None, True) #actual RRTree 190 | Points = kdTree(None, None, 0, source, RRTree) #for storing generated points to increase the search complexity 191 | NearestNeighbourTree = kdtree.create([source]) # for storing points same as Points, using it for finding points in a range 192 | current = source 193 | Pointmap = [[0 for i in range (YDIM)] for i in range(XDIM)] 194 | Pointmap[source[0]][source[1]] = 1 195 | 196 | count = 0 197 | 198 | while not self.check(current, goal): 199 | 200 | rand = self.generatePoints(Pointmap) 201 | current = self.addConnections(Points, Pointmap, rand, screen, source) 202 | count = count + 1 203 | #current = self.addConnection1(Points, rand, screen) 204 | 205 | pygame.display.update() 206 | for e in pygame.event.get(): 207 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 208 | sys.exit("Leaving .") 209 | 210 | ret = Points.search(current, 100000000000000000000, None, None, None, None, None) 211 | nde = ret[2] 212 | self.goalNode = nde 213 | path = [] 214 | while nde.parent != None: 215 | print nde.point, nde.cost 216 | path.append(nde.point) 217 | pygame.draw.line(screen, bright, nde.point, nde.parent.point) 218 | pygame.display.update() 219 | nde = nde.parent 220 | if nde.parent == nde: 221 | break 222 | time.sleep(0.05) 223 | for e in pygame.event.get(): 224 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 225 | sys.exit("Leaving.") 226 | print 'count', count 227 | self.path = path 228 | for i in range(10000-count): 229 | rand = self.generatePoints(Pointmap) 230 | current = self.addConnections(Points, Pointmap, rand, screen, source) 231 | pygame.display.update() 232 | for e in pygame.event.get(): 233 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 234 | sys.exit("Leaving.") 235 | 236 | def printchildren(self, nde): 237 | print nde.point, nde.cost 238 | for i in nde.children: 239 | print i.point, i.cost 240 | 241 | def check(self, point , goal): # checking if currently added node is at goal or not 242 | if point[0] > goal[0]-5 and point[0] < goal[0]+5 and point[1] > goal[1]-5 and point[1] < goal[1]+5: 243 | return True 244 | return False 245 | 246 | def dist(self, p1, p2): #returns euclid's distance between points p1 and p2 247 | return sqrt((p1[0] - p2[0]) * (p1[0] - p2[0]) + (p1[1] - p2[1]) * (p1[1] - p2[1])) 248 | 249 | def step_from_to(self,p1, p2): #returns point with at most epsilon distance from nearest neighbour in the direction of randomly generated point 250 | if self.dist(p1, p2) < EPSILON: 251 | return p2 252 | else: 253 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 254 | return [p1[0] + EPSILON * cos(theta), p1[1] + EPSILON * sin(theta)] 255 | 256 | def main(): 257 | tree = RRTAlgorithm([635, 475], [20, 40], 2000) 258 | 259 | main() -------------------------------------------------------------------------------- /RRTStar_Obstacles.py: -------------------------------------------------------------------------------- 1 | from kdTree import kdTree 2 | from kdTree import node 3 | import sys, random, pygame ,time 4 | from pygame.locals import * 5 | from math import sqrt,cos,sin,atan2, hypot 6 | #import kdtree 7 | import numpy as np 8 | import cv2 9 | 10 | import sys 11 | sys.setrecursionlimit(1500) 12 | 13 | XDIM ,YDIM = 0, 0 14 | WINSIZE = [XDIM, YDIM] 15 | EPSILON = 10 16 | NUMNODES = 5000 17 | dim = 2 18 | RADIUS = 20 19 | p = 10 20 | flag = False 21 | 22 | class RRTAlgorithm(object): 23 | 24 | def __init__(self): #initial and destination coordinates and number of nodes 25 | self.getSourceAndGoal() 26 | global XDIM, YDIM 27 | XDIM, YDIM, z = img.shape 28 | print XDIM, YDIM 29 | self.costMap = [[10000000000 for x in range(YDIM)] for x in range(XDIM)] 30 | self.path = None 31 | self.goalNode = None 32 | self.start() 33 | 34 | def checkBoundaries(self, point): 35 | x, y, z = img.shape 36 | if point[0] >= 0 and point[1] >= 0 and point[0] < y and point[1] < x: 37 | return True 38 | return False 39 | 40 | def checkforObstacles(self, p1, p2): 41 | if img[p1[1]][p1[0]][0] == 255 or img[int((p1[1]+p2[1])/2)][int((p1[0]+p2[0])/2)][0] == 255: 42 | return True 43 | else: 44 | return False 45 | 46 | def generatePoints(self): 47 | x, y, z = img.shape 48 | prob = 100.0*random.random() 49 | if prob < p: 50 | return self.goal 51 | else: 52 | return [int(random.random() * (y-1)*1.0), int(random.random() * (x-1)*1.0)] 53 | 54 | def sortdist(self, n): 55 | return n[1] 56 | 57 | def createNewLink(self, childlink, parentlink): 58 | 59 | pnt = childlink.point 60 | while True: 61 | childlink.propogateCost() 62 | oldParent = childlink.parent 63 | oldParent.children.remove(childlink) 64 | childlink.parent = parentlink 65 | parentlink.children.append(childlink) 66 | if oldParent.cost > childlink.cost + self.dist(oldParent.point, childlink.point): 67 | oldParent.cost = childlink.cost + self.dist(oldParent.point, childlink.point) 68 | if pnt == childlink.point: 69 | cv2.line(img, tuple(parentlink.point), tuple(childlink.point), (100, 100, 100), 1) 70 | parentlink = childlink 71 | childlink = oldParent 72 | 73 | else: 74 | cv2.line(img, tuple(oldParent.point), tuple(childlink.point), (0, 0, 0), 1) 75 | break 76 | 77 | def dist(self, p1, p2): 78 | return hypot(p1[0]-p2[0], p1[1]-p2[1]) 79 | 80 | def addConnections(self, Points, source): 81 | new_point = self.generatePoints() 82 | # print new_point 83 | ret = Points.search(new_point, 1000000000000000000, None, None, None, None, None) 84 | nodes = [] 85 | 86 | nodes = Points.searchNN(new_point, RADIUS) 87 | # print len(nodes) 88 | flag = False 89 | for i in nodes: 90 | if ret[1] == i[0]: 91 | flag = True 92 | break 93 | if not flag: 94 | nodes.append((ret[1], ret[0])) 95 | 96 | sorted(nodes, key=self.sortdist) 97 | 98 | nn = nodes[0][0] 99 | # print 'nn ', nn 100 | cost = [] 101 | for i in nodes: 102 | cost.append(Points.search(i[0], 100000000000000000000, None, None, None, None, None)[2].cost) 103 | mincost = 10000000000000000000000000 104 | for i in range(len(nodes)): 105 | nn1 = nodes[i][0] 106 | if cost[i] + nodes[i][1] < mincost: 107 | mincost = cost[i] + nodes[i][1] 108 | nn = nn1 109 | pnt = nn 110 | ret = Points.search(pnt, 100000000000000, None, None, None, None, None) 111 | nearest_neighbour = ret[1] 112 | new_point = self.step_from_to(nearest_neighbour, new_point) 113 | 114 | new_point = [int(new_point[0]), int(new_point[1])] 115 | 116 | while self.checkforObstacles(new_point, nearest_neighbour): 117 | new_point = self.generatePoints() 118 | #print new_point 119 | ret = Points.search(new_point, 1000000000000000000, None, None, None, None, None) 120 | nodes = [] 121 | 122 | nodes = Points.searchNN(new_point, RADIUS) 123 | #print len(nodes) 124 | flag = False 125 | for i in nodes: 126 | if ret[1] == i[0]: 127 | flag = True 128 | break 129 | if not flag: 130 | nodes.append((ret[1], ret[0])) 131 | 132 | sorted(nodes, key = self.sortdist) 133 | 134 | nn = nodes[0][0] 135 | #print 'nn ', nn 136 | cost = [] 137 | for i in nodes: 138 | cost.append(Points.search(i[0], 100000000000000000000, None, None, None, None, None)[2].cost) 139 | mincost = 10000000000000000000000000 140 | for i in range(len(nodes)): 141 | nn1 = nodes[i][0] 142 | if cost[i] + nodes[i][1] < mincost: 143 | mincost = cost[i] + nodes[i][1] 144 | nn = nn1 145 | pnt = nn 146 | ret = Points.search(pnt, 100000000000000, None, None, None, None, None) 147 | nearest_neighbour = ret[1] 148 | new_point = self.step_from_to(nearest_neighbour, new_point) 149 | 150 | new_point = [int(new_point[0]), int(new_point[1])] 151 | #Pointmap[new_point[0]][new_point[1]] = 1 152 | cv2.line(img, tuple(nn), tuple(new_point), (100, 100, 100), 1) 153 | c = mincost 154 | nde = node(new_point, [], ret[2], True, c) 155 | ret[2].add_child(nde) 156 | Points.insert(new_point, 2, nde) 157 | 158 | ''' 159 | 160 | Update Other links 161 | 162 | ''' 163 | 164 | flag = False 165 | if self.goalNode !=None: 166 | flag = True 167 | if self.path != None: 168 | for i in nodes: 169 | pnt1 = i[0] 170 | if pnt != pnt1 and pnt1 in self.path: 171 | flag = True 172 | if flag: 173 | nde1 = self.goalNode 174 | while nde1.parent != None: 175 | cv2.line(img, tuple(nde1.point), tuple(nde1.parent.point), (100, 100, 100), 1) 176 | nde1 = nde1.parent 177 | #print "rubbed" 178 | #time.sleep(10) 179 | #cv2.imshow('rubbed', img) 180 | #cv2.waitKey(0) 181 | 182 | for i in range(len(nodes)): 183 | pnt = nodes[i][0] 184 | if pnt != nearest_neighbour and pnt != source: 185 | if cost[i] > c + hypot(pnt[0]-new_point[0], pnt[1]-new_point[1]) and hypot(pnt[0]-new_point[0], pnt[1]-new_point[1]) < EPSILON: 186 | child = Points.search(pnt, 10000000000000000000000000, None, None, None, None, None)[2] 187 | child.cost = c + hypot(pnt[0] - new_point[0], pnt[1] - new_point[1]) 188 | self.createNewLink(child, nde) 189 | 190 | if flag: 191 | nodes = Points.searchNN(self.goal, 10) 192 | sorted(nodes, key=self.sortdist) 193 | pnt = nodes[0][0] 194 | self.path = [] 195 | nde = self.goalNode = Points.search(pnt, 1000000000000000000, None, None, None, None, None)[2] 196 | while nde.parent != None: 197 | cv2.line(img, tuple(nde.point), tuple(nde.parent.point), (200, 200,200), 1) 198 | self.path.append(nde.point) 199 | #print "point ", nde.point 200 | nde = nde.parent 201 | return new_point 202 | 203 | def start(self): 204 | white = 100, 100, 100 205 | black = 20, 20, 40 206 | bright = 255, 255, 255 207 | 208 | RRTree = node(self.source, [], None, True) #actual RRTree 209 | Points = kdTree(None, None, 0, self.source, RRTree) #for storing generated points to increase the search complexity 210 | 211 | current = self.source 212 | #Pointmap = [[0 for i in range (YDIM)] for i in range(XDIM)] 213 | #Pointmap[self.source[0]][self.source[1]] = 1 214 | 215 | count = 0 216 | 217 | while not self.check(current, self.goal): 218 | 219 | current = self.addConnections(Points, self.source) 220 | cv2.imshow('image', img) 221 | k = cv2.waitKey(1) 222 | count = count + 1 223 | #current = self.addConnection1(Points, rand, screen) 224 | 225 | ret = Points.search(current, 100000000000000000000, None, None, None, None, None) 226 | nde = ret[2] 227 | self.goalNode = nde 228 | path = [] 229 | while nde.parent != None: 230 | print nde.point, nde.cost 231 | path.append(nde.point) 232 | cv2.line(img, tuple(nde.point), tuple(nde.parent.point), (200, 200,200)) 233 | cv2.imshow('image', img) 234 | k = cv2.waitKey(1) 235 | nde = nde.parent 236 | 237 | if nde.parent == nde: 238 | break 239 | print 'count', count 240 | self.path = path 241 | for i in range(1000000-count): 242 | current = self.addConnections(Points, self.source) 243 | cv2.imshow('image', img) 244 | k = cv2.waitKey(1) 245 | 246 | def printchildren(self, nde): 247 | print nde.point, nde.cost 248 | for i in nde.children: 249 | print i.point, i.cost 250 | 251 | def check(self, point , goal): # checking if currently added node is at goal or not 252 | if point[0] > goal[0]-5 and point[0] < goal[0]+5 and point[1] > goal[1]-5 and point[1] < goal[1]+5: 253 | return True 254 | return False 255 | 256 | def dist(self, p1, p2): #returns euclid's distance between points p1 and p2 257 | return sqrt((p1[0] - p2[0]) * (p1[0] - p2[0]) + (p1[1] - p2[1]) * (p1[1] - p2[1])) 258 | 259 | def draw_circle(self, event, x, y, flags, param): 260 | global flag 261 | 262 | if event == cv2.EVENT_LBUTTONDBLCLK: 263 | # cv2.circle(img, (x, y), 100, (255, 0, 0), -1) 264 | if not flag: 265 | self.source = x, y 266 | print self.source 267 | flag = True 268 | else: 269 | self.goal = x, y 270 | print self.goal 271 | 272 | def getSourceAndGoal(self): 273 | cv2.namedWindow('image') 274 | cv2.setMouseCallback('image', self.draw_circle) 275 | cv2.imshow('image', img) 276 | cv2.waitKey(0) 277 | 278 | def step_from_to(self,p1, p2): #returns point with at most epsilon distance from nearest neighbour in the direction of randomly generated point 279 | if self.dist(p1, p2) < EPSILON: 280 | return p2 281 | else: 282 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 283 | return [p1[0] + EPSILON * cos(theta), p1[1] + EPSILON * sin(theta)] 284 | 285 | def main(): 286 | tree = RRTAlgorithm() 287 | 288 | img = cv2.imread('Images/test.png') 289 | 290 | main() -------------------------------------------------------------------------------- /RRT_KdTree.py: -------------------------------------------------------------------------------- 1 | from kdTree import kdTree 2 | from kdTree import node 3 | import sys, random, math, pygame ,time 4 | from pygame.locals import * 5 | from math import sqrt,cos,sin,atan2 6 | 7 | XDIM = 640 8 | YDIM = 480 9 | WINSIZE = [XDIM, YDIM] 10 | EPSILON = 20.0 11 | NUMNODES = 5000 12 | dim = 2 13 | 14 | class RRTAlgorithm(object): 15 | def __init__(self, source, goal, nodes): #initial and destination coordinates and number of nodes 16 | print source, goal 17 | self.start(source, goal, nodes) 18 | 19 | def start(self, source, goal, nodes): 20 | pygame.init() 21 | screen = pygame.display.set_mode(WINSIZE) 22 | pygame.display.set_caption('RRT brute force') 23 | white = 100, 100, 100 24 | black = 20, 20, 40 25 | bright = 255, 255, 255 26 | screen.fill(black) 27 | 28 | RRTree = node(source, [], None, True) #actual RRTree 29 | Points = kdTree(None, None, 0, source, RRTree) #for storing generated points to increase the search complexity 30 | current = source 31 | 32 | while not self.check(current, goal): 33 | rand = [random.random() * 640.0, random.random() * 480.0] 34 | 35 | ret = Points.search(rand, 100000000000000, None, None, None, None, None) 36 | nearest_neighbour = ret[1] 37 | new_point = self.step_from_to(nearest_neighbour, rand) 38 | #print rand, nearest_neighbour, new_point 39 | #time.sleep(0.1) 40 | nde = node(new_point, [], ret[2], True) 41 | ret[2].add_child(nde) 42 | Points.insert(new_point, dim, nde) 43 | current = new_point 44 | pnt = [int(new_point[0]), int (new_point[1])] 45 | pygame.draw.line(screen, white, nearest_neighbour , new_point) 46 | #pygame.draw.circle(screen, (255, 0, 0), pnt, 1) 47 | pygame.display.update() 48 | for e in pygame.event.get(): 49 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 50 | sys.exit("Leaving .") 51 | 52 | ret = Points.search(current, 100000000000000000000, None, None, None, None, None) 53 | nde = ret[2] 54 | 55 | while nde.parent != None: 56 | pygame.draw.line(screen, bright, nde.point, nde.parent.point) 57 | pygame.display.update() 58 | nde = nde.parent 59 | time.sleep(0.1) 60 | for e in pygame.event.get(): 61 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 62 | sys.exit("Leaving.") 63 | 64 | 65 | def check(self, point , goal): # checking if currently added node is at goal or not 66 | if point[0] > goal[0]-5 and point[0] < goal[0]+5 and point[1] > goal[1]-5 and point[1] < goal[1]+5: 67 | return True 68 | return False 69 | 70 | def dist(self, p1, p2): #returns euclid's distance between points p1 and p2 71 | return sqrt((p1[0] - p2[0]) * (p1[0] - p2[0]) + (p1[1] - p2[1]) * (p1[1] - p2[1])) 72 | 73 | def step_from_to(self,p1, p2): #returns point with at most epsilon distance from nearest neighbour in the direction of randomly generated point 74 | if self.dist(p1, p2) < EPSILON: 75 | return p2 76 | else: 77 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 78 | return [p1[0] + EPSILON * cos(theta), p1[1] + EPSILON * sin(theta)] 79 | 80 | def main(): 81 | tree = RRTAlgorithm([635, 475], [-5, -5], 2000) 82 | 83 | main() -------------------------------------------------------------------------------- /RRT_Scan.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | from math import pi, sin, cos, hypot, atan2 3 | import numpy as np 4 | from kdTree import kdTree 5 | from kdTree import node 6 | import random 7 | import copy 8 | from Queue import PriorityQueue 9 | import time 10 | 11 | #global Declarations 12 | sx, sy = -1, -1 13 | dx, dy = -1, -1 14 | flag = False 15 | EPSILON = 20.0 16 | NUMNODES = 5000 17 | dim = 2 18 | threshold = 5 #breaking condition of RRT loop 19 | 20 | class pnt(object): #for heuristic implemantation 21 | def __init__(self, x, y, nde, goal): 22 | self.x = x 23 | self.y = y 24 | self.nde = nde 25 | self.goal = goal 26 | 27 | def findHeuristic(self): 28 | return hypot(self.x - self.goal[0], self.y - self.goal[1]) 29 | 30 | def __cmp__(self, other): 31 | return cmp(self.nde.cost+ self.findHeuristic(), other.nde.cost+other.findHeuristic()) 32 | 33 | class RRTmodifiedAlgo(): 34 | 35 | def __init__(self): 36 | self.getSourceAndGoal() 37 | global sx, sy, dx, dy 38 | self.source = [sx, sy] 39 | self.goal = [dx, dy] 40 | self.RRTree = node(self.source, [], None, True) # actual RRTree 41 | self.Points = kdTree(None, None, 0, self.source, self.RRTree, None) # for storing generated points to increase the search complexity 42 | self.black = None 43 | self.gray = None 44 | self.path = [self.source] 45 | self.current = self.source #current position of a robot in map 46 | self.queue = PriorityQueue() 47 | self.turn = 0 48 | self.goalFound = False 49 | self.startProcessing() 50 | 51 | def dist(self, p1, p2): 52 | return hypot(p1[0]-p2[0], p1[1]-p2[1]) 53 | 54 | def checkBondaries(self, p, img): 55 | rx, ry, rz = img.shape 56 | if p[0] < 0 or p[1] < 0 or p[0] >= ry or p[1] >= rx: 57 | return False 58 | return True 59 | 60 | def check_same(self, p1, p2): 61 | if int(p1[0]) <= int(p2[0])+1 and int(p1[1]) <= int(p2[1])+1 and int(p1[0]) >= int(p2[0])-1 and int(p1[1]) >= int(p2[1])-1: 62 | return True 63 | return False 64 | 65 | def check_for_black(self, p1, p2): #check if a point is in black region or not by checking if edge joining it cuts any obstacle region 66 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 67 | t1 = p1 68 | i = 0 69 | pnt = (int(t1[0]), int(t1[1])) 70 | i = 0 71 | #cv2.circle(self.img, pnt, 2, (0, 255, 255), 1) 72 | #cv2.imshow('image', self.img) 73 | #cv2.waitKey(0) 74 | #print "entered Black" 75 | while not self.check_same(t1, p2): 76 | t1 = [p1[0] + i * cos(theta), p1[1] + i * sin(theta)] 77 | if not self.checkBondaries(t1, self.img): 78 | return True 79 | #print t1 80 | if self.img[int(t1[1])][int(t1[0])][0] == 255: 81 | # print "left black with true" 82 | return True 83 | i = i + 1 84 | #print "left black with false" 85 | return False 86 | 87 | def check_for_gray(self, p2): #Check if a point is in gray region or not by checking its reachability from source point 88 | p1 = self.current 89 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 90 | t1 = p1 91 | pnt = (t1[0], t1[1]) 92 | i = 0 93 | #cv2.circle(self.img, pnt, 2, (0, 255, 255), 1) 94 | #cv2.imshow('image', self.img) 95 | #cv2.waitKey(0) 96 | while not self.check_same(t1, p2): 97 | t1 = [int(p1[0] + i * cos(theta)), int(p1[1] + i * sin(theta))] 98 | if self.turn == 1: 99 | cv2.circle(self.img, tuple(t1), 1, (0, 255, 0), 1) 100 | cv2.waitKey(1) 101 | if not self.checkBondaries(t1, self.img): 102 | return True 103 | if self.img[int(t1[1])][int(t1[0])][0] == 255: 104 | return True 105 | i = i + 1 106 | return False 107 | 108 | def checkInsideBlack(self, nn): 109 | if self.black == None: 110 | return False 111 | ret = self.black.search(nn, 1000000000000000000000, None, None) 112 | if self.turn == 1: 113 | print ret[0] 114 | if ret[0] < 1: 115 | return True 116 | return False 117 | 118 | def printString(self, str): 119 | if self.turn == 1: 120 | print str 121 | 122 | def growRRT(self): 123 | print "queus", self.queue.empty() 124 | print 'Current ', self.current 125 | count = 0 126 | X, Y, Z = img.shape 127 | cv2.imshow('self image', img) 128 | cv2.waitKey(0) 129 | cv2.destroyAllWindows() 130 | img1 = self.img 131 | cv2.imshow('image1', img1) 132 | cv2.waitKey(1) 133 | 134 | while not self.goalFound and count < 50: 135 | 136 | rand = [int(random.random() * Y * 1.0), int(random.random() * X * 1.0)] 137 | ret = self.Points.search(rand, 100000000000000, None, None) 138 | nearest_neighbour = ret[1] 139 | new_point = self.step_from_to(nearest_neighbour, rand) 140 | 141 | if not self.check_for_black(nearest_neighbour, new_point): 142 | self.printString("not a black node") 143 | if not self.checkInsideBlack(nearest_neighbour): 144 | if not self.check_for_gray(new_point): 145 | self.printString("not a gray node even") 146 | cv2.line(img1, tuple(nearest_neighbour), tuple(new_point), (0, 0, 255), 1) 147 | cv2.imshow('image1', img1) 148 | cv2.waitKey(1) 149 | nde = node(new_point, [], ret[2], True) 150 | if self.checkIfPathExist(new_point): 151 | print "goal Found" 152 | 153 | self.goalFound = True 154 | ret[2].add_child(nde) 155 | self.Points.insert(new_point, dim, nde) 156 | if self.dist(new_point, nearest_neighbour) <= threshold: 157 | count = count + 1 158 | else: 159 | self.printString("gray node") 160 | if self.gray == None: 161 | self.gray = kdTree(None, None, 0, nearest_neighbour, ret[2], None) 162 | ret = self.gray.search(nearest_neighbour, 10000000000000000000, None, None) 163 | if ret[0] != 0: 164 | self.gray.insert(nearest_neighbour, 2, ret[2]) 165 | self.queue.put(pnt(nearest_neighbour[0], nearest_neighbour[1], ret[2], self.goal)) 166 | else: 167 | self.printString("black node") 168 | if self.black == None: 169 | self.black = kdTree(None, None, 0, nearest_neighbour, ret[2], None) 170 | ret = self.black.search(nearest_neighbour, 100000000000000000, None, None) 171 | if ret[0] != 0: 172 | self.black.insert(nearest_neighbour, 2, ret[2]) 173 | 174 | self.turn = self.turn + 1 175 | 176 | def step_from_to(self, p1, p2): # returns point with at most epsilon distance from nearest neighbour in the direction of randomly generated point 177 | if self.dist(p1, p2) < EPSILON: 178 | return p2 179 | else: 180 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 181 | return [int(p1[0] + EPSILON * cos(theta)), int(p1[1] + EPSILON * sin(theta))] 182 | 183 | def findNearestObstacle(self, Img, x, y, theta): 184 | #print theta 185 | rx, ry, rz = Img.shape 186 | theta = pi*theta/180 187 | step = 20 188 | while x < rx and y < ry and x >= 0 and y >= 0: 189 | if Img[int(x)][int(y)][0] == 255: 190 | break 191 | else: 192 | x = x + step*sin(theta) 193 | y = y + step*cos(theta) 194 | 195 | if x >= rx or y >= ry or x < 0 or y < 0: 196 | while x >= rx or y >= ry or x < 0 or y < 0: 197 | x = x - sin(theta) 198 | y = y - cos(theta) 199 | return x, y 200 | 201 | while Img[int(x)][int(y)][0] == 255: 202 | x = x-sin(theta) 203 | y = y-cos(theta) 204 | 205 | return x, y 206 | 207 | def markVisibleArea(self, originalImg, visibleImg, x, y): 208 | lx, ly = -200, -200 #last coordinates 209 | for i in range(361): 210 | nx, ny = self.findNearestObstacle(originalImg, x, y, i) 211 | nx = int(nx) 212 | ny = int(ny) 213 | visibleImg[nx][ny] = (255, 255, 255) 214 | if i != 0 and hypot(ny-ly, nx-lx) < 70: 215 | cv2.line(visibleImg, (ny, nx), (ly, lx), (255, 255, 255), 3) 216 | 217 | lx, ly = nx, ny 218 | 219 | cv2.imshow('image', visibleImg) 220 | cv2.imshow('original', originalImg) 221 | cv2.waitKey(0) 222 | self.img = visibleImg 223 | 224 | def draw_circle(self, event, x, y, flags, param): 225 | global sx, sy, dx, dy, flag 226 | 227 | if event==cv2.EVENT_LBUTTONDBLCLK: 228 | #cv2.circle(img, (x, y), 100, (255, 0, 0), -1) 229 | if not flag: 230 | sx, sy = x, y 231 | print sx, sy 232 | flag = True 233 | else: 234 | dx, dy = x, y 235 | print dx, dy 236 | 237 | def getSourceAndGoal(self): 238 | cv2.namedWindow('image') 239 | cv2.setMouseCallback('image', self.draw_circle) 240 | cv2.imshow('image', img) 241 | cv2.waitKey(0) 242 | 243 | def checkIfPathExist(self, p): # Checks if direct path has been found using RRT only 244 | if p[0] < self.goal[0] + 5 and p[1] < self.goal[1] + 5 and p[0] > self.goal[0] - 5 and p[1] > self.goal[1] - 5: 245 | return True 246 | return False 247 | 248 | def check_goal(self): # Ckecks if robot has reached the goal or not 249 | if self.current[0] < self.goal[0] + 2 and self.current[1] < self.goal[1] + 2 and self.current[0] > self.goal[0]-2 and self.current[1] > self.goal[1]-2: 250 | return True 251 | return False 252 | 253 | def walk_step(self): 254 | flag = False 255 | p = None 256 | while not flag: 257 | p = self.queue.get() 258 | ret = self.black.search([p.x, p.y], 100000000000000000, None, None) 259 | if ret[0] == 0: 260 | continue 261 | flag = True 262 | 263 | print "heuristic ", p.nde.point 264 | cv2.circle(self.img, tuple(p.nde.point), 3, (0, 255, 0), 3) 265 | cv2.imshow('image1', self.img) 266 | cv2.waitKey(0) 267 | 268 | nde = p.nde 269 | while nde.parent.parent != None: 270 | nde = nde.parent 271 | cv2.line(self.img, tuple(nde.point), tuple(self.current), (255, 255, 0), 1) 272 | self.current = nde.point 273 | 274 | nde.cost = 0 275 | nde.updateCost() 276 | 277 | def startProcessing(self): 278 | arr = np.zeros(img.shape) 279 | self.img = arr 280 | while not self.check_goal() and not self.goalFound: 281 | self.markVisibleArea(img, self.img, self.current[1], self.current[0]) 282 | print "visible marked" 283 | #time.sleep(5) 284 | self.growRRT() 285 | print "Tree has been grown" 286 | #time.sleep(5) 287 | self.walk_step() 288 | print "Step has been taken" 289 | time.sleep(5) 290 | cv2.destroyAllWindows() 291 | 292 | img = cv2.imread('Images/obstacle1.png') 293 | 294 | start = RRTmodifiedAlgo() 295 | 296 | cv2.destroyAllWindows() -------------------------------------------------------------------------------- /RRT_Scan_final.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | from math import pi, sin, cos, hypot, atan2 3 | import numpy as np 4 | from kdTree import kdTree 5 | from kdTree import node 6 | import random 7 | import copy 8 | import time 9 | import Queue as Q 10 | 11 | #global Declarations 12 | sx, sy = -1, -1 13 | dx, dy = -1, -1 14 | flag = False 15 | EPSILON = 20.0 16 | NUMNODES = 5000 17 | dim = 2 18 | threshold = 4 #breaking condition of RRT loop 19 | p = 5 20 | 21 | img = cv2.imread('Images/obstacle.png') 22 | 23 | class RRTmodifiedAlgo(): 24 | 25 | def __init__(self): 26 | self.getSourceAndGoal() 27 | global sx, sy, dx, dy 28 | self.source = [sx, sy] 29 | self.goal = [dx, dy] 30 | self.RRTree = node(self.source, [], None, True) # Permanent RRTree 31 | self.Points = kdTree(None, None, 0, self.source, self.RRTree) # for storing generated points to increase the search complexity, Currently storing points of normal RRT 32 | self.tempPoints = None # currently storing points of Goal biased which is being generated to form RRT complete and stores those extra points in kdTree data structure. 33 | self.leafNodes = [] #storing all the nodes 34 | self.path = [self.source] 35 | self.current = self.source #current position of a robot in map 36 | self.turn = 0 37 | self.goalFound = False 38 | self.extraPoints = [] 39 | self.steps = [] 40 | self.startProcessing() 41 | 42 | def dist(self, p1, p2): 43 | return hypot(p1[0]-p2[0], p1[1]-p2[1]) 44 | 45 | def checkBondaries(self, p, img): 46 | rx, ry= img.shape 47 | if p[0] < 0 or p[1] < 0 or p[0] >= rx or p[1] >= ry: 48 | return False 49 | return True 50 | 51 | def check_same(self, p1, p2): 52 | if int(p1[0]) <= int(p2[0])+1 and int(p1[1]) <= int(p2[1])+1 and int(p1[0]) >= int(p2[0])-1 and int(p1[1]) >= int(p2[1])-1: 53 | return True 54 | return False 55 | 56 | def check_for_black(self, p1, p2): #check if a point is in black region or not by checking if edge joining it cuts any obstacle region 57 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 58 | t1 = p1 59 | i = 0 60 | pnt = (int(t1[0]), int(t1[1])) 61 | i = 0 62 | while not self.check_same(t1, p2): 63 | t1 = [p1[0] + i * cos(theta), p1[1] + i * sin(theta)] 64 | if not self.checkBondaries(t1, self.img): 65 | return True 66 | if self.img[int(t1[0])][int(t1[1])] == 255: 67 | return True 68 | i = i + 1 69 | return False 70 | 71 | def check_for_gray(self, p2): #Check if a point is in gray region or not by checking its reachability from source point 72 | if self.img[p2[0]][p2[1]] == 100: 73 | return False 74 | else: 75 | return True 76 | 77 | def checkInsideBlack(self, nn): 78 | if self.black == None: 79 | return False 80 | ret = self.black.search(nn, 1000000000000000000000, None, None) 81 | # if self.turn == 1: 82 | # print ret[0] 83 | if ret[0] < 1: 84 | return True 85 | return False 86 | 87 | def printString(self, str): 88 | if self.turn == 1: 89 | print str 90 | 91 | def storeleaves(self, rrtnode): #for storing leaf nodes 92 | #print rrtnode.children 93 | if len(rrtnode.children) == 0: 94 | self.leafNodes.append(rrtnode) 95 | for i in rrtnode.children: 96 | self.storeleaves(i) 97 | 98 | def generateGoalBiasPoints(self): 99 | x = random.random()*100 100 | X,Y = self.img.shape 101 | if x > 70: 102 | return [int(random.random() * (X-1) * 1.0), int(random.random() * (Y-1) * 1.0)] 103 | else: 104 | return self.goal 105 | 106 | def checkIfGoalFound(self, p): #checks if goal has been reached by temporary extended goal biased RRT 107 | if p[0]< self.goal[0] + 2 and p[0] > self.goal[0]-2 and p[1] < self.goal[1]+2 and p[1] > self.goal[1]-2: 108 | return True 109 | return False 110 | 111 | def goalBiastempRRT(self): #grow tree with goal bias-ness 112 | 113 | while True: 114 | rand = self.generateGoalBiasPoints() 115 | ret = self.Points.search(rand, 100000000000000, None, None, None, None, None) 116 | ret1 = ret 117 | if self.tempPoints != None: 118 | ret1 = self.tempPoints.search(rand, 100000000000000, None, None, None, None, None) 119 | if ret[0] > ret1[0]: 120 | ret = ret1 121 | nearest_neighbour = ret[1] 122 | new_point = self.step_from_to(nearest_neighbour, rand) 123 | if new_point[0] == nearest_neighbour[0] and new_point[1] == nearest_neighbour[1]: 124 | print "same point" 125 | continue 126 | 127 | if not self.check_for_black(nearest_neighbour, new_point): 128 | nde = node(new_point, [], ret[2], True) 129 | ret[2].add_child(nde) 130 | self.leafNodes.append((ret[2], nde)) 131 | if self.tempPoints == None: 132 | self.tempPoints = kdTree(None, None, 0, new_point, nde) 133 | else: 134 | self.tempPoints.insert(new_point, dim, nde) 135 | self.extraPoints.append(new_point) 136 | if self.checkIfGoalFound(new_point): 137 | while nde.parent.point != self.current: 138 | nde = nde.parent 139 | nde1 = nde.parent 140 | nde.parent = None 141 | nde.children.append(nde1) 142 | nde1.children.remove(nde) 143 | nde1.parent = nde 144 | cv2.line(self.img, tuple(reversed(self.current)), tuple(reversed(nde.point)), 200 , 1) 145 | self.steps.append([self.current, nde.point]) 146 | self.current = nde.point 147 | break 148 | cv2.line(self.tempimg, tuple(reversed(nearest_neighbour)), tuple(reversed(new_point)), 200, 1) 149 | cv2.circle(self.tempimg, tuple(reversed(self.goal)), 3, 200 , 3) 150 | cv2.imshow('image2', self.tempimg) 151 | k = cv2.waitKey(1) 152 | if k == 27: 153 | exit() 154 | 155 | def removegeneratedLeafNodes(self): 156 | for rrtnode in self.leafNodes: 157 | pnt = rrtnode[0].point 158 | ret = self.Points.search(pnt, 100000000000000, None, None, None, None, None) 159 | if ret[0] < 1: 160 | rrtnode[0].children.remove(rrtnode[1]) 161 | rrtnode[1].parent = None 162 | 163 | def showCurrentTree(self, rrtnode): 164 | for i in rrtnode.children: 165 | cv2.line(self.img1, tuple(rrtnode.point), tuple(i.point), (0, 0, 255), 1) 166 | self.showCurrentTree(i) 167 | 168 | def checkIfRemoved(self): 169 | for i in self.extraPoints: 170 | ret = self.Points.search(i, 1000000000000000000, None, None, None, None, None) 171 | #print ret[0], ret[1], i 172 | 173 | def generatePoints(self): 174 | x = random.random() * 100 175 | X, Y = self.img.shape 176 | if x > p: 177 | return [int(random.random() * (X-1) * 1.0), int(random.random() * (Y-1) * 1.0)] 178 | else: 179 | return self.goal 180 | 181 | def normalRRT(self): 182 | count = 0 183 | X, Y = img.shape[:2] 184 | self.tempimg = copy.copy(self.treeimage) 185 | while not self.goalFound and count < 10: 186 | rand = self.generatePoints() 187 | if not self.checkBondaries(rand, self.img): 188 | continue 189 | ret = self.Points.search(rand, 100000000000000, None, None, None, None, None) 190 | nearest_neighbour = ret[1] 191 | new_point = self.step_from_to(nearest_neighbour, rand) 192 | 193 | if not self.check_for_black(nearest_neighbour, new_point): 194 | if not self.check_for_gray(new_point): 195 | #print new_point 196 | nde = node(new_point, [], ret[2], True) 197 | ret[2].add_child(nde) 198 | self.Points.insert(new_point, dim, nde) 199 | if self.dist(new_point, nearest_neighbour) <= threshold: 200 | count = count + 1 201 | 202 | cv2.line(self.treeimage, tuple(reversed(nearest_neighbour)), tuple(reversed(new_point)), (0, 0, 255), 1) 203 | cv2.imshow('image1', self.treeimage) 204 | cv2.line(self.tempimg, tuple(reversed(nearest_neighbour)), tuple(reversed(new_point)), (0, 0, 255), 1) 205 | cv2.imshow('image2', self.tempimg) 206 | k = cv2.waitKey(1) 207 | 208 | if k == 27: 209 | exit() 210 | if self.checkIfGoalFound(new_point): 211 | self.goalFound = True 212 | break 213 | 214 | def recPrint(self, point): 215 | if point.left != None: 216 | self.recPrint(point.left) 217 | nde = point.nde 218 | if nde.parent != None: 219 | cv2.line(self.treeimage, tuple(reversed(nde.point)), tuple(reversed(nde.parent.point)), 0, 1) 220 | if point.right != None: 221 | self.recPrint(point.right) 222 | 223 | 224 | def printWholeTree(self): 225 | self.treeimage = copy.copy(self.img) 226 | self.recPrint(self.Points) 227 | for i in self.steps: 228 | cv2.line(self.treeimage, tuple(reversed(i[0])), tuple(reversed(i[1])), 200, 1) 229 | # cv2.imshow('new_tree', self.treeimage) 230 | # cv2.waitKey(0) 231 | 232 | def growRRT(self): 233 | self.printWholeTree() 234 | self.normalRRT() 235 | if self.goalFound: 236 | return 237 | print len(self.leafNodes) 238 | self.goalBiastempRRT() 239 | self.removegeneratedLeafNodes() 240 | 241 | self.tempPoints = None 242 | self.leafNodes = [] 243 | 244 | def step_from_to(self, p1, p2): # returns point with at most epsilon distance from nearest neighbour in the direction of randomly generated point 245 | if self.dist(p1, p2) < EPSILON: 246 | return p2 247 | else: 248 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 249 | return [int(p1[0] + EPSILON * cos(theta)), int(p1[1] + EPSILON * sin(theta))] 250 | 251 | def findNearestObstacle(self, Img, x, y, theta): 252 | #print theta 253 | rx, ry, rz = Img.shape 254 | #print rx, ry 255 | theta = pi*theta/180.0 256 | step = 20 257 | while x < rx and y < ry and x >= 0 and y >= 0: 258 | if Img[int(x)][int(y)][0] == 255: 259 | break 260 | else: 261 | x = x + step*sin(theta) 262 | y = y + step*cos(theta) 263 | 264 | if x >= rx or y >= ry or x < 0 or y < 0: 265 | while x >= rx or y >= ry or x < 0 or y < 0: 266 | x = x - sin(theta) 267 | y = y - cos(theta) 268 | return x, y 269 | 270 | while Img[int(x)][int(y)][0] == 255: 271 | x = x-sin(theta) 272 | y = y-cos(theta) 273 | 274 | return x+sin(theta), y + cos(theta) 275 | 276 | def markVisibleArea(self, originalImg): 277 | visibleImg = np.zeros(self.img.shape, np.uint8) 278 | x, y = self.current[0], self.current[1] 279 | lx, ly = -200, -200 #last coordinates 280 | points = [] 281 | for i in range(1083): 282 | nx, ny = self.findNearestObstacle(originalImg, x, y, i/3) 283 | #print nx, ny 284 | nx = int(nx) 285 | ny = int(ny) 286 | points.append((ny, nx)) 287 | if i != 0: 288 | cv2.line(visibleImg, (ny, nx), (ly, lx), 100, 1) 289 | lx, ly = nx, ny 290 | h, w = visibleImg.shape 291 | 292 | mask = np.zeros((h+2, w+2), np.uint8) 293 | cv2.floodFill(visibleImg, mask, (y, x), 100) 294 | for i in points: 295 | cv2.circle(visibleImg, i, 3, 255, 6) 296 | 297 | self.img = cv2.bitwise_or(self.img, visibleImg) 298 | 299 | def draw_circle(self, event, x, y, flags, param): 300 | global sx, sy, dx, dy, flag 301 | 302 | if event==cv2.EVENT_LBUTTONDBLCLK: 303 | #cv2.circle(img, (x, y), 100, (255, 0, 0), -1) 304 | if not flag: 305 | sx, sy = y, x 306 | print sx, sy 307 | flag = True 308 | else: 309 | dx, dy = y, x 310 | print dx, dy 311 | 312 | def getSourceAndGoal(self): 313 | cv2.namedWindow('image') 314 | cv2.setMouseCallback('image', self.draw_circle) 315 | cv2.imshow('image', img) 316 | cv2.waitKey(0) 317 | 318 | def checkIfPathExist(self, p): # Checks if direct path has been found using RRT only 319 | if p[0] < self.goal[0] + 5 and p[1] < self.goal[1] + 5 and p[0] > self.goal[0] - 5 and p[1] > self.goal[1] - 5: 320 | return True 321 | return False 322 | 323 | def check_goal(self): # Ckecks if robot has reached the goal or not 324 | if self.current[0] < self.goal[0] + 2 and self.current[1] < self.goal[1] + 2 and self.current[0] > self.goal[0]-2 and self.current[1] > self.goal[1]-2: 325 | return True 326 | return False 327 | 328 | def startProcessing(self): 329 | arr = np.zeros(img.shape[:2], np.uint8) 330 | self.img = arr 331 | self.treeimage = np.zeros(img.shape[:2], np.uint8) 332 | #self.markVisibleArea(img) 333 | while not self.check_goal() and not self.goalFound: 334 | self.markVisibleArea(img) 335 | # cv2.imshow('image', self.img) 336 | # k = cv2.waitKey(0) 337 | print "visible marked" 338 | self.growRRT() 339 | print "Tree has been grown" 340 | print "goal Reached" 341 | 342 | start = RRTmodifiedAlgo() 343 | 344 | cv2.destroyAllWindows() -------------------------------------------------------------------------------- /RRT_Star_Obstacles_final.py: -------------------------------------------------------------------------------- 1 | from kdTree import kdTree 2 | from kdTree import node 3 | import sys, random, pygame ,time 4 | from pygame.locals import * 5 | from math import sqrt,cos,sin,atan2, hypot 6 | #import kdtree 7 | import numpy as np 8 | import cv2 9 | 10 | import sys 11 | sys.setrecursionlimit(1500) 12 | 13 | XDIM ,YDIM = 0, 0 14 | WINSIZE = [XDIM, YDIM] 15 | EPSILON = 10 16 | NUMNODES = 5000 17 | dim = 2 18 | RADIUS = 15 19 | p = 5 20 | flag = False 21 | 22 | class RRTAlgorithm(object): 23 | 24 | def __init__(self): #initial and destination coordinates and number of nodes 25 | self.getSourceAndGoal() 26 | global XDIM, YDIM 27 | XDIM, YDIM, z = img.shape 28 | print XDIM, YDIM 29 | self.costMap = [[10000000000 for x in range(YDIM)] for x in range(XDIM)] 30 | self.path = None 31 | self.goalNode = None 32 | self.start() 33 | 34 | def checkBoundaries(self, point): 35 | x, y, z = img.shape 36 | if point[0] >= 0 and point[1] >= 0 and point[0] < y and point[1] < x: 37 | return True 38 | return False 39 | 40 | def checkforObstacles(self, p1, p2): 41 | if img[p1[1]][p1[0]][0] == 255 or img[int((p1[1]+p2[1])/2)][int((p1[0]+p2[0])/2)][0] == 255: 42 | return True 43 | else: 44 | return False 45 | 46 | def generatePoints(self): 47 | x, y, z = img.shape 48 | prob = 100.0*random.random() 49 | if prob < p: 50 | return self.goal 51 | else: 52 | return [int(random.random() * (y-1)*1.0), int(random.random() * (x-1)*1.0)] 53 | 54 | def sortdist(self, n): 55 | return n[1] 56 | 57 | def createNewLink(self, childlink, parentlink): 58 | 59 | pnt = childlink.point 60 | while True: 61 | childlink.propogateCost() 62 | oldParent = childlink.parent 63 | oldParent.children.remove(childlink) 64 | childlink.parent = parentlink 65 | parentlink.children.append(childlink) 66 | if oldParent.cost > childlink.cost + self.dist(oldParent.point, childlink.point): 67 | oldParent.cost = childlink.cost + self.dist(oldParent.point, childlink.point) 68 | if pnt == childlink.point: 69 | cv2.line(img, tuple(parentlink.point), tuple(childlink.point), (100, 100, 100), 1) 70 | parentlink = childlink 71 | childlink = oldParent 72 | 73 | else: 74 | cv2.line(img, tuple(oldParent.point), tuple(childlink.point), (0, 0, 0), 1) 75 | break 76 | 77 | def dist(self, p1, p2): 78 | return hypot(p1[0]-p2[0], p1[1]-p2[1]) 79 | 80 | def addConnections(self, Points, source): 81 | 82 | new_point = self.generatePoints() 83 | ret = Points.search(new_point, 1000000000000000000, None, None, None, None, None) 84 | nearest_neighbour = ret[1] 85 | new_point = self.step_from_to(nearest_neighbour, new_point) 86 | new_point = [int(new_point[0]), int(new_point[1])] 87 | 88 | while self.checkforObstacles(new_point, nearest_neighbour): 89 | new_point = self.generatePoints() 90 | ret = Points.search(new_point, 1000000000000000000, None, None, None, None, None) 91 | nearest_neighbour = ret[1] 92 | new_point = self.step_from_to(nearest_neighbour, new_point) 93 | new_point = [int(new_point[0]), int(new_point[1])] 94 | 95 | #print new_point 96 | nos = Points.searchNN(new_point, RADIUS) 97 | #print len(nos) 98 | cost = 100000000000 99 | parent = None 100 | nodes = [] 101 | for i in nos: 102 | ret = Points.search(i[0], 1000000000000000000000, None, None, None, None, None) 103 | if ret[2].cost + self.dist(new_point, ret[1]) < cost: 104 | cost = ret[2].cost + self.dist(new_point, ret[1]) 105 | parent = ret[2] 106 | 107 | nodes.append(ret) 108 | 109 | cv2.line(img, tuple(parent.point), tuple(new_point), (100, 100, 100), 1) 110 | nde = node(new_point, [], parent, True, cost) 111 | parent.add_child(nde) 112 | Points.insert(new_point, 2, nde) 113 | 114 | flag = False 115 | if self.goalNode !=None: 116 | flag = True 117 | if flag: 118 | nde1 = self.goalNode 119 | while nde1.parent != None: 120 | cv2.line(img, tuple(nde1.point), tuple(nde1.parent.point), (100, 100, 100), 1) 121 | nde1 = nde1.parent 122 | 123 | 124 | 125 | for i in nodes: 126 | if i[1] != parent.point: 127 | if i[2].cost > self.dist(i[1], new_point) + cost: 128 | i[2].cost = self.dist(i[1], new_point) + cost 129 | self.createNewLink(i[2], nde) 130 | 131 | 132 | if flag: 133 | nodes = Points.searchNN(self.goal, 10) 134 | sorted(nodes, key=self.sortdist) 135 | pnt = nodes[0][0] 136 | self.path = [] 137 | nde = self.goalNode = Points.search(pnt, 1000000000000000000, None, None, None, None, None)[2] 138 | while nde.parent != None: 139 | cv2.line(img, tuple(nde.point), tuple(nde.parent.point), (200, 200,200), 1) 140 | self.path.append(nde.point) 141 | #print "point ", nde.point 142 | nde = nde.parent 143 | return new_point 144 | 145 | def start(self): 146 | white = 100, 100, 100 147 | black = 20, 20, 40 148 | bright = 255, 255, 255 149 | 150 | RRTree = node(self.source, [], None, True) #actual RRTree 151 | Points = kdTree(None, None, 0, self.source, RRTree) #for storing generated points to increase the search complexity 152 | 153 | current = self.source 154 | #Pointmap = [[0 for i in range (YDIM)] for i in range(XDIM)] 155 | #Pointmap[self.source[0]][self.source[1]] = 1 156 | 157 | count = 0 158 | 159 | while not self.check(current, self.goal): 160 | 161 | current = self.addConnections(Points, self.source) 162 | cv2.imshow('image', img) 163 | k = cv2.waitKey(1) 164 | count = count + 1 165 | #current = self.addConnection1(Points, rand, screen) 166 | 167 | ret = Points.search(current, 100000000000000000000, None, None, None, None, None) 168 | nde = ret[2] 169 | self.goalNode = nde 170 | path = [] 171 | while nde.parent != None: 172 | print nde.point, nde.cost 173 | path.append(nde.point) 174 | cv2.line(img, tuple(nde.point), tuple(nde.parent.point), (200, 200,200)) 175 | cv2.imshow('image', img) 176 | k = cv2.waitKey(1) 177 | nde = nde.parent 178 | 179 | if nde.parent == nde: 180 | break 181 | print 'count', count 182 | self.path = path 183 | for i in range(1000000-count): 184 | current = self.addConnections(Points, self.source) 185 | cv2.imshow('image', img) 186 | k = cv2.waitKey(1) 187 | 188 | def printchildren(self, nde): 189 | print nde.point, nde.cost 190 | for i in nde.children: 191 | print i.point, i.cost 192 | 193 | def check(self, point , goal): # checking if currently added node is at goal or not 194 | if point[0] > goal[0]-5 and point[0] < goal[0]+5 and point[1] > goal[1]-5 and point[1] < goal[1]+5: 195 | return True 196 | return False 197 | 198 | def dist(self, p1, p2): #returns euclid's distance between points p1 and p2 199 | return sqrt((p1[0] - p2[0]) * (p1[0] - p2[0]) + (p1[1] - p2[1]) * (p1[1] - p2[1])) 200 | 201 | def draw_circle(self, event, x, y, flags, param): 202 | global flag 203 | 204 | if event == cv2.EVENT_LBUTTONDBLCLK: 205 | # cv2.circle(img, (x, y), 100, (255, 0, 0), -1) 206 | if not flag: 207 | self.source = x, y 208 | print self.source 209 | flag = True 210 | else: 211 | self.goal = x, y 212 | print self.goal 213 | 214 | def getSourceAndGoal(self): 215 | cv2.namedWindow('image') 216 | cv2.setMouseCallback('image', self.draw_circle) 217 | cv2.imshow('image', img) 218 | cv2.waitKey(0) 219 | 220 | def step_from_to(self,p1, p2): #returns point with at most epsilon distance from nearest neighbour in the direction of randomly generated point 221 | if self.dist(p1, p2) < EPSILON: 222 | return p2 223 | else: 224 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 225 | return [p1[0] + EPSILON * cos(theta), p1[1] + EPSILON * sin(theta)] 226 | 227 | def main(): 228 | tree = RRTAlgorithm() 229 | 230 | img = cv2.imread('Images\\test.png') 231 | 232 | main() -------------------------------------------------------------------------------- /RRT_bidirectional.py: -------------------------------------------------------------------------------- 1 | from kdTree import kdTree 2 | from kdTree import node 3 | import sys, random, math, pygame, time 4 | from pygame.locals import * 5 | from math import sqrt, cos, sin, atan2 6 | 7 | XDIM = 640 8 | YDIM = 480 9 | WINSIZE = [XDIM, YDIM] 10 | EPSILON = 5.0 11 | dim = 2 12 | p = 1 13 | 14 | class RRTBidirectionalAlgorithm: 15 | def __init__(self, source, goal, nodes): # initial and destination coordinates and number of nodes 16 | self.start(source, goal, nodes) 17 | 18 | def generateRandom(self): 19 | return [random.random() * 640.0, random.random() * 480.0] 20 | 21 | def getPoints(self, goal): 22 | if 100.0 * random.random() < p: 23 | return goal 24 | else: 25 | return self.generateRandom() 26 | 27 | def drawObstacles(self, screen): 28 | pygame.draw.circle(screen, (255, 255, 255), (320,240), 50) 29 | 30 | def checkObstacles(self, pnt, screen): 31 | pixelarray = pygame.PixelArray(screen) 32 | pnt = [int(pnt[0]), int(pnt[1])] 33 | if pixelarray[pnt[0], pnt[1]] == screen.map_rgb((255, 255, 255)): 34 | #print pixelarray[pnt[1], pnt[0]] 35 | return True 36 | return False 37 | 38 | def start(self, source, goal, nodes): 39 | pygame.init() 40 | screen = pygame.display.set_mode(WINSIZE) 41 | pygame.display.set_caption('RRT brute force') 42 | white = 100, 100, 100 43 | black = 20, 20, 40 44 | bright = 255, 255, 255 45 | screen.fill(black) 46 | #self.drawObstacles(screen) 47 | 48 | RRTree1 = node(source, [], None, True) # first RRTree from source 49 | RRTree2 = node(goal, [], None, True) #second RRTree from goal 50 | Points1 = kdTree(None, None, 0, source, RRTree1, 51 | None) # for storing generated points in RRTree1 to increase the search complexity 52 | Points2 = kdTree(None, None, 0, goal, RRTree2, 53 | None) # for storing generated points in RRTree2 to increase the search complexity 54 | 55 | #Points1.print_tree() 56 | 57 | #Points2.print_tree() 58 | for i in range(10000): 59 | 60 | #Doing work for first tree 61 | rand = self.getPoints(goal) 62 | ret = Points1.search(rand, 100000000000000000, None, None, None) 63 | nearest_neighbour = ret[1] 64 | new_point = self.step_from_to(nearest_neighbour, rand) 65 | while self.checkObstacles(new_point, screen): 66 | rand = self.getPoints(goal) 67 | ret = Points1.search(rand, 100000000000000000, None, None, None) 68 | nearest_neighbour = ret[1] 69 | new_point = self.step_from_to(nearest_neighbour, rand) 70 | 71 | nde = node(new_point, [], ret[2], True) 72 | ret[2].add_child(nde) 73 | Points1.insert(new_point, dim, nde) 74 | 75 | pnt = [int(new_point[0]), int(new_point[1])] 76 | pygame.draw.line(screen, white, nearest_neighbour, new_point) 77 | # pygame.draw.circle(screen, (255, 0, 0), pnt, 1) 78 | pygame.display.update() 79 | 80 | #print 'a' 81 | 82 | #repeating same thing for second tree but not towards random point but towards the point generated in last tree i.e, new_point 83 | ret = Points2.search(new_point, 100000000000000000, None, None, None) 84 | nearest_neighbour = ret[1] 85 | current = self.step_from_to(nearest_neighbour, new_point) 86 | 87 | nde = node(current, [], ret[2], True) 88 | ret[2].add_child(nde) 89 | Points2.insert(current, dim, nde) 90 | #print 'b' 91 | 92 | pnt = [int(current[0]), int(current[1])] 93 | pygame.draw.line(screen, white, nearest_neighbour, current) 94 | 95 | #print 'points ', new_point, current 96 | # pygame.draw.circle(screen, (255, 0, 0), pnt, 1) 97 | pygame.display.update() 98 | for e in pygame.event.get(): 99 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 100 | sys.exit("Leaving .") 101 | #Points1.print_tree() 102 | #Points2.print_tree() 103 | if current == new_point: 104 | #print 'c' 105 | break 106 | 107 | temp1 = Points1 108 | temp2 = RRTree1 109 | Points1 = Points2 110 | RRTree1 = RRTree2 111 | Points2 = temp1 112 | RRTree2 = temp2 113 | 114 | ret1 = Points1.search(current, 100000000000000000000, None, None, None) 115 | nde1 = ret1[2] 116 | ret2 = Points2.search(current, 100000000000000000000, None, None, None) 117 | nde2 = ret2[2] 118 | 119 | while nde1.parent != None: 120 | pygame.draw.line(screen, bright, nde1.point, nde1.parent.point) 121 | #print nde1.point 122 | pygame.display.update() 123 | nde1 = nde1.parent 124 | time.sleep(0.1) 125 | for e in pygame.event.get(): 126 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 127 | sys.exit("Leaving.") 128 | 129 | while nde2.parent != None: 130 | pygame.draw.line(screen, bright, nde2.point, nde2.parent.point) 131 | #print nde2.point 132 | pygame.display.update() 133 | nde2 = nde2.parent 134 | time.sleep(0.1) 135 | for e in pygame.event.get(): 136 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 137 | sys.exit("Leaving.") 138 | 139 | def check(self, point, goal): # checking if currently added node is at goal or not 140 | if point[0] > goal[0] - 5 and point[0] < goal[0] + 5 and point[1] > goal[1] - 5 and point[1] < goal[1] + 5: 141 | return True 142 | return False 143 | 144 | def dist(self, p1, p2): # returns euclid's distance between points p1 and p2 145 | return sqrt((p1[0] - p2[0]) * (p1[0] - p2[0]) + (p1[1] - p2[1]) * (p1[1] - p2[1])) 146 | 147 | def step_from_to(self, p1, p2): # returns point with at most epsilon distance from nearest neighbour in the direction of randomly generated point 148 | if self.dist(p1, p2) < EPSILON: 149 | return p2 150 | else: 151 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 152 | return [p1[0] + EPSILON * cos(theta), p1[1] + EPSILON * sin(theta)] 153 | 154 | def main(): 155 | tree = RRTBidirectionalAlgorithm([500, 400], [5, 5], 2000) 156 | 157 | main() -------------------------------------------------------------------------------- /RRT_with_obstacles.py: -------------------------------------------------------------------------------- 1 | from kdTree import kdTree 2 | from kdTree import node 3 | import sys, random, math, pygame, time 4 | from pygame.locals import * 5 | from math import sqrt, cos, sin, atan2 6 | 7 | XDIM = 640 8 | YDIM = 480 9 | WINSIZE = [XDIM, YDIM] 10 | EPSILON = 10.0 11 | dim = 2 12 | p = 5 13 | 14 | class RRTAlgorithm: 15 | def __init__(self, source, goal, nodes): # initial and destination coordinates and number of nodes 16 | self.start(source, goal, nodes) 17 | 18 | def generateRandom(self): 19 | return [random.random() * 640.0, random.random() * 480.0] 20 | 21 | def getPoints(self, goal): 22 | if 100.0 * random.random() < p: 23 | return goal 24 | else: 25 | return self.generateRandom() 26 | 27 | def drawObstacles(self, screen): 28 | pygame.draw.circle(screen, (255, 255, 255), (320,240), 50) 29 | 30 | def checkObstacles(self, pnt, screen): 31 | pixelarray = pygame.PixelArray(screen) 32 | pnt = [int(pnt[0]), int(pnt[1])] 33 | if pixelarray[pnt[0], pnt[1]] == screen.map_rgb((255, 255, 255)): 34 | #print pixelarray[pnt[1], pnt[0]] 35 | return True 36 | return False 37 | 38 | def start(self, source, goal, nodes): 39 | pygame.init() 40 | screen = pygame.display.set_mode(WINSIZE) 41 | pygame.display.set_caption('RRT brute force') 42 | white = 100, 100, 100 43 | black = 20, 20, 40 44 | bright = 255, 255, 255 45 | screen.fill(black) 46 | self.drawObstacles(screen) 47 | 48 | RRTree = node(source, [], None, True) # actual RRTree 49 | Points = kdTree(None, None, 0, source, RRTree) # for storing generated points to increase the search complexity 50 | current = source 51 | 52 | while not self.check(current, goal): 53 | rand = self.getPoints(goal) 54 | ret = Points.search(rand, 100000000000000000, None, None, None, None, None) 55 | nearest_neighbour = ret[1] 56 | new_point = self.step_from_to(nearest_neighbour, rand) 57 | 58 | while self.checkObstacles(new_point, screen): 59 | rand = self.getPoints(goal) 60 | ret = Points.search(rand, 100000000000000000, None, None, None, None, None) 61 | nearest_neighbour = ret[1] 62 | new_point = self.step_from_to(nearest_neighbour, rand) 63 | 64 | # print rand, nearest_neighbour, new_point 65 | # time.sleep(0.1) 66 | nde = node(new_point, [], ret[2], True) 67 | ret[2].add_child(nde) 68 | Points.insert(new_point, dim, nde) 69 | current = new_point 70 | pnt = [int(new_point[0]), int(new_point[1])] 71 | pygame.draw.line(screen, white, nearest_neighbour, new_point) 72 | # pygame.draw.circle(screen, (255, 0, 0), pnt, 1) 73 | pygame.display.update() 74 | for e in pygame.event.get(): 75 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 76 | sys.exit("Leaving .") 77 | 78 | ret = Points.search(current, 100000000000000000000, None, None, None, None, None) 79 | nde = ret[2] 80 | 81 | while nde.parent != None: 82 | pygame.draw.line(screen, bright, nde.point, nde.parent.point) 83 | pygame.display.update() 84 | nde = nde.parent 85 | time.sleep(0.1) 86 | for e in pygame.event.get(): 87 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 88 | sys.exit("Leaving.") 89 | 90 | def check(self, point, goal): # checking if currently added node is at goal or not 91 | if point[0] > goal[0] - 5 and point[0] < goal[0] + 5 and point[1] > goal[1] - 5 and point[1] < goal[1] + 5: 92 | return True 93 | return False 94 | 95 | def dist(self, p1, p2): # returns euclid's distance between points p1 and p2 96 | return sqrt((p1[0] - p2[0]) * (p1[0] - p2[0]) + (p1[1] - p2[1]) * (p1[1] - p2[1])) 97 | 98 | def step_from_to(self, p1, 99 | p2): # returns point with at most epsilon distance from nearest neighbour in the direction of randomly generated point 100 | if self.dist(p1, p2) < EPSILON: 101 | return p2 102 | else: 103 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 104 | return [p1[0] + EPSILON * cos(theta), p1[1] + EPSILON * sin(theta)] 105 | 106 | def main(): 107 | tree = RRTAlgorithm([500, 400], [5, 5], 2000) 108 | 109 | main() -------------------------------------------------------------------------------- /RRTstar_Scan.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | from math import pi, sin, cos, hypot, atan2 3 | import numpy as np 4 | from kdTree import kdTree 5 | from kdTree import node 6 | import random 7 | import copy 8 | import time 9 | 10 | #global Declarations 11 | sx, sy = -1, -1 12 | dx, dy = -1, -1 13 | flag = False 14 | EPSILON = 10.0 15 | NUMNODES = 5000 16 | dim = 2 17 | threshold = 4 #breaking condition of RRT loop 18 | p = 5 19 | RADIUS = 12 20 | 21 | class RRTmodifiedAlgo(): 22 | 23 | def __init__(self): 24 | self.getSourceAndGoal() 25 | global sx, sy, dx, dy 26 | self.source = [sx, sy] 27 | self.goal = [dx, dy] 28 | self.RRTree = node(self.source, [], None, True) # Permanent RRTree 29 | self.Points = kdTree(None, None, 0, self.source, self.RRTree) # for storing generated points to increase the search complexity, Currently storing points of normal RRT 30 | self.tempPoints = None # currently storing points of Goal biased which is being generated to form RRT complete and stores those extra points in kdTree data structure. 31 | self.leafNodes = [] #storing all the nodes fron which nodes in new RRTree generated 32 | self.path = None # path from source to goal 33 | self.current = self.source #current position of a robot in map 34 | self.turn = 0 35 | self.goalFound = False 36 | self.goalNode = None 37 | self.extraPoints = [] 38 | self.ArchivedTree = None 39 | self.startProcessing() 40 | 41 | def sortdist(self, n): 42 | return n[1] 43 | 44 | def createNewLink(self, childlink, parentlink): 45 | 46 | pnt = childlink.point 47 | while True: 48 | childlink.propogateCost() 49 | oldParent = childlink.parent 50 | oldParent.children.remove(childlink) 51 | childlink.parent = parentlink 52 | parentlink.children.append(childlink) 53 | if oldParent.cost > childlink.cost + self.dist(oldParent.point, childlink.point): 54 | oldParent.cost = childlink.cost + self.dist(oldParent.point, childlink.point) 55 | if pnt == childlink.point: 56 | cv2.line(self.img, tuple(parentlink.point), tuple(childlink.point), (100, 100, 100), 1) 57 | cv2.line(self.tempimg, tuple(parentlink.point), tuple(childlink.point), (100, 100, 100), 1) 58 | parentlink = childlink 59 | childlink = oldParent 60 | 61 | else: 62 | cv2.line(self.img, tuple(oldParent.point), tuple(childlink.point), (0, 0, 0), 1) 63 | cv2.line(self.tempimg, tuple(oldParent.point), tuple(childlink.point), (0, 0, 0), 1) 64 | break 65 | 66 | def dist(self, p1, p2): 67 | return hypot(p1[0] - p2[0], p1[1] - p2[1]) 68 | 69 | def addConnections(self): 70 | source = self.current 71 | c = 0 72 | flag1 = False 73 | cost = [] 74 | nodes = [] 75 | pnt = None 76 | new_point = None 77 | nearest_neighbour = None 78 | ret = None 79 | 80 | while not flag1: 81 | #finding the nearest point to generated point 82 | new_point = self.generatePoints() 83 | ret = self.Points.search(new_point, 1000000000000000000, None, None, None, None, None) 84 | nearest_neighbour = ret[1] 85 | new_point = self.step_from_to(nearest_neighbour, new_point) 86 | new_point = [int(new_point[0]), int(new_point[1])] 87 | if not self.check_for_black(nearest_neighbour, new_point): 88 | if not self.check_for_gray(new_point): 89 | flag1 = True 90 | break 91 | 92 | nos = self.Points.searchNN(new_point, RADIUS) 93 | # print len(nos) 94 | cost = 100000000000 95 | parent = None 96 | nodes = [] 97 | for i in nos: 98 | ret = self.Points.search(i[0], 1000000000000000000000, None, None, None, None, None) 99 | if ret[2].cost + self.dist(new_point, ret[1]) < cost: 100 | cost = ret[2].cost + self.dist(new_point, ret[1]) 101 | parent = ret[2] 102 | 103 | nodes.append(ret) 104 | 105 | cv2.line(self.img, tuple(parent.point), tuple(new_point), (100, 100, 100), 1) 106 | cv2.line(self.tempimg, tuple(parent.point), tuple(new_point), (100, 100, 100), 1) 107 | 108 | nde = node(new_point, [], parent, True, cost) 109 | parent.add_child(nde) 110 | self.Points.insert(new_point, 2, nde) 111 | 112 | ''' 113 | Update Other links 114 | ''' 115 | 116 | flag = False 117 | if self.goalNode != None: 118 | flag = True 119 | 120 | if flag: 121 | nde1 = self.goalNode 122 | while nde1.parent != None: 123 | cv2.line(self.img, tuple(nde1.point), tuple(nde1.parent.point), (100, 100, 100), 1) 124 | cv2.line(self.tempimg, tuple(nde1.point), tuple(nde1.parent.point), (100, 100, 100), 1) 125 | nde1 = nde1.parent 126 | 127 | for i in nodes: 128 | if i[1] != parent.point: 129 | if i[2].cost > self.dist(i[1], new_point) + cost: 130 | i[2].cost = self.dist(i[1], new_point) + cost 131 | self.createNewLink(i[2], nde) 132 | 133 | if flag: 134 | nodes = self.Points.searchNN(self.goal, 10) 135 | sorted(nodes, key=self.sortdist) 136 | pnt = nodes[0][0] 137 | self.path = [] 138 | nde = self.goalNode = self.Points.search(pnt, 1000000000000000000, None, None, None, None, None)[2] 139 | while nde.parent != None: 140 | cv2.line(self.img, tuple(nde.point), tuple(nde.parent.point), (200, 200, 200), 1) 141 | cv2.line(self.tempimg, tuple(nde.point), tuple(nde.parent.point), (200, 200, 200), 1) 142 | self.path.append(nde.point) 143 | nde = nde.parent 144 | return new_point, parent.point 145 | 146 | def checkforObstacles(self, p1, p2): 147 | if img[p1[1]][p1[0]][0] == 255 or img[int((p1[1] + p2[1]) / 2)][int((p1[0] + p2[0]) / 2)][0] == 255: 148 | return True 149 | else: 150 | return False 151 | 152 | def checkBoundaries(self, p, img): 153 | rx, ry, rz = img.shape 154 | if p[0] < 0 or p[1] < 0 or p[0] >= ry or p[1] >= rx: 155 | return False 156 | return True 157 | 158 | def check_same(self, p1, p2): 159 | if int(p1[0]) <= int(p2[0])+1 and int(p1[1]) <= int(p2[1])+1 and int(p1[0]) >= int(p2[0])-1 and int(p1[1]) >= int(p2[1])-1: 160 | return True 161 | return False 162 | 163 | def check_for_black(self, p1, p2): # check if a point is in black region or not by checking if edge joining it cuts any obstacle region 164 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 165 | t1 = p1 166 | i = 0 167 | pnt = (int(t1[0]), int(t1[1])) 168 | i = 0 169 | while not self.check_same(t1, p2): 170 | t1 = [p1[0] + i * cos(theta), p1[1] + i * sin(theta)] 171 | if not self.checkBoundaries(t1, self.img): 172 | return True 173 | if self.img[int(t1[1])][int(t1[0])][0] == 255: 174 | return True 175 | i = i + 1 176 | return False 177 | 178 | def check_for_gray(self, p2): #Check if a point is in gray region or not by checking its reachability from source point 179 | p1 = self.current 180 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 181 | t1 = p1 182 | pnt = (t1[0], t1[1]) 183 | i = 0 184 | 185 | while not self.check_same(t1, p2): 186 | t1 = [int(p1[0] + i * cos(theta)), int(p1[1] + i * sin(theta))] 187 | if not self.checkBoundaries(t1, img): 188 | return True 189 | if img[int(t1[1])][int(t1[0])][0] == 255: 190 | return True 191 | i = i + 1 192 | return False 193 | 194 | def printString(self, str): 195 | if self.turn == 1: 196 | print str 197 | 198 | def storeleaves(self, rrtnode): #for storing leaf nodes 199 | #print rrtnode.children 200 | if len(rrtnode.children) == 0: 201 | self.leafNodes.append(rrtnode) 202 | for i in rrtnode.children: 203 | self.storeleaves(i) 204 | 205 | def generateGoalBiasPoints(self): 206 | x = random.random()*100 207 | X,Y,Z = self.img.shape 208 | if x > 70: 209 | return [int(random.random() * Y * 1.0), int(random.random() * X * 1.0)] 210 | else: 211 | return self.goal 212 | 213 | def checkIfGoalFound(self, p): #checks if goal has been reached by temporary extended goal biased RRT 214 | if p[0]< self.goal[0] + 2 and p[0] > self.goal[0]-2 and p[1] < self.goal[1]+2 and p[1] > self.goal[1]-2: 215 | return True 216 | return False 217 | 218 | def goalBiastempRRT(self): #grow tree with goal biasness 219 | 220 | while True: 221 | rand = self.generateGoalBiasPoints() 222 | ret = self.Points.search(rand, 100000000000000, None, None, None, None, None) 223 | ret1 = ret 224 | if self.tempPoints != None: 225 | ret1 = self.tempPoints.search(rand, 100000000000000, None, None, None, None, None) 226 | if ret[0] > ret1[0]: 227 | ret = ret1 228 | nearest_neighbour = ret[1] 229 | new_point = self.step_from_to(nearest_neighbour, rand) 230 | if new_point[0] == nearest_neighbour[0] and new_point[1] == nearest_neighbour[1]: 231 | print "same point" 232 | continue 233 | 234 | if not self.check_for_black(nearest_neighbour, new_point): 235 | nde = node(new_point, [], ret[2], True) 236 | ret[2].add_child(nde) 237 | self.leafNodes.append((ret[2], nde)) 238 | if self.tempPoints == None: 239 | self.tempPoints = kdTree(None, None, 0, new_point, nde) 240 | else: 241 | self.tempPoints.insert(new_point, dim, nde) 242 | self.extraPoints.append(new_point) 243 | if self.checkIfGoalFound(new_point): 244 | while nde.parent.point != self.current: 245 | nde = nde.parent 246 | nde1 = nde.parent 247 | nde.parent = None 248 | nde.children.append(nde1) 249 | nde1.children.remove(nde) 250 | nde1.parent = nde 251 | cv2.line(self.img, tuple(self.current), tuple(nde.point), (0, 255, 255), 1) 252 | self.current = nde.point 253 | break 254 | cv2.line(self.tempimg, tuple(nearest_neighbour), tuple(new_point), (0, 255, 255), 1) 255 | cv2.circle(self.tempimg, tuple(self.goal), 3, (0, 0, 255), 3) 256 | cv2.imshow('image2', self.tempimg) 257 | k = cv2.waitKey(1) 258 | if k == 27: 259 | exit() 260 | 261 | def removegeneratedLeafNodes(self): 262 | for rrtnode in self.leafNodes: 263 | pnt = rrtnode[0].point 264 | ret = self.Points.search(pnt, 100000000000000, None, None, None, None, None) 265 | if ret[0] < 1: 266 | rrtnode[0].children.remove(rrtnode[1]) 267 | rrtnode[1].parent = None 268 | 269 | def showCurrentTree(self, rrtnode): 270 | for i in rrtnode.children: 271 | cv2.line(self.img1, tuple(rrtnode.point), tuple(i.point), (0, 0, 255), 1) 272 | self.showCurrentTree(i) 273 | 274 | def generatePoints(self): 275 | x = random.random() * 100 276 | X, Y, Z = self.img.shape 277 | if x > p: 278 | return [int(random.random() * Y * 1.0), int(random.random() * X * 1.0)] 279 | else: 280 | return self.goal 281 | 282 | def normalRRTstar(self): 283 | count = 0 284 | X, Y, Z = img.shape 285 | self.tempimg = copy.copy(self.img) 286 | while not self.goalFound and count < 10: 287 | # rand = [int(random.random() * Y * 1.0), int(random.random() * X * 1.0)] 288 | new_point, nearest_neighbour = self.addConnections() 289 | 290 | if self.dist(new_point, nearest_neighbour) <= threshold: 291 | count = count + 1 292 | 293 | cv2.imshow('image1', self.img) 294 | cv2.imshow('image2', self.tempimg) 295 | k = cv2.waitKey(1) 296 | if k == 27: 297 | exit() 298 | 299 | if self.checkIfGoalFound(new_point): 300 | self.goalFound = True 301 | break 302 | 303 | def growRRT(self): 304 | 305 | self.normalRRTstar() 306 | return 307 | if self.goalFound: 308 | return 309 | #self.storeleaves(self.RRTree) 310 | print len(self.leafNodes) 311 | self.goalBiastempRRT() 312 | self.removegeneratedLeafNodes() 313 | self.showCurrentTree(self.RRTree) 314 | self.tempPoints = None 315 | self.leafNodes = [] 316 | #cv2.imshow('reduced tree', self.img1) 317 | #cv2.waitKey(1) 318 | 319 | def step_from_to(self, p1, p2): # returns point with at most epsilon distance from nearest neighbour in the direction of randomly generated point 320 | if self.dist(p1, p2) < EPSILON: 321 | return p2 322 | else: 323 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 324 | return [int(p1[0] + EPSILON * cos(theta)), int(p1[1] + EPSILON * sin(theta))] 325 | 326 | def findNearestObstacle(self, Img, x, y, theta): 327 | #print theta 328 | rx, ry, rz = Img.shape 329 | theta = pi*theta/180 330 | step = 20 331 | while x < rx and y < ry and x >= 0 and y >= 0: 332 | if Img[int(x)][int(y)][0] == 255: 333 | break 334 | else: 335 | x = x + step*sin(theta) 336 | y = y + step*cos(theta) 337 | 338 | if x >= rx or y >= ry or x < 0 or y < 0: 339 | while x >= rx or y >= ry or x < 0 or y < 0: 340 | x = x - sin(theta) 341 | y = y - cos(theta) 342 | return x, y 343 | 344 | while Img[int(x)][int(y)][0] == 255: 345 | x = x-sin(theta) 346 | y = y-cos(theta) 347 | 348 | return x+sin(theta), y + cos(theta) 349 | 350 | def markVisibleArea(self, originalImg, visibleImg, x, y): 351 | lx, ly = -200, -200 #last coordinates 352 | for i in range(361): 353 | nx, ny = self.findNearestObstacle(originalImg, x, y, i) 354 | nx = int(nx) 355 | ny = int(ny) 356 | #cv2. circle(visibleImg, (ny, nx), 2, (255, 255, 255), 2) 357 | visibleImg[nx][ny] = (255, 255, 255) 358 | 359 | if i != 0: 360 | theta = atan2(ly-ny, lx-nx) 361 | cx, cy = nx, ny 362 | j = 0 363 | if self.dist([nx, ny], [lx, ly]) < 5: 364 | while not(cx == lx and cy == ly) and originalImg[int(cx)][int(cy)][0] == 255: 365 | visibleImg[int(cx)][int(cy)] = (255, 255, 255) 366 | cx = nx + j*cos(theta) 367 | cy = ny + j*sin(theta) 368 | j = j+1 369 | visibleImg[int(cx)][int(cy)] = (255, 255, 255) 370 | cx, cy = lx, ly 371 | j = 0 372 | while not(cx == nx and cy == ny) and originalImg[int(cx)][int(cy)][0] == 255: 373 | visibleImg[int(cx)][int(cy)] = (255, 255, 255) 374 | cx = lx - j * cos(theta) 375 | cy = ly - j * sin(theta) 376 | j = j + 1 377 | visibleImg[int(cx)][int(cy)] = (255, 255, 255) 378 | visibleImg[int(nx)][int(ny)] = (255, 255, 255) 379 | visibleImg[int(lx)][int(ly)] = (255, 255, 255) 380 | lx, ly = nx, ny 381 | 382 | self.img1 = copy.copy(visibleImg) 383 | self.img = visibleImg 384 | 385 | def draw_circle(self, event, x, y, flags, param): 386 | global sx, sy, dx, dy, flag 387 | 388 | if event==cv2.EVENT_LBUTTONDBLCLK: 389 | #cv2.circle(img, (x, y), 100, (255, 0, 0), -1) 390 | if not flag: 391 | sx, sy = x, y 392 | print sx, sy 393 | flag = True 394 | else: 395 | dx, dy = x, y 396 | print dx, dy 397 | 398 | def getSourceAndGoal(self): 399 | cv2.namedWindow('image') 400 | cv2.setMouseCallback('image', self.draw_circle) 401 | cv2.imshow('image', img) 402 | cv2.waitKey(0) 403 | 404 | def checkIfPathExist(self, p): # Checks if direct path has been found using RRT only 405 | if p[0] < self.goal[0] + 5 and p[1] < self.goal[1] + 5 and p[0] > self.goal[0] - 5 and p[1] > self.goal[1] - 5: 406 | return True 407 | return False 408 | 409 | def check_goal(self): # Ckecks if robot has reached the goal or not 410 | if self.current[0] < self.goal[0] + 2 and self.current[1] < self.goal[1] + 2 and self.current[0] > self.goal[0]-2 and self.current[1] > self.goal[1]-2: 411 | return True 412 | return False 413 | 414 | def startProcessing(self): 415 | arr = np.zeros(img.shape) 416 | self.img = arr 417 | while not self.check_goal() and not self.goalFound: 418 | self.markVisibleArea(img, self.img, self.current[1], self.current[0]) 419 | print "visible marked" 420 | self.growRRT() 421 | print "Tree has been grown" 422 | 423 | print "goal Reached" 424 | 425 | img = cv2.imread('Images/obstacle.png') 426 | 427 | start = RRTmodifiedAlgo() 428 | 429 | cv2.destroyAllWindows() -------------------------------------------------------------------------------- /RRTstar_Scan1.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | from math import pi, sin, cos, hypot, atan2 3 | import numpy as np 4 | from kdTree import kdTree 5 | from kdTree import node 6 | import random 7 | import copy 8 | import time 9 | import Queue as Q 10 | 11 | #global Declarations 12 | sx, sy = -1, -1 13 | dx, dy = -1, -1 14 | flag = False 15 | EPSILON = 25.0 16 | NUMNODES = 5000 17 | dim = 2 18 | threshold = 5 #breaking condition of RRT loop 19 | p = 10 20 | RADIUS = 30 21 | 22 | img = cv2.imread('Images/obstacle.png') 23 | 24 | class RRTmodifiedAlgo(): 25 | 26 | def __init__(self): 27 | self.getSourceAndGoal() 28 | global sx, sy, dx, dy 29 | self.source = [sx, sy] 30 | self.goal = [dx, dy] 31 | self.RRTree = node(self.source, [], None, True) # Permanent RRTree 32 | self.Points = kdTree(None, None, 0, self.source, self.RRTree) # for storing generated points to increase the search complexity, Currently storing points of normal RRT 33 | self.tempPoints = None # currently storing points of Goal biased which is being generated to form RRT complete and stores those extra points in kdTree data structure. 34 | self.leafNodes = [] #storing all the nodes 35 | self.path = [self.source] 36 | self.current = self.source #current position of a robot in map 37 | self.turn = 0 38 | self.goalFound = False 39 | self.extraPoints = [] 40 | self.steps = [] 41 | self.goalNode = None 42 | self.allpoints = [] 43 | self.startProcessing() 44 | 45 | def sortdist(self, n): 46 | return n[1] 47 | 48 | def createNewLink(self, childlink, parentlink): 49 | 50 | pnt = childlink.point 51 | while True: 52 | childlink.propogateCost() 53 | oldParent = childlink.parent 54 | oldParent.children.remove(childlink) 55 | childlink.parent = parentlink 56 | parentlink.children.append(childlink) 57 | if oldParent.cost > childlink.cost + self.dist(oldParent.point, childlink.point): 58 | oldParent.cost = childlink.cost + self.dist(oldParent.point, childlink.point) 59 | if pnt == childlink.point: 60 | cv2.line(self.treeimage, tuple(reversed(parentlink.point)), tuple(reversed(childlink.point)), 0, 1) 61 | cv2.line(self.tempimg, tuple(reversed(parentlink.point)), tuple(reversed(childlink.point)), 0, 1) 62 | parentlink = childlink 63 | childlink = oldParent 64 | 65 | else: 66 | cv2.line(self.treeimage, tuple(reversed(oldParent.point)), tuple(reversed(childlink.point)), 100, 1) 67 | cv2.line(self.tempimg, tuple(reversed(oldParent.point)), tuple(reversed(childlink.point)), 100, 1) 68 | break 69 | 70 | def addConnections(self): 71 | source = self.current 72 | c = 0 73 | flag1 = False 74 | cost = [] 75 | nodes = [] 76 | pnt = None 77 | new_point = None 78 | nearest_neighbour = None 79 | ret = None 80 | 81 | while not flag1: 82 | # finding the nearest point to generated point 83 | new_point = self.generatePoints() 84 | ret = self.Points.search(new_point, 1000000000000000000, None, None, None, None, None) 85 | nearest_neighbour = ret[1] 86 | new_point = self.step_from_to(nearest_neighbour, new_point) 87 | new_point = [int(new_point[0]), int(new_point[1])] 88 | if not self.check_for_black(nearest_neighbour, new_point): 89 | if not self.check_for_gray(new_point): 90 | flag1 = True 91 | break 92 | 93 | nos = self.Points.searchNN(new_point, RADIUS) 94 | # print len(nos) 95 | cost = 100000000000 96 | parent = None 97 | nodes = [] 98 | for i in nos: 99 | ret = self.Points.search(i[0], 1000000000000000000000, None, None, None, None, None) 100 | if ret[2].cost + self.dist(new_point, ret[1]) < cost: 101 | cost = ret[2].cost + self.dist(new_point, ret[1]) 102 | parent = ret[2] 103 | 104 | nodes.append(ret) 105 | 106 | cv2.line(self.treeimage, tuple(reversed(parent.point)), tuple(reversed(new_point)), 0, 1) 107 | cv2.line(self.tempimg, tuple(reversed(parent.point)), tuple(reversed(new_point)), 0, 1) 108 | 109 | nde = node(new_point, [], parent, True, cost) 110 | parent.add_child(nde) 111 | self.Points.insert(new_point, 2, nde) 112 | 113 | ''' 114 | Update Other links 115 | ''' 116 | for i in nodes: 117 | if i[1] != parent.point: 118 | if i[2].cost > self.dist(i[1], new_point) + cost: 119 | i[2].cost = self.dist(i[1], new_point) + cost 120 | self.createNewLink(i[2], nde) 121 | 122 | return new_point, parent.point 123 | 124 | def updateTree(self): 125 | self.RRTree = node(self.current, [], None, True) # Permanent RRTree 126 | self.Points1 = kdTree(None, None, 0, self.current, self.RRTree) 127 | q = Q.Queue() 128 | nos = self.Points.searchNN(self.current, RADIUS) 129 | for i in nos: 130 | if i[0] != self.current: 131 | q.put(i[0]) 132 | break 133 | 134 | while not q.empty(): 135 | current = q.get() 136 | 137 | pass 138 | 139 | def dist(self, p1, p2): 140 | return hypot(p1[0]-p2[0], p1[1]-p2[1]) 141 | 142 | def checkBondaries(self, p, img): 143 | rx, ry= img.shape 144 | if p[0] < 0 or p[1] < 0 or p[0] >= rx or p[1] >= ry: 145 | return False 146 | return True 147 | 148 | def check_same(self, p1, p2): 149 | if int(p1[0]) <= int(p2[0])+1 and int(p1[1]) <= int(p2[1])+1 and int(p1[0]) >= int(p2[0])-1 and int(p1[1]) >= int(p2[1])-1: 150 | return True 151 | return False 152 | 153 | def check_for_black(self, p1, p2): #check if a point is in black region or not by checking if edge joining it cuts any obstacle region 154 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 155 | t1 = p1 156 | i = 0 157 | pnt = (int(t1[0]), int(t1[1])) 158 | i = 0 159 | while not self.check_same(t1, p2): 160 | t1 = [p1[0] + i * cos(theta), p1[1] + i * sin(theta)] 161 | if not self.checkBondaries(t1, self.img): 162 | return True 163 | if self.img[int(t1[0])][int(t1[1])] == 255: 164 | return True 165 | i = i + 1 166 | return False 167 | 168 | def check_for_gray(self, p2): #Check if a point is in gray region or not by checking its reachability from source point 169 | if self.img[p2[0]][p2[1]] == 100: 170 | return False 171 | else: 172 | return True 173 | 174 | def checkInsideBlack(self, nn): 175 | if self.black == None: 176 | return False 177 | ret = self.black.search(nn, 1000000000000000000000, None, None) 178 | # if self.turn == 1: 179 | # print ret[0] 180 | if ret[0] < 1: 181 | return True 182 | return False 183 | 184 | def printString(self, str): 185 | if self.turn == 1: 186 | print str 187 | 188 | def storeleaves(self, rrtnode): #for storing leaf nodes 189 | #print rrtnode.children 190 | if len(rrtnode.children) == 0: 191 | self.leafNodes.append(rrtnode) 192 | for i in rrtnode.children: 193 | self.storeleaves(i) 194 | 195 | def generateGoalBiasPoints(self): 196 | x = random.random()*100 197 | X,Y = self.img.shape 198 | if x > 90: 199 | return [int(random.random() * (X-1) * 1.0), int(random.random() * (Y-1) * 1.0)] 200 | else: 201 | return self.goal 202 | 203 | def checkIfGoalFound(self, p): #checks if goal has been reached by temporary extended goal biased RRT 204 | if p[0]< self.goal[0] + 2 and p[0] > self.goal[0]-2 and p[1] < self.goal[1]+2 and p[1] > self.goal[1]-2: 205 | return True 206 | return False 207 | 208 | 209 | 210 | def updateRoot(self, current): 211 | self.RRTree.children.remove(current) 212 | current.children.append(self.RRTree) 213 | self.RRTree.parent = current 214 | current.parent = None 215 | self.RRTree = current 216 | 217 | def checkCost(self, root): 218 | ret = self.Points.search(root.point, 1000000000000000000, None, None, None, None, None) 219 | if root.cost != ret[2].cost: 220 | print root.cost, ret[2].cost, ret[0] 221 | for i in root.children: 222 | self.checkCost(i) 223 | pass 224 | 225 | def goalBiastempRRT(self): #grow tree with goal bias-ness 226 | 227 | while True: 228 | rand = self.generateGoalBiasPoints() 229 | ret = self.Points.search(rand, 100000000000000, None, None, None, None, None) 230 | ret1 = ret 231 | if self.tempPoints != None: 232 | ret1 = self.tempPoints.search(rand, 100000000000000, None, None, None, None, None) 233 | if ret[0] > ret1[0]: 234 | ret = ret1 235 | nearest_neighbour = ret[1] 236 | new_point = self.step_from_to(nearest_neighbour, rand) 237 | if new_point[0] == nearest_neighbour[0] and new_point[1] == nearest_neighbour[1]: 238 | print "same point" 239 | continue 240 | 241 | if not self.check_for_black(nearest_neighbour, new_point): 242 | nde = node(new_point, [], ret[2], True) 243 | ret[2].add_child(nde) 244 | self.leafNodes.append((ret[2], nde)) 245 | if self.tempPoints == None: 246 | self.tempPoints = kdTree(None, None, 0, new_point, nde) 247 | else: 248 | self.tempPoints.insert(new_point, dim, nde) 249 | self.extraPoints.append(new_point) 250 | if self.checkIfGoalFound(new_point): 251 | 252 | 253 | while nde.parent.point != self.current: 254 | nde = nde.parent 255 | self.updateRoot(nde) 256 | # nde.parent.parent = nde 257 | # nde.parent = None 258 | cv2.line(self.treeimage, tuple(reversed(self.current)), tuple(reversed(nde.point)), 200 , 1) 259 | self.steps.append([self.current, nde.point]) 260 | self.current = nde.point 261 | break 262 | cv2.line(self.tempimg, tuple(reversed(nearest_neighbour)), tuple(reversed(new_point)), 200, 1) 263 | cv2.circle(self.tempimg, tuple(reversed(self.goal)), 3, 150 , 3) 264 | cv2.imshow('image2', self.tempimg) 265 | k = cv2.waitKey(1) 266 | if k == 27: 267 | exit() 268 | 269 | rand = self.generateGoalBiasPoints() 270 | nde = self.Points.search(rand, 100000000000000, None, None, None, None, None)[2] 271 | while nde.parent != None: 272 | nde = nde.parent 273 | print 'nde', nde.point, self.current 274 | time.sleep(100) 275 | 276 | def removegeneratedLeafNodes(self): 277 | for rrtnode in self.leafNodes: 278 | pnt = rrtnode[0].point 279 | ret = self.Points.search(pnt, 100000000000000, None, None, None, None, None) 280 | if ret[0] < 1: 281 | rrtnode[0].children.remove(rrtnode[1]) 282 | rrtnode[1].parent = None 283 | 284 | def showCurrentTree(self, rrtnode): 285 | for i in rrtnode.children: 286 | cv2.line(self.img1, tuple(rrtnode.point), tuple(i.point), (0, 0, 255), 1) 287 | self.showCurrentTree(i) 288 | 289 | def checkIfRemoved(self): 290 | for i in self.extraPoints: 291 | ret = self.Points.search(i, 1000000000000000000, None, None, None, None, None) 292 | #print ret[0], ret[1], i 293 | 294 | def generatePoints(self): 295 | x = random.random() * 100 296 | X, Y = self.img.shape 297 | if x > p: 298 | return [int(random.random() * (X-1) * 1.0), int(random.random() * (Y-1) * 1.0)] 299 | else: 300 | return self.goal 301 | 302 | def normalRRT(self, count1): 303 | count = 0 304 | X, Y, Z = img.shape 305 | self.tempimg = copy.copy(self.treeimage) 306 | while count < count1: 307 | new_point, nearest_neighbour = self.addConnections() 308 | self.allpoints.append(new_point) 309 | 310 | if self.dist(new_point, nearest_neighbour) <= threshold: 311 | count = count + 1 312 | 313 | cv2.imshow('image1', self.treeimage) 314 | cv2.imshow('image2', self.tempimg) 315 | k = cv2.waitKey(1) 316 | if k == 27: 317 | exit() 318 | 319 | if self.checkIfGoalFound(new_point): 320 | self.goalFound = True 321 | self.goalNode = self.Points.search(self.goal, 1000000000000000000, None, None, None, None, None)[2] 322 | print 'count', count 323 | 324 | def recPrint(self, point): 325 | if point.left != None: 326 | self.recPrint(point.left) 327 | nde = point.nde 328 | if nde.parent != None: 329 | cv2.line(self.treeimage, tuple(reversed(nde.point)), tuple(reversed(nde.parent.point)), 0, 1) 330 | if point.right != None: 331 | self.recPrint(point.right) 332 | 333 | def printWholeTree(self): 334 | self.treeimage = copy.copy(self.img) 335 | self.recPrint(self.Points) 336 | if self.goalFound: 337 | nodes = self.Points.searchNN(self.goal, 10) 338 | sorted(nodes, key=self.sortdist) 339 | pnt = nodes[0][0] 340 | self.path = [] 341 | nde = self.goalNode = self.Points.search(pnt, 1000000000000000000, None, None, None, None, None)[2] 342 | while nde.parent != None: 343 | cv2.line(self.treeimage, tuple(reversed(nde.point)), tuple(reversed(nde.parent.point)), 200, 1) 344 | self.path.append(nde.point) 345 | nde = nde.parent 346 | for i in self.steps: 347 | cv2.line(self.treeimage, tuple(reversed(i[0])), tuple(reversed(i[1])), 200, 1) 348 | # cv2.imshow('new_tree', self.treeimage) 349 | # cv2.waitKey(0) 350 | 351 | def growRRT(self, count): 352 | self.printWholeTree() 353 | self.normalRRT(count) 354 | self.checkCost(self.RRTree) 355 | time.sleep(100) 356 | if self.goalFound: 357 | return 358 | print len(self.leafNodes) 359 | self.goalBiastempRRT() 360 | self.removegeneratedLeafNodes() 361 | 362 | self.tempPoints = None 363 | self.leafNodes = [] 364 | 365 | def step_from_to(self, p1, p2): # returns point with at most epsilon distance from nearest neighbour in the direction of randomly generated point 366 | if self.dist(p1, p2) < EPSILON: 367 | return p2 368 | else: 369 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 370 | return [int(p1[0] + EPSILON * cos(theta)), int(p1[1] + EPSILON * sin(theta))] 371 | 372 | def findNearestObstacle(self, Img, x, y, theta): 373 | #print theta 374 | rx, ry, rz = Img.shape 375 | #print rx, ry 376 | theta = pi*theta/180.0 377 | step = 20 378 | while x < rx and y < ry and x >= 0 and y >= 0: 379 | if Img[int(x)][int(y)][0] == 255: 380 | break 381 | else: 382 | x = x + step*sin(theta) 383 | y = y + step*cos(theta) 384 | 385 | if x >= rx or y >= ry or x < 0 or y < 0: 386 | while x >= rx or y >= ry or x < 0 or y < 0: 387 | x = x - sin(theta) 388 | y = y - cos(theta) 389 | return x, y 390 | 391 | while Img[int(x)][int(y)][0] == 255: 392 | x = x-sin(theta) 393 | y = y-cos(theta) 394 | 395 | return x+sin(theta), y + cos(theta) 396 | 397 | def markVisibleArea(self, originalImg): 398 | visibleImg = np.zeros(self.img.shape, np.uint8) 399 | x, y = self.current[0], self.current[1] 400 | lx, ly = -200, -200 #last coordinates 401 | points = [] 402 | for i in range(1083): 403 | nx, ny = self.findNearestObstacle(originalImg, x, y, i/3) 404 | #print nx, ny 405 | nx = int(nx) 406 | ny = int(ny) 407 | points.append((ny, nx)) 408 | if i != 0: 409 | cv2.line(visibleImg, (ny, nx), (ly, lx), 100, 1) 410 | lx, ly = nx, ny 411 | h, w = visibleImg.shape 412 | 413 | mask = np.zeros((h+2, w+2), np.uint8) 414 | cv2.floodFill(visibleImg, mask, (y, x), 100) 415 | for i in points: 416 | cv2.circle(visibleImg, i, 3, 255, 6) 417 | 418 | self.img = cv2.bitwise_or(self.img, visibleImg) 419 | 420 | def draw_circle(self, event, x, y, flags, param): 421 | global sx, sy, dx, dy, flag 422 | 423 | if event==cv2.EVENT_LBUTTONDBLCLK: 424 | #cv2.circle(img, (x, y), 100, (255, 0, 0), -1) 425 | if not flag: 426 | sx, sy = y, x 427 | print sx, sy 428 | flag = True 429 | else: 430 | dx, dy = y, x 431 | print dx, dy 432 | 433 | def getSourceAndGoal(self): 434 | cv2.namedWindow('image') 435 | cv2.setMouseCallback('image', self.draw_circle) 436 | cv2.imshow('image', img) 437 | cv2.waitKey(0) 438 | 439 | def checkIfPathExist(self, p): # Checks if direct path has been found using RRT only 440 | if p[0] < self.goal[0] + 5 and p[1] < self.goal[1] + 5 and p[0] > self.goal[0] - 5 and p[1] > self.goal[1] - 5: 441 | return True 442 | return False 443 | 444 | def check_goal(self): # Ckecks if robot has reached the goal or not 445 | if self.current[0] < self.goal[0] + 2 and self.current[1] < self.goal[1] + 2 and self.current[0] > self.goal[0]-2 and self.current[1] > self.goal[1]-2: 446 | return True 447 | return False 448 | 449 | def startProcessing(self): 450 | arr = np.zeros(img.shape[:2], np.uint8) 451 | self.img = arr 452 | self.treeimage = np.zeros(img.shape[:2], np.uint8) 453 | count = 0 454 | # #self.markVisibleArea(img) 455 | # while not self.check_goal() and not self.goalFound: 456 | while not self.goalFound: 457 | self.markVisibleArea(img) 458 | # cv2.imshow('image', self.img) 459 | # k = cv2.waitKey(0) 460 | print "visible marked" 461 | if count == 0: 462 | self.growRRT(50) 463 | else: 464 | self.growRRT(10) 465 | count = count + 1 466 | print "Tree has been grown" 467 | print "goal Reached" 468 | self.printWholeTree() 469 | cv2.imshow('finalpath', self.treeimage) 470 | cv2.waitKey(0) 471 | 472 | start = RRTmodifiedAlgo() 473 | 474 | cv2.destroyAllWindows() -------------------------------------------------------------------------------- /input.txt: -------------------------------------------------------------------------------- 1 | 9 2 | 2 3 | 51 4 | 75 5 | 25 6 | 40 7 | 10 8 | 30 9 | 35 10 | 90 11 | -1 12 | 10 13 | 50 14 | 50 15 | 70 16 | 70 17 | 55 18 | -1 19 | 60 20 | 80 -------------------------------------------------------------------------------- /kdTree.py: -------------------------------------------------------------------------------- 1 | import math 2 | import Queue 3 | class node: 4 | def __init__(self, point, children, parent, actual, cost = 0): # node of RRT 5 | self.children = children # list of all children as a sigle node may have multiple number of nodes 6 | self.point = point 7 | self.actual = actual 8 | self.cost = cost 9 | self.parent = parent 10 | 11 | def add_child(self, node1): 12 | self.children.append(node1) 13 | 14 | def propogateCost(self): 15 | q = Queue.Queue() 16 | q.put(self) 17 | while not q.empty(): 18 | j = q.get() 19 | for i in j.children: 20 | i.cost = j.cost + math.hypot(i.point[0]-j.point[0], i.point[1]-j.point[1]) 21 | q.put(i) 22 | 23 | class kdTree(object): 24 | def __init__(self, left, right, axis, point, nde): 25 | self.left = left 26 | self.right = right 27 | self.axis = axis 28 | self.point = point 29 | self.nde = nde 30 | 31 | def insert(self, point, dim, nde): #for inserting a node in tree 32 | tp = self.point 33 | tx = self.axis 34 | if tp[tx] > point[tx]: 35 | if self.left == None: 36 | self.left = kdTree(None, None, (tx+1)%dim, point, nde) 37 | else: 38 | self.left.insert(point, dim, nde) 39 | else: 40 | if self.right == None: 41 | self.right = kdTree(None, None, (tx+1)%dim, point, nde) 42 | else: 43 | self.right.insert(point, dim, nde) 44 | 45 | def dist(self, point): 46 | total = 0 47 | for i in range(len(point)): 48 | total = total + (self.point[i]-point[i])*(self.point[i]-point[i]) 49 | total = math.sqrt(total) 50 | return total 51 | 52 | def search(self, point, dist, refp, nde, kdlink, kdlinkparent, selfparent): # for searching nearest neighbour 53 | axis = self.axis 54 | if self.left == None and self.right == None: 55 | #w = abs(self.point[axis]-point[axis]) 56 | w = self.dist(point) 57 | ret = [] 58 | if w < dist: 59 | ret.append(w) 60 | ret.append(self.point) 61 | ret.append(self.nde) 62 | ret.append(self) 63 | ret.append(selfparent) 64 | return ret 65 | else: 66 | ret.append(dist) 67 | ret.append(refp) 68 | ret.append(nde) 69 | ret.append(kdlink) 70 | ret.append(kdlinkparent) 71 | return ret 72 | 73 | else: 74 | d = self.dist(point) 75 | #d = abs(self.point[axis]-point[axis]) 76 | if d < dist: 77 | dist = d 78 | refp = self.point 79 | nde = self.nde 80 | kdlink = self 81 | kdlinkparent = selfparent 82 | if point[axis] <= self.point[axis]: 83 | if point[axis]-dist <= self.point[axis] and self.left != None: 84 | ret = self.left.search(point, dist, refp, nde, kdlink, kdlinkparent, self) 85 | dist = ret[0] 86 | refp = ret[1] 87 | nde = ret[2] 88 | kdlink = ret[3] 89 | kdlinkparent = ret[4] 90 | if point[axis]+dist > self.point[axis] and self.right != None: 91 | return self.right.search(point, dist, refp, nde, kdlink, kdlinkparent, self) 92 | else: 93 | if point[axis] + dist > self.point[axis] and self.right != None: 94 | ret = self.right.search(point, dist, refp, nde, kdlink, kdlinkparent, self) 95 | dist = ret[0] 96 | refp = ret[1] 97 | nde = ret[2] 98 | kdlink = ret[3] 99 | kdlinkparent = ret[4] 100 | if point[axis] - dist <= self.point[axis] and self.left != None: 101 | return self.left.search(point, dist, refp, nde, kdlink, kdlinkparent, self) 102 | ret = [] 103 | ret.append(dist) 104 | ret.append(refp) 105 | ret.append(nde) 106 | ret.append(kdlink) 107 | ret.append(kdlinkparent) 108 | return ret 109 | 110 | def searchNN(self, point, dist): # for searching nearest neighbour in radius dist 111 | axis = self.axis 112 | if self.left == None and self.right == None: 113 | # w = abs(self.point[axis]-point[axis]) 114 | w = self.dist(point) 115 | ret = [] 116 | if w < dist: 117 | temp = [self.point, w] 118 | ret.append(temp) 119 | return ret 120 | else: 121 | return ret 122 | 123 | else: 124 | d = self.dist(point) 125 | # d = abs(self.point[axis]-point[axis]) 126 | sol = [] 127 | if d < dist: 128 | temp = [self.point, d] 129 | sol.append(temp) 130 | if point[axis] <= self.point[axis]: 131 | if point[axis] - dist <= self.point[axis] and self.left != None: 132 | ret = self.left.searchNN(point, dist) 133 | sol = sol + ret 134 | if point[axis] + dist > self.point[axis] and self.right != None: 135 | ret = self.right.searchNN(point, dist) 136 | sol = sol + ret 137 | return sol 138 | else: 139 | if point[axis] + dist > self.point[axis] and self.right != None: 140 | ret = self.right.searchNN(point, dist) 141 | sol = sol+ret 142 | if point[axis] - dist <= self.point[axis] and self.left != None: 143 | ret = self.left.searchNN(point, dist) 144 | sol = sol + ret 145 | return sol 146 | return sol 147 | 148 | def findMin(self, d, dim, parent): # for finding minimum node in subtree rooted at nde 149 | #print 'traverse', self.point 150 | if self.right == None and self.left == None: 151 | return (self, parent) 152 | 153 | if self.axis == d: #condition that current node's division is equal to searched node'd division 154 | if self.left == None: 155 | return self, parent 156 | else: 157 | return self.left.findMin(d, dim, self) 158 | 159 | #Search both direction : left and right 160 | temp1 = None 161 | temp2 = None 162 | if self.left == None: 163 | po = [100000000000 for i in range(dim)] 164 | temp1 = kdTree(None, None, d, po, None), None 165 | else: 166 | temp1 = self.left.findMin(d, dim, self) 167 | 168 | if self.right == None: 169 | po = [100000000000 for i in range(dim)] 170 | temp2 = kdTree(None, None, d, po, None), None 171 | else: 172 | temp2 = self.right.findMin(d, dim, self) 173 | 174 | temp = self, parent 175 | x = min(temp[0].point[d], min(temp1[0].point[d], temp2[0].point[d])) 176 | if x == temp[0].point[d]: 177 | return temp 178 | elif x == temp1[0].point[d]: 179 | return temp1 180 | else: 181 | return temp2 182 | 183 | def set_none(self, node): 184 | pass 185 | 186 | def check_leaf(self): 187 | if self.right == None and self.left == None: 188 | return True 189 | return False 190 | 191 | def deleteNode(self, parent): #for deleting a node nde 192 | if self.check_leaf(): 193 | if parent != None: 194 | if parent.right == self: 195 | parent.right = None 196 | else: 197 | parent.left = None 198 | del self 199 | return 200 | 201 | if self.right != None: 202 | mini = self.right.findMin(self.axis, dim, self) 203 | self.point = mini[0].point 204 | self.nde = mini[0].nde 205 | self.right.deleteNode(self) 206 | 207 | elif self.left != None: 208 | mini = self.left.findMin(self.axis, dim, self) 209 | self.point = mini[0].point 210 | self.nde = mini[0].nde 211 | self.left.deleteNode(self) 212 | self.right = self.left 213 | self.left = None 214 | 215 | def print_tree(self): 216 | if self.left != None: 217 | self.left.print_tree() 218 | print self.point, self.axis 219 | if self.right != None: 220 | self.right.print_tree() 221 | 222 | dim = 2 223 | def main(): 224 | print 'Enter Points' 225 | n = int(input()) # number of points 226 | global dim 227 | temp = int(input()) #number of dimensions 228 | 229 | root = None 230 | 231 | for i in range(n): 232 | point = [] 233 | for j in range(dim): 234 | point.append(int(input())) 235 | if root == None: 236 | root = kdTree(None, None, 0, point, None) 237 | else: 238 | root.insert(point, dim, None) 239 | 240 | root.print_tree() 241 | 242 | ''' 243 | print "Enter a point to search" 244 | point = [] 245 | for i in range(dim): 246 | point.append(int(input())) 247 | 248 | ret = root.search(point, 1000000000000, None, None) 249 | print "Nearest Neighbour " 250 | print ret 251 | ''' 252 | print " " 253 | 254 | #ret = root.findMin(0, dim) 255 | #print ret.point, ret.axis 256 | #ret.deleteNode() 257 | #ret = root.findMin(1, dim) 258 | 259 | #print ret.point, ret.axis 260 | #ret = root.findMin(1, dim, None) 261 | ret = root.search([51, 75], 100000000000000000, None, None, None, None, None) 262 | print " " 263 | print ret[1] 264 | print " " 265 | ret[3].deleteNode(ret[4]) 266 | #ret.deleteNode() 267 | root.print_tree() 268 | 269 | if __name__ == "__main__" : 270 | main() -------------------------------------------------------------------------------- /rrt_brute_force.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys, random, math, pygame 4 | from pygame.locals import * 5 | from math import sqrt,cos,sin,atan2 6 | 7 | #constants 8 | XDIM = 640 9 | YDIM = 480 10 | WINSIZE = [XDIM, YDIM] 11 | EPSILON = 7.0 12 | NUMNODES = 5000 13 | 14 | def dist(p1,p2): 15 | return sqrt((p1[0]-p2[0])*(p1[0]-p2[0])+(p1[1]-p2[1])*(p1[1]-p2[1])) 16 | 17 | def step_from_to(p1,p2): 18 | if dist(p1,p2) < EPSILON: 19 | return p2 20 | else: 21 | theta = atan2(p2[1]-p1[1],p2[0]-p1[0]) 22 | return p1[0] + EPSILON*cos(theta), p1[1] + EPSILON*sin(theta) 23 | 24 | def main(): 25 | #initialize and prepare screen 26 | pygame.init() 27 | screen = pygame.display.set_mode(WINSIZE) 28 | pygame.display.set_caption('RRT brute force') 29 | white = 255, 240, 200 30 | black = 20, 20, 40 31 | screen.fill(black) 32 | 33 | nodes = [] 34 | 35 | nodes.append((XDIM/2.0,YDIM/2.0)) # Start in the center 36 | # nodes.append((0.0,0.0)) # Start in the corner 37 | 38 | for i in range(NUMNODES): 39 | rand = random.random()*640.0, random.random()*480.0 40 | nn = nodes[0] 41 | for p in nodes: 42 | if dist(p,rand) < dist(nn,rand): 43 | nn = p 44 | newnode = step_from_to(nn,rand) 45 | nodes.append(newnode) 46 | pygame.draw.line(screen,white,nn,newnode) 47 | pygame.display.update() 48 | #print i, " ", nodes 49 | 50 | for e in pygame.event.get(): 51 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 52 | sys.exit("Leaving because you requested it.") 53 | 54 | 55 | # if python says run, then we should run 56 | if __name__ == '__main__': 57 | main() 58 | 59 | 60 | -------------------------------------------------------------------------------- /rrt_star_kdtree.py: -------------------------------------------------------------------------------- 1 | from kdTree import kdTree 2 | from kdTree import node 3 | import sys, random, pygame ,time 4 | from pygame.locals import * 5 | from math import sqrt,cos,sin,atan2, hypot 6 | # import kdtree 7 | import numpy as np 8 | 9 | XDIM = 640 10 | YDIM = 480 11 | WINSIZE = [XDIM, YDIM] 12 | EPSILON = 50 13 | NUMNODES = 5000 14 | dim = 2 15 | RADIUS = 10 16 | 17 | class xpoint: 18 | def __init__(self, pnt): 19 | self.x = pnt[0] 20 | self.ypoint = ypoint(self, pnt[1]) 21 | self.left = None 22 | self.right = None 23 | self.count = 1 24 | self.points = [pnt] 25 | pass 26 | 27 | def insertpoint(self, pnt): 28 | self.count = self.count + 1 29 | if pnt[0] < self.x: 30 | if self.left == None: 31 | self.left = xpoint(pnt) 32 | else: 33 | self.left.insertpoint(pnt) 34 | elif pnt[0] > self.x: 35 | if self.right == None: 36 | self.right = xpoint(pnt) 37 | else: 38 | self.right.insertpoint(pnt) 39 | else: 40 | self.points.append(pnt) 41 | self.ypoint.insertpnt(self, pnt[1]) 42 | 43 | def query(self, l , r): 44 | pass 45 | 46 | class ypoint: 47 | def __init__(self, xpnt, y): 48 | self.xpnt = xpnt 49 | self.y = y 50 | self.pnts = [y] 51 | self.count = 1 52 | self.right = None 53 | self.left = None 54 | pass 55 | 56 | def insertpnt(self, xpnt, y): 57 | self.count = self.count + 1 58 | if y < self.y: 59 | self.pnts = [y] + self.pnts 60 | if self.left == None: 61 | self.left = ypoint(xpnt, y) 62 | else: 63 | self.left.insertpnt(xpnt, y) 64 | else: 65 | self.pnts.append(y) 66 | if self.right == None: 67 | self.right = ypoint(xpnt, y) 68 | else: 69 | self.right.insertpnt(xpnt, y) 70 | 71 | def query(self, l, r): 72 | if self.y < l: 73 | if self.right != None: 74 | return self.right.query(l, r) 75 | else: 76 | return [] 77 | elif self.y > r: 78 | if self.left != None: 79 | return self.left.query(l, r) 80 | else: 81 | return [] 82 | pass 83 | 84 | class RRTAlgorithm(object): 85 | def __init__(self, source, goal, nodes): #initial and destination coordinates and number of nodes 86 | print source, goal 87 | self.costMap = [[10000000000 for x in range(YDIM)] for x in range(XDIM)] 88 | self.path = None 89 | self.goalNode = None 90 | self.start(source, goal, nodes) 91 | 92 | def addConnection1(self, Points, rand, screen): 93 | 94 | ret = Points.search(rand, 100000000000000, None, None, None, None, None) 95 | nearest_neighbour = ret[1] 96 | new_point = self.step_from_to(nearest_neighbour, rand) 97 | nde = node(new_point, [], ret[2], True, ret[2].cost + hypot(new_point[0] - ret[1][0], new_point[1] - ret[1][1])) 98 | ret[2].add_child(nde) 99 | Points.insert(new_point, dim, nde) 100 | current = new_point 101 | new_point = [int(new_point[0]), int(new_point[1])] 102 | pygame.draw.line(screen, (100, 100, 100), nearest_neighbour, new_point) 103 | pygame.display.update() 104 | return new_point 105 | pass 106 | 107 | def sortdist(self, n): 108 | return n[1] 109 | 110 | def updateSubtreCost(self, nde): 111 | for i in nde.children: 112 | if self.costMap[i.point[0]][ i.point[1]] < self.costMap[nde.point[0]][ nde.point[1]] + hypot(i.point[0]-nde.point[0], i.point[1]-nde.point[1]): 113 | print "yes" 114 | self.costMap[i.point[0]][ i.point[1]] = self.costMap[nde.point[0]][ nde.point[1]] + hypot(i.point[0]-nde.point[0], i.point[1]-nde.point[1]) 115 | self.updateSubtreCost(i) 116 | 117 | def createNewLink(self, childlink, parentlink, screen): 118 | 119 | oldParent = childlink.parent 120 | oldParent.children.remove(childlink) 121 | childlink.parent = parentlink 122 | parentlink.children.append(childlink) 123 | childlink.propogateCost() 124 | #self.updateSubtreCost(childlink) 125 | 126 | pygame.draw.line(screen, (20, 20, 40), oldParent.point, childlink.point) 127 | pygame.draw.line(screen, (100, 100, 100), parentlink.point, childlink.point) 128 | 129 | def findKNN(self, point, Pointmap): 130 | start = [int(point[0]-RADIUS), int(point[1]-RADIUS)] 131 | end = [int(point[0] + RADIUS), int(point[1] + RADIUS)] 132 | if start[0] < 0: 133 | start[0] = 0 134 | if start[1] < 0: 135 | start[1] = 0 136 | if end[0] >= XDIM: 137 | end[0] = XDIM-1 138 | if end[1] >= YDIM: 139 | end[1] = YDIM-1 140 | ret = [] 141 | 142 | for i in range(start[0], end[0]): 143 | for j in range(start[1], end[1]): 144 | if Pointmap[i][j] == 1: 145 | ret.append(([i, j], hypot(point[0]-i, point[1]-j))) 146 | return ret 147 | pass 148 | 149 | def addConnections(self, Points, Pointmap, new_point, screen, source): 150 | ret = Points.search(new_point, 1000000000000000000, None, None, None, None, None) 151 | nodes = [] 152 | 153 | #nodes = self.findKNN(new_point, Pointmap) 154 | nodes = Points.searchNN(new_point, RADIUS) 155 | #print len(nodes) 156 | flag = False 157 | for i in nodes: 158 | if ret[1] == i[0]: 159 | flag = True 160 | break 161 | if not flag: 162 | nodes.append((ret[1], ret[0])) 163 | 164 | sorted(nodes, key = self.sortdist) 165 | 166 | nn = nodes[0][0] 167 | #print 'nn ', nn 168 | cost = [] 169 | for i in nodes: 170 | cost.append(Points.search(i[0], 100000000000000000000, None, None, None, None, None)[2].cost) 171 | mincost = 10000000000000000000000000 172 | for i in range(len(nodes)): 173 | nn1 = nodes[i][0] 174 | if cost[i] + nodes[i][1] < mincost: 175 | mincost = cost[i] + nodes[i][1] 176 | nn = nn1 177 | pnt = nn 178 | ret = Points.search(pnt, 100000000000000, None, None, None, None, None) 179 | nearest_neighbour = ret[1] 180 | new_point = self.step_from_to(nearest_neighbour, new_point) 181 | new_point = [int(new_point[0]), int(new_point[1])] 182 | Pointmap[new_point[0]][new_point[1]] = 1 183 | pygame.draw.line(screen, (100, 100, 100), nn, new_point) 184 | pygame.display.update() 185 | c = mincost 186 | nde = node(new_point, [], ret[2], True, c) 187 | ret[2].add_child(nde) 188 | Points.insert(new_point, 2, nde) 189 | 190 | ''' 191 | 192 | Update Other links 193 | 194 | ''' 195 | 196 | flag = False 197 | # if self.goalNode !=None: 198 | # flag = True 199 | if self.path != None: 200 | for i in nodes: 201 | pnt1 = i[0] 202 | if pnt != pnt1 and pnt1 in self.path: 203 | flag = True 204 | if flag: 205 | nde1 = self.goalNode 206 | while nde1.parent != None: 207 | pygame.draw.line(screen, (100, 100, 100), nde1.point, nde1.parent.point) 208 | nde1 = nde1.parent 209 | #print "rubbed" 210 | pygame.display.update() 211 | #time.sleep(10) 212 | 213 | for i in range(len(nodes)): 214 | pnt = nodes[i][0] 215 | if pnt != nearest_neighbour and pnt != source: 216 | if cost[i] > c + hypot(pnt[0]-new_point[0], pnt[1]-new_point[1]) and hypot(pnt[0]-new_point[0], pnt[1]-new_point[1]) < EPSILON: 217 | child = Points.search(pnt, 10000000000000000000000000, None, None, None, None, None)[2] 218 | child.cost = c + hypot(pnt[0] - new_point[0], pnt[1] - new_point[1]) 219 | self.createNewLink(child, nde, screen) 220 | 221 | if flag: 222 | nodes = Points.searchNN(self.goal, 10) 223 | sorted(nodes, key=self.sortdist) 224 | pnt = nodes[0][0] 225 | self.path = [] 226 | nde = self.goalNode = Points.search(pnt, 1000000000000000000, None, None, None, None, None)[2] 227 | while nde.parent != None: 228 | pygame.draw.line(screen, (255, 255, 255), nde.point, nde.parent.point) 229 | self.path.append(nde.point) 230 | #print "point ", nde.point 231 | nde = nde.parent 232 | pygame.display.update() 233 | return new_point 234 | 235 | def start(self, source, goal, nodes): 236 | self.source = source 237 | self.goal = goal 238 | pygame.init() 239 | screen = pygame.display.set_mode(WINSIZE) 240 | pygame.display.set_caption('RRT star using KdTrees') 241 | white = 100, 100, 100 242 | black = 20, 20, 40 243 | bright = 255, 255, 255 244 | screen.fill(black) 245 | 246 | RRTree = node(source, [], None, True) #actual RRTree 247 | Points = kdTree(None, None, 0, source, RRTree) #for storing generated points to increase the search complexity 248 | #NearestNeighbourTree = kdtree.create([source]) # for storing points same as Points, using it for finding points in a range 249 | current = source 250 | Pointmap = [[0 for i in range (YDIM)] for i in range(XDIM)] 251 | Pointmap[source[0]][source[1]] = 1 252 | 253 | count = 0 254 | 255 | while not self.check(current, goal): 256 | 257 | rand = [int(random.random() * 640.0), int(random.random() * 480.0)] 258 | while Pointmap[rand[0]][rand[1]] == 1: 259 | rand = [int(random.random() * 640.0), int(random.random() * 480.0)] 260 | current = self.addConnections(Points, Pointmap, rand, screen, source) 261 | count = count + 1 262 | #current = self.addConnection1(Points, rand, screen) 263 | 264 | pygame.display.update() 265 | for e in pygame.event.get(): 266 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 267 | sys.exit("Leaving .") 268 | 269 | ret = Points.search(current, 100000000000000000000, None, None, None, None, None) 270 | nde = ret[2] 271 | self.goalNode = nde 272 | path = [] 273 | while nde.parent != None: 274 | print nde.point, nde.cost 275 | path.append(nde.point) 276 | pygame.draw.line(screen, bright, nde.point, nde.parent.point) 277 | pygame.display.update() 278 | nde = nde.parent 279 | if nde.parent == nde: 280 | break 281 | time.sleep(0.05) 282 | for e in pygame.event.get(): 283 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 284 | sys.exit("Leaving.") 285 | print 'count', count 286 | self.path = path 287 | for i in range(10000-count): 288 | rand = [int(random.random() * 640.0), int(random.random() * 480.0)] 289 | while Pointmap[rand[0]][rand[1]] == 1: 290 | rand = [int(random.random() * 640.0), int(random.random() * 480.0)] 291 | current = self.addConnections(Points, Pointmap, rand, screen, source) 292 | pygame.display.update() 293 | for e in pygame.event.get(): 294 | if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): 295 | sys.exit("Leaving.") 296 | 297 | pass 298 | 299 | def printchildren(self, nde): 300 | print nde.point, nde.cost 301 | for i in nde.children: 302 | print i.point, i.cost 303 | 304 | def check(self, point , goal): # checking if currently added node is at goal or not 305 | if point[0] > goal[0]-5 and point[0] < goal[0]+5 and point[1] > goal[1]-5 and point[1] < goal[1]+5: 306 | return True 307 | return False 308 | 309 | def dist(self, p1, p2): #returns euclid's distance between points p1 and p2 310 | return sqrt((p1[0] - p2[0]) * (p1[0] - p2[0]) + (p1[1] - p2[1]) * (p1[1] - p2[1])) 311 | 312 | def step_from_to(self,p1, p2): #returns point with at most epsilon distance from nearest neighbour in the direction of randomly generated point 313 | if self.dist(p1, p2) < EPSILON: 314 | return p2 315 | else: 316 | theta = atan2(p2[1] - p1[1], p2[0] - p1[0]) 317 | return [p1[0] + EPSILON * cos(theta), p1[1] + EPSILON * sin(theta)] 318 | 319 | def main(): 320 | tree = RRTAlgorithm([635, 475], [20, 40], 2000) 321 | 322 | main() -------------------------------------------------------------------------------- /rrtstar_brute_force.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys, random, math, pygame 4 | from pygame.locals import * 5 | from math import sqrt,cos,sin,atan2 6 | 7 | #constants 8 | XDIM = 640 9 | YDIM = 480 10 | WINSIZE = [XDIM, YDIM] 11 | EPSILON = 7.0 12 | NUMNODES = 5000 13 | RADIUS=15 14 | 15 | def dist(p1,p2): 16 | return sqrt((p1[0]-p2[0])*(p1[0]-p2[0])+(p1[1]-p2[1])*(p1[1]-p2[1])) 17 | 18 | def step_from_to(p1,p2): 19 | if dist(p1,p2) < EPSILON: 20 | return p2 21 | else: 22 | theta = atan2(p2[1]-p1[1],p2[0]-p1[0]) 23 | return p1[0] + EPSILON*cos(theta), p1[1] + EPSILON*sin(theta) 24 | 25 | def chooseParent(nn,newnode,nodes): 26 | for p in nodes: 27 | if dist([p.x,p.y],[newnode.x,newnode.y])