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

bind a lambda instead of a static function

release/4.3a0
Asa Hammond 2021-02-23 18:27:20 -08:00
parent 2aa7144b7e
commit c0cd024b2a
1 changed files with 22 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/navigation/ConstantVelocityFactor.h
View File

@ -20,20 +20,8 @@ class ConstantVelocityFactor : public NoiseModelFactor2<NavState, NavState> {
gtsam::Vector evaluateError(const NavState &x1, const NavState &x2, gtsam::Vector evaluateError(const NavState &x1, const NavState &x2,
boost::optional<gtsam::Matrix &> H1 = boost::none, boost::optional<gtsam::Matrix &> H1 = boost::none,
boost::optional<gtsam::Matrix &> H2 = boost::none) const override { boost::optional<gtsam::Matrix &> H2 = boost::none) const override {
if (H1) { // so we can reuse this calculation below
(*H1) = numericalDerivative21<gtsam::Vector, NavState, NavState>( auto evaluateErrorInner = [dt = dt_](const NavState &x1, const NavState &x2) -> gtsam::Vector {
boost::bind(ConstantVelocityFactor::evaluateError_, _1, _2, dt_), x1, x2, 1e-5);
}
if (H2) {
(*H2) = numericalDerivative22<gtsam::Vector, NavState, NavState>(
boost::bind(ConstantVelocityFactor::evaluateError_, _1, _2, dt_), x1, x2, 1e-5);
}
return evaluateError_(x1, x2, dt_);
}
private:
static gtsam::Vector evaluateError_(const NavState &x1, const NavState &x2, double dt) {
const Velocity3 &v1 = x1.v(); const Velocity3 &v1 = x1.v();
const Velocity3 &v2 = x2.v(); const Velocity3 &v2 = x2.v();
const Point3 &p1 = x1.t(); const Point3 &p1 = x1.t();
@ -49,6 +37,16 @@ class ConstantVelocityFactor : public NoiseModelFactor2<NavState, NavState> {
const Point3 hx = p1 + Point3(v2 * dt); const Point3 hx = p1 + Point3(v2 * dt);
return p2 - hx; return p2 - hx;
};
if (H1) {
(*H1) = numericalDerivative21<gtsam::Vector, NavState, NavState>(evaluateErrorInner, x1, x2, 1e-5);
}
if (H2) {
(*H2) = numericalDerivative22<gtsam::Vector, NavState, NavState>(evaluateErrorInner, x1, x2, 1e-5);
}
return evaluateErrorInner(x1, x2);
} }
}; };