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

Add printing for CustomFactor

release/4.3a0
Fan Jiang 2021-05-17 19:19:20 -04:00
parent 3c327ff568
commit 5d1fd83a2c
3 changed files with 77 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/gtsam.i
View File

@ -2185,7 +2185,10 @@ virtual class CustomFactor: gtsam::NoiseModelFactor {
* cf = CustomFactor(noise_model, keys, error_func)
* ```
*/
CustomFactor(const gtsam::SharedNoiseModel& noiseModel, const gtsam::KeyVector& keys, const gtsam::CustomErrorFunction& errorFunction);
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>

31
gtsam/nonlinear/CustomFactor.h
View File

@ -23,7 +23,7 @@ using namespace gtsam;
namespace gtsam {
typedef std::vector<Matrix> JacobianVector;
using JacobianVector = std::vector<Matrix>;
class CustomFactor;
@ -35,7 +35,7 @@ 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.
*/
typedef std::function<Vector(const CustomFactor&, const Values&, const JacobianVector*)> CustomErrorFunction;
using CustomErrorFunction = std::function<Vector(const CustomFactor&, const Values&, const JacobianVector*)>;
/**
* @brief Custom factor that takes a std::function as the error
@ -73,10 +73,31 @@ public:
~CustomFactor() override = default;
/** Calls the errorFunction closure, which is a std::function object
/**
* 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,
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
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;
}
private:

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

@ -17,15 +17,26 @@ 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]):
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]):
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)
@ -34,22 +45,22 @@ class TestCustomFactor(GtsamTestCase):
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)
gT1 = Pose2(1, 2, np.pi / 2)
gT2 = Pose2(-1, 4, np.pi)
expected = Pose2(2, 2, np.pi/2)
expected = Pose2(2, 2, np.pi / 2)
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
"""
@ -62,19 +73,19 @@ class TestCustomFactor(GtsamTestCase):
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = Pose2(0, 0, 0).localCoordinates(gT1.between(gT2))
if not H is None:
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, Pose2(0, 0, 0), noise_model)
gf = cf.linearize(v)
@ -85,13 +96,34 @@ class TestCustomFactor(GtsamTestCase):
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]):
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = Pose2(0, 0, 0).localCoordinates(gT1.between(gT2))
return error
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)
gT1 = Pose2(1, 2, np.pi / 2)
gT2 = Pose2(-1, 4, np.pi)
expected = Pose2(2, 2, np.pi/2)
expected = Pose2(2, 2, np.pi / 2)
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
# print(f"{this = },\n{v = },\n{len(H) = }")
@ -101,9 +133,9 @@ class TestCustomFactor(GtsamTestCase):
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = Pose2(0, 0, 0).localCoordinates(gT1.between(gT2))
self.assertTrue(H is None) # Should be true if we only request the error
self.assertTrue(H is None) # Should be true if we only request the error
if not H is None:
if H is not None:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
@ -121,5 +153,6 @@ class TestCustomFactor(GtsamTestCase):
e_bf = bf.error(v)
np.testing.assert_allclose(e_cf, e_bf)
if __name__ == "__main__":
unittest.main()