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

a numerical derivative version for DiscreteEulerPoincare'Factor, but currently disabled.

release/4.3a0
Duy-Nguyen Ta 2013-05-01 17:30:21 +00:00
parent 9e2b11800a
commit 444ab957c4
1 changed files with 53 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -7,6 +7,7 @@
#pragma once
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
@ -20,6 +21,8 @@ namespace gtsam {
* \f$ \xi_k \f$: the body-fixed velocity (Lie algebra)
* It is somewhat similar to BetweenFactor, but treats the body-fixed velocity
* \f$ \xi_k \f$ as a variable. So it is a three-way factor.
* Note: this factor is necessary if one needs to smooth the entire graph. It's not needed
* in sequential update method.
*/
class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, LieVector> {
@ -149,6 +152,52 @@ public:
return hx;
}
#if 0
Vector computeError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk) const {
Vector pk = Pose3::dExpInv_exp(h_*xik).transpose()*Inertia_*xik;
Vector pk_1 = Pose3::dExpInv_exp(-h_*xik_1).transpose()*Inertia_*xik_1;
Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_));
Vector f_ext = Vector_(6, 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z());
Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext;
return hx;
}
Vector evaluateError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
if (H1) {
(*H1) = numericalDerivative31(
boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
),
xik, xik_1, gk, 1e-5
);
}
if (H2) {
(*H2) = numericalDerivative32(
boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
),
xik, xik_1, gk, 1e-5
);
}
if (H3) {
(*H3) = numericalDerivative33(
boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
),
xik, xik_1, gk, 1e-5
);
}
return computeError(xik, xik_1, gk);
}
#endif
};