Regression in Logmap (v4.2a7)

[guillaume-jcb]

Hi all,

As discussed in https://groups.google.com/g/gtsam-users/c/7dEKY53G4Jk:

After an upgrade from version 4.0.3 to 4.2a7, we've noticed a strange behavior in our unit tests which we were able to track down to back and forth conversions between AngleAxis and Rotation representations for rotations.

According to @ProfFan, this is a side-effect of #887.

Description

There is a precision error in Logmap for rotation angles close to PI. Furthermore, the conversion error exhibits a strong discontinuity.

Steps to reproduce

1. Generate random test data in the form of 3D vectors with a norm close to PI.
2. Apply Logmap / Expmap to each vector.
3. Compare the vectors before / after this transformation − using the same metric as Eigen::DenseBase::isApprox().

C++ code:

#include <gtsam/geometry/Rot3.h>
#include <fstream>
#include <iomanip>
#include <random>

template <typename Matrix>
double err(const Eigen::PlainObjectBase<Matrix>& v, const Eigen::PlainObjectBase<Matrix>& w)
{
  // Distance used in Eigen::DenseBase::isApprox(), see
  // https://eigen.tuxfamily.org/dox/classEigen_1_1DenseBase.html#ae8443357b808cd393be1b51974213f9c
  return (v - w).norm() / std::min(v.norm(), w.norm());
}

int main()
{
  std::srand(0);
  std::mt19937 gen(0);
  std::uniform_real_distribution<> dis(M_PI - 0.1, M_PI);
  
  std::ofstream ofs("val.txt");
  ofs << std::setprecision(10) << "norm(v),err\n";
  
  for (auto i = 0; i < 20000; ++i) {
    // Generate a 3D vector with a norm close to PI (in [PI - 0.1, PI))
    const gtsam::Vector3 v = dis(gen) * gtsam::Vector3::Random().normalized();
    const auto rot = gtsam::Rot3::Expmap(v);
    const auto v_calc = gtsam::Rot3::Logmap(rot);
    ofs << v.norm() << "," << err(v, v_calc) << "\n";
  }
}

Python script to generate the graphs:

import numpy as np
import matplotlib.pyplot as plt

FONTSIZE = 18
plt.rc('font', size=FONTSIZE)  # controls default text size
plt.rc('axes', titlesize=FONTSIZE, labelsize=FONTSIZE)  # fontsize of the title, x and y labels
plt.rc('xtick', labelsize=FONTSIZE)  # fontsize of the x tick labels
plt.rc('ytick', labelsize=FONTSIZE)  # fontsize of the y tick labels

data = np.loadtxt("val.txt", delimiter=',', skiprows=1)
plt.semilogy(data[:, 0], data[:, 1], '.')
plt.xlabel("$||v||$ (rotation angle)")
plt.ylabel("$err = \\frac{||v - v_{calc}||}{\min(||v||, ||v_{calc}||)}$")
plt.axvline(np.pi, linestyle='--', color='black', label="$\\pi$")
plt.legend()
plt.title(f"GTSAM 4.2a7")
plt.show()

Results

Expected behavior

The conversion error should remain reasonably low, i.e. not dramatically higher than in version 4.0.3. In the worst case it jumped by 7 orders of magnitude, from 1e-12 to 1e-5.

Environment

GTSAM 4.2a7, C++, Linux Fedora 36.

Additional information

Reducing the threshold value on the trace of the rotation matrix used to switch between the generic formula and the Taylor series reduces the high discontinuity in the conversion error and brings it down to a level similar to that of GTSAM 4.0.3.

gtsam/gtsam/geometry/SO3.cpp

Line 264 in 30412b8

if (tr + 1.0 < 1e-3) {

Results for various threshold values (cf. graph title):

[ProfFan]

Thanks! Judging from this data it seems 1e-8 is a better choice, however this may reduce the precision when detR is slightly off 1. The unit test for this case is also in #887, so if 1e-8 can pass that test I would say it's safe to change. @lpea

[guillaume-jcb]

HI @ProfFan, I've been quite busy these last days but will try to have a look at the test you mentioned soon with a threshold value of 1e-8.

Speaking of tests, it could be worth adding one to check this error introduced by applying Logmap-Expmap near the singularity and spot this kind of discrepancy if the approximation ever changes in the future.

Edit: had the time to run the AxisAngle2 test, it works well with 1e-5 but fails with 1e-6.

[ProfFan]

Yeah, we may need to think about this more closely... Numeric behavior is VERY tricky. One of the ways I can think of for doing this is introducing a slow path where we just do the full SVD when detecting a rotation that is not detA = 1. However this will give extra computation burden.

Actually, I wonder if it is solvable by just adding more Taylor terms. @lpea thoughts?

[guillaume-jcb]

Yep, I tried with

       // const double mag = M_PI - (2 * sgn_w * W) / norm;
      const double mag = M_PI - (2 * sgn_w * W) / norm + 0.5 * M_PI * W*W / (norm*norm) - 4 * sgn_w * W*W*W / (3 * norm*norm*norm);

and also replace

      // const double Qx = 2.0 + 2.0 * Rxx;
      const double Qx = 1 + 2 * Rxx - tr;

but it did not seem to change much... Could you double check that the maths is correct? BTW, could you share an updated version of the paper you mentioned in #892?

[ProfFan]

AxisAngleConversion.pdf

[ProfFan]

Yep, I tried with

       // const double mag = M_PI - (2 * sgn_w * W) / norm;
      const double mag = M_PI - (2 * sgn_w * W) / norm + 0.5 * M_PI * W*W / (norm*norm) - 4 * sgn_w * W*W*W / (3 * norm*norm*norm);

and also replace

      // const double Qx = 2.0 + 2.0 * Rxx;
      const double Qx = 1 + 2 * Rxx - tr;

but it did not seem to change much... Could you double check that the maths is correct? BTW, could you share an updated version of the paper you mentioned in #892?

Sorry for the late response! Was busy for other stuff. I think one can try to cross-check the approximated quaternion with the actual quaternion. I currently do not have bandwidth for re-checking the math, but I will be able to answer your questions against what I've written in the document.

[ProfFan]

@lpea wonder if it is numerically negatively impacting your application?

[ProfFan]

I can take a few hours to look at it immediately if that is the case

[guillaume-jcb]

@ProfFan It does not seem to be troublesome as far as I'm concerned.

[ProfFan]

Thanks @lpea , I'll take a look later then

[dellaert]

@ProfFan re-visiting this so we can close. Introducing a strong discontinuity where there was none before definitely needs re-checking the math :-)

[OznOg]

Is there a patch planed on this? would if (tr + 1.0 < 1e-8) { be a correct fix that could be merged?