Proposal for Simplifying Pose Initialization in Autoware

[xmfcx]

Proposal for Simplifying Pose Initialization in Autoware

The current pose initialization architecture in Autoware is complex and contains redundant parameters. Specifically, the use of an explicit method parameter in the InitializeLocalization.srv service leads to unnecessary complexity.

This proposal aims to simplify the initialization process by inferring the initialization method from the covariance values of the provided pose, thereby eliminating the need for the method parameter.

Related PRs and Issues:
These PRs/Issues were made before this proposal, we'll close them. Let's discuss and come to a consensus first.

• fix(pose_initializer): add topic subscription to direct initialization autoware.universe#8941
• fix(pose_initializer): switch direct init with gnss cov check autoware.universe#8948
• Add Direct Initialization Option with GNSS pose autoware.universe#8940

Initialization service

We currently use tier4_localization_msgs/srv/InitializeLocalization.srv

# constants for specifying initialization method
uint8 AUTO = 0
uint8 DIRECT = 1

# input pose for initialization
geometry_msgs/PoseWithCovarianceStamped[<=1] pose_with_covariance

# Variable to set initialization method
# AUTO: The initial position is automatically estimated with localization algorithm.
#       The input pose will be used as an initial guess if provided.
# DIRECT: The initial position is set directly by the input pose without going through localization algorithm.
uint8 method
---
# ERROR CODES used in response status in case of failure
uint16 ERROR_UNSAFE = 1
uint16 ERROR_GNSS_SUPPORT = 2
uint16 ERROR_GNSS = 3
uint16 ERROR_ESTIMATION = 4

autoware_common_msgs/ResponseStatus status

There is no need for the method parameter. We can infer the method from the covariance values of the pose_with_covariance parameter.

This makes sense because the covariance values can define the search space for the pose. If it's very small, it means that the pose is very accurate and we can use it directly.

• Zero Covariance: Indicates high confidence in the pose. The system will use the pose directly for initialization.
• Non-Zero Covariance: Indicates uncertainty in the pose. The system will use localization algorithms (e.g., NDT alignment) to refine the pose.

We can use autoware_adapi_v1_msgs/localization/srv/InitializeLocalization.srv as it is.

Utilize the existing AD API service definition to maintain consistency and reduce redundancy.

Maybe we can just add 2 comment lines to make it clear that the covariance values are used to infer the initialization method.

geometry_msgs/PoseWithCovarianceStamped[<=1] pose

# If the covariance values are zero, the pose will be used directly for initialization.
# If the covariance values are non-zero, the GNSS covariance values will be used for initialization.
---
uint16 ERROR_UNSAFE = 1
uint16 ERROR_GNSS_SUPPORT = 2
uint16 ERROR_GNSS = 3
uint16 ERROR_ESTIMATION = 4
autoware_adapi_v1_msgs/ResponseStatus status

Pose initialization architecture changes

Existing architecture

flowchart TD
  subgraph pose_initializer["Pose Initializer"]
    init_method{{"service parameter:<br/>initialization method"}}
    Auto
    Direct
    trigger_ekf_and_ndt["trigger ekf and ndt"]
  end


  subgraph Auto["AUTO"]
    init_pose_is_filled{{"service call contains a pose"}}
    get_gnss_pose["get gnss pose"]
    use_service_pose2["use the service pose"]
    override_gnss_cov(["override gnss cov<br/>from config"])
    ndt_align_pose["ndt->align_pose()"]
  end
  subgraph Direct["DIRECT"]
    use_service_pose["use the service pose"]

  end

  gnss_poser -- /sensing/gnss/pose_with_covariance --> get_gnss_pose["get gnss pose"]

  pose_via_cli["Pose via CLI"] -- /localization/initialize--> init_method
  ad_api["AD API"] -- /localization/initialize --> init_method
  init_method -- AUTO --> Auto
  init_method -- DIRECT --> Direct
  init_pose_is_filled -- yes --> use_service_pose2
  init_pose_is_filled -- no --> get_gnss_pose
  get_gnss_pose --> override_gnss_cov
  override_gnss_cov --> ndt_align_pose
  use_service_pose2 --> ndt_align_pose
  ndt_align_pose --> trigger_ekf_and_ndt
  use_service_pose --> trigger_ekf_and_ndt
  ndt_align_pose <-- /localization/pose_estimator/ndt_align_srv --> ndt_scan_matcher["ndt_scan_matcher"]
  trigger_ekf_and_ndt -->|/localization/pose_estimator/trigger_node| ndt_scan_matcher
  trigger_ekf_and_ndt --> |/localization/pose_twist_fusion_filter/trigger_node| ekf_localizer["ekf_localizer"]
  pose_initializer -->|/initialpose3D| ekf_localizer
  
  init_method:::cl_conditional
  use_service_pose:::cl_purple
  Auto:::cl_output
  Direct:::cl_output
  trigger_ekf_and_ndt:::cl_purple
  init_pose_is_filled:::cl_conditional
  get_gnss_pose:::cl_purple
  use_service_pose2:::cl_purple
  override_gnss_cov:::cl_rose
  ndt_align_pose:::cl_purple
  pose_via_cli:::cl_node
  ad_api:::cl_node
  ndt_scan_matcher:::cl_node
  ekf_localizer:::cl_node
  pose_initializer:::cl_node
  gnss_poser:::cl_node
  
  classDef cl_node fill:#FFF2CC, stroke:#D6B656, stroke-width:3px
  classDef cl_conditional fill:#FFE6CC, stroke:#D79B00, stroke-width:3px
  classDef cl_output fill:#D5E8D4, stroke:#82B366, stroke-width:3px
  classDef cl_purple fill:#E1D5E7, stroke:#9673A6, stroke-width:3px
  classDef cl_rose fill:#FFDFE5, stroke:#FF5978, stroke-width:3px

Loading

Proposal

flowchart TD
  subgraph pose_initializer["Pose Initializer"]

  init_pose_is_filled{{"service call contains a pose"}}
  get_gnss_pose["get gnss pose"]
  load_cov_param{{"load covariance from param"}}
  use_gnss_cov(["use gnss cov"])
  use_service_pose2["use the service pose"]
  cov_zero{{"covariance is zero"}}
  override_gnss_cov(["override gnss cov<br/>from config"])
  ndt_align_pose["ndt->align_pose()"]

  trigger_ekf_and_ndt["trigger ekf and ndt"]

  initialize_directly["initialize directly"]
  end


  gnss_poser -- /sensing/gnss/pose_with_covariance --> get_gnss_pose["get gnss pose"]

  pose_via_cli["Pose via CLI"] -- /localization/initialize--> init_pose_is_filled
  ad_api["AD API"] -- /localization/initialize --> init_pose_is_filled
  init_pose_is_filled -- yes --> use_service_pose2
  init_pose_is_filled -- no --> get_gnss_pose
  get_gnss_pose --> load_cov_param
  load_cov_param -- yes --> override_gnss_cov
  load_cov_param -- no --> use_gnss_cov

  use_gnss_cov --> cov_zero
  override_gnss_cov --> cov_zero
  use_service_pose2 --> cov_zero

  cov_zero -- no --> ndt_align_pose
  cov_zero -- yes --> initialize_directly

  ndt_align_pose --> trigger_ekf_and_ndt
  initialize_directly --> trigger_ekf_and_ndt

  ndt_scan_matcher["ndt_scan_matcher"]

  ndt_align_pose <-- /localization/pose_estimator/ndt_align_srv --> ndt_scan_matcher
  trigger_ekf_and_ndt -->|/localization/pose_estimator/trigger_node| ndt_scan_matcher
  trigger_ekf_and_ndt --> |/localization/pose_twist_fusion_filter/trigger_node| ekf_localizer["ekf_localizer"]

  pose_initializer -->|/initialpose3D| ekf_localizer

  initialize_directly:::cl_purple
  trigger_ekf_and_ndt:::cl_purple
  init_pose_is_filled:::cl_conditional
  get_gnss_pose:::cl_purple
  use_service_pose2:::cl_purple
  override_gnss_cov:::cl_rose
  ndt_align_pose:::cl_purple
  pose_via_cli:::cl_node
  ad_api:::cl_node
  ndt_scan_matcher:::cl_node
  ekf_localizer:::cl_node
  pose_initializer:::cl_node
  gnss_poser:::cl_node
  load_cov_param:::cl_conditional
  cov_zero:::cl_conditional
  use_gnss_cov:::cl_rose
  
  classDef cl_node fill:#FFF2CC, stroke:#D6B656, stroke-width:3px
  classDef cl_conditional fill:#FFE6CC, stroke:#D79B00, stroke-width:3px
  classDef cl_output fill:#D5E8D4, stroke:#82B366, stroke-width:3px
  classDef cl_purple fill:#E1D5E7, stroke:#9673A6, stroke-width:3px
  classDef cl_rose fill:#FFDFE5, stroke:#FF5978, stroke-width:3px

Loading

Comparison of Existing vs. Proposed Architecture:

Existing Architecture

• Method-based decision-making: Explicitly checks the service method (AUTO or DIRECT).
• Branches based on method: Different paths for GNSS-based or direct initialization.
• Separate processing for pose and GNSS data: Complex conditionals based on method.

Proposed Architecture

• Covariance-based decision-making: Infers initialization method from pose covariance values.
• Simplified flow: Directly checks if covariance is zero to initialize directly or use NDT alignment.
• Unified processing: Fewer conditionals, streamlining initialization logic by focusing on covariance.

Benefits of the Proposed Changes

• Simplification: Removing the method parameter reduces redundancy and simplifies the service interface.
• Intuitive Initialization: Inferring the initialization method from covariance values aligns with standard practices in robotics.
• Consistency: Using the existing AD API service definition promotes consistency across the system.
• Maintainability: Simplified code paths make the system easier to maintain and extend.

Considerations

• Robustness: Ensure that the system handles cases where covariance values are near zero but not exactly zero due to floating-point inaccuracies.
• Documentation: Provide clear documentation and examples to help users understand how to set covariance values appropriately.
• Backward Compatibility: Communicate changes clearly to developers and users to facilitate a smooth transition.

We've prepared this with @meliketanrikulu and I'm happy to hear your thoughts.

cc. @YamatoAndo @isamu-takagi @yukkysaito @mitsudome-r @SakodaShintaro

[yukkysaito]

Thank you for the proposal.
From looking at the flowchart, I found the existing architecture easier to understand compared to the proposal. One reason is that I’m not entirely convinced by the logic where the decision to run ndt->align_pose is based on whether the covariance is zero or not. Is my understanding correct that NDT-based pose correction is applied when the covariance is non-zero?

[xmfcx]

Is my understanding correct that NDT-based pose correction is applied when the covariance is non-zero?

Yes. Because if covariance is zero, it means you trust your pose fully. And will perform direct initialization.

That's why we wanted to remove the direct/auto options completely.

[YamatoAndo]

Hi. Thank you for the proposal.
It's still in the conceptual stage, but since this is a good opportunity, I'd like to share my thoughts.

I am considering dividing the role of initial position estimation into global position estimation and local position estimation, allowing the user to choose which method to use or whether to use none at all for each.

I am envisioning the structure of the launch parameters as follows:

user_defined_initial_pose:
  enable: false
   global_estimator_method: 0  # NO_ESTIMATION: 0, AUTO: 1, GNSS: 2, ...
   local_estimator_method: 0  # NO_ESTIMATION: 0, AUTO: 1, NDT: 2, ...
  pose: [0.0, 0.0, ...]  # In the case of `global_estimator_method: 0`, the local estimator will be executed using this Pose.

global_estimator:
  gnss:
    enable: ture
    covariance:
      override: true
      covariance: [1.0, 0.0, ...]
  place_recognition:       
    enable: false
    covariance:
      override: true
      covariance: [1.0, 0.0, ...]
  ...
local_estimator:
  ndt:
    enable: true
  yabloc:
    enable: true

node_trigger:
  ekf:
    enable: true
  ndt:
    enable: true

validator:
  stop_check:
    enable: true
    check_duration: 3.0
  ...

※ In the case of AUTO, I envision that the most suitable method will be automatically selected based on the situation from among the methods where enable: true.

And the structure of the ADAPI service as follows:

# constants for specifying initialization method
# NO_ESTIMATION: The initial position is set directly by the input pose without going through localization algorithm.
# AUTO: The initial position is automatically estimated with localization algorithm.

## For Global 
uint8 GLOBAL_NO_ESTIMATION = 0
uint8 GLOBAL_AUTO = 1
uint8 GLOBAL_GNSS = 2
uint8 GLOBAL_PLACE_RECOGNITION = 3
uint8 GLOBAL_LANDMARK = 4

uint8 LOCAL_NO_ESTIMATION = 0
uint8 LOCAL_AUTO = 1
uint8 LOCAL_NDT = 2
uint8 LOCAL_YABLOC = 3

# input pose for local estimator if `global_estimator_method` == 0
geometry_msgs/PoseWithCovarianceStamped[<=1] pose_with_covariance

uint8 global_estimator_method

uint8 local_estimator_method

---
# ERROR CODES used in response status in case of failure
uint16 ERROR_UNSAFE = 1
uint16 ERROR_GNSS_SUPPORT = 2
uint16 ERROR_GNSS = 3
uint16 ERROR_ESTIMATION = 4

autoware_common_msgs/ResponseStatus status

I believe it would be simpler and clearer to explicitly let the user specify which estimation method they want to use, rather than relying on whether the covariance is zero or not.

Please let me know if you have any feedback. @xmfcx @yukkysaito @mitsudome-r

[xmfcx]

I understand, it's a good proposal when you have many methods.

I think in the existing structure, local estimation should select "no estimation" if global estimation result covariance is zero.

But in your case, when there are many methods to choose from, explicitly selecting X_NO_ESTIMATION makes sense.

[Jinshen-Yan]

I think it's a good idea to design a flexible and scalable architecture. The current NDT-based method for initial positioning is slowly . After designing a flexible architecture, it would be worth exploring other solution to improve the speed of initial positioning. It can also be configured for users to choose.

[VRichardJP]

I am not conviced the method is really a hurdle. If the field is not set, the default value is AUTO which should be a good default.

On the other hand, using the covariance can definitely lead to unexpected behavior. For example, imagine the user gets a PoseWithCovarianceStamped message from a GNSS sensor and copy it to the InitializeLocalization request. The method field would then change erratically between AUTO and DIRECT for no apparent reason.

[xmfcx]

We proposed to remove the AUTO or DIRECT options altogether.

If your covariance is zero, we assume you trust your pose fully. And will perform direct initialization.
This is what covariance is there for.

If not, we'll proceed with the remaining methods.

[VRichardJP]

But in practice, it is very common not to know the covariance and not to fill it. In such a case the covariance is filled with 0.

So it means for the user not to misuse the API, they would have to explicitly set the covariance to a big value, which is awkward since they don't know it.

By convention, one is supposed to set covariance[0] to a negative value to specify the covariance is not known, but I have yet to find a piece of code that actually does so or check this value.

[YamatoAndo]

If we can implement the functionality of directly using GNSS position for initial position estimation without making significant changes to the existing structure, I agree with @SakodaShintaro 's suggestion
autowarefoundation/autoware.universe#8940 (comment)

[xmfcx]

I wanted to make significant changes to the existing structure, that's why I've opened up this discussion.

I'm aware I could achieve this by adding a new parameter but that only would make the structure more complicated.
That's why I proposed this to simplify the architecture.

But I'm also ok with your proposal above.

[xmfcx]

If we have so many options, then it makes sense to have "no estimation" as an option as well.