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

Deal with Rot3 changes

release/4.3a0
dellaert 2014-10-07 19:35:20 +02:00
parent e1c9ae95cb
commit 84987aa351
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/navigation/MagFactor.h
View File

@ -53,7 +53,7 @@ public:
static Point3 unrotate(const Rot2& R, const Point3& p,
boost::optional<Matrix&> HR = boost::none) {
Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR);
Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR, boost::none);
if (HR) {
// assign to temporary first to avoid error in Win-Debug mode
Matrix H = HR->col(2);
@ -106,7 +106,7 @@ public:
Vector evaluateError(const Rot3& nRb,
boost::optional<Matrix&> H = boost::none) const {
// measured bM = nRbÕ * nM + b
Point3 hx = nRb.unrotate(nM_, H) + bias_;
Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
return (hx - measured_).vector();
}
};

2
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -125,7 +125,7 @@ public:
Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(-h_*xik_1, muk_1, D_adjThxik1_muk1);
Matrix D_gravityBody_gk;
Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk);
Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk, boost::none);
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;