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.

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