From 46c266f8e58579590d304b840348b0adb0eab18f Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 29 Jan 2014 21:22:00 -0500 Subject: [PATCH] Header file --- gtsam/navigation/MagFactor.h | 121 +++++++++++++++++++++++ gtsam/navigation/tests/testMagFactor.cpp | 114 +-------------------- 2 files changed, 123 insertions(+), 112 deletions(-) create mode 100644 gtsam/navigation/MagFactor.h diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h new file mode 100644 index 000000000..82b120fab --- /dev/null +++ b/gtsam/navigation/MagFactor.h @@ -0,0 +1,121 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file MagFactor.h + * @brief Factors involving magnetometers + * @author Frank Dellaert + * @date January 29, 2014 + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/** + * Factor to calibrate local Earth magnetic field as well as magnetometer bias + * This version uses model measured bM = bRn * nM + bias + * and optimizes for both nM and the bias. + * Issue with it: expresses nM in units of magnetometer + */ +class MagFactor1: public NoiseModelFactor2 { + + Vector3 measured_; /** The measured magnetometer values */ + Matrix3 bRn_; /** The assumed known rotation from nav to body */ + +public: + + /** Constructor */ + MagFactor1(Key key1, Key key2, const Vector3& measured, const Rot3& nRb, + const SharedNoiseModel& model) : + NoiseModelFactor2(model, key1, key2), // + measured_(measured), bRn_(nRb.transpose()) { + } + + /// @return a deep copy of this factor + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new MagFactor1(*this))); + } + + /** + * @brief vector of errors + * @param nM (unknown) local earth magnetic field vector, in nav frame + * @param bias (unknown) 3D bias + */ + Vector evaluateError(const LieVector& nM, const LieVector& bias, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + // measured bM = nRbÕ * nM + b, where b is unknown bias + Vector3 hx = bRn_ * nM + bias; + if (H1) + *H1 = bRn_; + if (H2) + *H2 = eye(3); + return hx - measured_; + } +}; + +/** + * Factor to calibrate local Earth magnetic field as well as magnetometer bias + * This version uses model measured bM = scale * bRn * direction + bias + * and optimizes for both scale, direction, and the bias. + */ +class MagFactor2: public NoiseModelFactor3 { + + Vector3 measured_; /** The measured magnetometer values */ + Rot3 bRn_; /** The assumed known rotation from nav to body */ + +public: + + /** Constructor */ + MagFactor2(Key key1, Key key2, Key key3, const Vector3& measured, + const Rot3& nRb, const SharedNoiseModel& model) : + NoiseModelFactor3(model, key1, key2, key3), // + measured_(measured), bRn_(nRb.inverse()) { + } + + /// @return a deep copy of this factor + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new MagFactor2(*this))); + } + + /** + * @brief vector of errors + * @param nM (unknown) local earth magnetic field vector, in nav frame + * @param bias (unknown) 3D bias + */ + Vector evaluateError(const LieScalar& scale, const Sphere2& direction, + const LieVector& bias, boost::optional H1 = boost::none, + boost::optional H2 = boost::none, boost::optional H3 = + boost::none) const { + // measured bM = nRbÕ * nM + b, where b is unknown bias + Sphere2 rotated = bRn_.rotate(direction, boost::none, H2); + Vector3 hx = scale * rotated.unitVector() + bias; + if (H1) + *H1 = rotated.unitVector(); + if (H2) // I think H2 is 2*2, but we need 3*2 + { + Matrix H; + rotated.unitVector(H); + *H2 = scale * H * (*H2); + } + if (H3) + *H3 = eye(3); + return hx - measured_; + } +}; + diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 18c52edd5..090ef09f2 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -13,120 +13,10 @@ * @file testMagFactor.cpp * @brief Unit test for MagFactor * @author Frank Dellaert - * @date January 22, 2014 + * @date January 29, 2014 */ -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -/** - * Factor to calibrate local Earth magnetic field as well as magnetometer bias - * This version uses model measured bM = bRn * nM + bias - * and optimizes for both nM and the bias. - * Issue with it: expresses nM in units of magnetometer - */ -class MagFactor1: public NoiseModelFactor2 { - - Vector3 measured_; /** The measured magnetometer values */ - Matrix3 bRn_; /** The assumed known rotation from nav to body */ - -public: - - /** Constructor */ - MagFactor1(Key key1, Key key2, const Vector3& measured, const Rot3& nRb, - const SharedNoiseModel& model) : - NoiseModelFactor2(model, key1, key2), // - measured_(measured), bRn_(nRb.transpose()) { - } - - /// @return a deep copy of this factor - virtual NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - NonlinearFactor::shared_ptr(new MagFactor1(*this))); - } - - /** - * @brief vector of errors - * @param nM (unknown) local earth magnetic field vector, in nav frame - * @param bias (unknown) 3D bias - */ - Vector evaluateError(const LieVector& nM, const LieVector& bias, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - // measured bM = nRbÕ * nM + b, where b is unknown bias - Vector3 hx = bRn_ * nM + bias; - if (H1) - *H1 = bRn_; - if (H2) - *H2 = eye(3); - return hx - measured_; - } -}; - -/** - * Factor to calibrate local Earth magnetic field as well as magnetometer bias - * This version uses model measured bM = scale * bRn * direction + bias - * and optimizes for both scale, direction, and the bias. - */ -class MagFactor2: public NoiseModelFactor3 { - - Vector3 measured_; /** The measured magnetometer values */ - Rot3 bRn_; /** The assumed known rotation from nav to body */ - -public: - - /** Constructor */ - MagFactor2(Key key1, Key key2, Key key3, const Vector3& measured, - const Rot3& nRb, const SharedNoiseModel& model) : - NoiseModelFactor3(model, key1, key2, key3), // - measured_(measured), bRn_(nRb.inverse()) { - } - - /// @return a deep copy of this factor - virtual NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - NonlinearFactor::shared_ptr(new MagFactor2(*this))); - } - - /** - * @brief vector of errors - * @param nM (unknown) local earth magnetic field vector, in nav frame - * @param bias (unknown) 3D bias - */ - Vector evaluateError(const LieScalar& scale, const Sphere2& direction, - const LieVector& bias, boost::optional H1 = boost::none, - boost::optional H2 = boost::none, boost::optional H3 = - boost::none) const { - // measured bM = nRbÕ * nM + b, where b is unknown bias - Sphere2 rotated = bRn_.rotate(direction, boost::none, H2); - Vector3 hx = scale * rotated.unitVector() + bias; - if (H1) - *H1 = rotated.unitVector(); - if (H2) // I think H2 is 2*2, but we need 3*2 - { - Matrix H; - rotated.unitVector(H); - *H2 = scale * H * (*H2); - } - if (H3) - *H3 = eye(3); - return hx - measured_; - } -}; - -/** - * @file testMagFactor.cpp - * @brief Unit test for MagFactor - * @author Frank Dellaert - * @date January 22, 2014 - */ - -//#include +#include #include #include