add LAGO (for Pose2) to python wrapper

[johnwlambert]

Add's LAGO (for Pose2 slam without an initial estimate) to the Python wrapper, and ports the C++ example to Python.

[johnwlambert]

Currently there is a clash between LAGO's initialize() and InitializePose3's initialize(), since these two functions have the same name. Resolved by nested namespace.

/home/runner/work/gtsam/gtsam/build/python/slam.cpp:438:112: error: no member named 'initialize' in namespace 'gtsam'; did you mean 'gtsam::InitializePose3::initialize'?
    m_.def("initialize",[](const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess){return gtsam::initialize(graph, initialGuess);}, py::arg("graph"), py::arg("initialGuess"));
                                                                                                               ^~~~~~~~~~~~~~~~~
                                                                                                               gtsam::InitializePose3::initialize
/home/runner/work/gtsam/gtsam/gtsam/slam/InitializePose3.h:88:17: note: 'gtsam::InitializePose3::initialize' declared here
  static Values initialize(const NonlinearFactorGraph& graph,
                ^
/home/runner/work/gtsam/gtsam/build/python/slam.cpp:439:100: error: no member named 'initialize' in namespace 'gtsam'; did you mean 'gtsam::lago::initialize'?
    m_.def("initialize",[](const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath){return gtsam::initialize(graph, useOdometricPath);}, py::arg("graph"), py::arg("useOdometricPath"));
                                                                                                   ^~~~~~~~~~~~~~~~~
                                                                                                   gtsam::lago::initialize
/home/runner/work/gtsam/gtsam/gtsam/slam/lago.h:78:21: note: 'gtsam::lago::initialize' declared here
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
                    ^
[100%] Building CXX object python/CMakeFiles/gtsam_py.dir/sfm.cpp.o
2 errors generated.

[johnwlambert]

@dellaert can we change the name of the initialize() function to avoid the namespace clash? like initializeLago()?

[johnwlambert]

@ProfFan do you think we could get a 2-level namespace like gtsam.lago.initialize() to work in the wrapper?

[varunagrawal]

The LAGO functions are in their own namespace, so you can do this in the wrapper:

#include <gtsam/slam/lago.h>
namespace lago {
  gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess);
  gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath);
}

This will allow you to do gtsam.lago.initialize().

[johnwlambert]

The LAGO functions are in their own namespace, so you can do this in the wrapper:

#include <gtsam/slam/lago.h>
namespace lago {
  gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess);
  gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath);
}

This will allow you to do gtsam.lago.initialize().

Cool, thanks Varun

[johnwlambert]

The LAGO functions are in their own namespace, so you can do this in the wrapper:

#include <gtsam/slam/lago.h>
namespace lago {
  gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess);
  gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath);
}

This will allow you to do gtsam.lago.initialize().

Cool, thanks Varun

@varunagrawal unfortunately it looks like the nested namespace is not working. Any idea why that might be? working now

FAILED test_lago.py::test_lago - AttributeError: module 'gtsam' has no attribute 'lago'

[varunagrawal]

Umm I think you're doing something wrong because I pulled this branch and ran the test and I get this:

===================================================== FAILURES ======================================================
_____________________________________________________ test_lago _____________________________________________________

    def test_lago() -> None:
        """Smokescreen to ensure LAGO can be imported and run on toy data stored in a g2o file."""
        g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt")
    
        graph = gtsam.NonlinearFactorGraph()
        graph, initial = gtsam.readG2o(g2oFile)
    
        # Add prior on the pose having index (key) = 0
        priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
        graph.add(PriorFactorPose2(0, Pose2(), priorModel))
    
>       estimateLago: Values = gtsam.lago.initialize(graph)
E       TypeError: initialize(): incompatible function arguments. The following argument types are supported:
E           1. (graph: gtsam.gtsam.NonlinearFactorGraph, initialGuess: gtsam.gtsam.Values) -> gtsam.gtsam.Values
E           2. (graph: gtsam.gtsam.NonlinearFactorGraph, useOdometricPath: bool) -> gtsam.gtsam.Values
E       
E       Invoked with: NonlinearFactorGraph: size: 6
E       
E       Factor 0: BetweenFactor(0,1)
E         measured:  (0.774115, 1.18339, 1.57617)
E         noise model: unit (3) 
E       
E       Factor 1: BetweenFactor(1,2)
E         measured:  (0.869231, 1.03188, 1.57942)
E         noise model: unit (3) 
E       
E       Factor 2: BetweenFactor(2,3)
E         measured:  (1.35784, 1.03426, 1.56646)
E         noise model: unit (3) 
E       
E       Factor 3: BetweenFactor(2,0)
E         measured:  (0.303492, 1.86501, -3.1139)
E         noise model: unit (3) 
E       
E       Factor 4: BetweenFactor(0,3)
E         measured:  (-0.928526, 0.993695, -1.56354)
E         noise model: unit (3) 
E       
E       Factor 5: PriorFactor on 0
E         prior mean:  (0, 0, 0)
E         noise model: diagonal sigmas[0.001; 0.001; 0.0001];

test_lago.py:33: TypeError
============================================== short test summary info ==============================================
FAILED test_lago.py::test_lago - TypeError: initialize(): incompatible function arguments. The following argument ...
================================================= 1 failed in 0.21s =================================================

This is because you haven't passed in a second argument to initialize but the namespace trick definitely works. 🙂

[johnwlambert]

Hi @varunagrawal @dellaert @ProfFan I believe this is good to go now (all tests passing), if you could take a look

[varunagrawal]

Why not just call gtsam.Point3 instead of vector3?

[varunagrawal]

Same deal here.

[varunagrawal]

LGTM modulo 2 comments