Better support for relative pose measurements

[NikolausDemmel]

So at the moment relative pose measurements can only be incorporated as velocity measurements. This might work well when fusing one high-rate odometric sensor with one absolute pose sensor, but when fusing multiple odometries that are measured at different rates, and in particular if you have something like a key-frame odometry as input, you loose a lot of informaiton. On issue is that with a velocity measurement you propagate that through the general motion model in the filter. The lower frequency the measurements of your relative poses are, the more inaccurate this motion model likely is. [1] puts it the following way:

Once a displacement measurement is derived from an exteroceptive sensor, in most cases it must be combined with other position estimates derived from onboard proprioceptive sensors. An appealing solution to this problem is to convert the relative pose measurements to absolute position pseudo-measurements using the previous position estimates and treat them as such [13]. A different approach would be to process the displacement information as an (average) velocity measurement during the time interval between consecutive images [14]. Both these methodologies are approximate solutions founded on certain assumptions. The first approach is correct only when the orientation of the vehicle is precisely known. The second approach is valid only when the frequency of the displacement measurements is higher or at least equal to the frequency of any other velocity measurements available to the estimator. If both odometry and the relative pose measurements are available at the same rate, an exact solution would be to combine them as two independent measurements of the same displacement between two consecutive time steps. In most practical situations relative pose measurements are obtained at a significantly lower rate compared to odometric data. Application of this last methodology cannot provide state estimates between relative pose measurements unless two different estimators are involved; one for state estimation (absolute pose) and one for odometry integration.

The paper goes on to suggest a solution:

A more formal approach to this problem is to treat the relative displacement estimates as differences between the previous and current estimates of the position of the robot. In this paper we present a generalized framework, which we term stochastic cloning for processing such relative state (pose in this case) measurements within a Kalman filter (KF) estimator. To our knowledge, this paper repre- sents the first thorough investigation of how to fuse such relative displacement measurements with proprioceptive measurements in order to provide improved position estimates.

Have you ever considered incorporating something like this?

Support for frame-to-frame relative measurements would be a nice start (e.g. as an alternative for the differential option), but the extension to keyframe support would allow to deal with issues such as #260 properly.

[1] http://www-users.cs.umn.edu/~stergios/papers/sc-kf_icra02.pdf

[ayrton04]

Differential mode used to work by taking the delta between the current and previous measurement and adding it to the previous state to get a new measurement. For a number of reasons (excessive bookkeeping, uncertainty in how to compute covariance, etc.), I made differential mode convert the relative poses to velocities. I've discussed adding an "incremental" mode to the filter that replaces this functionality as you suggest, but I've not seen the paper you referenced.

In any case, I'm definitely in favor of adding this feature, but I just need to find the cycles to make it happen. Once I tackle some other pressing issues (the most notable being pluginlib-based filters), I can start to address some of these more complex improvements.

Thanks for taking the time to write this up!

[NikolausDemmel]

Thanks for your reply. It is great to see that this is of interest, and of course I understand if it is not on immediate ToDo list. I'll add some more remarks and references here of what my understanding is so far. Maybe someone else can even (start to) implement this (maybe myself, as time allows).

Differential mode used to work by taking the delta between the current and previous measurement and adding it to the previous state to get a new measurement.

That sounds very much like the "absolute position pseudo-measurements" in the citation above. This could still be an interesting third option. It is not clear to me how exactly this, and the stochastic cloning approach suggested in the paper relate.

excessive bookkeeping, uncertainty in how to compute covariance, etc.

Without thinking about it too much, it seems the bookkeeping is mostly keeping a copy of the state for each of the previous measurements of every sensor (with "incremental" mode). For covariances, my first gut-feeling would be to compose the pose-with-covariance from the previous state with the delta-pose-with-covariance to get a pose-with-covariance measurement of the current state ("combine" as implemented e.g. in MRPT, see http://wiki.ros.org/pose_cov_ops). Although I have no idea on the theoretical soundness of this approach. Update: it is actually mentioned in [3], that since with such an approach the measurement is corelated with the state, one needs to artificially increase the measurement covariance in order to avoid the filter becoming inconsistent.

I've discussed adding an "incremental" mode to the filter that replaces this functionality as you suggest, but I've not seen the paper you referenced.

So the stachastic cloning is different from what you had previously implemented in r_l in that it actually includes the previous pose in the filter state, such that a measurement that is a difference between two poses that are part of the state is possible. As said above, I don't know how exactly this compares to the "absolute position pseudo-measurements" in practice.

There are more recent works that use the stochastic cloning idea in localization filters. E.g. [2-4] are is followup work from the same lab as [1](although I have not read it all in detail yet). Another body of work that uses stochastic cloning for relative pose measurements is the loose visual-inertial filter developed at DLR [5,6]. In more recent work they even combine that with constantly re-referencing a purely relative filter to ensure that the state covariances don't grow without bounds [7].

I'm sure there is a lot more out there that uses similar ideas, which I am not yet aware of.

[2] http://www.ee.ucr.edu/~mourikis/papers/MourikisRoumeliotis_ICRA06.pdf [3] http://www.ee.ucr.edu/~mourikis/papers/MourikisRoumeliotisBurdick_TRO_07.pdf [4] http://www.ee.ucr.edu/~mourikis/papers/MourikisRoumeliotis-ICRA07.pdf [5] http://www6.in.tum.de/Main/Publications/Burschka2012a.pdf [6] http://elib.dlr.de/79891/1/IJRRPrePrint.pdf [7] http://www6.in.tum.de/Main/Publications/Burschka14b.pdf

[sarath-vanga]

Hi @ayrton04 is there any update on this feature request? Me too looking for something similar.

[ayrton04]

I've ceased any major feature development on this package, as we are moving over to fuse. That package, by the way, handles relative poses very nicely. It admittedly needs to be documented more (and bloomed).

In any case, I would be happy to look at PRs for this if anyone is so inclined, but I don't anticipate addressing this myself. Sorry about that!

[sarath-vanga]

Hi @ayrton04 fuse looks very interesting. Could you guide me to some ros examples on how it's used ?

[ayrton04]

Not at the moment, actually, no. We need to get a few example launch/config files in there, but it's been hard to find time for it. Hopefully we'll get to that soon!