Fix PartialPriorFactor (again)

[miloknowles]

Unfortunately, while "fixing" the PartialPriorFactor in PR #721, I made a mistake in the factor's error calculation. While the Jacobians pass the numerical check, the factor does not return zero error at the zero linearization point, which is now asserted in the unit tests here. Sorry!

The problem is that T::LocalCoordinates(p) does not simply return the translation part of p in the tangent vector (as I assumed), resulting in an incorrect error for translation priors. For example, in Pose3::Logmap, the translation part of the tangent depends on rotation:

const Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);

I think the same issue should come up in Pose2::Logmap if SLOW_BUT_CORRECT_EXPMAP is set.

I'm not sure how to fix this problem in a way that would work for any manifold. If we restrict PartialPriorFactor to work with Pose2 and Pose3, then the easy fix is to reuse the code from PoseRotationPrior and PoseTranslationPrior. If anyone can think of a nicer way to fix this, any help would be appreciated.

[dellaert]

Hey @miloknowles
I only see changes to a test here, is that correct? Also, some CI is failing...

[dellaert]

Re-reading: maybe the intent is just to show that unit tests fail?
I don't know how to fix - busy with other stuff now - but could this be resolved in general by seeing the "partial projection" as a function with its own Jacobian?

[miloknowles]

@dellaert the factor works now, but someone should probably review the implementation.

I don't think it makes sense to put a prior on the tangent vector directly, which is how PartialPriorFactor used to work. For example, the translation part of the tangent for Pose2/Pose3 != world_t_body, so it's inconvenient to use the tangent to constrain position in the world frame (e.g putting a prior on altitude). I also could not figure out how to use Local/Retract with a partial tangent vector, since that's all this factor has access to.

My solution is to put a prior on a "parameter vector", which can be different from the tangent and allows for more natural constraints (in my opinion). For example, I chose the Pose3 parameter vector to be [ rx ry rz | tx ty tz ], allowing us to achieve the "altitude" constraint by simply putting a prior on index 4.

The downside is that a parameter vector needs to be implemented for each variable type. It was easy to do this for Pose2, Pose3, and PoseRTV, but you may not like the added complexity.

[dellaert]

It may be a bit before I get to this, because I’m very busy with end of semester duties. However, I glanced and did see some PoseRTV specific code in the PR. If there is a way to make this generic, that would be better. Also, NavState is the modern way to go :-)

[varunagrawal]

@miloknowles I had to undo some formatting to understand the core changes so I apologize for that. I'll revert them after we figure out the necessary updates to this PR.

My $0.02: The docstring does mention that the error may be high when dealing with highly nonlinear manifolds, as well as the fact that we operate on the tangent space vector. Something to note is that when the tangent vector for the rotation component of Pose3 has a small norm (< 1e-10), the tangent vector for the full Pose3 has the same translation.

This leads to 2 possible cases which make sense to me:

1. The user has access to the rotation and can generate the tangent vector using the logmap. In which case the measurement should be the tangent sub-vector after calling Logmap.
2. The user only has access to the partial measurement: In this case, they can either tolerate the error or set the rotation to identity so they get access to the exact translation vector.

I don't quite agree with your parameterization since the translation component is not the true tangent vector. As you have already mentioned, this new parameterization is akin to PoseRotationPrior & PoseTranslationPrior, making PartialPriorFactor redundant since one can use the former factors.

For these reasons, I propose to update the tests to use the aforementioned tangent sub-vectors.

[dellaert]

Are we making an assumption here on the type of T ? If so, consider template specialization