Use of apriori known vaues and covariance matrix with local parameterization.

[xstamat...@gmail.com]

Hi all,

I was wondering if anyone has knowledge related to what is mentioned in the title.

Use of apriori-known values and covariance (constraints) is fairly simple when local parameterization is not applied. I can expand more on than if needed but as an example I've coded up in ceres a 7 DoF transformation between two random 3D points sets that is using Euler angles. The outcome of course is that the solution values are "constrained" by the provided variances-covariances.

However, I do not like the use of Euler angles due to the singularities that might arise. So I prefer the use of a quaternion with local parameterization. I am not sure if it is possible though to introduce apriori known values and covariance similarly to the case with no local parameterization and I have not been able to find much info in relation to that so hopefully someone has some info.

Since angles and covariances are easier to visualize in Euler angles the input is expected in that form. So we have to

1) Convert Euler angles to quaternion

2) Calculate Jacobian d_quataernion / d_euler which is a 4x3 matrix

3) Convert the Euler Covariance to quaternion by 

      (d_quataernion / d_euler) * Cov_euler * (d_quataernion / d_euler).transpose() which will be a 4x4 matrix

Up to here all is good.

Now we deal with the jacobian in relation to the equality constrains equations that is simply a 4x4 identity matrix.

Jacobian_constrains * Stiffness_quat.sqrt() * multiplied later with exp()

      identity 4x4          x          4x4                        x        4x3        

Now however we see a problem, the 4x4 stiffness matrix for the quaternion is not something we can calculate as it only has 3 Degrees of Freedom and thus its covariance is not invertible.

I've seen this post https://github.com/ceres-solver/ceres-solver/issues/311 where it was mentioned that another user took the inverse square root, so I wonder how ?

Unless we get its pseudo-inverse using SVD, but even then there are problems if an Euler covariance matrix were is to have a small sigma say 1e-6 for one angle only, the square root of the matrix is falling.

Another thought that I had was that maybe the approach to this is to convert the quaternion covariance to the local parameter space which "should reduce" it to the 3 Degrees of freedom and thus we should be able to invert it. Using the covariance propagation law we would have something like

Cov_local = (d_local_params / d_quat) * Cov_quat * (d_local_params / dq).tranpose()

                   3x4                    4x4                   4x3

I got not idea yet though on how the Jacobians aboce would look like or if it is even something valid.

For the next step in the calculation it would be something like this.

Idenity * exp() * Cov_local.inverse().sqrt()

  4x4   x  4x3      x      3x3

  

From this I think we also see that this wont work with the ceres API since the expectation is a that the jacobian before applying the local parameterization should be 4x4.

Regardless of ceres though I wonder if anyone has worked on something similar or is aware of any literature discussing introduction of constraints in the local parameter space (that are converted from Euler angles) and what is the proper way of doing it.

Any thoughts?

[Sameer Agarwal]

Hello,

You are right, the covariance for a quaternion lives in the tangent space that is centered on the mean. The problem is that currently the local parameterization object only implements the "plus" operation that allows you to add a vector in the tangent space to a point on the manifold. To be able to define the Gaussian we need the minus operation which given two points on the manifolds, finds the vector in the tangent space of one of the points which will take you to the other point via the retraction.

This does not exist right now, but it is on my agenda to add to Ceres for the next release. Another CL which adds marginalizations support to ceres is also waiting on this change. This however requires changing one of the public facing APIs of Ceres in a non-trivial way so it will take me a bit to do it right.

Sameer

--
You received this message because you are subscribed to the Google Groups "Ceres Solver" group.
To unsubscribe from this group and stop receiving emails from it, send an email to ceres-solver...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/47100f5f-a370-47ef-8122-b4fc826c6ae8n%40googlegroups.com.

[Christos Stamatopoulos]

Hi Sameer,

Thanks for the response.

While I get it in general terms I think I am missing an implementation detail probably, that I hope you can shed some light on.

On the retraction part, I get it, we need to find the

     residual = apriori_mean_quat - cur_iter_values

but in tangent space which is fairly easy. 

Using the Sophus library it will be something along the lines below, while for ceres it will be a something similar, a combination of conjugate quaternion, ceres::QuaternionProduct followed by ceres::QuaternionToAngleAxis.

    Eigen::Map<Sophus::SO3d const> const apriori( apriori_mean_quat.coeffs().data());

    Eigen::Map<Sophus::SO3d> cur_vals(cur_iter_values.coeffs().data());

    Sophus::SO3d delta_q = apriori.inverse() * cur_vals;
    Eigen::Vector3d delta_tangent = delta_q.log();

Am I correct in assuming that for the covariance we just need the covariance propagation to the tangent space, as in

Cov_angleAxis = d_AngleAxis / d_quaternion * Cov_quat * ( d_AngleAxis / d_quaternion).transpose()

     3x3                                     3 x 4                          4x4                 4x3

                                                                    [ quat.x ]
whre AngleAxis = 2 * atan(n / quat.w) / n * [ quat.y ]
                                                                    [ quat.z ]

with n = sqrt( quat.x^2 + quat. y^2 + quat. z^)  

Or is it something else ?

As a simple test I am using a simple 7 DoF transformation between generated points where I create an Euler covariance whereby the rotation of X axis is known, and thus has a sigma of 10e-6 (variance of 1e-12) while the other two are unknown so I just set them to 1 rad so they are free to adjust.

1 is sort of arbitrary, it could as well be 0.5. So yes in this case I don't have any covariances but this test is easier to deal with as a starting point. I expect that the rotation value around X will be minimal after the adjustment (and this is the case with the Euler angles). 

In the local parameterization case I convert the Euler covariance to Quaternion and then to Axis angle. Looking at the variances, the Euler one makes sense to me small variance --> large stifness matrix which leads to really small corrections to the known value.

Now looking at the converted covariances (it is a full covariance matrix after conversion) either quaternion or axis angle nothing is really that small to "constrain the adjustment" so even the value for the small sigma is adjusted.

Any thoughts ?

You received this message because you are subscribed to a topic in the Google Groups "Ceres Solver" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/ceres-solver/0JhOF_TuFAs/unsubscribe.
To unsubscribe from this group and all its topics, send an email to ceres-solver...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/CABqdRUCku8P%2Bht0u17YqpLOzdwUF8K_db-nJMEke17%2B0b8%3DPqQ%40mail.gmail.com.

[Dmitriy Korchemkin]

Hi,

I suspect there might be a problem with the uncommutativity of rotations and, therefore, two possible ways of computing covariance of the residual in the tangent space.

Let us denote an expected (mean) rotation as \omega, \Delta being 3-vector distributed as multi-variate normal distribution N(0, \Sigma). Then, there are two possible notations for rotation \omega_\Delta corresponding to \omega perturbed by \Delta (\circ being SO3 group product):

a. \omega_\Delta = \omega \circ \exp(\Delta)

b. \omega_\Delta = \exp(\Delta) \circ \omega

Then, the ways to obtain back the 3-vector \Delta [with distribution N(0,\Sigma)] are different, namely:

a. \Delta = \log (\omega^{-1} \circ \omega_\Delta)

b. \Delta = \log (\omega_\Delta \circ \omega^{-1})

In terms of camera's orientations these two notations correspond to  either ("1" and "2" used intently; the exact correspondence to "a" and "b" depends on whether you store transformations from the camera to the world or in a backwards way):

1.  Constraining camera orientation using a covariance prior expressed in the terms of this particular camera coordinate system.

2.  Constraining camera orientation using a covariance prior expressed in the terms of the world coordinate system.