[Rock-dev] RigidBodyState - Uncertainty and Composition

[Jakob Schwendner]

On 10/24/2012 10:33 AM, Javier Hidalgo Carrió wrote:
> On 10/24/2012 10:04 AM, Jakob Schwendner wrote:
>> 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.
> We currently have the same problem when creating a
> TransformWithUncertainty from a rbs and the other way around. The
> cross-covariance would be zero.
>
>> - As you say yourself what do you do with velocity?
> I first solution could be vector adding and for the cov-matrices the
> same procedure that for position and orientation (that one in the
> TransformWithUncertainty of envire)
>
>> - 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
> Do you mean the base::Transform3d or Affine3d which are Eigen classes?
> They don't have uncertainty.
>
>> 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.
> I agree on moving it to base.
> However, the reason for choosing rbs instead of adding
> TransformWithUncertainty  in base/types is the already existing rbs
> visualization of vizkit.
>
>> At some point we should be looking at cleaning up all our transformation
>> types....
> I see adding composition operator to rbs a good short-term solution for
> the time being without the need of using envire types.
I don't have a problem with adding the operator, but I think it should 
be as a forward to the TransformWithUncertainty type, which means it has 
to move from envire to base.

Jakob