Shonan key bug

[johnwlambert]

Suppose 4 images are captured by a camera rig at 4 separate poses, wTi0, wTi1, wTi2, wTi3.

Suppose relative rotations are estimated between poses (1,2), (2,3), (1,3), but pose 0 is orphaned. If keys are not ordered consecutively, from 0,...,N-1, then Shonan will throw the following error:

Traceback (most recent call last):
  File "shonan_bug.py", line 58, in <module>
    main()
  File "shonan_bug.py", line 51, in main
    obj = ShonanAveraging3(between_factors, shonan_params)
ValueError: As of now, ShonanAveraging expects keys numbered 0..N-1

However, I think arbitrary ordering should be allowed, in keeping with how keys can be arbitrary numbers throughout GTSAM:

Here is a minimal example to reproduce it:

import gtsam
import numpy as np
from gtsam import (
    BetweenFactorPose3,
    LevenbergMarquardtParams,
    Rot3,
    Pose3,
    ShonanAveraging3,
    ShonanAveragingParameters3,
)

def main():
    """ """
    p_min = 5
    p_max = 30

    num_images = 4

    wTi0 = Pose3(Rot3(), np.array([1,1,0]))
    wTi1 = Pose3(Rot3(), np.array([3,1,0]))
    wTi2 = Pose3(Rot3(), np.array([3,3,0]))
    wTi3 = Pose3(Rot3(), np.array([1,3,0]))

    # assume pose 0 is orphaned in the visibility graph

    # generate i2Ri1 rotations
    # (i1,i2) -> i2Ri1
    i2Ri1_dict = {
        (1,2): wTi2.between(wTi1).rotation(),
        (2,3): wTi3.between(wTi2).rotation(),
        (1,3): wTi3.between(wTi1).rotation()
    }

    lm_params = LevenbergMarquardtParams.CeresDefaults()
    shonan_params = ShonanAveragingParameters3(lm_params)
    shonan_params.setUseHuber(False)
    shonan_params.setCertifyOptimality(True)

    noise_model = gtsam.noiseModel.Unit.Create(6)

    between_factors = gtsam.BetweenFactorPose3s()

    for (i1, i2), i2Ri1 in i2Ri1_dict.items():

        # ignore translation during rotation averaging
        i2Ti1 = Pose3(i2Ri1, np.zeros(3))
        between_factors.append(BetweenFactorPose3(i2, i1, i2Ti1, noise_model))

    obj = ShonanAveraging3(between_factors, shonan_params)

    initial = obj.initializeRandomly()
    result_values, _ = obj.run(initial, p_min, p_max)

if __name__ == '__main__':
    main()

This is because of the following logic in ShonanAveraging.cpp, which loops over each BinaryMeasurement, and accumulates

keys = { 1, 2, 3 }
maxKey = 3

The problem is that keys.size() = N = 3, but maxKey is also 3, and maxKey != N - 1.

/* ************************************************************************* */
// Calculate number of unknown rotations referenced by measurements
// Throws exception of keys are not numbered as expected (may change in future).
template <size_t d>
static size_t NrUnknowns(
    const typename ShonanAveraging<d>::Measurements &measurements) {
  Key maxKey = 0;
  std::set<Key> keys;
  for (const auto &measurement : measurements) {
    for (const Key &key : measurement.keys()) {
      maxKey = std::max(key, maxKey);
      keys.insert(key);
    }
  }
  size_t N = keys.size();
  if (maxKey != N - 1) {
    throw std::invalid_argument(
        "As of now, ShonanAveraging expects keys numbered 0..N-1");
  }
  return N;
}

[dellaert]

Unfortunately, @johnwlambert, I think this assumption goes deeper than just this piece of code, which just checks the assumption. Several other functions that construct matrices related to the convergence certificate also use the fact that the graph is connected and keys are 0..N-1. There are about 10 places where nrUnknowns is used to dimension a sparse or dense matrix. I'm not saying it can't be solved, but it's not a quick fix, rather a major philosophical overhaul.

[johnwlambert]

Yeah this is what I was discovering also as I looked through the other Shonan code in Shonan.cpp.

@dellaert I don't see a way to do this without a re-mapping of keys to zero-indexed consecutive variables (e.g. relating to matrix rows/cols) either on the GTSAM end, or on the GTSFM end, to remove this assumption.

I assume we are doing that elsewhere in GTSAM for arbitrarily-named keys in the factor graph, when forming a sparse system. Is there something we could take from there?