Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add LAGO (for Pose2) to python wrapper #899

Merged
merged 10 commits into from
Oct 23, 2021
Merged

Conversation

johnwlambert
Copy link
Contributor

@johnwlambert johnwlambert commented Oct 21, 2021

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

@johnwlambert
Copy link
Contributor Author

johnwlambert commented Oct 21, 2021

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
Copy link
Contributor Author

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

@johnwlambert
Copy link
Contributor Author

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

@varunagrawal
Copy link
Collaborator

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
Copy link
Contributor Author

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
Copy link
Contributor Author

johnwlambert commented Oct 21, 2021

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
Copy link
Collaborator

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
Copy link
Contributor Author

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

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))
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why not just call gtsam.Point3 instead of vector3?

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))
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Same deal here.

Copy link
Collaborator

@varunagrawal varunagrawal left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM modulo 2 comments

@varunagrawal varunagrawal merged commit cb0e62b into develop Oct 23, 2021
@varunagrawal varunagrawal deleted the add-pose2-lago-to-wrapper branch October 23, 2021 02:17
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants