docs: update readme about particle filter (autowarefoundation#30)

* update mpf/README.md Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * update gnss_corrector/README.md Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * update camera_corrector/README.md Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> --------- Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>
KYabuuchi · May 31, 2023 · 4f4c2c2 · 4f4c2c2
1 parent 9c3757e
commit 4f4c2c2
Show file tree

Hide file tree

Showing 3 changed files with 79 additions and 59 deletions.
diff --git a/localization/yabloc/particle_filter/camera_particle_corrector/README.md b/localization/yabloc/particle_filter/camera_particle_corrector/README.md
@@ -2,26 +2,36 @@
 
 ## Purpose
 
-- Cameraを用いてパーティクルの重み付けを行うパッケージ。
+- A package that weight particles using Camera.
 
 ## Inputs / Outputs
 
 ### Input
 
-| Name         | Type                      | Description           |
-| ------------ | ------------------------- | --------------------- |
-| `/predicted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | predicted particles |
-| **TODO** |  |  |
+| Name                                                  | Type                                                   | Description                                                 |
+|-------------------------------------------------------|--------------------------------------------------------|-------------------------------------------------------------|
+| `/predicted_particles`                                | `modularized_particle_filter_msgs::msg::ParticleArray` | predicted particles                                         |
+| `/localization/map/ll2_road_marking`                  | `sensor_msgs::msg::PointCloud2`                        | road surface marking converted to line segments             |
+| `/localization/imgproc/projected_line_segments_cloud` | `sensor_msgs::msg::PointCloud2`                        | projected line segments                                     |
+| `/pose`                                               | `geometry_msgs::msg::PoseStamped`                      | reference to retrieve the area map around the self location |
+
 
 ### Output
 
-| Name              | Type                                 | Description                                              |
-| ----------------- | ------------------------------------ | -------------------------------------------------------- |
-| `/weighted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | 重み付けされたパーティクル |
-| **TODO** |  |  |
+| Name                  | Type                                                   | Description                              |
+|-----------------------|--------------------------------------------------------|------------------------------------------|
+| `/weighted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | weighted particles                       |
+| `/cost_map_image`     | `sensor_msgs::msg::Image`                              | cost map created from lanelet2           |
+| `/cost_map_range`     | `visualization_msgs::msg::MarkerArray`                 | cost map boundary                        |
+| `/match_image`        | `sensor_msgs::msg::Image`                              | projected line segments image            |
+| `/scored_cloud`       | `sensor_msgs::msg::PointCloud2`                        | weighted 3d line segments                |
+| `/scored_post_cloud`  | `sensor_msgs::msg::PointCloud2`                        | weighted 3d line segments which are iffy |
 
 ## Parameters
 
-| Name                     | Type    | Default | Description                                                                         |
-| ------------------------ | ------- | ------- | ----------------------------------------------------------------------------------- |
-| **TODO**  | |  |  |
+| Name              | Type  | Default | Description                                                                |
+|-------------------|-------|---------|----------------------------------------------------------------------------|
+| `max_range`       | float | 80      | width of heararchical cost map                                             |
+| `gamma`           | float | 40.0    | gamma value of the intensity gradient of the cost map                      |
+| `min_prob`        | float | 0.1     | minimum particle weight the corrector node gives                           |
+| `far_weight_gain` | float | 0.001   | `exp(-far_weight_gain_ * squared_distance_from_camera)` is reflected in the weight (If this is large, the nearby landmarks will be more important.)|
diff --git a/localization/yabloc/particle_filter/gnss_particle_corrector/README.md b/localization/yabloc/particle_filter/gnss_particle_corrector/README.md
@@ -2,23 +2,32 @@
 
 ## Purpose
 
-- GNSSを用いてパーティクルの重み付けを行うパッケージ。
-- `ublox_msgs::msg::NavPVT` と `geometry_msgs::msg::PoseWithCovarianceStamped` の2種類の入力に対応している。
+- Package for particle weighting using GNSS.
+- It supports two types of input: `ublox_msgs::msg::NavPVT` and `geometry_msgs::msg::PoseWithCovarianceStamped`.
 
 ## Inputs / Outputs
 
 ### Input
 
-| Name         | Type                      | Description           |
-| ------------ | ------------------------- | --------------------- |
-| `/predicted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | predicted particles |
-| `/input/navpvt` | `ublox_msgs::msg::NavPVT` | position measurement |
-| `/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | position measurement |
-| `/height` | `std_msgs::msg::Float32` | ground height (optional) |
+| Name                    | Type                                                   | Description                              |
+|-------------------------|--------------------------------------------------------|------------------------------------------|
+| `/predicted_particles`  | `modularized_particle_filter_msgs::msg::ParticleArray` | predicted particles                      |
+| `/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped`        | pose measurement for weighting           |
+| `/input/navpvt`         | `ublox_msgs::msg::NavPVT`                              | GNSS measurement for weighting           |
+| `/input/height`         | `std_msgs::msg::Float32`                               | ground height used for gnss_range_marker |
 
 ### Output
 
-| Name              | Type                                 | Description                                              |
-| ----------------- | ------------------------------------ | -------------------------------------------------------- |
-| `/weighted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | 重み付けされたパーティクル |
-| `/gnss/effect_marker` | `visualization_msgs::msg::MarkerArray` | GNSSによる重み付けの分布を同心円で可視化したもの |
+| Name                  | Type                                                   | Description                         |
+|-----------------------|--------------------------------------------------------|-------------------------------------|
+| `/weighted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | weighted particles                  |
+| `/gnss_range_marker`  | `visualization_msgs::msg::MarkerArray`                 | visualized GNSS weight distribution |
+
+
+
+### Parameters
+
+| Name                              | Type  | Default | Description                                                                                           |
+|-----------------------------------|-------|---------|-------------------------------------------------------------------------------------------------------|
+| `/ignore_less_than_float`         | bool  | true    | if this is true, only FIX or FLOAT is used for correction (No effect when using pose_with_covariance) |
+| `/mahalanobis_distance_threshold` | float | 20.f    | if the Mahalanobis distance to the GNSS for particle exceeds this, the correction skips.              |
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/README.md b/localization/yabloc/particle_filter/modularized_particle_filter/README.md
@@ -2,9 +2,9 @@
 
 ## Purpose
 
-- Particle Filterの機能を提供するパッケージ。
-- Correctorは抽象的なクラスのみを提供しており、それを継承して好みのCorrectorノードを実装することを想定している。
-- Predictorはそのまま使える。
+- This package provides the functionality of particle filter.
+- A corrector provides only an abstract class and is intended to be inherited to implement a corrector node of your choice.
+- A predictor can be used as is.
 
 ## How to test
 
@@ -17,49 +17,50 @@ colcon test-result --verbose --all
 
 ### Input
 
-| Name         | Type                      | Description           |
-| ------------ | ------------------------- | --------------------- |
-| `/pose` | `geometry_msgs::msg::Pose` | パーティクルの初期値位置指定用 |
-| `/initialpose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | パーティクルの初期値位置指定用 |
-| `/twist` | `geometry_msgs::msg::TwistStamped` | 予測更新の速度と角速度を受け取る |
-| `/weighted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | 重み付けされたパーティクル |
-| `/height` | `std_msgs::msg::Float32` | ground height (optional) パーティクルの高さを指定する |
+| Name                  | Type                                                   | Description                                               |
+|-----------------------|--------------------------------------------------------|-----------------------------------------------------------|
+| `/initialpose`        | `geometry_msgs::msg::PoseWithCovarianceStamped`        | to specity the initial position of particles              |
+| `/twist`              | `geometry_msgs::msg::TwistStamped`                     | linear velocity and angular velocity of prediction update |
+| `/twist_cov`          | `geometry_msgs::msg::TwistWithCovarianceStamped`       | linear velocity and angular velocity of prediction update |
+| `/weighted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | particles weighted  by corrector nodes                    |
+| `/height`             | `std_msgs::msg::Float32`                               | ground height                                             |
 
 ### Output
 
-| Name              | Type                                 | Description                                              |
-| ----------------- | ------------------------------------ | -------------------------------------------------------- |
-| `/predicted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | 予測更新されたパーティクル |
-| `/particle_pose` | `geometry_msgs::msg::PoseStamped` | パーティクルの重みつき平均 |
+| Name                          | Type                                                   | Description                           |
+|-------------------------------|--------------------------------------------------------|---------------------------------------|
+| `/predicted_particles`        | `modularized_particle_filter_msgs::msg::ParticleArray` | particles weighted by predictor nodes |
+| `/predicted_particles_marker` | `visualization_msgs::msg::MarkerArray`                 | markers for particle visualization    |
+| `/pose`                       | `geometry_msgs::msg::PoseStamped`                      | weighted mean of particles            |
+| `/pose_with_covariance`       | `geometry_msgs::msg::PoseWithCovarianceStamped`        | weighted mean of particles            |
 
 ### Parameters
 
-| Name                     | Type    | Default | Description                                                                         |
-| ------------------------ | ------- | ------- | ----------------------------------------------------------------------------------- |
-| `prediction_rate`  | double| 50 | 予測更新をする頻度単位はHz |
-| `visualize`  | bool | false   | パーティクルをvisualization_msgsでもpublishするか否か |
-| `num_of_particles`      | int | 500     | パーティクルの総数 |
-| `resampling_interval_seconds` | double  | 1.0     |  リサンプリングを実施する頻度 |
-| `static_linear_covariance` | double | 0.01| `/twist`の分散を上書きする値。`/twist`のもとの分散値を使うオプションは無い|
-| `static_angular_covariance`| double | 0.01| `/twist`の分散を上書きする値。`/twist`のもとの分散値を使うオプションは無い|
-| `use_dynamic_noise`| bool | false | 低速域において分散を小さくするかモードを使うか否か|
+| Name                          | Type   | Default | Description                                                                       |
+|-------------------------------|--------|---------|-----------------------------------------------------------------------------------|
+| `prediction_rate`             | double | 50      | frequency of forecast updates, in Hz                                              |
+| `visualize`                   | bool   | false   | whether particles are also published in visualization_msgs or not                 |
+| `num_of_particles`            | int    | 500     | the number of particles                                                           |
+| `resampling_interval_seconds` | double | 1.0     | the interval of particle resamping                                                |
+| `static_linear_covariance`    | double | 0.01    | to override the covariance of `/twist`. When using `/twist_cov`, it has no effect |
+| `static_angular_covariance`   | double | 0.01    | to override the covariance of `/twist`. When using `/twist_cov`, it has no effect |
 
 ## Corrector
 
-### Corrector Input
+### Input
 
-| Name         | Type                      | Description           |
-| ------------ | ------------------------- | --------------------- |
-| `/predicted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | 予測更新されたパーティクル |
+| Name                   | Type                                                   | Description                               |
+|------------------------|--------------------------------------------------------|-------------------------------------------|
+| `/predicted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | particles predicted by the predictor node |
 
-### Corrector Output
+### Output
 
-| Name              | Type                                 | Description                                              |
-| ----------------- | ------------------------------------ | -------------------------------------------------------- |
-| `/weighted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | 重み付けされたパーティクル |
+| Name                  | Type                                                   | Description                              |
+|-----------------------|--------------------------------------------------------|------------------------------------------|
+| `/weighted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | particles weighted by the corrector node |
 
-### Corrector Parameters
+### Parameters
 
-| Name                     | Type    | Default | Description                                                                         |
-| ------------------------ | ------- | ------- | ----------------------------------------------------------------------------------- |
-| `/visualize`  | bool | false   | パーティクルをvisualization_msgsでもpublishするか否か |
+| Name         | Type | Default | Description                                                       |
+|--------------|------|---------|-------------------------------------------------------------------|
+| `/visualize` | bool | false   | whether particles are also published in visualization_msgs or not |