Values-based FunctorizedFactor (for Python supercharging)

[gchenfc]

Feature

now that wrap supports functionals, could we make a factor in GTSAM similar to FunctorizedFactor but that accepts a functional on a values object rather than with a fixed list of actual types (i.e. in the style of NoiseModelFactor instead of NoiseModelFactorN)?

This would supercharge the python API so that you could write basically any custom factor you want in pure python without any re-compiles (although at the expense of slow runtime of the "linearize" step, if you choose to use this factor)

Example constructor:

  using OptionalJacobians = boost::optional<std::Vector<Matrix>&>;
  FunctorizedFactor(
      KeyVector keys, const SharedNoiseModel &model,
      const std::function<Vector(Values, OptionalJacobians)> unwhitenedError)
      : NoiseModelFactor(model, keys),
        func_(func) {}

Then in python:

def myFactorError(x, H):
  # do calculations
  # populate H
  return z - h(x)
graph.add(FunctorizedFactor([symbol('x', 0), ...], noiseModel, myFactorError)

Motivation

Fully custom factors in pure python - faster prototyping and easier adoption of GTSAM (based on my chats, it seems a lot of people intimidated by GTSAM are mostly intimidated by the C++ custom factors)

Pitch

Alternatives

Additional context

Currently there are only unary and binary versions: FunctorizedFactor1 and FunctorizedFactor2. They're templated which makes it difficult to anticipate in the python wrapper every combination of template arguments a user might want. Much easier IMO to just make a Values-based version to expose to the python wrapper:
FunctorizedFactor(keys, noiseModel, lambda values : return z - h(values))

Should be relatively trivial - almost just a wrapping of NoiseModelFactor

[ProfFan]

OK, I have a working prototype here: https://github.com/ProfFan/python-factor-test

If anyone wants to test it, just build like any other python project.

I'll start upstreaming if there is no objection to the API.

@gchenfc @varunagrawal @dellaert @skydes

[ProfFan]

Example code is in https://github.com/ProfFan/python-factor-test/blob/master/python/tests/test_custom_factor.py

Excerpt:

    def test_jacobian(self):
        """Tests if the factor result matches the GTSAM Pose2 unit test"""

        gT1 = Pose2(1, 2, np.pi/2)
        gT2 = Pose2(-1, 4, np.pi)

        expected = Pose2(2, 2, np.pi/2)

        def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
            # print(f"{this = },\n{v = },\n{len(H) = }")

            key0 = this.keys()[0]
            key1 = this.keys()[1]
            gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
            error = Pose2(0, 0, 0).localCoordinates(gT1.between(gT2))
            
            if len(H) > 0:
                result = gT1.between(gT2)
                H[0] = -result.inverse().AdjointMap()
                H[1] = np.eye(3)
            return error
        
        cf = ge.CustomFactor(gtsam.noiseModel.Unit.Create(3), gtsam.KeyVector([0, 1]), error_func)
        v = Values()
        v.insert(0, gT1)
        v.insert(1, gT2)
        
        bf = gtsam.BetweenFactorPose2(0, 1, Pose2(0, 0, 0), gtsam.noiseModel.Unit.Create(3))

        gf = cf.linearize(v)
        gf_b = bf.linearize(v)

        J_cf, b_cf = gf.jacobian()
        J_bf, b_bf = gf_b.jacobian()
        np.testing.assert_allclose(J_cf, J_bf)
        np.testing.assert_allclose(b_cf, b_bf)

[ProfFan]

Python factor is now in develop, please try and test it hard so we can know what to change :)