From a48c72ff63a7925f8fdf3ef894aae308cd79e898 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Feb 2014 10:33:17 -0500 Subject: [PATCH] Fixed sign of 2D version --- gtsam/navigation/MagFactor.h | 5 +++-- gtsam/navigation/tests/testMagFactor.cpp | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index eb40513e9..61ffd2d5d 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -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 { @@ -52,7 +53,7 @@ public: static Point3 unrotate(const Rot2& R, const Point3& p, boost::optional 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; } diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 6a6954e55..7e6b940c3 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -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