├── .gitignore ├── include ├── 3rdParty │ └── ALGLIB │ │ ├── stdafx.h │ │ ├── diffequations.h │ │ ├── fasttransforms.h │ │ └── integration.h ├── filtering │ ├── kalman_filter.hpp │ └── ekf.hpp ├── planning │ ├── prm.h │ ├── rrt.h │ └── a_star.h └── optimization │ └── minimum_snap_control.hpp ├── maps ├── map_1.png ├── map_2.png ├── map_3.png └── map_4.png ├── results ├── filtering │ └── kf.png ├── planning │ ├── astar.png │ ├── dstar.png │ ├── prm.png │ ├── rrt.png │ ├── rrt2.png │ ├── djikstra.png │ ├── lpastar1.png │ ├── lpastar2.png │ ├── rrt_star.png │ └── rrt_star2.png └── optimization │ ├── minimum_snap2.png │ ├── minimum_snap3.png │ ├── minimum_snap4.png │ └── minimum_snapQP.png ├── src ├── optimization │ └── minimum_snap_control.cpp ├── filtering │ ├── ekf.cpp │ └── kalman_filter.cpp └── planning │ ├── rrt.cpp │ ├── prm.cpp │ ├── a_star.cpp │ ├── rrt_star.cpp │ └── d_star.cpp ├── CMakeLists.txt ├── .vscode └── settings.json └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | -------------------------------------------------------------------------------- /include/3rdParty/ALGLIB/stdafx.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /maps/map_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/maps/map_1.png -------------------------------------------------------------------------------- /maps/map_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/maps/map_2.png -------------------------------------------------------------------------------- /maps/map_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/maps/map_3.png -------------------------------------------------------------------------------- /maps/map_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/maps/map_4.png -------------------------------------------------------------------------------- /results/filtering/kf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/results/filtering/kf.png -------------------------------------------------------------------------------- /results/planning/astar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/results/planning/astar.png -------------------------------------------------------------------------------- /results/planning/dstar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/results/planning/dstar.png -------------------------------------------------------------------------------- /results/planning/prm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/results/planning/prm.png -------------------------------------------------------------------------------- /results/planning/rrt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/results/planning/rrt.png -------------------------------------------------------------------------------- /results/planning/rrt2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/results/planning/rrt2.png -------------------------------------------------------------------------------- /results/planning/djikstra.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/results/planning/djikstra.png -------------------------------------------------------------------------------- /results/planning/lpastar1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/results/planning/lpastar1.png -------------------------------------------------------------------------------- /results/planning/lpastar2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/results/planning/lpastar2.png -------------------------------------------------------------------------------- /results/planning/rrt_star.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/results/planning/rrt_star.png -------------------------------------------------------------------------------- /results/planning/rrt_star2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/results/planning/rrt_star2.png -------------------------------------------------------------------------------- /results/optimization/minimum_snap2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/results/optimization/minimum_snap2.png -------------------------------------------------------------------------------- /results/optimization/minimum_snap3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/results/optimization/minimum_snap3.png -------------------------------------------------------------------------------- /results/optimization/minimum_snap4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/results/optimization/minimum_snap4.png -------------------------------------------------------------------------------- /results/optimization/minimum_snapQP.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CHOcho-quan/RoboticsAlgorithms/HEAD/results/optimization/minimum_snapQP.png -------------------------------------------------------------------------------- /src/optimization/minimum_snap_control.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by quan 3 | // 2020.3.28 4 | // 5 | 6 | #include "minimum_snap_control.hpp" 7 | 8 | int main() 9 | { 10 | std::vector x = {54, 157, 266, 280, 400, 450, 408, 307}, y = {78, 88, 100, 230, 435, 505, 600, 701}; 11 | MinSnapOptimizer m(6, x, y); 12 | // m.MinSnapOptimizeTest(); 13 | m.MinSnapOptimizeMultiple(2); 14 | // cv::resize(m.background, m.background, cv::Size(300, 300)); 15 | cv::imwrite("../results/optimization/minimum_snap4.png", m.background); 16 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(CppRobotics_quan) 2 | 3 | find_package(Eigen3 REQUIRED) 4 | find_package(OpenCV REQUIRED) 5 | 6 | message(STATUS ${OpenCV_LIBS}) 7 | 8 | add_definitions(-std=c++11) 9 | 10 | set(include_dir ${PROJECT_SOURCE_DIR}/include/planning 11 | ${PROJECT_SOURCE_DIR}/include/optimization 12 | ${PROJECT_SOURCE_DIR}/include/filtering 13 | ${PROJECT_SOURCE_DIR}/include/3rdParty 14 | ${EIGEN3_INCLUDE_DIR}) 15 | include_directories(${include_dir}) 16 | 17 | add_executable(astar src/planning/a_star.cpp) 18 | target_link_libraries(astar ${OpenCV_LIBS} ) 19 | 20 | add_executable(dstar src/planning/d_star.cpp) 21 | target_link_libraries(dstar ${OpenCV_LIBS} ) 22 | 23 | add_executable(rrt src/planning/rrt.cpp) 24 | target_link_libraries(rrt ${OpenCV_LIBS} ) 25 | 26 | add_executable(rrt_star src/planning/rrt_star.cpp) 27 | target_link_libraries(rrt_star ${OpenCV_LIBS} ) 28 | 29 | add_executable(prm src/planning/prm.cpp) 30 | target_link_libraries(prm ${OpenCV_LIBS} ) 31 | 32 | add_executable(kf src/filtering/kalman_filter.cpp) 33 | target_link_libraries(kf ${OpenCV_LIBS} ) 34 | 35 | add_executable(ekf src/filtering/ekf.cpp) 36 | target_link_libraries(ekf ${OpenCV_LIBS} ) 37 | 38 | file(GLOB ALGLIB_SOURCES include/3rdParty/ALGLIB/*.cpp ) 39 | file(GLOB ALGLIB_HEADERS include/3rdParty/ALGLIB/*.h ) 40 | add_executable(m_snap src/optimization/minimum_snap_control.cpp ${ALGLIB_HEADERS} ${ALGLIB_SOURCES}) 41 | target_link_libraries(m_snap ${OpenCV_LIBS}) 42 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "cmath": "cpp", 4 | "complex": "cpp", 5 | "cctype": "cpp", 6 | "clocale": "cpp", 7 | "cstdarg": "cpp", 8 | "cstddef": "cpp", 9 | "cstdio": "cpp", 10 | "cstdlib": "cpp", 11 | "cstring": "cpp", 12 | "ctime": "cpp", 13 | "cwchar": "cpp", 14 | "cwctype": "cpp", 15 | "array": "cpp", 16 | "atomic": "cpp", 17 | "strstream": "cpp", 18 | "*.tcc": "cpp", 19 | "bitset": "cpp", 20 | "chrono": "cpp", 21 | "cstdint": "cpp", 22 | "deque": "cpp", 23 | "list": "cpp", 24 | "unordered_map": "cpp", 25 | "vector": "cpp", 26 | "exception": "cpp", 27 | "algorithm": "cpp", 28 | "functional": "cpp", 29 | "optional": "cpp", 30 | "ratio": "cpp", 31 | "string_view": "cpp", 32 | "system_error": "cpp", 33 | "tuple": "cpp", 34 | "type_traits": "cpp", 35 | "fstream": "cpp", 36 | "initializer_list": "cpp", 37 | "iomanip": "cpp", 38 | "iosfwd": "cpp", 39 | "iostream": "cpp", 40 | "istream": "cpp", 41 | "limits": "cpp", 42 | "memory": "cpp", 43 | "new": "cpp", 44 | "ostream": "cpp", 45 | "numeric": "cpp", 46 | "sstream": "cpp", 47 | "stdexcept": "cpp", 48 | "streambuf": "cpp", 49 | "thread": "cpp", 50 | "cfenv": "cpp", 51 | "cinttypes": "cpp", 52 | "utility": "cpp", 53 | "typeindex": "cpp", 54 | "typeinfo": "cpp", 55 | "valarray": "cpp", 56 | "iterator": "cpp", 57 | "map": "cpp", 58 | "memory_resource": "cpp", 59 | "random": "cpp", 60 | "set": "cpp", 61 | "string": "cpp", 62 | "dense": "cpp" 63 | } 64 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RoboticsAlgorithms 2 | 3 | ### My Personal Space to practice Robotics Algorithms on planning, mapping, filtering & optimization. 4 | 5 | ## Planning 6 | 7 | - Search-based: A*, LP-A* and D* Algorithm Planning\ 8 | Simple A* planning with global obstacle map, compared to djikstra.\ 9 | !["A*"](./results/planning/astar.png) !["Djikstra"](./results/planning/djikstra.png) 10 | 11 | LP-A* planning to do replan after slight map change. First pic is LP-A* planning for the first time. Second pic is replanned path after map changing. Third pic is D* with local map only.\ 12 | !["LPA*-1"](./results/planning/lpastar1.png)!["LPA*-2"](./results/planning/lpastar2.png)!["D*-Lite"](./results/planning/dstar.png) 13 | 14 | - Sample-based: PRM & RRT\ 15 | Simple PRM & RRT(RRT*) planning with global obstacle map.\ 16 | !["PRM"](./results/planning/prm.png) !["RRT"](./results/planning/rrt.png)\ 17 | RRT* can get the result optimized, but they both perform bad in maps having narrow channels.\ 18 | !["RRT*"](./results/planning/rrt_star.png) !["RRT_Tough"](./results/planning/rrt2.png) 19 | 20 | ### Note: maps are from an open source github [repo](https://github.com/XM522706601/robotics_tutorial_for_zhihu) 21 | 22 | ## Filtering 23 | 24 | - Kalman Filter\ 25 | Kalman filter simulation with simple motion model. Extended Kalman Filter is also implemented with same motion model adding nonlinearity by angle velocity. 26 | 27 | ## Optimization 28 | 29 | - Minimum Snap Trajectory Optimization for quadrotor\ 30 | The algorithm was implemented only the version of equalized time split. Simply changing equality constraint to inequality can get it to the corridor version. In corridor version, it's OK for us to do collision detection and enable the robot to move. Time split can be calculated by acceleration and velocity. Also the trajectory was not the best. To be convenient for the simulation, we only consider 1st derivative continous on the connecting point.\ 31 | !["Minimum_snap"](./results/optimization/minimum_snapQP.png)!["ContinuousOpitimization"](results/optimization/minimum_snap2.png) 32 | >Reference: [1] Mellinger D, Kumar V. Minimum snap trajectory generation and control for quadrotors, Robotics and Automation (ICRA), 2011\ 33 | >[2] Chen J, Liu T, Shen S. Online generation of collision-free trajectories for quadrotor flight in unknown cluttered environments, Robotics and Automation (ICRA), 2016 34 | -------------------------------------------------------------------------------- /include/filtering/kalman_filter.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by quan 3 | // 2020.4.6 4 | // 5 | #ifndef INCLUDE_KALMAN_FILTER 6 | #define INCLUDE_KALMAN_FILTER 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define DEBUG 1 14 | 15 | class LinearDynamicSystem 16 | { 17 | public: 18 | /** 19 | * Formulation of a linear dynamic system model, simple 2D version 20 | * Xt = A * Xt-1 + B * u + e1 21 | * Zt = C * Xt + e2 22 | * Xt = [x, y, Vx, Vy], u = [ax, ay] 23 | * where e1 ~ N(0, sigma1) and e2 ~ N(0, sigma2) 24 | */ 25 | Eigen::Matrix4d A; // State Transfer Matrix 26 | Eigen::Matrix4d B; 27 | Eigen::MatrixXd C; // Observation Matrix 28 | const int state_size = 2, obs_size = 2; 29 | Eigen::Matrix2d R; // Error Covarience Matrix 30 | Eigen::Matrix4d Q; 31 | 32 | LinearDynamicSystem() {} 33 | LinearDynamicSystem &operator=(LinearDynamicSystem &lds) { 34 | A = lds.A; 35 | B = lds.B; 36 | C = lds.C; 37 | Q = lds.Q; 38 | R = lds.R; 39 | } 40 | 41 | LinearDynamicSystem(const LinearDynamicSystem &lds) { 42 | A = lds.A; 43 | B = lds.B; 44 | C = lds.C; 45 | Q = lds.Q; 46 | R = lds.R; 47 | } 48 | 49 | LinearDynamicSystem(double cov1, double cov2) { 50 | C = Eigen::MatrixXd(2, 4); 51 | Q(0, 0) = cov1; 52 | Q(1, 1) = cov1; 53 | Q(2, 2) = cov1; 54 | Q(3, 3) = cov1; 55 | R(0, 0) = cov2; 56 | R(1, 1) = cov2; 57 | } 58 | 59 | Eigen::Vector2d getObservation(Eigen::Vector4d input, Eigen::Vector2d error) { 60 | /** 61 | * @brief get current observation given input 62 | * @param input - current state input 63 | * @param error - the error caused by uncertainty - Gaussian 64 | * @return observation vector 65 | */ 66 | Eigen::Vector2d result = C * input + R * error; 67 | 68 | return result; 69 | } 70 | 71 | Eigen::Vector4d getNextState(Eigen::Vector4d last_state, Eigen::Vector4d control_input, Eigen::Vector4d error) { 72 | /** 73 | * @brief get next state given current state and control input 74 | * @param last_state - current state 75 | * @return next state vector 76 | */ 77 | Eigen::Vector4d state = A * last_state + B * control_input + Q * error; 78 | 79 | return state; 80 | } 81 | 82 | Eigen::Vector4d getReckonedNextState(Eigen::Vector4d last_state, Eigen::Vector4d control_input) { 83 | /** 84 | * @brief getting Dead Reckoned state to compare with 85 | * @return reckoned state vector 86 | */ 87 | return A * last_state + B * control_input; 88 | } 89 | }; 90 | 91 | #endif -------------------------------------------------------------------------------- /src/filtering/ekf.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by quan 3 | // 2020.4.11 4 | // 5 | #include "ekf.hpp" 6 | 7 | class ExtendedKalmanFilter { 8 | /** 9 | * Extended Linear Kalman Filter for Nonlinear dynamic system 10 | * predict - for the kalman filter predict section 11 | * update - for the kalman filter update section 12 | */ 13 | public: 14 | Eigen::MatrixXd kalman_gain; 15 | Eigen::Vector4d prediction; 16 | Eigen::Vector4d filtered; 17 | Eigen::Matrix4d covarience; 18 | Eigen::Matrix4d last_covarience; 19 | NonlinearDynamicSystem nlds; 20 | 21 | ExtendedKalmanFilter(NonlinearDynamicSystem s) { 22 | /** 23 | * @brief Initialization of kalman filter 24 | * @param s - the given dynamic system 25 | */ 26 | nlds = s; 27 | // Initial Eye matrix for covariences 28 | covarience = Eigen::Matrix4d::Identity(); 29 | last_covarience = Eigen::Matrix4d::Identity(); 30 | kalman_gain = Eigen::MatrixXd(4, 2); 31 | } 32 | void predict(Eigen::Vector2d control_input) { 33 | /** 34 | * @brief prediction phase of kalman filter 35 | */ 36 | auto jA = nlds.getJacobA(control_input); 37 | prediction = nlds.A * filtered + nlds.B * control_input; 38 | covarience = jA * last_covarience * jA.transpose() + nlds.Q; 39 | } 40 | void update(Eigen::Vector2d obs) { 41 | /** 42 | * @brief update phase of kalman filter 43 | * @param obs - given observation of current state 44 | */ 45 | kalman_gain = covarience * nlds.H.transpose() * 46 | (nlds.H * covarience * nlds.H.transpose() + nlds.R).inverse(); 47 | filtered = prediction + kalman_gain * (obs - nlds.H * prediction); 48 | last_covarience = covarience - kalman_gain * nlds.H * covarience; 49 | } 50 | }; 51 | 52 | int main() 53 | { 54 | NonlinearDynamicSystem nlds(0.5, 0.5); 55 | nlds.A << 1, 0, 0, 0, 56 | 0, 1, 0, 0, 57 | 0, 0, 1, 0, 58 | 0, 0, 0, 1; 59 | nlds.H << 1, 0, 0, 0, 60 | 0, 1, 0, 0; 61 | 62 | ExtendedKalmanFilter ekf(nlds); 63 | 64 | // Initial State of position 65 | Eigen::Vector4d pos(1.0, 1.0, 0.5, M_PI / 3), reckon(1.0, 1.0, 0.5, M_PI / 3); 66 | ekf.filtered(0) = pos(0); 67 | ekf.filtered(1) = pos(1); 68 | ekf.filtered(2) = pos(2); 69 | ekf.filtered(3) = pos(3); 70 | nlds.setCurState(pos); 71 | 72 | // Control Input 73 | Eigen::Vector2d u(1.0, M_PI / 720); 74 | 75 | // Randomized tools 76 | std::random_device rd{}; 77 | std::mt19937 gen{rd()}; 78 | std::normal_distribution<> n{0, 1}; // Normal Distribution 79 | 80 | double T = 50.0; 81 | for (double t = 0.0;t < T;t+=dT) { 82 | Eigen::Vector4d reckoned, errorState; 83 | Eigen::Vector2d errorObs; 84 | 85 | errorObs(0) = n(gen); 86 | errorObs(1) = n(gen); 87 | errorState(0) = n(gen); 88 | errorState(1) = n(gen); 89 | errorState(2) = n(gen); 90 | errorState(3) = n(gen); 91 | 92 | pos = nlds.getNextState(pos, u, errorState); 93 | reckon = nlds.getReckonedNextState(reckon, u); 94 | Eigen::Vector2d obs = nlds.getObservation(pos, errorObs); 95 | 96 | ekf.predict(u); 97 | ekf.update(obs); 98 | 99 | if (DEBUG) std::cout << "EKF: " << ekf.filtered(0) << ' ' << ekf.filtered(1) << std::endl; 100 | if (DEBUG) std::cout << "POS: " << pos(0) << ' ' << pos(1) << std::endl; 101 | if (DEBUG) std::cout << "OBS: " << obs(0) << ' ' << obs(1) << std::endl; 102 | if (DEBUG) std::cout << "RECKON: " << reckon(0) << ' ' << reckon(1) << std::endl; 103 | } 104 | } -------------------------------------------------------------------------------- /include/filtering/ekf.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by quan 3 | // 2020.4.11 4 | // 5 | #ifndef INCLUDE_EXTENDED_KALMAN_FILTER 6 | #define INCLUDE_EXTENDED_KALMAN_FILTER 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #define dT 0.1 15 | #define DEBUG 1 16 | 17 | class NonlinearDynamicSystem { 18 | /** 19 | * Formulation of a linear dynamic system model, simple 2D version 20 | * Xt = A * Xt-1 + B * u + e1 21 | * Zt = H * Xt + e2 22 | * Xt = [x, y, v, theta], u = [v, w] 23 | * where e1 ~ N(0, sigma1) and e2 ~ N(0, sigma2) 24 | */ 25 | private: 26 | Eigen::Matrix4d jacobA(Eigen::Vector2d u) { 27 | /** 28 | * @brief get the jacobian matrix of A (since there are nonlinearity only in A) 29 | * @param state - current state vector 30 | * @param u - current control input 31 | * @return jacobian matrix of A 32 | */ 33 | Eigen::Matrix4d JA = Eigen::Matrix4d::Identity(); 34 | 35 | auto v = u(0), w = u(1), theta = cur_state(3); 36 | 37 | JA(0, 2) = dT * cos(theta); 38 | JA(0, 3) = -dT * sin(theta) * v; 39 | JA(1, 2) = dT * sin(theta); 40 | JA(1, 3) = dT * cos(theta) * v; 41 | 42 | return JA; 43 | } 44 | public: 45 | Eigen::Matrix4d A; 46 | Eigen::MatrixXd B; 47 | Eigen::MatrixXd H; 48 | Eigen::Matrix4d Q; 49 | Eigen::Matrix2d R; 50 | Eigen::Vector4d cur_state; 51 | 52 | NonlinearDynamicSystem() {} 53 | NonlinearDynamicSystem &operator=(NonlinearDynamicSystem &lds) { 54 | A = lds.A; 55 | B = lds.B; 56 | H = lds.H; 57 | Q = lds.Q; 58 | R = lds.R; 59 | } 60 | 61 | NonlinearDynamicSystem(const NonlinearDynamicSystem &lds) { 62 | A = lds.A; 63 | B = lds.B; 64 | H = lds.H; 65 | Q = lds.Q; 66 | R = lds.R; 67 | } 68 | 69 | NonlinearDynamicSystem(double cov1, double cov2) { 70 | H = Eigen::MatrixXd(2, 4); 71 | B = Eigen::MatrixXd(4, 2); 72 | 73 | Q(0, 0) = cov1 * cov1; 74 | Q(1, 1) = cov1 * cov1; 75 | Q(2, 2) = (1.0/180 * M_PI) * (1.0/180 * M_PI); 76 | Q(3, 3) = cov1 * cov1; 77 | R(0, 0) = cov2 * cov2; 78 | R(1, 1) = cov2 * cov2; 79 | } 80 | 81 | void setCurState(Eigen::Vector4d state_vec) { 82 | /** 83 | * @brief set current state of nonlinear dynamic system 84 | * @param state_vec - given state vector 85 | */ 86 | cur_state(0) = state_vec(0); 87 | cur_state(1) = state_vec(1); 88 | cur_state(2) = state_vec(2); 89 | cur_state(3) = state_vec(3); 90 | } 91 | 92 | Eigen::Matrix4d getJacobA(Eigen::Vector2d u) { return jacobA(u); } 93 | 94 | Eigen::Vector2d getObservation(Eigen::Vector4d input, Eigen::Vector2d error) { 95 | /** 96 | * @brief get current observation given input 97 | * @param input - current state input 98 | * @param error - the error caused by uncertainty - Gaussian 99 | * @return observation vector 100 | */ 101 | Eigen::Vector2d result = H * input + R * error; 102 | 103 | return result; 104 | } 105 | 106 | Eigen::Vector4d getNextState(Eigen::Vector4d last_state, Eigen::Vector2d control_input, Eigen::Vector4d error) { 107 | /** 108 | * @brief get next state given current state and control input 109 | * @param last_state - current state 110 | * @return next state vector 111 | */ 112 | auto theta = cur_state(3); 113 | B << dT * cos(theta), 0, 114 | dT * sin(theta), 0, 115 | 1, 0, 116 | 0, dT; 117 | 118 | cur_state = A * last_state + B * control_input + Q * error; 119 | 120 | return cur_state; 121 | } 122 | 123 | Eigen::Vector4d getReckonedNextState(Eigen::Vector4d last_state, Eigen::Vector2d control_input) { 124 | /** 125 | * @brief getting Dead Reckoned state to compare with 126 | * @return reckoned state vector 127 | */ 128 | return A * last_state + B * control_input; 129 | } 130 | }; 131 | 132 | #endif 133 | -------------------------------------------------------------------------------- /include/planning/prm.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by quan 3 | // 2020.3.20 4 | // 5 | #ifndef INCLUDE_PRM 6 | #define INCLUDE_PRM 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | 20 | #define DEBUG 0 21 | 22 | class GlobalObstacleMap { 23 | /** 24 | * Global Obstacle Map for PRM Planning 25 | * @brief Containing the map information of obstacle, start and goal 26 | * Visualize the result 27 | */ 28 | public: 29 | cv::Mat background; 30 | int start_x; 31 | int start_y; 32 | int goal_x; 33 | int goal_y; 34 | int map_size_x; 35 | int map_size_y; 36 | string w_name; 37 | 38 | GlobalObstacleMap(int s_x, int s_y, int g_x, int g_y, string map_path="../maps/map_1.png", string window_name="PRM") { 39 | /** 40 | * @brief - Initialization of the Global Obstacle Map class 41 | * @param m_x, m_y - map size; s_x, s_y - start place; g_x, g_y - goal place 42 | * @param o_x, o_y - obstacle representation; name - window name of visualization 43 | */ 44 | start_x = s_x; 45 | start_y = s_y; 46 | goal_x = g_x; 47 | goal_y = g_y; 48 | w_name = window_name; 49 | 50 | cv::namedWindow(w_name, cv::WINDOW_NORMAL); 51 | background = cv::imread(map_path, cv::IMREAD_COLOR); 52 | cv::MatSize map_size = background.size; 53 | map_size_x = map_size[1]; 54 | map_size_y = map_size[0]; 55 | cout << "MAP SIZE: " << map_size_x << ' ' << map_size_y << endl; 56 | 57 | cv::circle(background, cv::Point(s_x, s_y), 5, cv::Scalar(0, 0, 255), -1); 58 | cv::circle(background, cv::Point(g_x, g_y), 5, cv::Scalar(255, 0, 0), -1); 59 | } 60 | 61 | bool checkCell(int x, int y) { 62 | /** 63 | * @breif - Collision check base function 64 | * @param x, y - the checking place in the obstacle map 65 | * @return - return the cell type of the place inside the map 66 | */ 67 | if (x < 0 || y < 0 || x >= map_size_x || y >= map_size_y) return true; 68 | int b = (int)*(background.data + background.step[0] * y + x * background.step[1]); 69 | int g = (int)*(background.data + background.step[0] * y + x * background.step[1] + background.elemSize1()); 70 | int r = (int)*(background.data + background.step[0] * y + x * background.step[1] + background.elemSize1() * 2); 71 | if (DEBUG) cout << b << ' ' << g << ' ' << r << endl; 72 | return (b == 0) && (g == 0) && (r == 0); 73 | } 74 | 75 | bool checkPath(cv::Point p1, cv::Point p2) { 76 | /** 77 | * @breif - Collision check function along a path 78 | * @param p1, p2 - two points defining the checking path 79 | * @return - return if collapsed 80 | */ 81 | float step_x = p1.x - p2.x, step_y = p1.y - p2.y; 82 | float step_length = sqrt(pow(step_x, 2) + pow(step_y, 2)); 83 | step_x /= step_length; 84 | step_y /= step_length; 85 | 86 | float init_x = p1.x, init_y = p1.y; 87 | while ((init_x - p2.x) * step_x >= 0 && (init_y - p2.y) * step_y >= 0) { 88 | init_x -= step_x; 89 | init_y -= step_y; 90 | // cout << cvRound(init_x) << ' ' << cvRound(init_y) << endl; 91 | if (checkCell(cvRound(init_x), cvRound(init_y))) return false; 92 | } 93 | 94 | return true; 95 | } 96 | 97 | float heuristic(int x, int y) { 98 | /** 99 | * @brief - Heuristic function using in A* 100 | * @param x, y - exact place in the map 101 | * @return - heuristic value of the place, i.e. distance to goal 102 | */ 103 | return sqrt(pow(x - goal_x, 2) + pow(y - goal_y, 2)); 104 | } 105 | 106 | void render(int s=0) { 107 | /** 108 | * @brief - Rendering the visualization of the planner and map 109 | * @param s - waitKey parameter, second 110 | */ 111 | cv::imshow(w_name, background); 112 | cv::waitKey(s); 113 | } 114 | }; 115 | 116 | #endif -------------------------------------------------------------------------------- /include/planning/rrt.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by quan 3 | // 2020.3.18 4 | // 5 | #ifndef INCLUDE_RRT 6 | #define INCLUDE_RRT 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | 20 | #define DEBUG 0 21 | 22 | class GlobalObstacleMap { 23 | /** 24 | * Global Obstacle Map for RRT Planning 25 | * @brief Containing the map information of obstacle, start and goal 26 | * Visualize the result 27 | */ 28 | public: 29 | cv::Mat background; 30 | int start_x; 31 | int start_y; 32 | int goal_x; 33 | int goal_y; 34 | int map_size_x; 35 | int map_size_y; 36 | string w_name; 37 | 38 | GlobalObstacleMap(int s_x, int s_y, int g_x, int g_y, string map_path="../maps/map_1.png", string window_name="RRT") { 39 | /** 40 | * @brief - Initialization of the Global Obstacle Map class 41 | * @param m_x, m_y - map size; s_x, s_y - start place; g_x, g_y - goal place 42 | * @param o_x, o_y - obstacle representation; name - window name of visualization 43 | */ 44 | start_x = s_x; 45 | start_y = s_y; 46 | goal_x = g_x; 47 | goal_y = g_y; 48 | w_name = window_name; 49 | 50 | cv::namedWindow(w_name, cv::WINDOW_NORMAL); 51 | background = cv::imread(map_path, cv::IMREAD_COLOR); 52 | cv::MatSize map_size = background.size; 53 | map_size_x = map_size[1]; 54 | map_size_y = map_size[0]; 55 | cout << "MAP SIZE: " << map_size_x << ' ' << map_size_y << endl; 56 | 57 | cv::circle(background, cv::Point(s_x, s_y), 5, cv::Scalar(0, 0, 255), -1); 58 | cv::circle(background, cv::Point(g_x, g_y), 5, cv::Scalar(255, 0, 0), -1); 59 | } 60 | 61 | bool checkCell(int x, int y) { 62 | /** 63 | * @breif - Collision check base function 64 | * @param x, y - the checking place in the obstacle map 65 | * @return - return the cell type of the place inside the map 66 | */ 67 | if (x < 0 || y < 0 || x >= map_size_x || y >= map_size_y) return true; 68 | int b = (int)*(background.data + background.step[0] * y + x * background.step[1]); 69 | int g = (int)*(background.data + background.step[0] * y + x * background.step[1] + background.elemSize1()); 70 | int r = (int)*(background.data + background.step[0] * y + x * background.step[1] + background.elemSize1() * 2); 71 | if (DEBUG) cout << b << ' ' << g << ' ' << r << endl; 72 | return (b == 0) && (g == 0) && (r == 0); 73 | } 74 | 75 | // If not convex shapes or big expand steps, use this as collision check 76 | bool checkPath(cv::Point p1, cv::Point p2) { 77 | /** 78 | * @breif - Collision check function along a path 79 | * @param p1, p2 - two points defining the checking path 80 | * @return - return if collapsed 81 | */ 82 | float step_x = p1.x - p2.x, step_y = p1.y - p2.y; 83 | float step_length = sqrt(pow(step_x, 2) + pow(step_y, 2)); 84 | step_x /= step_length; 85 | step_y /= step_length; 86 | 87 | float init_x = p1.x, init_y = p1.y; 88 | while ((init_x - p2.x) * step_x >= 0 && (init_y - p2.y) * step_y >= 0) { 89 | init_x -= step_x; 90 | init_y -= step_y; 91 | // cout << cvRound(init_x) << ' ' << cvRound(init_y) << endl; 92 | if (checkCell(cvRound(init_x), cvRound(init_y))) return false; 93 | } 94 | 95 | return true; 96 | } 97 | 98 | float heuristic(int x, int y) { 99 | /** 100 | * @brief - Heuristic function using in A* 101 | * @param x, y - exact place in the map 102 | * @return - heuristic value of the place, i.e. distance to goal 103 | */ 104 | return sqrt(pow(x - goal_x, 2) + pow(y - goal_y, 2)); 105 | } 106 | 107 | void render(int s=0) { 108 | /** 109 | * @brief - Rendering the visualization of the planner and map 110 | * @param s - waitKey parameter, second 111 | */ 112 | cv::imshow(w_name, background); 113 | cv::waitKey(s); 114 | } 115 | }; 116 | 117 | #endif -------------------------------------------------------------------------------- /src/filtering/kalman_filter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by quan 3 | // 2020.4.6 4 | // 5 | #include "kalman_filter.hpp" 6 | 7 | class KalmanFilter { 8 | /** 9 | * Simple 2D Linear Kalman Filter for dynamic system 10 | * predict - for the kalman filter predict section 11 | * update - for the kalman filter update section 12 | */ 13 | public: 14 | Eigen::MatrixXd kalman_gain; 15 | Eigen::Vector4d prediction; 16 | Eigen::Vector4d filtered; 17 | Eigen::Matrix4d covarience; 18 | Eigen::Matrix4d last_covarience; 19 | LinearDynamicSystem lds; 20 | 21 | KalmanFilter(LinearDynamicSystem s) { 22 | /** 23 | * @brief Initialization of kalman filter 24 | * @param s - the given dynamic system 25 | */ 26 | lds = s; 27 | // Initial Eye matrix for covariences 28 | covarience = Eigen::Matrix4d::Identity(); 29 | last_covarience = Eigen::Matrix4d::Identity(); 30 | kalman_gain = Eigen::MatrixXd(4, 2); 31 | } 32 | void predict(Eigen::Vector4d control_input) { 33 | /** 34 | * @brief prediction phase of kalman filter 35 | */ 36 | prediction = lds.A * filtered + lds.B * control_input; 37 | covarience = lds.A * last_covarience * lds.A.transpose() + lds.Q; 38 | } 39 | void update(Eigen::Vector2d obs) { 40 | /** 41 | * @brief update phase of kalman filter 42 | * @param obs - given observation of current state 43 | */ 44 | kalman_gain = covarience * lds.C.transpose() * 45 | (lds.C * covarience * lds.C.transpose() + lds.R).inverse(); 46 | filtered = prediction + kalman_gain * (obs - lds.C * prediction); 47 | last_covarience = covarience - kalman_gain * lds.C * covarience; 48 | } 49 | }; 50 | 51 | int main() 52 | { 53 | double dT = 0.1, T = 50; 54 | LinearDynamicSystem lds(0.1, 0.1); 55 | lds.A << 1.0, 0.0, dT, 0.0, 56 | 0.0, 1.0, 0.0, dT, 57 | 0.0, 0.0, 1.0, 0.0, 58 | 0.0, 0.0, 0.0, 1.0; 59 | lds.B << 0.0, 0.0, 0.0, 0.0, 60 | 0.0, 0.0, 0.0, 0.0, 61 | 0.0, 0.0, dT / 10, 0.0, 62 | 0.0, 0.0, 0.0, dT / 10; 63 | 64 | lds.C << 1.0, 0.0, 0.0, 0.0, 65 | 0.0, 1.0, 0.0, 0.0; 66 | KalmanFilter kf(lds); 67 | 68 | // Initial State of position 69 | Eigen::Vector4d pos(1.0, 1.0, 0.2, 0.2), reckon(1.0, 1.0, 0.2, 0.2); 70 | kf.filtered(0) = pos(0); 71 | kf.filtered(1) = pos(1); 72 | kf.filtered(2) = pos(2); 73 | kf.filtered(3) = pos(3); 74 | 75 | // Control Input 76 | Eigen::Vector4d u(0.0, 0.0, 1.0, 0.5); 77 | 78 | // Randomized tools 79 | std::random_device rd{}; 80 | std::mt19937 gen{rd()}; 81 | std::normal_distribution<> n(0, 1); // Normal Distribution 82 | 83 | cv::namedWindow("Kalman Filter"); 84 | cv::Mat background = cv::Mat(200, 200, CV_8UC3, cv::Scalar(255, 255, 255)); 85 | std::vector observations, reckoned, position, filtering; 86 | 87 | for (double t = 0.0;t < T;t += dT) { 88 | Eigen::Vector2d errorObs; 89 | Eigen::Vector4d errorState; 90 | errorObs(0) = n(gen); 91 | errorObs(1) = n(gen); 92 | errorState(0) = n(gen); 93 | errorState(1) = n(gen); 94 | errorState(2) = n(gen); 95 | errorState(3) = n(gen); 96 | 97 | pos = lds.getNextState(pos, u, errorState); 98 | reckon = lds.getReckonedNextState(reckon, u); 99 | Eigen::Vector2d obs = lds.getObservation(pos, errorObs); 100 | 101 | kf.predict(u); 102 | kf.update(obs); 103 | 104 | if (DEBUG) std::cout << "KF: " << kf.filtered(0) << ' ' << kf.filtered(1) << std::endl; 105 | if (DEBUG) std::cout << "POS: " << pos(0) << ' ' << pos(1) << std::endl; 106 | if (DEBUG) std::cout << "OBS: " << obs(0) << ' ' << obs(1) << std::endl; 107 | 108 | // Visualizing 109 | observations.push_back(cv::Point(cvRound(obs(0)), cvRound(obs(1)))); 110 | reckoned.push_back(cv::Point(cvRound(reckon(0)), cvRound(reckon(1)))); 111 | position.push_back(cv::Point(cvRound(pos(0)), cvRound(pos(1)))); 112 | filtering.push_back(cv::Point(cvRound(kf.filtered(0)), cvRound(kf.filtered(1)))); 113 | } 114 | 115 | cv::polylines(background, reckoned, false, cv::Scalar(0, 0, 0), 5); 116 | cv::polylines(background, position, false, cv::Scalar(255, 0, 0), 10); 117 | cv::polylines(background, filtering, false, cv::Scalar(0, 255, 0), 5); 118 | 119 | for (int i = 0;i < observations.size();i++) cv::circle(background, observations[i], 3, cv::Scalar(0, 0, 255), -1); 120 | 121 | cv::imshow("Kalman Filter", background); 122 | cv::waitKey(0); 123 | 124 | cv::resize(background, background, cv::Size(300, 300)); 125 | cv::imwrite("../results/filtering/kf.png", background); 126 | } -------------------------------------------------------------------------------- /src/planning/rrt.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by quan 3 | // 2020.3.18 4 | // 5 | #include "rrt.h" 6 | 7 | struct Node { 8 | /** 9 | * Node for RRT planning 10 | * @x, y - the coordinate of current point 11 | * @distance - the distance of the point to goal computed by heuristic 12 | */ 13 | int x; 14 | int y; 15 | Node *parent; 16 | Node(int x_c, int y_c, Node *p=NULL) : x(x_c), y(y_c), parent(p) {} 17 | }; 18 | 19 | class RRTPlanner { 20 | /** 21 | * Robotics RRT Planner 22 | * @calcPath - calculating the final path and draw it on Obstacle map 23 | * @RRTPlanning - planning the path from start to end by RRT method 24 | */ 25 | private: 26 | int expand_length; 27 | float towards_goal; 28 | public: 29 | RRTPlanner(float t_g = 0.5, int e_l = 20) : expand_length(e_l), towards_goal(t_g) {} 30 | 31 | float randDirection() { 32 | /** 33 | * @brief - randomize a direction of rrt to expand 34 | * @return - return the theta angle of the direction 35 | */ 36 | return rand() / double(RAND_MAX); 37 | } 38 | 39 | void calcPath(Node *goal, GlobalObstacleMap m) { 40 | /** 41 | * @brief - calculate the final path finded by RRT 42 | * @param goal - goal place; m - obstacle map 43 | */ 44 | while (goal != NULL) { 45 | Node *tmp = goal->parent; 46 | if (tmp != NULL) cv::line(m.background, cv::Point(goal->x, goal->y), cv::Point(tmp->x, tmp->y), cv::Scalar(128, 0, 128), 3); 47 | goal = goal->parent; 48 | } 49 | } 50 | 51 | Node* getNearestNode(vector vec, cv::Point cp) { 52 | /** 53 | * @brief - get the nearest node inside the open list 54 | * @param vec - open list; cp - current point 55 | * @return - returning the nearest node 56 | */ 57 | float min_distance = 2147483647; 58 | Node *re; 59 | for (int i = 0;i < vec.size();i++) { 60 | float dis = sqrt(pow(cp.x - vec[i]->x, 2) + pow(cp.y - vec[i]->y, 2)); 61 | if (dis < min_distance) { 62 | min_distance = dis; 63 | re = vec[i]; 64 | } 65 | } 66 | 67 | return re; 68 | } 69 | 70 | void RRTPlanning(GlobalObstacleMap m) { 71 | /** 72 | * @brief - RRT planning algorithm 73 | * @param m - obstacle map 74 | */ 75 | vector waiting_list; 76 | 77 | Node *start, *final; 78 | start = new Node(m.start_x, m.start_y); 79 | waiting_list.push_back(start); 80 | while (!waiting_list.empty()) 81 | { 82 | float p = randDirection(); 83 | cv::Point target; 84 | 85 | if (p > towards_goal) { 86 | int t_x = rand() % m.map_size_x; 87 | int t_y = rand() % m.map_size_y; 88 | 89 | target = cv::Point(t_x, t_y);// random select 90 | } 91 | else target = cv::Point(m.goal_x, m.goal_y); // towards goal 92 | 93 | Node *current = getNearestNode(waiting_list, target); 94 | if (DEBUG) cout << current->x << ' ' << current->y << endl; 95 | 96 | // Towards target, move expand_length distance! 97 | float total = sqrt(pow(target.x - current->x, 2) + pow(target.y - current->y, 2)); 98 | float x_length = (target.x - current->x) / total; 99 | float y_length = (target.y - current->y) / total; 100 | 101 | Node *next; 102 | int next_x = current->x + x_length * expand_length, next_y = current->y + y_length * expand_length; 103 | 104 | // Collision Check 105 | if (!m.checkPath(cv::Point(next_x, next_y), cv::Point(current->x, current->y))) continue; 106 | if (m.heuristic(next_x, next_y) < expand_length) { 107 | cout << "Path Finded!" << endl; 108 | cv::line(m.background, cv::Point(current->x, current->y), 109 | cv::Point(m.goal_x, m.goal_y), cv::Scalar(0, 120, 0), 3); 110 | final = new Node(m.goal_x, m.goal_y, current); 111 | break; 112 | } 113 | 114 | next = new Node(next_x, next_y, current); 115 | 116 | waiting_list.push_back(next); 117 | 118 | //Draw the new node 119 | cv::line(m.background, cv::Point(current->x, current->y), 120 | cv::Point(next_x, next_y), cv::Scalar(0, 120, 0), 3); 121 | cv::circle(m.background, cv::Point(next_x, next_y), 3, cv::Scalar(30, 50, 128), -1); 122 | m.render(0); 123 | } 124 | 125 | calcPath(final, m); 126 | m.render(0); 127 | } 128 | }; 129 | 130 | int main() 131 | { 132 | GlobalObstacleMap om(10, 10, 450, 450); 133 | om.render(0); 134 | RRTPlanner rrt = RRTPlanner(); 135 | srand(int(time(NULL))); 136 | rrt.RRTPlanning(om); 137 | cv::resize(om.background, om.background, cv::Size(200, 200)); 138 | cv::imwrite("../results/planning/rrt.png", om.background); 139 | } -------------------------------------------------------------------------------- /src/planning/prm.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by quan 3 | // 2020.3.20 4 | // 5 | #include "prm.h" 6 | 7 | struct Node { 8 | /** 9 | * Node for A* planning 10 | * @x, y - the coordinate of current point 11 | * @cost - the f cost of the point computed by A* 12 | */ 13 | int x; 14 | int y; 15 | float cost; 16 | int index; 17 | Node *pre; 18 | Node(int x_c, int y_c, float c, int ind, Node *p=NULL) : x(x_c), y(y_c), cost(c), index(ind), pre(p) {} 19 | }; 20 | 21 | class PRMPlanner { 22 | /** 23 | * Robotics PRM Planner 24 | * @calcPath - calculating the final path and draw it on Obstacle map 25 | * @PRMPlanning - planning the path from start to end by PRM method 26 | */ 27 | private: 28 | int rand_num; 29 | int neighbor_ro; 30 | vector close_list; 31 | vector path_cost; 32 | public: 33 | PRMPlanner(int r_n = 50, int n_r = 200) : rand_num(r_n), neighbor_ro(n_r) { 34 | close_list = vector(rand_num, 0); 35 | path_cost = vector(rand_num, 2147483647); 36 | } 37 | 38 | float distance(cv::Point p1, cv::Point p2) { 39 | /** 40 | * @brief - calculating the distance between point1 and point2 41 | * @param p1, p2 - the two points to be calculated distance 42 | * @return - returning the distance of the two points 43 | */ 44 | return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2)); 45 | } 46 | 47 | void PRMPlanning(GlobalObstacleMap m) { 48 | /** 49 | * @brief - PRM planning algorithm 50 | * @param m - obstacle map 51 | */ 52 | srand(time(NULL)); 53 | vector> AdjacentMatrix(rand_num, vector(rand_num, 0)); 54 | vector points; 55 | points.push_back(cv::Point(m.start_x, m.start_y)); 56 | points.push_back(cv::Point(m.goal_x, m.goal_y)); 57 | 58 | // Generate Random Points 59 | int tmp_rand_num = 2; 60 | while (tmp_rand_num < rand_num) { 61 | int tmp_x = rand() % m.map_size_x; 62 | int tmp_y = rand() % m.map_size_y; 63 | 64 | if (m.checkCell(tmp_x, tmp_y)) continue; 65 | else tmp_rand_num++; 66 | cv::Point current(tmp_x, tmp_y); 67 | points.push_back(current); 68 | cv::circle(m.background, current, 5, cv::Scalar(30, 50, 128), -1); 69 | 70 | // Line up if possible 71 | for (int i = 0;i < tmp_rand_num - 1;i++) { 72 | if (distance(current, points[i]) < neighbor_ro) { 73 | if (DEBUG) cout << "YES IN NEIGHBOR" << endl; 74 | // Check if can ligature 75 | if (m.checkPath(current, points[i])) { 76 | AdjacentMatrix[i][tmp_rand_num - 1] = 1; 77 | AdjacentMatrix[tmp_rand_num - 1][i] = 1; 78 | cv::line(m.background, current, points[i], cv::Scalar(0, 120, 0), 3); 79 | } 80 | } 81 | else continue; 82 | } 83 | } 84 | 85 | // Show construction of the G(V, E) 86 | m.render(0); 87 | 88 | // Next, do A* on G(V, E) 89 | auto cmp = [](const Node *a, const Node *b) { return a->cost > b->cost; }; 90 | priority_queue, decltype(cmp)> open(cmp); 91 | 92 | Node *start, *final; 93 | bool flag = false; 94 | start = new Node(m.start_x, m.start_y, 0.0, 0); 95 | path_cost[start->index] = 0.0; 96 | open.push(start); 97 | while (!open.empty()) { 98 | Node *current = open.top(); 99 | if (DEBUG) cout << current->x << ' ' << current->y << endl; 100 | open.pop(); 101 | if (close_list[current->index]) continue; 102 | else close_list[current->index] = 1; 103 | 104 | for (int i = 0;i < rand_num;i++) { 105 | if (AdjacentMatrix[current->index][i]) { 106 | Node *next; 107 | if (i == 1) { 108 | cout << "FIND PATH!" << endl; 109 | final = new Node(points[i].x, points[i].y, 0.0, i, current); 110 | flag = true; 111 | break; 112 | } 113 | if (DEBUG) cout << points[i].x << ' ' << points[i].y << endl; 114 | 115 | if (close_list[i]) continue; 116 | path_cost[i] = min(path_cost[i], 117 | distance(points[current->index], points[i]) + path_cost[i]); 118 | float cost = distance(points[current->index], points[i]) + path_cost[i] 119 | + m.heuristic(points[i].x, points[i].y); 120 | next = new Node(points[i].x, points[i].y, cost, i, current); 121 | 122 | open.push(next); 123 | } 124 | } 125 | 126 | if (flag) break; 127 | } 128 | 129 | calcPath(final, m); 130 | m.render(0); 131 | } 132 | 133 | void calcPath(Node *goal, GlobalObstacleMap m) 134 | { 135 | /** 136 | * @brief - calculate the final path finded by RRT* 137 | * @param goal - goal place; m - obstacle map 138 | */ 139 | while (goal != NULL) 140 | { 141 | Node *tmp = goal->pre; 142 | if (tmp != NULL) cv::line(m.background, cv::Point(goal->x, goal->y), cv::Point(tmp->x, tmp->y), cv::Scalar(128, 0, 128), 3); 143 | goal = goal->pre; 144 | } 145 | } 146 | }; 147 | 148 | int main() 149 | { 150 | GlobalObstacleMap om(10, 10, 450, 450); 151 | PRMPlanner prm; 152 | prm.PRMPlanning(om); 153 | // cv::resize(om.background, om.background, cv::Size(200, 200)); 154 | cv::imwrite("../results/planning/prm.png", om.background); 155 | } -------------------------------------------------------------------------------- /src/planning/a_star.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by quan 3 | // 2020.3.15 4 | // 5 | #include "a_star.h" 6 | 7 | class DjikstraPlanner { 8 | /** 9 | * Robotics Djikstra Planner 10 | * @calcPath - calculating the final path and draw it on Obstacle map 11 | * @AstarPlanning - plan the path on the given obstacle map 12 | */ 13 | private: 14 | vector > path_cost; 15 | vector > traversed; 16 | vector motions = { Node(1, 0, 1.0), Node(0, 1, 1.0), Node(0, -1, 1.0), Node(-1, 0, 1.0), 17 | Node(1, 1, sqrt(2)), Node(1, -1, sqrt(2)), Node(-1, -1, sqrt(2)), Node(-1, 1, sqrt(2))}; 18 | public: 19 | void calcPath(Node *end, GlobalObstacleMap om) 20 | { 21 | /** 22 | * @brief - calculate the final path finded by Djikstra 23 | * @param goal - goal place; m - obstacle map 24 | */ 25 | while (end != NULL) 26 | { 27 | om.annoteCell(end->x, end->y, PATH); 28 | end = end->pre; 29 | } 30 | } 31 | 32 | void DjikstraPlanning(GlobalObstacleMap m) { 33 | /** 34 | * @brief - Djikstra planning algorithm 35 | * @param m - obstacle map 36 | */ 37 | traversed = vector>(m.map_size_x, vector(m.map_size_y, 0)); 38 | path_cost = vector>(m.map_size_x, vector(m.map_size_y, 0.0)); 39 | 40 | auto cmp = [](const Node *a, const Node *b) { return a->cost > b->cost; }; 41 | priority_queue, decltype(cmp)> nodes(cmp); 42 | 43 | Node *start, *final; 44 | bool ending_flag = false; 45 | start = new Node(m.start_x, m.start_y, 0.0); 46 | nodes.push(start); 47 | path_cost[m.start_x][m.start_y] = 0.0; 48 | while (!nodes.empty()) { 49 | Node *current = nodes.top(); 50 | nodes.pop(); 51 | if (traversed[current->x][current->y]) continue; 52 | traversed[current->x][current->y] = 1; 53 | if (DEBUG) cout << current->x << ' ' << current->y << endl; 54 | 55 | m.annoteCell(current->x, current->y, ROBOT); 56 | m.render(0); 57 | 58 | // For each motion, update djikstra 59 | for (auto motion : motions) { 60 | int next_x = current->x + motion.x, next_y = current->y + motion.y; 61 | float current_path_cost = path_cost[current->x][current->y]; 62 | 63 | Node *next; 64 | if (m.checkCell(next_x, next_y) == GOAL) { 65 | next = new Node(next_x, next_y, 0.0, current); 66 | final = next; 67 | ending_flag = true; 68 | cout << "Path Find!" << endl; 69 | break; 70 | } 71 | if (traversed[next_x][next_y] || m.checkCell(next_x, next_y) == OBSTACLE) continue; 72 | 73 | float next_cost = current_path_cost + motion.cost; 74 | next = new Node(next_x, next_y, next_cost, current); 75 | path_cost[next_x][next_y] = current_path_cost + motion.cost; 76 | nodes.push(next); 77 | if (DEBUG) cout << next->cost << endl; 78 | } 79 | 80 | if (ending_flag) break; 81 | } 82 | 83 | calcPath(final, m); 84 | m.render(0); 85 | } 86 | }; 87 | 88 | class AstarPlanner { 89 | /** 90 | * Robotics A* Planner 91 | * @calcPath - calculating the final path and draw it on Obstacle map 92 | * @AstarPlanning - plan the path on the given obstacle map 93 | */ 94 | private: 95 | vector > close_list; 96 | vector > path_cost; 97 | vector motions = { Node(1, 0, 1.0), Node(0, 1, 1.0), Node(0, -1, 1.0), Node(-1, 0, 1.0), 98 | Node(1, 1, sqrt(2)), Node(1, -1, sqrt(2)), Node(-1, -1, sqrt(2)), Node(-1, 1, sqrt(2))}; 99 | 100 | public: 101 | void calcPath(Node *end, GlobalObstacleMap om) 102 | { 103 | /** 104 | * @brief - calculate the final path finded by A* 105 | * @param goal - goal place; m - obstacle map 106 | */ 107 | while (end != NULL) 108 | { 109 | om.annoteCell(end->x, end->y, PATH); 110 | end = end->pre; 111 | } 112 | } 113 | 114 | void AstarPlanning(GlobalObstacleMap om) 115 | { 116 | /** 117 | * @brief - A* planning algorithm 118 | * @param m - obstacle map 119 | */ 120 | // Initialize open & close lists 121 | for (int i = 0;i < om.map_size_x;i++) { 122 | vector tmp; 123 | vector tmp_c; 124 | for (int j = 0;j < om.map_size_y;j++) { 125 | tmp.push_back(0); 126 | tmp_c.push_back(2147483647); 127 | } 128 | path_cost.push_back(tmp_c); 129 | close_list.push_back(tmp); 130 | } 131 | 132 | auto cmp = [](const Node *a, const Node *b) { return a->cost > b->cost; }; 133 | priority_queue, decltype(cmp)> open(cmp); 134 | 135 | Node *start; 136 | start = new Node(om.start_x, om.start_y, 0.0); 137 | path_cost[om.start_x][om.start_y] = 0.0; 138 | open.push(start); 139 | 140 | bool ending_flag = false; 141 | Node *final; 142 | while (!open.empty()) 143 | { 144 | // Smallest element in open list, put it into close list 145 | Node *current = open.top(); 146 | open.pop(); 147 | if (close_list[current->x][current->y]) continue; 148 | if (DEBUG) cout << current->cost << endl; 149 | close_list[current->x][current->y] = 1; 150 | 151 | om.annoteCell(current->x, current->y, ROBOT); 152 | om.render(0); 153 | 154 | // For every motion, get its next Node 155 | if (DEBUG) cout << "-----------next----------" << endl; 156 | for (auto motion : motions) 157 | { 158 | int next_x = current->x + motion.x, next_y = current->y + motion.y; 159 | float current_path_cost = path_cost[current->x][current->y]; 160 | 161 | Node *next; 162 | if (om.checkCell(next_x, next_y) == GOAL) { 163 | next = new Node(next_x, next_y, 0.0, current); 164 | final = next; 165 | ending_flag = true; 166 | cout << "Path Find!" << endl; 167 | break; 168 | } 169 | if (close_list[next_x][next_y] || om.checkCell(next_x, next_y) == OBSTACLE) continue; 170 | 171 | float next_cost = current_path_cost + om.heuristic(next_x, next_y) + motion.cost; 172 | next = new Node(next_x, next_y, next_cost, current); 173 | path_cost[next_x][next_y] = min(path_cost[next_x][next_y], current_path_cost + motion.cost); 174 | open.push(next); 175 | if (DEBUG) cout << next->cost << endl; 176 | } 177 | if (DEBUG) cout << "-----------next----------" << endl; 178 | 179 | if (ending_flag) break; 180 | } 181 | 182 | calcPath(final, om); 183 | om.render(0); 184 | } 185 | }; 186 | 187 | int main() 188 | { 189 | vector o_x, o_y; 190 | for (int i = 0;i < 50;i++) { 191 | o_x.push_back(0); 192 | o_y.push_back(i); 193 | } 194 | for (int i = 0;i < 50;i++) { 195 | o_x.push_back(i); 196 | o_y.push_back(0); 197 | } 198 | for (int i = 0;i < 50;i++) { 199 | o_x.push_back(i); 200 | o_y.push_back(49); 201 | } 202 | for (int i = 0;i < 50;i++) { 203 | o_x.push_back(49); 204 | o_y.push_back(i); 205 | } 206 | for (int i = 0;i < 26;i++) { 207 | o_x.push_back(i); 208 | o_y.push_back(15); 209 | } 210 | for (int i = 0;i < 26;i++) { 211 | o_x.push_back(50 - i); 212 | o_y.push_back(35); 213 | } 214 | GlobalObstacleMap m(50, 50, 5, 5, 45, 45, o_x, o_y, "A*"); 215 | AstarPlanner planner; 216 | planner.AstarPlanning(m); 217 | // DjikstraPlanner planner; 218 | // planner.DjikstraPlanning(m); 219 | m.render(0); 220 | cv::resize(m.background, m.background, cv::Size(200, 200)); 221 | cv::imwrite("../results/planning/astar.png", m.background); 222 | } -------------------------------------------------------------------------------- /src/planning/rrt_star.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by quan 3 | // 2020.3.21 4 | // 5 | 6 | #include "rrt.h" 7 | 8 | struct Node { 9 | /** 10 | * Node for RRT planning 11 | * @x, y - the coordinate of current point 12 | * @distance - the distance of the point to goal computed by heuristic 13 | */ 14 | int x; 15 | int y; 16 | float distance; 17 | Node *parent; 18 | Node(int x_c, int y_c, float c, Node *p=NULL) : x(x_c), y(y_c), distance(c), parent(p) {} 19 | }; 20 | 21 | class RRTStarPlanner { 22 | /** 23 | * Robotics RRT* Planner 24 | * @calcPath - calculating the final path and draw it on Obstacle map 25 | * @RRTStarPlanning - planning the path from start to end by RRT method 26 | */ 27 | private: 28 | int expand_length; 29 | float towards_goal; 30 | float gamma_d; // for finding near potential parents 31 | public: 32 | RRTStarPlanner(float t_g = 0.5, int e_l = 20, float gamma = 100) : expand_length(e_l), towards_goal(t_g), gamma_d(gamma) {} 33 | 34 | float randDirection() { 35 | /** 36 | * @brief - randomize a direction of rrt to expand 37 | * @return - return the theta angle of the direction 38 | */ 39 | return rand() / double(RAND_MAX); 40 | } 41 | 42 | void calcPath(Node *goal, GlobalObstacleMap m) { 43 | /** 44 | * @brief - calculate the final path finded by RRT* 45 | * @param goal - goal place; m - obstacle map 46 | */ 47 | while (goal != NULL) { 48 | Node *tmp = goal->parent; 49 | if (tmp != NULL) cv::line(m.background, cv::Point(goal->x, goal->y), cv::Point(tmp->x, tmp->y), cv::Scalar(128, 0, 128), 3); 50 | goal = goal->parent; 51 | } 52 | } 53 | 54 | Node* getNearestNode(vector vec, cv::Point cp) { 55 | /** 56 | * @brief - get the nearest node inside the open list 57 | * @param vec - open list; cp - current point 58 | * @return - returning the nearest node 59 | */ 60 | float min_distance = 2147483647; 61 | Node *re; 62 | for (int i = 0;i < vec.size();i++) { 63 | float dis = sqrt(pow(cp.x - vec[i]->x, 2) + pow(cp.y - vec[i]->y, 2)); 64 | if (dis < min_distance) { 65 | min_distance = dis; 66 | re = vec[i]; 67 | } 68 | } 69 | 70 | return re; 71 | } 72 | 73 | float distance(cv::Point p1, cv::Point p2) { 74 | /** 75 | * @brief - calculating the distance between point1 and point2 76 | * @param p1, p2 - the two points to be calculated distance 77 | * @return - returning the distance of the two points 78 | */ 79 | return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2)); 80 | } 81 | 82 | vector nearSearch(Node* n, vector l) { 83 | /** 84 | * @brief - RRT* searching near nodes to do rewiring 85 | * @param n - node to be searched as center; l - node list 86 | * @return - returning a vector containing all near nodes 87 | */ 88 | vector result; 89 | int nodes_num = l.size() + 1; 90 | float criterion = gamma_d * sqrt(log(nodes_num) / nodes_num); 91 | // cout << criterion << endl; 92 | for (int i = 0;i < l.size();i++) { 93 | if (distance(cv::Point(l[i]->x, l[i]->y), cv::Point(n->x, n->y)) < criterion) { 94 | result.push_back(l[i]); 95 | } 96 | } 97 | 98 | return result; 99 | } 100 | 101 | void RRTStarPlanning(GlobalObstacleMap m) { 102 | /** 103 | * @brief - RRT* planning algorithm 104 | * @param m - obstacle map 105 | */ 106 | vector waiting_list; 107 | 108 | Node *start, *final; 109 | start = new Node(m.start_x, m.start_y, 0.0); 110 | waiting_list.push_back(start); 111 | while (!waiting_list.empty()) 112 | { 113 | // Do as normal RRT do 114 | float p = randDirection(); 115 | cv::Point target; 116 | 117 | if (p > towards_goal) { 118 | int t_x = rand() % m.map_size_x; 119 | int t_y = rand() % m.map_size_y; 120 | 121 | target = cv::Point(t_x, t_y);// random select 122 | } 123 | else target = cv::Point(m.goal_x, m.goal_y); // towards goal 124 | 125 | Node *current = getNearestNode(waiting_list, target); 126 | if (DEBUG) cout << current->x << ' ' << current->y << endl; 127 | 128 | // Towards target, move expand_length distance! 129 | float total = sqrt(pow(target.x - current->x, 2) + pow(target.y - current->y, 2)); 130 | float x_length = (target.x - current->x) / total; 131 | float y_length = (target.y - current->y) / total; 132 | 133 | Node *next, *tmp; 134 | int next_x = current->x + x_length * expand_length, next_y = current->y + y_length * expand_length; 135 | 136 | // Collision Check & End Status 137 | if (m.checkCell(next_x, next_y)) continue; 138 | if (m.heuristic(next_x, next_y) < expand_length) { 139 | cout << "Path Finded!" << endl; 140 | cv::line(m.background, cv::Point(current->x, current->y), 141 | cv::Point(m.goal_x, m.goal_y), cv::Scalar(0, 120, 0), 3); 142 | final = new Node(m.goal_x, m.goal_y, 0.0, current); 143 | break; 144 | } 145 | 146 | float next_dis = current->distance + distance(cv::Point(next_x, next_y), cv::Point(current->x, current->y)); 147 | Node *true_parent = current; 148 | tmp = new Node(next_x, next_y, next_dis); 149 | // RRT* steps - adding parents refine 150 | if (DEBUG) cout << next_x << ' ' << next_y << endl; 151 | vector potential_parents = nearSearch(tmp, waiting_list); 152 | for (int p = 0;p < potential_parents.size();p++) { 153 | Node *pp = potential_parents[p]; 154 | if (!m.checkPath(cv::Point(pp->x, pp->y), cv::Point(next_x, next_y))) continue; 155 | float p_dis = pp->distance + distance(cv::Point(next_x, next_y), cv::Point(pp->x, pp->y)); 156 | if (p_dis < next_dis) { 157 | next_dis = p_dis; 158 | true_parent = pp; 159 | } 160 | } 161 | 162 | //Draw the new node 163 | cv::line(m.background, cv::Point(true_parent->x, true_parent->y), 164 | cv::Point(next_x, next_y), cv::Scalar(0, 120, 0), 3); 165 | cv::circle(m.background, cv::Point(next_x, next_y), 3, cv::Scalar(30, 50, 128), -1); 166 | 167 | // Rewire the whole tree 168 | next = new Node(next_x, next_y, next_dis, true_parent); 169 | waiting_list.push_back(next); 170 | for (int p = 0;p < potential_parents.size();p++) { 171 | Node *pp = potential_parents[p]; 172 | if (pp->x == true_parent->x && pp->y == true_parent->y) continue; 173 | if (!m.checkPath(cv::Point(pp->x, pp->y), cv::Point(next_x, next_y))) continue; 174 | 175 | // If not this parent 176 | if (pp->distance > next_dis + distance(cv::Point(pp->x, pp->y), cv::Point(next->x, next->y))) { 177 | // Now set pp's parent to next! 178 | cv::line(m.background, cv::Point(pp->x, pp->y), 179 | cv::Point(pp->parent->x, pp->parent->y), cv::Scalar(255, 255, 255), 3); 180 | pp->parent = next; 181 | cv::line(m.background, cv::Point(pp->x, pp->y), 182 | cv::Point(next_x, next_y), cv::Scalar(0, 120, 0), 3); 183 | if (DEBUG) cout << "Rewire!" << endl; 184 | } 185 | } 186 | 187 | m.render(0); 188 | } 189 | 190 | calcPath(final, m); 191 | m.render(0); 192 | } 193 | }; 194 | 195 | int main() 196 | { 197 | GlobalObstacleMap om(10, 10, 450, 450); 198 | om.render(0); 199 | RRTStarPlanner rrt_star = RRTStarPlanner(); 200 | srand(int(time(NULL))); 201 | rrt_star.RRTStarPlanning(om); 202 | // cv::resize(om.background, om.background, cv::Size(200, 200)); 203 | cv::imwrite("../results/planning/rrt_star.png", om.background); 204 | } -------------------------------------------------------------------------------- /include/3rdParty/ALGLIB/diffequations.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | ALGLIB 3.16.0 (source code generated 2019-12-19) 3 | Copyright (c) Sergey Bochkanov (ALGLIB project). 4 | 5 | >>> SOURCE LICENSE >>> 6 | This program is free software; you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation (www.fsf.org); either version 2 of the 9 | License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | A copy of the GNU General Public License is available at 17 | http://www.fsf.org/licensing/licenses 18 | >>> END OF LICENSE >>> 19 | *************************************************************************/ 20 | #ifndef _diffequations_pkg_h 21 | #define _diffequations_pkg_h 22 | #include "ap.h" 23 | #include "alglibinternal.h" 24 | 25 | ///////////////////////////////////////////////////////////////////////// 26 | // 27 | // THIS SECTION CONTAINS COMPUTATIONAL CORE DECLARATIONS (DATATYPES) 28 | // 29 | ///////////////////////////////////////////////////////////////////////// 30 | namespace alglib_impl 31 | { 32 | #if defined(AE_COMPILE_ODESOLVER) || !defined(AE_PARTIAL_BUILD) 33 | typedef struct 34 | { 35 | ae_int_t n; 36 | ae_int_t m; 37 | double xscale; 38 | double h; 39 | double eps; 40 | ae_bool fraceps; 41 | ae_vector yc; 42 | ae_vector escale; 43 | ae_vector xg; 44 | ae_int_t solvertype; 45 | ae_bool needdy; 46 | double x; 47 | ae_vector y; 48 | ae_vector dy; 49 | ae_matrix ytbl; 50 | ae_int_t repterminationtype; 51 | ae_int_t repnfev; 52 | ae_vector yn; 53 | ae_vector yns; 54 | ae_vector rka; 55 | ae_vector rkc; 56 | ae_vector rkcs; 57 | ae_matrix rkb; 58 | ae_matrix rkk; 59 | rcommstate rstate; 60 | } odesolverstate; 61 | typedef struct 62 | { 63 | ae_int_t nfev; 64 | ae_int_t terminationtype; 65 | } odesolverreport; 66 | #endif 67 | 68 | } 69 | 70 | ///////////////////////////////////////////////////////////////////////// 71 | // 72 | // THIS SECTION CONTAINS C++ INTERFACE 73 | // 74 | ///////////////////////////////////////////////////////////////////////// 75 | namespace alglib 76 | { 77 | 78 | #if defined(AE_COMPILE_ODESOLVER) || !defined(AE_PARTIAL_BUILD) 79 | /************************************************************************* 80 | 81 | *************************************************************************/ 82 | class _odesolverstate_owner 83 | { 84 | public: 85 | _odesolverstate_owner(); 86 | _odesolverstate_owner(const _odesolverstate_owner &rhs); 87 | _odesolverstate_owner& operator=(const _odesolverstate_owner &rhs); 88 | virtual ~_odesolverstate_owner(); 89 | alglib_impl::odesolverstate* c_ptr(); 90 | alglib_impl::odesolverstate* c_ptr() const; 91 | protected: 92 | alglib_impl::odesolverstate *p_struct; 93 | }; 94 | class odesolverstate : public _odesolverstate_owner 95 | { 96 | public: 97 | odesolverstate(); 98 | odesolverstate(const odesolverstate &rhs); 99 | odesolverstate& operator=(const odesolverstate &rhs); 100 | virtual ~odesolverstate(); 101 | ae_bool &needdy; 102 | real_1d_array y; 103 | real_1d_array dy; 104 | double &x; 105 | 106 | }; 107 | 108 | 109 | /************************************************************************* 110 | 111 | *************************************************************************/ 112 | class _odesolverreport_owner 113 | { 114 | public: 115 | _odesolverreport_owner(); 116 | _odesolverreport_owner(const _odesolverreport_owner &rhs); 117 | _odesolverreport_owner& operator=(const _odesolverreport_owner &rhs); 118 | virtual ~_odesolverreport_owner(); 119 | alglib_impl::odesolverreport* c_ptr(); 120 | alglib_impl::odesolverreport* c_ptr() const; 121 | protected: 122 | alglib_impl::odesolverreport *p_struct; 123 | }; 124 | class odesolverreport : public _odesolverreport_owner 125 | { 126 | public: 127 | odesolverreport(); 128 | odesolverreport(const odesolverreport &rhs); 129 | odesolverreport& operator=(const odesolverreport &rhs); 130 | virtual ~odesolverreport(); 131 | ae_int_t &nfev; 132 | ae_int_t &terminationtype; 133 | 134 | }; 135 | #endif 136 | 137 | #if defined(AE_COMPILE_ODESOLVER) || !defined(AE_PARTIAL_BUILD) 138 | /************************************************************************* 139 | Cash-Karp adaptive ODE solver. 140 | 141 | This subroutine solves ODE Y'=f(Y,x) with initial conditions Y(xs)=Ys 142 | (here Y may be single variable or vector of N variables). 143 | 144 | INPUT PARAMETERS: 145 | Y - initial conditions, array[0..N-1]. 146 | contains values of Y[] at X[0] 147 | N - system size 148 | X - points at which Y should be tabulated, array[0..M-1] 149 | integrations starts at X[0], ends at X[M-1], intermediate 150 | values at X[i] are returned too. 151 | SHOULD BE ORDERED BY ASCENDING OR BY DESCENDING! 152 | M - number of intermediate points + first point + last point: 153 | * M>2 means that you need both Y(X[M-1]) and M-2 values at 154 | intermediate points 155 | * M=2 means that you want just to integrate from X[0] to 156 | X[1] and don't interested in intermediate values. 157 | * M=1 means that you don't want to integrate :) 158 | it is degenerate case, but it will be handled correctly. 159 | * M<1 means error 160 | Eps - tolerance (absolute/relative error on each step will be 161 | less than Eps). When passing: 162 | * Eps>0, it means desired ABSOLUTE error 163 | * Eps<0, it means desired RELATIVE error. Relative errors 164 | are calculated with respect to maximum values of Y seen 165 | so far. Be careful to use this criterion when starting 166 | from Y[] that are close to zero. 167 | H - initial step lenth, it will be adjusted automatically 168 | after the first step. If H=0, step will be selected 169 | automatically (usualy it will be equal to 0.001 of 170 | min(x[i]-x[j])). 171 | 172 | OUTPUT PARAMETERS 173 | State - structure which stores algorithm state between subsequent 174 | calls of OdeSolverIteration. Used for reverse communication. 175 | This structure should be passed to the OdeSolverIteration 176 | subroutine. 177 | 178 | SEE ALSO 179 | AutoGKSmoothW, AutoGKSingular, AutoGKIteration, AutoGKResults. 180 | 181 | 182 | -- ALGLIB -- 183 | Copyright 01.09.2009 by Bochkanov Sergey 184 | *************************************************************************/ 185 | void odesolverrkck(const real_1d_array &y, const ae_int_t n, const real_1d_array &x, const ae_int_t m, const double eps, const double h, odesolverstate &state, const xparams _xparams = alglib::xdefault); 186 | void odesolverrkck(const real_1d_array &y, const real_1d_array &x, const double eps, const double h, odesolverstate &state, const xparams _xparams = alglib::xdefault); 187 | 188 | 189 | /************************************************************************* 190 | This function provides reverse communication interface 191 | Reverse communication interface is not documented or recommended to use. 192 | See below for functions which provide better documented API 193 | *************************************************************************/ 194 | bool odesolveriteration(const odesolverstate &state, const xparams _xparams = alglib::xdefault); 195 | 196 | 197 | /************************************************************************* 198 | This function is used to launcn iterations of ODE solver 199 | 200 | It accepts following parameters: 201 | diff - callback which calculates dy/dx for given y and x 202 | ptr - optional pointer which is passed to diff; can be NULL 203 | 204 | 205 | -- ALGLIB -- 206 | Copyright 01.09.2009 by Bochkanov Sergey 207 | 208 | *************************************************************************/ 209 | void odesolversolve(odesolverstate &state, 210 | void (*diff)(const real_1d_array &y, double x, real_1d_array &dy, void *ptr), 211 | void *ptr = NULL, const xparams _xparams = alglib::xdefault); 212 | 213 | 214 | /************************************************************************* 215 | ODE solver results 216 | 217 | Called after OdeSolverIteration returned False. 218 | 219 | INPUT PARAMETERS: 220 | State - algorithm state (used by OdeSolverIteration). 221 | 222 | OUTPUT PARAMETERS: 223 | M - number of tabulated values, M>=1 224 | XTbl - array[0..M-1], values of X 225 | YTbl - array[0..M-1,0..N-1], values of Y in X[i] 226 | Rep - solver report: 227 | * Rep.TerminationType completetion code: 228 | * -2 X is not ordered by ascending/descending or 229 | there are non-distinct X[], i.e. X[i]=X[i+1] 230 | * -1 incorrect parameters were specified 231 | * 1 task has been solved 232 | * Rep.NFEV contains number of function calculations 233 | 234 | -- ALGLIB -- 235 | Copyright 01.09.2009 by Bochkanov Sergey 236 | *************************************************************************/ 237 | void odesolverresults(const odesolverstate &state, ae_int_t &m, real_1d_array &xtbl, real_2d_array &ytbl, odesolverreport &rep, const xparams _xparams = alglib::xdefault); 238 | #endif 239 | } 240 | 241 | ///////////////////////////////////////////////////////////////////////// 242 | // 243 | // THIS SECTION CONTAINS COMPUTATIONAL CORE DECLARATIONS (FUNCTIONS) 244 | // 245 | ///////////////////////////////////////////////////////////////////////// 246 | namespace alglib_impl 247 | { 248 | #if defined(AE_COMPILE_ODESOLVER) || !defined(AE_PARTIAL_BUILD) 249 | void odesolverrkck(/* Real */ ae_vector* y, 250 | ae_int_t n, 251 | /* Real */ ae_vector* x, 252 | ae_int_t m, 253 | double eps, 254 | double h, 255 | odesolverstate* state, 256 | ae_state *_state); 257 | ae_bool odesolveriteration(odesolverstate* state, ae_state *_state); 258 | void odesolverresults(odesolverstate* state, 259 | ae_int_t* m, 260 | /* Real */ ae_vector* xtbl, 261 | /* Real */ ae_matrix* ytbl, 262 | odesolverreport* rep, 263 | ae_state *_state); 264 | void _odesolverstate_init(void* _p, ae_state *_state, ae_bool make_automatic); 265 | void _odesolverstate_init_copy(void* _dst, void* _src, ae_state *_state, ae_bool make_automatic); 266 | void _odesolverstate_clear(void* _p); 267 | void _odesolverstate_destroy(void* _p); 268 | void _odesolverreport_init(void* _p, ae_state *_state, ae_bool make_automatic); 269 | void _odesolverreport_init_copy(void* _dst, void* _src, ae_state *_state, ae_bool make_automatic); 270 | void _odesolverreport_clear(void* _p); 271 | void _odesolverreport_destroy(void* _p); 272 | #endif 273 | 274 | } 275 | #endif 276 | 277 | -------------------------------------------------------------------------------- /include/planning/a_star.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by quan 3 | // 2020.3.15 4 | // 5 | 6 | #ifndef INCLUDE_ASTAR 7 | #define INCLUDE_ASTAR 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | using namespace std; 18 | 19 | #define DEBUG 0 20 | 21 | enum CELLTYPE{ 22 | FREE=1, 23 | ROBOT, 24 | START, 25 | GOAL, 26 | OBSTACLE, 27 | PATH, 28 | REPLAN 29 | }; 30 | 31 | struct Node { 32 | /** 33 | * Node for A* planning 34 | * @x, y - the coordinate of current point 35 | * @cost - the f cost of the point computed by A* 36 | */ 37 | int x; 38 | int y; 39 | float cost; 40 | Node *pre; 41 | Node(int x_c, int y_c, float c, Node *p=NULL) : x(x_c), y(y_c), cost(c), pre(p) {} 42 | }; 43 | 44 | class LocalObstacleMap { 45 | public: 46 | vector obstacle_x; 47 | vector obstacle_y; 48 | vector known_obs_x; 49 | vector known_obs_y; 50 | vector new_obs_x; 51 | vector new_obs_y; 52 | int start_x; 53 | int start_y; 54 | int goal_x; 55 | int goal_y; 56 | int map_size_x; 57 | int map_size_y; 58 | cv::Mat background; 59 | string w_name; 60 | 61 | vector motions = { Node(1, 0, 1.0), Node(0, 1, 1.0), Node(0, -1, 1.0), Node(-1, 0, 1.0), 62 | Node(1, 1, sqrt(2)), Node(1, -1, sqrt(2)), Node(-1, -1, sqrt(2)), Node(-1, 1, sqrt(2)), 63 | Node(2, 0, 1.0), Node(2, 1, 1.0), Node(2, 2, 1.0), Node(-2, 0, 1.0), 64 | Node(-2, 1, sqrt(2)), Node(-2, 2, sqrt(2)), Node(2, -1, sqrt(2)), Node(2, -2, sqrt(2)), 65 | Node(-2, -1, sqrt(2)), Node(-2, -2, sqrt(2)), Node(1, -2, sqrt(2)), Node(1, 2, sqrt(2)), 66 | Node(-1, 2, 1), Node(-1, -2, 1), Node(0, -2, 0), Node(0, 2, 0)}; 67 | 68 | LocalObstacleMap(int m_x, int m_y, int s_x, int s_y, int g_x, int g_y, 69 | vector o_x, vector o_y, string name="D*-Lite") { 70 | /** 71 | * @brief - Initialization of the Global Obstacle Map class 72 | * @param m_x, m_y - map size; s_x, s_y - start place; g_x, g_y - goal place 73 | * @param o_x, o_y - obstacle representation; name - window name of visualization 74 | */ 75 | start_x = s_x; 76 | start_y = s_y; 77 | goal_x = g_x; 78 | goal_y = g_y; 79 | map_size_x = m_x; 80 | map_size_y = m_y; 81 | obstacle_x = o_x; 82 | obstacle_y = o_y; 83 | known_obs_x = {}; 84 | known_obs_x = {}; 85 | new_obs_x = {}; 86 | new_obs_y = {}; 87 | w_name = name; 88 | 89 | background = cv::Mat(m_x, m_y, CV_8UC3, cv::Scalar(255, 255, 255)); 90 | cv::namedWindow(name, cv::WINDOW_NORMAL); 91 | annoteCell(s_x, s_y, START); 92 | annoteCell(g_x, g_y, GOAL); 93 | } 94 | 95 | void setObstacle(vector o_x, vector o_y) { 96 | /** 97 | * @brief - Set new Obstacles in map 98 | */ 99 | // for (int i = 0;i < o_x.size();i++) annoteCell(o_x[i], o_y[i], OBSTACLE); 100 | for (auto x : o_x) obstacle_x.push_back(x); 101 | for (auto y : o_y) obstacle_y.push_back(y); 102 | } 103 | 104 | bool collisionCheck(int x, int y) { 105 | for (int i = 0;i < obstacle_x.size();i++) if (obstacle_x[i] == x && obstacle_y[i] == y) return true; 106 | } 107 | 108 | void explore(int x, int y) { 109 | new_obs_x.clear(); 110 | new_obs_y.clear(); 111 | for (auto motion : motions) { 112 | if (collisionCheck(x+motion.x, y+motion.y)) { 113 | auto obs_x = x+motion.x, obs_y = y+motion.y; 114 | if (checkCell(obs_x, obs_y) != OBSTACLE) new_obs_x.push_back(obs_x); 115 | if (checkCell(obs_x, obs_y) != OBSTACLE) new_obs_y.push_back(obs_y); 116 | annoteCell(obs_x, obs_y, OBSTACLE); 117 | } 118 | } 119 | 120 | for (int i = 0;i < new_obs_x.size();i++) { 121 | known_obs_x.push_back(new_obs_x[i]); 122 | known_obs_y.push_back(new_obs_y[i]); 123 | } 124 | 125 | render(0); 126 | } 127 | 128 | void render(int s) { 129 | /** 130 | * @brief - Rendering the visualization of the planner and map 131 | * @param s - waitKey parameter, second 132 | */ 133 | cv::Mat render_map = background.clone(); 134 | cv::imshow(w_name, render_map); 135 | cv::waitKey(s); 136 | } 137 | 138 | CELLTYPE checkCell(int x, int y) 139 | { 140 | /** 141 | * @breif - Collision check base function 142 | * @param x, y - the checking place in the obstacle map 143 | * @return - return the cell type of the place inside the map 144 | */ 145 | if (x < 0 || y < 0 || x >= map_size_x || y >= map_size_y) return OBSTACLE; 146 | if (x == start_x && y == start_y) return START; 147 | if (x == goal_x && y == goal_y) return GOAL; 148 | for (int i = 0;i < known_obs_x.size();i++) if (known_obs_x[i] == x && known_obs_y[i] == y) return OBSTACLE; 149 | return FREE; 150 | } 151 | 152 | void annoteCell(int c_x, int c_y, CELLTYPE celltype) { 153 | /** 154 | * @breif - Visualization of the given cell type place in map 155 | * @param c_x, c_y - exact place in the map 156 | * @param celltype - given celltype of the place 157 | */ 158 | switch (celltype) { 159 | case FREE: break; 160 | case ROBOT: { 161 | cv::rectangle(background, cv::Rect(c_x, c_y, 1, 1), cv::Scalar(20, 255, 20), -1); 162 | break; 163 | } 164 | case START: { 165 | cv::rectangle(background, cv::Rect(c_x, c_y, 1, 1), cv::Scalar(255, 60, 0), -1); 166 | break; 167 | } 168 | case GOAL: { 169 | cv::rectangle(background, cv::Rect(c_x, c_y, 1, 1), cv::Scalar(0, 0, 255), -1); 170 | break; 171 | } 172 | case OBSTACLE: { 173 | cv::rectangle(background, cv::Rect(c_x, c_y, 1, 1), cv::Scalar(0, 0, 0), -1); 174 | break; 175 | } 176 | case PATH: { 177 | cv::rectangle(background, cv::Rect(c_x, c_y, 1, 1), cv::Scalar(128, 0, 128), -1); 178 | break; 179 | } 180 | case REPLAN: { 181 | cv::rectangle(background, cv::Rect(c_x, c_y, 1, 1), cv::Scalar(128, 128, 0), -1); 182 | break; 183 | } 184 | } 185 | } 186 | 187 | float heuristicStart(int x, int y) { 188 | return sqrt(pow(start_x - x, 2) + pow(start_y - y, 2)); 189 | } 190 | 191 | float heuristicGoal(int x, int y) 192 | { 193 | /** 194 | * @brief - Heuristic function using in A* 195 | * @param x, y - exact place in the map 196 | * @return - heuristic value of the place, i.e. distance to goal 197 | */ 198 | return sqrt(pow(goal_x - x, 2) + pow(goal_y - y, 2)); 199 | } 200 | }; 201 | 202 | class GlobalObstacleMap { 203 | /** 204 | * Global Obstacle Map class 205 | * @brief Containing the map information of obstacle, start and goal 206 | * Visualize the result 207 | */ 208 | public: 209 | vector obstacle_x; 210 | vector obstacle_y; 211 | int start_x; 212 | int start_y; 213 | int goal_x; 214 | int goal_y; 215 | int map_size_x; 216 | int map_size_y; 217 | cv::Mat background; 218 | string w_name; 219 | 220 | GlobalObstacleMap(int m_x, int m_y, int s_x, int s_y, int g_x, int g_y, 221 | vector o_x, vector o_y, string name="A*") { 222 | /** 223 | * @brief - Initialization of the Global Obstacle Map class 224 | * @param m_x, m_y - map size; s_x, s_y - start place; g_x, g_y - goal place 225 | * @param o_x, o_y - obstacle representation; name - window name of visualization 226 | */ 227 | start_x = s_x; 228 | start_y = s_y; 229 | goal_x = g_x; 230 | goal_y = g_y; 231 | map_size_x = m_x; 232 | map_size_y = m_y; 233 | obstacle_x = o_x; 234 | obstacle_y = o_y; 235 | w_name = name; 236 | 237 | background = cv::Mat(m_x, m_y, CV_8UC3, cv::Scalar(255, 255, 255)); 238 | cv::namedWindow(name, cv::WINDOW_NORMAL); 239 | annoteCell(s_x, s_y, START); 240 | annoteCell(g_x, g_y, GOAL); 241 | 242 | for (int i = 0;i < o_x.size();i++) annoteCell(o_x[i], o_y[i], OBSTACLE); 243 | } 244 | 245 | void setObstacle(vector o_x, vector o_y) { 246 | /** 247 | * @brief - Set new Obstacles in map 248 | */ 249 | for (int i = 0;i < o_x.size();i++) annoteCell(o_x[i], o_y[i], OBSTACLE); 250 | for (auto x : o_x) obstacle_x.push_back(x); 251 | for (auto y : o_y) obstacle_y.push_back(y); 252 | } 253 | 254 | void render(int s) { 255 | /** 256 | * @brief - Rendering the visualization of the planner and map 257 | * @param s - waitKey parameter, second 258 | */ 259 | cv::imshow(w_name, background); 260 | cv::waitKey(s); 261 | } 262 | 263 | CELLTYPE checkCell(int x, int y) 264 | { 265 | /** 266 | * @breif - Collision check base function 267 | * @param x, y - the checking place in the obstacle map 268 | * @return - return the cell type of the place inside the map 269 | */ 270 | if (x < 0 || y < 0 || x >= map_size_x || y >= map_size_y) return OBSTACLE; 271 | if (x == start_x && y == start_y) return START; 272 | if (x == goal_x && y == goal_y) return GOAL; 273 | for (int i = 0;i < obstacle_x.size();i++) if (obstacle_x[i] == x && obstacle_y[i] == y) return OBSTACLE; 274 | return FREE; 275 | } 276 | 277 | void annoteCell(int c_x, int c_y, CELLTYPE celltype) { 278 | /** 279 | * @breif - Visualization of the given cell type place in map 280 | * @param c_x, c_y - exact place in the map 281 | * @param celltype - given celltype of the place 282 | */ 283 | switch (celltype) { 284 | case FREE: break; 285 | case ROBOT: { 286 | cv::rectangle(background, cv::Rect(c_x, c_y, 1, 1), cv::Scalar(20, 255, 20), -1); 287 | break; 288 | } 289 | case START: { 290 | cv::rectangle(background, cv::Rect(c_x, c_y, 1, 1), cv::Scalar(255, 60, 0), -1); 291 | break; 292 | } 293 | case GOAL: { 294 | cv::rectangle(background, cv::Rect(c_x, c_y, 1, 1), cv::Scalar(0, 0, 255), -1); 295 | break; 296 | } 297 | case OBSTACLE: { 298 | cv::rectangle(background, cv::Rect(c_x, c_y, 1, 1), cv::Scalar(0, 0, 0), -1); 299 | break; 300 | } 301 | case PATH: { 302 | cv::rectangle(background, cv::Rect(c_x, c_y, 1, 1), cv::Scalar(128, 0, 128), -1); 303 | break; 304 | } 305 | case REPLAN: { 306 | cv::rectangle(background, cv::Rect(c_x, c_y, 1, 1), cv::Scalar(128, 128, 0), -1); 307 | break; 308 | } 309 | } 310 | } 311 | 312 | float heuristic(int x, int y) 313 | { 314 | /** 315 | * @brief - Heuristic function using in A* 316 | * @param x, y - exact place in the map 317 | * @return - heuristic value of the place, i.e. distance to goal 318 | */ 319 | return sqrt(pow(goal_x - x, 2) + pow(goal_y - y, 2)); 320 | } 321 | }; 322 | 323 | #endif -------------------------------------------------------------------------------- /include/optimization/minimum_snap_control.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by quan 3 | // 2020.4.3 4 | // 5 | #ifndef INCLUDE_MINIMUM_SNAP 6 | #define INCLUDE_MINIMUM_SNAP 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | using namespace alglib; 18 | 19 | #define DEBUG 1 20 | 21 | class MinSnapOptimizer { 22 | /** 23 | * Robotics Trajectory Optimization - Minimum Snap 24 | * @MinSnapOptimizeMultiple - optimize the path from start to end by Minimum Snap 25 | */ 26 | public: 27 | int degree; // Degree of the fitting polynomial 28 | double timeT; // Required time of the whole trajectory 29 | std::string window_name; 30 | std::vector Px; // Discrete trajectory points coordinate x 31 | std::vector Py; // Discrete trajectory points coordinate y 32 | std::vector coord; 33 | cv::Mat background; 34 | int map_size_x; 35 | int map_size_y; 36 | 37 | MinSnapOptimizer() {} 38 | MinSnapOptimizer(int d, std::vector x, std::vector y, std::string w_name="Minimum Snap", 39 | int m_x = 1000, int m_y = 1000, double time_spec = 4) : 40 | degree(d), Px(x), Py(y), window_name(w_name), map_size_x(m_x), map_size_y(m_y), timeT(time_spec) { 41 | coord = std::vector(degree, 1.0); 42 | background = cv::Mat(m_x, m_y, CV_8UC3, cv::Scalar(255, 255, 255)); 43 | 44 | for (int i = 0;i < Px.size();i++) { 45 | if (i == 0) { 46 | cv::circle(background, cv::Point(Px[i], Py[i]), 3, cv::Scalar(0, 0, 255), -1); 47 | continue; 48 | } 49 | 50 | cv::circle(background, cv::Point(Px[i], Py[i]), 3, cv::Scalar(0, 0, 255), -1); 51 | cv::line(background, cv::Point(Px[i-1], Py[i-1]), cv::Point(Px[i], Py[i]), cv::Scalar(255, 0, 0), 3); 52 | } 53 | 54 | cv::namedWindow(w_name, cv::WINDOW_NORMAL); 55 | } 56 | 57 | void render(int s) { 58 | /** 59 | * @brief - Rendering the visualization of the planner and map 60 | * @param s - waitKey parameter, second 61 | */ 62 | cv::imshow(window_name, background); 63 | cv::waitKey(s); 64 | } 65 | 66 | bool MinSnapOptimizeMultiple(int part) { 67 | /** 68 | * @brief - Optimize composited trajectory planned by discrete method 69 | * @param part - number of the different trajectory 70 | */ 71 | if (part <= 0) { 72 | std::cout << "What are you doing?" << std::endl; 73 | return false; 74 | } 75 | 76 | std::vector time_stamp; 77 | double unit = timeT / Px.size(); 78 | for (double stamp = timeT / Px.size();stamp <= timeT;stamp += timeT / Px.size()) time_stamp.push_back(stamp); 79 | 80 | real_1d_array x, y; 81 | int step = Px.size() / part; 82 | for (int i = 0;i < part;i++) { 83 | // Current Points to be generating trajectory 84 | std::vector c_x, c_y; 85 | std::vector time_; 86 | std::vector> constraintx, constrainty; 87 | 88 | if (i == part - 1) { 89 | for (int j = std::max(0, i * step - 1);j < Px.size();j++) { 90 | // Deal with extra constraints 91 | if (j == i * step - 1) { 92 | std::vector tmp; 93 | for (int k = 0;k < degree;k++) tmp.push_back(k * pow(time_stamp[j], k - 1)); 94 | double xnum = 0; 95 | for (int k = 0;k < degree;k++) xnum += k * x[k] * pow(time_stamp[j], k - 1); 96 | tmp.push_back(xnum); 97 | constraintx.push_back(tmp); 98 | 99 | tmp.clear(); 100 | for (int k = 0;k < degree;k++) tmp.push_back(k * pow(time_stamp[j], k - 1)); 101 | double ynum = 0; 102 | for (int k = 0;k < degree;k++) ynum += k * y[k] * pow(time_stamp[j], k - 1); 103 | tmp.push_back(ynum); 104 | constrainty.push_back(tmp); 105 | } 106 | 107 | c_x.push_back(Px[j]); 108 | c_y.push_back(Py[j]); 109 | time_.push_back(time_stamp[j]); 110 | } 111 | } 112 | else { 113 | for (int j = std::max(0, i * step - 1);j < (i+1) * step;j++) { 114 | // Deal with extra constraints 115 | if (j == i * step - 1) { 116 | std::vector tmp; 117 | for (int k = 0;k < degree;k++) tmp.push_back(k * pow(time_stamp[j], k - 1)); 118 | double xnum = 0; 119 | for (int k = 0;k < degree;k++) xnum += k * x[k] * pow(time_stamp[j], k - 1); 120 | tmp.push_back(xnum); 121 | constraintx.push_back(tmp); 122 | 123 | tmp.clear(); 124 | for (int k = 0;k < degree;k++) tmp.push_back(k * pow(time_stamp[j], k - 1)); 125 | double ynum = 0; 126 | for (int k = 0;k < degree;k++) ynum += k * y[k] * pow(time_stamp[j], k - 1); 127 | tmp.push_back(ynum); 128 | constrainty.push_back(tmp); 129 | } 130 | 131 | c_x.push_back(Px[j]); 132 | c_y.push_back(Py[j]); 133 | time_.push_back(time_stamp[j]); 134 | } 135 | } 136 | 137 | double last_time_spec = time_.at(0), time_spec = time_.at(time_.size()-1); 138 | if (DEBUG) std::cout << last_time_spec << ' ' << time_spec << std::endl; 139 | MinSnapOptimizeSingle(last_time_spec, time_spec, unit, constraintx, constrainty, c_x, c_y, x, y); 140 | } 141 | } 142 | 143 | void MinSnapOptimizeSingle(double last_time_spec, double time_spec, double unit, std::vector> constraintx, 144 | std::vector> constrainty, std::vector Px, std::vector Py, real_1d_array &x, real_1d_array &y) { 145 | /** 146 | * @brief - Optimize single trajectory planned by discrete method 147 | * @param constraintx, constrainty - extra constraint on connecting points 148 | * @param time_spec, unit - time stamp of the trajectory and unit time stamp 149 | */ 150 | real_2d_array qua_term, cx, cy; 151 | real_1d_array linear_term, p0, scale; 152 | integer_1d_array ct; 153 | ae_int_t kx, ky; 154 | 155 | // Intializing Quadratic Term 156 | double *qua_term_; 157 | qua_term_ = new double[degree * degree]; 158 | for (int i = 0;i < degree;i++) { 159 | for (int j = 0;j < degree;j++) { 160 | if (i <= 3 || j <= 3) qua_term_[i * degree + j] = 0; 161 | else qua_term_[i * degree + j] = ((pow(time_spec, i + j - 7) - pow(last_time_spec, i + j - 7)) 162 | * std::tgamma(i+1) * std::tgamma(j+1) / std::tgamma(i-3) / std::tgamma(j-3) / (i + j - 7)); 163 | } 164 | } 165 | qua_term.setcontent(ae_int_t(degree), ae_int_t(degree), qua_term_); 166 | 167 | // Initializing Linear Term 168 | double *linear_term_; 169 | linear_term_ = new double[degree]; 170 | // Hint: DO NOT USE MEMSET 171 | for (int i = 0;i < degree;i++) linear_term_[i] = 0; 172 | linear_term.setcontent(ae_int_t(degree), linear_term_); 173 | 174 | // Initializing start state & sacle 175 | double *p0_, *scale_; 176 | p0_ = new double[degree]; 177 | scale_ = new double[degree]; 178 | for (int i = 0;i < degree;i++) p0_[i] = 1; 179 | for (int i = 0;i < degree;i++) scale_[i] = 1; 180 | p0.setcontent(ae_int_t(degree), p0_); 181 | scale.setcontent(ae_int_t(degree), scale_); 182 | 183 | // Initializing Equality Constraint 184 | ae_int_t *ct_; 185 | int total = Px.size() + constraintx.size(); 186 | ct_ = new ae_int_t[total]; 187 | for (int i = 0;i < total;i++) ct_[i] = ae_int_t(0); 188 | ct.setcontent(ae_int_t(total), ct_); 189 | 190 | // Equality Constraint for x 191 | double *cx_; 192 | cx_ = new double[total * (degree + 1)]; 193 | for (int i = 0;i < Px.size();i++) { 194 | for (int j = 0;j < degree + 1;j++) { 195 | if (j == degree) cx_[i * (degree + 1) + j] = Px[i]; 196 | else cx_[i * (degree + 1) + j] = pow(i * unit + last_time_spec, j); 197 | } 198 | } 199 | 200 | // Extra Constraint for x - derivative continuous 201 | for (int i = 0;i < constraintx.size();i++) { 202 | for (int j = 0;j < constraintx[i].size();j++) { 203 | cx_[Px.size() * (degree + 1) + i * (degree + 1) + j] = constraintx[i][j]; 204 | } 205 | } 206 | cx.setcontent(ae_int_t(total), ae_int_t(degree+1), cx_); 207 | 208 | // Equality Constraint for y 209 | double *cy_; 210 | cy_ = new double[total * (degree + 1)]; 211 | for (int i = 0;i < Py.size();i++) { 212 | for (int j = 0;j < degree + 1;j++) { 213 | if (j == degree) cy_[i * (degree + 1) + j] = Py[i]; 214 | else cy_[i * (degree + 1) + j] = pow(i * unit + last_time_spec, j); 215 | } 216 | } 217 | 218 | // Extra Constraint for y - derivative continuous 219 | for (int i = 0;i < constrainty.size();i++) { 220 | for (int j = 0;j < constrainty[i].size();j++) { 221 | cy_[Py.size() * (degree + 1) + i * (degree + 1) + j] = constrainty[i][j]; 222 | } 223 | } 224 | cy.setcontent(ae_int_t(total), ae_int_t(degree+1), cy_); 225 | 226 | // Initializing kx ky 227 | kx = ae_int_t(total); 228 | ky = ae_int_t(total); 229 | 230 | MinSnapQPOptimize(qua_term, linear_term, p0, scale, cx, ct, kx, x); 231 | MinSnapQPOptimize(qua_term, linear_term, p0, scale, cy, ct, ky, y); 232 | 233 | // Now annote the background for the result 234 | for (double t = last_time_spec;t < time_spec;t+=0.001) { 235 | double xnum = 0, ynum = 0; 236 | for (int j = 0;j < degree;j++) { 237 | xnum += x[j] * pow(t, j); 238 | ynum += y[j] * pow(t, j); 239 | } 240 | cv::circle(background, cv::Point(int(xnum), int(ynum)), 1, cv::Scalar(0, 0, 255), -1); 241 | } 242 | render(0); 243 | } 244 | 245 | void MinSnapQPOptimize(real_2d_array qua_term, real_1d_array linear_term, real_1d_array p0, 246 | real_1d_array scale, const real_2d_array c, const integer_1d_array ct, const ae_int_t k, real_1d_array &x) { 247 | /** 248 | * @brief - QP Optimizer given by ALGLIB 249 | */ 250 | // QP 251 | minqpstate state; 252 | minqpreport rep; 253 | minqpcreate(6, state); 254 | minqpsetquadraticterm(state, qua_term); 255 | minqpsetlinearterm(state, linear_term); 256 | minqpsetstartingpoint(state, p0); 257 | minqpsetlc(state, c, ct, k); 258 | minqpsetscale(state, scale); 259 | 260 | minqpsetalgobleic(state, 0.0, 0.0, 0.0, 0); 261 | minqpoptimize(state); 262 | minqpresults(state, x, rep); 263 | if (DEBUG) printf("%d\n", int(rep.terminationtype)); 264 | if (DEBUG) printf("%s\n", x.tostring(2).c_str()); 265 | } 266 | 267 | void MinSnapOptimizeTest() { 268 | /** 269 | * @brief - A test version of trajectory generation by minimum snap 270 | */ 271 | cv::Mat bbk = cv::Mat(500, 500, CV_8UC3, cv::Scalar(255, 255, 255)); 272 | std::vector xx = {58, 47, 210, 477}, yy = {69, 249, 405, 109}; 273 | 274 | for (int i = 0;i < xx.size();i++) { 275 | if (i == 0) { 276 | cv::circle(bbk, cv::Point(xx[i], yy[i]), 3, cv::Scalar(0, 0, 255), -1); 277 | continue; 278 | } 279 | 280 | cv::circle(bbk, cv::Point(xx[i], yy[i]), 3, cv::Scalar(0, 0, 255), -1); 281 | cv::line(bbk, cv::Point(xx[i-1], yy[i-1]), cv::Point(xx[i], yy[i]), cv::Scalar(255, 0, 0), 3); 282 | } 283 | 284 | // Test 6 version for x 285 | real_2d_array qua_term = "[[0, 0, 0, 0, 0, 0]," 286 | "[0, 0, 0, 0, 0, 0]," 287 | "[0, 0, 0, 0, 0, 0]," 288 | "[0, 0, 0, 0, 0, 0]," 289 | "[0, 0, 0, 0, 2304, 23040]," 290 | "[0, 0, 0, 0, 23040, 307200]]"; 291 | real_1d_array linear_term = "[0, 0, 0, 0, 0, 0]"; 292 | real_1d_array p0 = "[1, 1, 1, 1, 1, 1]"; 293 | real_1d_array scale = "[1, 1, 1, 1, 1, 1]"; 294 | real_2d_array c; 295 | 296 | // Note: our time starts from 1.0 instead of 0 to avoid singularity 297 | double _c[] = {1, 1, 1, 1, 1, 1, 58, 298 | 1, 2, 4, 8, 16, 32, 47, 299 | 1, 3, 9, 27, 81, 243, 210, 300 | 1, 4, 16, 64, 256, 1024, 477}; 301 | c.setcontent(ae_int_t(4), ae_int_t(7), _c); 302 | integer_1d_array ct = "[0, 0, 0, 0]"; 303 | ae_int_t k = 4; 304 | real_1d_array x; 305 | 306 | MinSnapQPOptimize(qua_term, linear_term, p0, scale, c, ct, k, x); 307 | 308 | // for y 309 | real_2d_array qua_term2 = "[[0, 0, 0, 0, 0, 0]," 310 | "[0, 0, 0, 0, 0, 0]," 311 | "[0, 0, 0, 0, 0, 0]," 312 | "[0, 0, 0, 0, 0, 0]," 313 | "[0, 0, 0, 0, 2304, 23040]," 314 | "[0, 0, 0, 0, 23040, 307200]]"; 315 | real_1d_array linear_term2 = "[0, 0, 0, 0, 0, 0]"; 316 | real_1d_array p02 = "[1, 1, 1, 1, 1, 1]"; 317 | real_1d_array scale2 = "[1, 1, 1, 1, 1, 1]"; 318 | real_2d_array c2; 319 | double _c2[] = {1, 1, 1, 1, 1, 1, 69, 320 | 1, 2, 4, 8, 16, 32, 249, 321 | 1, 3, 9, 27, 81, 243, 405, 322 | 1, 4, 16, 64, 256, 1024, 109}; 323 | c2.setcontent(ae_int_t(4), ae_int_t(7), _c2); 324 | integer_1d_array ct2 = "[0, 0, 0, 0]"; 325 | ae_int_t k2 = 4; 326 | real_1d_array y; 327 | 328 | MinSnapQPOptimize(qua_term2, linear_term2, p02, scale2, c2, ct2, k2, y); 329 | 330 | // Now annote the background for the result 331 | for (double t = 1;t < 4;t+=0.001) { 332 | int xnum = x[0] + x[1] * t + x[2] * pow(t, 2) + x[3] * pow(t, 3) + x[4] * pow(t, 4) + x[5] * pow(t, 5); 333 | int ynum = y[0] + y[1] * t + y[2] * pow(t, 2) + y[3] * pow(t, 3) + y[4] * pow(t, 4) + y[5] * pow(t, 5); 334 | cv::circle(bbk, cv::Point(xnum, ynum), 1, cv::Scalar(0, 0, 255), -1); 335 | } 336 | 337 | cv::imshow("Test", bbk); 338 | cv::waitKey(0); 339 | cv::resize(bbk, bbk, cv::Size(300, 300)); 340 | cv::imwrite("../results/optimization/minimum_snapQP.png", bbk); 341 | } 342 | }; 343 | 344 | #endif 345 | -------------------------------------------------------------------------------- /src/planning/d_star.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by quan 3 | // 2020.4.19 4 | // 5 | #include "a_star.h" 6 | 7 | struct LPNode { 8 | /** 9 | * Node for LP-A* planning 10 | * @x, y - the coordinate of current point 11 | * @cost - the f cost of the point computed by A* 12 | */ 13 | int x; 14 | int y; 15 | float k1; 16 | float k2; 17 | LPNode *pre; 18 | LPNode(int x_c, int y_c, float c1, float c2, LPNode *p=NULL) : x(x_c), y(y_c), k1(c1), k2(c2), pre(p) {} 19 | }; 20 | 21 | class DstarLitePlanner { 22 | /** 23 | * Robotics D*-Lite Planner 24 | * @calcPath - calculating the final path and draw it on Obstacle map 25 | * @DstarLitePlanning - plan the path on the given obstacle map 26 | */ 27 | public: 28 | static bool cmp(const LPNode *a, const LPNode *b) 29 | { 30 | if (a->k1 == b->k1) return a->k2 < b->k2; 31 | return a->k1 < b->k1; 32 | } 33 | int current_x; 34 | int current_y; 35 | vector> g; 36 | vector> rhs; 37 | vector pq; 38 | vector motions = { Node(1, 0, 1.0), Node(0, 1, 1.0), Node(0, -1, 1.0), Node(-1, 0, 1.0), 39 | Node(1, 1, sqrt(2)), Node(1, -1, sqrt(2)), Node(-1, -1, sqrt(2)), Node(-1, 1, sqrt(2))}; 40 | 41 | // K1 comparison 42 | float k1(LocalObstacleMap m, int x, int y) { 43 | if (min(g[x][y], rhs[x][y]) == 2147483647) return 2147483647; 44 | return min(g[x][y], rhs[x][y]) + m.heuristicStart(x, y); 45 | } 46 | 47 | // K2 comparison 48 | float k2(int x, int y) { return min(g[x][y], rhs[x][y]); } 49 | 50 | void pushNode(LPNode *n) { 51 | /** 52 | * @brief - push a node into current open list 53 | * @param n - node to be pushed 54 | */ 55 | pq.push_back(n); 56 | sort(pq.begin(), pq.end(), cmp); 57 | } 58 | 59 | void removeNode(LPNode *n) { 60 | /** 61 | * @brief - remove a node in current open list 62 | * @param n - node to be removed 63 | */ 64 | for (auto iter = pq.begin();iter != pq.end();iter++) { 65 | LPNode *tmp = *iter; 66 | if (tmp->x == n->x && tmp->y == n->y) { 67 | pq.erase(iter); 68 | break; 69 | } 70 | } 71 | } 72 | 73 | void updateVertex(LocalObstacleMap m, LPNode *n) { 74 | /** 75 | * @brief - update vertex node inside open list 76 | * @param n - node to be updated 77 | */ 78 | if (m.goal_x == n->x && m.goal_y == n->y) return ;//n; 79 | 80 | // Calculate RHS value 81 | float mini = 2147483647; 82 | int xx, yy; 83 | for (auto motion : motions) { 84 | int nx = n->x + motion.x, ny = n->y + motion.y; 85 | if (m.checkCell(nx, ny) == OBSTACLE) continue; 86 | if (g[nx][ny] != 2147483647) { 87 | float costs = g[nx][ny] + motion.cost; 88 | if (costs < mini) { 89 | mini = costs; 90 | xx = nx; 91 | yy = ny; 92 | } 93 | } 94 | } 95 | rhs[n->x][n->y] = mini; 96 | 97 | removeNode(n); 98 | n->k1 = k1(m, n->x, n->y); 99 | n->k2 = k2(n->x, n->y); 100 | 101 | if (g[n->x][n->y] != rhs[n->x][n->y]) pushNode(n); 102 | } 103 | 104 | void computeShortestPath(LocalObstacleMap m) { 105 | LPNode *final = NULL; 106 | while (pq.size() > 0 && (rhs[m.start_x][m.start_y] == 2147483647 || 107 | k2(m.start_x, m.start_y) > k2(pq[0]->x, pq[0]->y) || rhs[m.start_x][m.start_y] != g[m.start_x][m.start_y])) { 108 | LPNode *cur = pq[0]; 109 | pq.erase(pq.begin()); 110 | 111 | int cx = cur->x, cy = cur->y; 112 | if (g[cx][cy] > rhs[cx][cy]) { 113 | g[cx][cy] = rhs[cx][cy]; 114 | for (auto motion : motions) { 115 | int nx = cx + motion.x, ny = cy + motion.y; 116 | if (m.checkCell(nx, ny) == OBSTACLE) continue; 117 | LPNode* next = new LPNode(nx, ny, k1(m, nx, ny), k2(nx, ny), cur); 118 | updateVertex(m, next); 119 | if (nx == m.start_x && ny == m.start_y) final = next; 120 | } 121 | } 122 | else { 123 | cout << "BAD" << endl; 124 | g[cx][cy] = 2147483647; 125 | updateVertex(m, cur); 126 | for (auto motion : motions) { 127 | int nx = cx + motion.x, ny = cy + motion.y; 128 | if (m.checkCell(nx, ny) == OBSTACLE) continue; 129 | LPNode* next = new LPNode(nx, ny, k1(m, nx, ny), k2(nx, ny), cur); 130 | updateVertex(m, next); 131 | } 132 | } 133 | 134 | if (final != NULL) break; 135 | } 136 | } 137 | 138 | void DstarLitePlanning(LocalObstacleMap m) { 139 | // Initialization 140 | current_x = m.start_x; 141 | current_y = m.start_y; 142 | for (int i = 0;i < m.map_size_x;i++) { 143 | vector tmpg; 144 | vector tmpr; 145 | for (int j = 0;j < m.map_size_y;j++) { 146 | tmpg.push_back(2147483647); 147 | tmpr.push_back(2147483647); 148 | } 149 | 150 | g.push_back(tmpg); 151 | rhs.push_back(tmpr); 152 | } 153 | 154 | rhs[m.goal_x][m.goal_y] = 0; 155 | LPNode *final = NULL, *goal = new LPNode(m.goal_x, m.goal_y, 156 | k1(m, m.goal_x, m.goal_y), k2(m.goal_x, m.goal_y)); 157 | pushNode(goal); 158 | 159 | computeShortestPath(m); 160 | 161 | while (current_x != m.goal_x || current_y != m.goal_y) { 162 | float mini = 2147483647; 163 | int xx = -1, yy = -1; 164 | for (auto motion : motions) { 165 | int nx = current_x + motion.x, ny = current_y + motion.y; 166 | LPNode* next = new LPNode(nx, ny, k1(m, nx, ny), k2(nx, ny)); 167 | if (m.checkCell(nx, ny) == OBSTACLE) continue; 168 | cout << nx << ' ' << ny << " and cost: " << g[nx][ny] << endl; 169 | if (g[nx][ny] != 2147483647) { 170 | float costs = g[nx][ny] + motion.cost; 171 | if (costs < mini) { 172 | mini = costs; 173 | xx = nx; 174 | yy = ny; 175 | } 176 | } 177 | } 178 | 179 | if (xx == -1 || yy == -1) { 180 | cout << "Internal Error" << endl; 181 | break; 182 | } 183 | current_x = xx; 184 | m.start_x = xx; 185 | current_y = yy; 186 | m.start_y = yy; 187 | cout << current_x << ' ' << current_y << endl; 188 | 189 | m.annoteCell(current_x, current_y, REPLAN); 190 | m.explore(current_x, current_y); 191 | 192 | bool flag = false; 193 | for (auto x : m.new_obs_x) { 194 | for (auto y : m.new_obs_y) { 195 | cout << "New obstacle: " << x << ' ' << y << endl; 196 | g[x][y] = rhs[x][y] = 2147483647; 197 | flag = true; 198 | LPNode* next = new LPNode(x, y, k1(m, x, y), k2(x, y)); 199 | updateVertex(m, next); 200 | for (auto motion : motions) { 201 | int nx = next->x + motion.x, ny = next->y + motion.y; 202 | cout << "Updated: " << nx << ' ' << ny << " and cost: " << g[nx][ny] << endl; 203 | } 204 | } 205 | } 206 | 207 | for (auto node_s : pq) { 208 | LPNode* tmp = node_s; 209 | tmp->k1 = k1(m, tmp->x, tmp->y); 210 | tmp->k2 = k2(tmp->x, tmp->y); 211 | removeNode(node_s); 212 | pushNode(tmp); 213 | } 214 | 215 | if (true) computeShortestPath(m); 216 | } 217 | } 218 | }; 219 | 220 | class LPAstarPlanner { 221 | /** 222 | * Robotics LP-A* Planner 223 | * @calcPath - calculating the final path and draw it on Obstacle map 224 | * @LPAstarPlanning - plan the path on the given obstacle map 225 | * @LPAstarReplanning - replan when the map info changed 226 | */ 227 | public: 228 | static bool cmp(const LPNode *a, const LPNode *b) 229 | { 230 | if (a->k1 == b->k1) return a->k2 < b->k2; 231 | return a->k1 < b->k1; 232 | } 233 | vector> g; 234 | vector> rhs; 235 | vector pq; 236 | vector motions = { Node(1, 0, 1.0), Node(0, 1, 1.0), Node(0, -1, 1.0), Node(-1, 0, 1.0), 237 | Node(1, 1, sqrt(2)), Node(1, -1, sqrt(2)), Node(-1, -1, sqrt(2)), Node(-1, 1, sqrt(2))}; 238 | map, pair> path; 239 | 240 | void calcPath(LPNode *end, GlobalObstacleMap om, CELLTYPE celltype = PATH) 241 | { 242 | /** 243 | * @brief - calculate the final path finded by LP-A* 244 | * @param goal - goal place; m - obstacle map 245 | */ 246 | pair e(end->x, end->y); 247 | while (e.first != om.start_x && e.second != om.start_y) 248 | { 249 | om.annoteCell(e.first, e.second, celltype); 250 | cout << e.first << " YYYY " << e.second << endl; 251 | e = path[e]; 252 | } 253 | om.annoteCell(e.first, e.second, celltype); 254 | } 255 | 256 | // K1 comparison 257 | float k1(GlobalObstacleMap m, int x, int y) { 258 | if (min(g[x][y], rhs[x][y]) == 2147483647) return 2147483647; 259 | return min(g[x][y], rhs[x][y]) + m.heuristic(x, y); 260 | } 261 | 262 | // K2 comparison 263 | float k2(int x, int y) { return min(g[x][y], rhs[x][y]); } 264 | 265 | void pushNode(LPNode *n) { 266 | /** 267 | * @brief - push a node into current open list 268 | * @param n - node to be pushed 269 | */ 270 | pq.push_back(n); 271 | sort(pq.begin(), pq.end(), cmp); 272 | } 273 | 274 | void removeNode(LPNode *n) { 275 | /** 276 | * @brief - remove a node in current open list 277 | * @param n - node to be removed 278 | */ 279 | for (auto iter = pq.begin();iter != pq.end();iter++) { 280 | LPNode *tmp = *iter; 281 | if (tmp->x == n->x && tmp->y == n->y) { 282 | pq.erase(iter); 283 | break; 284 | } 285 | } 286 | } 287 | 288 | void updateVertex(GlobalObstacleMap m, LPNode *n) { 289 | /** 290 | * @brief - update vertex node inside open list 291 | * @param n - node to be updated 292 | */ 293 | if (m.start_x == n->x && m.start_y == n->y) return ;//n; 294 | 295 | // Calculate RHS value 296 | float mini = 2147483647; 297 | int xx, yy; 298 | for (auto motion : motions) { 299 | int nx = n->x + motion.x, ny = n->y + motion.y; 300 | if (m.checkCell(nx, ny) == OBSTACLE) continue; 301 | if (g[nx][ny] != 2147483647) { 302 | float costs = g[nx][ny] + motion.cost; 303 | if (costs < mini) { 304 | mini = costs; 305 | xx = nx; 306 | yy = ny; 307 | } 308 | } 309 | } 310 | rhs[n->x][n->y] = mini; 311 | 312 | removeNode(n); 313 | n->k1 = k1(m, n->x, n->y); 314 | n->k2 = k2(n->x, n->y); 315 | 316 | // Map to remember the path for visualization 317 | pair original(n->x, n->y); 318 | pair par(xx, yy); 319 | path[original] = par; 320 | 321 | if (g[n->x][n->y] != rhs[n->x][n->y]) pushNode(n); 322 | } 323 | 324 | void LPAstarReplanning(GlobalObstacleMap m, vector changex, vector changey) { 325 | /** 326 | * @brief - LP-A* replanning algorithm after slightly changing obstacle map 327 | * @param m - obstacle map 328 | */ 329 | bool flag = false; 330 | for (auto x : changex) { 331 | for (auto y : changey) { 332 | g[x][y] = rhs[x][y] = 2147483647; 333 | if (!path.count(pair(x, y))) continue; 334 | else flag = true; 335 | } 336 | if (flag) break; 337 | } 338 | 339 | if (!flag) return ; 340 | 341 | pq.clear(); 342 | 343 | for (auto x : changex) { 344 | for (auto y : changey) { 345 | for (auto motion : motions) { 346 | int nx = x + motion.x, ny = y + motion.y; 347 | if (m.checkCell(nx, ny) == OBSTACLE) continue; 348 | LPNode* next = new LPNode(nx, ny, k1(m, nx, ny), k2(nx, ny)); 349 | updateVertex(m, next); 350 | } 351 | } 352 | } 353 | 354 | LPNode *final = NULL; 355 | while (pq.size() > 0 && (rhs[m.goal_x][m.goal_y] == 2147483647 || 356 | k2(m.goal_x, m.goal_y) > k2(pq[0]->x, pq[0]->y) || rhs[m.goal_x][m.goal_y] != g[m.goal_x][m.goal_y])) { 357 | LPNode *cur = pq[0]; 358 | pq.erase(pq.begin()); 359 | 360 | int cx = cur->x, cy = cur->y; 361 | std::cout << "IN QUEUE: " << cx << ' ' << cy << std::endl; 362 | if (g[cx][cy] > rhs[cx][cy]) { 363 | g[cx][cy] = rhs[cx][cy]; 364 | for (auto motion : motions) { 365 | int nx = cx + motion.x, ny = cy + motion.y; 366 | if (m.checkCell(nx, ny) == OBSTACLE) continue; 367 | LPNode* next = new LPNode(nx, ny, k1(m, nx, ny), k2(nx, ny), cur); 368 | updateVertex(m, next); 369 | if (nx == m.goal_x && ny == m.goal_y) final = next; 370 | } 371 | } 372 | else { 373 | cout << "BAD" << endl; 374 | g[cx][cy] = 2147483647; 375 | updateVertex(m, cur); 376 | for (auto motion : motions) { 377 | int nx = cx + motion.x, ny = cy + motion.y; 378 | if (m.checkCell(nx, ny) == OBSTACLE) continue; 379 | LPNode* next = new LPNode(nx, ny, k1(m, nx, ny), k2(nx, ny), cur); 380 | updateVertex(m, next); 381 | } 382 | } 383 | 384 | if (final != NULL) break; 385 | } 386 | 387 | calcPath(final, m, REPLAN); 388 | } 389 | 390 | void LPAstarPlanning(GlobalObstacleMap m) { 391 | /** 392 | * @brief - LP-A* planning algorithm 393 | * @param m - obstacle map 394 | */ 395 | // Initialization 396 | for (int i = 0;i < m.map_size_x;i++) { 397 | vector tmpg; 398 | vector tmpr; 399 | for (int j = 0;j < m.map_size_y;j++) { 400 | tmpg.push_back(2147483647); 401 | tmpr.push_back(2147483647); 402 | } 403 | 404 | g.push_back(tmpg); 405 | rhs.push_back(tmpr); 406 | } 407 | 408 | rhs[m.start_x][m.start_y] = 0; 409 | LPNode *final = NULL, *start = new LPNode(m.start_x, m.start_y, 410 | k1(m, m.start_x, m.start_y), k2(m.start_x, m.start_y)); 411 | pushNode(start); 412 | 413 | while (pq.size() > 0 && (rhs[m.goal_x][m.goal_y] == 2147483647 || 414 | k2(m.goal_x, m.goal_y) > k2(pq[0]->x, pq[0]->y) || rhs[m.goal_x][m.goal_y] != g[m.goal_x][m.goal_y])) { 415 | LPNode *cur = pq[0]; 416 | pq.erase(pq.begin()); 417 | 418 | int cx = cur->x, cy = cur->y; 419 | if (g[cx][cy] > rhs[cx][cy]) { 420 | g[cx][cy] = rhs[cx][cy]; 421 | for (auto motion : motions) { 422 | int nx = cx + motion.x, ny = cy + motion.y; 423 | if (m.checkCell(nx, ny) == OBSTACLE) continue; 424 | LPNode* next = new LPNode(nx, ny, k1(m, nx, ny), k2(nx, ny), cur); 425 | updateVertex(m, next); 426 | if (nx == m.goal_x && ny == m.goal_y) final = next; 427 | } 428 | } 429 | else { 430 | cout << "BAD" << endl; 431 | g[cx][cy] = 2147483647; 432 | updateVertex(m, cur); 433 | for (auto motion : motions) { 434 | int nx = cx + motion.x, ny = cy + motion.y; 435 | if (m.checkCell(nx, ny) == OBSTACLE) continue; 436 | LPNode* next = new LPNode(nx, ny, k1(m, nx, ny), k2(nx, ny), cur); 437 | updateVertex(m, next); 438 | } 439 | } 440 | 441 | if (final != NULL) break; 442 | } 443 | 444 | calcPath(final, m); 445 | } 446 | }; 447 | 448 | int main() 449 | { 450 | vector o_x, o_y; 451 | for (int i = 0;i < 50;i++) { 452 | o_x.push_back(0); 453 | o_y.push_back(i); 454 | } 455 | for (int i = 0;i < 50;i++) { 456 | o_x.push_back(i); 457 | o_y.push_back(0); 458 | } 459 | for (int i = 0;i < 50;i++) { 460 | o_x.push_back(i); 461 | o_y.push_back(49); 462 | } 463 | for (int i = 0;i < 50;i++) { 464 | o_x.push_back(49); 465 | o_y.push_back(i); 466 | } 467 | for (int i = 0;i < 23;i++) { 468 | o_x.push_back(i); 469 | o_y.push_back(15); 470 | } 471 | for (int i = 0;i < 20;i++) { 472 | o_x.push_back(50 - i); 473 | o_y.push_back(35); 474 | } 475 | LocalObstacleMap m(50, 50, 5, 5, 30, 45, o_x, o_y, "D*-Lite"); 476 | DstarLitePlanner planner; 477 | planner.DstarLitePlanning(m); 478 | m.render(0); 479 | cv::resize(m.background, m.background, cv::Size(200, 200)); 480 | cv::imwrite("../results/planning/dstar.png", m.background); 481 | 482 | // GlobalObstacleMap m(50, 50, 5, 5, 45, 45, o_x, o_y, "LPA*"); 483 | // LPAstarPlanner planner; 484 | // planner.LPAstarPlanning(m); 485 | // std::cout << planner.pq.size(); 486 | // m.render(0); 487 | 488 | // cv::Mat ori, final; 489 | // cv::resize(m.background, ori, cv::Size(200, 200)); 490 | // cv::imwrite("../results/planning/lpastar1.png", ori); 491 | 492 | // vector c_x = {26, 27, 28, 29}, c_y = {15, 15, 15, 15}; 493 | // m.setObstacle(c_x, c_y); 494 | // m.render(0); 495 | // planner.LPAstarReplanning(m, c_x, c_y); 496 | // m.render(0); 497 | 498 | // cv::resize(m.background, final, cv::Size(200, 200)); 499 | // cv::imwrite("../results/planning/lpastar2.png", final); 500 | 501 | return 0; 502 | } -------------------------------------------------------------------------------- /include/3rdParty/ALGLIB/fasttransforms.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | ALGLIB 3.16.0 (source code generated 2019-12-19) 3 | Copyright (c) Sergey Bochkanov (ALGLIB project). 4 | 5 | >>> SOURCE LICENSE >>> 6 | This program is free software; you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation (www.fsf.org); either version 2 of the 9 | License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | A copy of the GNU General Public License is available at 17 | http://www.fsf.org/licensing/licenses 18 | >>> END OF LICENSE >>> 19 | *************************************************************************/ 20 | #ifndef _fasttransforms_pkg_h 21 | #define _fasttransforms_pkg_h 22 | #include "ap.h" 23 | #include "alglibinternal.h" 24 | 25 | ///////////////////////////////////////////////////////////////////////// 26 | // 27 | // THIS SECTION CONTAINS COMPUTATIONAL CORE DECLARATIONS (DATATYPES) 28 | // 29 | ///////////////////////////////////////////////////////////////////////// 30 | namespace alglib_impl 31 | { 32 | #if defined(AE_COMPILE_FFT) || !defined(AE_PARTIAL_BUILD) 33 | #endif 34 | #if defined(AE_COMPILE_FHT) || !defined(AE_PARTIAL_BUILD) 35 | #endif 36 | #if defined(AE_COMPILE_CONV) || !defined(AE_PARTIAL_BUILD) 37 | #endif 38 | #if defined(AE_COMPILE_CORR) || !defined(AE_PARTIAL_BUILD) 39 | #endif 40 | 41 | } 42 | 43 | ///////////////////////////////////////////////////////////////////////// 44 | // 45 | // THIS SECTION CONTAINS C++ INTERFACE 46 | // 47 | ///////////////////////////////////////////////////////////////////////// 48 | namespace alglib 49 | { 50 | 51 | #if defined(AE_COMPILE_FFT) || !defined(AE_PARTIAL_BUILD) 52 | 53 | #endif 54 | 55 | #if defined(AE_COMPILE_FHT) || !defined(AE_PARTIAL_BUILD) 56 | 57 | #endif 58 | 59 | #if defined(AE_COMPILE_CONV) || !defined(AE_PARTIAL_BUILD) 60 | 61 | #endif 62 | 63 | #if defined(AE_COMPILE_CORR) || !defined(AE_PARTIAL_BUILD) 64 | 65 | #endif 66 | 67 | #if defined(AE_COMPILE_FFT) || !defined(AE_PARTIAL_BUILD) 68 | /************************************************************************* 69 | 1-dimensional complex FFT. 70 | 71 | Array size N may be arbitrary number (composite or prime). Composite N's 72 | are handled with cache-oblivious variation of a Cooley-Tukey algorithm. 73 | Small prime-factors are transformed using hard coded codelets (similar to 74 | FFTW codelets, but without low-level optimization), large prime-factors 75 | are handled with Bluestein's algorithm. 76 | 77 | Fastests transforms are for smooth N's (prime factors are 2, 3, 5 only), 78 | most fast for powers of 2. When N have prime factors larger than these, 79 | but orders of magnitude smaller than N, computations will be about 4 times 80 | slower than for nearby highly composite N's. When N itself is prime, speed 81 | will be 6 times lower. 82 | 83 | Algorithm has O(N*logN) complexity for any N (composite or prime). 84 | 85 | INPUT PARAMETERS 86 | A - array[0..N-1] - complex function to be transformed 87 | N - problem size 88 | 89 | OUTPUT PARAMETERS 90 | A - DFT of a input array, array[0..N-1] 91 | A_out[j] = SUM(A_in[k]*exp(-2*pi*sqrt(-1)*j*k/N), k = 0..N-1) 92 | 93 | 94 | -- ALGLIB -- 95 | Copyright 29.05.2009 by Bochkanov Sergey 96 | *************************************************************************/ 97 | void fftc1d(complex_1d_array &a, const ae_int_t n, const xparams _xparams = alglib::xdefault); 98 | void fftc1d(complex_1d_array &a, const xparams _xparams = alglib::xdefault); 99 | 100 | 101 | /************************************************************************* 102 | 1-dimensional complex inverse FFT. 103 | 104 | Array size N may be arbitrary number (composite or prime). Algorithm has 105 | O(N*logN) complexity for any N (composite or prime). 106 | 107 | See FFTC1D() description for more information about algorithm performance. 108 | 109 | INPUT PARAMETERS 110 | A - array[0..N-1] - complex array to be transformed 111 | N - problem size 112 | 113 | OUTPUT PARAMETERS 114 | A - inverse DFT of a input array, array[0..N-1] 115 | A_out[j] = SUM(A_in[k]/N*exp(+2*pi*sqrt(-1)*j*k/N), k = 0..N-1) 116 | 117 | 118 | -- ALGLIB -- 119 | Copyright 29.05.2009 by Bochkanov Sergey 120 | *************************************************************************/ 121 | void fftc1dinv(complex_1d_array &a, const ae_int_t n, const xparams _xparams = alglib::xdefault); 122 | void fftc1dinv(complex_1d_array &a, const xparams _xparams = alglib::xdefault); 123 | 124 | 125 | /************************************************************************* 126 | 1-dimensional real FFT. 127 | 128 | Algorithm has O(N*logN) complexity for any N (composite or prime). 129 | 130 | INPUT PARAMETERS 131 | A - array[0..N-1] - real function to be transformed 132 | N - problem size 133 | 134 | OUTPUT PARAMETERS 135 | F - DFT of a input array, array[0..N-1] 136 | F[j] = SUM(A[k]*exp(-2*pi*sqrt(-1)*j*k/N), k = 0..N-1) 137 | 138 | NOTE: 139 | F[] satisfies symmetry property F[k] = conj(F[N-k]), so just one half 140 | of array is usually needed. But for convinience subroutine returns full 141 | complex array (with frequencies above N/2), so its result may be used by 142 | other FFT-related subroutines. 143 | 144 | 145 | -- ALGLIB -- 146 | Copyright 01.06.2009 by Bochkanov Sergey 147 | *************************************************************************/ 148 | void fftr1d(const real_1d_array &a, const ae_int_t n, complex_1d_array &f, const xparams _xparams = alglib::xdefault); 149 | void fftr1d(const real_1d_array &a, complex_1d_array &f, const xparams _xparams = alglib::xdefault); 150 | 151 | 152 | /************************************************************************* 153 | 1-dimensional real inverse FFT. 154 | 155 | Algorithm has O(N*logN) complexity for any N (composite or prime). 156 | 157 | INPUT PARAMETERS 158 | F - array[0..floor(N/2)] - frequencies from forward real FFT 159 | N - problem size 160 | 161 | OUTPUT PARAMETERS 162 | A - inverse DFT of a input array, array[0..N-1] 163 | 164 | NOTE: 165 | F[] should satisfy symmetry property F[k] = conj(F[N-k]), so just one 166 | half of frequencies array is needed - elements from 0 to floor(N/2). F[0] 167 | is ALWAYS real. If N is even F[floor(N/2)] is real too. If N is odd, then 168 | F[floor(N/2)] has no special properties. 169 | 170 | Relying on properties noted above, FFTR1DInv subroutine uses only elements 171 | from 0th to floor(N/2)-th. It ignores imaginary part of F[0], and in case 172 | N is even it ignores imaginary part of F[floor(N/2)] too. 173 | 174 | When you call this function using full arguments list - "FFTR1DInv(F,N,A)" 175 | - you can pass either either frequencies array with N elements or reduced 176 | array with roughly N/2 elements - subroutine will successfully transform 177 | both. 178 | 179 | If you call this function using reduced arguments list - "FFTR1DInv(F,A)" 180 | - you must pass FULL array with N elements (although higher N/2 are still 181 | not used) because array size is used to automatically determine FFT length 182 | 183 | 184 | -- ALGLIB -- 185 | Copyright 01.06.2009 by Bochkanov Sergey 186 | *************************************************************************/ 187 | void fftr1dinv(const complex_1d_array &f, const ae_int_t n, real_1d_array &a, const xparams _xparams = alglib::xdefault); 188 | void fftr1dinv(const complex_1d_array &f, real_1d_array &a, const xparams _xparams = alglib::xdefault); 189 | #endif 190 | 191 | #if defined(AE_COMPILE_FHT) || !defined(AE_PARTIAL_BUILD) 192 | /************************************************************************* 193 | 1-dimensional Fast Hartley Transform. 194 | 195 | Algorithm has O(N*logN) complexity for any N (composite or prime). 196 | 197 | INPUT PARAMETERS 198 | A - array[0..N-1] - real function to be transformed 199 | N - problem size 200 | 201 | OUTPUT PARAMETERS 202 | A - FHT of a input array, array[0..N-1], 203 | A_out[k] = sum(A_in[j]*(cos(2*pi*j*k/N)+sin(2*pi*j*k/N)), j=0..N-1) 204 | 205 | 206 | -- ALGLIB -- 207 | Copyright 04.06.2009 by Bochkanov Sergey 208 | *************************************************************************/ 209 | void fhtr1d(real_1d_array &a, const ae_int_t n, const xparams _xparams = alglib::xdefault); 210 | 211 | 212 | /************************************************************************* 213 | 1-dimensional inverse FHT. 214 | 215 | Algorithm has O(N*logN) complexity for any N (composite or prime). 216 | 217 | INPUT PARAMETERS 218 | A - array[0..N-1] - complex array to be transformed 219 | N - problem size 220 | 221 | OUTPUT PARAMETERS 222 | A - inverse FHT of a input array, array[0..N-1] 223 | 224 | 225 | -- ALGLIB -- 226 | Copyright 29.05.2009 by Bochkanov Sergey 227 | *************************************************************************/ 228 | void fhtr1dinv(real_1d_array &a, const ae_int_t n, const xparams _xparams = alglib::xdefault); 229 | #endif 230 | 231 | #if defined(AE_COMPILE_CONV) || !defined(AE_PARTIAL_BUILD) 232 | /************************************************************************* 233 | 1-dimensional complex convolution. 234 | 235 | For given A/B returns conv(A,B) (non-circular). Subroutine can automatically 236 | choose between three implementations: straightforward O(M*N) formula for 237 | very small N (or M), overlap-add algorithm for cases where max(M,N) is 238 | significantly larger than min(M,N), but O(M*N) algorithm is too slow, and 239 | general FFT-based formula for cases where two previois algorithms are too 240 | slow. 241 | 242 | Algorithm has max(M,N)*log(max(M,N)) complexity for any M/N. 243 | 244 | INPUT PARAMETERS 245 | A - array[0..M-1] - complex function to be transformed 246 | M - problem size 247 | B - array[0..N-1] - complex function to be transformed 248 | N - problem size 249 | 250 | OUTPUT PARAMETERS 251 | R - convolution: A*B. array[0..N+M-2]. 252 | 253 | NOTE: 254 | It is assumed that A is zero at T<0, B is zero too. If one or both 255 | functions have non-zero values at negative T's, you can still use this 256 | subroutine - just shift its result correspondingly. 257 | 258 | -- ALGLIB -- 259 | Copyright 21.07.2009 by Bochkanov Sergey 260 | *************************************************************************/ 261 | void convc1d(const complex_1d_array &a, const ae_int_t m, const complex_1d_array &b, const ae_int_t n, complex_1d_array &r, const xparams _xparams = alglib::xdefault); 262 | 263 | 264 | /************************************************************************* 265 | 1-dimensional complex non-circular deconvolution (inverse of ConvC1D()). 266 | 267 | Algorithm has M*log(M)) complexity for any M (composite or prime). 268 | 269 | INPUT PARAMETERS 270 | A - array[0..M-1] - convolved signal, A = conv(R, B) 271 | M - convolved signal length 272 | B - array[0..N-1] - response 273 | N - response length, N<=M 274 | 275 | OUTPUT PARAMETERS 276 | R - deconvolved signal. array[0..M-N]. 277 | 278 | NOTE: 279 | deconvolution is unstable process and may result in division by zero 280 | (if your response function is degenerate, i.e. has zero Fourier coefficient). 281 | 282 | NOTE: 283 | It is assumed that A is zero at T<0, B is zero too. If one or both 284 | functions have non-zero values at negative T's, you can still use this 285 | subroutine - just shift its result correspondingly. 286 | 287 | -- ALGLIB -- 288 | Copyright 21.07.2009 by Bochkanov Sergey 289 | *************************************************************************/ 290 | void convc1dinv(const complex_1d_array &a, const ae_int_t m, const complex_1d_array &b, const ae_int_t n, complex_1d_array &r, const xparams _xparams = alglib::xdefault); 291 | 292 | 293 | /************************************************************************* 294 | 1-dimensional circular complex convolution. 295 | 296 | For given S/R returns conv(S,R) (circular). Algorithm has linearithmic 297 | complexity for any M/N. 298 | 299 | IMPORTANT: normal convolution is commutative, i.e. it is symmetric - 300 | conv(A,B)=conv(B,A). Cyclic convolution IS NOT. One function - S - is a 301 | signal, periodic function, and another - R - is a response, non-periodic 302 | function with limited length. 303 | 304 | INPUT PARAMETERS 305 | S - array[0..M-1] - complex periodic signal 306 | M - problem size 307 | B - array[0..N-1] - complex non-periodic response 308 | N - problem size 309 | 310 | OUTPUT PARAMETERS 311 | R - convolution: A*B. array[0..M-1]. 312 | 313 | NOTE: 314 | It is assumed that B is zero at T<0. If it has non-zero values at 315 | negative T's, you can still use this subroutine - just shift its result 316 | correspondingly. 317 | 318 | -- ALGLIB -- 319 | Copyright 21.07.2009 by Bochkanov Sergey 320 | *************************************************************************/ 321 | void convc1dcircular(const complex_1d_array &s, const ae_int_t m, const complex_1d_array &r, const ae_int_t n, complex_1d_array &c, const xparams _xparams = alglib::xdefault); 322 | 323 | 324 | /************************************************************************* 325 | 1-dimensional circular complex deconvolution (inverse of ConvC1DCircular()). 326 | 327 | Algorithm has M*log(M)) complexity for any M (composite or prime). 328 | 329 | INPUT PARAMETERS 330 | A - array[0..M-1] - convolved periodic signal, A = conv(R, B) 331 | M - convolved signal length 332 | B - array[0..N-1] - non-periodic response 333 | N - response length 334 | 335 | OUTPUT PARAMETERS 336 | R - deconvolved signal. array[0..M-1]. 337 | 338 | NOTE: 339 | deconvolution is unstable process and may result in division by zero 340 | (if your response function is degenerate, i.e. has zero Fourier coefficient). 341 | 342 | NOTE: 343 | It is assumed that B is zero at T<0. If it has non-zero values at 344 | negative T's, you can still use this subroutine - just shift its result 345 | correspondingly. 346 | 347 | -- ALGLIB -- 348 | Copyright 21.07.2009 by Bochkanov Sergey 349 | *************************************************************************/ 350 | void convc1dcircularinv(const complex_1d_array &a, const ae_int_t m, const complex_1d_array &b, const ae_int_t n, complex_1d_array &r, const xparams _xparams = alglib::xdefault); 351 | 352 | 353 | /************************************************************************* 354 | 1-dimensional real convolution. 355 | 356 | Analogous to ConvC1D(), see ConvC1D() comments for more details. 357 | 358 | INPUT PARAMETERS 359 | A - array[0..M-1] - real function to be transformed 360 | M - problem size 361 | B - array[0..N-1] - real function to be transformed 362 | N - problem size 363 | 364 | OUTPUT PARAMETERS 365 | R - convolution: A*B. array[0..N+M-2]. 366 | 367 | NOTE: 368 | It is assumed that A is zero at T<0, B is zero too. If one or both 369 | functions have non-zero values at negative T's, you can still use this 370 | subroutine - just shift its result correspondingly. 371 | 372 | -- ALGLIB -- 373 | Copyright 21.07.2009 by Bochkanov Sergey 374 | *************************************************************************/ 375 | void convr1d(const real_1d_array &a, const ae_int_t m, const real_1d_array &b, const ae_int_t n, real_1d_array &r, const xparams _xparams = alglib::xdefault); 376 | 377 | 378 | /************************************************************************* 379 | 1-dimensional real deconvolution (inverse of ConvC1D()). 380 | 381 | Algorithm has M*log(M)) complexity for any M (composite or prime). 382 | 383 | INPUT PARAMETERS 384 | A - array[0..M-1] - convolved signal, A = conv(R, B) 385 | M - convolved signal length 386 | B - array[0..N-1] - response 387 | N - response length, N<=M 388 | 389 | OUTPUT PARAMETERS 390 | R - deconvolved signal. array[0..M-N]. 391 | 392 | NOTE: 393 | deconvolution is unstable process and may result in division by zero 394 | (if your response function is degenerate, i.e. has zero Fourier coefficient). 395 | 396 | NOTE: 397 | It is assumed that A is zero at T<0, B is zero too. If one or both 398 | functions have non-zero values at negative T's, you can still use this 399 | subroutine - just shift its result correspondingly. 400 | 401 | -- ALGLIB -- 402 | Copyright 21.07.2009 by Bochkanov Sergey 403 | *************************************************************************/ 404 | void convr1dinv(const real_1d_array &a, const ae_int_t m, const real_1d_array &b, const ae_int_t n, real_1d_array &r, const xparams _xparams = alglib::xdefault); 405 | 406 | 407 | /************************************************************************* 408 | 1-dimensional circular real convolution. 409 | 410 | Analogous to ConvC1DCircular(), see ConvC1DCircular() comments for more details. 411 | 412 | INPUT PARAMETERS 413 | S - array[0..M-1] - real signal 414 | M - problem size 415 | B - array[0..N-1] - real response 416 | N - problem size 417 | 418 | OUTPUT PARAMETERS 419 | R - convolution: A*B. array[0..M-1]. 420 | 421 | NOTE: 422 | It is assumed that B is zero at T<0. If it has non-zero values at 423 | negative T's, you can still use this subroutine - just shift its result 424 | correspondingly. 425 | 426 | -- ALGLIB -- 427 | Copyright 21.07.2009 by Bochkanov Sergey 428 | *************************************************************************/ 429 | void convr1dcircular(const real_1d_array &s, const ae_int_t m, const real_1d_array &r, const ae_int_t n, real_1d_array &c, const xparams _xparams = alglib::xdefault); 430 | 431 | 432 | /************************************************************************* 433 | 1-dimensional complex deconvolution (inverse of ConvC1D()). 434 | 435 | Algorithm has M*log(M)) complexity for any M (composite or prime). 436 | 437 | INPUT PARAMETERS 438 | A - array[0..M-1] - convolved signal, A = conv(R, B) 439 | M - convolved signal length 440 | B - array[0..N-1] - response 441 | N - response length 442 | 443 | OUTPUT PARAMETERS 444 | R - deconvolved signal. array[0..M-N]. 445 | 446 | NOTE: 447 | deconvolution is unstable process and may result in division by zero 448 | (if your response function is degenerate, i.e. has zero Fourier coefficient). 449 | 450 | NOTE: 451 | It is assumed that B is zero at T<0. If it has non-zero values at 452 | negative T's, you can still use this subroutine - just shift its result 453 | correspondingly. 454 | 455 | -- ALGLIB -- 456 | Copyright 21.07.2009 by Bochkanov Sergey 457 | *************************************************************************/ 458 | void convr1dcircularinv(const real_1d_array &a, const ae_int_t m, const real_1d_array &b, const ae_int_t n, real_1d_array &r, const xparams _xparams = alglib::xdefault); 459 | #endif 460 | 461 | #if defined(AE_COMPILE_CORR) || !defined(AE_PARTIAL_BUILD) 462 | /************************************************************************* 463 | 1-dimensional complex cross-correlation. 464 | 465 | For given Pattern/Signal returns corr(Pattern,Signal) (non-circular). 466 | 467 | Correlation is calculated using reduction to convolution. Algorithm with 468 | max(N,N)*log(max(N,N)) complexity is used (see ConvC1D() for more info 469 | about performance). 470 | 471 | IMPORTANT: 472 | for historical reasons subroutine accepts its parameters in reversed 473 | order: CorrC1D(Signal, Pattern) = Pattern x Signal (using traditional 474 | definition of cross-correlation, denoting cross-correlation as "x"). 475 | 476 | INPUT PARAMETERS 477 | Signal - array[0..N-1] - complex function to be transformed, 478 | signal containing pattern 479 | N - problem size 480 | Pattern - array[0..M-1] - complex function to be transformed, 481 | pattern to search withing signal 482 | M - problem size 483 | 484 | OUTPUT PARAMETERS 485 | R - cross-correlation, array[0..N+M-2]: 486 | * positive lags are stored in R[0..N-1], 487 | R[i] = sum(conj(pattern[j])*signal[i+j] 488 | * negative lags are stored in R[N..N+M-2], 489 | R[N+M-1-i] = sum(conj(pattern[j])*signal[-i+j] 490 | 491 | NOTE: 492 | It is assumed that pattern domain is [0..M-1]. If Pattern is non-zero 493 | on [-K..M-1], you can still use this subroutine, just shift result by K. 494 | 495 | -- ALGLIB -- 496 | Copyright 21.07.2009 by Bochkanov Sergey 497 | *************************************************************************/ 498 | void corrc1d(const complex_1d_array &signal, const ae_int_t n, const complex_1d_array &pattern, const ae_int_t m, complex_1d_array &r, const xparams _xparams = alglib::xdefault); 499 | 500 | 501 | /************************************************************************* 502 | 1-dimensional circular complex cross-correlation. 503 | 504 | For given Pattern/Signal returns corr(Pattern,Signal) (circular). 505 | Algorithm has linearithmic complexity for any M/N. 506 | 507 | IMPORTANT: 508 | for historical reasons subroutine accepts its parameters in reversed 509 | order: CorrC1DCircular(Signal, Pattern) = Pattern x Signal (using 510 | traditional definition of cross-correlation, denoting cross-correlation 511 | as "x"). 512 | 513 | INPUT PARAMETERS 514 | Signal - array[0..N-1] - complex function to be transformed, 515 | periodic signal containing pattern 516 | N - problem size 517 | Pattern - array[0..M-1] - complex function to be transformed, 518 | non-periodic pattern to search withing signal 519 | M - problem size 520 | 521 | OUTPUT PARAMETERS 522 | R - convolution: A*B. array[0..M-1]. 523 | 524 | 525 | -- ALGLIB -- 526 | Copyright 21.07.2009 by Bochkanov Sergey 527 | *************************************************************************/ 528 | void corrc1dcircular(const complex_1d_array &signal, const ae_int_t m, const complex_1d_array &pattern, const ae_int_t n, complex_1d_array &c, const xparams _xparams = alglib::xdefault); 529 | 530 | 531 | /************************************************************************* 532 | 1-dimensional real cross-correlation. 533 | 534 | For given Pattern/Signal returns corr(Pattern,Signal) (non-circular). 535 | 536 | Correlation is calculated using reduction to convolution. Algorithm with 537 | max(N,N)*log(max(N,N)) complexity is used (see ConvC1D() for more info 538 | about performance). 539 | 540 | IMPORTANT: 541 | for historical reasons subroutine accepts its parameters in reversed 542 | order: CorrR1D(Signal, Pattern) = Pattern x Signal (using traditional 543 | definition of cross-correlation, denoting cross-correlation as "x"). 544 | 545 | INPUT PARAMETERS 546 | Signal - array[0..N-1] - real function to be transformed, 547 | signal containing pattern 548 | N - problem size 549 | Pattern - array[0..M-1] - real function to be transformed, 550 | pattern to search withing signal 551 | M - problem size 552 | 553 | OUTPUT PARAMETERS 554 | R - cross-correlation, array[0..N+M-2]: 555 | * positive lags are stored in R[0..N-1], 556 | R[i] = sum(pattern[j]*signal[i+j] 557 | * negative lags are stored in R[N..N+M-2], 558 | R[N+M-1-i] = sum(pattern[j]*signal[-i+j] 559 | 560 | NOTE: 561 | It is assumed that pattern domain is [0..M-1]. If Pattern is non-zero 562 | on [-K..M-1], you can still use this subroutine, just shift result by K. 563 | 564 | -- ALGLIB -- 565 | Copyright 21.07.2009 by Bochkanov Sergey 566 | *************************************************************************/ 567 | void corrr1d(const real_1d_array &signal, const ae_int_t n, const real_1d_array &pattern, const ae_int_t m, real_1d_array &r, const xparams _xparams = alglib::xdefault); 568 | 569 | 570 | /************************************************************************* 571 | 1-dimensional circular real cross-correlation. 572 | 573 | For given Pattern/Signal returns corr(Pattern,Signal) (circular). 574 | Algorithm has linearithmic complexity for any M/N. 575 | 576 | IMPORTANT: 577 | for historical reasons subroutine accepts its parameters in reversed 578 | order: CorrR1DCircular(Signal, Pattern) = Pattern x Signal (using 579 | traditional definition of cross-correlation, denoting cross-correlation 580 | as "x"). 581 | 582 | INPUT PARAMETERS 583 | Signal - array[0..N-1] - real function to be transformed, 584 | periodic signal containing pattern 585 | N - problem size 586 | Pattern - array[0..M-1] - real function to be transformed, 587 | non-periodic pattern to search withing signal 588 | M - problem size 589 | 590 | OUTPUT PARAMETERS 591 | R - convolution: A*B. array[0..M-1]. 592 | 593 | 594 | -- ALGLIB -- 595 | Copyright 21.07.2009 by Bochkanov Sergey 596 | *************************************************************************/ 597 | void corrr1dcircular(const real_1d_array &signal, const ae_int_t m, const real_1d_array &pattern, const ae_int_t n, real_1d_array &c, const xparams _xparams = alglib::xdefault); 598 | #endif 599 | } 600 | 601 | ///////////////////////////////////////////////////////////////////////// 602 | // 603 | // THIS SECTION CONTAINS COMPUTATIONAL CORE DECLARATIONS (FUNCTIONS) 604 | // 605 | ///////////////////////////////////////////////////////////////////////// 606 | namespace alglib_impl 607 | { 608 | #if defined(AE_COMPILE_FFT) || !defined(AE_PARTIAL_BUILD) 609 | void fftc1d(/* Complex */ ae_vector* a, ae_int_t n, ae_state *_state); 610 | void fftc1dinv(/* Complex */ ae_vector* a, ae_int_t n, ae_state *_state); 611 | void fftr1d(/* Real */ ae_vector* a, 612 | ae_int_t n, 613 | /* Complex */ ae_vector* f, 614 | ae_state *_state); 615 | void fftr1dinv(/* Complex */ ae_vector* f, 616 | ae_int_t n, 617 | /* Real */ ae_vector* a, 618 | ae_state *_state); 619 | void fftr1dinternaleven(/* Real */ ae_vector* a, 620 | ae_int_t n, 621 | /* Real */ ae_vector* buf, 622 | fasttransformplan* plan, 623 | ae_state *_state); 624 | void fftr1dinvinternaleven(/* Real */ ae_vector* a, 625 | ae_int_t n, 626 | /* Real */ ae_vector* buf, 627 | fasttransformplan* plan, 628 | ae_state *_state); 629 | #endif 630 | #if defined(AE_COMPILE_FHT) || !defined(AE_PARTIAL_BUILD) 631 | void fhtr1d(/* Real */ ae_vector* a, ae_int_t n, ae_state *_state); 632 | void fhtr1dinv(/* Real */ ae_vector* a, ae_int_t n, ae_state *_state); 633 | #endif 634 | #if defined(AE_COMPILE_CONV) || !defined(AE_PARTIAL_BUILD) 635 | void convc1d(/* Complex */ ae_vector* a, 636 | ae_int_t m, 637 | /* Complex */ ae_vector* b, 638 | ae_int_t n, 639 | /* Complex */ ae_vector* r, 640 | ae_state *_state); 641 | void convc1dinv(/* Complex */ ae_vector* a, 642 | ae_int_t m, 643 | /* Complex */ ae_vector* b, 644 | ae_int_t n, 645 | /* Complex */ ae_vector* r, 646 | ae_state *_state); 647 | void convc1dcircular(/* Complex */ ae_vector* s, 648 | ae_int_t m, 649 | /* Complex */ ae_vector* r, 650 | ae_int_t n, 651 | /* Complex */ ae_vector* c, 652 | ae_state *_state); 653 | void convc1dcircularinv(/* Complex */ ae_vector* a, 654 | ae_int_t m, 655 | /* Complex */ ae_vector* b, 656 | ae_int_t n, 657 | /* Complex */ ae_vector* r, 658 | ae_state *_state); 659 | void convr1d(/* Real */ ae_vector* a, 660 | ae_int_t m, 661 | /* Real */ ae_vector* b, 662 | ae_int_t n, 663 | /* Real */ ae_vector* r, 664 | ae_state *_state); 665 | void convr1dinv(/* Real */ ae_vector* a, 666 | ae_int_t m, 667 | /* Real */ ae_vector* b, 668 | ae_int_t n, 669 | /* Real */ ae_vector* r, 670 | ae_state *_state); 671 | void convr1dcircular(/* Real */ ae_vector* s, 672 | ae_int_t m, 673 | /* Real */ ae_vector* r, 674 | ae_int_t n, 675 | /* Real */ ae_vector* c, 676 | ae_state *_state); 677 | void convr1dcircularinv(/* Real */ ae_vector* a, 678 | ae_int_t m, 679 | /* Real */ ae_vector* b, 680 | ae_int_t n, 681 | /* Real */ ae_vector* r, 682 | ae_state *_state); 683 | void convc1dx(/* Complex */ ae_vector* a, 684 | ae_int_t m, 685 | /* Complex */ ae_vector* b, 686 | ae_int_t n, 687 | ae_bool circular, 688 | ae_int_t alg, 689 | ae_int_t q, 690 | /* Complex */ ae_vector* r, 691 | ae_state *_state); 692 | void convr1dx(/* Real */ ae_vector* a, 693 | ae_int_t m, 694 | /* Real */ ae_vector* b, 695 | ae_int_t n, 696 | ae_bool circular, 697 | ae_int_t alg, 698 | ae_int_t q, 699 | /* Real */ ae_vector* r, 700 | ae_state *_state); 701 | #endif 702 | #if defined(AE_COMPILE_CORR) || !defined(AE_PARTIAL_BUILD) 703 | void corrc1d(/* Complex */ ae_vector* signal, 704 | ae_int_t n, 705 | /* Complex */ ae_vector* pattern, 706 | ae_int_t m, 707 | /* Complex */ ae_vector* r, 708 | ae_state *_state); 709 | void corrc1dcircular(/* Complex */ ae_vector* signal, 710 | ae_int_t m, 711 | /* Complex */ ae_vector* pattern, 712 | ae_int_t n, 713 | /* Complex */ ae_vector* c, 714 | ae_state *_state); 715 | void corrr1d(/* Real */ ae_vector* signal, 716 | ae_int_t n, 717 | /* Real */ ae_vector* pattern, 718 | ae_int_t m, 719 | /* Real */ ae_vector* r, 720 | ae_state *_state); 721 | void corrr1dcircular(/* Real */ ae_vector* signal, 722 | ae_int_t m, 723 | /* Real */ ae_vector* pattern, 724 | ae_int_t n, 725 | /* Real */ ae_vector* c, 726 | ae_state *_state); 727 | #endif 728 | 729 | } 730 | #endif 731 | 732 | -------------------------------------------------------------------------------- /include/3rdParty/ALGLIB/integration.h: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | ALGLIB 3.16.0 (source code generated 2019-12-19) 3 | Copyright (c) Sergey Bochkanov (ALGLIB project). 4 | 5 | >>> SOURCE LICENSE >>> 6 | This program is free software; you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation (www.fsf.org); either version 2 of the 9 | License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | A copy of the GNU General Public License is available at 17 | http://www.fsf.org/licensing/licenses 18 | >>> END OF LICENSE >>> 19 | *************************************************************************/ 20 | #ifndef _integration_pkg_h 21 | #define _integration_pkg_h 22 | #include "ap.h" 23 | #include "alglibinternal.h" 24 | #include "alglibmisc.h" 25 | #include "linalg.h" 26 | #include "specialfunctions.h" 27 | 28 | ///////////////////////////////////////////////////////////////////////// 29 | // 30 | // THIS SECTION CONTAINS COMPUTATIONAL CORE DECLARATIONS (DATATYPES) 31 | // 32 | ///////////////////////////////////////////////////////////////////////// 33 | namespace alglib_impl 34 | { 35 | #if defined(AE_COMPILE_GQ) || !defined(AE_PARTIAL_BUILD) 36 | #endif 37 | #if defined(AE_COMPILE_GKQ) || !defined(AE_PARTIAL_BUILD) 38 | #endif 39 | #if defined(AE_COMPILE_AUTOGK) || !defined(AE_PARTIAL_BUILD) 40 | typedef struct 41 | { 42 | ae_int_t terminationtype; 43 | ae_int_t nfev; 44 | ae_int_t nintervals; 45 | } autogkreport; 46 | typedef struct 47 | { 48 | double a; 49 | double b; 50 | double eps; 51 | double xwidth; 52 | double x; 53 | double f; 54 | ae_int_t info; 55 | double r; 56 | ae_matrix heap; 57 | ae_int_t heapsize; 58 | ae_int_t heapwidth; 59 | ae_int_t heapused; 60 | double sumerr; 61 | double sumabs; 62 | ae_vector qn; 63 | ae_vector wg; 64 | ae_vector wk; 65 | ae_vector wr; 66 | ae_int_t n; 67 | rcommstate rstate; 68 | } autogkinternalstate; 69 | typedef struct 70 | { 71 | double a; 72 | double b; 73 | double alpha; 74 | double beta; 75 | double xwidth; 76 | double x; 77 | double xminusa; 78 | double bminusx; 79 | ae_bool needf; 80 | double f; 81 | ae_int_t wrappermode; 82 | autogkinternalstate internalstate; 83 | rcommstate rstate; 84 | double v; 85 | ae_int_t terminationtype; 86 | ae_int_t nfev; 87 | ae_int_t nintervals; 88 | } autogkstate; 89 | #endif 90 | 91 | } 92 | 93 | ///////////////////////////////////////////////////////////////////////// 94 | // 95 | // THIS SECTION CONTAINS C++ INTERFACE 96 | // 97 | ///////////////////////////////////////////////////////////////////////// 98 | namespace alglib 99 | { 100 | 101 | #if defined(AE_COMPILE_GQ) || !defined(AE_PARTIAL_BUILD) 102 | 103 | #endif 104 | 105 | #if defined(AE_COMPILE_GKQ) || !defined(AE_PARTIAL_BUILD) 106 | 107 | #endif 108 | 109 | #if defined(AE_COMPILE_AUTOGK) || !defined(AE_PARTIAL_BUILD) 110 | /************************************************************************* 111 | Integration report: 112 | * TerminationType = completetion code: 113 | * -5 non-convergence of Gauss-Kronrod nodes 114 | calculation subroutine. 115 | * -1 incorrect parameters were specified 116 | * 1 OK 117 | * Rep.NFEV countains number of function calculations 118 | * Rep.NIntervals contains number of intervals [a,b] 119 | was partitioned into. 120 | *************************************************************************/ 121 | class _autogkreport_owner 122 | { 123 | public: 124 | _autogkreport_owner(); 125 | _autogkreport_owner(const _autogkreport_owner &rhs); 126 | _autogkreport_owner& operator=(const _autogkreport_owner &rhs); 127 | virtual ~_autogkreport_owner(); 128 | alglib_impl::autogkreport* c_ptr(); 129 | alglib_impl::autogkreport* c_ptr() const; 130 | protected: 131 | alglib_impl::autogkreport *p_struct; 132 | }; 133 | class autogkreport : public _autogkreport_owner 134 | { 135 | public: 136 | autogkreport(); 137 | autogkreport(const autogkreport &rhs); 138 | autogkreport& operator=(const autogkreport &rhs); 139 | virtual ~autogkreport(); 140 | ae_int_t &terminationtype; 141 | ae_int_t &nfev; 142 | ae_int_t &nintervals; 143 | 144 | }; 145 | 146 | 147 | /************************************************************************* 148 | This structure stores state of the integration algorithm. 149 | 150 | Although this class has public fields, they are not intended for external 151 | use. You should use ALGLIB functions to work with this class: 152 | * autogksmooth()/AutoGKSmoothW()/... to create objects 153 | * autogkintegrate() to begin integration 154 | * autogkresults() to get results 155 | *************************************************************************/ 156 | class _autogkstate_owner 157 | { 158 | public: 159 | _autogkstate_owner(); 160 | _autogkstate_owner(const _autogkstate_owner &rhs); 161 | _autogkstate_owner& operator=(const _autogkstate_owner &rhs); 162 | virtual ~_autogkstate_owner(); 163 | alglib_impl::autogkstate* c_ptr(); 164 | alglib_impl::autogkstate* c_ptr() const; 165 | protected: 166 | alglib_impl::autogkstate *p_struct; 167 | }; 168 | class autogkstate : public _autogkstate_owner 169 | { 170 | public: 171 | autogkstate(); 172 | autogkstate(const autogkstate &rhs); 173 | autogkstate& operator=(const autogkstate &rhs); 174 | virtual ~autogkstate(); 175 | ae_bool &needf; 176 | double &x; 177 | double &xminusa; 178 | double &bminusx; 179 | double &f; 180 | 181 | }; 182 | #endif 183 | 184 | #if defined(AE_COMPILE_GQ) || !defined(AE_PARTIAL_BUILD) 185 | /************************************************************************* 186 | Computation of nodes and weights for a Gauss quadrature formula 187 | 188 | The algorithm generates the N-point Gauss quadrature formula with weight 189 | function given by coefficients alpha and beta of a recurrence relation 190 | which generates a system of orthogonal polynomials: 191 | 192 | P-1(x) = 0 193 | P0(x) = 1 194 | Pn+1(x) = (x-alpha(n))*Pn(x) - beta(n)*Pn-1(x) 195 | 196 | and zeroth moment Mu0 197 | 198 | Mu0 = integral(W(x)dx,a,b) 199 | 200 | INPUT PARAMETERS: 201 | Alpha - array[0..N-1], alpha coefficients 202 | Beta - array[0..N-1], beta coefficients 203 | Zero-indexed element is not used and may be arbitrary. 204 | Beta[I]>0. 205 | Mu0 - zeroth moment of the weight function. 206 | N - number of nodes of the quadrature formula, N>=1 207 | 208 | OUTPUT PARAMETERS: 209 | Info - error code: 210 | * -3 internal eigenproblem solver hasn't converged 211 | * -2 Beta[i]<=0 212 | * -1 incorrect N was passed 213 | * 1 OK 214 | X - array[0..N-1] - array of quadrature nodes, 215 | in ascending order. 216 | W - array[0..N-1] - array of quadrature weights. 217 | 218 | -- ALGLIB -- 219 | Copyright 2005-2009 by Bochkanov Sergey 220 | *************************************************************************/ 221 | void gqgeneraterec(const real_1d_array &alpha, const real_1d_array &beta, const double mu0, const ae_int_t n, ae_int_t &info, real_1d_array &x, real_1d_array &w, const xparams _xparams = alglib::xdefault); 222 | 223 | 224 | /************************************************************************* 225 | Computation of nodes and weights for a Gauss-Lobatto quadrature formula 226 | 227 | The algorithm generates the N-point Gauss-Lobatto quadrature formula with 228 | weight function given by coefficients alpha and beta of a recurrence which 229 | generates a system of orthogonal polynomials. 230 | 231 | P-1(x) = 0 232 | P0(x) = 1 233 | Pn+1(x) = (x-alpha(n))*Pn(x) - beta(n)*Pn-1(x) 234 | 235 | and zeroth moment Mu0 236 | 237 | Mu0 = integral(W(x)dx,a,b) 238 | 239 | INPUT PARAMETERS: 240 | Alpha - array[0..N-2], alpha coefficients 241 | Beta - array[0..N-2], beta coefficients. 242 | Zero-indexed element is not used, may be arbitrary. 243 | Beta[I]>0 244 | Mu0 - zeroth moment of the weighting function. 245 | A - left boundary of the integration interval. 246 | B - right boundary of the integration interval. 247 | N - number of nodes of the quadrature formula, N>=3 248 | (including the left and right boundary nodes). 249 | 250 | OUTPUT PARAMETERS: 251 | Info - error code: 252 | * -3 internal eigenproblem solver hasn't converged 253 | * -2 Beta[i]<=0 254 | * -1 incorrect N was passed 255 | * 1 OK 256 | X - array[0..N-1] - array of quadrature nodes, 257 | in ascending order. 258 | W - array[0..N-1] - array of quadrature weights. 259 | 260 | -- ALGLIB -- 261 | Copyright 2005-2009 by Bochkanov Sergey 262 | *************************************************************************/ 263 | void gqgenerategausslobattorec(const real_1d_array &alpha, const real_1d_array &beta, const double mu0, const double a, const double b, const ae_int_t n, ae_int_t &info, real_1d_array &x, real_1d_array &w, const xparams _xparams = alglib::xdefault); 264 | 265 | 266 | /************************************************************************* 267 | Computation of nodes and weights for a Gauss-Radau quadrature formula 268 | 269 | The algorithm generates the N-point Gauss-Radau quadrature formula with 270 | weight function given by the coefficients alpha and beta of a recurrence 271 | which generates a system of orthogonal polynomials. 272 | 273 | P-1(x) = 0 274 | P0(x) = 1 275 | Pn+1(x) = (x-alpha(n))*Pn(x) - beta(n)*Pn-1(x) 276 | 277 | and zeroth moment Mu0 278 | 279 | Mu0 = integral(W(x)dx,a,b) 280 | 281 | INPUT PARAMETERS: 282 | Alpha - array[0..N-2], alpha coefficients. 283 | Beta - array[0..N-1], beta coefficients 284 | Zero-indexed element is not used. 285 | Beta[I]>0 286 | Mu0 - zeroth moment of the weighting function. 287 | A - left boundary of the integration interval. 288 | N - number of nodes of the quadrature formula, N>=2 289 | (including the left boundary node). 290 | 291 | OUTPUT PARAMETERS: 292 | Info - error code: 293 | * -3 internal eigenproblem solver hasn't converged 294 | * -2 Beta[i]<=0 295 | * -1 incorrect N was passed 296 | * 1 OK 297 | X - array[0..N-1] - array of quadrature nodes, 298 | in ascending order. 299 | W - array[0..N-1] - array of quadrature weights. 300 | 301 | 302 | -- ALGLIB -- 303 | Copyright 2005-2009 by Bochkanov Sergey 304 | *************************************************************************/ 305 | void gqgenerategaussradaurec(const real_1d_array &alpha, const real_1d_array &beta, const double mu0, const double a, const ae_int_t n, ae_int_t &info, real_1d_array &x, real_1d_array &w, const xparams _xparams = alglib::xdefault); 306 | 307 | 308 | /************************************************************************* 309 | Returns nodes/weights for Gauss-Legendre quadrature on [-1,1] with N 310 | nodes. 311 | 312 | INPUT PARAMETERS: 313 | N - number of nodes, >=1 314 | 315 | OUTPUT PARAMETERS: 316 | Info - error code: 317 | * -4 an error was detected when calculating 318 | weights/nodes. N is too large to obtain 319 | weights/nodes with high enough accuracy. 320 | Try to use multiple precision version. 321 | * -3 internal eigenproblem solver hasn't converged 322 | * -1 incorrect N was passed 323 | * +1 OK 324 | X - array[0..N-1] - array of quadrature nodes, 325 | in ascending order. 326 | W - array[0..N-1] - array of quadrature weights. 327 | 328 | 329 | -- ALGLIB -- 330 | Copyright 12.05.2009 by Bochkanov Sergey 331 | *************************************************************************/ 332 | void gqgenerategausslegendre(const ae_int_t n, ae_int_t &info, real_1d_array &x, real_1d_array &w, const xparams _xparams = alglib::xdefault); 333 | 334 | 335 | /************************************************************************* 336 | Returns nodes/weights for Gauss-Jacobi quadrature on [-1,1] with weight 337 | function W(x)=Power(1-x,Alpha)*Power(1+x,Beta). 338 | 339 | INPUT PARAMETERS: 340 | N - number of nodes, >=1 341 | Alpha - power-law coefficient, Alpha>-1 342 | Beta - power-law coefficient, Beta>-1 343 | 344 | OUTPUT PARAMETERS: 345 | Info - error code: 346 | * -4 an error was detected when calculating 347 | weights/nodes. Alpha or Beta are too close 348 | to -1 to obtain weights/nodes with high enough 349 | accuracy, or, may be, N is too large. Try to 350 | use multiple precision version. 351 | * -3 internal eigenproblem solver hasn't converged 352 | * -1 incorrect N/Alpha/Beta was passed 353 | * +1 OK 354 | X - array[0..N-1] - array of quadrature nodes, 355 | in ascending order. 356 | W - array[0..N-1] - array of quadrature weights. 357 | 358 | 359 | -- ALGLIB -- 360 | Copyright 12.05.2009 by Bochkanov Sergey 361 | *************************************************************************/ 362 | void gqgenerategaussjacobi(const ae_int_t n, const double alpha, const double beta, ae_int_t &info, real_1d_array &x, real_1d_array &w, const xparams _xparams = alglib::xdefault); 363 | 364 | 365 | /************************************************************************* 366 | Returns nodes/weights for Gauss-Laguerre quadrature on [0,+inf) with 367 | weight function W(x)=Power(x,Alpha)*Exp(-x) 368 | 369 | INPUT PARAMETERS: 370 | N - number of nodes, >=1 371 | Alpha - power-law coefficient, Alpha>-1 372 | 373 | OUTPUT PARAMETERS: 374 | Info - error code: 375 | * -4 an error was detected when calculating 376 | weights/nodes. Alpha is too close to -1 to 377 | obtain weights/nodes with high enough accuracy 378 | or, may be, N is too large. Try to use 379 | multiple precision version. 380 | * -3 internal eigenproblem solver hasn't converged 381 | * -1 incorrect N/Alpha was passed 382 | * +1 OK 383 | X - array[0..N-1] - array of quadrature nodes, 384 | in ascending order. 385 | W - array[0..N-1] - array of quadrature weights. 386 | 387 | 388 | -- ALGLIB -- 389 | Copyright 12.05.2009 by Bochkanov Sergey 390 | *************************************************************************/ 391 | void gqgenerategausslaguerre(const ae_int_t n, const double alpha, ae_int_t &info, real_1d_array &x, real_1d_array &w, const xparams _xparams = alglib::xdefault); 392 | 393 | 394 | /************************************************************************* 395 | Returns nodes/weights for Gauss-Hermite quadrature on (-inf,+inf) with 396 | weight function W(x)=Exp(-x*x) 397 | 398 | INPUT PARAMETERS: 399 | N - number of nodes, >=1 400 | 401 | OUTPUT PARAMETERS: 402 | Info - error code: 403 | * -4 an error was detected when calculating 404 | weights/nodes. May be, N is too large. Try to 405 | use multiple precision version. 406 | * -3 internal eigenproblem solver hasn't converged 407 | * -1 incorrect N/Alpha was passed 408 | * +1 OK 409 | X - array[0..N-1] - array of quadrature nodes, 410 | in ascending order. 411 | W - array[0..N-1] - array of quadrature weights. 412 | 413 | 414 | -- ALGLIB -- 415 | Copyright 12.05.2009 by Bochkanov Sergey 416 | *************************************************************************/ 417 | void gqgenerategausshermite(const ae_int_t n, ae_int_t &info, real_1d_array &x, real_1d_array &w, const xparams _xparams = alglib::xdefault); 418 | #endif 419 | 420 | #if defined(AE_COMPILE_GKQ) || !defined(AE_PARTIAL_BUILD) 421 | /************************************************************************* 422 | Computation of nodes and weights of a Gauss-Kronrod quadrature formula 423 | 424 | The algorithm generates the N-point Gauss-Kronrod quadrature formula with 425 | weight function given by coefficients alpha and beta of a recurrence 426 | relation which generates a system of orthogonal polynomials: 427 | 428 | P-1(x) = 0 429 | P0(x) = 1 430 | Pn+1(x) = (x-alpha(n))*Pn(x) - beta(n)*Pn-1(x) 431 | 432 | and zero moment Mu0 433 | 434 | Mu0 = integral(W(x)dx,a,b) 435 | 436 | 437 | INPUT PARAMETERS: 438 | Alpha - alpha coefficients, array[0..floor(3*K/2)]. 439 | Beta - beta coefficients, array[0..ceil(3*K/2)]. 440 | Beta[0] is not used and may be arbitrary. 441 | Beta[I]>0. 442 | Mu0 - zeroth moment of the weight function. 443 | N - number of nodes of the Gauss-Kronrod quadrature formula, 444 | N >= 3, 445 | N = 2*K+1. 446 | 447 | OUTPUT PARAMETERS: 448 | Info - error code: 449 | * -5 no real and positive Gauss-Kronrod formula can 450 | be created for such a weight function with a 451 | given number of nodes. 452 | * -4 N is too large, task may be ill conditioned - 453 | x[i]=x[i+1] found. 454 | * -3 internal eigenproblem solver hasn't converged 455 | * -2 Beta[i]<=0 456 | * -1 incorrect N was passed 457 | * +1 OK 458 | X - array[0..N-1] - array of quadrature nodes, 459 | in ascending order. 460 | WKronrod - array[0..N-1] - Kronrod weights 461 | WGauss - array[0..N-1] - Gauss weights (interleaved with zeros 462 | corresponding to extended Kronrod nodes). 463 | 464 | -- ALGLIB -- 465 | Copyright 08.05.2009 by Bochkanov Sergey 466 | *************************************************************************/ 467 | void gkqgeneraterec(const real_1d_array &alpha, const real_1d_array &beta, const double mu0, const ae_int_t n, ae_int_t &info, real_1d_array &x, real_1d_array &wkronrod, real_1d_array &wgauss, const xparams _xparams = alglib::xdefault); 468 | 469 | 470 | /************************************************************************* 471 | Returns Gauss and Gauss-Kronrod nodes/weights for Gauss-Legendre 472 | quadrature with N points. 473 | 474 | GKQLegendreCalc (calculation) or GKQLegendreTbl (precomputed table) is 475 | used depending on machine precision and number of nodes. 476 | 477 | INPUT PARAMETERS: 478 | N - number of Kronrod nodes, must be odd number, >=3. 479 | 480 | OUTPUT PARAMETERS: 481 | Info - error code: 482 | * -4 an error was detected when calculating 483 | weights/nodes. N is too large to obtain 484 | weights/nodes with high enough accuracy. 485 | Try to use multiple precision version. 486 | * -3 internal eigenproblem solver hasn't converged 487 | * -1 incorrect N was passed 488 | * +1 OK 489 | X - array[0..N-1] - array of quadrature nodes, ordered in 490 | ascending order. 491 | WKronrod - array[0..N-1] - Kronrod weights 492 | WGauss - array[0..N-1] - Gauss weights (interleaved with zeros 493 | corresponding to extended Kronrod nodes). 494 | 495 | 496 | -- ALGLIB -- 497 | Copyright 12.05.2009 by Bochkanov Sergey 498 | *************************************************************************/ 499 | void gkqgenerategausslegendre(const ae_int_t n, ae_int_t &info, real_1d_array &x, real_1d_array &wkronrod, real_1d_array &wgauss, const xparams _xparams = alglib::xdefault); 500 | 501 | 502 | /************************************************************************* 503 | Returns Gauss and Gauss-Kronrod nodes/weights for Gauss-Jacobi 504 | quadrature on [-1,1] with weight function 505 | 506 | W(x)=Power(1-x,Alpha)*Power(1+x,Beta). 507 | 508 | INPUT PARAMETERS: 509 | N - number of Kronrod nodes, must be odd number, >=3. 510 | Alpha - power-law coefficient, Alpha>-1 511 | Beta - power-law coefficient, Beta>-1 512 | 513 | OUTPUT PARAMETERS: 514 | Info - error code: 515 | * -5 no real and positive Gauss-Kronrod formula can 516 | be created for such a weight function with a 517 | given number of nodes. 518 | * -4 an error was detected when calculating 519 | weights/nodes. Alpha or Beta are too close 520 | to -1 to obtain weights/nodes with high enough 521 | accuracy, or, may be, N is too large. Try to 522 | use multiple precision version. 523 | * -3 internal eigenproblem solver hasn't converged 524 | * -1 incorrect N was passed 525 | * +1 OK 526 | * +2 OK, but quadrature rule have exterior nodes, 527 | x[0]<-1 or x[n-1]>+1 528 | X - array[0..N-1] - array of quadrature nodes, ordered in 529 | ascending order. 530 | WKronrod - array[0..N-1] - Kronrod weights 531 | WGauss - array[0..N-1] - Gauss weights (interleaved with zeros 532 | corresponding to extended Kronrod nodes). 533 | 534 | 535 | -- ALGLIB -- 536 | Copyright 12.05.2009 by Bochkanov Sergey 537 | *************************************************************************/ 538 | void gkqgenerategaussjacobi(const ae_int_t n, const double alpha, const double beta, ae_int_t &info, real_1d_array &x, real_1d_array &wkronrod, real_1d_array &wgauss, const xparams _xparams = alglib::xdefault); 539 | 540 | 541 | /************************************************************************* 542 | Returns Gauss and Gauss-Kronrod nodes for quadrature with N points. 543 | 544 | Reduction to tridiagonal eigenproblem is used. 545 | 546 | INPUT PARAMETERS: 547 | N - number of Kronrod nodes, must be odd number, >=3. 548 | 549 | OUTPUT PARAMETERS: 550 | Info - error code: 551 | * -4 an error was detected when calculating 552 | weights/nodes. N is too large to obtain 553 | weights/nodes with high enough accuracy. 554 | Try to use multiple precision version. 555 | * -3 internal eigenproblem solver hasn't converged 556 | * -1 incorrect N was passed 557 | * +1 OK 558 | X - array[0..N-1] - array of quadrature nodes, ordered in 559 | ascending order. 560 | WKronrod - array[0..N-1] - Kronrod weights 561 | WGauss - array[0..N-1] - Gauss weights (interleaved with zeros 562 | corresponding to extended Kronrod nodes). 563 | 564 | -- ALGLIB -- 565 | Copyright 12.05.2009 by Bochkanov Sergey 566 | *************************************************************************/ 567 | void gkqlegendrecalc(const ae_int_t n, ae_int_t &info, real_1d_array &x, real_1d_array &wkronrod, real_1d_array &wgauss, const xparams _xparams = alglib::xdefault); 568 | 569 | 570 | /************************************************************************* 571 | Returns Gauss and Gauss-Kronrod nodes for quadrature with N points using 572 | pre-calculated table. Nodes/weights were computed with accuracy up to 573 | 1.0E-32 (if MPFR version of ALGLIB is used). In standard double precision 574 | accuracy reduces to something about 2.0E-16 (depending on your compiler's 575 | handling of long floating point constants). 576 | 577 | INPUT PARAMETERS: 578 | N - number of Kronrod nodes. 579 | N can be 15, 21, 31, 41, 51, 61. 580 | 581 | OUTPUT PARAMETERS: 582 | X - array[0..N-1] - array of quadrature nodes, ordered in 583 | ascending order. 584 | WKronrod - array[0..N-1] - Kronrod weights 585 | WGauss - array[0..N-1] - Gauss weights (interleaved with zeros 586 | corresponding to extended Kronrod nodes). 587 | 588 | 589 | -- ALGLIB -- 590 | Copyright 12.05.2009 by Bochkanov Sergey 591 | *************************************************************************/ 592 | void gkqlegendretbl(const ae_int_t n, real_1d_array &x, real_1d_array &wkronrod, real_1d_array &wgauss, double &eps, const xparams _xparams = alglib::xdefault); 593 | #endif 594 | 595 | #if defined(AE_COMPILE_AUTOGK) || !defined(AE_PARTIAL_BUILD) 596 | /************************************************************************* 597 | Integration of a smooth function F(x) on a finite interval [a,b]. 598 | 599 | Fast-convergent algorithm based on a Gauss-Kronrod formula is used. Result 600 | is calculated with accuracy close to the machine precision. 601 | 602 | Algorithm works well only with smooth integrands. It may be used with 603 | continuous non-smooth integrands, but with less performance. 604 | 605 | It should never be used with integrands which have integrable singularities 606 | at lower or upper limits - algorithm may crash. Use AutoGKSingular in such 607 | cases. 608 | 609 | INPUT PARAMETERS: 610 | A, B - interval boundaries (AB) 611 | 612 | OUTPUT PARAMETERS 613 | State - structure which stores algorithm state 614 | 615 | SEE ALSO 616 | AutoGKSmoothW, AutoGKSingular, AutoGKResults. 617 | 618 | 619 | -- ALGLIB -- 620 | Copyright 06.05.2009 by Bochkanov Sergey 621 | *************************************************************************/ 622 | void autogksmooth(const double a, const double b, autogkstate &state, const xparams _xparams = alglib::xdefault); 623 | 624 | 625 | /************************************************************************* 626 | Integration of a smooth function F(x) on a finite interval [a,b]. 627 | 628 | This subroutine is same as AutoGKSmooth(), but it guarantees that interval 629 | [a,b] is partitioned into subintervals which have width at most XWidth. 630 | 631 | Subroutine can be used when integrating nearly-constant function with 632 | narrow "bumps" (about XWidth wide). If "bumps" are too narrow, AutoGKSmooth 633 | subroutine can overlook them. 634 | 635 | INPUT PARAMETERS: 636 | A, B - interval boundaries (AB) 637 | 638 | OUTPUT PARAMETERS 639 | State - structure which stores algorithm state 640 | 641 | SEE ALSO 642 | AutoGKSmooth, AutoGKSingular, AutoGKResults. 643 | 644 | 645 | -- ALGLIB -- 646 | Copyright 06.05.2009 by Bochkanov Sergey 647 | *************************************************************************/ 648 | void autogksmoothw(const double a, const double b, const double xwidth, autogkstate &state, const xparams _xparams = alglib::xdefault); 649 | 650 | 651 | /************************************************************************* 652 | Integration on a finite interval [A,B]. 653 | Integrand have integrable singularities at A/B. 654 | 655 | F(X) must diverge as "(x-A)^alpha" at A, as "(B-x)^beta" at B, with known 656 | alpha/beta (alpha>-1, beta>-1). If alpha/beta are not known, estimates 657 | from below can be used (but these estimates should be greater than -1 too). 658 | 659 | One of alpha/beta variables (or even both alpha/beta) may be equal to 0, 660 | which means than function F(x) is non-singular at A/B. Anyway (singular at 661 | bounds or not), function F(x) is supposed to be continuous on (A,B). 662 | 663 | Fast-convergent algorithm based on a Gauss-Kronrod formula is used. Result 664 | is calculated with accuracy close to the machine precision. 665 | 666 | INPUT PARAMETERS: 667 | A, B - interval boundaries (AB) 668 | Alpha - power-law coefficient of the F(x) at A, 669 | Alpha>-1 670 | Beta - power-law coefficient of the F(x) at B, 671 | Beta>-1 672 | 673 | OUTPUT PARAMETERS 674 | State - structure which stores algorithm state 675 | 676 | SEE ALSO 677 | AutoGKSmooth, AutoGKSmoothW, AutoGKResults. 678 | 679 | 680 | -- ALGLIB -- 681 | Copyright 06.05.2009 by Bochkanov Sergey 682 | *************************************************************************/ 683 | void autogksingular(const double a, const double b, const double alpha, const double beta, autogkstate &state, const xparams _xparams = alglib::xdefault); 684 | 685 | 686 | /************************************************************************* 687 | This function provides reverse communication interface 688 | Reverse communication interface is not documented or recommended to use. 689 | See below for functions which provide better documented API 690 | *************************************************************************/ 691 | bool autogkiteration(const autogkstate &state, const xparams _xparams = alglib::xdefault); 692 | 693 | 694 | /************************************************************************* 695 | This function is used to launcn iterations of the 1-dimensional integrator 696 | 697 | It accepts following parameters: 698 | func - callback which calculates f(x) for given x 699 | ptr - optional pointer which is passed to func; can be NULL 700 | 701 | 702 | -- ALGLIB -- 703 | Copyright 07.05.2009 by Bochkanov Sergey 704 | 705 | *************************************************************************/ 706 | void autogkintegrate(autogkstate &state, 707 | void (*func)(double x, double xminusa, double bminusx, double &y, void *ptr), 708 | void *ptr = NULL, const xparams _xparams = alglib::xdefault); 709 | 710 | 711 | /************************************************************************* 712 | Adaptive integration results 713 | 714 | Called after AutoGKIteration returned False. 715 | 716 | Input parameters: 717 | State - algorithm state (used by AutoGKIteration). 718 | 719 | Output parameters: 720 | V - integral(f(x)dx,a,b) 721 | Rep - optimization report (see AutoGKReport description) 722 | 723 | -- ALGLIB -- 724 | Copyright 14.11.2007 by Bochkanov Sergey 725 | *************************************************************************/ 726 | void autogkresults(const autogkstate &state, double &v, autogkreport &rep, const xparams _xparams = alglib::xdefault); 727 | #endif 728 | } 729 | 730 | ///////////////////////////////////////////////////////////////////////// 731 | // 732 | // THIS SECTION CONTAINS COMPUTATIONAL CORE DECLARATIONS (FUNCTIONS) 733 | // 734 | ///////////////////////////////////////////////////////////////////////// 735 | namespace alglib_impl 736 | { 737 | #if defined(AE_COMPILE_GQ) || !defined(AE_PARTIAL_BUILD) 738 | void gqgeneraterec(/* Real */ ae_vector* alpha, 739 | /* Real */ ae_vector* beta, 740 | double mu0, 741 | ae_int_t n, 742 | ae_int_t* info, 743 | /* Real */ ae_vector* x, 744 | /* Real */ ae_vector* w, 745 | ae_state *_state); 746 | void gqgenerategausslobattorec(/* Real */ ae_vector* alpha, 747 | /* Real */ ae_vector* beta, 748 | double mu0, 749 | double a, 750 | double b, 751 | ae_int_t n, 752 | ae_int_t* info, 753 | /* Real */ ae_vector* x, 754 | /* Real */ ae_vector* w, 755 | ae_state *_state); 756 | void gqgenerategaussradaurec(/* Real */ ae_vector* alpha, 757 | /* Real */ ae_vector* beta, 758 | double mu0, 759 | double a, 760 | ae_int_t n, 761 | ae_int_t* info, 762 | /* Real */ ae_vector* x, 763 | /* Real */ ae_vector* w, 764 | ae_state *_state); 765 | void gqgenerategausslegendre(ae_int_t n, 766 | ae_int_t* info, 767 | /* Real */ ae_vector* x, 768 | /* Real */ ae_vector* w, 769 | ae_state *_state); 770 | void gqgenerategaussjacobi(ae_int_t n, 771 | double alpha, 772 | double beta, 773 | ae_int_t* info, 774 | /* Real */ ae_vector* x, 775 | /* Real */ ae_vector* w, 776 | ae_state *_state); 777 | void gqgenerategausslaguerre(ae_int_t n, 778 | double alpha, 779 | ae_int_t* info, 780 | /* Real */ ae_vector* x, 781 | /* Real */ ae_vector* w, 782 | ae_state *_state); 783 | void gqgenerategausshermite(ae_int_t n, 784 | ae_int_t* info, 785 | /* Real */ ae_vector* x, 786 | /* Real */ ae_vector* w, 787 | ae_state *_state); 788 | #endif 789 | #if defined(AE_COMPILE_GKQ) || !defined(AE_PARTIAL_BUILD) 790 | void gkqgeneraterec(/* Real */ ae_vector* alpha, 791 | /* Real */ ae_vector* beta, 792 | double mu0, 793 | ae_int_t n, 794 | ae_int_t* info, 795 | /* Real */ ae_vector* x, 796 | /* Real */ ae_vector* wkronrod, 797 | /* Real */ ae_vector* wgauss, 798 | ae_state *_state); 799 | void gkqgenerategausslegendre(ae_int_t n, 800 | ae_int_t* info, 801 | /* Real */ ae_vector* x, 802 | /* Real */ ae_vector* wkronrod, 803 | /* Real */ ae_vector* wgauss, 804 | ae_state *_state); 805 | void gkqgenerategaussjacobi(ae_int_t n, 806 | double alpha, 807 | double beta, 808 | ae_int_t* info, 809 | /* Real */ ae_vector* x, 810 | /* Real */ ae_vector* wkronrod, 811 | /* Real */ ae_vector* wgauss, 812 | ae_state *_state); 813 | void gkqlegendrecalc(ae_int_t n, 814 | ae_int_t* info, 815 | /* Real */ ae_vector* x, 816 | /* Real */ ae_vector* wkronrod, 817 | /* Real */ ae_vector* wgauss, 818 | ae_state *_state); 819 | void gkqlegendretbl(ae_int_t n, 820 | /* Real */ ae_vector* x, 821 | /* Real */ ae_vector* wkronrod, 822 | /* Real */ ae_vector* wgauss, 823 | double* eps, 824 | ae_state *_state); 825 | #endif 826 | #if defined(AE_COMPILE_AUTOGK) || !defined(AE_PARTIAL_BUILD) 827 | void autogksmooth(double a, 828 | double b, 829 | autogkstate* state, 830 | ae_state *_state); 831 | void autogksmoothw(double a, 832 | double b, 833 | double xwidth, 834 | autogkstate* state, 835 | ae_state *_state); 836 | void autogksingular(double a, 837 | double b, 838 | double alpha, 839 | double beta, 840 | autogkstate* state, 841 | ae_state *_state); 842 | ae_bool autogkiteration(autogkstate* state, ae_state *_state); 843 | void autogkresults(autogkstate* state, 844 | double* v, 845 | autogkreport* rep, 846 | ae_state *_state); 847 | void _autogkreport_init(void* _p, ae_state *_state, ae_bool make_automatic); 848 | void _autogkreport_init_copy(void* _dst, void* _src, ae_state *_state, ae_bool make_automatic); 849 | void _autogkreport_clear(void* _p); 850 | void _autogkreport_destroy(void* _p); 851 | void _autogkinternalstate_init(void* _p, ae_state *_state, ae_bool make_automatic); 852 | void _autogkinternalstate_init_copy(void* _dst, void* _src, ae_state *_state, ae_bool make_automatic); 853 | void _autogkinternalstate_clear(void* _p); 854 | void _autogkinternalstate_destroy(void* _p); 855 | void _autogkstate_init(void* _p, ae_state *_state, ae_bool make_automatic); 856 | void _autogkstate_init_copy(void* _dst, void* _src, ae_state *_state, ae_bool make_automatic); 857 | void _autogkstate_clear(void* _p); 858 | void _autogkstate_destroy(void* _p); 859 | #endif 860 | 861 | } 862 | #endif 863 | 864 | --------------------------------------------------------------------------------