Skip to content

Commit

Permalink
feat(ndt_scan_matcher): add particles param (#330)
Browse files Browse the repository at this point in the history
* feat(ndt_scan_matcher): add particles param

Signed-off-by: Yamato Ando <yamato.ando@gmail.com>

* fix data type

Signed-off-by: YamatoAndo <yamato.ando@gmail.com>

* ci(pre-commit): autofix

* fix data type

Signed-off-by: YamatoAndo <yamato.ando@gmail.com>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
YamatoAndo and pre-commit-ci[bot] authored Feb 4, 2022
1 parent f17693d commit 8f5bfc4
Show file tree
Hide file tree
Showing 6 changed files with 16 additions and 6 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,9 @@
# Threshold for deciding whether to trust the estimation result
converged_param_transform_probability: 3.0

# The number of particles to estimate initial pose
initial_estimate_particles_num: 100

# neighborhood search method in OMP
# 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1
omp_neighborhood_search_method: 0
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,9 @@
# Threshold for deciding whether to trust the estimation result
converged_param_transform_probability: 3.0

# The number of particles to estimate initial pose
initial_estimate_particles_num: 100

# neighborhood search method in OMP
# 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1
omp_neighborhood_search_method: 0
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -153,6 +153,7 @@ class NDTScanMatcher : public rclcpp::Node
std::string ndt_base_frame_;
std::string map_frame_;
double converged_param_transform_probability_;
int initial_estimate_particles_num_;
float inversion_vector_threshold_;
float oscillation_threshold_;
std::array<double, 36> output_pose_covariance_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -65,8 +65,7 @@ void popOldPose(
Eigen::Affine3d fromRosPoseToEigen(const geometry_msgs::msg::Pose & ros_pose);

std::vector<geometry_msgs::msg::Pose> createRandomPoseArray(
const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov,
const size_t particle_num);
const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, const int particle_num);

template <class T>
T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform)
Expand Down
7 changes: 6 additions & 1 deletion localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,7 @@ NDTScanMatcher::NDTScanMatcher()
ndt_base_frame_("ndt_base_link"),
map_frame_("map"),
converged_param_transform_probability_(4.5),
initial_estimate_particles_num_(100),
inversion_vector_threshold_(-0.9),
oscillation_threshold_(10)
{
Expand Down Expand Up @@ -165,6 +166,9 @@ NDTScanMatcher::NDTScanMatcher()
converged_param_transform_probability_ = this->declare_parameter(
"converged_param_transform_probability", converged_param_transform_probability_);

initial_estimate_particles_num_ =
this->declare_parameter("initial_estimate_particles_num", initial_estimate_particles_num_);

std::vector<double> output_pose_covariance =
this->declare_parameter<std::vector<double>>("output_pose_covariance");
for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) {
Expand Down Expand Up @@ -602,7 +606,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::alignUsingMonteCar
}

// generateParticle
const auto initial_poses = createRandomPoseArray(initial_pose_with_cov, 100);
const auto initial_poses =
createRandomPoseArray(initial_pose_with_cov, initial_estimate_particles_num_);

std::vector<Particle> particle_array;
auto output_cloud = std::make_shared<pcl::PointCloud<PointSource>>();
Expand Down
5 changes: 2 additions & 3 deletions localization/ndt_scan_matcher/src/util_func.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -213,8 +213,7 @@ Eigen::Affine3d fromRosPoseToEigen(const geometry_msgs::msg::Pose & ros_pose)
}

std::vector<geometry_msgs::msg::Pose> createRandomPoseArray(
const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov,
const size_t particle_num)
const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, const int particle_num)
{
std::default_random_engine engine(seed_gen());
const Eigen::Map<const RowMatrixXd> covariance =
Expand All @@ -230,7 +229,7 @@ std::vector<geometry_msgs::msg::Pose> createRandomPoseArray(
const auto base_rpy = getRPY(base_pose_with_cov);

std::vector<geometry_msgs::msg::Pose> poses;
for (size_t i = 0; i < particle_num; ++i) {
for (int i = 0; i < particle_num; ++i) {
geometry_msgs::msg::Vector3 xyz;
geometry_msgs::msg::Vector3 rpy;

Expand Down

0 comments on commit 8f5bfc4

Please sign in to comment.