[Rock-dev] RigidBodyState - Uncertainty and Composition

[Jakob Schwendner]

On 10/23/2012 05:35 PM, Javier Hidalgo Carrió wrote:
> On 10/23/2012 04:58 PM, Leif Christensen wrote:
>> Am 23.10.2012 16:22, schrieb Javier Hidalgo Carrió:
>>> I want to extend RigidBodyState class, adding composition of Rigid
>>> Bodies States as operator *
>>> using the already implemented solution of envire
>>> (TransformWithUncertainty class) which handles this as a first order
>>> approximation of Jacobians which perfectly maps to the covariance
>>> matrices of the existing class.
>> What about non-parametric filters, for example histogram or particle
>> filters? Do they also have to be covered by the * operator?
> The operator * would just perform the composition of two rigid body states.
> How you computed the uncertainties is another different question. For
> it,  you could use filters mentioned above (particle, histogram, kalman
> or whatever ) this is an estimation problem depending on the probability
> distribution that better model your case.
>
> However, at the end logging to a port it will be stored in a covariance
> matrix (e.g.: base::Matrix3d cov_position of the RigidBodyState class)
> which assume a mean and a (co)-variance w.r.t to the mean (which is a
> Gaussian distribution).
> If you have a clear no Gaussian pdf as an output of a particle filter I
> would not use RigidBodyState to represent the uncertainty (I guess must
> be another class in rock for that)
>
> This is how I understand RigidBodyState: Let's have the opinion of more
> people, perhaps, they had something different in mind to it.
No, your interpretation is fine :)
>
>>    Obviously,
>> there is no need for jacobians there and they don't have covariance
>> matrices, but use the RigidBodyState. Are they covered somewhere else,
>> or do we just overload the operator then? Or am I completely on the
>> wrong track?
>>
>> Leif
>>
> Nevertheless, I could see more problematic how to propagate values as
> velocity and angular_velocity of the RigidBodyState when the composition
> * would be computed.
There are some problems with adding a composition operator to rbs:
- rbs does not store the cross-covariance of position and orientation, 
only the individual covariance matrices.
- As you say yourself what do you do with velocity?
- I see rbs more as a transport type than a computational type. Within 
the modules I usually use either base::Transform or in rare cases 
base::Pose. There is a conversion function in the 
envire::TransformWithUncertainty class, which takes an rbs.

Also, the TransformWithUncertainty is currently in envire, and in order 
to support the composition you would have to add a dependency. I would 
be happy if we moved it to base though. If you wanted you can then add a 
convenience method in rbs, converting to TransformWithUncertainty, 
perform the composition and convert back. I wouldn't go the length of 
integrating it properly.

At some point we should be looking at cleaning up all our transformation 
types....

Jakob