diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index bee6bcef6..cbd1096a9 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -80,10 +80,10 @@ std::pair AHRS::initialize(double g_e) double g3 = b_g(2); double g23 = g2 * g2 + g3 * g3; double g123 = g1 * g1 + g23; - double f = 1 / (sqrt(g23) * g123); + double f = 1 / (std::sqrt(g23) * g123); Matrix H_g = Matrix_(3, 3, 0.0, g3 / g23, -(g2 / g23), // roll - sqrt(g23) / g123, -f * (g1 * g2), -f * (g1 * g3), // pitch + std::sqrt(g23) / g123, -f * (g1 * g2), -f * (g1 * g3), // pitch 0.0, 0.0, 0.0); // we don't know anything on yaw // Calculate the initial covariance matrix for the error state dx, Farrell08book eq. 10.66 diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h index 89f8b2b5e..284fe2fc6 100644 --- a/gtsam_unstable/slam/AHRS.h +++ b/gtsam_unstable/slam/AHRS.h @@ -9,13 +9,14 @@ #define AHRS_H_ #include "Mechanization_bRn2.h" +#include #include namespace gtsam { Matrix cov(const Matrix& m); -class AHRS { +class GTSAM_UNSTABLE_EXPORT AHRS { private: diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 0a71da06d..b81e685d4 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -9,11 +9,12 @@ #include #include +#include #include namespace gtsam { -class Mechanization_bRn2 { +class GTSAM_UNSTABLE_EXPORT Mechanization_bRn2 { private: Rot3 bRn_; ///< rotation from nav to body