Pose3 factor implementation

[Lauri Suomela]

Hi everybody,

I'm trying to implement a factor to incorporate full pose measurements (orientation + translation) expressed in world coordinates, along the lines of https://gtsam.org/tutorials/intro.html example 3.2 (the custom unary factor).

I just wanted to verify if my approach is correct / makes sense.

1) In GTSAM, which is the preferred way of expressing angular differences (in computing the residual between orientations) ?

2) Jacobian computation

To compute the matrix H for the 'evaluateError' function required by each factor, I need to find the derivative of the measurement function w.r.t. the pose parameters expressed in tangent space. To do this, my understanding is that I need to:

1. Use the retractions for SO(3) and SE(3) from page 22 of https://github.com/borglab/gtsam/blob/develop/doc/math.pdf to calculate the closed form of SE(3) retract
   
2. Pick the matrix entries corresponding to the rotation matrix (== R*retraction_SO(3)), and use https://github.com/Eskilade/orient/blob/master/documentation/conversion_formulas.pdf to go from rotation matrix to angle-axis representation
3. Pick the matrix entries corresponding to the translation vector (== t + Rv)
   
4. Form a 6-entry vector from the entries picked in steps 2. and 3.
5. Compute the jacobian of the 6-entry vector w.r.t. pose components

Does this seem correct? I was just wondering because especially going from the rotation matrix to axis angle seems very complex.

Thanks in advance,

Lauri

[Brice Rebsamen]

Hello

Angular differences are expressed as the so3 tangent vector. note that
if you get the log of a pose3 you will get a 6dof vector with the
rotation part first then the translation part.

why not simply use the existing Pose3 based factors
gtsam::PriorFactor<gtsam::Pose3> ?

If you absolutely want to create your own custom factor for some
reason, then I'd recommend getting the jacobian using the Pose3 method
localCoordinates() that will return the tangent vector corresponding
to the error and the associated jacobian. Or use the expression
variant of the same.

> --
> You received this message because you are subscribed to the Google Groups "gtsam users" group.
> To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
> To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/484984b0-03f9-47d1-8b90-63ca445d813cn%40googlegroups.com.

[Lauri Suomela]

Thanks for the clarification!

Yes, I noticed that the PriorFactor::Pose3 actually achieves what I wanted to do - the naming confused me a bit at the beginning.

Just as an exercise, how would you implement the tutorial example 3.2 using the methods provided by gtsam? In the example, the derivate of the measurement function in exp coordinates is calculated by hand. When I compare the output of these equations (analytical_H) to the output of ' Pose2::ExpmapDerivative ' (w.r.t. q.x, q.y, q.theta) (denoted in the code as numer_H), the outputs don't quite match. Am I correct in assuming that the rows of analytical_H should match the columns of numer_H ? Is this caused by the tutorial example using more drastic simplifications than those used in the actual gtsam implementation?

Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const override {

// The measurement function for a GPS-like measurement h(q) which predicts the measurement (m) is h(q) = q, q = [qx qy qtheta]

// The error is then simply calculated as E(q) = h(q) - m:

// error_x = q.x - mx // error_y = q.y - my

// Node's orientation reflects in the Jacobian, in tangent space this is equal to the right-hand rule rotation matrix

 // H = [ cos(q.theta) -sin(q.theta) 0 ]

//        [ sin(q.theta) cos(q.theta) 0 ]

const Rot2& R = q.rotation();

// This is the derivative from tutorial example 3.2.

 auto analytical_H = (gtsam::Matrix(2, 3) << R.c(), -R.s(), 0.0, R.s(), R.c(), 0.0).finished();

// This is the derivative from the gtsam method

auto numer_H = Pose2::ExpmapDerivative(Vector3(q.x(), q.y(), q.theta()));

cout << "\n------------" << endl;

cout << "Analytical: \n" << analytical_H << endl;

cout << "\n" << endl;

cout << "Numerical: \n" << numer_H << endl;

if (H) (*H) = analytical_H;

return (Vector(2) << q.x() - mx_, q.y() - my_).finished();

 }

​

Output from two iterations of optimization (running this as part of https://github.com/borglab/gtsam/blob/develop/examples/LocalizationExample.cpp):

------------

Analytical:

 0.587440388 -0.809267441 0

 0.809267441 0.587440388 0

 Numerical:

0.858271294   0.437541478   0.161924745

-0.437541478  0.858271294   0.84848915

0                         0                        1

 ------------

Analytical:

0.587490952   -0.809230734    0

0.809230734    0.587490952    0

Numerical:

0.85828924     0.437516844   -0.579822049

-0.437516844  0.85828924      1.30812419

0                         0                         1

​

Thanks,

-Lauri

[Dellaert, Frank]

I unfortunately have no time to sanity check your example below, but I will say the following in general. Derivatives in GTsam might not agree with your intuition, because they are in the local frame, as we update manifold objects with a “retraction” on the right. Hence, what you think is the analytical derivative might not match what GTSam expects. This is why we never allow any derivatives to be checked in unless they are unit tested using numerical derivatives. :-)

Best!

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Lauri Suomela <lauria...@gmail.com>
Sent: Friday, February 3, 2023 4:06:30 AM
To: gtsam users <gtsam...@googlegroups.com>
Subject: Re: [GTSAM] Pose3 factor implementation

 

To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/279e364e-12a3-45a5-80fe-001986c1403cn%40googlegroups.com.

[Lauri Suomela]

Allright, thanks for the hint. I'll keep studying!

Br,

Lauri