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

Fixed sign of 2D version

release/4.3a0
dellaert 2014-02-01 10:33:17 -05:00
parent 5369a7bd17
commit a48c72ff63
2 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/navigation/MagFactor.h
View File

@ -26,7 +26,8 @@ namespace gtsam {
/**
* Factor to estimate rotation given magnetometer reading
* This version uses model measured bM = scale * bRn * direction + bias
* and assumes scale, direction, and the bias are given
* and assumes scale, direction, and the bias are given.
* Rotation is around negative Z axis, i.e. positive is yaw to right!
*/
class MagFactor: public NoiseModelFactor1<Rot2> {
@ -52,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()).rotate(p,HR);
Point3 q = Rot3::yaw(R.theta()).unrotate(p,HR);
if (HR) *HR = HR->col(2);
return q;
}

2
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -41,7 +41,7 @@ Point3 nM(22653.29982, -1956.83010, 44202.47862);
double scale = 255.0 / 50000.0;
// ...ground truth orientation,
Rot3 nRb = Rot3::yaw(-0.1);
Rot2 theta = -nRb.yaw();
Rot2 theta = nRb.yaw();
// ...and bias
Point3 bias(10, -10, 50);
// ... then we measure