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

46
gtsam/navigation/ConstantVelocityFactor.h
View File

@ -20,35 +20,33 @@ class ConstantVelocityFactor : public NoiseModelFactor2<NavState, NavState> {
gtsam::Vector evaluateError(const NavState &x1, const NavState &x2,
boost::optional<gtsam::Matrix &> H1 = boost::none,
boost::optional<gtsam::Matrix &> H2 = boost::none) const override {
// so we can reuse this calculation below
auto evaluateErrorInner = [dt = dt_](const NavState &x1, const NavState &x2) -> gtsam::Vector {
const Velocity3 &v1 = x1.v();
const Velocity3 &v2 = x2.v();
const Point3 &p1 = x1.t();
const Point3 &p2 = x2.t();
// trapezoidal integration constant accelleration
// const Point3 hx = p1 + Point3((v1 + v2) * dt / 2.0);
// euler start
// const Point3 hx = p1 + Point3(v1 * dt);
// euler end
const Point3 hx = p1 + Point3(v2 * dt);
return p2 - hx;
};
if (H1) {
(*H1) = numericalDerivative21<gtsam::Vector, NavState, NavState>(
boost::bind(ConstantVelocityFactor::evaluateError_, _1, _2, dt_), x1, x2, 1e-5);
(*H1) = numericalDerivative21<gtsam::Vector, NavState, NavState>(evaluateErrorInner, x1, x2, 1e-5);
}
if (H2) {
(*H2) = numericalDerivative22<gtsam::Vector, NavState, NavState>(
boost::bind(ConstantVelocityFactor::evaluateError_, _1, _2, dt_), x1, x2, 1e-5);
(*H2) = numericalDerivative22<gtsam::Vector, NavState, NavState>(evaluateErrorInner, 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 &v2 = x2.v();
const Point3 &p1 = x1.t();
const Point3 &p2 = x2.t();
// trapezoidal integration constant accelleration
// const Point3 hx = p1 + Point3((v1 + v2) * dt / 2.0);
// euler start
// const Point3 hx = p1 + Point3(v1 * dt);
// euler end
const Point3 hx = p1 + Point3(v2 * dt);
return p2 - hx;
return evaluateErrorInner(x1, x2);
}
};