Cannot construct `JacobianFactor` in Python

[gchenfc]

Description

I believe something is wrong with constructing JacobianFactors from the result of a linearize() operation when using the python wrapper. Tagging @varunagrawal and @ProfFan due to the fact this is python related.

Specifically, the RTTI dynamic casts used in JacobianFactor constructors do not seem to be working properly. Here are two example usages:

https://github.com/borglab/gtsam/blob/ff7ddf48bd8bb2f993437f01cd32a4727d6f023b/gtsam/linear/JacobianFactor.cpp#L58-L67

https://github.com/borglab/gtsam/blob/ff7ddf48bd8bb2f993437f01cd32a4727d6f023b/gtsam/linear/JacobianFactor.cpp#L196-L211

For whatever reason, nonlinear factors constructed in python, which are then called with .linearize() to create a JacobianFactor, cannot be passed into the JacobianFactor(factor) constructors because boost::dynamic_pointer_cast<JacobianFactor>(factor) returns a null pointer.

Steps to reproduce

This example python script fails:

import gtsam
import gtdynamics as gtd

def main():
    f = gtd.MinTorqueFactor(0, gtsam.noiseModel.Unit.Create(1))

    init = gtsam.Values()
    init.insert(0, 0.0)

    flin = f.linearize(init)
    flin.print("Linearized Factor: ")
    print("\nLinearized factor has type: ", type(flin), '\n')
    # output: Linearized factor has type:  <class 'gtsam.gtsam.JacobianFactor'>

    gtsam.JacobianFactor(flin)  # this throws an error

if __name__ == '__main__':
    main()

Which throws the error:

In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor

Expected behavior

The python result should match that of this example C++ script succeeds:

#include <gtdynamics/factors/MinTorqueFactor.h>

#include <gtsam/nonlinear/NonlinearFactorGraph.h>

#include <iostream>

using namespace gtsam;
using namespace gtdynamics;

int main() {
  auto f = MinTorqueFactor(0, noiseModel::Unit::Create(1));

  auto init = Values();
  init.insert(0, 0.0);

  auto flin = f.linearize(init);
  flin->print("Linearized Factor: ");
  std::cout << "\nLinearized factor has type: " << typeid(*flin).name() << std::endl<< std::endl;
  // Output: Linearized factor has type: N5gtsam14JacobianFactorE

  auto jf = JacobianFactor(*flin);
}

Environment

MacOS 11.3.1, MacBook Air (M1, 2020),
AppleClang 12.0.5.12050022
gtsam hash b99bf4e92912f4ad0020f79d6b222a3d6593514f

I also experienced similar errors when I ran on an ubuntu intel lab machine, but I didn't rigorously test these particular test files.

Additional information

This only occurs for nonlinear factors defined outside of the GTSAM repo. So, for example, PriorFactor, BetweenFactor, etc all still work as expected.

This causes noticeable failures on optimizations with Constrained noise models, may be causing silent correctness errors on other optimizations, and is likely causing all non-GTSAM factors to be calling the HessianFactor constructor instead of the JacobianFactor constructor (performance penalty).

[ProfFan]

I hate C++ RTTI, we should adopt a LLVM-styled RTTI some day...

*No capacity to work on this for now, need to get ICRA ddl done

[gchenfc]

Temporarily using this workaround to use name-based type matching:

JacobianFactor::JacobianFactor(const GaussianFactor& gf) {
  // Copy the matrix data depending on what type of factor we're copying from
  if (!strcmp(typeid(gf).name(), typeid(JacobianFactor).name()))
    *this = JacobianFactor(*static_cast<const JacobianFactor*>(&gf));
  else if (!strcmp(typeid(gf).name(), typeid(HessianFactor).name()))
    *this = JacobianFactor(*static_cast<const HessianFactor*>(&gf));
  else
    throw std::invalid_argument(
        "In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
}

And do a similar thing in _convertOrCastToJacobians. See borglab/gtsam@bb39346

Note: for constrained optimization, you still need to manually set the linearSolverType param to QR - for some reason GTSAM is also not properly recognizing that the constrained noise models and auto-switching to QR. e.g.

params = gtsam.LevenbergMarquardtParams()
params.setLinearSolverType("MULTIFRONTAL_QR")
result = gtsam.LevenbergMarquardtOptimizer(graph, init, params).optimize()

I'm leaving this open because it needs to eventually be fixed, but this workaround works for now.

[varunagrawal]

Yeah we'll figure it out after ICRA :)

[gchenfc]

Just checking, does anyone else get this error? For me just compiling everything standard and running make python-test in gtdynamics produces this issue in the Simulator. Alternatively, minimum failing example (copy-pasted from above):

import gtsam
import gtdynamics as gtd

def main():
    f = gtd.MinTorqueFactor(0, gtsam.noiseModel.Unit.Create(1))

    init = gtsam.Values()
    init.insert(0, 0.0)

    flin = f.linearize(init)
    flin.print("Linearized Factor: ")
    print("\nLinearized factor has type: ", type(flin), '\n')
    # output: Linearized factor has type:  <class 'gtsam.gtsam.JacobianFactor'>

    gtsam.JacobianFactor(flin)  # this throws an error

if __name__ == '__main__':
    main()

@varunagrawal @yetongumich @ProfFan

[ProfFan]

I think I already fixed this in GTSAM, maybe the GTD wrapper does not have such changes applied.

Search for RTTI in the GTSAM Issues

[gchenfc]

Are you referring to borglab/gtsam#218 ?

If so, it looks like -fvisibility-ms-compat flag is already set inside function pybind_wrap

But this definitely sounds like the right issue (Mac dynamic linking related)

[ProfFan]

AFAIK I am suspecting that we have some violation of ODR such that multiple copies of the same class is generated.
Have no proof, the only way to be sure is adding prints in the JacobianFactor constructor and print out the typeinfo struct.

[gchenfc]

If that were the case then wouldn't this be affecting all systems and not just mac?