add failing unit test on axisAngle for Rot3 in c++ #887

johnwlambert · 2021-10-07T19:55:01Z

Adds tests to expose broken axis-angle for edge cases. See #886 for more details.

ProfFan · 2021-10-07T20:26:11Z

Wait, why CI is successful if the unit tests fail?

johnwlambert · 2021-10-08T02:10:33Z

@ProfFan I'm pleased to say the new python unit test now successfully fails : - )

For some reason, I don't see the output of any c++ unit tests in the CI. Are we still running those in CI?

======================================================================
FAIL: test_axisangle (test_Rot3.TestRot3)
Test .axisAngle() method.
----------------------------------------------------------------------
Traceback (most recent call last):
  File "/home/runner/work/gtsam/gtsam/python/gtsam/tests/test_Rot3.py", line 37, in test_axisangle
    np.testing.assert_almost_equal(actual_angle, expected_angle, 1e-7)
  File "/usr/local/lib/python3.6/dist-packages/numpy/testing/_private/utils.py", line 595, in assert_almost_equal
    raise AssertionError(_build_err_msg())
AssertionError: 
Arrays are not almost equal to 0 decimals
 ACTUAL: 6.075185318083525
 DESIRED: 3.1396582

----------------------------------------------------------------------
Ran 126 tests in 1.843s

FAILED (failures=1, skipped=3)

ProfFan · 2021-10-08T02:12:41Z

Oh dang. It seems that we accidentally disabled all C++ tests? @varunagrawal

varunagrawal · 2021-10-08T07:24:18Z

I am seeing the C++ CI no problem. Am I missing something?

johnwlambert · 2021-10-08T14:54:01Z

I am seeing the C++ CI no problem. Am I missing something?

@varunagrawal could you share a screenshot of the c++ unit test summary you see being run in the CI (from the logs)?

varunagrawal · 2021-10-08T15:44:07Z

I misunderstood what the issue is. This is my fault, I updated the CI to be faster and made a small mistake...

ProfFan · 2021-10-08T18:26:13Z

@johnwlambert Could you merge develop?

dellaert · 2021-10-08T18:30:30Z

I think Fan means merge develop into this branch. We can’t merge TO develop until issue is fixed.

…ailing-axis-angle-test-c++

This reverts commit f819b1a.

This reverts commit 21279e6.

gtsam/geometry/SO3.cpp

ProfFan · 2021-10-13T00:48:28Z

Robust Conversion from Matrix to Axis Angle Form

1 Introduction

Robust conversion from matrix to axis-angle form is not trivial because the
transform is plagued with numeric instabilities. In this document we will
review the normal form of such a conversion, and provide detailed instructions
on how to properly do it.

1.1 Basic Conversion

It is known that the conversion from a $\operatorname{SO} (3)$ matrix can be
written as

Theorem 1: The rotation angle $\theta$ can be calculated as

$\theta = \arccos \left( \frac{\operatorname{tr} (R) \- 1}{2} \right) \label{eq-orig-angle}\tag{1}$

where $\operatorname{tr} (R)$ is the trace of the rotation matrix.

As such, the normalized rotation axis is:

Theorem 2: The normalized rotation axis for a rotation matrix $R$ is

$\omega = \frac{\theta}{2 \sin (\theta)} \left(\begin{array}{c} R\_{3, 2} \- R\_{2, 3}\\\\\\ R\_{1, 3} \- R\_{3, 1}\\\\\\ R\_{2, 1} \- R\_{1, 2} \end{array}\right) \label{eq-orig-axis}\tag{2}$

where $R\_{i, j}$ are the elements in the matrix.

2 Problem

sin(x)02*piCannot process embedded image

Figure 1.$\sin (x)$

It's pretty straightforward that when $\theta \rightarrow 0 + k \pi$ ,
(undefined label: 'eq-orig-axis') will not work because $\sin
(\theta)$ will be 0. From (undefined label: 'eq-orig-angle')
we know that, since $\theta = k \pi$ ,

$\begin{eqnarray*} \arccos \left( \frac{\operatorname{tr} (R) \- 1}{2} \right) & = & k \pi\\\\\\ \frac{1}{2} (\operatorname{tr} (R) \- 1) & = & \pm 1\\\\\\ \operatorname{tr} (R) & = & 1 \pm 2 \tag{3} \end{eqnarray*}$

Thus we know that there are two singular points, $- 1$ and $3$, for the
axis-angle conversion. Next thing is how to deal with these two points.

3 Properties of the $\operatorname{SO} (3)$ transform at the singular points

3.1 $\theta \rightarrow 0$

For $\theta = 0 (\operatorname{tr} (R) = 3)$ the solution is simple: We simply
take the Taylor expansion of

\[ \frac{\theta}{2 \sin (\theta)} \]

at $\theta \rightarrow 0$ . Note this is not a simple Taylor expansion, but a
limit. We can do this in Maxima:

maximadefault

Maxima 5.45.1 https://maxima.sourceforge.io

using Lisp SBCL 2.1.9

Distributed under the GNU Public License. See the file COPYING.

Dedicated to the memory of William Schelter.

The function bug_report() provides bug reporting information.

(%i1)θ:acos(t-12) $\arccos \left( \frac{t \- 1}{2} \right)$

(%i2)mag:θ2*sin(θ) $\frac{\arccos \left( \frac{t \- 1}{2} \right)}{2 \sqrt{1 \- \frac{(t \- 1)^2}{4}}}$

(%i3)taylor(mag,t,3,3)

$\frac{1}{2} \- \frac{t \- 3}{12} + \frac{(t \- 3)^2}{60} \- \frac{(t \- 3)^3}{280} + \cdots$

Thus the solution around $\theta \rightarrow 0$ is

$\frac{1}{2} \- \frac{t \- 3}{12} + \frac{(t \- 3)^2}{60} \tag{4}$

3.2 $\theta \rightarrow \pi$

The solution becomes a lot more complicated when at $\theta \rightarrow \pi$ .
This is because at this extreme point you have two solutions: you can go from
both left and right of the unit circle!

Note 3: The trace $\operatorname{tr} (R) = \- 1$ is negative.

In this case we can first convert $R$ to quaternion, which is done by

$(w, v) = \left(\begin{array}{c} \frac{1}{2 r} (R\_{c, b} \- R\_{b, c})\\\\\\ \frac{1}{2} r\\\\\\ \frac{1}{2 r} (R\_{a, b} + R\_{b, a})\\\\\\ \frac{1}{2 r} (R\_{c, a} + R\_{a, c}) \end{array}\right) \tag{5}$

where $r := \sqrt{1 + R\_{a, a} \- R\_{b, b} \- R\_{c, c}}$ , $a := \arg \max \_{i \in \lbrace 1, 2, 3 \rbrace} R\_{i, i}$ , $b := (a + 1) \operatorname{mod}3$, $c := (a + 2) \operatorname{mod}3$ .

Normally we want the angle $\theta \in [0, \pi]$ , thus we need to make sure
that $w > 0$ ,

and now we can easily get the angle $\theta$ as

$2\operatorname{atan}2 \left( \sqrt{q\_{x}^2 + q\_{y}^2 + q\_{z}^2}, w \right) \tag{6}$

and now the magnitude of the vector is

$| v | := \frac{\theta}{\sin (\theta / 2)} \tag{7}$

However, in GTSAM, we do the following simplification to avoid doing the atan2
(expensive).

First note that $q\_{x}^2 + q\_{y}^2 + q\_{z}^2 + w^2 = 1$ . Thus we have
(around $w = 0$) this Taylor expansion:

maximadefault

Maxima 5.45.1 https://maxima.sourceforge.io

using Lisp SBCL 2.1.9

Distributed under the GNU Public License. See the file COPYING.

Dedicated to the memory of William Schelter.

The function bug_report() provides bug reporting information.

(%i1)θ:2*atan2(1-w,w) $\- 2 \textrm{atan2} (w \- 1, w)$

(%i2)scale:θ/sin(θ/2) $\frac{2 \textrm{atan2} (w \- 1, w) \sqrt{w^2 + (w \- 1)^2}}{w \- 1}$

(%i3)taylor(scale,w,0,3)

$\pi \- 2 w + \frac{(\pi \- 4) w^2}{2} + \frac{(3 \pi \- 7) w^3}{3} + \cdots$

which indicates that we only need a correction term of $\pi \- 2 w$ to get the
true magnitude near $\pi$.

ProfFan · 2021-10-13T00:50:31Z

AxisAngleConversion.pdf

dellaert

Fan, John, i think we should merge even in absence of the revised PDF from Fan (which I asked for in a meeting) because this is a bug that needs to be squashed. However, I’m asking Fan to fix the logic for argmax, after which I will take another look. email me when it’s done.

dellaert · 2021-10-13T16:59:11Z

gtsam/geometry/SO3.cpp

+  if (tr + 1.0 < 1e-4) {
+    std::vector<double> diags = {R11, R22, R33};
+    size_t max_elem = std::distance(diags.begin(), std::max_element(diags.begin(), diags.end()));
+    if (max_elem == 2) {


I’d prefer if we do

If (R11>R22 && R11>R33) Else if (R22>R33) Else

dellaert

@johnwlambert could you merge after running your python tests again and pasting the results in the conversation? I remember you had a script that did a statistical analysis as to how many times it failed in 100K calls or so?

johnwlambert · 2021-10-13T22:55:31Z

@dellaert @ProfFan just ran some analysis. The results are definitely much better than before now. We still get several dozen cases where the angle exceeds 180 degrees from those 165 previous failures. The failures are now on the order of 180.1 to 180.5 degrees, vs. expected 179.x.

Do we want to continue to try to get the last decimal place? I can add these test cases to the unit tests, if so.

ProfFan · 2021-10-13T23:03:54Z

@johnwlambert Let's land this first. Could you put up another PR that includes the case that fails?

johnwlambert · 2021-10-13T23:39:55Z

@johnwlambert Let's land this first. Could you put up another PR that includes the case that fails?

Sure, thanks for merging

add failing unit test on axisAngle for Rot3 in c++

7083de3

johnwlambert added 3 commits October 7, 2021 17:20

add python unit test for Rot3

bb87dcf

fix typo

0b0897d

fix assert

225ac77

Merge branch 'develop' of https://github.com/borglab/gtsam into add-f…

e7af99c

…ailing-axis-angle-test-c++

varunagrawal assigned johnwlambert Oct 8, 2021

ProfFan added 3 commits October 10, 2021 16:20

Revert "Revert "replace deprecated tbb functionality""

21279e6

This reverts commit f819b1a.

Fixed numerical problems near +-pi

1a97b14

Revert "Revert "Revert "replace deprecated tbb functionality"""

dae4093

This reverts commit 21279e6.

ProfFan self-assigned this Oct 12, 2021

ProfFan requested review from dellaert and varunagrawal October 12, 2021 23:10

johnwlambert commented Oct 12, 2021

View reviewed changes

gtsam/geometry/SO3.cpp Show resolved Hide resolved

Cleanup equation

aa6b432

varunagrawal linked an issue Oct 13, 2021 that may be closed by this pull request

Buggy axis-angle computation in GTSAM #886

Closed

dellaert requested changes Oct 13, 2021

View reviewed changes

ProfFan added 2 commits October 13, 2021 13:46

simplify logic of biggest diagonal

35061ca

Fix typo

48cd979

dellaert approved these changes Oct 13, 2021

View reviewed changes

ProfFan merged commit d314fda into develop Oct 13, 2021

ProfFan deleted the add-failing-axis-angle-test-c++ branch October 13, 2021 23:22

lpea mentioned this pull request Jul 4, 2022

Regression in Logmap (v4.2a7) #1233

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

add failing unit test on axisAngle for Rot3 in c++ #887

add failing unit test on axisAngle for Rot3 in c++ #887

johnwlambert commented Oct 7, 2021 •

edited

ProfFan commented Oct 7, 2021

johnwlambert commented Oct 8, 2021

ProfFan commented Oct 8, 2021

varunagrawal commented Oct 8, 2021

johnwlambert commented Oct 8, 2021

varunagrawal commented Oct 8, 2021

ProfFan commented Oct 8, 2021

dellaert commented Oct 8, 2021

ProfFan commented Oct 13, 2021

ProfFan commented Oct 13, 2021

dellaert left a comment

dellaert Oct 13, 2021

ProfFan Oct 13, 2021

dellaert left a comment

johnwlambert commented Oct 13, 2021

ProfFan commented Oct 13, 2021

johnwlambert commented Oct 13, 2021

add failing unit test on axisAngle for Rot3 in c++ #887

add failing unit test on axisAngle for Rot3 in c++ #887

Conversation

johnwlambert commented Oct 7, 2021 • edited

ProfFan commented Oct 7, 2021

johnwlambert commented Oct 8, 2021

ProfFan commented Oct 8, 2021

varunagrawal commented Oct 8, 2021

johnwlambert commented Oct 8, 2021

varunagrawal commented Oct 8, 2021

ProfFan commented Oct 8, 2021

dellaert commented Oct 8, 2021

ProfFan commented Oct 13, 2021

1 Introduction

1.1 Basic Conversion

2 Problem

3 Properties of the $\operatorname{SO} (3)$ transform at the singular points

3.1

3.2 $\theta \rightarrow \pi$

ProfFan commented Oct 13, 2021

dellaert left a comment

Choose a reason for hiding this comment

dellaert Oct 13, 2021

Choose a reason for hiding this comment

ProfFan Oct 13, 2021

Choose a reason for hiding this comment

dellaert left a comment

Choose a reason for hiding this comment

johnwlambert commented Oct 13, 2021

ProfFan commented Oct 13, 2021

johnwlambert commented Oct 13, 2021

johnwlambert commented Oct 7, 2021 •

edited

3.1 $\theta \rightarrow 0$