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

attempt to get custom factor tests passing

release/4.3a0
Varun Agrawal 2022-01-31 18:55:40 -05:00
parent ef912eefd3
commit fbe9a21070
3 changed files with 29 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/nonlinear/CustomFactor.cpp
View File

@ -22,13 +22,14 @@ 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 {
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
* As the type `std::vector<Matrix>` has been marked as opaque in `preamble/base.h`, any changes in
* Python will be immediately reflected on the C++ side.
*
* Example:
@ -43,13 +44,20 @@ Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vecto
* return error
* ```
*/
return this->error_function_(*this, x, H.get_ptr());
std::pair<Vector, JacobianVector> errorAndJacobian =
this->error_function_(*this, x, H.get_ptr());
Vector error = errorAndJacobian.first;
(*H) = errorAndJacobian.second;
return error;
} 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);
auto errorAndJacobian = this->error_function_(*this, x, nullptr);
return errorAndJacobian.first;
}
} else {
return Vector::Zero(this->dim());

5
gtsam/nonlinear/CustomFactor.h
View File

@ -35,7 +35,8 @@ class CustomFactor;
* 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 *)>;
using CustomErrorFunction = std::function<std::pair<Vector, JacobianVector>(
const CustomFactor &, const Values &, JacobianVector *)>;
/**
* @brief Custom factor that takes a std::function as the error
@ -77,7 +78,7 @@ public:
* 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;
Vector unwhitenedError(const Values &x, boost::optional<std::vector<Matrix>&> H = boost::none) const override;
/** print */
void print(const std::string &s,

29
python/gtsam/tests/test_custom_factor.py
View File

@ -8,13 +8,12 @@ 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
from typing import List
import gtsam
import numpy as np
from gtsam import CustomFactor, JacobianFactor, Pose2, Values
from gtsam.utils.test_case import GtsamTestCase
@ -24,17 +23,17 @@ class TestCustomFactor(GtsamTestCase):
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
"""Minimal error function stub"""
return np.array([1, 0, 0])
return np.array([1, 0, 0]), H
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, gtsam.KeyVector([0]), error_func)
cf = CustomFactor(noise_model, [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])
return np.array([1, 0, 0]), H
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0], error_func)
@ -47,7 +46,7 @@ class TestCustomFactor(GtsamTestCase):
"""Minimal error function with no Jacobian"""
key0 = this.keys()[0]
error = -v.atPose2(key0).localCoordinates(expected_pose)
return error
return error, H
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0], error_func)
@ -81,10 +80,10 @@ class TestCustomFactor(GtsamTestCase):
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
return error, H
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func)
cf = CustomFactor(noise_model, [0, 1], error_func)
v = Values()
v.insert(0, gT1)
v.insert(1, gT2)
@ -104,9 +103,9 @@ class TestCustomFactor(GtsamTestCase):
gT1 = Pose2(1, 2, np.pi / 2)
gT2 = Pose2(-1, 4, np.pi)
def error_func(this: CustomFactor, v: gtsam.Values, _: List[np.ndarray]):
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
"""Minimal error function stub"""
return np.array([1, 0, 0])
return np.array([1, 0, 0]), H
noise_model = gtsam.noiseModel.Unit.Create(3)
from gtsam.symbol_shorthand import X
@ -144,10 +143,10 @@ class TestCustomFactor(GtsamTestCase):
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
return error, H
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func)
cf = CustomFactor(noise_model, [0, 1], error_func)
v = Values()
v.insert(0, gT1)
v.insert(1, gT2)
@ -182,7 +181,7 @@ class TestCustomFactor(GtsamTestCase):
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
return error, H
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0, 1], error_func)