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

undo CustomFactor changes

release/4.3a0
Varun Agrawal 2022-07-17 16:05:59 -04:00
parent a1d6ca207a
commit b60c5e3ab5
3 changed files with 11 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/nonlinear/CustomFactor.cpp
View File

@ -44,20 +44,14 @@ Vector CustomFactor::unwhitenedError(
* return error
* ```
*/
std::pair<Vector, JacobianVector> errorAndJacobian =
this->error_function_(*this, x, H.get_ptr());
return 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.
*/
auto errorAndJacobian = this->error_function_(*this, x, nullptr);
return errorAndJacobian.first;
return this->error_function_(*this, x, nullptr);
}
} else {
return Vector::Zero(this->dim());

5
gtsam/nonlinear/CustomFactor.h
View File

@ -35,8 +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.
*/
using CustomErrorFunction = std::function<std::pair<Vector, JacobianVector>(
const CustomFactor &, const Values &, JacobianVector *)>;
using CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, const JacobianVector *)>;
/**
* @brief Custom factor that takes a std::function as the error
@ -78,7 +77,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,

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

@ -33,7 +33,7 @@ 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]), H
return np.array([1, 0, 0])
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0], error_func)
@ -46,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, H
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0], error_func)
@ -80,7 +80,7 @@ class TestCustomFactor(GtsamTestCase):
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error, H
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0, 1], error_func)
@ -103,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, H: List[np.ndarray]):
def error_func(this: CustomFactor, v: gtsam.Values, _: List[np.ndarray]):
"""Minimal error function stub"""
return np.array([1, 0, 0]), H
return np.array([1, 0, 0])
noise_model = gtsam.noiseModel.Unit.Create(3)
from gtsam.symbol_shorthand import X
@ -143,7 +143,7 @@ class TestCustomFactor(GtsamTestCase):
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error, H
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0, 1], error_func)
@ -181,7 +181,7 @@ class TestCustomFactor(GtsamTestCase):
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error, H
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0, 1], error_func)