Matrix / Quaternion methods

Created by: AtDinesh

Hi !

I was wondering who would need to implement new function on top of Eigen's Matrix and Quaternion classes ?

Just to find out which branch is the most adapted to do to : @joanvallve --> if you need to do it also and if you are the only one we could work on motion2 branch if anyone else needs it we will have to work on master branch.

Created by: joansola

Hello,

Indeed , there is some need for what you propose. Here a wish-list:

• v2q(), q2v() --> transform rotation vector to and from quaternion.

uhm... I think my list ends here by now.

These two functions are now defined in wolf.h, with a solution that is ugly and that we need to improve. At least, we need a quatTools.h, or a solution as the one you suggest.

Then, there is all the part with Jacobians that could be nice.

How do you think this can be done? By inheriting from Eigen::Quaternion?

Created by: joansola

Regarding branches, I would create a whole new branch for this, departing from master.

Created by: joanvallve

I am working in 2D, so I think I don't need this..

Created by: AtDinesh

I was thinking about 2 ways to do that :

• First one is to inherit from Eigen::Quaternion if possible But I don't know yet if this would mean that we have to program thinks like it is done in Eigen. Since it is simple inheritance I don't think so.
• the other method would be to define a Quaternion class which has Eigen::Quaternion as a member. advantages : this solution might be ugly but it allows us to avoid multiple inheritance.

Ex : with First method we may have : Class Quaternion : public Eigen::Quaternion, LieGroup<SO3,3>{ ... } Right now I don't know what kind of strange behaviour I could have with this.

Second method : Class Quaternion : public LieGroup<SO3,3>{ protected : Eigen::Quaternion quaternion_; ... }

Created by: joansola

I guess you need to inherit, otherwise you will need to overload all the functionality of Quaternion, which is the nice part of Eigen.

I ignore if just inheriting is OK, or if we need to do it in a particular way, for example CRTP...

Created by: AtDinesh

I think it is possible to extend a class from Eigen with functions that we create thanks to preprocessor tokens. I'll have a look into this !

I let this issue open so that anyone can suggest new functions.

Created by: artivis

This could be useful later on : ceres_extensions Eigen quaternion

Created by: joansola

@artivis, 404 Page Not Found

Created by: AtDinesh

I found this : http://eigen.tuxfamily.org/dox/TopicCustomizingEigen.html

But another idea would be namespace functions.

Created by: joansola

We are already using the LocalParametrization features, which are essentially the same as the Manifold. We got inspired by the implementation in Ceres, precisely (if this is what you were suggesting @artivis)

Please check classes wolf::LocalParametrizationBase, LocalParametrizationQuaternion, LocalParametrizationHomogeneous

Created by: joansola

I have this feeling now:

• It is certainly cool to extend Eigen::quaternion to do the local parametrization thing, or the Manifold thing, or the exponential map thing, or the error state thing, which are all different names for the same thing!
• It is possibly an overkill to do it with advanced C++ techniques. I think the functionality can be achieved with much lighter tools. @AtDinesh : I like that you investigate on this. Please keep on. But please also consider having it in a lighter version, e.g. with a few functions that do the tricks that we need (we are actually in this trend in Wolf now). We can discuss it in more depth once you have a clear proposition.

Created by: AtDinesh

@artivis is this the link you wanted to share ? https://github.com/system123/ceres_extensions

Created by: artivis

oups.

I was indeed trying to link what @AtDinesh shared.

Created by: AtDinesh

OK

So I would like to know who is using the LocalParametrization classes ? A quick search gave me these results : test_local_params.cpp local_parametrization_wrapper local_parametrization_* state_block state_quaternion (using state_block)

maybe these are also used in ceres branch/tests?

Created by: joansola

Hi @AtDinesh. Long answer for your question. But I try to give you a hint here.

In Ceres, there is a LocalParametrization class, that implements the following two operations:

• The additive composition:

Plus(const double* state, const double * increment, double * updated_state)

this simply adds the increment in the minimal space (the tangent space) onto the state defining the manifold (the non-minimal state).

• The Jacobian of the above

computeJacobian(const double * state, double * jacobian)

this computes a jacobian matrix of the above with respect to the state increment, evaluated at the full state.

Because in Wolf we do not want to depend on Ceres, we replicated this in wolf, using Eigen::Map over the input/output parameters to get a cost-free interface against Ceres if we need it (Maps are essentially pointers), yet be able to pass full Eigen units (vectors, quaternions, matrices) to do the algebra (Maps can be used as real Eigen units).

You can check the Wolf classes LocalParametrizationQuaternion and others.

I give you two examples of the algebra inside Plus(), for quaternion (group H) and rotation matrices (grou SO3) using the rotation vector (so3) as the minimal error space.

• Quaternions:

  1. Compute a dq from the increment: dq = v2q(increment)
  2. Update the quaternion: q_out = q * dq; // Eigen quaternion product

• Rotation matrices:

  1. Compute dR from the increment: dR = rodrigues(increment);
  2. Update the rotation matrix: R_out = R * dR; // matrix product

where increment is the (small) rotation increment we want to compose with the orientation specified in q or R.

In both cases, dq and dR are the exponentials of increment, and conversely, increment is the log of dq and dR. The definitions of the exp() and log() operations are defined from the Taylor series of e^x applied to a 3D vector x=increment. You can check my PDF 'Quaternion kinematics for the error-state Kalman Filter' for all the algebraic details using quaternions, and also my 'courseSLAM.pdf' for the use of the Manifold thing in optimization.

In Wolf, we implemented the Quaternion, and the homogeneous vector (which, surprisingly, has the same algebra than the quaternion! see comments in the class)

To be complete with the nomenclature, let me add here a little list of terms that are equivalent (if not strictly, at least in the concepts)

• error = increment = small update, usually in the minimal, tangent, space.
• error state = tangent error = log of global error = error in the tangent space = error tangent to the manifold
• global error = exp of the tangent error
• q2v(q) is the same as log(q).
• rodrigues(R) is the same as log(R)
• v2q(v) is the same as exp(v).
• Rodrigues(v) is the same as exp([v]_x).

Also, mind that you can think of 'error' or 'increment' as being a small thing in the tangent space. Then, it can also be a small update, for example derived from integrating a single IMU data. In fact, Ceres uses it as the output of the optimization iteration, dx, to update the real state, x, using, for each state block, the code:

Plus(x, dx, x_out)

I think, after all, that only having rodrigues() and v2q() and q2v() functions we are able to do all we need to do with our quaternions or rotation matrices regarding all these concepts exposed here.

Now, having said all this, I am interested to know how elegant and pertinent is to have a second, more elaborated view, based on what Dinesh is investigating. However, keep in mind that the advantages need to be clear enough in order to justify the addition of these new C++ features, Eigen extensions, and so on.

J

Created by: joansola

Another answer to your question , Dinesh.

the LocalParametrization is used by the solver in order to compose the optimized state increment, dx, with the real state, as in:

x <-- x (+) dx

where (+) is the additive composition, implemented as Plus() in Ceres, and plus() in Wolf.

Therefore, what we have is that, each state block in Wolf has an associated LocalParametrization, which is a pointer:

• For Euclidean state blocks, the pointer is nullptr
• For other state blocks, it points to the appropriate LocalParametrizationDerived

Then, the update of the state block is done like so:

if (state_block.local_param_ptr == nullptr)
    state_block.x = state_block.x + dx; // normal sum
else
    state_block.local_param_ptr->plus(state_block.x, dx, state_block.x); // sum and project to the manifold

Created by: joansola

And still a third answer:

We will be using the same 'manifold' things to do the integration of small motions on top of

• states
• pre-integrated deltas

Both the states and the pre-integrated deltas need to be in the full state space, whereas the small motions are in the tangent-space.

The class ProcessorMotion asks all derived classes to implement both methods:

xPlusDelta(x, dx, x_plus_dx) = 0;               // implements x_plus_dx = x (+) dx
deltaPlusDelta(Dx, dx, Dx_plus_dx) = 0;    // implements Dx_plus_dx = Dx (+) dx

that are precisely the two that we need.

Note: In many many cases, these two functions do the same. But for the IMU, xPlusDelta composes the result with the initial conditions of x, which I think are expressed with a different algebra.

As an example, you can check the implementation of these functions in the class ProcessorOdom3d, which uses a Euclidean position increment, and a 3d-vector as the orientation increment. Hints:

• We use Eigen::Map over the vector and quaternion parts, so we can use Eigen algebra.
• We use remap() to update these maps at nearly no runtime cost.
• We use then the normal Eigen algebra

// class members:
Map<const Vector3s> p1_, p2_;
Map<Vector3s> p_out_;
Map<const Quaternions> q1_, q2_;
Map<Quaternions> q_out_;

// implemented pure virtual from ProcessorMotion:
void deltaPlusDelta(const VectorXs& _delta1, const VectorXs& _delta2, VectorXs& _delta1_plus_delta2)
{
    remap(_delta1, _delta2, _delta1_plus_delta2);
    p_out_ = p1_ + q1_ * p2_;
    q_out_ = q1_ * q2_;
}

// helper function remapping all the temporary members:
void remap(const VectorXs& _x1, const VectorXs& _x2, VectorXs& _x_out)
{
    new (&p1_) Map<const Vector3s>(_x1.data());
    new (&q1_) Map<const Quaternions>(_x1.data() + 3);
    new (&p2_) Map<const Vector3s>(_x2.data());
    new (&q2_) Map<const Quaternions>(_x2.data() + 3);
    new (&p_out_) Map<Vector3s>(_x_out.data());
    new (&q_out_) Map<Quaternions>(_x_out.data() + 3);
}

Final note: mapping and remapping the Euclidean parts is sure not necessary. Mapping the Quaternion part is essential to use Eigen algebra. Remapping it is good for speed.

Created by: joansola

OK guys -- I stop! :-D

Created by: joansola

Well, no. I go on with a fourth answwer.

We can, certainly, re-use the plus() and computeJacobian() methods of the LocalParametrization in our ProcessorMotion, in order to build the xPlusDelta() and deltaPlusDelta() methods, in a way like:

xPlusDelta(...)
{
    p = p + dp; // using Eigen Map and Euclidean sum
    LocalParametrizationQuaternion::plus(q, delta, q_plus_delta);  // Using Map and local param.
}

Then, the Jacobians are easy: the Identity matrix for the Euclidean part, and computeJacobian() for the quaternion part.

Created by: joansola

Hi ! Still me...

There are a few incorrections in my last two posts. Please do not take them literally.

In particular, in the last post the delta-integration function deltaPlusDelta() should not take inputs in the error space, but in the real space. Therefore, it cannot be implemented with Plus().

However, we can consider this sequence of operations after a motion data from the IMU arrives:

delta = data2delta(data, dt);
delta_integrated = deltaPlusDelta(delta_integrated, delta);

where, now, it is the data2delta the one responsible of passing from the tangent data space to the delta space. In the case of IMU data, this function could be implemented as follows:

// we suppose we have the current state estimate
state = [p q v a_b w_b]; // pos, quat, vel, acc bias, gyro bias

// we suppose we receive the following data
data = [a w 0 0]; // each entry is a 3-vector; the zeros are the bias random walks (they do have non-null covariance!)

// now we integrate the data over dt
a_dt = (a - a_b)*dt;
w_dt = (w - w_b)*dt;

// now we transform it into the delta-space
dq = v2q(w_dt); // this is the only exp() mapping we need: all the rest is Euclidean.
dv = dq * a_dt;
dp = 0.5 * dq * a_dt;
da_b = 0;
dw_b = 0;
delta = [dp dq dv da_b dw_b];

// now we integrate on top of the delta-integral
deltaPlusDelta(delta_int, delta, delta_int);

This code, again, is not 100% sure. In particular, I do not know if I am performing the pre-integrated delta in the proper way for the IMU, by isolating the initial conditions. But the idea of the algorithm I think holds:

1. integrate the data in its own space over dt;
2. do the exp() mapping to go to delta-space;

• integrate the delta.

Finer details need to be defined by the research of Dinesh. Moreover, we need also to account for the Jacobians and covariances, but these would be just the logical consequence of writing the proper mathematics in the code above.

Created by: AtDinesh

So First of all thank you for all these explanations.

I'm reading again the papers just to see which functions we will need to implement. For now I think that extending Eigen with our own functions may not be easy to do. It would be an elegant solution but I don't think that there are so many advantages in doing so. However, for simple functions it's still a solution.

The code I am working with (including Group, SO3, LieGroup, ...) is well done but includes dependencies to boost and it is quite complex for people who are not used to work with advanced C++ tools that are used.

Otherwise, here at LAAS, Gepetto worked on this : https://github.com/stack-of-tasks/pinocchio/wiki This library implements the Lie Algebra,it is very efficient and it works with Eigen ! but Pinocchio is a lot more than just the Lie Algebra C++ implementation. It is used to compute rigid body dynamics... Now, according to Florent and Olivier, it would be difficult to reimplement all those methods in WOLF by ourselves (and it would take time..). Most of the dependencies of this library being optional, installing it/putting it in wolf can be done. But in their code corresponding to SE3 I didn't see those dependencies and it seems that I can just use some of the files and import them in WOLF (this is also what another PhD working on Pinocchio's development thinks). So I am investigating this but I think we could use their implementation of SE3, code the missing functions that we will need (rodrigues, tangent space, ..) and maybe even make LocalParametrizationBase inherit from SE3 if needed ! By doing so we would not have to modify codes in which LocalParametrization classes are already used.

Created by: artivis

Regarding the Lie Group, Sophus library might worth a look.

Created by: joansola

Closing. For reference, please consult the closed issues.

Created by: joansola

@AtDinesh , I looked at the explog.hpp code from pinocchio and it looks neat. I wonder if we can implement the same thing using the quaternion and not the matrix representation.

What I like is that it interfaces very well with Eigen, much better than my v2q() and q2v() which are dumb generic templates.

Created by: joansola

I really want to use the quaternion in WOLF. I have interesting questions to investigate, which will lead to a paper that I think will be very good. We'll discuss it when we have the occasion.

Eigen already provides the group operation * for quaternions.

If we incorporate exp() and log(), then we are ready to do everything "manifold' style, as we discussed during my last visit to LAAS.

If we code all well integrated into Eigen then we'll have a solid alternative to SO3/so3 implemented with matrices, and we'll be able to evaluate their differences -- that's what I want to do.

This is indeed an interesting point.

Created by: AtDinesh

The integration of spatial part from pinnochio has been moved to the "spatial pinnochio" branch created from matrix_quaternion_tools branch. I am considering first implementing some of the quat_tools function from rtslam that can also compute jacobians (just to get the imu thing working at least). We may also have to write a simple documentation if we use se3 from pinnochio.

Created by: joansola

OK. Can you please rename v2q --> exp and q2v --> log ? Or better, keep both names (one calls the other, but the implementations are in exp and log)?

Can we typedef methods? as in typedef exp v2q;? I thnkk it's possible, though not that straightforward. Look at this line (from Factory.h):

typedef TypeBase* (*CreatorCallback)(const std::string & _filename);

otherwise, just define v2q() that calls exp()... easy.

Created by: AtDinesh

I am still working with quaternions. But I agree that we need something well integrated into Eigen.

So at the end would you prefer to have something like pinnochio (something working fine with Eigen) integrated into Wolf? Another possibility would be just to continue like this, keep the current branches, keep working on the "spatial pinnochio" and implement functions from rtslam (maybe modified but still not working with Eigen as well as pinnochio) in the imu branch so that we can test things even if spatial pinnochio is not ready.

Created by: joansola

Well, yes, the branches are for developing out of the master branch. For me it's OK if we have the IMU branch for rtslam-based stuff, more dedicated to make the IMU work, and the spatial pinocchio for other tests and investigations. At one point, however, I will want to compare both quaternion and rotation matrix things. But I think this can be done later in the future if we feel like.

Therefore OK, just keep doing.

Created by: joansola

Take into account that the pinocchio stuff is matrix-related. My point is that I want a quaternion-related SO3-so3 code. And that, seing the code of Pinocchio, I think we can take inspiration to write that for the quaternion in a more proper way than rtslam's quat_tools.hpp. But again, do as you wish by now to make the IMU work.

Created by: AtDinesh

Therefore there have been changes in spatial part of pinnochio but they did not push it yet. I have been talking to Justin (he's working on pinnochio's development), we are investigating on how to get jacobians while doing products etc. And yes pinnochio is now mostly matrix related (from Justin's investigation matrices were mote interesting in a computationnal point of view.) It is possible to get inspired from it or extend pinnochio to make it use quaternions.

Created by: joansola

That's a good point. From my side, I'll be motivated to write that code myself, if needed, but it'll have to be after end of June as now I am busy with other (boring) stuff.

Created by: AtDinesh

I may come to think about how to use quaternions properly. I'll give more info here.

Created by: nmansard

About efficiency of matrix vs quaternions: it is difficult to compare directly both. Nominally, quaternion (product for example) induces less operations, but matrix can be vectorized, which finally induces less CPU operations. But more cache. Or maybe not. All in all, it is pretty much the same and very context dependant.

@joansola typdef defines a type, while for q2v you rather want an "alias" on a function. You can you #define, which is ugly. #define q2v log I think the good solution is to make q2v an inline function, calling log with the same arguments. It is then removed by -0n (n>1) flags at compile time.

@AtDinesh Pinocchio is matrix based and will not become quaternion based (for various reasons that I can explain directly to you if you want).

Created by: joansola

Thank you @nmansard

My idea is that q and R are the same in terms of the SO3, so3, exp, log, etc.

My idea is also that, being the same, we can perfectly use q where others use R.

My idea is also that, if it's the same, it will be computing considerations that will make the difference:

• Computing time
• rubustness to numerical errors, stability, etc.

As Nico says, q is lighter but R can be vectorized. In this view, I think that R will win. Moreover because, once you need to rotate more than one vector (which happens all the time), then it is better to convert q to R, and then use R, than just use q directly each time.

My point finally is that it is possible that q wins over R only when we consider the Jacobians, because in R we'll have 3x9 Jacobians whereas in q we'll have 3x4 Jacobians. Moreover, when integrating the Delta observations of the IMU, the integrated covariance will be 4x4 in q, 9x9 in R (the delta pre-integration is done in exp or manifold space, not in log or tangent space, for reasons also of efficiency, and even for the covariance).

The point is that:

1. we represent the SO3 state with q
2. we convert it to R to operate if we need to rotate more than 1 vector.
3. we compute Jacobians wrt q, not wrt R (wrt means "with respect to")
4. we update the state on the manifold of q, not that of R, using smaller 4x3 Jacobians instead of 9x3
5. we never have to care about R not being orthonormal etc, because q can be enforced to unity with much simpler methods than R orthonormal.

This is what I would like to investigate. Of course, if we want to compare computing time, we must be as careful when coding the q code as we have been when coding the R code.

I think it makes sense. We spend some extra time in 2., but we expect to recover it in 3. and 4., and we have the bonus of 5. Only if we managed to pre-integrate the covariance in the tangent space (which is an interesting idea), then would R regain its privileged position. But otherwise, the 3x9 and 9x9 matrices would be too big.

Created by: nmansard

I think that using q makes perfectly sense. I agree with the logic of 1-5. I think that it is the best choice for Wolf.

Two extra remarks, for any interest.

• In FCL (fast collision library), they are always storing q, and sometime storing R, with a boolean flag to remember if R as been computed or not.

• When working with R directly, you never compute the Jacobian (9 rows) of R (say f_R, the jacobian of f(R) with respect to R) but the tangent map of f T_f = exp_f f_R , i.e. the product of the Jacobian of exp times the product of the jacobian of f (size of T_f: 3 rows). Then to make a step, you directly integrate with the exp.

1. Represent with R
2. NR
3. Compute tangent map (3 rows)
4. When update the state (with a small rotation wdt -- w being an angular velocity -- instead of a small rotation dq), do R = Rexp(w*dt).
5. With my experience, this does not drift out of the space of orthogonal matrices. But it might be logical to assume that it does.

On 05/26/2016 01:42 PM, Joan Solà wrote:

Thank you @nmansard https://github.com/nmansard

My idea is that q and R are the same in terms of the SO3, so3, exp, log, etc.

My idea is also that, being the same, we can perfectly use q where others use R.

My idea is also that, if it's the same, it will be computing considerations that will make the difference:

• Computing time
• rubustness to numerical errors, stability, etc.

As Nico says, q is lighter but R can be vectorized. In this view, I think that R will win. Moreover because, once you need to rotate more than one vector (which happens all the time), then it is better to convert q to R, and then use R, than just use q directly each time.

My point finally is that it is possible that q wins over R only when we consider the Jacobians, because in R we'll have 3x9 Jacobians whereas in q we'll have 3x4 Jacobians. Moreover, when integrating the Delta observations of the IMU, the integrated covariance will be 4x4 in q, 9x9 in R (the delta pre-integration is done in exp or manifold space, not in log or tangent space, and even for the covariance, for reasons also of efficiency).

The point is that:

1. we represent the SO3 state with q
2. we convert it to R to operate if we need to rotate more than 1 vector.
3. we compute Jacobians wrt q, not wrt R (wrt means with respect to)
4. we update the state on the manifold of q, not that of R, using smaller 4x3 Jacobians instead of 9x3
5. we never have to care about R not being orthonormal etc, because q can be enforced to unity with much simpler methods than R orthonormal.

This is what I would like to investigate. Of course, if we want to compare computing time, we must be as careful when coding the q code as we have been when coding the R code.

I think it makes sense. We spend some extra time in 2., but we expect to recover it in 4., and we have the bonus of 5. Only if we managed to pre-integrate the covariance in the tangent space (which is an interesting idea), then would R regain its privileged position. But otherwise, the 3x9 and 9x9 matrices would be too big.

— You are receiving this because you were mentioned. Reply to this email directly or view it on GitHub https://github.com/IRI-MobileRobotics/wolf/issues/54#issuecomment-221848450

Created by: joansola

I see. So, during the optimization iterations, you are already compacting the two Jacobians together to avoid the 9x3 (or 3x9) Jacobians. This I felt it needed to be done, great. I think Ceres is designed differently, because it operates on the state vector (a 9-vector for R for what we discuss here), and asks you for the Jacobian wrt the tangent space. This last operation is done by the LocalParametrization class. My point is that, if it asks us for this Jacobian, it means it is going to multiply it by another Jacobian that it computed. Like this:

1. let's say we have an error function f(R) of dimension n ---> n vector
2. Ceres computes df/dR by any means (e.g. automatic differentiation) ---> F_R = nx9 matrix
3. Ceres calls us for the local parametrization, having dR/d(w_dt) ---> R_wdt = 9x3 matrix
4. The total Jacobian is df/dw_dt = df/dR * dR/dw_dt ---> F_wdt = nx3 matrix

So if I get you correctly, you compute directly the Jacobian F_wdt in 4. without going through 2. and 3. right? then, if this is correct, using q at the optimization time will not bring great benefits. Actually, this scheme would be parametrization-agnostic for the Jacobian.

We have a second case where Jacobians count: during pre-integration. Here, we integrate in the manifold space (R in the paper's case Dinesh will be basing the developments on, q in our proposal for Wolf), also the covariance. Integrating in R or q is convenient, because it's the space where we can accumulate rotation properly. But then we need to integrate also the covariance. In my view, they integrate it in a 9x9 matrix (which is singular by definition), and only at the last moment, when it is time to close the integral and produce a motion factor, we express this covariance in the tangent space. Here I illustrate the cases for R and q:

1. Get a motion increment in ltangent space: wdt, with covariance WDT
2. Integrate into SO3: R = R * exp( [wdt]x ) // q = q * exp(wdt)
3. Get the Jacobians of the above: R_wdt (9x3 matrix) // q_wdt (4x3 matrix)
4. Integrate the Covariance: P = P + R_wdt * WDT * R_wdt' (9x9 matrix) // (4x4 matrix)
5. At the time creating (or using?) the motion factor:

• mean: theta = log(R) // theta = log(q)
• Jacobian: T_R = dtheta / dR (3x9 matrix) // (3x4 matrix)
• Covariance: T = T_R * P * T_R' (3x3 matrix)
• Info matrix: W = T^-1, possibly store the sqrt of W too using Cholesky (3x3 triangular matrix)

Assuming an IMU at 1kHz, points 1--4 are done 1000 times per second. 5 is done once per solver iteration (using the motion factor), or possibly only once at the time of creating the factor (not sure about this)

If I am right in what I think, then using quaternions gives us advantage at the time of pre-integration.

Finally, I guess that storing the covariance at 1kHz always in the tangent space would require multiple conversions exp-log at 1kHz, which are unnecessary. But I also think that this needs extra investigations, and we may want to try to compact the computation as we did above -- obtaining also a representation-agnostic scheme for the covariance.

Created by: nmansard

I am much less confident with covariance. I understood that covariances were living in the tangent space. Shouldn't the covariance matrix be 3x3 (agnostically of the paramatrization as well)?

On 05/26/2016 05:47 PM, Joan Solà wrote:

I see. So, during the optimization iterations, you are already compacting the two Jacobians together to avoid the 9x3 (or 3x9) Jacobians. This I felt it needed to be done, great. I think Ceres is designed differently, because it operates on the state vector (a 9-vector for R for what we discuss here), and asks you for the Jacobian wrt the tangent space. This last operation is done by the LocalParametrization class. My point is that, if it asks us for this Jacobian, it means it is going to multiply it by another Jacobian that it computed. Like this:

1. let's say we have an error function f(R) of dimension n ---> n vector
2. Ceres computes df/dR by any means (e.g. automatic differentiation) ---> F_R = nx9 matrix
3. Ceres calls us for the local parametrization, having dR/d(w_dt) ---> R_wdt = 9x3 matrix
4. The total Jacobian is df/dw_dt = df/dR * dR/dw_dt ---> F_wdt = nx3 matrix

So if I get you correctly, you compute directly the Jacobian F_wdt in 4. without going through 2. and 3. right? then, if this is correct, using q at the optimization time will not bring great benefits. Actually, this scheme would be parametrization-agnostic for the Jacobian.

We have a second case where Jacobians count: during pre-integration. Here, we integrate in the manifold space (R in the paper's case Dinesh will be basing the developments on, q in our proposal for Wolf), also the covariance. Integrating in R or q is convenient, because it's the space where we can accumulate rotation properly. But then we need to integrate also the covariance. In my view, they integrate it in a 9x9 matrix (which is singular by definition), and only at the last moment, when it is time to close the integral and produce a motion factor, we express this covariance in the tangent space. Here I illustrate the cases for R and q:

1. Get a motion increment in ltangent space: wdt, with covariance WDT
2. Integrate into SO3: R = R/exp( [wdt]x ) // q = q/exp(wdt)
3. Get the Jacobians of the above: R_wdt (9x3 matrix) // q_wdt (4x3 matrix)
4. Integrate the Covariance: P = P + R_wdt * WDT * R_wdt' (9x9 matrix) // (4x4 matrix)
5. At the time creating (or using?) the motion factor:
   • mean: theta = log(R) // theta = log(q)
   • Jacobian: T_R = dtheta / dR (3x9 matrix) // (3x4 matrix)
   • Covariance: T = T_R * P * T_R' (3x3 matrix)
   • Info matrix: W = T^-1, possibly store the sqrt of W too using Cholesky (3x3 triangular matrix)

Assuming an IMU at 1kHz, points 1--4 are done 1000 times per second. 5 is done once per solver iteration (using the motion factor), or possibly only once at the time of creating the factor (not sure about this)

If I am right in what I think, then using quaternions gives us advantage at the time of pre-integration.

Finally, I guess that storing the covariance at 1kHz always in the tangent space would require multiple conversions exp-log at 1kHz, which are unnecessary. But I also think that this needs extra investigations, and we may want to try to compact the computation as we did above -- obtaining also a representation-agnostic scheme for the covariance.

— You are receiving this because you were mentioned. Reply to this email directly or view it on GitHub https://github.com/IRI-MobileRobotics/wolf/issues/54#issuecomment-221911171

Created by: joansola

They should, but they do not necessarily have to, at least not until one wants to use them properly. Any covariance larger than 3x3 will be singular. But as long as we do not try to invert it, it's OK.

But I think you are right, and computing it in the tangent space will be the way to go.

Finally, using quaternions is not going to be any better than using R directly... it's a pity if that's the case! I have no time now to go into the math, neither to check on the paper to see in which format they are integrating the covariance. Maybe @AtDinesh could help! :-D

Created by: AtDinesh

I'm on it !

As far as I read I think that : noise is expressed is tangent space and is considered to be zero-mean and to have a covariance WDT then with exp() it is expressed in SO3. So I agree with point 1 to 3 above.

To give more information about the covariance (9x9 matrix), it is computed iteratively using IMU specifications. If we note Delta_noise = [rotation_noise, velocity_noise, position_noise] (9x1 vector) and measurement_noise = [acc_noise, gyro_noise], they define an iterative computation for Delta_noise as : Delta_noise(i->j) = A(j-1) * Delta_noise(i->j-1) + B(j-1) * measurement_noise(j-1)

The covariance of the IMU measurements is a 6x6 matrix. Then Covariances are iteratively computed as Delta_cov = A * Delta_cov * A' + B * Measurement_noise_cov * B'

Since members of Delta_noise are expressed in the tangent space, the Delta_cov should remain in the tangent space right ? But in this case I don't see where the multiple conversions with exp and log may be required yet. Then as you said the covariance would be used at the creation of a factor and during optimization to find a maximum a posteriori.

I continue to investigate this.

Created by: joansola

@AtDinesh , are you saying that, in the paper, the covariance is integrated as a 9x9 matrix? Or as a 3x3 matrix? That's the question we need to solve!

Created by: AtDinesh

From what I understand their preintegrated measurement covariance is a 9x9 matrix that includes preintegrated velocity covariance, preintegrated position covariance and preintegrated rotation covariance (all 3x3 matrix)

But talking about rotation only, the jacobian is not computed from R, but from wdt living in the tangent space (vector 3) resulting in a 3x3 jacobian. This is what they call the right jacobian supposed to relate additive increments in tangent space to multiplicative increments applied on right hand side. From this and speaking about the rotational part of the covariance, it is expressed as a 3x3 matrix.

I am trying to confirm this from the code.

Created by: joansola

OK, this makes a lot of sense. Please confirm when you have definitive info.

Created by: AtDinesh

I could not find the covariance matrix I have been looking for. I think it is included in the preintegrated measurement covariance matrix as a 3x3 block.

Created by: AtDinesh

This is a note concerning one the point discussed above.

The Jacobian of a rotation matrix wrt quaternion parameters can be summarized as 2(w^)', w^ being the skew symmetric matrix of w actually, : f = R*x --> df/dq= (dR/dq)*x = [0 2z -2y 0; -2z 0 2x 0; 2y -2x 0 0]

note : This was done with the [vector scalar] quaternion form instead of [scalar vector] source : Wheeler, Ikeuchi, "Iterative Estimation of Rotation and Translation using the Quaternion"

Created by: joansola

Le document attache es le cours graphSLAM courseSLAM.pdf

Dans 2.38 il y a le modele cinematique IMU. Il n’est pas celui qui fait la pre-integration, mais l’aspect SO3 y apparait de la meme maniere. Dinesh a les equations correctes opur la pre-integration, et mon discurs qui suit doit s’appliquer aux deux cases sans grande difficulte.

Dans ces equations,

• R{q} est la matrice issue du quaterninon q
• q{wdt+theta} est exactement exp{wdt+theta}
• \otimes est le produit de quaternions, que j’ecris ** dans cet email.

Tu peux donc facilement passer de cette representation ‘q’ a une representation ‘R’ en modifiant:

• R{q} ==> R (equations a. et b.)
• q <— q ** q{wdt+theta} ==> R <— R * exp(wdt+theta) (equation c.)

En appliquant plusierurs fois 2.38, on construit une integrale de l’etat, que j’appelle ici x_integral.

Ensuite, la construction des facteurs se fait une fois on a integre 2.38 le long de plusieurs '\Delta t’. A la fin de cette integral , pour construir le facteur, on doit avoir :

• moyenne de l’etat integre, x_integral = [p, q, v]
• Covariance de l’etat integre, E( (X-x) * (X-x)’ ), ou de l’etat erreur, E(dx * dx’), c’est le sujet de discussion ici.
• La Jacobienne, c’est aussi le sujet de la discussion :
  • de qui ?, et
  • par rapport a qui ?, et
  • calcule de quelle maniere ?

A mon avis, le facteur doit etre fait en definissant un residu ‘e’ comme suit (attention aux parentheses):

e = f ( x_0, x_T, u_k) = log [ ( (x_0 ** exp( dx_0) ** (x_integral) )^-1 ** (x_T ** exp(dx_T) )  ]

ou,

• x_0 et x_T sont les estimations de l’etat au debout et a la fin de l’integral,
• dx_0 et dx_T sont ses erreurs (ou 'vitesses', ou 'steps', ou 'corrections'), dans l'espace tangent.

Maintenant, log() et exp() sont les expressions qui permettent passer du manifold au tangent et viceversa (je crois qu’ils meritent un autre nom dans ce contexte generique de ‘manifold'. Probablement tu le sais.), et ** est l’operateur de groupe du manifold. Ces operateurs reviennent, respectivement, a :

• L’identite et la somme pour ce qui concerne les variables rectangulaires (ou Euclidiennes),
• Le exp() et log() de ‘q' ou ‘R', et le produit de ‘q' ou 'R', pour ce qui concerne SO3.
• De la meme maniere on doit interpreter l’inverse ^-1 dans l’equation precedente, etant soit l’inverse ou transpose de R, l’inverse ou conjugue de q, et le negatif des grandeurs rectangulaires.

Les facteurs sont decrits en page 32, dont \Omega_k est la matrice d’Information, ou inverse de la covariance, du residu du facteur. Dans notre cas, et parce qu’on a fait le log(), on se retrouve avec un facteur dans l’espace tangent, et donc, sa covariance est dans l’espace tangent.

Le doc continue, et definit les Jacobiennes du residu ‘e_k' en deux etapes:

1. d’abord, par rapport a l’etat, J_ki = de_k / dx_i (eq. 4.44)
2. Ensuite, on considere le manifold separemment, et on trouve la Jac du bloc etat x_i par rapport au bloc erreur dx_i: M_i = dx_i / d dx_i (eq. 4.58)
3. Finalement, chaque bloc de la Jacobienne fait la composition par regle de la chaine, J'_ki = J_ki * M_i (eq. 4.56, 57 et 59)

C’est precisement cette maniere de proceder, en coupant les operations comme decrit en 1–3 ci-dessus, qu'on se retrouve a calculer des Jacs grosses (J et M depennent, dans une de ses dimensions, de ‘q' ou ‘R'). Et c’est ce qui Ceres impose en nous demandant M_i (qui est precisement la Jac de la LocalParametrization). Si on calcule directement J’, alors tout est plus petit et agnostique des parametrisations.

Une question, neanmoins, reste ouverte, et a mon avis elle est tres importante pour nous: bien avant de construire le facteur, on doit pre-integrer, et on a a peine 1ms pour ce faire a chaque fois. Dans ce cas, la question est:

• est-il interessant d’integrer la covariance dans l’espace tangent?,
• ou il vaut mieux integrer dans l’espace globale et la transformer a l’espace tangent juste a la fin de l’integral?
• Quel est le cout de calculer une Jacobienne dans l’espace globale?
• Et dans l’espace tangent?,
• et de faire les produits pas des matrices de covariance qui peuvent etre grosses?

Created by: joansola

Pour donner une reponse aux questions de la fin du post precedent, je me concentre ici sur la partie SO3. En particulier, on cherche a trouver une alternative a 1. 2. 3. dans le post precedent qui nous permete de calculer J' = d e / d wdt = J * M dans un seul coup.

Je vois ca:

1. Imagine la derivee d'une rotation par rapport a l'erreur ou pas d'orientation wdt:

J' = d (R{q} * p) / d(wdt),

ou q = q_0 ** exp{wdt}

2. On fait bien en ecrivant

R{q} = R{q_0 ** exp{wdt}} = R{q_0} * exp{ [wdt]_x } = R_0 * exp{ [wdt]_x } ~= R_0 * (I + [wdt]_x)

3. Alors

J' = d( R_0 * (I+ [wdt]_x) * p ) / d wdt = -R_0 * [p]_x <-- je me trompe?

et on voit ici que la parametrisation 'q' disparait ! C'est ceci que j'appelait 'parametrization agnostic'.

Ce resultat est connu, mais il nous donne la reponse cle. En plus, il corresponds je crois au lien que Dinesh avait passe sur github dernierement, meme si en lisant le texte je trouvait l’ensemble trop confus.

Il est clair que (3. J' = - R_0 * [p]_x ) es tres tres facile a calculer! Comme comparation, avec l’approche par regle de la chaine, pour trouver J’ en utilisant q on devrait faire:

1. J = d (R{q} * p) / dq —> 3x4
2. M = d q / d wdt —> 4x3
3. J’ = J * M —> 3x3

ou encore pire, avec la matrice de rotation,

1. J = d (R * p) / dR —> 3x9
2. M = dR / dwdt —> 9x3
3. J’ = J * M —> 3x3

Which is the status of this issue? Is it going to be replaced by manif library? @jsola

added 1 deleted label

I´d say yes. This can be closed.

closed