12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #767 from borglab/feature/custom_factor 

Custom Factors in Python
release/4.3a0
Fan Jiang 2021-06-07 14:49:24 -04:00 committed by GitHub
parent 5c9e4136b7 1ebf675201
commit b1e91671fd
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
11 changed files with 763 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/gtsam.i
View File

@ -2166,6 +2166,34 @@ virtual class NoiseModelFactor: gtsam::NonlinearFactor {
Vector whitenedError(const gtsam::Values& x) const;
};
#include <gtsam/nonlinear/CustomFactor.h>
virtual class CustomFactor: gtsam::NoiseModelFactor {
/*
* Note CustomFactor will not be wrapped for MATLAB, as there is no supporting machinery there.
* This is achieved by adding `gtsam::CustomFactor` to the ignore list in `matlab/CMakeLists.txt`.
*/
CustomFactor();
/*
* Example:
* ```
* def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
* <calculated error>
* if not H is None:
* <calculate the Jacobian>
* H[0] = J1 # 2-d numpy array for a Jacobian block
* H[1] = J2
* ...
* return error # 1-d numpy array
*
* cf = CustomFactor(noise_model, keys, error_func)
* ```
*/
CustomFactor(const gtsam::SharedNoiseModel& noiseModel, const gtsam::KeyVector& keys,
const gtsam::CustomErrorFunction& errorFunction);
void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
};
#include <gtsam/nonlinear/Values.h>
class Values {
Values();

76
gtsam/nonlinear/CustomFactor.cpp Normal file
View File

@ -0,0 +1,76 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file CustomFactor.cpp
* @brief Class to enable arbitrary factors with runtime swappable error function.
* @author Fan Jiang
*/
#include <gtsam/nonlinear/CustomFactor.h>
namespace gtsam {
/*
* Calculates the unwhitened error by invoking the callback functor (i.e. from Python).
*/
Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H) const {
if(this->active(x)) {
if(H) {
/*
* In this case, we pass the raw pointer to the `std::vector<Matrix>` object directly to pybind.
* As the type `std::vector<Matrix>` has been marked as opaque in `preamble.h`, any changes in
* Python will be immediately reflected on the C++ side.
*
* Example:
* ```
* def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
* <calculated error>
* if not H is None:
* <calculate the Jacobian>
* H[0] = J1
* H[1] = J2
* ...
* return error
* ```
*/
return this->error_function_(*this, x, H.get_ptr());
} else {
/*
* In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python.
* Users can check for `None` in their callback to determine if the Jacobian is requested.
*/
return this->error_function_(*this, x, nullptr);
}
} else {
return Vector::Zero(this->dim());
}
}
void CustomFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const {
std::cout << s << "CustomFactor on ";
auto keys_ = this->keys();
bool f = false;
for (const Key &k: keys_) {
if (f)
std::cout << ", ";
std::cout << keyFormatter(k);
f = true;
}
std::cout << "\n";
if (this->noiseModel_)
this->noiseModel_->print(" noise model: ");
else
std::cout << "no noise model" << std::endl;
}
}

104
gtsam/nonlinear/CustomFactor.h Normal file
View File

@ -0,0 +1,104 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file CustomFactor.h
* @brief Class to enable arbitrary factors with runtime swappable error function.
* @author Fan Jiang
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
using namespace gtsam;
namespace gtsam {
using JacobianVector = std::vector<Matrix>;
class CustomFactor;
/*
* NOTE
* ==========
* pybind11 will invoke a copy if this is `JacobianVector &`, and modifications in Python will not be reflected.
*
* This is safe because this is passing a const pointer, and pybind11 will maintain the `std::vector` memory layout.
* Thus the pointer will never be invalidated.
*/
using CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, const JacobianVector *)>;
/**
* @brief Custom factor that takes a std::function as the error
* @addtogroup nonlinear
* \nosubgrouping
*
* This factor is mainly for creating a custom factor in Python.
*/
class CustomFactor: public NoiseModelFactor {
protected:
CustomErrorFunction error_function_;
protected:
using Base = NoiseModelFactor;
using This = CustomFactor;
public:
/**
* Default Constructor for I/O
*/
CustomFactor() = default;
/**
* Constructor
* @param noiseModel shared pointer to noise model
* @param keys keys of the variables
* @param errorFunction the error functional
*/
CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const CustomErrorFunction &errorFunction) :
Base(noiseModel, keys) {
this->error_function_ = errorFunction;
}
~CustomFactor() override = default;
/**
* Calls the errorFunction closure, which is a std::function object
* One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array
*/
Vector unwhitenedError(const Values &x, boost::optional<std::vector<Matrix> &> H = boost::none) const override;
/** print */
void print(const std::string &s,
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
/**
* Mark not sendable
*/
bool sendable() const override {
return false;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("CustomFactor",
boost::serialization::base_object<Base>(*this));
}
};
};

10
gtsam/nonlinear/NonlinearFactor.h
View File

@ -95,7 +95,7 @@ public:
/**
* Checks whether a factor should be used based on a set of values.
* This is primarily used to implment inequality constraints that
* This is primarily used to implement inequality constraints that
* require a variable active set. For all others, the default implementation
* returning true solves this problem.
*
@ -134,6 +134,14 @@ public:
*/
virtual shared_ptr rekey(const KeyVector& new_keys) const;
/**
* 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 {
return true;
}
}; // \class NonlinearFactor
/// traits

12
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -325,7 +325,7 @@ public:
// Operator that linearizes a given range of the factors
void operator()(const tbb::blocked_range<size_t>& blocked_range) const {
for (size_t i = blocked_range.begin(); i != blocked_range.end(); ++i) {
if (nonlinearGraph_[i])
if (nonlinearGraph_[i] && nonlinearGraph_[i]->sendable())
result_[i] = nonlinearGraph_[i]->linearize(linearizationPoint_);
else
result_[i] = GaussianFactor::shared_ptr();
@ -348,9 +348,19 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
linearFG->resize(size());
TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
// First linearize all sendable factors
tbb::parallel_for(tbb::blocked_range<size_t>(0, size()),
_LinearizeOneFactor(*this, linearizationPoint, *linearFG));
// Linearize all non-sendable factors
for(int i = 0; i < size(); i++) {
auto& factor = (*this)[i];
if(factor && !(factor->sendable())) {
(*linearFG)[i] = factor->linearize(linearizationPoint);
}
}
#else
linearFG->reserve(size());

3
matlab/CMakeLists.txt
View File

@ -61,7 +61,8 @@ endif()
# ignoring the non-concrete types (type aliases)
set(ignore
gtsam::Point2
gtsam::Point3)
gtsam::Point3
gtsam::CustomFactor)
# Wrap
matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}"

111
python/CustomFactors.md Normal file
View File

@ -0,0 +1,111 @@
# GTSAM Python-based factors
One now can build factors purely in Python using the `CustomFactor` factor.
## Usage
In order to use a Python-based factor, one needs to have a Python function with the following signature:
```python
import gtsam
import numpy as np
from typing import List
def error_func(this: gtsam.CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
...
```
`this` is a reference to the `CustomFactor` object. This is required because one can reuse the same
`error_func` for multiple factors. `v` is a reference to the current set of values, and `H` is a list of
**references** to the list of required Jacobians (see the corresponding C++ documentation).
If `H` is `None`, it means the current factor evaluation does not need Jacobians. For example, the `error`
method on a factor does not need Jacobians, so we don't evaluate them to save CPU. If `H` is not `None`,
each entry of `H` can be assigned a `numpy` array, as the Jacobian for the corresponding variable.
After defining `error_func`, one can create a `CustomFactor` just like any other factor in GTSAM:
```python
noise_model = gtsam.noiseModel.Unit.Create(3)
# constructor(<noise model>, <list of keys>, <error callback>)
cf = gtsam.CustomFactor(noise_model, [X(0), X(1)], error_func)
```
## Example
The following is a simple `BetweenFactor` implemented in Python.
```python
import gtsam
import numpy as np
from typing import List
expected = Pose2(2, 2, np.pi / 2)
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
"""
Error function that mimics a BetweenFactor
:param this: reference to the current CustomFactor being evaluated
:param v: Values object
:param H: list of references to the Jacobian arrays
:return: the non-linear error
"""
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = expected.localCoordinates(gT1.between(gT2))
if H is not None:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = gtsam.CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func)
```
In general, the Python-based factor works just like their C++ counterparts.
## Known Issues
Because of the `pybind11`-based translation, the performance of `CustomFactor` is not guaranteed.
Also, because `pybind11` needs to lock the Python GIL lock for evaluation of each factor, parallel
evaluation of `CustomFactor` is not possible.
## Implementation
`CustomFactor` is a `NonlinearFactor` that has a `std::function` as its callback.
This callback can be translated to a Python function call, thanks to `pybind11`'s functional support.
The constructor of `CustomFactor` is
```c++
/**
* Constructor
* @param noiseModel shared pointer to noise model
* @param keys keys of the variables
* @param errorFunction the error functional
*/
CustomFactor(const SharedNoiseModel& noiseModel, const KeyVector& keys, const CustomErrorFunction& errorFunction) :
Base(noiseModel, keys) {
this->error_function_ = errorFunction;
}
```
At construction time, `pybind11` will pass the handle to the Python callback function as a `std::function` object.
Something worth special mention is this:
```c++
/*
* NOTE
* ==========
* pybind11 will invoke a copy if this is `JacobianVector &`, and modifications in Python will not be reflected.
*
* This is safe because this is passing a const pointer, and pybind11 will maintain the `std::vector` memory layout.
* Thus the pointer will never be invalidated.
*/
using CustomErrorFunction = std::function<Vector(const CustomFactor&, const Values&, const JacobianVector*)>;
```
which is not documented in `pybind11` docs. One needs to be aware of this if they wanted to implement similar
"mutable" arguments going across the Python-C++ boundary.

179
python/gtsam/examples/CustomFactorExample.py Normal file
View File

@ -0,0 +1,179 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
CustomFactor demo that simulates a 1-D sensor fusion task.
Author: Fan Jiang, Frank Dellaert
"""
import gtsam
import numpy as np
from typing import List, Optional
from functools import partial
def simulate_car():
# Simulate a car for one second
x0 = 0
dt = 0.25 # 4 Hz, typical GPS
v = 144 * 1000 / 3600 # 144 km/hour = 90mph, pretty fast
x = [x0 + v * dt * i for i in range(5)]
return x
x = simulate_car()
print(f"Simulated car trajectory: {x}")
# %%
add_noise = True # set this to False to run with "perfect" measurements
# GPS measurements
sigma_gps = 3.0 # assume GPS is +/- 3m
g = [x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0)
for k in range(5)]
# Odometry measurements
sigma_odo = 0.1 # assume Odometry is 10cm accurate at 4Hz
o = [x[k + 1] - x[k] + (np.random.normal(scale=sigma_odo) if add_noise else 0)
for k in range(4)]
# Landmark measurements:
sigma_lm = 1 # assume landmark measurement is accurate up to 1m
# Assume first landmark is at x=5, we measure it at time k=0
lm_0 = 5.0
z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0)
# Assume other landmark is at x=28, we measure it at time k=3
lm_3 = 28.0
z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0)
unknown = [gtsam.symbol('x', k) for k in range(5)]
print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown)))
# We now can use nonlinear factor graphs
factor_graph = gtsam.NonlinearFactorGraph()
# Add factors for GPS measurements
I = np.eye(1)
gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps)
def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
"""GPS Factor error function
:param measurement: GPS measurement, to be filled with `partial`
:param this: gtsam.CustomFactor handle
:param values: gtsam.Values
:param jacobians: Optional list of Jacobians
:return: the unwhitened error
"""
key = this.keys()[0]
estimate = values.atVector(key)
error = estimate - measurement
if jacobians is not None:
jacobians[0] = I
return error
# Add the GPS factors
for k in range(5):
gf = gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, np.array([g[k]])))
factor_graph.add(gf)
# New Values container
v = gtsam.Values()
# Add initial estimates to the Values container
for i in range(5):
v.insert(unknown[i], np.array([0.0]))
# Initialize optimizer
params = gtsam.GaussNewtonParams()
optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
# Optimize the factor graph
result = optimizer.optimize()
# calculate the error from ground truth
error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
print("Result with only GPS")
print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")
# Adding odometry will improve things a lot
odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo)
def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
"""Odometry Factor error function
:param measurement: Odometry measurement, to be filled with `partial`
:param this: gtsam.CustomFactor handle
:param values: gtsam.Values
:param jacobians: Optional list of Jacobians
:return: the unwhitened error
"""
key1 = this.keys()[0]
key2 = this.keys()[1]
pos1, pos2 = values.atVector(key1), values.atVector(key2)
error = measurement - (pos1 - pos2)
if jacobians is not None:
jacobians[0] = I
jacobians[1] = -I
return error
for k in range(4):
odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], partial(error_odom, np.array([o[k]])))
factor_graph.add(odof)
params = gtsam.GaussNewtonParams()
optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
result = optimizer.optimize()
error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
print("Result with GPS+Odometry")
print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")
# This is great, but GPS noise is still apparent, so now we add the two landmarks
lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm)
def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
"""Landmark Factor error function
:param measurement: Landmark measurement, to be filled with `partial`
:param this: gtsam.CustomFactor handle
:param values: gtsam.Values
:param jacobians: Optional list of Jacobians
:return: the unwhitened error
"""
key = this.keys()[0]
pos = values.atVector(key)
error = pos - measurement
if jacobians is not None:
jacobians[0] = I
return error
factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[0]], partial(error_lm, np.array([lm_0 + z_0]))))
factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[3]], partial(error_lm, np.array([lm_3 + z_3]))))
params = gtsam.GaussNewtonParams()
optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
result = optimizer.optimize()
error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
print("Result with GPS+Odometry+Landmark")
print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")

20
python/gtsam/preamble.h
View File

@ -1,14 +1,30 @@
// Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
// These are required to save one copy operation on Python calls
/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
* These are required to save one copy operation on Python calls.
*
* NOTES
* =================
*
* `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding,
* such that the raw objects can be accessed in Python. Without this they will be automatically
* converted to a Python object, and all mutations on Python side will not be reflected on C++.
*/
#ifdef GTSAM_ALLOCATOR_TBB
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key>>);
#else
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key>);
#endif
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> >);
PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> >);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> >);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Matrix>); // JacobianVector
// TODO(fan): This is to fix the Argument-dependent lookup (ADL) of std::pair. We should find a way to NOT do this.
namespace std {
using gtsam::operator<<;
}

20
python/gtsam/specializations.h
View File

@ -1,10 +1,25 @@
// Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
// These are required to save one copy operation on Python calls
/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
* These are required to save one copy operation on Python calls.
*
* NOTES
* =================
*
* `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding,
* such that the raw objects can be accessed in Python. Without this they will be automatically
* converted to a Python object, and all mutations on Python side will not be reflected on C++.
*
* `py::bind_vector` and similar machinery gives the std container a Python-like interface, but
* without the `<pybind11/stl.h>` copying mechanism. Combined with `PYBIND11_MAKE_OPAQUE` this
* allows the types to be modified with Python, and saves one copy operation.
*/
#ifdef GTSAM_ALLOCATOR_TBB
py::bind_vector<std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >(m_, "KeyVector");
py::implicitly_convertible<py::list, std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >();
#else
py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
py::implicitly_convertible<py::list, std::vector<gtsam::Key> >();
#endif
py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
py::bind_vector<std::vector<gtsam::Point3Pair> >(m_, "Point3Pairs");
py::bind_vector<std::vector<gtsam::Pose3Pair> >(m_, "Pose3Pairs");
@ -17,3 +32,4 @@ py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> > >(m_, "CameraSetCal3_S2");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> > >(m_, "CameraSetCal3Bundler");
py::bind_vector<std::vector<gtsam::Matrix> >(m_, "JacobianVector");

207
python/gtsam/tests/test_custom_factor.py Normal file
View File

@ -0,0 +1,207 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
CustomFactor unit tests.
Author: Fan Jiang
"""
from typing import List
import unittest
from gtsam import Values, Pose2, CustomFactor
import numpy as np
import gtsam
from gtsam.utils.test_case import GtsamTestCase
class TestCustomFactor(GtsamTestCase):
def test_new(self):
"""Test the creation of a new CustomFactor"""
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
"""Minimal error function stub"""
return np.array([1, 0, 0])
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, gtsam.KeyVector([0]), error_func)
def test_new_keylist(self):
"""Test the creation of a new CustomFactor"""
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
"""Minimal error function stub"""
return np.array([1, 0, 0])
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0], error_func)
def test_call(self):
"""Test if calling the factor works (only error)"""
expected_pose = Pose2(1, 1, 0)
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.ndarray:
"""Minimal error function with no Jacobian"""
key0 = this.keys()[0]
error = -v.atPose2(key0).localCoordinates(expected_pose)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0], error_func)
v = Values()
v.insert(0, Pose2(1, 0, 0))
e = cf.error(v)
self.assertEqual(e, 0.5)
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]):
"""
the custom error function. One can freely use variables captured
from the outside scope. Or the variables can be acquired by indexing `v`.
Jacobian is passed by modifying the H array of numpy matrices.
"""
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = expected.localCoordinates(gT1.between(gT2))
if H is not None:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func)
v = Values()
v.insert(0, gT1)
v.insert(1, gT2)
bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model)
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)
def test_printing(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)
def error_func(this: CustomFactor, v: gtsam.Values, _: List[np.ndarray]):
"""Minimal error function stub"""
return np.array([1, 0, 0])
noise_model = gtsam.noiseModel.Unit.Create(3)
from gtsam.symbol_shorthand import X
cf = CustomFactor(noise_model, [X(0), X(1)], error_func)
cf_string = """CustomFactor on x0, x1
noise model: unit (3)
"""
self.assertEqual(cf_string, repr(cf))
def test_no_jacobian(self):
"""Tests that we will not calculate the Jacobian if not requested"""
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]):
"""
Error function that mimics a BetweenFactor
:param this: reference to the current CustomFactor being evaluated
:param v: Values object
:param H: list of references to the Jacobian arrays
:return: the non-linear error
"""
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = expected.localCoordinates(gT1.between(gT2))
self.assertTrue(H is None) # Should be true if we only request the error
if H is not None:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func)
v = Values()
v.insert(0, gT1)
v.insert(1, gT2)
bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model)
e_cf = cf.error(v)
e_bf = bf.error(v)
np.testing.assert_allclose(e_cf, e_bf)
def test_optimization(self):
"""Tests if a factor graph with a CustomFactor can be properly optimized"""
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]):
"""
Error function that mimics a BetweenFactor
:param this: reference to the current CustomFactor being evaluated
:param v: Values object
:param H: list of references to the Jacobian arrays
:return: the non-linear error
"""
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = expected.localCoordinates(gT1.between(gT2))
if H is not None:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0, 1], error_func)
fg = gtsam.NonlinearFactorGraph()
fg.add(cf)
fg.add(gtsam.PriorFactorPose2(0, gT1, noise_model))
v = Values()
v.insert(0, Pose2(0, 0, 0))
v.insert(1, Pose2(0, 0, 0))
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(fg, v, params)
result = optimizer.optimize()
self.gtsamAssertEquals(result.atPose2(0), gT1, tol=1e-5)
self.gtsamAssertEquals(result.atPose2(1), gT2, tol=1e-5)
if __name__ == "__main__":
unittest.main()