Issue with code in section 3.2 of "Factor Graphs and GTSAM"

[Nicolas Perez]

I noticed in section 3.2 of Factor Graphs and GTSAM there is some code for defining the Unary factor that is different from the code in the corresponding C++ example on github.

I am a bit confused as to why the code and the mathematical explanation in section 3.2 of the tutorial is wrong, and why the updated code is correct. I assume the new code is correct, because the pull request was accepted. 

If section 3.2 in the tutorial is incorrect, perhaps someone could update it?

[Frank Dellaert]

Check the tutorial now? I did make some corrections.

[Nicolas Perez]

Hi Frank,

The tutorial is still explaining this part in terms of the old code. See the two equations at the end of section 3.2.

I'm also still confused... Why is the old code that was stated in the tutorial incorrect?

[Frank Dellaert]

You are right :-/ The new code is correct, the explanation is still incorrect. One issue is that the blog post was converted from the PDF and it's a pain to update. But I'll get to it.

[Nicolas Perez]

See the below picture. What am I doing wrong here, w.r.t. calculating the Jacobian? My measurement function is a bit different as well... I include the theta.

associated latex code:

The measurement function is:

$$
\mathbf{h}\left(\left[\begin{array}{l}
q_x\\
q_y\\
q_\theta
\end{array}\right]\right)=\left[\begin{array}{l}
h_{1}(q_x,q_y,q_\theta) \\
h_{2}(q_x,q_y,q_\theta)\\
h_{3}(q_x,q_y,q_\theta)
\end{array}\right]=\left[\begin{array}{c}
q_x\\
q_y\\
q_\theta
\end{array}\right]
$$

So that means the Jacobian is:

$$
\begin{aligned}
&\mathbf{J}=\left[\begin{array}{lll}
\frac{\partial \mathbf{h}}{\partial q_x} & \frac{\partial \mathbf{h}}{\partial q_y} & \frac{\partial \mathbf{h}}{\partial q_\theta}
\end{array}\right]=\left[\begin{array}{ccc}
\frac{\partial h_{1}}{\partial q_x} & \frac{\partial h_{1}}{\partial q_y} & \frac{\partial h_{1}}{\partial q_\theta} \\
\frac{\partial h_{2}}{\partial q_x} & \frac{\partial h_{2}}{\partial q_y} & \frac{\partial h_{2}}{\partial q_\theta}\\
\frac{\partial h_{3}}{\partial q_x} & \frac{\partial h_{3}}{\partial q_y} & \frac{\partial h_{3}}{\partial q_\theta}
\end{array}\right]
\end{aligned}
=\begin{bmatrix}
1 & 0 & 0\\
0 & 1 & 0\\
0 & 0 & 1
\end{bmatrix}$$

What am I doing wrong here? For instance, $\mathbf{J_{0,0}}$ is simply just the derivative of $q_x$ with respect to $x$, which is $1$. $\mathbf{J_{1,0}}$ is the derivative of $q_y$ with respect to $x$, which is $0$. 

[Graham Fletcher]

Nicolas, I would recommend carefully studying the "Reducing the uncertainty about uncertainties, part 2" blog post, especially the section on reference frames. GTSAM uses the right-hand convention, implying that updates should be applied in the robot's local reference frame.

The unary factor in the tutorial mimics a GPS, which measures in the world frame, by definition. It's therefore necessary to rotate the GPS measurement into the local frame in order to evaluate the error for the factor. The old code did not do this, but essentially applied an update in the world frame, yielding the identity matrix for the Jacobian.

Graham

[Dellaert, Frank]

Look in math.pdf and let unit tests be your guide 😊

 

 

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Nicolas Perez <nicolasa...@gmail.com>
Date: Friday, March 4, 2022 at 3:57 PM
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] Re: Issue with code in section 3.2 of "Factor Graphs and GTSAM"

See the below picture. What am I doing wrong here, w.r.t. calculating the Jacobian? My measurement function is a bit different as well... I include the theta.

--
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/4b2af39e-5158-4a32-99c3-a79fe7ace79en%40googlegroups.com.

[Nicolas Perez]

Graham: I read most of the blog post you mentioned. It is very helpful thanks. I am still a bit confused though: why do we have to rotate the measurement from the world frame into the body frame? Shouldn't the measurement be translated into the body frame? 

Frank: thanks for the reference. I think you use the 'derivative of x' notation incorrectly at the beginning of section 2.1 (in definition 1) as well as in the sentence leading up to equation 1.3. Not sure... The math.pdf document seems more like lecture notes to me as opposed to a thorough explanation of topics. The post Graham mentioned to me seems more useful for the topic at hand.

I'd suggest explaining in the GTSAM tutorial what the 'H' parameter in the EvaluateError function is supposed to be. It seems it is not just a Jacobian, but also a transformation that bring measurements into the body frame. Being able to create custom factors is one of the most important parts of GTSAM. 

[Frank Dellaert]

This is a great suggestion, and I fixed the docs in PR #1139 and also copied the explanation to

gtsam.org/tutorials/intro.html#magicparlabel-65484

Please let me know if you see any bugs in the text or code. I already saw one small typo,  which I will fix before I merge.

The python custom factor example is still wrong, and I'm counting on its author (Fan!) to fix it.

Best, and thanks for helping to make GTSAM better!

Frank

[Nicolas Perez]

Hi Frank,

Thanks for the ongoing help. I really appreciate it. I am also happy to help improve GTSAM.

Here are some edits: 

you define the variable 'q' in two different ways. In one instance you define it as a vector of dimension 3, in the other instance you define it as a rigid body transformation matrix.

you also use 2 different notations for the exponential map. In one instance you use just e, in another you use exp.

also, you should add a reference for further reading on how Jacobians are defined for manifolds and exponential maps, etc... They are not something that is as widely known (from my experience anyways) thus making the math in that section hard to follow. Unless there is further explanation in the tutorial document or a reference to further reading, it will be very difficult for someone unfamiliar with manifolds to understand. You should also add the exponential map for SE(3) in the tutorial somewhere so someone will know how to define a unary factor for poses in 3D.

I found one reference that might be useful to add in the tutorial: https://arxiv.org/pdf/1812.01537.pdf (but it seems to focus too much on Lie groups, which I remember reading somewhere is not that relevant to GTSAM)

An additional comment:

I noticed in this document unary factors are not explained (both other types of factors are). There is quite a bit of documentation outside of the GTSAM tutorial. I'd additionally suggest adding a small section in the tutorial with a sort 'mind map' of extra materials to look at. That is, a list of further readings along with how they relate to the GTSAM tutorial.

[Nicolas Perez]

oh, and in case it wasn't clear... my previous message was about edits to the text you wrote (not the code).

[Dellaert, Frank]

Thanks for the comments. I'll add some references, but for now:

https://github.com/borglab/gtsam/blob/develop/doc/LieGroups.pdf

https://github.com/borglab/gtsam/blob/develop/doc/math.pdf

These are more notes done fully fledged tutorials, but maybe they are useful.

PS, Lie groups are very relevant in GTSAM. In fact, initially we had the wrong idea that every unknown variable needed to be in a Lie Group, and most of our examples are in fact using Lie groups. However, in some cases you want to go beyond Lie groups to a looser concept, manifolds. In manifolds we don't always have an exponential map, but we have a retraction that plays the same role. Some of this is explained here:

https://gtsam.org/notes/GTSAM-Concepts.html

I acknowledge our documentation is a bit scattered, and I am still looking for volunteers (after several aborted attempts) to unify all documentation in a "readthedocs" site.

FD

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Nicolas Perez <nicolasa...@gmail.com>

Sent: Sunday, March 20, 2022 15:08
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] Re: Issue with code in section 3.2 of "Factor Graphs and GTSAM"

 

--
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/9913be26-7a47-42aa-81d2-3c5f4db05270n%40googlegroups.com.

[Nicolas Perez]

Ok sounds good.

I found this reference which might be useful to mention in the GTSAM docs. It discusses Lie algebra and Lie group.