From f9f6cbf56b46b697f24eb388c7adbb2012b72ae0 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 1 Aug 2019 14:18:36 +0900 Subject: [PATCH 1/7] Speed-up global localization --- CMakeLists.txt | 3 ++- include/mcl_3dl/pf.h | 37 ++++++++++++++++++++++++++----------- src/mcl_3dl.cpp | 7 ++++++- test/src/test_pf.cpp | 21 ++++++++++++--------- 4 files changed, 46 insertions(+), 22 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7e041e07..ef4c3e74 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,7 +34,7 @@ catkin_package( ) -add_compile_options(-std=c++11) +add_compile_options(-std=c++17) include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${PCL_INCLUDE_DIR}) add_definitions(${PCL_DEFINITIONS}) @@ -86,6 +86,7 @@ if(CATKIN_ENABLE_TESTING) add_subdirectory(test) find_package(roslint REQUIRED) + set(ROSLINT_CPP_OPTS "--filter=-runtime/references,-build/c++11,-build/c++14") roslint_cpp() roslint_add_test() endif() diff --git a/include/mcl_3dl/pf.h b/include/mcl_3dl/pf.h index 0584cbd6..3b4a1a2d 100644 --- a/include/mcl_3dl/pf.h +++ b/include/mcl_3dl/pf.h @@ -186,7 +186,7 @@ class ParticleFilter const FLT_TYPE prob = 1.0 / particles_.size(); for (size_t i = 0; i < particles_.size(); ++i) { - auto &p = particles_[i]; + auto& p = particles_[i]; const FLT_TYPE pscan = std::nextafter(pstep * (i + 1), static_cast(0.0)); it = std::lower_bound(it, particles_dup_.end(), Particle(pscan)); p.probability_ = prob; @@ -274,14 +274,16 @@ class ParticleFilter } return mean.getMean(); } - std::vector covariance(const FLT_TYPE pass_ratio = 1.0) + std::vector covariance( + const FLT_TYPE pass_ratio = 1.0, + const FLT_TYPE random_sample_ratio = 1.0) { T e = expectation(pass_ratio); FLT_TYPE p_sum = 0; - size_t p_num = 0; std::vector cov; cov.resize(e.size()); + size_t p_num = 0; for (auto& p : particles_) { p_num++; @@ -289,9 +291,26 @@ class ParticleFilter if (p_sum > pass_ratio) break; } - p_sum = 0; - for (auto& p : particles_) + + const size_t num = + std::min( + p_num, + std::max( + size_t(0), + static_cast(p_num * random_sample_ratio))); + std::vector full_indices(p_num); + std::iota(full_indices.begin(), full_indices.end(), 0); + std::vector indices; + indices.reserve(num); + std::sample( + full_indices.begin(), full_indices.end(), + std::back_inserter(indices), num, engine_); + + p_sum = 0.0; + for (size_t i : indices) { + auto& p = particles_[i]; + p_sum += p.probability_; for (size_t j = 0; j < ie_.size(); j++) { for (size_t k = j; k < ie_.size(); k++) @@ -299,17 +318,13 @@ class ParticleFilter cov[k][j] = cov[j][k] += p.state_.covElement(e, j, k) * p.probability_; } } - - p_sum += p.probability_; - if (p_sum > pass_ratio) - break; } + std::cerr << particles_.size() << "/" << p_num << "/" << indices.size() << " (" << p_sum << std::endl; for (size_t j = 0; j < ie_.size(); j++) { - for (size_t k = j; k < ie_.size(); k++) + for (size_t k = 0; k < ie_.size(); k++) { cov[k][j] /= p_sum; - cov[j][k] /= p_sum; } } diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp index d0498b4d..f42cb3d1 100644 --- a/src/mcl_3dl.cpp +++ b/src/mcl_3dl.cpp @@ -655,7 +655,12 @@ class MCL3dlNode if (publish_tf_) tfb_.sendTransform(transforms); - auto cov = pf_->covariance(); + // Calculate covariance from sampled particles to reduce calculation cost on global localization. + // Use the number of original particles or at least 10% of full particles. + auto cov = pf_->covariance( + 1.0, + std::max( + 0.1f, static_cast(params_.num_particles_) / pf_->getParticleSize())); geometry_msgs::PoseWithCovarianceStamped pose; pose.header.stamp = msg->header.stamp; diff --git a/test/src/test_pf.cpp b/test/src/test_pf.cpp index 6726121c..35db93b9 100644 --- a/test/src/test_pf.cpp +++ b/test/src/test_pf.cpp @@ -137,6 +137,9 @@ TEST(Pf, BayesianEstimation) pf.resample(State(0.0)); ASSERT_NEAR(avg, pf.expectation()[0], abs_error); ASSERT_NEAR(var, pf.covariance()[0][0], abs_error); + + // 50% Random sampled covaliance calculation + ASSERT_NEAR(var, pf.covariance(1.0, 0.5)[0][0], abs_error * 2); } } } @@ -202,17 +205,17 @@ TEST(Pf, ResampleFlatLikelihood) TEST(Pf, ResampleFirstParticle) { const std::vector probs = - { - 0.0001f, 0.2f, 0.2f, 0.2f, 0.3999f - }; + { + 0.0001f, 0.2f, 0.2f, 0.2f, 0.3999f + }; const std::vector states = - { - 0.0f, 1.0f, 2.0f, 3.0f, 4.0f - }; + { + 0.0f, 1.0f, 2.0f, 3.0f, 4.0f + }; const std::vector expected_resampled_states = - { - 1.0f, 2.0f, 3.0f, 4.0f, 4.0f - }; + { + 1.0f, 2.0f, 3.0f, 4.0f, 4.0f + }; const size_t particle_num = probs.size(); mcl_3dl::pf::ParticleFilter pf(particle_num); From 2cb37b12395bdc6f650220e25eed56639bedae6c Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 1 Aug 2019 19:14:40 +0900 Subject: [PATCH 2/7] Back to C++11 --- CMakeLists.txt | 2 -- include/mcl_3dl/pf.h | 15 ++++++++------- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ef4c3e74..c4755cee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,7 +34,6 @@ catkin_package( ) -add_compile_options(-std=c++17) include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${PCL_INCLUDE_DIR}) add_definitions(${PCL_DEFINITIONS}) @@ -86,7 +85,6 @@ if(CATKIN_ENABLE_TESTING) add_subdirectory(test) find_package(roslint REQUIRED) - set(ROSLINT_CPP_OPTS "--filter=-runtime/references,-build/c++11,-build/c++14") roslint_cpp() roslint_add_test() endif() diff --git a/include/mcl_3dl/pf.h b/include/mcl_3dl/pf.h index 3b4a1a2d..15e04de1 100644 --- a/include/mcl_3dl/pf.h +++ b/include/mcl_3dl/pf.h @@ -298,13 +298,14 @@ class ParticleFilter std::max( size_t(0), static_cast(p_num * random_sample_ratio))); - std::vector full_indices(p_num); - std::iota(full_indices.begin(), full_indices.end(), 0); - std::vector indices; - indices.reserve(num); - std::sample( - full_indices.begin(), full_indices.end(), - std::back_inserter(indices), num, engine_); + + std::vector indices(p_num); + std::iota(indices.begin(), indices.end(), 0); + if (random_sample_ratio < 1.0) + { + std::shuffle(indices.begin(), indices.end(), engine_); + indices.resize(num); + } p_sum = 0.0; for (size_t i : indices) From dba43f970ac4d1e0ca3b2d70ea916b09010efef5 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 1 Aug 2019 19:16:12 +0900 Subject: [PATCH 3/7] Remove debug output --- include/mcl_3dl/pf.h | 1 - 1 file changed, 1 deletion(-) diff --git a/include/mcl_3dl/pf.h b/include/mcl_3dl/pf.h index 15e04de1..9490b694 100644 --- a/include/mcl_3dl/pf.h +++ b/include/mcl_3dl/pf.h @@ -320,7 +320,6 @@ class ParticleFilter } } } - std::cerr << particles_.size() << "/" << p_num << "/" << indices.size() << " (" << p_sum << std::endl; for (size_t j = 0; j < ie_.size(); j++) { for (size_t k = 0; k < ie_.size(); k++) From 61bd2a10ac21e20c49a4549708d0c5259949a94f Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 1 Aug 2019 19:22:40 +0900 Subject: [PATCH 4/7] Fix CMakeLists --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index c4755cee..7e041e07 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,6 +34,7 @@ catkin_package( ) +add_compile_options(-std=c++11) include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${PCL_INCLUDE_DIR}) add_definitions(${PCL_DEFINITIONS}) From 1b4035a2240c53dcce46237f66e9615b73a716e7 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 1 Aug 2019 19:23:05 +0900 Subject: [PATCH 5/7] Minor fix in ChunkedKdtree --- include/mcl_3dl/chunked_kdtree.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/mcl_3dl/chunked_kdtree.h b/include/mcl_3dl/chunked_kdtree.h index eb27dac4..92735bd8 100644 --- a/include/mcl_3dl/chunked_kdtree.h +++ b/include/mcl_3dl/chunked_kdtree.h @@ -71,7 +71,7 @@ class ChunkedKdtree { return (id.x_) ^ (id.y_ << 11) ^ (id.z_ << 22); } - ChunkId(const int& x, const int& y, const int& z) + ChunkId(const int x, const int y, const int z) : x_(x) , y_(y) , z_(z) @@ -228,7 +228,7 @@ class ChunkedKdtree return ret; } - typename pcl::KdTreeFLANN::Ptr getChunkKdtree(const POINT_TYPE p) + typename pcl::KdTreeFLANN::Ptr getChunkKdtree(const POINT_TYPE& p) { return getChunkKdtree(getChunkId(p)); } @@ -238,7 +238,7 @@ class ChunkedKdtree return typename pcl::KdTreeFLANN::Ptr(); return chunks_[c].kdtree_; } - typename pcl::PointCloud::Ptr getChunkCloud(const POINT_TYPE p) + typename pcl::PointCloud::Ptr getChunkCloud(const POINT_TYPE& p) { return getChunkCloud(getChunkId(p)); } @@ -248,7 +248,7 @@ class ChunkedKdtree return typename pcl::PointCloud::Ptr(); return chunks_[c].cloud_; } - ChunkId getChunkId(const POINT_TYPE p) const + ChunkId getChunkId(const POINT_TYPE& p) const { return ChunkId(static_cast(floor(p.x * pos_to_chunk_)), static_cast(floor(p.y * pos_to_chunk_)), From 34efdbd0b60137793a42f9b1a9e746aab127d527 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 1 Aug 2019 19:35:39 +0900 Subject: [PATCH 6/7] Fix returned particle number --- src/mcl_3dl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp index f42cb3d1..f85338a5 100644 --- a/src/mcl_3dl.cpp +++ b/src/mcl_3dl.cpp @@ -1052,7 +1052,7 @@ class MCL3dlNode } } response.success = true; - response.message = std::to_string(points->size()) + " particles"; + response.message = std::to_string(pf_->getParticleSize()) + " particles"; return true; } From 007ad29efd6903ce698eab0f20d2893af1d09fa6 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Fri, 2 Aug 2019 12:28:59 +0900 Subject: [PATCH 7/7] Address review comment --- include/mcl_3dl/pf.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/include/mcl_3dl/pf.h b/include/mcl_3dl/pf.h index 9490b694..c4adef28 100644 --- a/include/mcl_3dl/pf.h +++ b/include/mcl_3dl/pf.h @@ -292,19 +292,19 @@ class ParticleFilter break; } - const size_t num = - std::min( - p_num, - std::max( - size_t(0), - static_cast(p_num * random_sample_ratio))); - std::vector indices(p_num); std::iota(indices.begin(), indices.end(), 0); if (random_sample_ratio < 1.0) { std::shuffle(indices.begin(), indices.end(), engine_); - indices.resize(num); + + const size_t sample_num = + std::min( + p_num, + std::max( + size_t(0), + static_cast(p_num * random_sample_ratio))); + indices.resize(sample_num); } p_sum = 0.0;