[TASK] Line filtering library

[jorgenfj]

Description of task

It is time to implement a proper library for filtering of lines. Filtering of detected lines is essential for robust tracking and estimation.
Lines are parametrized by rho and theta.

1. rho is the shortest(perpendicular) distance from origo to the lines.
2. theta is the corresponding angle between the perpendicular line and origin.

This year we plan to use line filtering for both the docking and pipeline task. Both filters should ideally have their state in the world(odom) frame, and the measurement model should map the state into the image pixel space.

Docking

• Lines detected on the 2d sonar image scans.
• use SonarInfo to map detections from pixel space into the sonar frame.
• Retrieve current pose needed for the measurement model using the tf2 library.

Pipeline

• Lines detected on the 2d image of the downwards facing camera.
• Use CameraInfo to map detections from pixel space into the camera frame.
• Retrieve current pose needed for the measurement model using the tf2 library.
• Retrieve current pose needed for the measurement model using the tf2 library.

Suggested Workflow

• Look at the previous implementation of line filtering that just filtered the midpoint of the lines, and kept the line parametrization as additional non-filtering data on the track struct.
• Look at how the unfinished pose filtering library is set up for inspiration.
• Research how to use the current sensor model setup in vortex_filtering to solve this task. Note, the RangeBearingSensor defined assumes target state is in cartesian coordinates.

Specifications

• Assume constant camera matrix, i.e. no dynamic camera calibration.
• Initially just use the plain transform from tf2 for the pose. The covariance from the pose produced by the ESKF state estimation can be neglected for now.

Contacts

@jorgenfj
@fnilsen1 for SonarInfo message definition
@gardeg for how this library should be integrated into the pipeline following code

Code Quality

• Every function in header files are documented (inputs/returns/exceptions)
• The project has automated tests that cover MOST of the functions and branches in functions (pytest/gtest)
• The code is documented on the wiki (provide link)

[jorgenfj]

I believe an IPDA filter is the correct choice here for track management. Currently IPDA using gating based on configured min and max euclidean distance. This will most likely cause problems here since rho and theta are of different magnitudes. I will look into refactoring and maybe removed the min max gating logic from IPDA.