Move cell covariance inversion from the run-time static map to the map publisher
Description
With !122 (merged) merged, the static map will do the cell covariance inverse computation (and cache it) in run-time (more like whenever a map is received). However in reality this could be delegated to the map publisher(hence to the DynamicNDTMap).
Overlooking the fact that only the inverse of the covariance is needed by ndt (and never the actual covariance) results in a possible computational overhead in the run time of the ndt localizer. Ideally only the inverse covariance should be received by the static ndt map along with the position.
Benefits of this refactor:
- Inversion can happen on the non-RT process (map publisher).
- Cells with un-usable covariance can be removed from the start -> less data to transport.
While we are at it, covariance validation logic should also be added. See #140 (comment 258631064) for a description.
Current behavior
-
NDTMapPublisherNode
reads raw point cloud and pushes these points into theDynamicNDTMap
. -
DynamicNDTMap
feeds raw points intoDynamicNDTVoxel
s which compute the covariance on the run. - When all points are added to
DynamicNDTMap
,NDTMapPublisherNode
iterates over voxels and serializes the following information from each voxel into the point cloud:- Voxel grid cell ID
- Centroid: x, y, z
- covariance: cov_xx, cov_xy, cov_xz, cov_yy, cov_yz, cov_zz
-
NDTMapPublisherNode
publishes this point cloud map(pc2 format). -
RelativeLocalizerNode
receives this pc2 map, and forwards it to aStaticNDTMap
. -
StaticNDTMap
decodes the data mentioned in item 3., constructsStaticNDTVoxel
s from this data and adds them to its grid. During construction of aStaticNDTVoxel
, the covariance is also inverted and stored for future access.- This step occurs whenever a new map is received!
-
StaticNDTMap
is used by theP2DNDTLocalizer
in real time.P2DNDTObjective
of the localizer accesses the inverse covariance computed during the construction of theStaticNDTMap
.
Expected behavior
Following adjustments are made to the current behavior:
- Dynamic ndt map has the API to invert its current covariance matrix with appropriate validation steps.
- Published pc2 map has the following fields: icov_xx, icov_xy, icov_xz, icov_yy, icov_yz, icov_zz
- Covariance information is not present in the pc2 message.
- Static ndt voxel only stores the inverse covariance. (To satisfy the
CRTP
interface,covariance_()
method may return the inverse of the inverse covariance.)
Definition of Done
-
Covariance matrix is validated. -
Covariance inversion is executed in the dynamic voxel. -
Inverse of covariance is used in the ndt map pointcloud specs and the validation code/tests are updated. -
Covariance inversion is removed from the static voxel.
Edited by Yunus Emre Çalışkan