Custom Factors in Python #767
Conversation
gtsam/nonlinear/CustomFactor.cpp
Outdated
| Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H) const { | ||
| if(this->active(x)) { | ||
| if(H) { | ||
| return this->errorFunction(*this, x, H.get_ptr()); |
There was a problem hiding this comment.
Document what the mechanism is by which python can access. I expect you'll talk about pybind, types, how it appears in python, what python function should do.
gtsam/nonlinear/CustomFactor.cpp
Outdated
| return this->errorFunction(*this, x, H.get_ptr()); | ||
| } else { | ||
| JacobianVector dummy; | ||
| return this->errorFunction(*this, x, &dummy); |
There was a problem hiding this comment.
Document what happens in this case.
There was a problem hiding this comment.
Looks good overall, some comments and needs formatting. Awesome job @ProfFan!
|
@ProfFan can you please re-request a review after you make the changes? Overall it looks great but I want to do a thorough review again so I can start using this ASAP. |
|
I am thinking about adding an optional constructor param that allows one to specify the name of the custom factor (stored as a string), so people can distinguish different |
|
I have addressed all comments @dellaert @varunagrawal |
|
Changes:
|
| * Should the factor be evaluated in the same thread as the caller | ||
| * This is to enable factors that has shared states (like the Python GIL lock) | ||
| */ | ||
| virtual bool sendable() const { |
There was a problem hiding this comment.
I don’t understand the name sendable. Since this is a change to the nonlinear factor graph, let’s think about a name that makes sense locally, without the “outside” python context.
There was a problem hiding this comment.
I shamelessly borrowed this concept from Rust (here). Basically it indicates whether an object can be shared with another thread. Maybe shareable is a better name?
|
|
||
| ## Example | ||
|
|
||
| The following is a simple `BetweenFactor` implemented in Python. |
There was a problem hiding this comment.
Either it’s a betweenfactor or it’s not. Pose to compare with still seems to be zero? I thought we had discussed a better, more representative example, but I don’t recall what.
There was a problem hiding this comment.
It's not zero now, if you look at the last unit test.
| @@ -0,0 +1,154 @@ | |||
| import gtsam | |||
| from typing import List, Optional | ||
| from functools import partial | ||
|
|
||
| # Simulate a car for one second |
There was a problem hiding this comment.
Make a function for this that you call below.
| :param this: gtsam.CustomFactor handle | ||
| :param values: gtsam.Values | ||
| :param jacobians: Optional list of Jacobians | ||
| :param measurement: GPS measurement, to be filled with `partial` |
There was a problem hiding this comment.
Can you put measurement first? That makes it like “currying” in functionali languages.
| return error | ||
|
|
||
|
|
||
| for k in range(5): |
There was a problem hiding this comment.
Finally, maybe assign factor to a variable
| for k in range(5): | ||
| factor_graph.add(gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, measurement=np.array([g[k]])))) | ||
|
|
||
| v = gtsam.Values() |
There was a problem hiding this comment.
Remove new line at end and Document block
|
|
||
| result = optimizer.optimize() | ||
|
|
||
| error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) |
| :param this: gtsam.CustomFactor handle | ||
| :param values: gtsam.Values | ||
| :param jacobians: Optional list of Jacobians | ||
| :param measurement: Odometry measurement, to be filled with `partial` |
|
Merged! 🤯 |
Per #748 and #764, this is a factor that allow one to create the evaluateError function in Python.
The unit tests document the usage.
It also contains a fix for the proper binding of
Point3Pairs. @johnwlambert