From e619675ee915ca3ca153482d5d9d734b5fcde3c8 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Sun, 9 Sep 2018 16:57:03 +0900 Subject: [PATCH 01/29] Make lidar measurement model class --- .../mcl_3dl/lidar_measurement_model_base.h | 68 ++ .../lidar_measurement_model_likelihood.h | 166 +++++ include/mcl_3dl/point_cloud_random_sampler.h | 77 +++ include/mcl_3dl/state_6dof.h | 276 ++++++++ src/mcl_3dl.cpp | 624 +++++------------- 5 files changed, 770 insertions(+), 441 deletions(-) create mode 100644 include/mcl_3dl/lidar_measurement_model_base.h create mode 100644 include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h create mode 100644 include/mcl_3dl/point_cloud_random_sampler.h create mode 100644 include/mcl_3dl/state_6dof.h diff --git a/include/mcl_3dl/lidar_measurement_model_base.h b/include/mcl_3dl/lidar_measurement_model_base.h new file mode 100644 index 00000000..81015178 --- /dev/null +++ b/include/mcl_3dl/lidar_measurement_model_base.h @@ -0,0 +1,68 @@ +/* + * Copyright (c) 2018, the mcl_3dl authors + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef MCL_3DL_LIDAR_MEASUREMENT_MODEL_BASE_H +#define MCL_3DL_LIDAR_MEASUREMENT_MODEL_BASE_H + +#include + +#include +#include + +#include + +namespace mcl_3dl +{ +template +class LidarMeasurementModelBase +{ + static_assert( + std::is_base_of, STATE_TYPE>::value, + "STATE_TYPE is not based on pf::ParticleBase"); + +public: + using Ptr = std::shared_ptr>; + + virtual void loadConfig( + const ros::NodeHandle &nh, + const std::string &name) = 0; + virtual void setGlobalLocalizationStatus( + size_t, size_t) = 0; + + virtual typename pcl::PointCloud::Ptr filter( + const typename pcl::PointCloud::Ptr &) const = 0; + + virtual std::pair measure( + typename mcl_3dl::ChunkedKdtree::Ptr &, + const typename pcl::PointCloud::Ptr &, + const STATE_TYPE &) const = 0; +}; +} // namespace mcl_3dl + +#endif // MCL_3DL_LIDAR_MEASUREMENT_MODEL_BASE_H diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h new file mode 100644 index 00000000..96d04dfc --- /dev/null +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h @@ -0,0 +1,166 @@ +/* + * Copyright (c) 2018, the mcl_3dl authors + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDEDNode BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef MCL_3DL_LIDAR_MEASUREMENT_MODEL_LIDAR_MEASUREMENT_MODEL_LIKELIHOOD_H +#define MCL_3DL_LIDAR_MEASUREMENT_MODEL_LIDAR_MEASUREMENT_MODEL_LIKELIHOOD_H + +#include + +#include +#include + +#include +#include + +namespace mcl_3dl +{ +template +class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase +{ +private: + size_t num_points_; + size_t num_points_default_; + size_t num_points_global_; + float clip_far_sq_; + float clip_near_sq_; + float clip_z_min_; + float clip_z_max_; + float match_weight_; + float match_dist_min_; + float match_dist_flat_; + + PointCloudRandomSampler sampler_; + +public: + void loadConfig( + const ros::NodeHandle &nh, + const std::string &name) + { + ros::NodeHandle pnh(nh, name); + + int num_points, num_points_global; + pnh.param("num_points", num_points, 96); + pnh.param("num_points_global", num_points_global, 8); + num_points_default_ = num_points_ = num_points; + num_points_global_ = num_points_global; + + double clip_near, clip_far; + pnh.param("clip_near", clip_near, 0.5); + pnh.param("clip_far", clip_far, 10.0); + clip_near_sq_ = clip_near * clip_near; + clip_far_sq_ = clip_far * clip_far; + + double clip_z_min, clip_z_max; + pnh.param("clip_z_min", clip_z_min, -2.0); + pnh.param("clip_z_max", clip_z_max, 2.0); + clip_z_min_ = clip_z_min; + clip_z_max_ = clip_z_max; + + double match_weight; + pnh.param("match_weight", match_weight, 5.0); + match_weight_ = match_weight; + + double match_dist_min, match_dist_flat; + pnh.param("match_dist_min", match_dist_min, 0.2); + pnh.param("match_dist_flat", match_dist_flat, 0.05); + match_dist_min_ = match_dist_min; + match_dist_flat_ = match_dist_flat; + } + void setGlobalLocalizationStatus( + size_t num_particles, + size_t current_num_particles) + { + if (current_num_particles <= num_particles) + { + num_points_ = num_points_default_; + return; + } + size_t num = num_points_; + if (current_num_particles > num_particles) + num = num * num_particles / current_num_particles; + if (num < num_points_global_) + num = num_points_global_; + + num_points_ = num; + } + + typename pcl::PointCloud::Ptr filter( + const typename pcl::PointCloud::Ptr &pc) const + { + const auto local_points_filter = [this](const POINT_TYPE &p) + { + if (p.x * p.x + p.y * p.y > clip_far_sq_) + return true; + if (p.x * p.x + p.y * p.y < clip_near_sq_) + return true; + if (p.z < clip_z_min_ || clip_z_max_ < p.z) + return true; + return false; + }; + pc->erase( + std::remove_if(pc->begin(), pc->end(), local_points_filter), pc->end()); + pc->width = 1; + pc->height = pc->points.size(); + + return sampler_.sample(pc, num_points_); + } + + std::pair measure( + typename mcl_3dl::ChunkedKdtree::Ptr &kdtree, + const typename pcl::PointCloud::Ptr &pc, + const STATE_TYPE &s) const + { + pcl::PointCloud::Ptr pc_particle(new pcl::PointCloud); + std::vector id(1); + std::vector sqdist(1); + + float score_like = 0; + *pc_particle = *pc; + s.transform(*pc_particle); + size_t num = 0; + for (auto &p : pc_particle->points) + { + if (kdtree->radiusSearch(p, match_dist_min_, id, sqdist, 1)) + { + const float dist = match_dist_min_ - std::max(sqrtf(sqdist[0]), match_dist_flat_); + if (dist < 0.0) + continue; + + score_like += dist * match_weight_; + num++; + } + } + const float match_ratio = static_cast(num) / pc_particle->points.size(); + + return std::pair(score_like, match_ratio); + } +}; +} // namespace mcl_3dl + +#endif // MCL_3DL_LIDAR_MEASUREMENT_MODEL_LIDAR_MEASUREMENT_MODEL_LIKELIHOOD_H diff --git a/include/mcl_3dl/point_cloud_random_sampler.h b/include/mcl_3dl/point_cloud_random_sampler.h new file mode 100644 index 00000000..29b449ce --- /dev/null +++ b/include/mcl_3dl/point_cloud_random_sampler.h @@ -0,0 +1,77 @@ +/* + * Copyright (c) 2018, the mcl_3dl authors + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef MCL_3DL_POINT_CLOUD_RANDOM_SAMPLER_H +#define MCL_3DL_POINT_CLOUD_RANDOM_SAMPLER_H + +#include + +#include +#include + +namespace mcl_3dl +{ +template +class PointCloudRandomSampler +{ +private: + std::random_device seed_gen_; + std::shared_ptr engine_; + +public: + PointCloudRandomSampler() + : engine_(new std::default_random_engine(seed_gen_())) + { + } + typename pcl::PointCloud::Ptr sample( + const typename pcl::PointCloud::Ptr &pc, + const size_t num) const + { + if (pc->points.size() == 0) + return typename pcl::PointCloud::Ptr(); + + typename pcl::PointCloud::Ptr output(new pcl::PointCloud); + output->points.resize(num); + + std::uniform_int_distribution ud(0, pc->points.size() - 1); + + for (size_t i = 0; i < num; i++) + { + output->points.push_back(pc->points[ud(*engine_)]); + } + output->width = 1; + output->height = output->points.size(); + output->header = pc->header; + + return output; + } +}; +} // namespace mcl_3dl + +#endif // MCL_3DL_POINT_CLOUD_RANDOM_SAMPLER_H diff --git a/include/mcl_3dl/state_6dof.h b/include/mcl_3dl/state_6dof.h new file mode 100644 index 00000000..4819c522 --- /dev/null +++ b/include/mcl_3dl/state_6dof.h @@ -0,0 +1,276 @@ +/* + * Copyright (c) 2018, the mcl_3dl authors + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef MCL_3DL_STATE_6DOF_H +#define MCL_3DL_STATE_6DOF_H + +#include +#include +#include + +namespace mcl_3dl +{ +class State6DOF : public mcl_3dl::pf::ParticleBase +{ +public: + mcl_3dl::Vec3 pos; + mcl_3dl::Quat rot; + bool diff; + float noise_ll_; + float noise_la_; + float noise_al_; + float noise_aa_; + mcl_3dl::Vec3 odom_err_integ_lin; + mcl_3dl::Vec3 odom_err_integ_ang; + class RPYVec + { + public: + mcl_3dl::Vec3 v; + RPYVec() + { + } + explicit RPYVec(const mcl_3dl::Vec3 &v) + { + this->v = v; + } + RPYVec(const float &r, const float &p, const float y) + { + this->v.x = r; + this->v.y = p; + this->v.z = y; + } + }; + RPYVec rpy; + float &operator[](const size_t i)override + { + switch (i) + { + case 0: + return pos.x; + case 1: + return pos.y; + case 2: + return pos.z; + case 3: + return rot.x; + case 4: + return rot.y; + case 5: + return rot.z; + case 6: + return rot.w; + case 7: + return odom_err_integ_lin.x; + case 8: + return odom_err_integ_lin.y; + case 9: + return odom_err_integ_lin.z; + case 10: + return odom_err_integ_ang.x; + case 11: + return odom_err_integ_ang.y; + case 12: + return odom_err_integ_ang.z; + } + return pos.x; + } + const float &operator[](const size_t i) const + { + switch (i) + { + case 0: + return pos.x; + case 1: + return pos.y; + case 2: + return pos.z; + case 3: + return rot.x; + case 4: + return rot.y; + case 5: + return rot.z; + case 6: + return rot.w; + case 7: + return odom_err_integ_lin.x; + case 8: + return odom_err_integ_lin.y; + case 9: + return odom_err_integ_lin.z; + case 10: + return odom_err_integ_ang.x; + case 11: + return odom_err_integ_ang.y; + case 12: + return odom_err_integ_ang.z; + } + return pos.x; + } + size_t size() const override + { + return 10; + } + void normalize() override + { + rot.normalize(); + } + State6DOF() + { + diff = false; + noise_ll_ = noise_la_ = noise_aa_ = noise_al_ = 0.0; + odom_err_integ_lin = mcl_3dl::Vec3(0.0, 0.0, 0.0); + odom_err_integ_ang = mcl_3dl::Vec3(0.0, 0.0, 0.0); + } + State6DOF(const mcl_3dl::Vec3 pos, const mcl_3dl::Quat rot) + { + this->pos = pos; + this->rot = rot; + noise_ll_ = noise_la_ = noise_aa_ = noise_al_ = 0.0; + odom_err_integ_lin = mcl_3dl::Vec3(0.0, 0.0, 0.0); + odom_err_integ_ang = mcl_3dl::Vec3(0.0, 0.0, 0.0); + diff = false; + } + State6DOF(const mcl_3dl::Vec3 pos, const mcl_3dl::Vec3 rpy) + { + this->pos = pos; + this->rpy = RPYVec(rpy); + noise_ll_ = noise_la_ = noise_aa_ = noise_al_ = 0.0; + odom_err_integ_lin = mcl_3dl::Vec3(0.0, 0.0, 0.0); + odom_err_integ_ang = mcl_3dl::Vec3(0.0, 0.0, 0.0); + diff = true; + } + bool isDiff() + { + return diff; + } + void transform(pcl::PointCloud &pc) const + { + auto r = rot.normalized(); + for (auto &p : pc.points) + { + auto t = r * mcl_3dl::Vec3(p.x, p.y, p.z) + pos; + p.x = t.x; + p.y = t.y; + p.z = t.z; + } + } + static State6DOF generateNoise( + std::default_random_engine &engine_, + State6DOF mean, State6DOF sigma) + { + State6DOF noise; + if (mean.isDiff() || !sigma.isDiff()) + { + ROS_ERROR("Failed to generate noise. mean must be mcl_3dl::Quat and sigma must be rpy vec."); + } + for (size_t i = 0; i < 3; i++) + { + std::normal_distribution nd(mean[i], sigma[i]); + noise[i] = noise[i + 7] = nd(engine_); + } + mcl_3dl::Vec3 rpy_noise; + for (size_t i = 0; i < 3; i++) + { + std::normal_distribution nd(0.0, sigma.rpy.v[i]); + rpy_noise[i] = noise[i + 10] = nd(engine_); + } + noise.rot = mcl_3dl::Quat(rpy_noise) * mean.rot; + return noise; + } + State6DOF operator+(const State6DOF &a) const + { + State6DOF in = a; + State6DOF ret; + for (size_t i = 0; i < size(); i++) + { + if (3 <= i && i <= 6) + continue; + ret[i] = (*this)[i] + in[i]; + } + ret.rot = a.rot * rot; + return ret; + } + State6DOF operator-(const State6DOF &a) const + { + State6DOF in = a; + State6DOF ret; + for (size_t i = 0; i < size(); i++) + { + if (3 <= i && i <= 6) + continue; + ret[i] = (*this)[i] - in[i]; + } + ret.rot = a.rot * rot.inv(); + return ret; + } +}; +class ParticleWeightedMeanQuat : public mcl_3dl::pf::ParticleWeightedMean +{ +protected: + mcl_3dl::Vec3 front_sum_; + mcl_3dl::Vec3 up_sum_; + +public: + ParticleWeightedMeanQuat() + : ParticleWeightedMean() + , front_sum_(0.0, 0.0, 0.0) + , up_sum_(0.0, 0.0, 0.0) + { + } + + void add(const State6DOF &s, const float &prob) + { + p_sum_ += prob; + + State6DOF e1 = s; + e_.pos += e1.pos * prob; + + const mcl_3dl::Vec3 front = s.rot * mcl_3dl::Vec3(1.0, 0.0, 0.0) * prob; + const mcl_3dl::Vec3 up = s.rot * mcl_3dl::Vec3(0.0, 0.0, 1.0) * prob; + + front_sum_ += front; + up_sum_ += up; + } + + State6DOF getMean() + { + assert(p_sum_ > 0.0); + + return State6DOF(e_.pos / p_sum_, mcl_3dl::Quat(front_sum_, up_sum_)); + } + + float getTotalProbability() + { + return p_sum_; + } +}; +} // namespace mcl_3dl + +#endif // MCL_3DL_STATE_6DOF_H diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp index cc664d8d..2850b2a8 100644 --- a/src/mcl_3dl.cpp +++ b/src/mcl_3dl.cpp @@ -62,16 +62,21 @@ #include #include -#include -#include -#include +#include #include +#include +#include #include +#include +#include #include -#include +#include +#include #include +namespace mcl_3dl +{ class MCL3dlNode { protected: @@ -151,242 +156,7 @@ class MCL3dlNode double lpf_step; }; - class State : public mcl_3dl::pf::ParticleBase - { - public: - mcl_3dl::Vec3 pos; - mcl_3dl::Quat rot; - bool diff; - float noise_ll_; - float noise_la_; - float noise_al_; - float noise_aa_; - mcl_3dl::Vec3 odom_err_integ_lin; - mcl_3dl::Vec3 odom_err_integ_ang; - class RPYVec - { - public: - mcl_3dl::Vec3 v; - RPYVec() - { - } - explicit RPYVec(const mcl_3dl::Vec3 &v) - { - this->v = v; - } - RPYVec(const float &r, const float &p, const float y) - { - this->v.x = r; - this->v.y = p; - this->v.z = y; - } - }; - RPYVec rpy; - float &operator[](const size_t i)override - { - switch (i) - { - case 0: - return pos.x; - case 1: - return pos.y; - case 2: - return pos.z; - case 3: - return rot.x; - case 4: - return rot.y; - case 5: - return rot.z; - case 6: - return rot.w; - case 7: - return odom_err_integ_lin.x; - case 8: - return odom_err_integ_lin.y; - case 9: - return odom_err_integ_lin.z; - case 10: - return odom_err_integ_ang.x; - case 11: - return odom_err_integ_ang.y; - case 12: - return odom_err_integ_ang.z; - } - return pos.x; - } - const float &operator[](const size_t i) const - { - switch (i) - { - case 0: - return pos.x; - case 1: - return pos.y; - case 2: - return pos.z; - case 3: - return rot.x; - case 4: - return rot.y; - case 5: - return rot.z; - case 6: - return rot.w; - case 7: - return odom_err_integ_lin.x; - case 8: - return odom_err_integ_lin.y; - case 9: - return odom_err_integ_lin.z; - case 10: - return odom_err_integ_ang.x; - case 11: - return odom_err_integ_ang.y; - case 12: - return odom_err_integ_ang.z; - } - return pos.x; - } - size_t size() const override - { - return 10; - } - void normalize() override - { - rot.normalize(); - } - State() - { - diff = false; - noise_ll_ = noise_la_ = noise_aa_ = noise_al_ = 0.0; - odom_err_integ_lin = mcl_3dl::Vec3(0.0, 0.0, 0.0); - odom_err_integ_ang = mcl_3dl::Vec3(0.0, 0.0, 0.0); - } - State(const mcl_3dl::Vec3 pos, const mcl_3dl::Quat rot) - { - this->pos = pos; - this->rot = rot; - noise_ll_ = noise_la_ = noise_aa_ = noise_al_ = 0.0; - odom_err_integ_lin = mcl_3dl::Vec3(0.0, 0.0, 0.0); - odom_err_integ_ang = mcl_3dl::Vec3(0.0, 0.0, 0.0); - diff = false; - } - State(const mcl_3dl::Vec3 pos, const mcl_3dl::Vec3 rpy) - { - this->pos = pos; - this->rpy = RPYVec(rpy); - noise_ll_ = noise_la_ = noise_aa_ = noise_al_ = 0.0; - odom_err_integ_lin = mcl_3dl::Vec3(0.0, 0.0, 0.0); - odom_err_integ_ang = mcl_3dl::Vec3(0.0, 0.0, 0.0); - diff = true; - } - bool isDiff() - { - return diff; - } - void transform(pcl::PointCloud &pc) const - { - auto r = rot.normalized(); - for (auto &p : pc.points) - { - auto t = r * mcl_3dl::Vec3(p.x, p.y, p.z) + pos; - p.x = t.x; - p.y = t.y; - p.z = t.z; - } - } - static State generateNoise( - std::default_random_engine &engine_, - State mean, State sigma) - { - State noise; - if (mean.isDiff() || !sigma.isDiff()) - { - ROS_ERROR("Failed to generate noise. mean must be mcl_3dl::Quat and sigma must be rpy vec."); - } - for (size_t i = 0; i < 3; i++) - { - std::normal_distribution nd(mean[i], sigma[i]); - noise[i] = noise[i + 7] = nd(engine_); - } - mcl_3dl::Vec3 rpy_noise; - for (size_t i = 0; i < 3; i++) - { - std::normal_distribution nd(0.0, sigma.rpy.v[i]); - rpy_noise[i] = noise[i + 10] = nd(engine_); - } - noise.rot = mcl_3dl::Quat(rpy_noise) * mean.rot; - return noise; - } - State operator+(const State &a) const - { - State in = a; - State ret; - for (size_t i = 0; i < size(); i++) - { - if (3 <= i && i <= 6) - continue; - ret[i] = (*this)[i] + in[i]; - } - ret.rot = a.rot * rot; - return ret; - } - State operator-(const State &a) const - { - State in = a; - State ret; - for (size_t i = 0; i < size(); i++) - { - if (3 <= i && i <= 6) - continue; - ret[i] = (*this)[i] - in[i]; - } - ret.rot = a.rot * rot.inv(); - return ret; - } - }; - class ParticleWeightedMeanQuat : public mcl_3dl::pf::ParticleWeightedMean - { - protected: - mcl_3dl::Vec3 front_sum_; - mcl_3dl::Vec3 up_sum_; - - public: - ParticleWeightedMeanQuat() - : ParticleWeightedMean() - , front_sum_(0.0, 0.0, 0.0) - , up_sum_(0.0, 0.0, 0.0) - { - } - - void add(const State &s, const float &prob) - { - p_sum_ += prob; - - State e1 = s; - e_.pos += e1.pos * prob; - - const mcl_3dl::Vec3 front = s.rot * mcl_3dl::Vec3(1.0, 0.0, 0.0) * prob; - const mcl_3dl::Vec3 up = s.rot * mcl_3dl::Vec3(0.0, 0.0, 1.0) * prob; - - front_sum_ += front; - up_sum_ += up; - } - - State getMean() - { - assert(p_sum_ > 0.0); - - return State(e_.pos / p_sum_, mcl_3dl::Quat(front_sum_, up_sum_)); - } - - float getTotalProbability() - { - return p_sum_; - } - }; - std::shared_ptr> pf_; + std::shared_ptr> pf_; class MyPointRepresentation : public pcl::PointRepresentation { @@ -463,24 +233,24 @@ class MCL3dlNode return; } pf_->init( - State( - mcl_3dl::Vec3(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z), - mcl_3dl::Quat(pose.pose.orientation.x, - pose.pose.orientation.y, - pose.pose.orientation.z, - pose.pose.orientation.w)), - State( - mcl_3dl::Vec3(msg->pose.covariance[0], - msg->pose.covariance[6 * 1 + 1], - msg->pose.covariance[6 * 2 + 2]), - mcl_3dl::Vec3(msg->pose.covariance[6 * 3 + 3], - msg->pose.covariance[6 * 4 + 4], - msg->pose.covariance[6 * 5 + 5]))); + State6DOF( + Vec3(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z), + Quat(pose.pose.orientation.x, + pose.pose.orientation.y, + pose.pose.orientation.z, + pose.pose.orientation.w)), + State6DOF( + Vec3(msg->pose.covariance[0], + msg->pose.covariance[6 * 1 + 1], + msg->pose.covariance[6 * 2 + 2]), + Vec3(msg->pose.covariance[6 * 3 + 3], + msg->pose.covariance[6 * 4 + 4], + msg->pose.covariance[6 * 5 + 5]))); pc_update_.reset(); - auto integ_reset_func = [](State &s) + auto integ_reset_func = [](State6DOF &s) { - s.odom_err_integ_lin = mcl_3dl::Vec3(); - s.odom_err_integ_ang = mcl_3dl::Vec3(); + s.odom_err_integ_lin = Vec3(); + s.odom_err_integ_ang = Vec3(); }; pf_->predict(integ_reset_func); } @@ -488,14 +258,14 @@ class MCL3dlNode void cbOdom(const nav_msgs::Odometry::ConstPtr &msg) { odom_ = - State( - mcl_3dl::Vec3(msg->pose.pose.position.x, - msg->pose.pose.position.y, - msg->pose.pose.position.z), - mcl_3dl::Quat(msg->pose.pose.orientation.x, - msg->pose.pose.orientation.y, - msg->pose.pose.orientation.z, - msg->pose.pose.orientation.w)); + State6DOF( + Vec3(msg->pose.pose.position.x, + msg->pose.pose.position.y, + msg->pose.pose.position.z), + Quat(msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z, + msg->pose.pose.orientation.w)); if (!has_odom_) { odom_prev_ = odom_; @@ -512,22 +282,22 @@ class MCL3dlNode } else if (dt > 0.05) { - const mcl_3dl::Vec3 v = odom_prev_.rot.inv() * (odom_.pos - odom_prev_.pos); - const mcl_3dl::Quat r = odom_prev_.rot.inv() * odom_.rot; - mcl_3dl::Vec3 axis; + const Vec3 v = odom_prev_.rot.inv() * (odom_.pos - odom_prev_.pos); + const Quat r = odom_prev_.rot.inv() * odom_.rot; + Vec3 axis; float ang; r.getAxisAng(axis, ang); const float trans = v.norm(); - auto prediction_func = [this, &v, &r, axis, ang, trans, &dt](State &s) + auto prediction_func = [this, &v, &r, axis, ang, trans, &dt](State6DOF &s) { - const mcl_3dl::Vec3 diff = v * (1.0 + s.noise_ll_) + mcl_3dl::Vec3(s.noise_al_ * ang, 0.0, 0.0); + const Vec3 diff = v * (1.0 + s.noise_ll_) + Vec3(s.noise_al_ * ang, 0.0, 0.0); s.odom_err_integ_lin += (diff - v); s.pos += s.rot * diff; const float yaw_diff = s.noise_la_ * trans + s.noise_aa_ * ang; - s.rot = mcl_3dl::Quat(mcl_3dl::Vec3(0.0, 0.0, 1.0), yaw_diff) * s.rot * r; + s.rot = Quat(Vec3(0.0, 0.0, 1.0), yaw_diff) * s.rot * r; s.rot.normalize(); - s.odom_err_integ_ang += mcl_3dl::Vec3(0.0, 0.0, yaw_diff); + s.odom_err_integ_ang += Vec3(0.0, 0.0, yaw_diff); s.odom_err_integ_lin *= (1.0 - dt / params_.odom_err_integ_lin_tc); s.odom_err_integ_ang *= (1.0 - dt / params_.odom_err_integ_ang_tc); }; @@ -602,7 +372,7 @@ class MCL3dlNode try { if (!pcl_ros::transformPointCloud( - frame_ids_["base_link"], *pc_local_accum_, *pc_local_accum_, tfl_)) + frame_ids_["base_link"], *pc_local_accum_, *pc_local_accum_, tfl_)) { ROS_INFO("Failed to transform pointcloud."); pc_local_accum_.reset(new pcl::PointCloud); @@ -617,7 +387,7 @@ class MCL3dlNode pc_accum_header_.clear(); return; } - std::vector origins; + std::vector origins; for (auto &h : pc_accum_header_) { try @@ -627,7 +397,7 @@ class MCL3dlNode h.frame_id, h.stamp, frame_ids_["odom"], trans); auto origin = trans.getOrigin(); - origins.push_back(mcl_3dl::Vec3(origin.x(), origin.y(), origin.z())); + origins.push_back(Vec3(origin.x(), origin.y(), origin.z())); } catch (tf::TransformException &e) { @@ -640,27 +410,13 @@ class MCL3dlNode const auto ts = boost::chrono::high_resolution_clock::now(); - pcl::PointCloud::Ptr pc_local(new pcl::PointCloud); + pcl::PointCloud::Ptr pc_local_full(new pcl::PointCloud); pcl::VoxelGrid ds; ds.setInputCloud(pc_local_accum_); ds.setLeafSize(params_.downsample_x, params_.downsample_y, params_.downsample_z); - ds.filter(*pc_local); - - pcl::PointCloud::Ptr pc_local_full(new pcl::PointCloud); - *pc_local_full = *pc_local; + ds.filter(*pc_local_full); std::uniform_real_distribution ud(0.0, 1.0); - int num_valid = 0; - for (auto &p : pc_local->points) - { - if (p.x * p.x + p.y * p.y > params_.clip_far_sq) - continue; - if (p.x * p.x + p.y * p.y < params_.clip_near_sq) - continue; - if (p.z < params_.clip_z_min || params_.clip_z_max < p.z) - continue; - num_valid++; - } auto random_sample = [this]( const pcl::PointCloud::Ptr &pc, const size_t &num) -> pcl::PointCloud::Ptr @@ -679,40 +435,21 @@ class MCL3dlNode return output; }; - auto local_points_filter = [this](const pcl::PointXYZI &p) + std::map::Ptr> pc_locals; + for (auto &lm : lidar_measurements_) { - if (p.x * p.x + p.y * p.y > params_.clip_far_sq) - return true; - if (p.x * p.x + p.y * p.y < params_.clip_near_sq) - return true; - if (p.z < params_.clip_z_min || params_.clip_z_max < p.z) - return true; - return false; - }; - pc_local->erase( - std::remove_if(pc_local->begin(), pc_local->end(), local_points_filter), - pc_local->end()); - if (pc_local->size() == 0) + lm.second->setGlobalLocalizationStatus( + params_.num_particles, pf_->getParticleSize()); + pc_locals[lm.first] = lm.second->filter(pc_local_full); + } + + if (pc_locals["likelihood"]->size() == 0) { ROS_ERROR("All points are filtered out. Failed to localize."); return; } - if (static_cast(pf_->getParticleSize()) > params_.num_particles) - { - size_t num = params_.num_points; - if (static_cast(pf_->getParticleSize()) > params_.num_particles) - num = num * params_.num_particles / pf_->getParticleSize(); - if (static_cast(num) < params_.num_points_global) - num = params_.num_points_global; - pc_local = random_sample(pc_local, static_cast(num)); - } - else - { - pc_local = random_sample(pc_local, static_cast(params_.num_points)); - } - pcl::PointCloud::Ptr pc_local_beam(new pcl::PointCloud); - *pc_local_beam = *pc_local; + *pc_local_beam = *pc_local_full; auto local_beam_filter = [this](const pcl::PointXYZI &p) { if (p.x * p.x + p.y * p.y > params_.clip_beam_far_sq) @@ -740,9 +477,7 @@ class MCL3dlNode pc_local_beam = random_sample(pc_local_beam, num); } - pcl::PointCloud::Ptr pc_particle(new pcl::PointCloud); pcl::PointCloud::Ptr pc_particle_beam(new pcl::PointCloud); - pcl::PointCloud::Ptr pc_dummy(new pcl::PointCloud); std::vector id(1); std::vector sqdist(1); @@ -752,36 +487,27 @@ class MCL3dlNode const float match_dist_min = params_.match_dist_min; const float match_dist_flat = params_.match_dist_flat; const float match_weight = params_.match_weight; - mcl_3dl::NormalLikelihood odom_error_lin_nd(params_.odom_err_integ_lin_sigma); - mcl_3dl::NormalLikelihood odom_error_ang_nd(params_.odom_err_integ_ang_sigma); + NormalLikelihood odom_error_lin_nd(params_.odom_err_integ_lin_sigma); + NormalLikelihood odom_error_ang_nd(params_.odom_err_integ_ang_sigma); auto measure_func = [this, &match_dist_min, &match_dist_flat, &match_weight, - &id, &sqdist, &pc_particle, &pc_local, + &id, &sqdist, &pc_locals, &pc_particle_beam, &pc_local_beam, &origins, &odom_error_lin_nd, - &match_ratio_min, &match_ratio_max](const State &s) -> float + &match_ratio_min, &match_ratio_max](const State6DOF &s) -> float { - // likelihood model - float score_like = 0; - *pc_particle = *pc_local; - s.transform(*pc_particle); - size_t num = 0; - for (auto &p : pc_particle->points) + float likelihood = 1; + std::map qualities; + for (auto &lm : lidar_measurements_) { - if (kdtree_->radiusSearch(p, match_dist_min, id, sqdist, 1)) - { - const float dist = match_dist_min - std::max(sqrtf(sqdist[0]), match_dist_flat); - if (dist < 0.0) - continue; - - score_like += dist * match_weight; - num++; - } + const std::pair result = lm.second->measure( + kdtree_, pc_locals[lm.first], s); + likelihood *= result.first; + qualities[lm.first] = result.second; } - const float match_ratio = static_cast(num) / pc_particle->points.size(); - if (match_ratio_min > match_ratio) - match_ratio_min = match_ratio; - if (match_ratio_max < match_ratio) - match_ratio_max = match_ratio; + if (match_ratio_min > qualities["likelihood"]) + match_ratio_min = qualities["likelihood"]; + else if (match_ratio_max < qualities["likelihood"]) + match_ratio_max = qualities["likelihood"]; // approximative beam model float score_beam = 1.0; @@ -790,10 +516,10 @@ class MCL3dlNode for (auto &p : pc_particle_beam->points) { const int beam_header_id = lroundf(p.intensity); - mcl_3dl::Raycast ray( + Raycast ray( kdtree_, s.pos + s.rot * origins[beam_header_id], - mcl_3dl::Vec3(p.x, p.y, p.z), + Vec3(p.x, p.y, p.z), params_.map_grid_min, params_.map_grid_max); for (auto point : ray) { @@ -814,13 +540,13 @@ class MCL3dlNode // odometry error integration const float odom_error = odom_error_lin_nd(s.odom_err_integ_lin.norm()); - return score_like * score_beam * odom_error; + return likelihood * score_beam * odom_error; }; pf_->measure(measure_func); if (static_cast(pf_->getParticleSize()) > params_.num_particles) { - auto bias_func = [](const State &s, float &p_bias) -> void + auto bias_func = [](const State6DOF &s, float &p_bias) -> void { p_bias = 1.0; }; @@ -828,12 +554,12 @@ class MCL3dlNode } else { - mcl_3dl::NormalLikelihood nl_lin(params_.bias_var_dist); - mcl_3dl::NormalLikelihood nl_ang(params_.bias_var_ang); - auto bias_func = [this, &nl_lin, &nl_ang](const State &s, float &p_bias) -> void + NormalLikelihood nl_lin(params_.bias_var_dist); + NormalLikelihood nl_ang(params_.bias_var_ang); + auto bias_func = [this, &nl_lin, &nl_ang](const State6DOF &s, float &p_bias) -> void { const float lin_diff = (s.pos - state_prev_.pos).norm(); - mcl_3dl::Vec3 axis; + Vec3 axis; float ang_diff; (s.rot * state_prev_.rot.inv()).getAxisAng(axis, ang_diff); p_bias = nl_lin(lin_diff) * nl_ang(ang_diff) + 1e-6; @@ -862,8 +588,8 @@ class MCL3dlNode for (auto &p : pc_particle_beam->points) { int beam_header_id = lroundf(p.intensity); - mcl_3dl::Vec3 pos = e.pos + e.rot * origins[beam_header_id]; - mcl_3dl::Vec3 end(p.x, p.y, p.z); + Vec3 pos = e.pos + e.rot * origins[beam_header_id]; + Vec3 end(p.x, p.y, p.z); visualization_msgs::Marker marker; marker.header.frame_id = frame_ids_["map"]; @@ -904,10 +630,10 @@ class MCL3dlNode for (auto &p : pc_particle_beam->points) { const int beam_header_id = lroundf(p.intensity); - mcl_3dl::Raycast ray( + Raycast ray( kdtree_, e.pos + e.rot * origins[beam_header_id], - mcl_3dl::Vec3(p.x, p.y, p.z), + Vec3(p.x, p.y, p.z), params_.map_grid_min, params_.map_grid_max); for (auto point : ray) { @@ -944,7 +670,8 @@ class MCL3dlNode } } - *pc_particle = *pc_local; + pcl::PointCloud::Ptr pc_particle(new pcl::PointCloud); + *pc_particle = *pc_locals["likelihood"]; e.transform(*pc_particle); for (auto &p : pc_particle->points) { @@ -976,8 +703,8 @@ class MCL3dlNode pub_debug_marker_.publish(markers); } - mcl_3dl::Vec3 map_pos; - mcl_3dl::Quat map_rot; + Vec3 map_pos; + Quat map_rot; map_pos = e.pos - e.rot * odom_.rot.inv() * odom_.pos; map_rot = e.rot * odom_.rot.inv(); @@ -989,7 +716,7 @@ class MCL3dlNode } else { - mcl_3dl::Vec3 jump_axis; + Vec3 jump_axis; float jump_ang; float jump_dist = (e.pos - state_prev_.pos).norm(); (e.rot.inv() * state_prev_.rot).getAxisAng(jump_axis, jump_ang); @@ -999,10 +726,10 @@ class MCL3dlNode ROS_INFO("Pose jumped pos:%0.3f, ang:%0.3f", jump_dist, jump_ang); jump = true; - auto integ_reset_func = [](State &s) + auto integ_reset_func = [](State6DOF &s) { - s.odom_err_integ_lin = mcl_3dl::Vec3(); - s.odom_err_integ_ang = mcl_3dl::Vec3(); + s.odom_err_integ_lin = Vec3(); + s.odom_err_integ_ang = Vec3(); }; pf_->predict(integ_reset_func); } @@ -1079,7 +806,7 @@ class MCL3dlNode { bool fix = false; - mcl_3dl::Vec3 fix_axis; + Vec3 fix_axis; const float fix_ang = sqrtf(cov[3][3] + cov[4][4] + cov[5][5]); const float fix_dist = sqrtf(cov[0][0] + cov[1][1] + cov[2][2]); ROS_DEBUG("cov: lin %0.3f ang %0.3f", fix_dist, fix_ang); @@ -1092,7 +819,8 @@ class MCL3dlNode if (fix) ROS_DEBUG("Localization fixed"); } - *pc_particle = *pc_local; + pcl::PointCloud::Ptr pc_particle(new pcl::PointCloud); + *pc_particle = *pc_locals["likelihood"]; e.transform(*pc_particle); if (output_pcd_) @@ -1166,16 +894,16 @@ class MCL3dlNode } pub_particle_.publish(pa); - pf_->resample(State( - mcl_3dl::Vec3(params_.resample_var_x, - params_.resample_var_y, - params_.resample_var_z), - mcl_3dl::Vec3(params_.resample_var_roll, - params_.resample_var_pitch, - params_.resample_var_yaw))); + pf_->resample(State6DOF( + Vec3(params_.resample_var_x, + params_.resample_var_y, + params_.resample_var_z), + Vec3(params_.resample_var_roll, + params_.resample_var_pitch, + params_.resample_var_yaw))); std::normal_distribution noise(0.0, 1.0); - auto update_noise_func = [this, &noise](State &s) + auto update_noise_func = [this, &noise](State6DOF &s) { s.noise_ll_ = noise(engine_) * params_.odom_err_lin_lin; s.noise_la_ = noise(engine_) * params_.odom_err_lin_ang; @@ -1213,13 +941,13 @@ class MCL3dlNode if (match_ratio_max < params_.match_ratio_thresh) { ROS_WARN_THROTTLE(3.0, "Low match_ratio. Expansion resetting."); - pf_->noise(State( - mcl_3dl::Vec3(params_.expansion_var_x, - params_.expansion_var_y, - params_.expansion_var_z), - mcl_3dl::Vec3(params_.expansion_var_roll, - params_.expansion_var_pitch, - params_.expansion_var_yaw))); + pf_->noise(State6DOF( + Vec3(params_.expansion_var_x, + params_.expansion_var_y, + params_.expansion_var_z), + Vec3(params_.expansion_var_roll, + params_.expansion_var_pitch, + params_.expansion_var_yaw))); status.status = mcl_3dl_msgs::Status::EXPANSION_RESETTING; } pc_local_accum_.reset(new pcl::PointCloud); @@ -1260,22 +988,22 @@ class MCL3dlNode } void cbLandmark(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg) { - mcl_3dl::NormalLikelihoodNd nd( + NormalLikelihoodNd nd( Eigen::Matrix( msg->pose.covariance.data()) .cast()); - const State measured( - mcl_3dl::Vec3(msg->pose.pose.position.x, - msg->pose.pose.position.y, - msg->pose.pose.position.z), - mcl_3dl::Quat(msg->pose.pose.orientation.x, - msg->pose.pose.orientation.y, - msg->pose.pose.orientation.z, - msg->pose.pose.orientation.w)); - auto measure_func = [this, &measured, &nd](const State &s) -> float - { - State diff = s - measured; - const mcl_3dl::Vec3 rot_rpy = diff.rot.getRPY(); + const State6DOF measured( + Vec3(msg->pose.pose.position.x, + msg->pose.pose.position.y, + msg->pose.pose.position.z), + Quat(msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z, + msg->pose.pose.orientation.w)); + auto measure_func = [this, &measured, &nd](const State6DOF &s) -> float + { + State6DOF diff = s - measured; + const Vec3 rot_rpy = diff.rot.getRPY(); const Eigen::Matrix diff_vec = (Eigen::MatrixXf(6, 1) << diff.pos.x, diff.pos.y, @@ -1291,7 +1019,7 @@ class MCL3dlNode } void cbImu(const sensor_msgs::Imu::ConstPtr &msg) { - mcl_3dl::Vec3 acc; + Vec3 acc; acc.x = f_acc_[0]->in(msg->linear_acceleration.x); acc.y = f_acc_[1]->in(msg->linear_acceleration.y); acc.z = f_acc_[2]->in(msg->linear_acceleration.z); @@ -1315,7 +1043,7 @@ class MCL3dlNode } else if (dt > 0.05) { - mcl_3dl::Vec3 acc_measure = acc / acc.norm(); + Vec3 acc_measure = acc / acc.norm(); try { tf::Stamped in, out; @@ -1325,7 +1053,7 @@ class MCL3dlNode in.setY(acc_measure.y); in.setZ(acc_measure.z); tfl_.transformVector(frame_ids_["base_link"], in, out); - acc_measure = mcl_3dl::Vec3(out.x(), out.y(), out.z()); + acc_measure = Vec3(out.x(), out.y(), out.z()); tf::StampedTransform trans; // Static transform @@ -1335,13 +1063,13 @@ class MCL3dlNode imu_quat_.y = msg->orientation.y; imu_quat_.z = msg->orientation.z; imu_quat_.w = msg->orientation.w; - mcl_3dl::Vec3 axis; + Vec3 axis; float angle; imu_quat_.getAxisAng(axis, angle); - axis = mcl_3dl::Quat(trans.getRotation().x(), - trans.getRotation().y(), - trans.getRotation().z(), - trans.getRotation().w()) * + axis = Quat(trans.getRotation().x(), + trans.getRotation().y(), + trans.getRotation().z(), + trans.getRotation().w()) * axis; imu_quat_.setAxisAng(axis, angle); } @@ -1350,10 +1078,10 @@ class MCL3dlNode return; } const float acc_measure_norm = acc_measure.norm(); - mcl_3dl::NormalLikelihood nd(params_.acc_var); - auto imu_measure_func = [this, &nd, &acc_measure, &acc_measure_norm](const State &s) -> float + NormalLikelihood nd(params_.acc_var); + auto imu_measure_func = [this, &nd, &acc_measure, &acc_measure_norm](const State6DOF &s) -> float { - const mcl_3dl::Vec3 acc_estim = s.rot.inv() * mcl_3dl::Vec3(0.0, 0.0, 1.0); + const Vec3 acc_estim = s.rot.inv() * Vec3(0.0, 0.0, 1.0); const float diff = acosf( acc_estim.dot(acc_measure) / (acc_measure_norm * acc_estim.norm())); return nd(diff); @@ -1372,13 +1100,13 @@ class MCL3dlNode bool cbExpansionReset(std_srvs::TriggerRequest &request, std_srvs::TriggerResponse &response) { - pf_->noise(State( - mcl_3dl::Vec3(params_.expansion_var_x, - params_.expansion_var_y, - params_.expansion_var_z), - mcl_3dl::Vec3(params_.expansion_var_roll, - params_.expansion_var_pitch, - params_.expansion_var_yaw))); + pf_->noise(State6DOF( + Vec3(params_.expansion_var_x, + params_.expansion_var_y, + params_.expansion_var_z), + Vec3(params_.expansion_var_roll, + params_.expansion_var_pitch, + params_.expansion_var_yaw))); return true; } bool cbGlobalLocalization(std_srvs::TriggerRequest &request, @@ -1431,7 +1159,7 @@ class MCL3dlNode particle.state.pos.x = pit->x; particle.state.pos.y = pit->y; particle.state.pos.z = pit->z; - particle.state.rot = mcl_3dl::Quat(mcl_3dl::Vec3(0.0, 0.0, 2.0 * M_PI * cnt / dir)) * imu_quat_; + particle.state.rot = Quat(Vec3(0.0, 0.0, 2.0 * M_PI * cnt / dir)) * imu_quat_; particle.state.rot.normalize(); if (++cnt >= dir) { @@ -1547,7 +1275,7 @@ class MCL3dlNode params_.match_output_dist, std::max(params_.match_dist_min, params_.map_grid_max * 4.0))); ROS_DEBUG("max_search_radius: %0.3f", max_search_radius); - kdtree_.reset(new mcl_3dl::ChunkedKdtree(map_chunk, max_search_radius)); + kdtree_.reset(new ChunkedKdtree(map_chunk, max_search_radius)); kdtree_->setEpsilon(params_.map_grid_min / 16); kdtree_->setPointRepresentation( boost::dynamic_pointer_cast< @@ -1560,7 +1288,7 @@ class MCL3dlNode params_.global_localization_div_yaw = lroundf(2 * M_PI / grid_ang); pnh_.param("num_particles", params_.num_particles, 64); - pf_.reset(new mcl_3dl::pf::ParticleFilter(params_.num_particles)); + pf_.reset(new pf::ParticleFilter(params_.num_particles)); pnh_.param("num_points", params_.num_points, 96); pnh_.param("num_points_global", params_.num_points_global, 8); pnh_.param("num_points_beam", params_.num_points_beam, 3); @@ -1612,26 +1340,26 @@ class MCL3dlNode pnh_.param("init_var_pitch", v_pitch, 0.1); pnh_.param("init_var_yaw", v_yaw, 0.5); pf_->init( - State( - mcl_3dl::Vec3(x, y, z), - mcl_3dl::Quat(mcl_3dl::Vec3(roll, pitch, yaw))), - State( - mcl_3dl::Vec3(v_x, v_y, v_z), - mcl_3dl::Vec3(v_roll, v_pitch, v_yaw))); + State6DOF( + Vec3(x, y, z), + Quat(Vec3(roll, pitch, yaw))), + State6DOF( + Vec3(v_x, v_y, v_z), + Vec3(v_roll, v_pitch, v_yaw))); pnh_.param("lpf_step", params_.lpf_step, 16.0); - f_pos_[0].reset(new mcl_3dl::Filter(mcl_3dl::Filter::FILTER_LPF, params_.lpf_step, 0.0)); - f_pos_[1].reset(new mcl_3dl::Filter(mcl_3dl::Filter::FILTER_LPF, params_.lpf_step, 0.0)); - f_pos_[2].reset(new mcl_3dl::Filter(mcl_3dl::Filter::FILTER_LPF, params_.lpf_step, 0.0)); - f_ang_[0].reset(new mcl_3dl::Filter(mcl_3dl::Filter::FILTER_LPF, params_.lpf_step, 0.0, true)); - f_ang_[1].reset(new mcl_3dl::Filter(mcl_3dl::Filter::FILTER_LPF, params_.lpf_step, 0.0, true)); - f_ang_[2].reset(new mcl_3dl::Filter(mcl_3dl::Filter::FILTER_LPF, params_.lpf_step, 0.0, true)); + f_pos_[0].reset(new Filter(Filter::FILTER_LPF, params_.lpf_step, 0.0)); + f_pos_[1].reset(new Filter(Filter::FILTER_LPF, params_.lpf_step, 0.0)); + f_pos_[2].reset(new Filter(Filter::FILTER_LPF, params_.lpf_step, 0.0)); + f_ang_[0].reset(new Filter(Filter::FILTER_LPF, params_.lpf_step, 0.0, true)); + f_ang_[1].reset(new Filter(Filter::FILTER_LPF, params_.lpf_step, 0.0, true)); + f_ang_[2].reset(new Filter(Filter::FILTER_LPF, params_.lpf_step, 0.0, true)); double acc_lpf_step; pnh_.param("acc_lpf_step", acc_lpf_step, 128.0); - f_acc_[0].reset(new mcl_3dl::Filter(mcl_3dl::Filter::FILTER_LPF, acc_lpf_step, 0.0)); - f_acc_[1].reset(new mcl_3dl::Filter(mcl_3dl::Filter::FILTER_LPF, acc_lpf_step, 0.0)); - f_acc_[2].reset(new mcl_3dl::Filter(mcl_3dl::Filter::FILTER_LPF, acc_lpf_step, 0.0)); + f_acc_[0].reset(new Filter(Filter::FILTER_LPF, acc_lpf_step, 0.0)); + f_acc_[1].reset(new Filter(Filter::FILTER_LPF, acc_lpf_step, 0.0)); + f_acc_[2].reset(new Filter(Filter::FILTER_LPF, acc_lpf_step, 0.0)); pnh_.param("acc_var", params_.acc_var, M_PI / 4.0); // 45 deg pnh_.param("jump_dist", params_.jump_dist, 1.0); @@ -1659,10 +1387,19 @@ class MCL3dlNode pnh_.param("publish_tf", publish_tf_, true); pnh_.param("output_pcd", output_pcd_, false); - imu_quat_ = mcl_3dl::Quat(0.0, 0.0, 0.0, 1.0); + imu_quat_ = Quat(0.0, 0.0, 0.0, 1.0); has_odom_ = has_map_ = has_imu_ = false; - localize_rate_.reset(new mcl_3dl::Filter(mcl_3dl::Filter::FILTER_LPF, 5.0, 0.0)); + localize_rate_.reset(new Filter(Filter::FILTER_LPF, 5.0, 0.0)); + + lidar_measurements_["likelihood"] = + LidarMeasurementModelBase::Ptr( + new LidarMeasurementModelLikelihood()); + + for (auto &lm : lidar_measurements_) + { + lm.second->loadConfig(nh_, lm.first); + } map_update_timer_ = nh_.createTimer( *params_.map_update_interval, @@ -1733,10 +1470,10 @@ class MCL3dlNode tf::TransformListener tfl_; tf::TransformBroadcaster tfb_; - std::shared_ptr f_pos_[3]; - std::shared_ptr f_ang_[3]; - std::shared_ptr f_acc_[3]; - std::shared_ptr localize_rate_; + std::shared_ptr f_pos_[3]; + std::shared_ptr f_ang_[3]; + std::shared_ptr f_acc_[3]; + std::shared_ptr localize_rate_; ros::Time localized_last_; ros::Duration tf_tolerance_base_; @@ -1750,16 +1487,16 @@ class MCL3dlNode bool has_map_; bool has_odom_; bool has_imu_; - State odom_; - State odom_prev_; + State6DOF odom_; + State6DOF odom_prev_; std::map frames_; std::vector frames_v_; size_t frame_num_; - State state_prev_; + State6DOF state_prev_; ros::Time imu_last_; int cnt_measure_; int cnt_accum_; - mcl_3dl::Quat imu_quat_; + Quat imu_quat_; size_t global_localization_fix_cnt_; MyPointRepresentation point_rep_; @@ -1769,18 +1506,23 @@ class MCL3dlNode pcl::PointCloud::Ptr pc_update_; pcl::PointCloud::Ptr pc_all_accum_; pcl::PointCloud::Ptr pc_local_accum_; - mcl_3dl::ChunkedKdtree::Ptr kdtree_; + ChunkedKdtree::Ptr kdtree_; std::vector pc_accum_header_; + std::map< + std::string, + LidarMeasurementModelBase::Ptr> lidar_measurements_; + std::random_device seed_gen_; std::default_random_engine engine_; }; +} // namespace mcl_3dl int main(int argc, char *argv[]) { ros::init(argc, argv, "mcl_3dl"); - MCL3dlNode mcl(argc, argv); + mcl_3dl::MCL3dlNode mcl(argc, argv); ros::spin(); return 0; From 81cf634f10b91a4915e4e99ec31e14e0302d88ff Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Sun, 9 Sep 2018 23:52:00 +0900 Subject: [PATCH 02/29] Fix model --- .../mcl_3dl/lidar_measurement_model_base.h | 2 +- .../lidar_measurement_model_likelihood.h | 26 +++++++++------- include/mcl_3dl/point_cloud_random_sampler.h | 2 +- src/mcl_3dl.cpp | 31 ++++++++++--------- test/tests/expansion_resetting_rostest.test | 8 ++--- test/tests/global_localization_rostest.test | 6 ++-- 6 files changed, 40 insertions(+), 35 deletions(-) diff --git a/include/mcl_3dl/lidar_measurement_model_base.h b/include/mcl_3dl/lidar_measurement_model_base.h index 81015178..a0a274d0 100644 --- a/include/mcl_3dl/lidar_measurement_model_base.h +++ b/include/mcl_3dl/lidar_measurement_model_base.h @@ -53,7 +53,7 @@ class LidarMeasurementModelBase const ros::NodeHandle &nh, const std::string &name) = 0; virtual void setGlobalLocalizationStatus( - size_t, size_t) = 0; + const size_t, const size_t) = 0; virtual typename pcl::PointCloud::Ptr filter( const typename pcl::PointCloud::Ptr &) const = 0; diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h index 96d04dfc..06437970 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h @@ -93,17 +93,15 @@ class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase num_particles) - num = num * num_particles / current_num_particles; + size_t num = num_points_default_ * num_particles / current_num_particles; if (num < num_points_global_) num = num_points_global_; @@ -123,12 +121,14 @@ class LidarMeasurementModelLikelihood : public LidarMeasurementModelBaseerase( - std::remove_if(pc->begin(), pc->end(), local_points_filter), pc->end()); - pc->width = 1; - pc->height = pc->points.size(); - - return sampler_.sample(pc, num_points_); + pcl::PointCloud::Ptr pc_filtered(new pcl::PointCloud); + *pc_filtered = *pc; + pc_filtered->erase( + std::remove_if(pc_filtered->begin(), pc_filtered->end(), local_points_filter), pc_filtered->end()); + pc_filtered->width = 1; + pc_filtered->height = pc_filtered->points.size(); + + return sampler_.sample(pc_filtered, num_points_); } std::pair measure( @@ -136,6 +136,10 @@ class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase::Ptr &pc, const STATE_TYPE &s) const { + if (!pc) + return std::pair(1, 0); + if (pc->size() == 0) + return std::pair(1, 0); pcl::PointCloud::Ptr pc_particle(new pcl::PointCloud); std::vector id(1); std::vector sqdist(1); diff --git a/include/mcl_3dl/point_cloud_random_sampler.h b/include/mcl_3dl/point_cloud_random_sampler.h index 29b449ce..b4626514 100644 --- a/include/mcl_3dl/point_cloud_random_sampler.h +++ b/include/mcl_3dl/point_cloud_random_sampler.h @@ -57,7 +57,7 @@ class PointCloudRandomSampler return typename pcl::PointCloud::Ptr(); typename pcl::PointCloud::Ptr output(new pcl::PointCloud); - output->points.resize(num); + output->points.reserve(num); std::uniform_int_distribution ud(0, pc->points.size() - 1); diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp index 2850b2a8..ca664f16 100644 --- a/src/mcl_3dl.cpp +++ b/src/mcl_3dl.cpp @@ -435,6 +435,7 @@ class MCL3dlNode return output; }; + std::map::Ptr> pc_locals; for (auto &lm : lidar_measurements_) { @@ -479,18 +480,11 @@ class MCL3dlNode pcl::PointCloud::Ptr pc_particle_beam(new pcl::PointCloud); - std::vector id(1); - std::vector sqdist(1); - float match_ratio_min = 1.0; float match_ratio_max = 0.0; - const float match_dist_min = params_.match_dist_min; - const float match_dist_flat = params_.match_dist_flat; - const float match_weight = params_.match_weight; NormalLikelihood odom_error_lin_nd(params_.odom_err_integ_lin_sigma); NormalLikelihood odom_error_ang_nd(params_.odom_err_integ_ang_sigma); - auto measure_func = [this, &match_dist_min, &match_dist_flat, &match_weight, - &id, &sqdist, &pc_locals, + auto measure_func = [this, &pc_locals, &pc_particle_beam, &pc_local_beam, &origins, &odom_error_lin_nd, &match_ratio_min, &match_ratio_max](const State6DOF &s) -> float @@ -506,7 +500,7 @@ class MCL3dlNode } if (match_ratio_min > qualities["likelihood"]) match_ratio_min = qualities["likelihood"]; - else if (match_ratio_max < qualities["likelihood"]) + if (match_ratio_max < qualities["likelihood"]) match_ratio_max = qualities["likelihood"]; // approximative beam model @@ -819,12 +813,14 @@ class MCL3dlNode if (fix) ROS_DEBUG("Localization fixed"); } - pcl::PointCloud::Ptr pc_particle(new pcl::PointCloud); - *pc_particle = *pc_locals["likelihood"]; - e.transform(*pc_particle); if (output_pcd_) + { + pcl::PointCloud::Ptr pc_particle(new pcl::PointCloud); + *pc_particle = *pc_locals["likelihood"]; + e.transform(*pc_particle); *pc_all_accum_ += *pc_particle; + } if ((msg->header.stamp - match_output_last_ > *params_.match_output_interval || msg->header.stamp < match_output_last_ - ros::Duration(1.0)) && @@ -839,10 +835,15 @@ class MCL3dlNode pc_unmatch.header.stamp = msg->header.stamp; pc_unmatch.header.frame_id = frame_ids_["map"]; - e.transform(*pc_local_full); + pcl::PointCloud::Ptr pc_local(new pcl::PointCloud); + *pc_local = *pc_local_full; + + e.transform(*pc_local); + std::vector id(1); + std::vector sqdist(1); const double match_dist_sq = params_.match_output_dist * params_.match_output_dist; - for (auto &p : pc_local_full->points) + for (auto &p : pc_local->points) { geometry_msgs::Point32 pp; pp.x = p.x; @@ -1398,7 +1399,7 @@ class MCL3dlNode for (auto &lm : lidar_measurements_) { - lm.second->loadConfig(nh_, lm.first); + lm.second->loadConfig(pnh_, lm.first); } map_update_timer_ = nh_.createTimer( diff --git a/test/tests/expansion_resetting_rostest.test b/test/tests/expansion_resetting_rostest.test index bc05788c..5b7feb51 100644 --- a/test/tests/expansion_resetting_rostest.test +++ b/test/tests/expansion_resetting_rostest.test @@ -4,11 +4,11 @@ - - - - + + + + diff --git a/test/tests/global_localization_rostest.test b/test/tests/global_localization_rostest.test index 4cb74a86..7ec3f8c5 100644 --- a/test/tests/global_localization_rostest.test +++ b/test/tests/global_localization_rostest.test @@ -4,10 +4,10 @@ - - + + + - From 0a7a36674f15f26a0ca847ab774bde4b88be5009 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Sun, 9 Sep 2018 23:55:44 +0900 Subject: [PATCH 03/29] Fix includes --- include/mcl_3dl/lidar_measurement_model_base.h | 3 +++ .../lidar_measurement_model_likelihood.h | 11 ++++++++--- include/mcl_3dl/state_6dof.h | 2 ++ src/mcl_3dl.cpp | 15 ++++++++------- 4 files changed, 21 insertions(+), 10 deletions(-) diff --git a/include/mcl_3dl/lidar_measurement_model_base.h b/include/mcl_3dl/lidar_measurement_model_base.h index a0a274d0..a3140912 100644 --- a/include/mcl_3dl/lidar_measurement_model_base.h +++ b/include/mcl_3dl/lidar_measurement_model_base.h @@ -30,6 +30,9 @@ #ifndef MCL_3DL_LIDAR_MEASUREMENT_MODEL_BASE_H #define MCL_3DL_LIDAR_MEASUREMENT_MODEL_BASE_H +#include +#include + #include #include diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h index 06437970..98c7d337 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h @@ -27,8 +27,13 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef MCL_3DL_LIDAR_MEASUREMENT_MODEL_LIDAR_MEASUREMENT_MODEL_LIKELIHOOD_H -#define MCL_3DL_LIDAR_MEASUREMENT_MODEL_LIDAR_MEASUREMENT_MODEL_LIKELIHOOD_H +#ifndef MCL_3DL_LIDAR_MEASUREMENT_MODELS_LIDAR_MEASUREMENT_MODEL_LIKELIHOOD_H +#define MCL_3DL_LIDAR_MEASUREMENT_MODELS_LIDAR_MEASUREMENT_MODEL_LIKELIHOOD_H + +#include +#include +#include +#include #include @@ -167,4 +172,4 @@ class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase + #include #include #include diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp index ca664f16..d387b63b 100644 --- a/src/mcl_3dl.cpp +++ b/src/mcl_3dl.cpp @@ -27,6 +27,14 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include +#include +#include +#include +#include +#include +#include + #include #include #include @@ -55,13 +63,6 @@ #include -#include -#include -#include -#include -#include -#include - #include #include #include From 2b8bff56c82e6ee6e859cc6efff7cfe7b8b3a065 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 18 Sep 2018 17:58:15 +0900 Subject: [PATCH 04/29] Don't return null shared_ptr from random sampler --- include/mcl_3dl/point_cloud_random_sampler.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/include/mcl_3dl/point_cloud_random_sampler.h b/include/mcl_3dl/point_cloud_random_sampler.h index b4626514..968ff984 100644 --- a/include/mcl_3dl/point_cloud_random_sampler.h +++ b/include/mcl_3dl/point_cloud_random_sampler.h @@ -53,21 +53,21 @@ class PointCloudRandomSampler const typename pcl::PointCloud::Ptr &pc, const size_t num) const { + typename pcl::PointCloud::Ptr output(new pcl::PointCloud); + + output->width = 1; + output->height = 0; + output->header = pc->header; + if (pc->points.size() == 0) - return typename pcl::PointCloud::Ptr(); + return output; - typename pcl::PointCloud::Ptr output(new pcl::PointCloud); output->points.reserve(num); - std::uniform_int_distribution ud(0, pc->points.size() - 1); - for (size_t i = 0; i < num; i++) { - output->points.push_back(pc->points[ud(*engine_)]); + output->push_back(pc->points[ud(*engine_)]); } - output->width = 1; - output->height = output->points.size(); - output->header = pc->header; return output; } From 26e606be6e4d10a205f8b172630531bf4f8d0c2c Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 18 Sep 2018 18:41:21 +0900 Subject: [PATCH 05/29] Add test for PointCloudRandomSampler class --- include/mcl_3dl/point_cloud_random_sampler.h | 5 +- test/CMakeLists.txt | 18 ++-- test/src/test_point_cloud_random_sampler.cpp | 91 ++++++++++++++++++++ 3 files changed, 102 insertions(+), 12 deletions(-) create mode 100644 test/src/test_point_cloud_random_sampler.cpp diff --git a/include/mcl_3dl/point_cloud_random_sampler.h b/include/mcl_3dl/point_cloud_random_sampler.h index 968ff984..4fe76fd1 100644 --- a/include/mcl_3dl/point_cloud_random_sampler.h +++ b/include/mcl_3dl/point_cloud_random_sampler.h @@ -50,13 +50,10 @@ class PointCloudRandomSampler { } typename pcl::PointCloud::Ptr sample( - const typename pcl::PointCloud::Ptr &pc, + const typename pcl::PointCloud::ConstPtr &pc, const size_t num) const { typename pcl::PointCloud::Ptr output(new pcl::PointCloud); - - output->width = 1; - output->height = 0; output->header = pc->header; if (pc->points.size() == 0) diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index cd74e6dc..9110be33 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,17 +1,19 @@ -catkin_add_gtest(test_vec3 src/test_vec3.cpp) -target_link_libraries(test_vec3 ${catkin_LIBRARIES}) -catkin_add_gtest(test_quat src/test_quat.cpp) -target_link_libraries(test_quat ${catkin_LIBRARIES}) -catkin_add_gtest(test_nd src/test_nd.cpp) -target_link_libraries(test_nd ${catkin_LIBRARIES}) +catkin_add_gtest(test_chunked_kdtree src/test_chunked_kdtree.cpp) +target_link_libraries(test_chunked_kdtree ${catkin_LIBRARIES} ${PCL_LIBRARIES}) catkin_add_gtest(test_filter src/test_filter.cpp) target_link_libraries(test_filter ${catkin_LIBRARIES}) +catkin_add_gtest(test_nd src/test_nd.cpp) +target_link_libraries(test_nd ${catkin_LIBRARIES}) catkin_add_gtest(test_pf src/test_pf.cpp) target_link_libraries(test_pf ${catkin_LIBRARIES}) -catkin_add_gtest(test_chunked_kdtree src/test_chunked_kdtree.cpp) -target_link_libraries(test_chunked_kdtree ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +catkin_add_gtest(test_point_cloud_random_sampler src/test_point_cloud_random_sampler.cpp) +target_link_libraries(test_point_cloud_random_sampler ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +catkin_add_gtest(test_quat src/test_quat.cpp) +target_link_libraries(test_quat ${catkin_LIBRARIES}) catkin_add_gtest(test_raycast src/test_raycast.cpp) target_link_libraries(test_raycast ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +catkin_add_gtest(test_vec3 src/test_vec3.cpp) +target_link_libraries(test_vec3 ${catkin_LIBRARIES}) add_executable(performance_raycast src/performance_raycast.cpp) target_link_libraries(performance_raycast ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${PCL_LIBRARIES}) diff --git a/test/src/test_point_cloud_random_sampler.cpp b/test/src/test_point_cloud_random_sampler.cpp new file mode 100644 index 00000000..00b99f07 --- /dev/null +++ b/test/src/test_point_cloud_random_sampler.cpp @@ -0,0 +1,91 @@ +/* + * Copyright (c) 2018, the mcl_3dl authors + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include + +#include + +TEST(PointCloudRandomSampler, Sampling) +{ + pcl::PointCloud::Ptr pc_input(new pcl::PointCloud); + pc_input->width = 1; + pc_input->height = 0; + pc_input->header.frame_id = "frame0"; + pc_input->header.stamp = 12345; + + const float points_ref[][3] = + { + { 10, 11, 12 }, + { 20, 21, 22 }, + { 30, 31, 32 } + }; + for (const auto &p_ref : points_ref) + { + pc_input->push_back(pcl::PointXYZ(p_ref[0], p_ref[1], p_ref[2])); + } + + mcl_3dl::PointCloudRandomSampler sampler; + + for (size_t num = 1; num < 4; num++) + { + pcl::PointCloud::Ptr pc_output = sampler.sample(pc_input, num); + + // Check header and number of the points + ASSERT_EQ(pc_output->header.frame_id, pc_input->header.frame_id); + ASSERT_EQ(pc_output->header.stamp, pc_input->header.stamp); + ASSERT_EQ(pc_output->height, 1u); + ASSERT_EQ(pc_output->width, num); + + // Check that the all sampled points are in the original point array + for (const pcl::PointXYZ &p : *pc_output) + { + bool found = false; + for (const auto &p_ref : points_ref) + { + if (p_ref[0] == p.x && p_ref[1] == p.y && p_ref[2] == p.z) + found = true; + } + ASSERT_TRUE(found) << "A sampled point is not in the original points array"; + } + } + + // Make sure that the sampler returns 0 point output for 0 point input + pcl::PointCloud::Ptr pc_output0 = sampler.sample(pc_input, 0); + ASSERT_EQ(pc_output0->points.size(), 0); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} From 4ba9695636579f3c1cbbf0e40ed85f329b1e5378 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 18 Sep 2018 18:41:35 +0900 Subject: [PATCH 06/29] Fix constness --- include/mcl_3dl/lidar_measurement_model_base.h | 4 ++-- .../lidar_measurement_model_likelihood.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/mcl_3dl/lidar_measurement_model_base.h b/include/mcl_3dl/lidar_measurement_model_base.h index a3140912..f85aaa25 100644 --- a/include/mcl_3dl/lidar_measurement_model_base.h +++ b/include/mcl_3dl/lidar_measurement_model_base.h @@ -59,11 +59,11 @@ class LidarMeasurementModelBase const size_t, const size_t) = 0; virtual typename pcl::PointCloud::Ptr filter( - const typename pcl::PointCloud::Ptr &) const = 0; + const typename pcl::PointCloud::ConstPtr &) const = 0; virtual std::pair measure( typename mcl_3dl::ChunkedKdtree::Ptr &, - const typename pcl::PointCloud::Ptr &, + const typename pcl::PointCloud::ConstPtr &, const STATE_TYPE &) const = 0; }; } // namespace mcl_3dl diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h index 98c7d337..6e8e0640 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h @@ -114,7 +114,7 @@ class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase::Ptr filter( - const typename pcl::PointCloud::Ptr &pc) const + const typename pcl::PointCloud::ConstPtr &pc) const { const auto local_points_filter = [this](const POINT_TYPE &p) { @@ -138,7 +138,7 @@ class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase measure( typename mcl_3dl::ChunkedKdtree::Ptr &kdtree, - const typename pcl::PointCloud::Ptr &pc, + const typename pcl::PointCloud::ConstPtr &pc, const STATE_TYPE &s) const { if (!pc) From 29829602c7771c27fa958968923de222b81418e3 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 18 Sep 2018 19:08:04 +0900 Subject: [PATCH 07/29] Fix signed/unsigned compare --- test/src/test_point_cloud_random_sampler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/src/test_point_cloud_random_sampler.cpp b/test/src/test_point_cloud_random_sampler.cpp index 00b99f07..3c489dd1 100644 --- a/test/src/test_point_cloud_random_sampler.cpp +++ b/test/src/test_point_cloud_random_sampler.cpp @@ -80,7 +80,7 @@ TEST(PointCloudRandomSampler, Sampling) // Make sure that the sampler returns 0 point output for 0 point input pcl::PointCloud::Ptr pc_output0 = sampler.sample(pc_input, 0); - ASSERT_EQ(pc_output0->points.size(), 0); + ASSERT_EQ(pc_output0->points.size(), 0u); } int main(int argc, char **argv) From 59645add7a329b477b430bc6f4139378abf5a8be Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 18 Sep 2018 20:20:14 +0900 Subject: [PATCH 08/29] Define beam model as a class --- .../mcl_3dl/lidar_measurement_model_base.h | 3 + .../lidar_measurement_model_beam.h | 205 ++++++++++++++++++ .../lidar_measurement_model_likelihood.h | 2 + src/mcl_3dl.cpp | 71 +----- 4 files changed, 221 insertions(+), 60 deletions(-) create mode 100644 include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h diff --git a/include/mcl_3dl/lidar_measurement_model_base.h b/include/mcl_3dl/lidar_measurement_model_base.h index f85aaa25..0112adf5 100644 --- a/include/mcl_3dl/lidar_measurement_model_base.h +++ b/include/mcl_3dl/lidar_measurement_model_base.h @@ -32,6 +32,7 @@ #include #include +#include #include @@ -39,6 +40,7 @@ #include #include +#include namespace mcl_3dl { @@ -64,6 +66,7 @@ class LidarMeasurementModelBase virtual std::pair measure( typename mcl_3dl::ChunkedKdtree::Ptr &, const typename pcl::PointCloud::ConstPtr &, + const std::vector &, const STATE_TYPE &) const = 0; }; } // namespace mcl_3dl diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h new file mode 100644 index 00000000..a6ec398b --- /dev/null +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h @@ -0,0 +1,205 @@ +/* + * Copyright (c) 2018, the mcl_3dl authors + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDEDNode BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef MCL_3DL_LIDAR_MEASUREMENT_MODELS_LIDAR_MEASUREMENT_MODEL_BEAM_H +#define MCL_3DL_LIDAR_MEASUREMENT_MODELS_LIDAR_MEASUREMENT_MODEL_BEAM_H + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +namespace mcl_3dl +{ +template +class LidarMeasurementModelBeam : public LidarMeasurementModelBase +{ + static_assert(std::is_same(), "Supported POINT_TYPE is PointXYZI"); + +private: + size_t num_points_; + size_t num_points_default_; + size_t num_points_global_; + float clip_far_sq_; + float clip_near_sq_; + float clip_z_min_; + float clip_z_max_; + float match_dist_min_; + float match_dist_flat_; + float beam_likelihood_min_; + float beam_likelihood_; + float sin_total_ref_; + float map_grid_min_; + float map_grid_max_; + + PointCloudRandomSampler sampler_; + +public: + void loadConfig( + const ros::NodeHandle &nh, + const std::string &name) + { + ros::NodeHandle pnh(nh, name); + + int num_points, num_points_global; + pnh.param("num_points", num_points, 3); + pnh.param("num_points_global", num_points_global, 0); + num_points_default_ = num_points_ = num_points; + num_points_global_ = num_points_global; + + double clip_near, clip_far; + pnh.param("clip_near", clip_near, 0.5); + pnh.param("clip_far", clip_far, 4.0); + clip_near_sq_ = clip_near * clip_near; + clip_far_sq_ = clip_far * clip_far; + + double clip_z_min, clip_z_max; + pnh.param("clip_z_min", clip_z_min, -2.0); + pnh.param("clip_z_max", clip_z_max, 2.0); + clip_z_min_ = clip_z_min; + clip_z_max_ = clip_z_max; + + double match_dist_min, match_dist_flat; + pnh.param("match_dist_min", match_dist_min, 0.2); + pnh.param("match_dist_flat", match_dist_flat, 0.05); + match_dist_min_ = match_dist_min; + match_dist_flat_ = match_dist_flat; + + double beam_likelihood_min; + pnh.param("beam_likelihood", beam_likelihood_min, 0.2); + beam_likelihood_min_ = beam_likelihood_min; + beam_likelihood_ = powf(beam_likelihood_min, 1.0 / static_cast(num_points)); + + double ang_total_ref; + pnh.param("ang_total_ref", ang_total_ref, M_PI / 6.0); + sin_total_ref_ = sinf(ang_total_ref); + } + void setGlobalLocalizationStatus( + const size_t num_particles, + const size_t current_num_particles) + { + if (current_num_particles <= num_particles) + { + num_points_ = num_points_default_; + return; + } + size_t num = num_points_default_ * num_particles / current_num_particles; + if (num < num_points_global_) + num = num_points_global_; + + num_points_ = num; + } + LidarMeasurementModelBeam(const float x, const float y, const float z) + { + map_grid_min_ = std::min(std::min(x, y), z); + map_grid_max_ = std::max(std::max(x, y), z); + } + + typename pcl::PointCloud::Ptr filter( + const typename pcl::PointCloud::ConstPtr &pc) const + { + const auto local_points_filter = [this](const POINT_TYPE &p) + { + if (p.x * p.x + p.y * p.y > clip_far_sq_) + return true; + if (p.x * p.x + p.y * p.y < clip_near_sq_) + return true; + if (p.z < clip_z_min_ || clip_z_max_ < p.z) + return true; + if (p.intensity - roundf(p.intensity) > 0.01) + return true; + return false; + }; + pcl::PointCloud::Ptr pc_filtered(new pcl::PointCloud); + *pc_filtered = *pc; + pc_filtered->erase( + std::remove_if(pc_filtered->begin(), pc_filtered->end(), local_points_filter), pc_filtered->end()); + pc_filtered->width = 1; + pc_filtered->height = pc_filtered->points.size(); + + return sampler_.sample(pc_filtered, num_points_); + } + + std::pair measure( + typename mcl_3dl::ChunkedKdtree::Ptr &kdtree, + const typename pcl::PointCloud::ConstPtr &pc, + const std::vector &origins, + const STATE_TYPE &s) const + { + if (!pc) + return std::pair(1, 0); + if (pc->size() == 0) + return std::pair(1, 0); + pcl::PointCloud::Ptr pc_particle(new pcl::PointCloud); + std::vector id(1); + std::vector sqdist(1); + + float score_beam = 1.0; + *pc_particle = *pc; + s.transform(*pc_particle); + for (auto &p : pc_particle->points) + { + const int beam_header_id = lroundf(p.intensity); + Raycast ray( + kdtree, + s.pos + s.rot * origins[beam_header_id], + Vec3(p.x, p.y, p.z), + map_grid_min_, map_grid_max_); + for (auto point : ray) + { + if (point.collision_) + { + // reject total reflection + if (point.sin_angle_ > sin_total_ref_) + { + score_beam *= beam_likelihood_; + } + break; + } + } + } + if (score_beam < beam_likelihood_min_) + score_beam = beam_likelihood_min_; + + return std::pair(score_beam, 1.0); + } +}; +} // namespace mcl_3dl + +#endif // MCL_3DL_LIDAR_MEASUREMENT_MODELS_LIDAR_MEASUREMENT_MODEL_BEAM_H diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h index 6e8e0640..477fa521 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h @@ -42,6 +42,7 @@ #include #include +#include namespace mcl_3dl { @@ -139,6 +140,7 @@ class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase measure( typename mcl_3dl::ChunkedKdtree::Ptr &kdtree, const typename pcl::PointCloud::ConstPtr &pc, + const std::vector &origins, const STATE_TYPE &s) const { if (!pc) diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp index d387b63b..cdcae9b3 100644 --- a/src/mcl_3dl.cpp +++ b/src/mcl_3dl.cpp @@ -66,11 +66,11 @@ #include #include #include +#include #include #include #include #include -#include #include #include @@ -450,43 +450,17 @@ class MCL3dlNode ROS_ERROR("All points are filtered out. Failed to localize."); return; } - pcl::PointCloud::Ptr pc_local_beam(new pcl::PointCloud); - *pc_local_beam = *pc_local_full; - auto local_beam_filter = [this](const pcl::PointXYZI &p) - { - if (p.x * p.x + p.y * p.y > params_.clip_beam_far_sq) - return true; - if (p.x * p.x + p.y * p.y < params_.clip_beam_near_sq) - return true; - if (p.z < params_.clip_beam_z_min || params_.clip_beam_z_max < p.z) - return true; - if (p.intensity - roundf(p.intensity) > 0.01) - return true; - return false; - }; - pc_local_beam->erase( - std::remove_if(pc_local_beam->begin(), pc_local_beam->end(), local_beam_filter), - pc_local_beam->end()); - if (pc_local_beam->size() == 0) + if (pc_locals["beam"] && pc_locals["beam"]->size() == 0) { ROS_ERROR("All beam points are filtered out. Skipping beam model."); } - else - { - size_t num = params_.num_points_beam; - if (static_cast(pf_->getParticleSize()) > params_.num_particles) - num = num * params_.num_particles / pf_->getParticleSize(); - pc_local_beam = random_sample(pc_local_beam, num); - } - - pcl::PointCloud::Ptr pc_particle_beam(new pcl::PointCloud); float match_ratio_min = 1.0; float match_ratio_max = 0.0; NormalLikelihood odom_error_lin_nd(params_.odom_err_integ_lin_sigma); NormalLikelihood odom_error_ang_nd(params_.odom_err_integ_ang_sigma); auto measure_func = [this, &pc_locals, - &pc_particle_beam, &pc_local_beam, &origins, + &origins, &odom_error_lin_nd, &match_ratio_min, &match_ratio_max](const State6DOF &s) -> float { @@ -495,7 +469,7 @@ class MCL3dlNode for (auto &lm : lidar_measurements_) { const std::pair result = lm.second->measure( - kdtree_, pc_locals[lm.first], s); + kdtree_, pc_locals[lm.first], origins, s); likelihood *= result.first; qualities[lm.first] = result.second; } @@ -504,38 +478,10 @@ class MCL3dlNode if (match_ratio_max < qualities["likelihood"]) match_ratio_max = qualities["likelihood"]; - // approximative beam model - float score_beam = 1.0; - *pc_particle_beam = *pc_local_beam; - s.transform(*pc_particle_beam); - for (auto &p : pc_particle_beam->points) - { - const int beam_header_id = lroundf(p.intensity); - Raycast ray( - kdtree_, - s.pos + s.rot * origins[beam_header_id], - Vec3(p.x, p.y, p.z), - params_.map_grid_min, params_.map_grid_max); - for (auto point : ray) - { - if (point.collision_) - { - // reject total reflection - if (point.sin_angle_ > params_.sin_total_ref) - { - score_beam *= params_.beam_likelihood; - } - break; - } - } - } - if (score_beam < params_.beam_likelihood_min) - score_beam = params_.beam_likelihood_min; - // odometry error integration const float odom_error = odom_error_lin_nd(s.odom_err_integ_lin.norm()); - return likelihood * score_beam * odom_error; + return likelihood * odom_error; }; pf_->measure(measure_func); @@ -578,7 +524,8 @@ class MCL3dlNode { visualization_msgs::MarkerArray markers; - *pc_particle_beam = *pc_local_beam; + pcl::PointCloud::Ptr pc_particle_beam(new pcl::PointCloud); + *pc_particle_beam = *pc_locals["beam"]; e.transform(*pc_particle_beam); for (auto &p : pc_particle_beam->points) { @@ -1397,6 +1344,10 @@ class MCL3dlNode lidar_measurements_["likelihood"] = LidarMeasurementModelBase::Ptr( new LidarMeasurementModelLikelihood()); + lidar_measurements_["beam"] = + LidarMeasurementModelBase::Ptr( + new LidarMeasurementModelBeam( + params_.map_downsample_x, params_.map_downsample_y, params_.map_downsample_z)); for (auto &lm : lidar_measurements_) { From c7de3175968961f11fa2191d5a3ca030f45b8056 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 18 Sep 2018 20:35:38 +0900 Subject: [PATCH 09/29] Remove unused variables --- src/mcl_3dl.cpp | 20 -------------------- 1 file changed, 20 deletions(-) diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp index cdcae9b3..53eb4c0d 100644 --- a/src/mcl_3dl.cpp +++ b/src/mcl_3dl.cpp @@ -417,26 +417,6 @@ class MCL3dlNode ds.setLeafSize(params_.downsample_x, params_.downsample_y, params_.downsample_z); ds.filter(*pc_local_full); - std::uniform_real_distribution ud(0.0, 1.0); - auto random_sample = [this]( - const pcl::PointCloud::Ptr &pc, const size_t &num) - -> pcl::PointCloud::Ptr - { - pcl::PointCloud::Ptr output(new pcl::PointCloud); - std::uniform_int_distribution ud(0, pc->points.size() - 1); - - for (size_t i = 0; i < num; i++) - { - output->points.push_back(pc->points[ud(engine_)]); - } - output->width = 1; - output->height = output->points.size(); - output->header.frame_id = pc->header.frame_id; - output->header.stamp = pc->header.stamp; - - return output; - }; - std::map::Ptr> pc_locals; for (auto &lm : lidar_measurements_) { From 4c5350c2218f0aa0ed435fc3ec9a12bdb821d578 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 19 Sep 2018 10:59:22 +0900 Subject: [PATCH 10/29] Add comment --- .../lidar_measurement_models/lidar_measurement_model_beam.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h index a6ec398b..27302dd9 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h @@ -50,6 +50,7 @@ namespace mcl_3dl template class LidarMeasurementModelBeam : public LidarMeasurementModelBase { + // POINT_TYPE must have label field (currently using intensity field as a label) static_assert(std::is_same(), "Supported POINT_TYPE is PointXYZI"); private: From 7caf1e00fc3d8b3b72d89f5a4904265bba4e263a Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 19 Sep 2018 12:05:25 +0900 Subject: [PATCH 11/29] Transfar old parameter values to new ones --- .../mcl_3dl/lidar_measurement_model_base.h | 1 + .../lidar_measurement_model_beam.h | 15 ++- .../lidar_measurement_model_likelihood.h | 4 + include/mcl_3dl_compat/compatibility.h | 26 +++++ src/mcl_3dl.cpp | 109 +++++++----------- 5 files changed, 86 insertions(+), 69 deletions(-) diff --git a/include/mcl_3dl/lidar_measurement_model_base.h b/include/mcl_3dl/lidar_measurement_model_base.h index 0112adf5..ac33db5d 100644 --- a/include/mcl_3dl/lidar_measurement_model_base.h +++ b/include/mcl_3dl/lidar_measurement_model_base.h @@ -59,6 +59,7 @@ class LidarMeasurementModelBase const std::string &name) = 0; virtual void setGlobalLocalizationStatus( const size_t, const size_t) = 0; + virtual float getMaxSearchRange() const = 0; virtual typename pcl::PointCloud::Ptr filter( const typename pcl::PointCloud::ConstPtr &) const = 0; diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h index 27302dd9..9d1c8ed9 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h @@ -72,6 +72,12 @@ class LidarMeasurementModelBeam : public LidarMeasurementModelBase sampler_; public: + LidarMeasurementModelBeam(const float x, const float y, const float z) + { + map_grid_min_ = std::min(std::min(x, y), z); + map_grid_max_ = std::max(std::max(x, y), z); + } + void loadConfig( const ros::NodeHandle &nh, const std::string &name) @@ -126,10 +132,13 @@ class LidarMeasurementModelBeam : public LidarMeasurementModelBase::Ptr filter( diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h index 477fa521..4423ca05 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h @@ -113,6 +113,10 @@ class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase::Ptr filter( const typename pcl::PointCloud::ConstPtr &pc) const diff --git a/include/mcl_3dl_compat/compatibility.h b/include/mcl_3dl_compat/compatibility.h index bd54fceb..e8b15c2d 100644 --- a/include/mcl_3dl_compat/compatibility.h +++ b/include/mcl_3dl_compat/compatibility.h @@ -194,6 +194,32 @@ ros::ServiceServer advertiseService( return nh_new.advertiseService(service_new, srv_func, obj); } } + +template +void param_rename( + ros::NodeHandle &nh, + const std::string ¶m_name_new, + const std::string ¶m_name_old) +{ + if (nh.hasParam(param_name_old)) + { + ROS_ERROR( + "Use %s parameter instead of %s", + nh.resolveName(param_name_new, false).c_str(), + nh.resolveName(param_name_old, false).c_str()); + if (nh.hasParam(param_name_new)) + { + ROS_ERROR( + "%s is also defined. Ignoring %s.", + nh.resolveName(param_name_new, false).c_str(), + nh.resolveName(param_name_old, false).c_str()); + return; + } + T value; + nh.getParam(param_name_old, value); + nh.setParam(param_name_new, value); + } +} } // namespace mcl_3dl_compat #endif // MCL_3DL_COMPAT_COMPATIBILITY_H diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp index 53eb4c0d..5d9eef1a 100644 --- a/src/mcl_3dl.cpp +++ b/src/mcl_3dl.cpp @@ -27,13 +27,15 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include #include -#include #include -#include +#include +#include #include +#include + +#include +#include #include #include @@ -84,18 +86,6 @@ class MCL3dlNode class Parameters { public: - double clip_near; - double clip_far; - double clip_near_sq; - double clip_far_sq; - double clip_z_min; - double clip_z_max; - double clip_beam_near; - double clip_beam_far; - double clip_beam_near_sq; - double clip_beam_far_sq; - double clip_beam_z_min; - double clip_beam_z_max; double map_downsample_x; double map_downsample_y; double map_downsample_z; @@ -122,9 +112,6 @@ class MCL3dlNode double expansion_var_pitch; double expansion_var_yaw; double match_ratio_thresh; - double match_dist_min; - double match_dist_flat; - double match_weight; double jump_dist; double jump_ang; double fix_dist; @@ -135,18 +122,12 @@ class MCL3dlNode double odom_err_ang_ang; std::shared_ptr map_update_interval; int num_particles; - int num_points; - int num_points_global; - int num_points_beam; int skip_measure; int accum_cloud; double match_output_dist; double unmatch_output_dist; double bias_var_dist; double bias_var_ang; - float beam_likelihood; - double beam_likelihood_min; - float sin_total_ref; double acc_var; double odom_err_integ_lin_tc; double odom_err_integ_lin_sigma; @@ -501,6 +482,7 @@ class MCL3dlNode e.rot.normalize(); + if (lidar_measurements_["beam"]) { visualization_msgs::MarkerArray markers; @@ -549,6 +531,10 @@ class MCL3dlNode markers.markers.push_back(marker); } + const auto beam_model = + std::dynamic_pointer_cast>( + lidar_measurements_["beam"]); + const float sin_total_ref = beam_model->getSinTotalRef(); for (auto &p : pc_particle_beam->points) { const int beam_header_id = lroundf(p.intensity); @@ -582,7 +568,7 @@ class MCL3dlNode marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 0.0; - if (point.sin_angle_ < params_.sin_total_ref) + if (point.sin_angle_ < sin_total_ref) { marker.color.a = 0.2; } @@ -1153,18 +1139,25 @@ class MCL3dlNode pnh_.param("robot_frame", frame_ids_["base_link"], std::string("base_link")); pnh_.param("odom_frame", frame_ids_["odom"], std::string("odom")); pnh_.param("floor_frame", frame_ids_["floor"], std::string("floor")); - pnh_.param("clip_near", params_.clip_near, 0.5); - pnh_.param("clip_far", params_.clip_far, 10.0); - params_.clip_near_sq = pow(params_.clip_near, 2.0); - params_.clip_far_sq = pow(params_.clip_far, 2.0); - pnh_.param("clip_z_min", params_.clip_z_min, -2.0); - pnh_.param("clip_z_max", params_.clip_z_max, 2.0); - pnh_.param("clip_beam_near", params_.clip_beam_near, 0.5); - pnh_.param("clip_beam_far", params_.clip_beam_far, 4.0); - params_.clip_beam_near_sq = pow(params_.clip_beam_near, 2.0); - params_.clip_beam_far_sq = pow(params_.clip_beam_far, 2.0); - pnh_.param("clip_beam_z_min", params_.clip_beam_z_min, -2.0); - pnh_.param("clip_beam_z_max", params_.clip_beam_z_max, 2.0); + + mcl_3dl_compat::param_rename(pnh_, "clip_near", "likelihood/clip_near"); + mcl_3dl_compat::param_rename(pnh_, "clip_far", "likelihood/clip_far"); + mcl_3dl_compat::param_rename(pnh_, "clip_z_min", "likelihood/clip_z_min"); + mcl_3dl_compat::param_rename(pnh_, "clip_z_max", "likelihood/clip_z_max"); + mcl_3dl_compat::param_rename(pnh_, "match_dist_min", "likelihood/match_dist_min"); + mcl_3dl_compat::param_rename(pnh_, "match_dist_flat", "likelihood/match_dist_flat"); + mcl_3dl_compat::param_rename(pnh_, "match_weight", "likelihood/match_weight"); + mcl_3dl_compat::param_rename(pnh_, "num_points", "likelihood/num_points"); + mcl_3dl_compat::param_rename(pnh_, "num_points_global", "likelihood/num_points_global"); + + mcl_3dl_compat::param_rename(pnh_, "clip_beam_near", "beam/clip_near"); + mcl_3dl_compat::param_rename(pnh_, "clip_beam_far", "beam/clip_far"); + mcl_3dl_compat::param_rename(pnh_, "clip_beam_z_min", "beam/clip_z_min"); + mcl_3dl_compat::param_rename(pnh_, "clip_beam_z_max", "beam/clip_z_max"); + mcl_3dl_compat::param_rename(pnh_, "num_points_beam", "likelihood/num_points"); + mcl_3dl_compat::param_rename(pnh_, "beam_likelihood", "likelihood/beam_likelihood"); + mcl_3dl_compat::param_rename(pnh_, "ang_total_ref", "likelihood/ang_total_ref"); + pnh_.param("map_downsample_x", params_.map_downsample_x, 0.1); pnh_.param("map_downsample_y", params_.map_downsample_y, 0.1); pnh_.param("map_downsample_z", params_.map_downsample_z, 0.1); @@ -1182,10 +1175,6 @@ class MCL3dlNode pnh_.param("map_update_interval_interval", map_update_interval_t, 2.0); params_.map_update_interval.reset(new ros::Duration(map_update_interval_t)); - pnh_.param("match_dist_min", params_.match_dist_min, 0.2); - pnh_.param("match_dist_flat", params_.match_dist_flat, 0.05); - pnh_.param("match_weight", params_.match_weight, 5.0); - double weight[3]; float weight_f[4]; pnh_.param("dist_weight_x", weight[0], 1.0); @@ -1196,21 +1185,6 @@ class MCL3dlNode weight_f[3] = 0.0; point_rep_.setRescaleValues(weight_f); - double map_chunk; - pnh_.param("map_chunk", map_chunk, 20.0); - const double max_search_radius = std::max( - params_.unmatch_output_dist, - std::max( - params_.match_output_dist, - std::max(params_.match_dist_min, params_.map_grid_max * 4.0))); - ROS_DEBUG("max_search_radius: %0.3f", max_search_radius); - kdtree_.reset(new ChunkedKdtree(map_chunk, max_search_radius)); - kdtree_->setEpsilon(params_.map_grid_min / 16); - kdtree_->setPointRepresentation( - boost::dynamic_pointer_cast< - pcl::PointRepresentation, - MyPointRepresentation>(boost::make_shared(point_rep_))); - pnh_.param("global_localization_grid_lin", params_.global_localization_grid, 0.3); double grid_ang; pnh_.param("global_localization_grid_ang", grid_ang, 0.524); @@ -1218,15 +1192,6 @@ class MCL3dlNode pnh_.param("num_particles", params_.num_particles, 64); pf_.reset(new pf::ParticleFilter(params_.num_particles)); - pnh_.param("num_points", params_.num_points, 96); - pnh_.param("num_points_global", params_.num_points_global, 8); - pnh_.param("num_points_beam", params_.num_points_beam, 3); - - pnh_.param("beam_likelihood", params_.beam_likelihood_min, 0.2); - params_.beam_likelihood = powf(params_.beam_likelihood_min, 1.0 / static_cast(params_.num_points_beam)); - double ang_total_ref; - pnh_.param("ang_total_ref", ang_total_ref, M_PI / 6.0); - params_.sin_total_ref = sinf(ang_total_ref); pnh_.param("resample_var_x", params_.resample_var_x, 0.05); pnh_.param("resample_var_y", params_.resample_var_y, 0.05); @@ -1329,11 +1294,23 @@ class MCL3dlNode new LidarMeasurementModelBeam( params_.map_downsample_x, params_.map_downsample_y, params_.map_downsample_z)); + float max_search_radius = 0; for (auto &lm : lidar_measurements_) { lm.second->loadConfig(pnh_, lm.first); + max_search_radius = std::max(max_search_radius, lm.second->getMaxSearchRange()); } + double map_chunk; + pnh_.param("map_chunk", map_chunk, 20.0); + ROS_DEBUG("max_search_radius: %0.3f", max_search_radius); + kdtree_.reset(new ChunkedKdtree(map_chunk, max_search_radius)); + kdtree_->setEpsilon(params_.map_grid_min / 16); + kdtree_->setPointRepresentation( + boost::dynamic_pointer_cast< + pcl::PointRepresentation, + MyPointRepresentation>(boost::make_shared(point_rep_))); + map_update_timer_ = nh_.createTimer( *params_.map_update_interval, &MCL3dlNode::cbMapUpdateTimer, this); From 3c87670df9ecc31e1b7cb4e72efba27006fdd430 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 19 Sep 2018 12:06:18 +0900 Subject: [PATCH 12/29] Remove unused parameters --- .../lidar_measurement_model_beam.h | 8 -------- 1 file changed, 8 deletions(-) diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h index 9d1c8ed9..dcf0f6c2 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h @@ -61,8 +61,6 @@ class LidarMeasurementModelBeam : public LidarMeasurementModelBase Date: Wed, 19 Sep 2018 12:07:32 +0900 Subject: [PATCH 13/29] Change log output level --- 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 5d9eef1a..5ef67119 100644 --- a/src/mcl_3dl.cpp +++ b/src/mcl_3dl.cpp @@ -413,7 +413,7 @@ class MCL3dlNode } if (pc_locals["beam"] && pc_locals["beam"]->size() == 0) { - ROS_ERROR("All beam points are filtered out. Skipping beam model."); + ROS_DEBUG("All beam points are filtered out. Skipping beam model."); } float match_ratio_min = 1.0; From d024a99a22b7c1ff5cea0f06140540db8b0f24f3 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 19 Sep 2018 12:19:50 +0900 Subject: [PATCH 14/29] Fix parameter name migration rule --- src/mcl_3dl.cpp | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp index 5ef67119..e8f27037 100644 --- a/src/mcl_3dl.cpp +++ b/src/mcl_3dl.cpp @@ -1140,23 +1140,23 @@ class MCL3dlNode pnh_.param("odom_frame", frame_ids_["odom"], std::string("odom")); pnh_.param("floor_frame", frame_ids_["floor"], std::string("floor")); - mcl_3dl_compat::param_rename(pnh_, "clip_near", "likelihood/clip_near"); - mcl_3dl_compat::param_rename(pnh_, "clip_far", "likelihood/clip_far"); - mcl_3dl_compat::param_rename(pnh_, "clip_z_min", "likelihood/clip_z_min"); - mcl_3dl_compat::param_rename(pnh_, "clip_z_max", "likelihood/clip_z_max"); - mcl_3dl_compat::param_rename(pnh_, "match_dist_min", "likelihood/match_dist_min"); - mcl_3dl_compat::param_rename(pnh_, "match_dist_flat", "likelihood/match_dist_flat"); - mcl_3dl_compat::param_rename(pnh_, "match_weight", "likelihood/match_weight"); - mcl_3dl_compat::param_rename(pnh_, "num_points", "likelihood/num_points"); - mcl_3dl_compat::param_rename(pnh_, "num_points_global", "likelihood/num_points_global"); - - mcl_3dl_compat::param_rename(pnh_, "clip_beam_near", "beam/clip_near"); - mcl_3dl_compat::param_rename(pnh_, "clip_beam_far", "beam/clip_far"); - mcl_3dl_compat::param_rename(pnh_, "clip_beam_z_min", "beam/clip_z_min"); - mcl_3dl_compat::param_rename(pnh_, "clip_beam_z_max", "beam/clip_z_max"); - mcl_3dl_compat::param_rename(pnh_, "num_points_beam", "likelihood/num_points"); - mcl_3dl_compat::param_rename(pnh_, "beam_likelihood", "likelihood/beam_likelihood"); - mcl_3dl_compat::param_rename(pnh_, "ang_total_ref", "likelihood/ang_total_ref"); + mcl_3dl_compat::param_rename(pnh_, "likelihood/clip_near", "clip_near"); + mcl_3dl_compat::param_rename(pnh_, "likelihood/clip_far", "clip_far"); + mcl_3dl_compat::param_rename(pnh_, "likelihood/clip_z_min", "clip_z_min"); + mcl_3dl_compat::param_rename(pnh_, "likelihood/clip_z_max", "clip_z_max"); + mcl_3dl_compat::param_rename(pnh_, "likelihood/match_dist_min", "match_dist_min"); + mcl_3dl_compat::param_rename(pnh_, "likelihood/match_dist_flat", "match_dist_flat"); + mcl_3dl_compat::param_rename(pnh_, "likelihood/match_weight", "match_weight"); + mcl_3dl_compat::param_rename(pnh_, "likelihood/num_points", "num_points"); + mcl_3dl_compat::param_rename(pnh_, "likelihood/num_points_global", "num_points_global"); + + mcl_3dl_compat::param_rename(pnh_, "beam/clip_near", "clip_beam_near"); + mcl_3dl_compat::param_rename(pnh_, "beam/clip_far", "clip_beam_far"); + mcl_3dl_compat::param_rename(pnh_, "beam/clip_z_min", "clip_beam_z_min"); + mcl_3dl_compat::param_rename(pnh_, "beam/clip_z_max", "clip_beam_z_max"); + mcl_3dl_compat::param_rename(pnh_, "beam/num_points", "num_points_beam"); + mcl_3dl_compat::param_rename(pnh_, "beam/beam_likelihood", "beam_likelihood"); + mcl_3dl_compat::param_rename(pnh_, "beam/ang_total_ref", "ang_total_ref"); pnh_.param("map_downsample_x", params_.map_downsample_x, 0.1); pnh_.param("map_downsample_y", params_.map_downsample_y, 0.1); From a28be1486b7501824a5fe5de2c79f3d55a53a701 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 19 Sep 2018 12:31:44 +0900 Subject: [PATCH 15/29] Fix function name --- include/mcl_3dl_compat/compatibility.h | 2 +- src/mcl_3dl.cpp | 34 +++++++++++++------------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/include/mcl_3dl_compat/compatibility.h b/include/mcl_3dl_compat/compatibility.h index e8b15c2d..ac90c703 100644 --- a/include/mcl_3dl_compat/compatibility.h +++ b/include/mcl_3dl_compat/compatibility.h @@ -196,7 +196,7 @@ ros::ServiceServer advertiseService( } template -void param_rename( +void paramRename( ros::NodeHandle &nh, const std::string ¶m_name_new, const std::string ¶m_name_old) diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp index e8f27037..21a91ca5 100644 --- a/src/mcl_3dl.cpp +++ b/src/mcl_3dl.cpp @@ -1140,23 +1140,23 @@ class MCL3dlNode pnh_.param("odom_frame", frame_ids_["odom"], std::string("odom")); pnh_.param("floor_frame", frame_ids_["floor"], std::string("floor")); - mcl_3dl_compat::param_rename(pnh_, "likelihood/clip_near", "clip_near"); - mcl_3dl_compat::param_rename(pnh_, "likelihood/clip_far", "clip_far"); - mcl_3dl_compat::param_rename(pnh_, "likelihood/clip_z_min", "clip_z_min"); - mcl_3dl_compat::param_rename(pnh_, "likelihood/clip_z_max", "clip_z_max"); - mcl_3dl_compat::param_rename(pnh_, "likelihood/match_dist_min", "match_dist_min"); - mcl_3dl_compat::param_rename(pnh_, "likelihood/match_dist_flat", "match_dist_flat"); - mcl_3dl_compat::param_rename(pnh_, "likelihood/match_weight", "match_weight"); - mcl_3dl_compat::param_rename(pnh_, "likelihood/num_points", "num_points"); - mcl_3dl_compat::param_rename(pnh_, "likelihood/num_points_global", "num_points_global"); - - mcl_3dl_compat::param_rename(pnh_, "beam/clip_near", "clip_beam_near"); - mcl_3dl_compat::param_rename(pnh_, "beam/clip_far", "clip_beam_far"); - mcl_3dl_compat::param_rename(pnh_, "beam/clip_z_min", "clip_beam_z_min"); - mcl_3dl_compat::param_rename(pnh_, "beam/clip_z_max", "clip_beam_z_max"); - mcl_3dl_compat::param_rename(pnh_, "beam/num_points", "num_points_beam"); - mcl_3dl_compat::param_rename(pnh_, "beam/beam_likelihood", "beam_likelihood"); - mcl_3dl_compat::param_rename(pnh_, "beam/ang_total_ref", "ang_total_ref"); + mcl_3dl_compat::paramRename(pnh_, "likelihood/clip_near", "clip_near"); + mcl_3dl_compat::paramRename(pnh_, "likelihood/clip_far", "clip_far"); + mcl_3dl_compat::paramRename(pnh_, "likelihood/clip_z_min", "clip_z_min"); + mcl_3dl_compat::paramRename(pnh_, "likelihood/clip_z_max", "clip_z_max"); + mcl_3dl_compat::paramRename(pnh_, "likelihood/match_dist_min", "match_dist_min"); + mcl_3dl_compat::paramRename(pnh_, "likelihood/match_dist_flat", "match_dist_flat"); + mcl_3dl_compat::paramRename(pnh_, "likelihood/match_weight", "match_weight"); + mcl_3dl_compat::paramRename(pnh_, "likelihood/num_points", "num_points"); + mcl_3dl_compat::paramRename(pnh_, "likelihood/num_points_global", "num_points_global"); + + mcl_3dl_compat::paramRename(pnh_, "beam/clip_near", "clip_beam_near"); + mcl_3dl_compat::paramRename(pnh_, "beam/clip_far", "clip_beam_far"); + mcl_3dl_compat::paramRename(pnh_, "beam/clip_z_min", "clip_beam_z_min"); + mcl_3dl_compat::paramRename(pnh_, "beam/clip_z_max", "clip_beam_z_max"); + mcl_3dl_compat::paramRename(pnh_, "beam/num_points", "num_points_beam"); + mcl_3dl_compat::paramRename(pnh_, "beam/beam_likelihood", "beam_likelihood"); + mcl_3dl_compat::paramRename(pnh_, "beam/ang_total_ref", "ang_total_ref"); pnh_.param("map_downsample_x", params_.map_downsample_x, 0.1); pnh_.param("map_downsample_y", params_.map_downsample_y, 0.1); From 6c4befcf276096bfd7f114417f9fd07be495f929 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 19 Sep 2018 12:32:04 +0900 Subject: [PATCH 16/29] Update parameter name in test launches --- test/tests/expansion_resetting_rostest.test | 2 +- test/tests/global_localization_rostest.test | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/test/tests/expansion_resetting_rostest.test b/test/tests/expansion_resetting_rostest.test index 5b7feb51..b2cc791f 100644 --- a/test/tests/expansion_resetting_rostest.test +++ b/test/tests/expansion_resetting_rostest.test @@ -7,7 +7,7 @@ - + diff --git a/test/tests/global_localization_rostest.test b/test/tests/global_localization_rostest.test index 7ec3f8c5..67036671 100644 --- a/test/tests/global_localization_rostest.test +++ b/test/tests/global_localization_rostest.test @@ -7,8 +7,8 @@ + - From 5cd5ff2a42b40ecced8add89dce757a90b6b4c5a Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 19 Sep 2018 12:52:45 +0900 Subject: [PATCH 17/29] Add test for mcl_3dl_compat::paramRename --- test/CMakeLists.txt | 4 ++ test/src/test_mcl_3dl_compat.cpp | 73 ++++++++++++++++++++++++++ test/tests/mcl_3dl_compat_rostest.test | 4 ++ 3 files changed, 81 insertions(+) create mode 100644 test/src/test_mcl_3dl_compat.cpp create mode 100644 test/tests/mcl_3dl_compat_rostest.test diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 9110be33..bc6883bc 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -28,6 +28,10 @@ add_rostest_gtest(test_global_localization tests/global_localization_rostest.tes target_link_libraries(test_global_localization ${catkin_LIBRARIES}) add_dependencies(test_global_localization mcl_3dl) +add_rostest_gtest(test_mcl_3dl_compat tests/mcl_3dl_compat_rostest.test + src/test_mcl_3dl_compat.cpp) +target_link_libraries(test_mcl_3dl_compat ${catkin_LIBRARIES}) + add_rostest_gtest(test_transform_failure tests/transform_rostest.test src/test_transform_failure.cpp) target_link_libraries(test_transform_failure ${catkin_LIBRARIES}) diff --git a/test/src/test_mcl_3dl_compat.cpp b/test/src/test_mcl_3dl_compat.cpp new file mode 100644 index 00000000..402a0414 --- /dev/null +++ b/test/src/test_mcl_3dl_compat.cpp @@ -0,0 +1,73 @@ +/* + * Copyright (c) 2018, the mcl_3dl authors + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include + +#include + +TEST(Mcl3DlCompat, ParamRename) +{ + ros::NodeHandle nh; + + nh.setParam("param1", 1.0); + nh.setParam("param2", 2.0); + nh.setParam("param2_new", 3.0); + nh.setParam("param3_new", 4.0); + + mcl_3dl_compat::paramRename(nh, "param1_new", "param1"); + mcl_3dl_compat::paramRename(nh, "param2_new", "param2"); + + ASSERT_TRUE(nh.hasParam("param1_new")); + ASSERT_TRUE(nh.hasParam("param2_new")); + ASSERT_TRUE(nh.hasParam("param3_new")); + + double param1; + double param2; + double param3; + nh.getParam("param1_new", param1); + nh.getParam("param2_new", param2); + nh.getParam("param3_new", param3); + + // Only old parameter is provided for param1. + ASSERT_EQ(param1, 1.0); + // Both old and new paramters are provided for param2. New one must be active. + ASSERT_EQ(param2, 3.0); + // Only new paramter is provided for param3. + ASSERT_EQ(param3, 4.0); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_mcl_3dl_compat"); + + return RUN_ALL_TESTS(); +} diff --git a/test/tests/mcl_3dl_compat_rostest.test b/test/tests/mcl_3dl_compat_rostest.test new file mode 100644 index 00000000..3b9daa9c --- /dev/null +++ b/test/tests/mcl_3dl_compat_rostest.test @@ -0,0 +1,4 @@ + + + + From 9a8a45198658b0d89a436a6e2cd27b0e12e92469 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 20 Sep 2018 13:55:11 +0900 Subject: [PATCH 18/29] Address review comments --- include/mcl_3dl/lidar_measurement_model_base.h | 5 +++-- .../lidar_measurement_models/lidar_measurement_model_beam.h | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/include/mcl_3dl/lidar_measurement_model_base.h b/include/mcl_3dl/lidar_measurement_model_base.h index ac33db5d..d1f0e547 100644 --- a/include/mcl_3dl/lidar_measurement_model_base.h +++ b/include/mcl_3dl/lidar_measurement_model_base.h @@ -48,8 +48,9 @@ template class LidarMeasurementModelBase { static_assert( - std::is_base_of, STATE_TYPE>::value, - "STATE_TYPE is not based on pf::ParticleBase"); + std::is_base_of, STATE_TYPE>::value || + std::is_base_of, STATE_TYPE>::value, + "STATE_TYPE is not based on pf::ParticleBase"); public: using Ptr = std::shared_ptr>; diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h index dcf0f6c2..e4a28779 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h @@ -72,8 +72,9 @@ class LidarMeasurementModelBeam : public LidarMeasurementModelBase Date: Thu, 20 Sep 2018 14:42:33 +0900 Subject: [PATCH 19/29] Address review comments --- .../lidar_measurement_models/lidar_measurement_model_beam.h | 4 ++-- .../lidar_measurement_model_likelihood.h | 4 ++-- include/mcl_3dl/point_cloud_random_sampler.h | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h index e4a28779..b30cd0ae 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h @@ -67,7 +67,7 @@ class LidarMeasurementModelBeam : public LidarMeasurementModelBase sampler_; + PointCloudRandomSampler sampler_; public: LidarMeasurementModelBeam(const float x, const float y, const float z) @@ -156,7 +156,7 @@ class LidarMeasurementModelBeam : public LidarMeasurementModelBasewidth = 1; pc_filtered->height = pc_filtered->points.size(); - return sampler_.sample(pc_filtered, num_points_); + return sampler_.sample(pc_filtered, num_points_); } std::pair measure( diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h index 4423ca05..c93faed2 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h @@ -61,7 +61,7 @@ class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase sampler_; + PointCloudRandomSampler sampler_; public: void loadConfig( @@ -138,7 +138,7 @@ class LidarMeasurementModelLikelihood : public LidarMeasurementModelBasewidth = 1; pc_filtered->height = pc_filtered->points.size(); - return sampler_.sample(pc_filtered, num_points_); + return sampler_.sample(pc_filtered, num_points_); } std::pair measure( diff --git a/include/mcl_3dl/point_cloud_random_sampler.h b/include/mcl_3dl/point_cloud_random_sampler.h index 4fe76fd1..15dcf42e 100644 --- a/include/mcl_3dl/point_cloud_random_sampler.h +++ b/include/mcl_3dl/point_cloud_random_sampler.h @@ -37,7 +37,6 @@ namespace mcl_3dl { -template class PointCloudRandomSampler { private: @@ -49,6 +48,7 @@ class PointCloudRandomSampler : engine_(new std::default_random_engine(seed_gen_())) { } + template typename pcl::PointCloud::Ptr sample( const typename pcl::PointCloud::ConstPtr &pc, const size_t num) const From f9444b8a072b65632fd596406a8dbcee802f1c34 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 20 Sep 2018 15:00:11 +0900 Subject: [PATCH 20/29] Fix test --- test/src/test_point_cloud_random_sampler.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/src/test_point_cloud_random_sampler.cpp b/test/src/test_point_cloud_random_sampler.cpp index 3c489dd1..aa3f4351 100644 --- a/test/src/test_point_cloud_random_sampler.cpp +++ b/test/src/test_point_cloud_random_sampler.cpp @@ -53,11 +53,11 @@ TEST(PointCloudRandomSampler, Sampling) pc_input->push_back(pcl::PointXYZ(p_ref[0], p_ref[1], p_ref[2])); } - mcl_3dl::PointCloudRandomSampler sampler; + mcl_3dl::PointCloudRandomSampler sampler; for (size_t num = 1; num < 4; num++) { - pcl::PointCloud::Ptr pc_output = sampler.sample(pc_input, num); + pcl::PointCloud::Ptr pc_output = sampler.sample(pc_input, num); // Check header and number of the points ASSERT_EQ(pc_output->header.frame_id, pc_input->header.frame_id); @@ -79,7 +79,7 @@ TEST(PointCloudRandomSampler, Sampling) } // Make sure that the sampler returns 0 point output for 0 point input - pcl::PointCloud::Ptr pc_output0 = sampler.sample(pc_input, 0); + pcl::PointCloud::Ptr pc_output0 = sampler.sample(pc_input, 0); ASSERT_EQ(pc_output0->points.size(), 0u); } From b37c72e8d869929bb42040d7da553683f3ec41b8 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Fri, 21 Sep 2018 12:28:06 +0900 Subject: [PATCH 21/29] Remove STATE_TYPE from LidarMeasurementModel template parameter --- include/mcl_3dl/lidar_measurement_model_base.h | 15 +++++---------- .../lidar_measurement_model_beam.h | 8 ++++---- .../lidar_measurement_model_likelihood.h | 8 ++++---- src/mcl_3dl.cpp | 12 ++++++------ 4 files changed, 19 insertions(+), 24 deletions(-) diff --git a/include/mcl_3dl/lidar_measurement_model_base.h b/include/mcl_3dl/lidar_measurement_model_base.h index d1f0e547..2a8bc078 100644 --- a/include/mcl_3dl/lidar_measurement_model_base.h +++ b/include/mcl_3dl/lidar_measurement_model_base.h @@ -39,21 +39,16 @@ #include #include -#include +#include #include namespace mcl_3dl { -template +template class LidarMeasurementModelBase { - static_assert( - std::is_base_of, STATE_TYPE>::value || - std::is_base_of, STATE_TYPE>::value, - "STATE_TYPE is not based on pf::ParticleBase"); - public: - using Ptr = std::shared_ptr>; + using Ptr = std::shared_ptr>; virtual void loadConfig( const ros::NodeHandle &nh, @@ -66,10 +61,10 @@ class LidarMeasurementModelBase const typename pcl::PointCloud::ConstPtr &) const = 0; virtual std::pair measure( - typename mcl_3dl::ChunkedKdtree::Ptr &, + typename ChunkedKdtree::Ptr &, const typename pcl::PointCloud::ConstPtr &, const std::vector &, - const STATE_TYPE &) const = 0; + const State6DOF &) const = 0; }; } // namespace mcl_3dl diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h index b30cd0ae..81c4092a 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h @@ -47,8 +47,8 @@ namespace mcl_3dl { -template -class LidarMeasurementModelBeam : public LidarMeasurementModelBase +template +class LidarMeasurementModelBeam : public LidarMeasurementModelBase { // POINT_TYPE must have label field (currently using intensity field as a label) static_assert(std::is_same(), "Supported POINT_TYPE is PointXYZI"); @@ -160,10 +160,10 @@ class LidarMeasurementModelBeam : public LidarMeasurementModelBase measure( - typename mcl_3dl::ChunkedKdtree::Ptr &kdtree, + typename ChunkedKdtree::Ptr &kdtree, const typename pcl::PointCloud::ConstPtr &pc, const std::vector &origins, - const STATE_TYPE &s) const + const State6DOF &s) const { if (!pc) return std::pair(1, 0); diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h index c93faed2..842cadb0 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h @@ -46,8 +46,8 @@ namespace mcl_3dl { -template -class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase +template +class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase { private: size_t num_points_; @@ -142,10 +142,10 @@ class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase measure( - typename mcl_3dl::ChunkedKdtree::Ptr &kdtree, + typename ChunkedKdtree::Ptr &kdtree, const typename pcl::PointCloud::ConstPtr &pc, const std::vector &origins, - const STATE_TYPE &s) const + const State6DOF &s) const { if (!pc) return std::pair(1, 0); diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp index 21a91ca5..dc4be7d0 100644 --- a/src/mcl_3dl.cpp +++ b/src/mcl_3dl.cpp @@ -532,7 +532,7 @@ class MCL3dlNode markers.markers.push_back(marker); } const auto beam_model = - std::dynamic_pointer_cast>( + std::dynamic_pointer_cast>( lidar_measurements_["beam"]); const float sin_total_ref = beam_model->getSinTotalRef(); for (auto &p : pc_particle_beam->points) @@ -1287,11 +1287,11 @@ class MCL3dlNode localize_rate_.reset(new Filter(Filter::FILTER_LPF, 5.0, 0.0)); lidar_measurements_["likelihood"] = - LidarMeasurementModelBase::Ptr( - new LidarMeasurementModelLikelihood()); + LidarMeasurementModelBase::Ptr( + new LidarMeasurementModelLikelihood()); lidar_measurements_["beam"] = - LidarMeasurementModelBase::Ptr( - new LidarMeasurementModelBeam( + LidarMeasurementModelBase::Ptr( + new LidarMeasurementModelBeam( params_.map_downsample_x, params_.map_downsample_y, params_.map_downsample_z)); float max_search_radius = 0; @@ -1421,7 +1421,7 @@ class MCL3dlNode std::map< std::string, - LidarMeasurementModelBase::Ptr> lidar_measurements_; + LidarMeasurementModelBase::Ptr> lidar_measurements_; std::random_device seed_gen_; std::default_random_engine engine_; From de81133a9fbd0c54c8b73980a63505712b699a38 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Fri, 21 Sep 2018 13:09:31 +0900 Subject: [PATCH 22/29] Remove POINT_TYPE from LidarMeasurementModel template parameter --- include/mcl_3dl/lidar_measurement_model_base.h | 12 ++++++------ .../lidar_measurement_model_beam.h | 18 +++++++----------- .../lidar_measurement_model_likelihood.h | 15 +++++++-------- src/mcl_3dl.cpp | 12 ++++++------ 4 files changed, 26 insertions(+), 31 deletions(-) diff --git a/include/mcl_3dl/lidar_measurement_model_base.h b/include/mcl_3dl/lidar_measurement_model_base.h index 2a8bc078..7fd51cf9 100644 --- a/include/mcl_3dl/lidar_measurement_model_base.h +++ b/include/mcl_3dl/lidar_measurement_model_base.h @@ -44,11 +44,11 @@ namespace mcl_3dl { -template class LidarMeasurementModelBase { public: - using Ptr = std::shared_ptr>; + using Ptr = std::shared_ptr; + using PointType = pcl::PointXYZI; virtual void loadConfig( const ros::NodeHandle &nh, @@ -57,12 +57,12 @@ class LidarMeasurementModelBase const size_t, const size_t) = 0; virtual float getMaxSearchRange() const = 0; - virtual typename pcl::PointCloud::Ptr filter( - const typename pcl::PointCloud::ConstPtr &) const = 0; + virtual typename pcl::PointCloud::Ptr filter( + const typename pcl::PointCloud::ConstPtr &) const = 0; virtual std::pair measure( - typename ChunkedKdtree::Ptr &, - const typename pcl::PointCloud::ConstPtr &, + typename ChunkedKdtree::Ptr &, + const typename pcl::PointCloud::ConstPtr &, const std::vector &, const State6DOF &) const = 0; }; diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h index 81c4092a..ba63f8d8 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h @@ -47,12 +47,8 @@ namespace mcl_3dl { -template -class LidarMeasurementModelBeam : public LidarMeasurementModelBase +class LidarMeasurementModelBeam : public LidarMeasurementModelBase { - // POINT_TYPE must have label field (currently using intensity field as a label) - static_assert(std::is_same(), "Supported POINT_TYPE is PointXYZI"); - private: size_t num_points_; size_t num_points_default_; @@ -134,10 +130,10 @@ class LidarMeasurementModelBeam : public LidarMeasurementModelBase return sin_total_ref_; } - typename pcl::PointCloud::Ptr filter( - const typename pcl::PointCloud::ConstPtr &pc) const + typename pcl::PointCloud::Ptr filter( + const typename pcl::PointCloud::ConstPtr &pc) const { - const auto local_points_filter = [this](const POINT_TYPE &p) + const auto local_points_filter = [this](const PointType &p) { if (p.x * p.x + p.y * p.y > clip_far_sq_) return true; @@ -156,12 +152,12 @@ class LidarMeasurementModelBeam : public LidarMeasurementModelBase pc_filtered->width = 1; pc_filtered->height = pc_filtered->points.size(); - return sampler_.sample(pc_filtered, num_points_); + return sampler_.sample(pc_filtered, num_points_); } std::pair measure( - typename ChunkedKdtree::Ptr &kdtree, - const typename pcl::PointCloud::ConstPtr &pc, + typename ChunkedKdtree::Ptr &kdtree, + const typename pcl::PointCloud::ConstPtr &pc, const std::vector &origins, const State6DOF &s) const { diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h index 842cadb0..00850a3e 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h @@ -46,8 +46,7 @@ namespace mcl_3dl { -template -class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase +class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase { private: size_t num_points_; @@ -118,10 +117,10 @@ class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase::Ptr filter( - const typename pcl::PointCloud::ConstPtr &pc) const + typename pcl::PointCloud::Ptr filter( + const typename pcl::PointCloud::ConstPtr &pc) const { - const auto local_points_filter = [this](const POINT_TYPE &p) + const auto local_points_filter = [this](const PointType &p) { if (p.x * p.x + p.y * p.y > clip_far_sq_) return true; @@ -138,12 +137,12 @@ class LidarMeasurementModelLikelihood : public LidarMeasurementModelBasewidth = 1; pc_filtered->height = pc_filtered->points.size(); - return sampler_.sample(pc_filtered, num_points_); + return sampler_.sample(pc_filtered, num_points_); } std::pair measure( - typename ChunkedKdtree::Ptr &kdtree, - const typename pcl::PointCloud::ConstPtr &pc, + typename ChunkedKdtree::Ptr &kdtree, + const typename pcl::PointCloud::ConstPtr &pc, const std::vector &origins, const State6DOF &s) const { diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp index dc4be7d0..a8b61b72 100644 --- a/src/mcl_3dl.cpp +++ b/src/mcl_3dl.cpp @@ -532,7 +532,7 @@ class MCL3dlNode markers.markers.push_back(marker); } const auto beam_model = - std::dynamic_pointer_cast>( + std::dynamic_pointer_cast( lidar_measurements_["beam"]); const float sin_total_ref = beam_model->getSinTotalRef(); for (auto &p : pc_particle_beam->points) @@ -1287,11 +1287,11 @@ class MCL3dlNode localize_rate_.reset(new Filter(Filter::FILTER_LPF, 5.0, 0.0)); lidar_measurements_["likelihood"] = - LidarMeasurementModelBase::Ptr( - new LidarMeasurementModelLikelihood()); + LidarMeasurementModelBase::Ptr( + new LidarMeasurementModelLikelihood()); lidar_measurements_["beam"] = - LidarMeasurementModelBase::Ptr( - new LidarMeasurementModelBeam( + LidarMeasurementModelBase::Ptr( + new LidarMeasurementModelBeam( params_.map_downsample_x, params_.map_downsample_y, params_.map_downsample_z)); float max_search_radius = 0; @@ -1421,7 +1421,7 @@ class MCL3dlNode std::map< std::string, - LidarMeasurementModelBase::Ptr> lidar_measurements_; + LidarMeasurementModelBase::Ptr> lidar_measurements_; std::random_device seed_gen_; std::default_random_engine engine_; From b2e8e7180eccc58f13abe21ce2dec1e286e54101 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Fri, 21 Sep 2018 13:35:31 +0900 Subject: [PATCH 23/29] Move impl from .h to .cpp --- CMakeLists.txt | 6 +- .../mcl_3dl/lidar_measurement_model_base.h | 9 +- .../lidar_measurement_model_beam.h | 141 ++------------ .../lidar_measurement_model_likelihood.h | 121 ++---------- src/lidar_measurement_model_beam.cpp | 176 ++++++++++++++++++ src/lidar_measurement_model_likelihood.cpp | 156 ++++++++++++++++ src/mcl_3dl.cpp | 1 + 7 files changed, 373 insertions(+), 237 deletions(-) create mode 100644 src/lidar_measurement_model_beam.cpp create mode 100644 src/lidar_measurement_model_likelihood.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 73024e2b..ee9fbe6e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -85,7 +85,11 @@ add_definitions(${PCL_DEFINITIONS}) # Define PCL_NO_PRECOMPILE to disable using the binary version. add_definitions(-DPCL_NO_PRECOMPILE) -add_executable(mcl_3dl src/mcl_3dl.cpp) +add_executable(mcl_3dl + src/lidar_measurement_model_beam.cpp + src/lidar_measurement_model_likelihood.cpp + src/mcl_3dl.cpp +) target_link_libraries(mcl_3dl ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${PCL_LIBRARIES}) add_dependencies(mcl_3dl ${catkin_EXPORTED_TARGETS}) diff --git a/include/mcl_3dl/lidar_measurement_model_base.h b/include/mcl_3dl/lidar_measurement_model_base.h index 7fd51cf9..9575c4d2 100644 --- a/include/mcl_3dl/lidar_measurement_model_base.h +++ b/include/mcl_3dl/lidar_measurement_model_base.h @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -57,12 +58,12 @@ class LidarMeasurementModelBase const size_t, const size_t) = 0; virtual float getMaxSearchRange() const = 0; - virtual typename pcl::PointCloud::Ptr filter( - const typename pcl::PointCloud::ConstPtr &) const = 0; + virtual pcl::PointCloud::Ptr filter( + const pcl::PointCloud::ConstPtr &) const = 0; virtual std::pair measure( - typename ChunkedKdtree::Ptr &, - const typename pcl::PointCloud::ConstPtr &, + ChunkedKdtree::Ptr &, + const pcl::PointCloud::ConstPtr &, const std::vector &, const State6DOF &) const = 0; }; diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h index ba63f8d8..ce26f3f9 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h @@ -30,9 +30,7 @@ #ifndef MCL_3DL_LIDAR_MEASUREMENT_MODELS_LIDAR_MEASUREMENT_MODEL_BEAM_H #define MCL_3DL_LIDAR_MEASUREMENT_MODELS_LIDAR_MEASUREMENT_MODEL_BEAM_H -#include #include -#include #include #include @@ -40,9 +38,10 @@ #include #include +#include +#include #include #include -#include #include namespace mcl_3dl @@ -66,138 +65,30 @@ class LidarMeasurementModelBeam : public LidarMeasurementModelBase PointCloudRandomSampler sampler_; public: - LidarMeasurementModelBeam(const float x, const float y, const float z) - { - // FIXME(at-wat): remove NOLINT after clang-format or roslint supports it - map_grid_min_ = std::min({ x, y, z }); // NOLINT(whitespace/braces) - map_grid_max_ = std::max({ x, y, z }); // NOLINT(whitespace/braces) - } - - void loadConfig( - const ros::NodeHandle &nh, - const std::string &name) - { - ros::NodeHandle pnh(nh, name); - - int num_points, num_points_global; - pnh.param("num_points", num_points, 3); - pnh.param("num_points_global", num_points_global, 0); - num_points_default_ = num_points_ = num_points; - num_points_global_ = num_points_global; - - double clip_near, clip_far; - pnh.param("clip_near", clip_near, 0.5); - pnh.param("clip_far", clip_far, 4.0); - clip_near_sq_ = clip_near * clip_near; - clip_far_sq_ = clip_far * clip_far; - - double clip_z_min, clip_z_max; - pnh.param("clip_z_min", clip_z_min, -2.0); - pnh.param("clip_z_max", clip_z_max, 2.0); - clip_z_min_ = clip_z_min; - clip_z_max_ = clip_z_max; - - double beam_likelihood_min; - pnh.param("beam_likelihood", beam_likelihood_min, 0.2); - beam_likelihood_min_ = beam_likelihood_min; - beam_likelihood_ = powf(beam_likelihood_min, 1.0 / static_cast(num_points)); - - double ang_total_ref; - pnh.param("ang_total_ref", ang_total_ref, M_PI / 6.0); - sin_total_ref_ = sinf(ang_total_ref); - } - void setGlobalLocalizationStatus( - const size_t num_particles, - const size_t current_num_particles) - { - if (current_num_particles <= num_particles) - { - num_points_ = num_points_default_; - return; - } - size_t num = num_points_default_ * num_particles / current_num_particles; - if (num < num_points_global_) - num = num_points_global_; + LidarMeasurementModelBeam(const float x, const float y, const float z); - num_points_ = num; - } - float getMaxSearchRange() const + inline float getMaxSearchRange() const { return map_grid_max_ * 4; } - float getSinTotalRef() const + inline float getSinTotalRef() const { return sin_total_ref_; } - typename pcl::PointCloud::Ptr filter( - const typename pcl::PointCloud::ConstPtr &pc) const - { - const auto local_points_filter = [this](const PointType &p) - { - if (p.x * p.x + p.y * p.y > clip_far_sq_) - return true; - if (p.x * p.x + p.y * p.y < clip_near_sq_) - return true; - if (p.z < clip_z_min_ || clip_z_max_ < p.z) - return true; - if (p.intensity - roundf(p.intensity) > 0.01) - return true; - return false; - }; - pcl::PointCloud::Ptr pc_filtered(new pcl::PointCloud); - *pc_filtered = *pc; - pc_filtered->erase( - std::remove_if(pc_filtered->begin(), pc_filtered->end(), local_points_filter), pc_filtered->end()); - pc_filtered->width = 1; - pc_filtered->height = pc_filtered->points.size(); - - return sampler_.sample(pc_filtered, num_points_); - } - + void loadConfig( + const ros::NodeHandle &nh, + const std::string &name); + void setGlobalLocalizationStatus( + const size_t num_particles, + const size_t current_num_particles); + pcl::PointCloud::Ptr filter( + const pcl::PointCloud::ConstPtr &pc) const; std::pair measure( - typename ChunkedKdtree::Ptr &kdtree, - const typename pcl::PointCloud::ConstPtr &pc, + ChunkedKdtree::Ptr &kdtree, + const pcl::PointCloud::ConstPtr &pc, const std::vector &origins, - const State6DOF &s) const - { - if (!pc) - return std::pair(1, 0); - if (pc->size() == 0) - return std::pair(1, 0); - pcl::PointCloud::Ptr pc_particle(new pcl::PointCloud); - std::vector id(1); - std::vector sqdist(1); - - float score_beam = 1.0; - *pc_particle = *pc; - s.transform(*pc_particle); - for (auto &p : pc_particle->points) - { - const int beam_header_id = lroundf(p.intensity); - Raycast ray( - kdtree, - s.pos + s.rot * origins[beam_header_id], - Vec3(p.x, p.y, p.z), - map_grid_min_, map_grid_max_); - for (auto point : ray) - { - if (point.collision_) - { - // reject total reflection - if (point.sin_angle_ > sin_total_ref_) - { - score_beam *= beam_likelihood_; - } - break; - } - } - } - if (score_beam < beam_likelihood_min_) - score_beam = beam_likelihood_min_; - - return std::pair(score_beam, 1.0); - } + const State6DOF &s) const; }; } // namespace mcl_3dl diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h index 00850a3e..c2f2e094 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h @@ -30,9 +30,7 @@ #ifndef MCL_3DL_LIDAR_MEASUREMENT_MODELS_LIDAR_MEASUREMENT_MODEL_LIKELIHOOD_H #define MCL_3DL_LIDAR_MEASUREMENT_MODELS_LIDAR_MEASUREMENT_MODEL_LIKELIHOOD_H -#include #include -#include #include #include @@ -40,6 +38,8 @@ #include #include +#include +#include #include #include #include @@ -63,117 +63,24 @@ class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase PointCloudRandomSampler sampler_; public: - void loadConfig( - const ros::NodeHandle &nh, - const std::string &name) - { - ros::NodeHandle pnh(nh, name); - - int num_points, num_points_global; - pnh.param("num_points", num_points, 96); - pnh.param("num_points_global", num_points_global, 8); - num_points_default_ = num_points_ = num_points; - num_points_global_ = num_points_global; - - double clip_near, clip_far; - pnh.param("clip_near", clip_near, 0.5); - pnh.param("clip_far", clip_far, 10.0); - clip_near_sq_ = clip_near * clip_near; - clip_far_sq_ = clip_far * clip_far; - - double clip_z_min, clip_z_max; - pnh.param("clip_z_min", clip_z_min, -2.0); - pnh.param("clip_z_max", clip_z_max, 2.0); - clip_z_min_ = clip_z_min; - clip_z_max_ = clip_z_max; - - double match_weight; - pnh.param("match_weight", match_weight, 5.0); - match_weight_ = match_weight; - - double match_dist_min, match_dist_flat; - pnh.param("match_dist_min", match_dist_min, 0.2); - pnh.param("match_dist_flat", match_dist_flat, 0.05); - match_dist_min_ = match_dist_min; - match_dist_flat_ = match_dist_flat; - } - void setGlobalLocalizationStatus( - const size_t num_particles, - const size_t current_num_particles) - { - if (current_num_particles <= num_particles) - { - num_points_ = num_points_default_; - return; - } - size_t num = num_points_default_ * num_particles / current_num_particles; - if (num < num_points_global_) - num = num_points_global_; - - num_points_ = num; - } - float getMaxSearchRange() const + inline float getMaxSearchRange() const { return match_dist_min_; } - typename pcl::PointCloud::Ptr filter( - const typename pcl::PointCloud::ConstPtr &pc) const - { - const auto local_points_filter = [this](const PointType &p) - { - if (p.x * p.x + p.y * p.y > clip_far_sq_) - return true; - if (p.x * p.x + p.y * p.y < clip_near_sq_) - return true; - if (p.z < clip_z_min_ || clip_z_max_ < p.z) - return true; - return false; - }; - pcl::PointCloud::Ptr pc_filtered(new pcl::PointCloud); - *pc_filtered = *pc; - pc_filtered->erase( - std::remove_if(pc_filtered->begin(), pc_filtered->end(), local_points_filter), pc_filtered->end()); - pc_filtered->width = 1; - pc_filtered->height = pc_filtered->points.size(); - - return sampler_.sample(pc_filtered, num_points_); - } - + void loadConfig( + const ros::NodeHandle &nh, + const std::string &name); + void setGlobalLocalizationStatus( + const size_t num_particles, + const size_t current_num_particles); + pcl::PointCloud::Ptr filter( + const pcl::PointCloud::ConstPtr &pc) const; std::pair measure( - typename ChunkedKdtree::Ptr &kdtree, - const typename pcl::PointCloud::ConstPtr &pc, + ChunkedKdtree::Ptr &kdtree, + const pcl::PointCloud::ConstPtr &pc, const std::vector &origins, - const State6DOF &s) const - { - if (!pc) - return std::pair(1, 0); - if (pc->size() == 0) - return std::pair(1, 0); - pcl::PointCloud::Ptr pc_particle(new pcl::PointCloud); - std::vector id(1); - std::vector sqdist(1); - - float score_like = 0; - *pc_particle = *pc; - s.transform(*pc_particle); - size_t num = 0; - for (auto &p : pc_particle->points) - { - if (kdtree->radiusSearch(p, match_dist_min_, id, sqdist, 1)) - { - const float dist = match_dist_min_ - std::max(sqrtf(sqdist[0]), match_dist_flat_); - if (dist < 0.0) - continue; - - score_like += dist * match_weight_; - num++; - } - } - const float match_ratio = static_cast(num) / pc_particle->points.size(); - - return std::pair(score_like, match_ratio); - } + const State6DOF &s) const; }; } // namespace mcl_3dl diff --git a/src/lidar_measurement_model_beam.cpp b/src/lidar_measurement_model_beam.cpp new file mode 100644 index 00000000..02f234e6 --- /dev/null +++ b/src/lidar_measurement_model_beam.cpp @@ -0,0 +1,176 @@ +/* + * Copyright (c) 2018, the mcl_3dl authors + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDEDNode BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#include + +namespace mcl_3dl +{ +LidarMeasurementModelBeam::LidarMeasurementModelBeam( + const float x, const float y, const float z) +{ + // FIXME(at-wat): remove NOLINT after clang-format or roslint supports it + map_grid_min_ = std::min({ x, y, z }); // NOLINT(whitespace/braces) + map_grid_max_ = std::max({ x, y, z }); // NOLINT(whitespace/braces) +} + +void LidarMeasurementModelBeam::loadConfig( + const ros::NodeHandle &nh, + const std::string &name) +{ + ros::NodeHandle pnh(nh, name); + + int num_points, num_points_global; + pnh.param("num_points", num_points, 3); + pnh.param("num_points_global", num_points_global, 0); + num_points_default_ = num_points_ = num_points; + num_points_global_ = num_points_global; + + double clip_near, clip_far; + pnh.param("clip_near", clip_near, 0.5); + pnh.param("clip_far", clip_far, 4.0); + clip_near_sq_ = clip_near * clip_near; + clip_far_sq_ = clip_far * clip_far; + + double clip_z_min, clip_z_max; + pnh.param("clip_z_min", clip_z_min, -2.0); + pnh.param("clip_z_max", clip_z_max, 2.0); + clip_z_min_ = clip_z_min; + clip_z_max_ = clip_z_max; + + double beam_likelihood_min; + pnh.param("beam_likelihood", beam_likelihood_min, 0.2); + beam_likelihood_min_ = beam_likelihood_min; + beam_likelihood_ = powf(beam_likelihood_min, 1.0 / static_cast(num_points)); + + double ang_total_ref; + pnh.param("ang_total_ref", ang_total_ref, M_PI / 6.0); + sin_total_ref_ = sinf(ang_total_ref); +} + +void LidarMeasurementModelBeam::setGlobalLocalizationStatus( + const size_t num_particles, + const size_t current_num_particles) +{ + if (current_num_particles <= num_particles) + { + num_points_ = num_points_default_; + return; + } + size_t num = num_points_default_ * num_particles / current_num_particles; + if (num < num_points_global_) + num = num_points_global_; + + num_points_ = num; +} + +typename pcl::PointCloud::Ptr +LidarMeasurementModelBeam::filter( + const typename pcl::PointCloud::ConstPtr &pc) const +{ + const auto local_points_filter = [this](const LidarMeasurementModelBase::PointType &p) + { + if (p.x * p.x + p.y * p.y > clip_far_sq_) + return true; + if (p.x * p.x + p.y * p.y < clip_near_sq_) + return true; + if (p.z < clip_z_min_ || clip_z_max_ < p.z) + return true; + if (p.intensity - roundf(p.intensity) > 0.01) + return true; + return false; + }; + pcl::PointCloud::Ptr pc_filtered(new pcl::PointCloud); + *pc_filtered = *pc; + pc_filtered->erase( + std::remove_if(pc_filtered->begin(), pc_filtered->end(), local_points_filter), pc_filtered->end()); + pc_filtered->width = 1; + pc_filtered->height = pc_filtered->points.size(); + + return sampler_.sample(pc_filtered, num_points_); +} + +std::pair LidarMeasurementModelBeam::measure( + typename ChunkedKdtree::Ptr &kdtree, + const typename pcl::PointCloud::ConstPtr &pc, + const std::vector &origins, + const State6DOF &s) const +{ + if (!pc) + return std::pair(1, 0); + if (pc->size() == 0) + return std::pair(1, 0); + pcl::PointCloud::Ptr pc_particle(new pcl::PointCloud); + std::vector id(1); + std::vector sqdist(1); + + float score_beam = 1.0; + *pc_particle = *pc; + s.transform(*pc_particle); + for (auto &p : pc_particle->points) + { + const int beam_header_id = lroundf(p.intensity); + Raycast ray( + kdtree, + s.pos + s.rot * origins[beam_header_id], + Vec3(p.x, p.y, p.z), + map_grid_min_, map_grid_max_); + for (auto point : ray) + { + if (point.collision_) + { + // reject total reflection + if (point.sin_angle_ > sin_total_ref_) + { + score_beam *= beam_likelihood_; + } + break; + } + } + } + if (score_beam < beam_likelihood_min_) + score_beam = beam_likelihood_min_; + + return std::pair(score_beam, 1.0); +} +} // namespace mcl_3dl diff --git a/src/lidar_measurement_model_likelihood.cpp b/src/lidar_measurement_model_likelihood.cpp new file mode 100644 index 00000000..69ab4905 --- /dev/null +++ b/src/lidar_measurement_model_likelihood.cpp @@ -0,0 +1,156 @@ +/* + * Copyright (c) 2018, the mcl_3dl authors + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDEDNode BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include + +#include + +namespace mcl_3dl +{ +void LidarMeasurementModelLikelihood::loadConfig( + const ros::NodeHandle &nh, + const std::string &name) +{ + ros::NodeHandle pnh(nh, name); + + int num_points, num_points_global; + pnh.param("num_points", num_points, 96); + pnh.param("num_points_global", num_points_global, 8); + num_points_default_ = num_points_ = num_points; + num_points_global_ = num_points_global; + + double clip_near, clip_far; + pnh.param("clip_near", clip_near, 0.5); + pnh.param("clip_far", clip_far, 10.0); + clip_near_sq_ = clip_near * clip_near; + clip_far_sq_ = clip_far * clip_far; + + double clip_z_min, clip_z_max; + pnh.param("clip_z_min", clip_z_min, -2.0); + pnh.param("clip_z_max", clip_z_max, 2.0); + clip_z_min_ = clip_z_min; + clip_z_max_ = clip_z_max; + + double match_weight; + pnh.param("match_weight", match_weight, 5.0); + match_weight_ = match_weight; + + double match_dist_min, match_dist_flat; + pnh.param("match_dist_min", match_dist_min, 0.2); + pnh.param("match_dist_flat", match_dist_flat, 0.05); + match_dist_min_ = match_dist_min; + match_dist_flat_ = match_dist_flat; +} +void LidarMeasurementModelLikelihood::setGlobalLocalizationStatus( + const size_t num_particles, + const size_t current_num_particles) +{ + if (current_num_particles <= num_particles) + { + num_points_ = num_points_default_; + return; + } + size_t num = num_points_default_ * num_particles / current_num_particles; + if (num < num_points_global_) + num = num_points_global_; + + num_points_ = num; +} + +typename pcl::PointCloud::Ptr +LidarMeasurementModelLikelihood::filter( + const typename pcl::PointCloud::ConstPtr &pc) const +{ + const auto local_points_filter = [this](const LidarMeasurementModelBase::PointType &p) + { + if (p.x * p.x + p.y * p.y > clip_far_sq_) + return true; + if (p.x * p.x + p.y * p.y < clip_near_sq_) + return true; + if (p.z < clip_z_min_ || clip_z_max_ < p.z) + return true; + return false; + }; + pcl::PointCloud::Ptr pc_filtered(new pcl::PointCloud); + *pc_filtered = *pc; + pc_filtered->erase( + std::remove_if(pc_filtered->begin(), pc_filtered->end(), local_points_filter), pc_filtered->end()); + pc_filtered->width = 1; + pc_filtered->height = pc_filtered->points.size(); + + return sampler_.sample(pc_filtered, num_points_); +} + +std::pair LidarMeasurementModelLikelihood::measure( + typename ChunkedKdtree::Ptr &kdtree, + const typename pcl::PointCloud::ConstPtr &pc, + const std::vector &origins, + const State6DOF &s) const +{ + if (!pc) + return std::pair(1, 0); + if (pc->size() == 0) + return std::pair(1, 0); + pcl::PointCloud::Ptr pc_particle(new pcl::PointCloud); + std::vector id(1); + std::vector sqdist(1); + + float score_like = 0; + *pc_particle = *pc; + s.transform(*pc_particle); + size_t num = 0; + for (auto &p : pc_particle->points) + { + if (kdtree->radiusSearch(p, match_dist_min_, id, sqdist, 1)) + { + const float dist = match_dist_min_ - std::max(sqrtf(sqdist[0]), match_dist_flat_); + if (dist < 0.0) + continue; + + score_like += dist * match_weight_; + num++; + } + } + const float match_ratio = static_cast(num) / pc_particle->points.size(); + + return std::pair(score_like, match_ratio); +} +} // namespace mcl_3dl diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp index a8b61b72..341e0f39 100644 --- a/src/mcl_3dl.cpp +++ b/src/mcl_3dl.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include From a038fadea6d8377d7dab2a90a02ced495300c8c3 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Fri, 21 Sep 2018 13:50:24 +0900 Subject: [PATCH 24/29] Add LidarMeasurementResult struct to return measurement result --- include/mcl_3dl/lidar_measurement_model_base.h | 14 +++++++++++++- .../lidar_measurement_model_beam.h | 2 +- .../lidar_measurement_model_likelihood.h | 2 +- src/lidar_measurement_model_beam.cpp | 9 ++++----- src/lidar_measurement_model_likelihood.cpp | 9 ++++----- src/mcl_3dl.cpp | 6 +++--- 6 files changed, 26 insertions(+), 16 deletions(-) diff --git a/include/mcl_3dl/lidar_measurement_model_base.h b/include/mcl_3dl/lidar_measurement_model_base.h index 9575c4d2..65d35f5f 100644 --- a/include/mcl_3dl/lidar_measurement_model_base.h +++ b/include/mcl_3dl/lidar_measurement_model_base.h @@ -45,6 +45,18 @@ namespace mcl_3dl { +struct LidarMeasurementResult +{ + float likelihood; + float quality; + + LidarMeasurementResult(const float likelihood_value, const float quality_value) + : likelihood(likelihood_value) + , quality(quality_value) + { + } +}; + class LidarMeasurementModelBase { public: @@ -61,7 +73,7 @@ class LidarMeasurementModelBase virtual pcl::PointCloud::Ptr filter( const pcl::PointCloud::ConstPtr &) const = 0; - virtual std::pair measure( + virtual LidarMeasurementResult measure( ChunkedKdtree::Ptr &, const pcl::PointCloud::ConstPtr &, const std::vector &, diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h index ce26f3f9..614a9dcf 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h @@ -84,7 +84,7 @@ class LidarMeasurementModelBeam : public LidarMeasurementModelBase const size_t current_num_particles); pcl::PointCloud::Ptr filter( const pcl::PointCloud::ConstPtr &pc) const; - std::pair measure( + LidarMeasurementResult measure( ChunkedKdtree::Ptr &kdtree, const pcl::PointCloud::ConstPtr &pc, const std::vector &origins, diff --git a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h index c2f2e094..f7cb9d0f 100644 --- a/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h +++ b/include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_likelihood.h @@ -76,7 +76,7 @@ class LidarMeasurementModelLikelihood : public LidarMeasurementModelBase const size_t current_num_particles); pcl::PointCloud::Ptr filter( const pcl::PointCloud::ConstPtr &pc) const; - std::pair measure( + LidarMeasurementResult measure( ChunkedKdtree::Ptr &kdtree, const pcl::PointCloud::ConstPtr &pc, const std::vector &origins, diff --git a/src/lidar_measurement_model_beam.cpp b/src/lidar_measurement_model_beam.cpp index 02f234e6..0a53fdac 100644 --- a/src/lidar_measurement_model_beam.cpp +++ b/src/lidar_measurement_model_beam.cpp @@ -29,7 +29,6 @@ #include #include -#include #include #include @@ -130,16 +129,16 @@ LidarMeasurementModelBeam::filter( return sampler_.sample(pc_filtered, num_points_); } -std::pair LidarMeasurementModelBeam::measure( +LidarMeasurementResult LidarMeasurementModelBeam::measure( typename ChunkedKdtree::Ptr &kdtree, const typename pcl::PointCloud::ConstPtr &pc, const std::vector &origins, const State6DOF &s) const { if (!pc) - return std::pair(1, 0); + return LidarMeasurementResult(1, 0); if (pc->size() == 0) - return std::pair(1, 0); + return LidarMeasurementResult(1, 0); pcl::PointCloud::Ptr pc_particle(new pcl::PointCloud); std::vector id(1); std::vector sqdist(1); @@ -171,6 +170,6 @@ std::pair LidarMeasurementModelBeam::measure( if (score_beam < beam_likelihood_min_) score_beam = beam_likelihood_min_; - return std::pair(score_beam, 1.0); + return LidarMeasurementResult(score_beam, 1.0); } } // namespace mcl_3dl diff --git a/src/lidar_measurement_model_likelihood.cpp b/src/lidar_measurement_model_likelihood.cpp index 69ab4905..4c6f056b 100644 --- a/src/lidar_measurement_model_likelihood.cpp +++ b/src/lidar_measurement_model_likelihood.cpp @@ -29,7 +29,6 @@ #include #include -#include #include #include @@ -119,16 +118,16 @@ LidarMeasurementModelLikelihood::filter( return sampler_.sample(pc_filtered, num_points_); } -std::pair LidarMeasurementModelLikelihood::measure( +LidarMeasurementResult LidarMeasurementModelLikelihood::measure( typename ChunkedKdtree::Ptr &kdtree, const typename pcl::PointCloud::ConstPtr &pc, const std::vector &origins, const State6DOF &s) const { if (!pc) - return std::pair(1, 0); + return LidarMeasurementResult(1, 0); if (pc->size() == 0) - return std::pair(1, 0); + return LidarMeasurementResult(1, 0); pcl::PointCloud::Ptr pc_particle(new pcl::PointCloud); std::vector id(1); std::vector sqdist(1); @@ -151,6 +150,6 @@ std::pair LidarMeasurementModelLikelihood::measure( } const float match_ratio = static_cast(num) / pc_particle->points.size(); - return std::pair(score_like, match_ratio); + return LidarMeasurementResult(score_like, match_ratio); } } // namespace mcl_3dl diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp index 341e0f39..af22cb74 100644 --- a/src/mcl_3dl.cpp +++ b/src/mcl_3dl.cpp @@ -430,10 +430,10 @@ class MCL3dlNode std::map qualities; for (auto &lm : lidar_measurements_) { - const std::pair result = lm.second->measure( + const LidarMeasurementResult result = lm.second->measure( kdtree_, pc_locals[lm.first], origins, s); - likelihood *= result.first; - qualities[lm.first] = result.second; + likelihood *= result.likelihood; + qualities[lm.first] = result.quality; } if (match_ratio_min > qualities["likelihood"]) match_ratio_min = qualities["likelihood"]; From 257a144661d8c263c49c9c2a6714398480562090 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 25 Sep 2018 13:30:15 +0900 Subject: [PATCH 25/29] Fix constness --- include/mcl_3dl/state_6dof.h | 6 +++--- include/mcl_3dl/vec3.h | 18 ++++++++++++++++++ 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/include/mcl_3dl/state_6dof.h b/include/mcl_3dl/state_6dof.h index 8586a3fc..425aefde 100644 --- a/include/mcl_3dl/state_6dof.h +++ b/include/mcl_3dl/state_6dof.h @@ -102,7 +102,7 @@ class State6DOF : public mcl_3dl::pf::ParticleBase } return pos.x; } - const float &operator[](const size_t i) const + float operator[](const size_t i) const { switch (i) { @@ -168,7 +168,7 @@ class State6DOF : public mcl_3dl::pf::ParticleBase odom_err_integ_ang = mcl_3dl::Vec3(0.0, 0.0, 0.0); diff = true; } - bool isDiff() + bool isDiff() const { return diff; } @@ -185,7 +185,7 @@ class State6DOF : public mcl_3dl::pf::ParticleBase } static State6DOF generateNoise( std::default_random_engine &engine_, - State6DOF mean, State6DOF sigma) + const State6DOF &mean, const State6DOF &sigma) { State6DOF noise; if (mean.isDiff() || !sigma.isDiff()) diff --git a/include/mcl_3dl/vec3.h b/include/mcl_3dl/vec3.h index a8281a0f..7a824c15 100644 --- a/include/mcl_3dl/vec3.h +++ b/include/mcl_3dl/vec3.h @@ -66,6 +66,24 @@ class Vec3 } return x; } + float operator[](const size_t i) const + { + switch (i) + { + case 0: + return x; + break; + case 1: + return y; + break; + case 2: + return z; + break; + default: + break; + } + return x; + } bool operator==(const Vec3 &q) const { return x == q.x && y == q.y && z == q.z; From 49572be0358ca790060c6a7123f8832fed56fdbd Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 25 Sep 2018 13:30:36 +0900 Subject: [PATCH 26/29] Fix State6Dof's size value --- include/mcl_3dl/state_6dof.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/mcl_3dl/state_6dof.h b/include/mcl_3dl/state_6dof.h index 425aefde..10c0f113 100644 --- a/include/mcl_3dl/state_6dof.h +++ b/include/mcl_3dl/state_6dof.h @@ -137,7 +137,7 @@ class State6DOF : public mcl_3dl::pf::ParticleBase } size_t size() const override { - return 10; + return 13; } void normalize() override { From 239b38767758dab8df2adc8d33cddd266997abd4 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 25 Sep 2018 13:39:40 +0900 Subject: [PATCH 27/29] Add range assert to the State6Dof::[] operator --- include/mcl_3dl/state_6dof.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/mcl_3dl/state_6dof.h b/include/mcl_3dl/state_6dof.h index 10c0f113..da598fab 100644 --- a/include/mcl_3dl/state_6dof.h +++ b/include/mcl_3dl/state_6dof.h @@ -71,6 +71,7 @@ class State6DOF : public mcl_3dl::pf::ParticleBase RPYVec rpy; float &operator[](const size_t i)override { + assert(i < 13); switch (i) { case 0: @@ -104,6 +105,7 @@ class State6DOF : public mcl_3dl::pf::ParticleBase } float operator[](const size_t i) const { + assert(i < 13); switch (i) { case 0: From 2efa4a74f245596ba906ed3d286a155f959dc4e7 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 25 Sep 2018 14:49:36 +0900 Subject: [PATCH 28/29] Address review comments --- include/mcl_3dl/state_6dof.h | 8 ++++---- test/src/test_point_cloud_random_sampler.cpp | 3 +++ 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/include/mcl_3dl/state_6dof.h b/include/mcl_3dl/state_6dof.h index da598fab..d77c42da 100644 --- a/include/mcl_3dl/state_6dof.h +++ b/include/mcl_3dl/state_6dof.h @@ -71,7 +71,6 @@ class State6DOF : public mcl_3dl::pf::ParticleBase RPYVec rpy; float &operator[](const size_t i)override { - assert(i < 13); switch (i) { case 0: @@ -100,12 +99,12 @@ class State6DOF : public mcl_3dl::pf::ParticleBase return odom_err_integ_ang.y; case 12: return odom_err_integ_ang.z; + default: + assert(false); } - return pos.x; } float operator[](const size_t i) const { - assert(i < 13); switch (i) { case 0: @@ -134,8 +133,9 @@ class State6DOF : public mcl_3dl::pf::ParticleBase return odom_err_integ_ang.y; case 12: return odom_err_integ_ang.z; + default: + assert(false); } - return pos.x; } size_t size() const override { diff --git a/test/src/test_point_cloud_random_sampler.cpp b/test/src/test_point_cloud_random_sampler.cpp index aa3f4351..25dfa432 100644 --- a/test/src/test_point_cloud_random_sampler.cpp +++ b/test/src/test_point_cloud_random_sampler.cpp @@ -72,7 +72,10 @@ TEST(PointCloudRandomSampler, Sampling) for (const auto &p_ref : points_ref) { if (p_ref[0] == p.x && p_ref[1] == p.y && p_ref[2] == p.z) + { found = true; + break; + } } ASSERT_TRUE(found) << "A sampled point is not in the original points array"; } From cca96e35f86908522cb894132caed7c4b6806817 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 25 Sep 2018 14:59:20 +0900 Subject: [PATCH 29/29] Add dummy return to State6Dof::operator[] for out-of-range index --- include/mcl_3dl/state_6dof.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/mcl_3dl/state_6dof.h b/include/mcl_3dl/state_6dof.h index d77c42da..a282fb30 100644 --- a/include/mcl_3dl/state_6dof.h +++ b/include/mcl_3dl/state_6dof.h @@ -102,6 +102,7 @@ class State6DOF : public mcl_3dl::pf::ParticleBase default: assert(false); } + return pos.x; } float operator[](const size_t i) const { @@ -136,6 +137,7 @@ class State6DOF : public mcl_3dl::pf::ParticleBase default: assert(false); } + return 0; } size_t size() const override {