The fuse stack provides a general architecture for performing sensor fusion live on a robot. Some possible applications include state estimation, localization, mapping, and calibration.
A refactor of processAbsolutePose3DWithCovariance in sensor_proc logic is needed.
In case of partial measurements, is probably better to implement something like AbsolutePose3DStampedEulerConstraint.
This should accept partial pose, partial covariance and measured variable indices.
Moreover, this should create a constraint for the partial measurement: the translational part of the cost function is trivial, while the orientation part should be the same as in absolute_orientation_3d_stamped_euler.
PRO:
1) no need to double convert from quaternion to rpy and back
2) allows for refactor of AbsolutePose3DStampedConstraint (and relative cost functions) with fixed size matrixes/vectors
CONS:
1) a conversion from quaternion to rpy will be performed also for measurements which have partial position but full orientation. On the other hand, to distinguish all the cases we would need a lot of combinations.
A refactor of
processAbsolutePose3DWithCovariancein sensor_proc logic is needed.In case of partial measurements, is probably better to implement something like
AbsolutePose3DStampedEulerConstraint. This should accept partial pose, partial covariance and measured variable indices. Moreover, this should create a constraint for the partial measurement: the translational part of the cost function is trivial, while the orientation part should be the same as inabsolute_orientation_3d_stamped_euler.PRO: 1) no need to double convert from quaternion to rpy and back 2) allows for refactor of
AbsolutePose3DStampedConstraint(and relative cost functions) with fixed size matrixes/vectorsCONS: 1) a conversion from quaternion to rpy will be performed also for measurements which have partial position but full orientation. On the other hand, to distinguish all the cases we would need a lot of combinations.