├── .editorconfig ├── .github └── workflows │ └── main.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── package.xml ├── plugins.xml ├── rosdoc.yaml └── src ├── kdtree.cpp ├── kdtree.h ├── laser_model.cpp ├── laser_model.h ├── localization_plugin.cpp ├── localization_plugin.h ├── localization_plugin_old.cpp ├── localization_tf_plugin.cpp ├── localization_tf_plugin.h ├── odom_model.cpp ├── odom_model.h ├── particle_filter.cpp └── particle_filter.h /.editorconfig: -------------------------------------------------------------------------------- 1 | root = true 2 | 3 | [*] 4 | end_of_line = lf 5 | charset = utf-8 6 | trim_trailing_whitespace = true 7 | insert_final_newline = true 8 | 9 | indent_style = space 10 | indent_size = 4 11 | 12 | [{**/*.json,**/*.yaml,**/*.yml}] 13 | indent_size = 2 14 | 15 | [**/*.md] 16 | trim_trailing_whitespace = false 17 | 18 | [**/*.xml] 19 | indent_size = 2 20 | 21 | [{CMakeLists.txt,package.xml}] 22 | indent_size = 2 23 | -------------------------------------------------------------------------------- /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | tue-ci: 7 | name: TUe CI - ${{ github.event_name }} 8 | runs-on: ubuntu-latest 9 | steps: 10 | - name: TUe CI 11 | uses: tue-robotics/tue-env/ci/main@master 12 | with: 13 | package: ${{ github.event.repository.name }} 14 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeLists.txt.user 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ed_localization) 3 | 4 | add_compile_options(-Wall -Werror=all) 5 | add_compile_options(-Wextra -Werror=extra) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | ed 9 | geometry_msgs 10 | sensor_msgs 11 | tf2 12 | tf2_geometry_msgs 13 | tf2_ros 14 | ) 15 | 16 | catkin_package( 17 | INCLUDE_DIRS 18 | LIBRARIES ed_localization_pf ed_localization_odom ed_localization_laser ed_localization_plugin ed_localization_tf_plugin 19 | CATKIN_DEPENDS ed geometry_msgs sensor_msgs tf2 tf2_geometry_msgs 20 | DEPENDS 21 | ) 22 | 23 | include_directories( 24 | SYSTEM 25 | ${catkin_INCLUDE_DIRS} 26 | ) 27 | 28 | add_library(ed_localization_pf 29 | src/kdtree.cpp 30 | src/kdtree.h 31 | src/particle_filter.cpp 32 | src/particle_filter.h 33 | ) 34 | 35 | add_library(ed_localization_odom 36 | src/odom_model.cpp 37 | src/odom_model.h 38 | ) 39 | target_link_libraries(ed_localization_odom ed_localization_pf) 40 | 41 | add_library(ed_localization_laser 42 | src/laser_model.cpp 43 | src/laser_model.h 44 | ) 45 | target_link_libraries(ed_localization_laser ed_localization_pf) 46 | 47 | add_library(ed_localization_plugin 48 | src/localization_plugin.cpp 49 | src/localization_plugin.h 50 | ) 51 | target_link_libraries(ed_localization_plugin ed_localization_pf ed_localization_odom ed_localization_laser ${catkin_LIBRARIES}) 52 | add_dependencies(ed_localization_plugin ${catkin_EXPORTED_TARGETS}) 53 | 54 | add_library(ed_localization_tf_plugin 55 | src/localization_tf_plugin.cpp 56 | src/localization_tf_plugin.h 57 | ) 58 | target_link_libraries(ed_localization_tf_plugin ${catkin_LIBRARIES}) 59 | add_dependencies(ed_localization_tf_plugin ${catkin_EXPORTED_TARGETS}) 60 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2021, Eindhoven University of Technology - CST Robotics Group 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ED Localization 2 | 3 | [![CI](https://github.com/tue-robotics/ed_localization/actions/workflows/main.yml/badge.svg)](https://github.com/tue-robotics/ed_localization/actions/workflows/main.yml) 4 | 5 | A fast particle filter implementation and sensor models for localizing a robot which always take into account the most recent state of the world. This means that if the world representation improves while the robot is running, localization becomes better. The localization module is more efficient and accurate than the well-known [AMCL-module](http://wiki.ros.org/amcl) and *no* separate occupancy grid is needed. 6 | 7 | ## Installation 8 | 9 | Requirements: 10 | 11 | * ED () 12 | * A 2D Range Finder () which scans in a plane parallel to the floor 13 | * A [TF](wiki.ros.org/tf) containing transforms from the robots' odometry frame to the laser range finder frame 14 | 15 | Check out the following packages in your workspace: 16 | 17 | cd /src 18 | git clone https://github.com/tue-robotics/ed_localization.git 19 | 20 | And compile 21 | 22 | cd : 23 | catkin build 24 | 25 | ## Tutorial 26 | 27 | All ED tutorials can be found in the ed_tutorials package: 28 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ed_localization 7 | 0.0.0 8 | No description available 9 | 10 | Sjoerd van den Dries 11 | 12 | Matthijs van der Burgh 13 | 14 | BSD-2-Clause 15 | 16 | catkin 17 | 18 | ed 19 | geometry_msgs 20 | sensor_msgs 21 | tf2 22 | tf2_geometry_msgs 23 | tf2_ros 24 | 25 | ed 26 | geometry_msgs 27 | sensor_msgs 28 | tf2 29 | tf2_geometry_msgs 30 | tf2_ros 31 | 32 | doxygen 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Plugin to localize the robot with th use of laser scans and AMCL. 6 | 7 | 8 | 9 | 10 | Plugin set robot entity pose based on TF. 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | name: C++ API 3 | file_patterns: '*.cpp *.h' 4 | tab_size: 4 5 | use_mdfile_as_mainpage: README.md 6 | -------------------------------------------------------------------------------- /src/kdtree.cpp: -------------------------------------------------------------------------------- 1 | #include "kdtree.h" 2 | 3 | #include 4 | 5 | // ---------------------------------------------------------------------------------------------------- 6 | 7 | 8 | KDTree::KDTree(unsigned int initial_size, const std::array& cell_size) : res_(cell_size), root_(nullptr), node_count_(0), nodes_(initial_size), leaf_count_(0) 9 | { 10 | } 11 | 12 | // ---------------------------------------------------------------------------------------------------- 13 | 14 | KDTree::~KDTree() 15 | { 16 | for (auto ptr : nodes_) 17 | delete ptr; 18 | } 19 | 20 | // ---------------------------------------------------------------------------------------------------- 21 | 22 | void KDTree::clear() 23 | { 24 | root_ = nullptr; 25 | leaf_count_ = 0; 26 | node_count_ = 0; 27 | } 28 | 29 | // ---------------------------------------------------------------------------------------------------- 30 | 31 | void KDTree::insert(const geo::Transform2& pose, double value) 32 | { 33 | root_ = insertNode(nullptr, root_, generateKey(pose), value); 34 | } 35 | 36 | // ---------------------------------------------------------------------------------------------------- 37 | 38 | void KDTree::cluster() 39 | { 40 | unsigned int queue_count = 0; 41 | unsigned int cluster_count = 0; 42 | 43 | std::vector queue(node_count_); 44 | 45 | // Put all the leaves in a queue 46 | for (uint i=0; ileaf) 50 | { 51 | node->cluster = -1; 52 | queue[queue_count++] = node; 53 | } 54 | } 55 | 56 | for (uint i=queue_count; i>0; --i) 57 | { 58 | KDTreeNode* node = queue[i-1]; 59 | 60 | // If this node has already been labelled, skip it 61 | if (node->cluster >= 0) 62 | continue; 63 | 64 | // Assign a label to this cluster 65 | node->cluster = cluster_count++; 66 | 67 | // Recursively label nodes in this cluster 68 | clusterNode(node, 0); 69 | } 70 | } 71 | 72 | // ---------------------------------------------------------------------------------------------------- 73 | 74 | int KDTree::getCluster(const geo::Transform2& pose) 75 | { 76 | KDTreeNode* node = findNode(root_, generateKey(pose)); 77 | if (!node) 78 | return -1; 79 | return node->cluster; 80 | } 81 | 82 | // ---------------------------------------------------------------------------------------------------- 83 | 84 | double KDTree::getValue(const geo::Transform2& pose) 85 | { 86 | KDTreeNode* node = findNode(root_, generateKey(pose)); 87 | if (!node) 88 | return 0; 89 | return node->value; 90 | } 91 | 92 | // ---------------------------------------------------------------------------------------------------- 93 | 94 | std::array KDTree::generateKey(const geo::Transform2 &pose) 95 | { 96 | std::array key; 97 | key[0] = std::floor(pose.t.x / res_[0]); 98 | key[1] = std::floor(pose.t.y / res_[1]); 99 | key[2] = std::floor(pose.rotation() / res_[2]); 100 | 101 | return key; 102 | } 103 | 104 | // ---------------------------------------------------------------------------------------------------- 105 | 106 | bool KDTree::equal(const std::array& key_a, const std::array& key_b) 107 | { 108 | if (key_a[0] != key_b[0]) 109 | return false; 110 | if (key_a[1] != key_b[1]) 111 | return false; 112 | if (key_a[2] != key_b[2]) 113 | return false; 114 | 115 | return true; 116 | } 117 | 118 | // ---------------------------------------------------------------------------------------------------- 119 | 120 | KDTreeNode* KDTree::insertNode(const KDTreeNode* parent, KDTreeNode* node, const std::array& key, double value) 121 | { 122 | // If the node doesnt exist yet 123 | if (!node) 124 | { 125 | // Resize if end is reached 126 | if (node_count_ >= nodes_.size()) 127 | nodes_.resize(node_count_ + 10); 128 | 129 | 130 | if (nodes_[node_count_]) 131 | nodes_[node_count_] = new(nodes_[node_count_]) KDTreeNode; 132 | else 133 | nodes_[node_count_] = new KDTreeNode; 134 | node = nodes_[node_count_++]; 135 | node->leaf = true; 136 | 137 | if (!parent) 138 | node->depth = 0; 139 | else 140 | node->depth = parent->depth + 1; 141 | 142 | node->key = key; 143 | node->value = value; 144 | leaf_count_ += 1; 145 | } 146 | 147 | // If the node exists, and it is a leaf node 148 | else if (node->leaf) 149 | { 150 | // If the keys are equal, increment the value 151 | if (equal(key, node->key)) 152 | { 153 | node->value += value; 154 | } 155 | 156 | // The keys are not equal, so split this node 157 | else 158 | { 159 | // Pivot dimension should be x, y, th, x, y, th, etc. 160 | node->pivot_dim = node->depth % 3; 161 | 162 | node->pivot_value = (key[node->pivot_dim] + node->key[node->pivot_dim]) / 2.; 163 | 164 | if (key[node->pivot_dim] < node->pivot_value) 165 | { 166 | node->children[0] = insertNode(node, nullptr, key, value); 167 | node->children[1] = insertNode(node, nullptr, node->key, node->value); 168 | } 169 | else 170 | { 171 | node->children[0] = insertNode(node, nullptr, node->key, node->value); 172 | node->children[1] = insertNode(node, nullptr, key, value); 173 | } 174 | 175 | node->leaf = 0; 176 | leaf_count_ -= 1; 177 | } 178 | } 179 | 180 | // If the node exists and it has children 181 | else 182 | { 183 | if (key[node->pivot_dim] < node->pivot_value) 184 | insertNode(node, node->children[0], key, value); 185 | else 186 | insertNode(node, node->children[1], key, value); 187 | } 188 | 189 | return node; 190 | } 191 | 192 | // ---------------------------------------------------------------------------------------------------- 193 | 194 | KDTreeNode* KDTree::findNode(KDTreeNode* node, const std::array& key) 195 | { 196 | if (node->leaf) 197 | { 198 | // If the keys are the same 199 | if (equal(key, node->key)) 200 | return node; 201 | else 202 | return nullptr; 203 | } 204 | else 205 | { 206 | // If the keys are different 207 | if (key[node->pivot_dim] < node->pivot_value) 208 | return findNode(node->children[0], key); 209 | else 210 | return findNode(node->children[1], key); 211 | } 212 | } 213 | 214 | // ---------------------------------------------------------------------------------------------------- 215 | 216 | void KDTree::clusterNode(const KDTreeNode* node, int depth) 217 | { 218 | std::array nkey; 219 | KDTreeNode* nnode; 220 | for (uint i=0; i<27; ++i) // all surrounding bins, including yourself 221 | { 222 | nkey[0] = node->key[0] + (i / 9) - 1; 223 | nkey[1] = node->key[1] + ((i % 9) / 3) - 1; 224 | nkey[2] = node->key[2] + ((i % 9) % 3) - 1; 225 | 226 | nnode = findNode(root_, nkey); 227 | if (!nnode) 228 | continue; 229 | 230 | // This node already has a label, skip it. 231 | if (nnode->cluster >= 0) 232 | continue; 233 | 234 | // Label this node and recurse 235 | nnode->cluster = node->cluster; 236 | clusterNode(nnode, depth + 1); 237 | } 238 | } 239 | -------------------------------------------------------------------------------- /src/kdtree.h: -------------------------------------------------------------------------------- 1 | #ifndef ED_LOCALIZATION_KDTREE_H_ 2 | #define ED_LOCALIZATION_KDTREE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | struct KDTreeNode 11 | { 12 | KDTreeNode() : depth(0), leaf(true), pivot_dim(0), pivot_value(0), key({0,0,0}), value(0), cluster(-1), children({nullptr, nullptr}) 13 | { 14 | } 15 | 16 | // Depth in the tree 17 | unsigned int depth; 18 | 19 | // Leaf node 20 | bool leaf; 21 | 22 | // Pivot dimension and value 23 | unsigned int pivot_dim; 24 | double pivot_value; 25 | 26 | // The key for this node 27 | std::array key; 28 | 29 | // The value for this node, summed weights in this bin 30 | double value; 31 | 32 | // The cluster label (leaf nodes) 33 | int cluster; 34 | 35 | // Child nodes 36 | std::array children; 37 | }; 38 | 39 | class KDTree 40 | { 41 | 42 | public: 43 | 44 | KDTree(unsigned int initial_size, const std::array& cell_size); 45 | 46 | ~KDTree(); 47 | 48 | /** 49 | * @brief Clear the tree 50 | */ 51 | void clear(); 52 | 53 | /** 54 | * @brief Insert #pose into tree and set value 55 | * @param pose pose to add 56 | * @param value value to add to the new node 57 | */ 58 | void insert(const geo::Transform2& pose, double value); 59 | 60 | /** 61 | * @brief Label the current nodes by cluster 62 | */ 63 | void cluster(); 64 | 65 | /** 66 | * @brief Get the cluster corresponding to a pose 67 | * @param pose Pose to find the cluster for 68 | * @return cluster number, -1 if pose is not in the tree 69 | */ 70 | int getCluster(const geo::Transform2& pose); 71 | 72 | /** 73 | * @brief Get the value corresponding to a pose 74 | * @param pose Pose to find the value for 75 | * @return cluster number, 0 if pose is not in the tree 76 | */ 77 | double getValue(const geo::Transform2& pose); 78 | 79 | /** 80 | * @brief Get the current number of leaves 81 | * @return number of leaves 82 | */ 83 | unsigned int getLeafCount() { return leaf_count_; } 84 | 85 | private: 86 | 87 | /** 88 | * @brief Generate the key from a pose taking into account the #cell_size 89 | * @param pose pose to convert to a key 90 | * @return generated key 91 | */ 92 | std::array generateKey(const geo::Transform2& pose); 93 | 94 | /** 95 | * @brief Compare keys to see if they are equal 96 | * @param key_a 97 | * @param key_b 98 | * @return 99 | */ 100 | bool equal(const std::array& key_a, const std::array& key_b); 101 | 102 | /** 103 | * @brief Insert a node into the tree 104 | * @param parent parent node 105 | * @param node current node 106 | * @param key key to add 107 | * @param value value corresponding to the key 108 | * @return Pointer to the inserted node 109 | */ 110 | KDTreeNode* insertNode(const KDTreeNode* parent, KDTreeNode* node, const std::array& key, double value); 111 | 112 | /** 113 | * @brief Recursive node search, checks if #node corresponds to #key, otherwise search in its children 114 | * @param node node to check 115 | * @param key key to get corresponding node 116 | * @return Pointer to the found node, nullptr if not found 117 | */ 118 | KDTreeNode* findNode(KDTreeNode* node, const std::array& key); 119 | 120 | /** 121 | * @brief Recursively label nodes in this cluster 122 | * @param node node to label including its children 123 | * @param depth not used 124 | */ 125 | void clusterNode(const KDTreeNode* node, int depth); 126 | 127 | // Cell size 128 | std::array res_; 129 | 130 | // The root node of the tree 131 | KDTreeNode* root_; 132 | 133 | // The number of nodes in the tree 134 | unsigned int node_count_; 135 | std::vector nodes_; 136 | 137 | // The number of leaf nodes in the tree 138 | unsigned int leaf_count_; 139 | }; 140 | 141 | #endif // ED_LOCALIZATION_KDTREE_H_ 142 | -------------------------------------------------------------------------------- /src/laser_model.cpp: -------------------------------------------------------------------------------- 1 | #include "laser_model.h" 2 | 3 | #include "particle_filter.h" 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | // ---------------------------------------------------------------------------------------------------- 11 | 12 | class LineRenderResult : public geo::LaserRangeFinder::RenderResult 13 | { 14 | 15 | public: 16 | 17 | LineRenderResult(std::vector& lines_start, std::vector& lines_end, double max_distance) 18 | : geo::LaserRangeFinder::RenderResult(dummy_ranges_), 19 | lines_start_(lines_start), lines_end_(lines_end), max_distance_sq_(max_distance * max_distance) {} 20 | 21 | void renderLine(const geo::Vec2& p1, const geo::Vec2& p2) 22 | { 23 | // Calculate distance to the line 24 | 25 | geo::Vec2 diff = p2 - p1; 26 | double line_length_sq = diff.length2(); 27 | 28 | double t = p1.dot(diff) / -line_length_sq; 29 | 30 | double distance_sq; 31 | 32 | if (t < 0) 33 | distance_sq = p1.length2(); 34 | else if (t > 1) 35 | distance_sq = p2.length2(); 36 | else 37 | distance_sq = (p1 + t * diff).length2(); 38 | 39 | // If too far, skip 40 | if (distance_sq > max_distance_sq_) 41 | return; 42 | 43 | lines_start_.push_back(p1); 44 | lines_end_.push_back(p2); 45 | } 46 | 47 | private: 48 | 49 | std::vector dummy_ranges_; 50 | std::vector& lines_start_; 51 | std::vector& lines_end_; 52 | double max_distance_sq_; 53 | 54 | }; 55 | 56 | // ---------------------------------------------------------------------------------------------------- 57 | 58 | LaserModel::LaserModel() 59 | { 60 | // DEFAULT: 61 | z_hit = 0.95; 62 | sigma_hit = 0.2; 63 | z_short = 0.1; 64 | z_max = 0.05; 65 | z_rand = 0.05; 66 | lambda_short = 0.1; 67 | range_max = 10; // m 68 | 69 | laser_height_ = 0.3; 70 | laser_offset_ = geo::Transform2(0.3, 0, 0); 71 | } 72 | 73 | // ---------------------------------------------------------------------------------------------------- 74 | 75 | LaserModel::~LaserModel() 76 | { 77 | } 78 | 79 | // ---------------------------------------------------------------------------------------------------- 80 | 81 | void LaserModel::configure(tue::Configuration config) 82 | { 83 | int temp_num_beams = 0; 84 | config.value("num_beams", temp_num_beams); 85 | num_beams = temp_num_beams; 86 | 87 | config.value("z_hit", z_hit); 88 | config.value("sigma_hit", sigma_hit); 89 | config.value("z_short", z_short); 90 | config.value("z_max", z_max); 91 | config.value("z_rand", z_rand); 92 | config.value("lambda_short", lambda_short); 93 | config.value("range_max", range_max); 94 | config.value("min_particle_distance", min_particle_distance_); 95 | config.value("min_particle_rotation_distance", min_particle_rotation_distance_); 96 | 97 | // Pre-calculate expensive operations 98 | int resolution = 1000; // mm accuracy 99 | 100 | exp_hit_.resize(range_max * resolution + 1); 101 | for(unsigned int i = 0; i < exp_hit_.size(); ++i) 102 | { 103 | double z = static_cast(i) / resolution; 104 | exp_hit_[i] = exp(-(z * z) / (2 * this->sigma_hit * this->sigma_hit)); 105 | } 106 | 107 | exp_short_.resize(range_max * resolution + 1); 108 | for(unsigned int i = 0; i < exp_hit_.size(); ++i) 109 | { 110 | double obs_range = static_cast(i) / resolution; 111 | exp_short_[i] = exp(-this->lambda_short * obs_range); 112 | } 113 | } 114 | 115 | // ---------------------------------------------------------------------------------------------------- 116 | 117 | void LaserModel::updateWeights(const ed::WorldModel& world, const sensor_msgs::LaserScan& scan, ParticleFilter& pf) 118 | { 119 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 120 | // - Find unique samples 121 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 122 | 123 | // If N samples are nearly identical, we only want to calculate the probability update 124 | // once and share it for all N samples. Therefore, we create a sample list 'unique_samples' 125 | // that only contain samples that are further apart than a given threshold. We will only 126 | // calculate the probabilities of those samples, and share them with the similar samples. 127 | 128 | // unique samples 129 | std::vector unique_samples; 130 | 131 | // mapping of samples from the particle filter to the unique sample list 132 | std::vector sample_to_unique(pf.samples().size()); 133 | 134 | double min_particle_distance_sq = min_particle_distance_ * min_particle_distance_; 135 | 136 | for(unsigned int i = 0; i < pf.samples().size(); ++i) 137 | { 138 | const Sample& s1 = pf.samples()[i]; 139 | const geo::Transform2& t1 = s1.pose; 140 | 141 | bool found = false; 142 | for(unsigned int j = 0; j < unique_samples.size(); ++j) 143 | { 144 | const geo::Transform2& t2 = unique_samples[j]; 145 | 146 | // Calculate difference in rotation 147 | double rot_diff = std::abs(t1.rotation() - t2.rotation()); 148 | if (rot_diff > M_PI) 149 | rot_diff = 2 * M_PI - rot_diff; 150 | 151 | // Check if translation and rotational difference are within boundaries 152 | if ((t1.t - t2.t).length2() < min_particle_distance_sq && rot_diff < min_particle_rotation_distance_) 153 | { 154 | found = true; 155 | sample_to_unique[i] = j; 156 | break; 157 | } 158 | } 159 | 160 | if (!found) 161 | { 162 | sample_to_unique[i] = unique_samples.size(); 163 | unique_samples.push_back(t1); 164 | } 165 | } 166 | 167 | // If there is only one unique sample, it means are particles are (almost) identical, and the laser model 168 | // update is not neccesary. This typically holds if the robot is standing still. 169 | if (unique_samples.size() == 1) 170 | return; 171 | 172 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 173 | // - Update world renderer 174 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 175 | 176 | if (num_beams <= 0) 177 | num_beams = scan.ranges.size(); 178 | else 179 | num_beams = std::min(scan.ranges.size(), num_beams); 180 | 181 | uint i_step = scan.ranges.size() / num_beams; 182 | sensor_ranges_.clear(); 183 | for (unsigned int i = 0; i < scan.ranges.size(); i += i_step) 184 | { 185 | double r = scan.ranges[i]; 186 | 187 | // Check for Inf 188 | if (r != r || r > scan.range_max) 189 | r = 0; 190 | sensor_ranges_.push_back(r); 191 | } 192 | num_beams = sensor_ranges_.size(); 193 | 194 | if (lrf_.getNumBeams() != num_beams) 195 | { 196 | lrf_.setNumBeams(num_beams); 197 | lrf_.setAngleLimits(scan.angle_min, scan.angle_max); 198 | range_max = std::min(range_max, scan.range_max); 199 | } 200 | 201 | // If the laser is upside down, we need to mirror the sensor data 202 | if (laser_upside_down_) 203 | std::reverse(sensor_ranges_.begin(), sensor_ranges_.end()); 204 | 205 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 206 | // - Determine center and maximum range of world model cross section 207 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 208 | 209 | // Find the bounding rectangle around all sample poses. This will be used to determine 210 | // the largest render distance (anything max_range beyond the sample boundaries does 211 | // not have to be considered) 212 | 213 | geo::Vec2 sample_min(1e9, 1e9); 214 | geo::Vec2 sample_max(-1e9, -1e9); 215 | for(std::vector::iterator it = pf.samples().begin(); it != pf.samples().end(); ++it) 216 | { 217 | Sample& sample = *it; 218 | 219 | geo::Transform2 laser_pose = sample.pose * laser_offset_; 220 | 221 | sample_min.x = std::min(sample_min.x, laser_pose.t.x); 222 | sample_min.y = std::min(sample_min.y, laser_pose.t.y); 223 | sample_max.x = std::max(sample_min.x, laser_pose.t.x); 224 | sample_max.y = std::max(sample_min.y, laser_pose.t.y); 225 | } 226 | 227 | double temp_range_max = 0; 228 | for(unsigned int i = 0; i < sensor_ranges_.size(); ++i) 229 | { 230 | double r = sensor_ranges_[i]; 231 | if (r < range_max) 232 | temp_range_max = std::max(temp_range_max, r); 233 | } 234 | 235 | // Add a small buffer to the distance to allow model data that is 236 | // slightly further away to still match 237 | temp_range_max += lambda_short; 238 | 239 | // Calculate the sample boundary center and boundary render distance 240 | geo::Vec2 sample_center = (sample_min + sample_max) / 2; 241 | double max_distance = (sample_max - sample_min).length() / 2 + temp_range_max; 242 | 243 | // Set the range limit to the lrf renderer. This will make sure all shapes that 244 | // are too far away will not be rendered (object selection, not line selection) 245 | lrf_.setRangeLimits(scan.range_min, max_distance); 246 | 247 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 248 | // - Create world model cross section 249 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 250 | 251 | geo::Pose3D laser_pose(sample_center.x, sample_center.y, laser_height_); 252 | 253 | lines_start_.clear(); 254 | lines_end_.clear(); 255 | 256 | // Give the max_distance also to the line renderer. It will discard all LINES that 257 | // are further away. Note that this is an even finer selection than the default 258 | // object selection that is done by the lrf renderer. 259 | LineRenderResult render_result(lines_start_, lines_end_, max_distance); 260 | 261 | for(ed::WorldModel::const_iterator it = world.begin(); it != world.end(); ++it) 262 | { 263 | const ed::EntityConstPtr& e = *it; 264 | if (e->visual() && e->has_pose()) 265 | { 266 | // Do not render the robot itself (we're trying to localize it!) 267 | if (e->hasFlag("self")) 268 | continue; 269 | 270 | if (e->hasFlag("non-localizable")) 271 | continue; 272 | 273 | geo::LaserRangeFinder::RenderOptions options; 274 | geo::Transform t_inv = laser_pose.inverse() * e->pose(); 275 | options.setMesh(e->visual()->getMesh(), t_inv); 276 | lrf_.render(options, render_result); 277 | } 278 | } 279 | 280 | for(unsigned int i = 0; i < lines_start_.size(); ++i) 281 | { 282 | lines_start_[i] += sample_center; 283 | lines_end_[i] += sample_center; 284 | } 285 | 286 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 287 | // - Calculate sample weight updates 288 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 289 | 290 | lrf_.setRangeLimits(scan.range_min, temp_range_max); 291 | 292 | std::vector weight_updates(unique_samples.size()); 293 | for(unsigned int j = 0; j < unique_samples.size(); ++j) 294 | { 295 | geo::Transform2 laser_pose = unique_samples[j] * laser_offset_; 296 | geo::Transform2 pose_inv = laser_pose.inverse(); 297 | 298 | // Calculate sensor model for this pose 299 | std::vector model_ranges(sensor_ranges_.size(), 0); 300 | 301 | for(unsigned int i = 0; i < lines_start_.size(); ++i) 302 | { 303 | const geo::Vec2& p1 = lines_start_[i]; 304 | const geo::Vec2& p2 = lines_end_[i]; 305 | 306 | // Transform the points to the laser pose 307 | geo::Vec2 p1_t = pose_inv * p1; 308 | geo::Vec2 p2_t = pose_inv * p2; 309 | 310 | // Render the line as if seen by the sensor 311 | lrf_.renderLine(p1_t, p2_t, model_ranges); 312 | } 313 | 314 | double p = 1; 315 | 316 | for(unsigned int i = 0; i < sensor_ranges_.size(); ++i) 317 | { 318 | double obs_range = sensor_ranges_[i]; 319 | double map_range = model_ranges[i]; 320 | 321 | double z = obs_range - map_range; 322 | 323 | double pz = 0; 324 | 325 | // Part 1: good, but noisy, hit 326 | // pz += this->z_hit * exp(-(z * z) / (2 * this->sigma_hit * this->sigma_hit)); 327 | pz += this->z_hit * exp_hit_[std::min(std::abs(z), range_max) * 1000]; 328 | 329 | // Part 2: short reading from unexpected obstacle (e.g., a person) 330 | if(z < 0) 331 | // pz += this->z_short * this->lambda_short * exp(-this->lambda_short*obs_range); 332 | pz += this->z_short * this->lambda_short * exp_short_[std::min(obs_range, range_max) * 1000]; 333 | 334 | // Part 3: Failure to detect obstacle, reported as max-range 335 | if(obs_range >= this->range_max) 336 | pz += this->z_max * 1.0; 337 | 338 | // Part 4: Random measurements 339 | if(obs_range < this->range_max) 340 | pz += this->z_rand * 1.0 / this->range_max; 341 | 342 | // here we have an ad-hoc weighting scheme for combining beam probs 343 | // works well, though... 344 | p += pz * pz * pz; 345 | } 346 | 347 | weight_updates[j] = p; 348 | } 349 | 350 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 351 | // - Update the particle filter 352 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 353 | 354 | for(unsigned int j = 0; j < pf.samples().size(); ++j) 355 | { 356 | Sample& sample = pf.samples()[j]; 357 | sample.weight *= weight_updates[sample_to_unique[j]]; 358 | } 359 | 360 | pf.normalize(true); 361 | } 362 | 363 | 364 | -------------------------------------------------------------------------------- /src/laser_model.h: -------------------------------------------------------------------------------- 1 | #ifndef ED_LOCALIZATION_LASER_MODEL_H_ 2 | #define ED_LOCALIZATION_LASER_MODEL_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | 11 | class ParticleFilter; 12 | 13 | class LaserModel 14 | { 15 | 16 | public: 17 | 18 | LaserModel(); 19 | 20 | ~LaserModel(); 21 | 22 | void configure(tue::Configuration config); 23 | 24 | void updateWeights(const ed::WorldModel& world, const sensor_msgs::LaserScan& scan, ParticleFilter& pf); 25 | 26 | const std::vector& lines_start() const { return lines_start_; } 27 | const std::vector& lines_end() const { return lines_end_; } 28 | 29 | const geo::LaserRangeFinder& renderer() const { return lrf_; } 30 | const std::vector& sensor_ranges() const { return sensor_ranges_; } 31 | 32 | const geo::Transform2& laser_offset() const { return laser_offset_; } 33 | 34 | void setLaserOffset(const geo::Transform2& offset, double height, bool upside_down) 35 | { 36 | laser_offset_ = offset; 37 | laser_height_ = height; 38 | laser_upside_down_ = upside_down; 39 | } 40 | 41 | private: 42 | 43 | double z_hit; 44 | double sigma_hit; 45 | double z_short; 46 | double z_max; 47 | double z_rand; 48 | double lambda_short; 49 | double range_max; 50 | 51 | double laser_height_; 52 | geo::Transform2 laser_offset_; 53 | bool laser_upside_down_; 54 | 55 | uint num_beams; 56 | 57 | double min_particle_distance_; 58 | double min_particle_rotation_distance_; 59 | 60 | // CACHING 61 | std::vector exp_hit_; 62 | std::vector exp_short_; 63 | 64 | // RENDERING 65 | geo::LaserRangeFinder lrf_; 66 | 67 | // Visualization 68 | std::vector lines_start_; 69 | std::vector lines_end_; 70 | std::vector sensor_ranges_; 71 | 72 | }; 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /src/localization_plugin.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "localization_plugin.h" 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | class ConfigurationException: public std::exception 30 | { 31 | public: 32 | ConfigurationException(const std::string& msg){message_ = msg;} 33 | 34 | virtual const char* what() const throw() 35 | { 36 | return message_.c_str(); 37 | } 38 | private: 39 | std::string message_; 40 | }; 41 | 42 | // ---------------------------------------------------------------------------------------------------- 43 | 44 | LocalizationPlugin::LocalizationPlugin() : 45 | visualize_(false), 46 | resample_interval_(1), 47 | resample_count_(0), 48 | update_min_d_(0), 49 | update_min_a_(0), 50 | have_previous_odom_pose_(false), 51 | latest_map_odom_valid_(false), 52 | update_(false), 53 | laser_offset_initialized_(false), 54 | last_map_size_revision_(0), 55 | tf_broadcaster_(nullptr) 56 | { 57 | } 58 | 59 | // ---------------------------------------------------------------------------------------------------- 60 | 61 | LocalizationPlugin::~LocalizationPlugin() 62 | { 63 | 64 | // Get transform between map and odom frame 65 | try 66 | { 67 | geometry_msgs::TransformStamped ts = tf_buffer_->lookupTransform(map_frame_id_, odom_frame_id_, ros::Time(0)); 68 | tf2::Quaternion rotation_map_odom; 69 | tf2::convert(ts.transform.rotation, rotation_map_odom); // Returns a quaternion 70 | 71 | // Store the x, y and yaw on the parameter server 72 | ros::NodeHandle nh; 73 | nh.setParam("initial_pose/x", ts.transform.translation.x); 74 | nh.setParam("initial_pose/y", ts.transform.translation.y); 75 | nh.setParam("initial_pose/yaw", rotation_map_odom.getAngle()); 76 | } 77 | catch (const tf2::TransformException& ex) 78 | { 79 | ROS_ERROR_STREAM_NAMED("Localization", ex.what()); 80 | } 81 | } 82 | 83 | // ---------------------------------------------------------------------------------------------------- 84 | 85 | void LocalizationPlugin::configure(tue::Configuration config) 86 | { 87 | tf_broadcaster_ = std::make_unique(); 88 | 89 | visualize_ = false; 90 | config.value("visualize", visualize_, tue::config::OPTIONAL); 91 | 92 | config.value("resample_interval", resample_interval_); 93 | 94 | config.value("update_min_d", update_min_d_); 95 | config.value("update_min_a", update_min_a_); 96 | 97 | std::string laser_topic; 98 | 99 | if (config.readGroup("odom_model", tue::config::REQUIRED)) 100 | { 101 | config.value("map_frame", map_frame_id_); 102 | config.value("odom_frame", odom_frame_id_); 103 | config.value("base_link_frame", base_link_frame_id_); 104 | 105 | odom_model_.configure(config); 106 | config.endGroup(); 107 | } 108 | 109 | if (config.readGroup("laser_model", tue::config::REQUIRED)) 110 | { 111 | config.value("topic", laser_topic); 112 | laser_model_.configure(config); 113 | config.endGroup(); 114 | } 115 | 116 | if (config.readGroup("particle_filter", tue::config::REQUIRED)) 117 | { 118 | particle_filter_.configure(config); 119 | config.endGroup(); 120 | } 121 | 122 | double tmp_transform_tolerance = 0.1; 123 | config.value("transform_tolerance", tmp_transform_tolerance, tue::config::OPTIONAL); 124 | transform_tolerance_.fromSec(tmp_transform_tolerance); 125 | 126 | if (config.hasError()) 127 | return; 128 | 129 | ros::NodeHandle nh; 130 | 131 | // Subscribe to laser topic 132 | ros::SubscribeOptions sub_options = 133 | ros::SubscribeOptions::create( 134 | laser_topic, 1, boost::bind(&LocalizationPlugin::laserCallback, this, _1), ros::VoidPtr(), &cb_queue_); 135 | sub_laser_ = nh.subscribe(sub_options); 136 | 137 | std::string initial_pose_topic; 138 | if (config.value("initial_pose_topic", initial_pose_topic, tue::config::OPTIONAL)) 139 | { 140 | // Subscribe to initial pose topic 141 | ros::SubscribeOptions sub_opts = 142 | ros::SubscribeOptions::create( 143 | initial_pose_topic, 1, boost::bind(&LocalizationPlugin::initialPoseCallback, this, _1), ros::VoidPtr(), &cb_queue_); 144 | sub_initial_pose_ = nh.subscribe(sub_opts); 145 | } 146 | 147 | geo::Transform2d initial_pose = getInitialPose(nh, config); 148 | 149 | initParticleFilterUniform(initial_pose); 150 | 151 | config.value("robot_name", robot_name_); 152 | 153 | pub_particles_ = nh.advertise("ed/localization/particles", 10); 154 | } 155 | 156 | // ---------------------------------------------------------------------------------------------------- 157 | 158 | geo::Transform2d LocalizationPlugin::getInitialPose(const ros::NodeHandle& nh, tue::Configuration& config) 159 | { 160 | try 161 | { 162 | return tryGetInitialPoseFromParamServer(nh); 163 | } 164 | catch (const ConfigurationException&) 165 | { 166 | } 167 | 168 | try 169 | { 170 | return tryGetInitialPoseFromConfig(config); 171 | } 172 | catch (const ConfigurationException&) 173 | { 174 | } 175 | 176 | return geo::Transform2d::identity(); 177 | } 178 | 179 | // ---------------------------------------------------------------------------------------------------- 180 | 181 | geo::Transform2 LocalizationPlugin::tryGetInitialPoseFromParamServer(const ros::NodeHandle& nh) 182 | { 183 | // Getting last pose from parameter server 184 | std::map ros_param_position; 185 | if (!nh.getParam("initial_pose", ros_param_position)) 186 | { 187 | std::string msg = "Could not read initial pose from the parameter server"; 188 | ROS_WARN_STREAM_NAMED("Localization", msg); 189 | throw ConfigurationException(msg); 190 | } 191 | 192 | // Make a homogeneous transformation with the variables from the parameter server 193 | tf2::Transform homogtrans_map_odom; 194 | homogtrans_map_odom.setOrigin(tf2::Vector3(ros_param_position["x"], ros_param_position["y"], 0.0)); 195 | tf2::Quaternion q_map_odom; 196 | q_map_odom.setRPY(0, 0, ros_param_position["yaw"]); 197 | homogtrans_map_odom.setRotation(q_map_odom); 198 | 199 | if (!tf_buffer_->canTransform(odom_frame_id_, base_link_frame_id_, ros::Time(0), ros::Duration(1))) 200 | { 201 | std::string msg = "No transform between odom and base_link"; 202 | ROS_ERROR_STREAM_NAMED("Localization", msg); 203 | throw ConfigurationException(msg); 204 | } 205 | 206 | try 207 | { 208 | geometry_msgs::TransformStamped ts = tf_buffer_->lookupTransform(odom_frame_id_, base_link_frame_id_, ros::Time(0)); 209 | tf2::Stamped tf_odom_base_link; 210 | tf2::convert(ts, tf_odom_base_link); 211 | // Calculate base link position in map frame 212 | tf2::Transform tf_map_base_link = homogtrans_map_odom * tf_odom_base_link; 213 | tf2::Vector3 pos_map_baselink = tf_map_base_link.getOrigin(); // Returns a vector 214 | tf2::Quaternion rotation_map_baselink = tf_map_base_link.getRotation(); // Returns a quaternion 215 | 216 | geo::Transform2d result; 217 | result.t.x = pos_map_baselink.x(); 218 | result.t.y = pos_map_baselink.y(); 219 | result.setRotation(rotation_map_baselink.getAngle()); 220 | 221 | ROS_DEBUG_STREAM_NAMED("Localization", "Initial pose from parameter server: [" << 222 | result.t.x << ", " << result.t.y << "], yaw:" << result.rotation() 223 | ); 224 | return result; 225 | } 226 | catch (const tf2::TransformException& ex) 227 | { 228 | std::string msg = std::string(ex.what()); 229 | ROS_ERROR_STREAM_NAMED("Localization", msg); 230 | throw ConfigurationException(msg); 231 | } 232 | } 233 | 234 | // ---------------------------------------------------------------------------------------------------- 235 | 236 | geo::Transform2 LocalizationPlugin::tryGetInitialPoseFromConfig(tue::Configuration& config) 237 | { 238 | double x, y, yaw; 239 | if (config.readGroup("initial_pose", tue::config::OPTIONAL)) 240 | { 241 | config.value("x", x); 242 | config.value("y", y); 243 | config.value("rz", yaw); 244 | config.endGroup(); 245 | } 246 | else 247 | { 248 | std::string message = "Initial pose not present in config"; 249 | ROS_WARN_STREAM_NAMED("Localization", message); 250 | throw ConfigurationException(message); 251 | } 252 | 253 | geo::Transform2d result(x, y, yaw); 254 | return result; 255 | } 256 | 257 | // ---------------------------------------------------------------------------------------------------- 258 | 259 | void LocalizationPlugin::initialize() 260 | { 261 | } 262 | 263 | // ---------------------------------------------------------------------------------------------------- 264 | 265 | void LocalizationPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req) 266 | { 267 | initial_pose_msg_.reset(); 268 | cb_queue_.callAvailable(); 269 | 270 | if (initial_pose_msg_) 271 | { 272 | // Set initial pose 273 | geo::Pose3D pose; 274 | geo::convert(initial_pose_msg_->pose.pose, pose); 275 | initParticleFilterUniform(pose.projectTo2d()); 276 | } 277 | 278 | while(!scan_buffer_.empty()) 279 | { 280 | TransformStatus status = update(scan_buffer_.front(), world, req); 281 | if (status == OK || status == TOO_OLD || status == UNKNOWN_ERROR) 282 | scan_buffer_.pop(); 283 | else 284 | break; 285 | } 286 | } 287 | 288 | // ---------------------------------------------------------------------------------------------------- 289 | 290 | TransformStatus LocalizationPlugin::update(const sensor_msgs::LaserScanConstPtr& scan, const ed::WorldModel& world, ed::UpdateRequest& req) 291 | { 292 | // Get transformation from base_link to laser_frame 293 | if (!laser_offset_initialized_) 294 | { 295 | TransformStatus ts = initLaserOffset(scan->header.frame_id, scan->header.stamp); 296 | if (ts != OK) 297 | return ts; 298 | } 299 | 300 | 301 | // Check if particle filter is initialized 302 | if (particle_filter_.samples().empty()) 303 | { 304 | ROS_ERROR_NAMED("Localization", "(update) Empty particle filter"); 305 | return UNKNOWN_ERROR; 306 | } 307 | 308 | 309 | // Calculate delta movement based on odom (fetched from TF) 310 | geo::Pose3D odom_to_base_link; 311 | geo::Transform2 movement; 312 | 313 | tf2::Stamped odom_to_base_link_tf; 314 | TransformStatus ts = transform(odom_frame_id_, base_link_frame_id_, scan->header.stamp, odom_to_base_link_tf); 315 | if (ts != OK) 316 | return ts; 317 | 318 | geo::convert(odom_to_base_link_tf, odom_to_base_link); 319 | 320 | if (have_previous_odom_pose_) 321 | { 322 | // Get displacement and project to 2D 323 | movement = (previous_odom_pose_.inverse() * odom_to_base_link).projectTo2d(); 324 | 325 | update_ = std::abs(movement.t.x) >= update_min_d_ || std::abs(movement.t.y) >= update_min_d_ || std::abs(movement.rotation()) >= update_min_a_; 326 | } 327 | 328 | bool force_publication = false; 329 | if (!have_previous_odom_pose_) 330 | { 331 | previous_odom_pose_ = odom_to_base_link; 332 | have_previous_odom_pose_ = true; 333 | update_ = true; 334 | force_publication = true; 335 | } 336 | else if (have_previous_odom_pose_ && update_) 337 | { 338 | // Update motion 339 | odom_model_.updatePoses(movement, particle_filter_); 340 | } 341 | 342 | bool resampled = false; 343 | if (update_) 344 | { 345 | ROS_DEBUG_NAMED("Localization", "Updating laser"); 346 | // Update sensor 347 | laser_model_.updateWeights(world, *scan, particle_filter_); 348 | 349 | previous_odom_pose_ = odom_to_base_link; 350 | have_previous_odom_pose_ = true; 351 | 352 | update_ = false; 353 | 354 | 355 | // (Re)sample 356 | resampled = resample(world); 357 | 358 | // Publish particles 359 | publishParticles(scan->header.stamp); 360 | } 361 | 362 | // Update map-odom 363 | if(resampled || force_publication) 364 | { 365 | updateMapOdom(odom_to_base_link); 366 | } 367 | 368 | // Publish result 369 | if (latest_map_odom_valid_) 370 | { 371 | publishMapOdom(scan->header.stamp); 372 | 373 | // This should be executed allways. map_odom * odom_base_link 374 | if (!robot_name_.empty()) 375 | req.setPose(robot_name_, latest_map_odom_ * previous_odom_pose_); 376 | } 377 | 378 | // Visualization 379 | if (visualize_) 380 | { 381 | visualize(); 382 | } 383 | else 384 | { 385 | cv::destroyAllWindows(); 386 | } 387 | 388 | return OK; 389 | } 390 | 391 | // ---------------------------------------------------------------------------------------------------- 392 | 393 | TransformStatus LocalizationPlugin::initLaserOffset(const std::string& frame_id, const ros::Time& stamp) 394 | { 395 | tf2::Stamped p_laser; 396 | TransformStatus ts = transform(base_link_frame_id_, frame_id, stamp, p_laser); 397 | 398 | if (ts != OK) 399 | return ts; 400 | 401 | geo::Transform2 offset(geo::Mat2(p_laser.getBasis()[0][0], p_laser.getBasis()[0][1], 402 | p_laser.getBasis()[1][0], p_laser.getBasis()[1][1]), 403 | geo::Vec2(p_laser.getOrigin().getX(), p_laser.getOrigin().getY())); 404 | 405 | bool upside_down = p_laser.getBasis()[2][2] < 0; 406 | if (upside_down) 407 | { 408 | offset.R.yx = -offset.R.yx; 409 | offset.R.yy = -offset.R.yy; 410 | } 411 | 412 | double laser_height = p_laser.getOrigin().getZ(); 413 | 414 | laser_model_.setLaserOffset(offset, laser_height, upside_down); 415 | 416 | laser_offset_initialized_ = true; 417 | 418 | return OK; 419 | } 420 | 421 | // ---------------------------------------------------------------------------------------------------- 422 | 423 | void LocalizationPlugin::initParticleFilterUniform(const geo::Transform2& pose) 424 | { 425 | const geo::Vec2& p = pose.getOrigin(); 426 | const double yaw = pose.rotation(); 427 | particle_filter_.initUniform(p - geo::Vec2(0.3, 0.3), p + geo::Vec2(0.3, 0.3), yaw - 0.3, yaw + 0.3); 428 | have_previous_odom_pose_ = false; 429 | resample_count_ = 0; 430 | } 431 | 432 | // ---------------------------------------------------------------------------------------------------- 433 | 434 | bool LocalizationPlugin::resample(const ed::WorldModel& world) 435 | { 436 | if(++resample_count_ % resample_interval_) 437 | return false; 438 | 439 | ROS_DEBUG_NAMED("Localization", "resample particle filter"); 440 | const std::function update_map_size_func = std::bind(&LocalizationPlugin::updateMapSize, this, std::ref(world)); 441 | const std::function gen_random_pose_func = std::bind(&LocalizationPlugin::generateRandomPose, this, update_map_size_func); 442 | particle_filter_.resample(gen_random_pose_func); 443 | return true; 444 | } 445 | 446 | // ---------------------------------------------------------------------------------------------------- 447 | 448 | void LocalizationPlugin::publishParticles(const ros::Time &stamp) 449 | { 450 | ROS_DEBUG_NAMED("Localization", "Publishing particles"); 451 | const std::vector& samples = particle_filter_.samples(); 452 | geometry_msgs::PoseArray particles_msg; 453 | particles_msg.poses.resize(samples.size()); 454 | for(unsigned int i = 0; i < samples.size(); ++i) 455 | { 456 | const geo::Transform2& p = samples[i].pose; 457 | 458 | geo::Pose3D pose_3d = p.projectTo3d(); 459 | 460 | geo::convert(pose_3d, particles_msg.poses[i]); 461 | } 462 | 463 | particles_msg.header.frame_id = map_frame_id_; 464 | particles_msg.header.stamp = stamp; 465 | 466 | pub_particles_.publish(particles_msg); 467 | } 468 | 469 | // ---------------------------------------------------------------------------------------------------- 470 | 471 | void LocalizationPlugin::updateMapOdom(const geo::Pose3D& odom_to_base_link) 472 | { 473 | ROS_DEBUG_NAMED("Localization", "Updating map_odom"); 474 | // Get the best pose (2D) 475 | geo::Transform2 mean_pose = particle_filter_.calculateMeanPose(); 476 | ROS_DEBUG_STREAM("mean_pose: x: " << mean_pose.t.x << ", y: " << mean_pose.t.y << ", yaw: " << mean_pose.rotation()); 477 | 478 | // Convert best pose to 3D 479 | geo::Pose3D map_to_base_link; 480 | map_to_base_link = mean_pose.projectTo3d(); 481 | 482 | latest_map_odom_ = map_to_base_link * odom_to_base_link.inverse(); 483 | latest_map_odom_valid_ = true; 484 | } 485 | 486 | // ---------------------------------------------------------------------------------------------------- 487 | 488 | void LocalizationPlugin::publishMapOdom(const ros::Time &stamp) 489 | { 490 | ROS_DEBUG_THROTTLE_NAMED(2, "Localization", "Publishing map_odom"); 491 | // Convert to TF transform 492 | geometry_msgs::TransformStamped latest_map_odom_tf; 493 | geo::convert(latest_map_odom_, latest_map_odom_tf.transform); 494 | 495 | // Set frame id's and time stamp 496 | latest_map_odom_tf.header.frame_id = map_frame_id_; 497 | latest_map_odom_tf.child_frame_id = odom_frame_id_; 498 | latest_map_odom_tf.header.stamp = stamp + transform_tolerance_; 499 | 500 | // Publish TF 501 | tf_broadcaster_->sendTransform(latest_map_odom_tf); 502 | } 503 | 504 | // ---------------------------------------------------------------------------------------------------- 505 | 506 | TransformStatus LocalizationPlugin::transform(const std::string& target_frame, const std::string& source_frame, 507 | const ros::Time& time, tf2::Stamped& transform) 508 | { 509 | try 510 | { 511 | geometry_msgs::TransformStamped ts = tf_buffer_->lookupTransform(target_frame, source_frame, time); 512 | tf2::convert(ts, transform); 513 | return OK; 514 | } 515 | catch (const tf2::ExtrapolationException& ex) 516 | { 517 | try 518 | { 519 | // Now we have to check if the error was an interpolation or extrapolation error 520 | // (i.e., the scan is too old or too new, respectively) 521 | geometry_msgs::TransformStamped latest_transform = tf_buffer_->lookupTransform(target_frame, source_frame, ros::Time(0)); 522 | 523 | if (scan_buffer_.front()->header.stamp > latest_transform.header.stamp) 524 | { 525 | // Scan is too new 526 | return TOO_RECENT; 527 | } 528 | else 529 | { 530 | // Otherwise it has to be too old 531 | return TOO_OLD; 532 | } 533 | } 534 | catch (const tf2::TransformException& exc) 535 | { 536 | return UNKNOWN_ERROR; 537 | } 538 | } 539 | catch (const tf2::TransformException& ex) 540 | { 541 | return UNKNOWN_ERROR; 542 | } 543 | } 544 | 545 | // ---------------------------------------------------------------------------------------------------- 546 | 547 | void LocalizationPlugin::laserCallback(const sensor_msgs::LaserScanConstPtr& msg) 548 | { 549 | scan_buffer_.push(msg); 550 | } 551 | 552 | // ---------------------------------------------------------------------------------------------------- 553 | 554 | void LocalizationPlugin::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) 555 | { 556 | initial_pose_msg_ = msg; 557 | } 558 | 559 | // ---------------------------------------------------------------------------------------------------- 560 | 561 | geo::Transform2 LocalizationPlugin::generateRandomPose(std::function update_map_size) 562 | { 563 | update_map_size(); 564 | geo::Transform2 pose; 565 | pose.t = min_map_; 566 | const geo::Vec2 map_size = max_map_ - min_map_; 567 | pose.t.x += drand48() * map_size.x; 568 | pose.t.y += drand48() * map_size.y; 569 | pose.setRotation(drand48() * 2 * M_PI - M_PI); 570 | return pose; 571 | } 572 | 573 | // ---------------------------------------------------------------------------------------------------- 574 | 575 | void LocalizationPlugin::updateMapSize(const ed::WorldModel& world) 576 | { 577 | if (world.revision() <= last_map_size_revision_) 578 | return; 579 | 580 | geo::Vec2 min(1e6, 1e6), max(-1e6, -1e6); 581 | 582 | for (auto it = world.begin(); it != world.end(); ++ it) 583 | { 584 | const ed::EntityConstPtr& e = *it; 585 | 586 | const geo::ShapeConstPtr& shape = e->visual(); 587 | 588 | // Skip robot and entities without pose or shape 589 | if (!e->has_pose() || !shape || e->hasFlag("self") || shape->getBoundingBox().getMax().z < 0.05) 590 | continue; 591 | 592 | geo::Vector3 min_entity_world = e->pose() * shape->getBoundingBox().getMin(); 593 | geo::Vector3 max_entity_world = e->pose() * shape->getBoundingBox().getMax(); 594 | 595 | min.x = std::min(min.x, min_entity_world.x); 596 | min.y = std::min(min.y, min_entity_world.y); 597 | max.x = std::max(max.x, max_entity_world.x); 598 | max.y = std::max(max.y, max_entity_world.y); 599 | } 600 | 601 | min_map_ = min; 602 | max_map_ = max; 603 | last_map_size_revision_ = world.revision(); 604 | 605 | } 606 | 607 | // ---------------------------------------------------------------------------------------------------- 608 | 609 | void LocalizationPlugin::visualize() 610 | { 611 | ROS_DEBUG_NAMED("Localization", "Visualize"); 612 | int grid_size = 800; 613 | double grid_resolution = 0.025; 614 | 615 | cv::Mat rgb_image(grid_size, grid_size, CV_8UC3, cv::Scalar(10, 10, 10)); 616 | 617 | std::vector sensor_points; 618 | laser_model_.renderer().rangesToPoints(laser_model_.sensor_ranges(), sensor_points); 619 | 620 | geo::Transform2 best_pose = (latest_map_odom_ * previous_odom_pose_).projectTo2d(); 621 | 622 | geo::Transform2 laser_pose = best_pose * laser_model_.laser_offset(); 623 | for(unsigned int i = 0; i < sensor_points.size(); ++i) 624 | { 625 | const geo::Vec2& p = laser_pose * geo::Vec2(sensor_points[i].x, sensor_points[i].y); 626 | int mx = -(p.y - best_pose.t.y) / grid_resolution + grid_size / 2; 627 | int my = -(p.x - best_pose.t.x) / grid_resolution + grid_size / 2; 628 | 629 | if (mx >= 0 && my >= 0 && mx < grid_size && my (my, mx) = cv::Vec3b(0, 255, 0); 632 | } 633 | } 634 | 635 | const std::vector& lines_start = laser_model_.lines_start(); 636 | const std::vector& lines_end = laser_model_.lines_end(); 637 | 638 | for(unsigned int i = 0; i < lines_start.size(); ++i) 639 | { 640 | const geo::Vec2& p1 = lines_start[i]; 641 | int mx1 = -(p1.y - best_pose.t.y) / grid_resolution + grid_size / 2; 642 | int my1 = -(p1.x - best_pose.t.x) / grid_resolution + grid_size / 2; 643 | 644 | const geo::Vec2& p2 = lines_end[i]; 645 | int mx2 = -(p2.y - best_pose.t.y) / grid_resolution + grid_size / 2; 646 | int my2 = -(p2.x - best_pose.t.x) / grid_resolution + grid_size / 2; 647 | 648 | cv::line(rgb_image, cv::Point(mx1, my1), cv::Point(mx2, my2), cv::Scalar(255, 255, 255), 1); 649 | } 650 | 651 | const std::vector& samples = particle_filter_.samples(); 652 | for(std::vector::const_iterator it = samples.begin(); it != samples.end(); ++it) 653 | { 654 | const geo::Transform2& pose = it->pose; 655 | 656 | // Visualize sensor 657 | int lmx = -(pose.t.y - best_pose.t.y) / grid_resolution + grid_size / 2; 658 | int lmy = -(pose.t.x - best_pose.t.x) / grid_resolution + grid_size / 2; 659 | cv::circle(rgb_image, cv::Point(lmx,lmy), 0.1 / grid_resolution, cv::Scalar(0, 0, 255), 1); 660 | 661 | geo::Vec2 d = pose.R * geo::Vec2(0.2, 0); 662 | int dmx = -d.y / grid_resolution; 663 | int dmy = -d.x / grid_resolution; 664 | cv::line(rgb_image, cv::Point(lmx, lmy), cv::Point(lmx + dmx, lmy + dmy), cv::Scalar(0, 0, 255), 1); 665 | } 666 | 667 | cv::imshow("localization", rgb_image); 668 | cv::waitKey(1); 669 | } 670 | 671 | // ---------------------------------------------------------------------------------------------------- 672 | 673 | ED_REGISTER_PLUGIN(LocalizationPlugin) 674 | -------------------------------------------------------------------------------- /src/localization_plugin.h: -------------------------------------------------------------------------------- 1 | #ifndef ED_LOCALIZATION_PLUGIN_H_ 2 | #define ED_LOCALIZATION_PLUGIN_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | // ROS 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // SCAN BUFFER 18 | #include 19 | 20 | // TF2 21 | #include 22 | 23 | // MODELS 24 | #include "particle_filter.h" 25 | #include "odom_model.h" 26 | #include "laser_model.h" 27 | 28 | #include 29 | #include 30 | 31 | namespace tf2 { 32 | class Transform; 33 | } 34 | 35 | namespace tf2_ros { 36 | class TransformBroadcaster; 37 | } 38 | 39 | enum TransformStatus 40 | { 41 | TOO_RECENT, 42 | TOO_OLD, 43 | OK, 44 | UNKNOWN_ERROR 45 | }; 46 | 47 | class LocalizationPlugin : public ed::Plugin 48 | { 49 | 50 | public: 51 | 52 | LocalizationPlugin(); 53 | 54 | virtual ~LocalizationPlugin(); 55 | 56 | void configure(tue::Configuration config); 57 | 58 | void initialize(); 59 | 60 | void process(const ed::WorldModel& world, ed::UpdateRequest& req); 61 | 62 | private: 63 | 64 | std::string robot_name_; 65 | 66 | // Config 67 | bool visualize_; 68 | 69 | int resample_interval_; 70 | int resample_count_; 71 | 72 | double update_min_d_; 73 | double update_min_a_; 74 | 75 | // PARTICLE FILTER 76 | ParticleFilter particle_filter_; 77 | 78 | // MODELS 79 | LaserModel laser_model_; 80 | OdomModel odom_model_; 81 | 82 | // Poses 83 | bool have_previous_odom_pose_; 84 | geo::Pose3D previous_odom_pose_; 85 | 86 | bool latest_map_odom_valid_; 87 | geo::Pose3D latest_map_odom_; 88 | 89 | // State 90 | bool update_; 91 | bool laser_offset_initialized_; 92 | 93 | // random pose generation 94 | geo::Vec2 min_map_, max_map_; 95 | unsigned long last_map_size_revision_; 96 | 97 | // Initial pose 98 | geometry_msgs::PoseWithCovarianceStampedConstPtr initial_pose_msg_; 99 | 100 | // Scan buffer 101 | std::queue scan_buffer_; 102 | 103 | std::string map_frame_id_; 104 | std::string odom_frame_id_; 105 | std::string base_link_frame_id_; 106 | 107 | // TF2 108 | ros::Duration transform_tolerance_; 109 | std::unique_ptr tf_broadcaster_; 110 | 111 | // ROS 112 | ros::CallbackQueue cb_queue_; 113 | ros::Subscriber sub_laser_; 114 | ros::Subscriber sub_initial_pose_; 115 | ros::Publisher pub_particles_; 116 | 117 | // Configuration 118 | geo::Transform2 getInitialPose(const ros::NodeHandle& nh, tue::Configuration& config); 119 | geo::Transform2 tryGetInitialPoseFromParamServer(const ros::NodeHandle& nh); 120 | geo::Transform2 tryGetInitialPoseFromConfig(tue::Configuration& config); 121 | 122 | // Init 123 | TransformStatus initLaserOffset(const std::string& frame_id, const ros::Time& stamp); 124 | 125 | void initParticleFilterUniform(const geo::Transform2& pose); 126 | 127 | // Callbacks 128 | void laserCallback(const sensor_msgs::LaserScanConstPtr& msg); 129 | 130 | void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); 131 | 132 | // random pose generation 133 | geo::Transform2 generateRandomPose(std::function update_map_size); 134 | 135 | void updateMapSize(const ed::WorldModel& world); 136 | 137 | TransformStatus update(const sensor_msgs::LaserScanConstPtr& laser_msg_, const ed::WorldModel& world, ed::UpdateRequest& req); 138 | 139 | bool resample(const ed::WorldModel& world); 140 | 141 | void publishParticles(const ros::Time& stamp); 142 | 143 | void updateMapOdom(const geo::Pose3D& odom_to_base_link); 144 | 145 | void publishMapOdom(const ros::Time &stamp); 146 | 147 | TransformStatus transform(const std::string& target_frame, const std::string& source_frame, 148 | const ros::Time& time, tf2::Stamped& transform); 149 | 150 | void visualize(); 151 | 152 | }; 153 | 154 | #endif 155 | -------------------------------------------------------------------------------- /src/localization_plugin_old.cpp: -------------------------------------------------------------------------------- 1 | #include "localization_plugin.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | 15 | // Inflation 16 | #include 17 | 18 | // ---------------------------------------------------------------------------------------------------- 19 | 20 | class CellData 21 | { 22 | public: 23 | 24 | CellData(int index_, int dx_, int dy_, int distance_) : 25 | index(index_), dx(dx_), dy(dy_), distance(distance_) 26 | {} 27 | 28 | int index; 29 | int dx, dy; 30 | int distance; 31 | 32 | }; 33 | 34 | // ---------------------------------------------------------------------------------------------------- 35 | 36 | inline bool operator<(const CellData &a, const CellData &b) 37 | { 38 | return a.distance > b.distance; 39 | } 40 | 41 | // ---------------------------------------------------------------------------------------------------- 42 | 43 | class LineRenderResult : public geo::LaserRangeFinder::RenderResult 44 | { 45 | 46 | public: 47 | 48 | LineRenderResult(std::queue& Q, int grid_size, double res) : Q_(Q), grid_size_(grid_size), res_(res), 49 | p_min(1e9), p_max(-1e9) {} 50 | 51 | void renderLine(const geo::Vector3& p1, const geo::Vector3& p2) 52 | { 53 | geo::Vector3 v_diff = p2 - p1; 54 | double n_diff = v_diff.length(); 55 | 56 | geo::Vector3 vd = v_diff / n_diff * res_; 57 | int n = n_diff / res_ + 1; 58 | 59 | geo::Vector3 p = p1; 60 | for(int i = 0; i < n; ++i) 61 | { 62 | int x = -p.y / res_ + grid_size_ / 2; 63 | int y = -p.x / res_ + grid_size_ / 2; 64 | 65 | if (x >= 0 && y >= 0 && x < grid_size_ && y & Q_; 82 | int grid_size_; 83 | double res_; 84 | 85 | public: 86 | 87 | bool empty; 88 | geo::Vec2 p_min, p_max; 89 | 90 | }; 91 | 92 | // ---------------------------------------------------------------------------------------------------- 93 | 94 | void drawLaserPoints(cv::Mat& img, const geo::LaserRangeFinder& lrf, const std::vector& ranges, const cv::Vec3b& clr) 95 | { 96 | std::vector points; 97 | if (lrf.rangesToPoints(ranges, points)) { 98 | for(unsigned int i = 0; i < points.size(); ++i) { 99 | const geo::Vector3& p = points[i]; 100 | double x = (-p.y * 25) + img.cols / 2; 101 | double y = (-p.x * 25) + img.rows / 2; 102 | 103 | if (x >= 0 && y >= 0 && x < img.cols && y < img.rows) { 104 | img.at(y, x) = clr; 105 | } 106 | } 107 | } 108 | } 109 | 110 | // ---------------------------------------------------------------------------------------------------- 111 | 112 | class KernelCell 113 | { 114 | public: 115 | 116 | KernelCell(int d_index_, int dx_, int dy_) : 117 | d_index(d_index_), dx(dx_), dy(dy_) 118 | {} 119 | 120 | int d_index; 121 | int dx, dy; 122 | }; 123 | 124 | // ---------------------------------------------------------------------------------------------------- 125 | 126 | LocalizationPlugin::LocalizationPlugin() : pose_initialized_(false) 127 | { 128 | } 129 | 130 | // ---------------------------------------------------------------------------------------------------- 131 | 132 | LocalizationPlugin::~LocalizationPlugin() 133 | { 134 | } 135 | 136 | // ---------------------------------------------------------------------------------------------------- 137 | 138 | void LocalizationPlugin::configure(tue::Configuration config) 139 | { 140 | std::string laser_topic; 141 | config.value("laser_topic", laser_topic); 142 | 143 | if (config.hasError()) 144 | return; 145 | 146 | ros::NodeHandle nh; 147 | 148 | // Subscribe to laser topic 149 | ros::SubscribeOptions sub_options = 150 | ros::SubscribeOptions::create( 151 | laser_topic, 1, boost::bind(&LocalizationPlugin::laserCallback, this, _1), ros::VoidPtr(), &cb_queue_); 152 | 153 | sub_laser_ = nh.subscribe(sub_options); 154 | 155 | a_current_ = 0; 156 | laser_pose_ = geo::Pose3D::identity(); 157 | laser_pose_.t.z = 0.3; 158 | } 159 | 160 | // ---------------------------------------------------------------------------------------------------- 161 | 162 | void LocalizationPlugin::initialize() 163 | { 164 | } 165 | 166 | // ---------------------------------------------------------------------------------------------------- 167 | 168 | void LocalizationPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req) 169 | { 170 | last_laser_msg_.reset(); 171 | cb_queue_.callAvailable(); 172 | 173 | if (!last_laser_msg_) 174 | return; 175 | 176 | tue::Timer timer; 177 | timer.start(); 178 | 179 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 180 | // - Update sensor model 181 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 182 | 183 | if (lrf_.getNumBeams() != last_laser_msg_->ranges.size()) 184 | { 185 | lrf_.setNumBeams(last_laser_msg_->ranges.size()); 186 | lrf_.setRangeLimits(last_laser_msg_->range_min, last_laser_msg_->range_max); 187 | lrf_.setAngleLimits(last_laser_msg_->angle_min, last_laser_msg_->angle_max); 188 | } 189 | 190 | std::vector sensor_ranges(last_laser_msg_->ranges.size()); 191 | for (unsigned int i = 0; i < last_laser_msg_->ranges.size(); ++i) 192 | sensor_ranges[i] = last_laser_msg_->ranges[i]; 193 | 194 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 195 | // - Create world model cross section 196 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 197 | 198 | std::vector entities; 199 | for(ed::WorldModel::const_iterator it = world.begin(); it != world.end(); ++it) 200 | { 201 | if (it->second->visual()) 202 | entities.push_back(it->second); 203 | } 204 | 205 | geo::Pose3D laser_pose; 206 | laser_pose.t = laser_pose_.t + geo::Vector3(0, 0, 0); 207 | laser_pose.R.setRPY(0, 0, 0); 208 | 209 | double grid_resolution = 0.025; 210 | int grid_size = 800; 211 | 212 | std::queue Q; 213 | LineRenderResult render_result(Q, grid_size, grid_resolution); 214 | 215 | for(std::vector::const_iterator it = entities.begin(); it != entities.end(); ++it) 216 | { 217 | const ed::EntityConstPtr& e = *it; 218 | 219 | geo::LaserRangeFinder::RenderOptions options; 220 | geo::Transform t_inv = laser_pose.inverse() * e->pose(); 221 | options.setMesh(e->visual()->getMesh(), t_inv); 222 | lrf_.render(options, render_result); 223 | } 224 | 225 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 226 | // - Create distance map 227 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 228 | 229 | double max_dist = 0.3; 230 | 231 | cv::Mat distance_map(grid_size, grid_size, CV_32FC1, (max_dist / grid_resolution) * (max_dist / grid_resolution)); 232 | for(int i = 0; i < grid_size; ++i) 233 | { 234 | distance_map.at(i, 0) = 0; 235 | distance_map.at(i, grid_size - 1) = 0; 236 | distance_map.at(0, i) = 0; 237 | distance_map.at(grid_size - 1, i) = 0; 238 | } 239 | 240 | std::vector kernel; 241 | 242 | kernel.push_back(KernelCell(-distance_map.cols, 0, -1)); 243 | kernel.push_back(KernelCell(distance_map.cols, 0, 1)); 244 | kernel.push_back(KernelCell(-1, -1, 0)); 245 | kernel.push_back(KernelCell( 1, 1, 0)); 246 | 247 | while(!Q.empty()) 248 | { 249 | const CellData& c = Q.front(); 250 | 251 | double current_distance = distance_map.at(c.index); 252 | if (c.distance < current_distance) 253 | { 254 | distance_map.at(c.index) = c.distance; 255 | for(unsigned int i = 0; i < kernel.size(); ++i) 256 | { 257 | const KernelCell& kc = kernel[i]; 258 | int new_index = c.index + kc.d_index; 259 | int dx_new = c.dx + kc.dx; 260 | int dy_new = c.dy + kc.dy; 261 | 262 | int new_distance = (dx_new * dx_new) + (dy_new * dy_new); 263 | Q.push(CellData(new_index, dx_new, dy_new, new_distance)); 264 | } 265 | } 266 | 267 | Q.pop(); 268 | } 269 | 270 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 271 | // - Create samples 272 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 273 | 274 | std::vector poses; 275 | 276 | if (!pose_initialized_) 277 | { 278 | // Uniform sampling over world 279 | 280 | for(double x = render_result.p_min.x; x < render_result.p_max.x; x += 0.2) 281 | { 282 | for(double y = render_result.p_min.y; y < render_result.p_max.y; y += 0.2) 283 | { 284 | for(double a = 0; a < 6.28; a += 0.1) 285 | { 286 | geo::Pose3D laser_pose; 287 | laser_pose.t = geo::Vector3(x, y, 0); 288 | laser_pose.R.setRPY(0, 0, a); 289 | 290 | poses.push_back(laser_pose); 291 | } 292 | } 293 | } 294 | } 295 | else 296 | { 297 | poses.push_back(best_laser_pose_); 298 | 299 | for(double dx = -0.3; dx < 0.3; dx += 0.1) 300 | { 301 | for(double dy = -0.3; dy < 0.3; dy += 0.1) 302 | { 303 | for(double da = -1; da < 1; da += 0.1) 304 | { 305 | geo::Pose3D dT; 306 | dT.t = geo::Vector3(dx, dy, 0); 307 | dT.R.setRPY(0, 0, da); 308 | 309 | poses.push_back(best_laser_pose_ * dT); 310 | } 311 | } 312 | } 313 | } 314 | 315 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 316 | // - Test samples and find sample with lowest error 317 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 318 | 319 | std::vector sensor_points; 320 | lrf_.rangesToPoints(sensor_ranges, sensor_points); 321 | 322 | int i_step = 1; 323 | if (poses.size() > 10000) 324 | i_step = sensor_points.size() / 100; // limit to 100 beams 325 | 326 | std::cout << "i_step = " << i_step << std::endl; 327 | 328 | double min_sum_sq_error = 1e10; 329 | for(std::vector::const_iterator it = poses.begin(); it != poses.end(); ++it) 330 | { 331 | const geo::Pose3D& laser_pose = *it; 332 | 333 | double z1 = laser_pose.R.xx / grid_resolution; 334 | double z2 = laser_pose.R.xy / grid_resolution; 335 | double z3 = laser_pose.R.yx / grid_resolution; 336 | double z4 = laser_pose.R.yy / grid_resolution; 337 | double t1 = laser_pose.t.x / grid_resolution - distance_map.cols / 2; 338 | double t2 = laser_pose.t.y / grid_resolution - distance_map.cols / 2; 339 | 340 | double sum_sq_error = 0; 341 | for(unsigned int i = 0; i < sensor_points.size(); i += i_step) 342 | { 343 | const geo::Vector3& v = sensor_points[i]; 344 | double py = z3 * v.x + z4 * v.y + t2; 345 | int mx = -py; 346 | 347 | if (mx > 0 && mx < grid_size) 348 | { 349 | double px = z1 * v.x + z2 * v.y + t1; 350 | int my = -px; 351 | 352 | if (my > 0 && my < grid_size) 353 | { 354 | sum_sq_error += distance_map.at(my, mx); 355 | } 356 | } 357 | } 358 | 359 | if (sum_sq_error < min_sum_sq_error) 360 | { 361 | min_sum_sq_error = sum_sq_error; 362 | best_laser_pose_ = laser_pose; 363 | pose_initialized_ = true; 364 | } 365 | } 366 | 367 | std::cout << min_sum_sq_error << std::endl; 368 | std::cout << best_laser_pose_ << std::endl; 369 | 370 | std::cout << timer.getElapsedTimeInMilliSec() << " ms" << std::endl; 371 | 372 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 373 | // - Visualization 374 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 375 | 376 | bool visualize = true; 377 | if (visualize) 378 | { 379 | cv::Mat rgb_image(distance_map.rows, distance_map.cols, CV_8UC3, cv::Scalar(0, 0, 0)); 380 | for(int y = 0; y < rgb_image.rows; ++y) 381 | { 382 | for(int x = 0; x < rgb_image.cols; ++x) 383 | { 384 | int c = distance_map.at(y, x) / ((max_dist / grid_resolution) * (max_dist / grid_resolution)) * 255; 385 | rgb_image.at(y, x) = cv::Vec3b(c, c, c); 386 | } 387 | } 388 | 389 | for(unsigned int i = 0; i < sensor_points.size(); ++i) 390 | { 391 | const geo::Vector3& p = best_laser_pose_ * sensor_points[i]; 392 | int mx = -p.y / grid_resolution + grid_size / 2; 393 | int my = -p.x / grid_resolution + grid_size / 2; 394 | 395 | if (mx >= 0 && my >= 0 && mx < grid_size && my (my, mx) = cv::Vec3b(0, 255, 0); 398 | } 399 | } 400 | 401 | // Visualize sensor 402 | int lmx = -best_laser_pose_.t.y / grid_resolution + grid_size / 2; 403 | int lmy = -best_laser_pose_.t.x / grid_resolution + grid_size / 2; 404 | cv::circle(rgb_image, cv::Point(lmx,lmy), 0.3 / grid_resolution, cv::Scalar(0, 0, 255), 1); 405 | 406 | geo::Vector3 d = best_laser_pose_.R * geo::Vector3(0.3, 0, 0); 407 | int dmx = -d.y / grid_resolution; 408 | int dmy = -d.x / grid_resolution; 409 | cv::line(rgb_image, cv::Point(lmx, lmy), cv::Point(lmx + dmx, lmy + dmy), cv::Scalar(0, 0, 255), 1); 410 | 411 | cv::imshow("distance_map", rgb_image); 412 | cv::waitKey(1); 413 | } 414 | } 415 | 416 | // ---------------------------------------------------------------------------------------------------- 417 | 418 | void LocalizationPlugin::laserCallback(const sensor_msgs::LaserScanConstPtr& msg) 419 | { 420 | last_laser_msg_ = msg; 421 | } 422 | 423 | // ---------------------------------------------------------------------------------------------------- 424 | 425 | ED_REGISTER_PLUGIN(LocalizationPlugin) 426 | -------------------------------------------------------------------------------- /src/localization_tf_plugin.cpp: -------------------------------------------------------------------------------- 1 | #include "localization_tf_plugin.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | 11 | // ---------------------------------------------------------------------------------------------------- 12 | 13 | LocalizationTFPlugin::LocalizationTFPlugin() 14 | { 15 | } 16 | 17 | // ---------------------------------------------------------------------------------------------------- 18 | 19 | LocalizationTFPlugin::~LocalizationTFPlugin() 20 | { 21 | } 22 | 23 | // ---------------------------------------------------------------------------------------------------- 24 | 25 | void LocalizationTFPlugin::configure(tue::Configuration config) 26 | { 27 | config.value("robot_name", robot_name_); 28 | } 29 | 30 | // ---------------------------------------------------------------------------------------------------- 31 | 32 | void LocalizationTFPlugin::process(const ed::WorldModel& /*world*/, ed::UpdateRequest& req) 33 | { 34 | try 35 | { 36 | geometry_msgs::TransformStamped ts = tf_buffer_->lookupTransform(robot_name_ + "/base_link", "map", ros::Time(0)); 37 | 38 | geo::Pose3D pose; 39 | geo::convert(ts.transform, pose); 40 | 41 | req.setPose(robot_name_, pose.inverse()); 42 | } 43 | catch(tf2::TransformException& exc) 44 | { 45 | ROS_ERROR_STREAM("ED LocalizationTFPlugin: " << exc.what()); 46 | } 47 | } 48 | 49 | // ---------------------------------------------------------------------------------------------------- 50 | 51 | ED_REGISTER_PLUGIN(LocalizationTFPlugin) 52 | -------------------------------------------------------------------------------- /src/localization_tf_plugin.h: -------------------------------------------------------------------------------- 1 | #ifndef ED_TF_LOCALIZATION_PLUGIN_H_ 2 | #define ED_TF_LOCALIZATION_PLUGIN_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | class LocalizationTFPlugin : public ed::Plugin 12 | { 13 | 14 | public: 15 | 16 | LocalizationTFPlugin(); 17 | 18 | virtual ~LocalizationTFPlugin(); 19 | 20 | void configure(tue::Configuration config); 21 | 22 | void initialize(); 23 | 24 | void process(const ed::WorldModel& world, ed::UpdateRequest& req); 25 | 26 | private: 27 | 28 | std::string robot_name_; 29 | 30 | }; 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /src/odom_model.cpp: -------------------------------------------------------------------------------- 1 | #include "odom_model.h" 2 | 3 | // ---------------------------------------------------------------------------------------------------- 4 | 5 | // Draw randomly from a zero-mean Gaussian distribution, with standard 6 | // deviation sigma. 7 | // We use the polar form of the Box-Muller transformation, explained here: 8 | // http://www.taygeta.com/random/gaussian.html 9 | double generateRandomGaussian(double sigma) 10 | { 11 | double x1, x2, w, r; 12 | 13 | do 14 | { 15 | do { r = drand48(); } while (r==0.0); 16 | x1 = 2.0 * r - 1.0; 17 | do { r = drand48(); } while (r==0.0); 18 | x2 = 2.0 * r - 1.0; 19 | w = x1*x1 + x2*x2; 20 | } while(w > 1.0 || w==0.0); 21 | 22 | return(sigma * x2 * sqrt(-2.0*log(w)/w)); 23 | } 24 | 25 | // ---------------------------------------------------------------------------------------------------- 26 | 27 | OdomModel::OdomModel() : alpha1_(0.2), alpha2_(0.2), alpha3_(0.2), alpha4_(0.2), alpha5_(0.2) 28 | { 29 | } 30 | 31 | // ---------------------------------------------------------------------------------------------------- 32 | 33 | OdomModel::~OdomModel() 34 | { 35 | } 36 | 37 | // ---------------------------------------------------------------------------------------------------- 38 | 39 | void OdomModel::configure(tue::Configuration config) 40 | { 41 | config.value("alpha1", alpha1_); 42 | config.value("alpha2", alpha2_); 43 | config.value("alpha3", alpha3_); 44 | config.value("alpha4", alpha4_); 45 | config.value("alpha5", alpha5_); 46 | } 47 | 48 | // ---------------------------------------------------------------------------------------------------- 49 | 50 | void OdomModel::updatePoses(const geo::Transform2& movement, ParticleFilter& pf) 51 | { 52 | double delta_trans_sq = movement.t.length2(); 53 | 54 | double delta_rot = movement.rotation(); 55 | double delta_rot_sq = delta_rot * delta_rot; 56 | 57 | // Compute noise standard deviations 58 | double trans_hat_stddev = sqrt(alpha3_ * delta_trans_sq + alpha4_ * delta_rot_sq); 59 | double rot_hat_stddev = sqrt(alpha1_ * delta_rot_sq + alpha2_ * delta_trans_sq); 60 | double strafe_hat_stddev = sqrt(alpha4_ * delta_rot_sq + alpha5_ * delta_trans_sq); 61 | 62 | for(std::vector::iterator it = pf.samples().begin(); it != pf.samples().end(); ++it) 63 | { 64 | Sample& sample = *it; 65 | 66 | // Sample pose differences 67 | double delta_trans_hat = generateRandomGaussian(trans_hat_stddev); 68 | double delta_rot_hat = generateRandomGaussian(rot_hat_stddev); 69 | double delta_strafe_hat = generateRandomGaussian(strafe_hat_stddev); 70 | 71 | geo::Transform2 noise; 72 | noise.t = geo::Vec2(delta_trans_hat, delta_strafe_hat); 73 | noise.setRotation(delta_rot_hat); 74 | 75 | sample.pose = sample.pose * movement * noise; 76 | } 77 | } 78 | -------------------------------------------------------------------------------- /src/odom_model.h: -------------------------------------------------------------------------------- 1 | #ifndef ED_LOCALIZATION_ODOM_MODEL_H_ 2 | #define ED_LOCALIZATION_ODOM_MODEL_H_ 3 | 4 | #include "particle_filter.h" 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | class OdomModel 11 | { 12 | 13 | public: 14 | 15 | OdomModel(); 16 | 17 | ~OdomModel(); 18 | 19 | void configure(tue::Configuration config); 20 | 21 | void updatePoses(const geo::Transform2& movement, ParticleFilter& pf); 22 | 23 | private: 24 | 25 | double alpha1_; 26 | double alpha2_; 27 | double alpha3_; 28 | double alpha4_; 29 | double alpha5_; 30 | 31 | }; 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /src/particle_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "particle_filter.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | // ---------------------------------------------------------------------------------------------------- 9 | 10 | ParticleFilter::ParticleFilter() : 11 | min_samples_(0), 12 | max_samples_(0), 13 | kld_err_(0), 14 | kld_z_(0), 15 | alpha_slow_(0), 16 | alpha_fast_(0), 17 | w_slow_(0), 18 | w_fast_(0), 19 | i_current_(0), 20 | kd_tree_(nullptr) 21 | { 22 | } 23 | 24 | // ---------------------------------------------------------------------------------------------------- 25 | 26 | void ParticleFilter::configure(tue::Configuration config) 27 | { 28 | // Extra variable needed as tue::Configuration doesn't support 29 | // unisgned interters 30 | int min = 0, max = 0; 31 | config.value("min_particles", min); 32 | config.value("max_particles", max); 33 | min_samples_ = min; 34 | max_samples_ = max; 35 | 36 | kld_err_ = 0.01; 37 | kld_z_ = 0.99; 38 | config.value("kld_err", kld_err_, tue::config::OPTIONAL); 39 | config.value("kld_z", kld_z_, tue::config::OPTIONAL); 40 | 41 | alpha_slow_ = 0; 42 | alpha_fast_ = 0; 43 | config.value("recovery_alpha_slow", alpha_slow_, tue::config::OPTIONAL); 44 | config.value("recovery_alpha_fast", alpha_fast_, tue::config::OPTIONAL); 45 | 46 | std::array cell_size = {0.5, 0.5, 10*M_PI/180}; 47 | config.value("cell_size_x", cell_size[0], tue::config::OPTIONAL); 48 | config.value("cell_size_y", cell_size[1], tue::config::OPTIONAL); 49 | config.value("cell_size_theta", cell_size[2], tue::config::OPTIONAL); 50 | 51 | samples_[0].reserve(max_samples_); 52 | samples_[1].reserve(max_samples_); 53 | 54 | cluster_cache_.reserve(max_samples_); 55 | 56 | limit_cache_.clear(); 57 | limit_cache_.resize(max_samples_, 0); 58 | 59 | kd_tree_.reset(new KDTree(max_samples_, cell_size)); 60 | 61 | ROS_INFO_STREAM_NAMED("Localization", "min_samples: " << min_samples_ << ", max_samples: " << max_samples_ << std::endl 62 | << "kld_err: " << kld_err_ << ", kld_z: " << kld_z_ << std::endl 63 | << "recovery_alpha_slow: " << alpha_slow_ << ", recovery_alpha_fast: " << alpha_fast_ << std::endl 64 | << "cell_size_x: " << cell_size[0] << ", cell_size_y: " << cell_size[1] << ", cell_size_theta: " << cell_size[2]); 65 | } 66 | 67 | // ---------------------------------------------------------------------------------------------------- 68 | 69 | ParticleFilter::~ParticleFilter() 70 | { 71 | } 72 | 73 | // ---------------------------------------------------------------------------------------------------- 74 | 75 | void ParticleFilter::initUniform(const geo::Vec2& min, const geo::Vec2& max, double a_min, double a_max) 76 | { 77 | clearCache(); 78 | 79 | std::vector& smpls = samples(); 80 | 81 | smpls.clear(); 82 | 83 | const double range_x = max.x - min.x; 84 | const double range_y = max.y - min.y; 85 | const double range_yaw = a_max - a_min; 86 | 87 | const double cbrt_samples = ceil(std::cbrt(max_samples_)); 88 | const double step_x = range_x / cbrt_samples; 89 | const double step_y = range_y / cbrt_samples; 90 | const double step_yaw = range_yaw / cbrt_samples; 91 | 92 | for(double x = min.x; x < max.x; x += step_x) 93 | for(double y = min.y; y < max.y; y += step_y) 94 | for(double a = a_min; a < a_max; a += step_yaw) 95 | smpls.push_back(Sample(geo::Transform2(x, y, a))); 96 | 97 | setUniformWeights(); 98 | 99 | kd_tree_->clear(); 100 | for (const Sample& sample : samples()) 101 | kd_tree_->insert(sample.pose, sample.weight); 102 | } 103 | 104 | // ---------------------------------------------------------------------------------------------------- 105 | 106 | bool compareSamples(const Sample& a, const Sample& b) 107 | { 108 | return a.weight > b.weight; 109 | } 110 | 111 | // ---------------------------------------------------------------------------------------------------- 112 | 113 | void ParticleFilter::resample(std::function gen_random_pose_function) 114 | { 115 | std::vector& old_samples = samples_[i_current_]; 116 | std::vector& new_samples = samples_[1 - i_current_]; 117 | 118 | if (old_samples.empty()) 119 | return; 120 | 121 | // Build up cumulative probability table for resampling. 122 | // TODO: Replace this with a more efficient procedure 123 | // (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html) 124 | std::vector c; 125 | c.resize(old_samples.size() + 1, 0); 126 | for (uint i=0; iclear(); 135 | 136 | // Draw samples from set a to create set b. 137 | new_samples.clear(); 138 | 139 | while(new_samples.size() < max_samples_) 140 | { 141 | new_samples.push_back(Sample()); 142 | Sample& new_sample = new_samples.back(); 143 | 144 | double r = drand48(); 145 | 146 | if(r < w_diff) 147 | new_sample.pose = gen_random_pose_function(); 148 | else 149 | { 150 | // Naive discrete event sampler 151 | uint i=0; 152 | for(; iinsert(new_sample.pose, new_sample.weight); 166 | 167 | // See if we have enough samples yet 168 | if (new_samples.size() >= resampleLimit(kd_tree_->getLeafCount())) 169 | break; 170 | } 171 | 172 | switchSamples(); 173 | 174 | normalize(); 175 | } 176 | 177 | // ---------------------------------------------------------------------------------------------------- 178 | 179 | unsigned int ParticleFilter::resampleLimit(unsigned int k) 180 | { 181 | if (limit_cache_[k-1] != 0) 182 | return limit_cache_[k-1]; 183 | 184 | if (k <= 1) 185 | { 186 | limit_cache_[k-1] = max_samples_; 187 | return max_samples_; 188 | } 189 | 190 | // double a = 1; 191 | double b = 2 / (9 * (static_cast(k - 1))); 192 | double c = sqrt(2 / (9 * (static_cast(k - 1)))) * kld_z_; 193 | double x = 1 - b + c; // x = a - b + c 194 | 195 | unsigned int n = std::ceil((k - 1) / (2 * kld_err_) * x * x * x); 196 | 197 | if (n < min_samples_) 198 | { 199 | limit_cache_[k-1] = min_samples_; 200 | return min_samples_; 201 | } 202 | if (n > max_samples_) 203 | { 204 | limit_cache_[k-1] = min_samples_; 205 | return max_samples_; 206 | } 207 | 208 | limit_cache_[k-1] = n; 209 | return n; 210 | } 211 | 212 | // ---------------------------------------------------------------------------------------------------- 213 | 214 | const Sample& ParticleFilter::bestSample() const 215 | { 216 | const std::vector& smpls = samples(); 217 | 218 | const Sample* best_sample = &smpls.front(); 219 | for(std::vector::const_iterator it = smpls.begin(); it != smpls.end(); ++it) 220 | { 221 | const Sample& s = *it; 222 | if (s.weight > best_sample->weight) 223 | best_sample = &s; 224 | } 225 | 226 | return *best_sample; 227 | } 228 | 229 | // ---------------------------------------------------------------------------------------------------- 230 | 231 | const std::vector& ParticleFilter::clusters() const 232 | { 233 | if (cluster_cache_.empty()) 234 | computeClusterStats(); 235 | 236 | return cluster_cache_; 237 | } 238 | 239 | // ---------------------------------------------------------------------------------------------------- 240 | 241 | geo::Transform2 ParticleFilter::calculateMeanPose() const 242 | { 243 | const std::vector& clstrs = clusters(); 244 | 245 | geo::Transform2 mean(0, 0, 0); 246 | 247 | double max_weight = 0; 248 | for(const Cluster& cluster : clstrs) 249 | { 250 | if (cluster.weight > max_weight) 251 | { 252 | mean = cluster.mean; 253 | max_weight = cluster.weight; 254 | } 255 | } 256 | 257 | return mean; 258 | } 259 | 260 | // ---------------------------------------------------------------------------------------------------- 261 | 262 | void ParticleFilter::normalize(bool update_filter) 263 | { 264 | std::vector& smpls = samples(); 265 | 266 | double total_weight = 0; 267 | for(std::vector::iterator it = smpls.begin(); it != smpls.end(); ++it) 268 | total_weight += it->weight; 269 | 270 | double w_avg = total_weight / samples().size(); 271 | 272 | if (total_weight > 0) 273 | { 274 | if (update_filter) 275 | { 276 | // slow 277 | if(w_slow_ == 0) 278 | w_slow_ = w_avg; 279 | else 280 | w_slow_ += alpha_slow_ * (w_avg - w_slow_); 281 | 282 | // Fast 283 | if(w_fast_ == 0) 284 | w_fast_ = w_avg; 285 | else 286 | w_fast_ += alpha_fast_ * (w_avg - w_fast_); 287 | } 288 | 289 | for(std::vector::iterator it = smpls.begin(); it != smpls.end(); ++it) 290 | it->weight /= total_weight; 291 | } 292 | else 293 | { 294 | setUniformWeights(); 295 | } 296 | } 297 | 298 | // ---------------------------------------------------------------------------------------------------- 299 | 300 | void ParticleFilter::computeClusterStats() const 301 | { 302 | // Cluster the samples 303 | kd_tree_->cluster(); 304 | 305 | // Initialize overall filter stats 306 | double weight = 0; 307 | 308 | // Workspace 309 | std::array m{ {0, 0, 0, 0} }; 310 | std::array, 2> c{ {{0, 0}, {0, 0}} }; 311 | 312 | // Compute cluster stats 313 | for (const Sample& sample : samples()) 314 | { 315 | // Get the cluster label for this sample 316 | int cidx = kd_tree_->getCluster(sample.pose); 317 | if (cidx < 0) 318 | continue; 319 | 320 | if (static_cast(cidx + 1) > cluster_cache_.size()) 321 | cluster_cache_.resize(cidx + 1); 322 | 323 | Cluster& cluster = cluster_cache_[cidx]; 324 | 325 | cluster.count += 1; 326 | cluster.weight += sample.weight; 327 | weight += sample.weight; 328 | 329 | // Compute mean 330 | cluster.m[0] += sample.weight * sample.pose.t.x; 331 | cluster.m[1] += sample.weight * sample.pose.t.y; 332 | cluster.m[2] += sample.weight * cos(sample.pose.rotation()); 333 | cluster.m[3] += sample.weight * sin(sample.pose.rotation()); 334 | 335 | m[0] += cluster.m[0]; 336 | m[1] += cluster.m[1]; 337 | m[2] += cluster.m[2]; 338 | m[3] += cluster.m[3]; 339 | 340 | // Compute covariance in linear components 341 | for (unsigned int j=0; j<2; ++j) 342 | { 343 | for (unsigned int k=0; k<2; ++k) 344 | { 345 | cluster.c[j][k] += sample.weight * sample.pose.t.m[j] * sample.pose.t.m[k]; 346 | c[j][k] += cluster.c[j][k]; 347 | } 348 | } 349 | } 350 | 351 | // Normalize 352 | for (Cluster& cluster : cluster_cache_) 353 | { 354 | cluster.mean.t.x = cluster.m[0] / cluster.weight; 355 | cluster.mean.t.y = cluster.m[1] / cluster.weight; 356 | cluster.mean.setRotation(atan2(cluster.m[3], cluster.m[2])); 357 | 358 | // Covariance in linear components 359 | for (unsigned int j=0; j<2; ++j) 360 | for (unsigned int k=0; k<2; ++k) 361 | cluster.cov.m[j * 3 + k] = cluster.c[j][k] / cluster.weight - cluster.mean.t.m[j] * cluster.mean.t.m[k]; 362 | 363 | // Covariance in angular components 364 | cluster.cov.m[8] = -2 * log(sqrt(cluster.m[2] * cluster.m[2] + cluster.m[3] * cluster.m[3])); 365 | 366 | } 367 | 368 | // Compute overall filter stats 369 | mean_cache_.t.x = m[0] / weight; 370 | mean_cache_.t.y = m[1] / weight; 371 | mean_cache_.setRotation(atan2(m[3], m[2])); 372 | 373 | // Covariance in linear components 374 | for (unsigned int j=0; j<2; ++j) 375 | for (unsigned int k=0; k<2; ++k) 376 | cov_cache_.m[j * 3 + k] = c[j][k] / weight - mean_cache_.t.m[j] * mean_cache_.t.m[k]; 377 | 378 | // Covariance in angular components 379 | cov_cache_.m[8] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3])); 380 | } 381 | 382 | // ---------------------------------------------------------------------------------------------------- 383 | 384 | void ParticleFilter::clearCache() const 385 | { 386 | cluster_cache_.clear(); 387 | mean_cache_ = geo::Transform2::identity(); 388 | cov_cache_ = geo::Mat3::identity(); 389 | } 390 | 391 | // ---------------------------------------------------------------------------------------------------- 392 | 393 | void ParticleFilter::switchSamples() 394 | { 395 | clearCache(); 396 | i_current_ = 1 - i_current_; 397 | } 398 | 399 | // ---------------------------------------------------------------------------------------------------- 400 | 401 | void ParticleFilter::setUniformWeights() 402 | { 403 | double uni_weight = 1.0 / samples().size(); 404 | for(std::vector::iterator it = samples().begin(); it != samples().end(); ++it) 405 | it->weight = uni_weight; 406 | } 407 | 408 | -------------------------------------------------------------------------------- /src/particle_filter.h: -------------------------------------------------------------------------------- 1 | #ifndef ED_LOCALIZATION_PARTICLE_FILTER_H_ 2 | #define ED_LOCALIZATION_PARTICLE_FILTER_H_ 3 | 4 | #include "kdtree.h" 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | // ---------------------------------------------------------------------------------------------------- 13 | 14 | struct Sample 15 | { 16 | Sample(const geo::Transform2& t=geo::Transform2::identity(), const double& w=0) : weight(w), pose(t) 17 | { 18 | } 19 | 20 | double weight; 21 | geo::Transform2 pose; 22 | }; 23 | 24 | // ---------------------------------------------------------------------------------------------------- 25 | 26 | struct Cluster 27 | { 28 | Cluster() : count(0), weight(0), mean(geo::Transform2::identity()), cov(0.f), m({0, 0, 0, 0}), c({ {{0, 0}, {0, 0}} }) 29 | { 30 | } 31 | 32 | // Number of samples 33 | unsigned int count; 34 | 35 | // Total weight of samples in this cluster 36 | double weight; 37 | 38 | // Cluster statistics 39 | geo::Transform2 mean; 40 | geo::Mat3 cov; 41 | 42 | // Workspace 43 | std::array m; 44 | std::array, 2> c; 45 | }; 46 | 47 | // ---------------------------------------------------------------------------------------------------- 48 | 49 | class ParticleFilter 50 | { 51 | 52 | public: 53 | 54 | ParticleFilter(); 55 | 56 | ~ParticleFilter(); 57 | 58 | void configure(tue::Configuration config); 59 | 60 | void initUniform(const geo::Vec2& min, const geo::Vec2& max, double a_min, double a_max); 61 | 62 | void resample(std::function gen_random_pose_function); 63 | 64 | unsigned int resampleLimit(unsigned int number_bins); 65 | 66 | std::vector& samples() { return samples_[i_current_]; } 67 | 68 | const std::vector& samples() const { return samples_[i_current_]; } 69 | 70 | const Sample& bestSample() const; 71 | 72 | const std::vector& clusters() const; 73 | 74 | geo::Transform2 calculateMeanPose() const; 75 | 76 | void normalize(bool update_filter=false); 77 | 78 | private: 79 | 80 | unsigned int min_samples_, max_samples_; 81 | double kld_err_, kld_z_; 82 | double alpha_slow_, alpha_fast_; 83 | 84 | /** 85 | * @brief Current running averages of likelihood of samples 86 | */ 87 | double w_slow_, w_fast_; 88 | 89 | unsigned int i_current_; 90 | std::vector samples_[2]; 91 | 92 | std::unique_ptr kd_tree_; 93 | 94 | mutable std::vector cluster_cache_; 95 | mutable geo::Transform2 mean_cache_; 96 | mutable geo::Mat3 cov_cache_; 97 | 98 | mutable std::vector limit_cache_; 99 | 100 | void computeClusterStats() const; 101 | 102 | void clearCache() const; 103 | 104 | void switchSamples(); 105 | 106 | void setUniformWeights(); 107 | 108 | }; 109 | 110 | #endif 111 | --------------------------------------------------------------------------------