From 3975d0a6420dba73a4a7123f406c86a264acf934 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 16 Jan 2016 18:33:56 -0800 Subject: [PATCH 1/3] New tests --- gtsam/geometry/tests/testPoint3.cpp | 11 +++++++++++ gtsam/geometry/tests/testRot3.cpp | 26 +++++++++++++++++--------- 2 files changed, 28 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 6811ed122..246d23c9a 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -133,6 +133,17 @@ TEST (Point3, distance) { EXPECT(assert_equal(numH2, H2, 1e-8)); } +/* ************************************************************************* */ +TEST(Point3, cross) { + Matrix aH1, aH2; + boost::function f = + boost::bind(&Point3::cross, _1, _2, boost::none, boost::none); + const Point3 omega(0, 1, 0), theta(4, 6, 8); + omega.cross(theta, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index eb6573c30..59b365b2a 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -270,19 +270,27 @@ TEST( Rot3, ExpmapDerivative) { /* ************************************************************************* */ TEST( Rot3, ExpmapDerivative2) { - const Vector3 thetahat(0.1, 0, 0.1); + const Vector3 theta(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( - boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); + boost::bind(&Rot3::Expmap, _1, boost::none), theta); - const Matrix Jactual = Rot3::ExpmapDerivative(thetahat); - CHECK(assert_equal(Jexpected, Jactual)); - - const Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat); - CHECK(assert_equal(Jexpected, Jactual2)); + CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta))); + CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta))); } /* ************************************************************************* */ -TEST(Rot3, ExpmapDerivative3) { +TEST( Rot3, ExpmapDerivative3) +{ + const Vector3 theta(10, 20, 30); + const Matrix Jexpected = numericalDerivative11( + boost::bind(&Rot3::Expmap, _1, boost::none), theta); + + CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta))); + CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta))); +} + +/* ************************************************************************* */ +TEST(Rot3, ExpmapDerivative4) { // Iserles05an (Lie-group Methods) says: // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) @@ -309,7 +317,7 @@ TEST(Rot3, ExpmapDerivative3) { } /* ************************************************************************* */ -TEST(Rot3, ExpmapDerivative4) { +TEST(Rot3, ExpmapDerivative5) { auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; From 7c66ea132355584627ff3a73fef1a9be8ff70a7b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 16 Jan 2016 18:34:16 -0800 Subject: [PATCH 2/3] Synchronized write-up with code in manifold branch --- doc/ImuFactor.lyx | 244 ++++++++++++++++++++++------------------------ 1 file changed, 116 insertions(+), 128 deletions(-) diff --git a/doc/ImuFactor.lyx b/doc/ImuFactor.lyx index 23b64b1aa..7e5ceac33 100644 --- a/doc/ImuFactor.lyx +++ b/doc/ImuFactor.lyx @@ -98,7 +98,6 @@ Navigation States \begin_layout Standard Let us assume a setup where frames with image and/or laser measurements are processed at some fairly low rate, e.g., 10 Hz. - \end_layout \begin_layout Standard @@ -191,7 +190,7 @@ tangent vector , and for the NavState manifold this will be a triplet \begin_inset Formula \[ -\left[W(t,X),V(t,X),A(t,X)\right]\in\sothree\times\Rthree\times\Rthree +\left[\dot{R}(t,X),\dot{P}(t,X),\dot{V}(t,X)\right]\in\sothree\times\Rthree\times\Rthree \] \end_inset @@ -264,30 +263,42 @@ Valid vector fields on a NavState manifold are special, in that the attitude : \begin_inset Formula \begin{equation} -\dot{X}(t)=\left[W(X,t),V(t),A(X,t)\right]\label{eq:validField} +\dot{X}(t)=\left[\dot{R}(X,t),V(t),\dot{V}(X,t)\right]\label{eq:validField} \end{equation} \end_inset -If we know +Suppose we are given the +\series bold +body angular velocity +\series default + \begin_inset Formula $\omega^{b}(t)$ \end_inset and non-gravity +\series bold +acceleration +\series default + \begin_inset Formula $a^{b}(t)$ \end_inset - in the body frame, we know (from Murray84book) that the body angular velocity - an be written as + in the body frame. + We know (from Murray84book) that the derivative of +\begin_inset Formula $R$ +\end_inset + + can be written as \begin_inset Formula \[ -\Skew{\omega^{b}(t)}=R(t)^{T}W(X,t) +\dot{R}(X,t)=R(t)\Skew{\omega^{b}(t)} \] \end_inset where -\begin_inset Formula $\Skew{\omega^{b}(t)}\in so(3)$ +\begin_inset Formula $\Skew{\theta}\in so(3)$ \end_inset is the skew-symmetric matrix corresponding to @@ -297,7 +308,7 @@ where , and hence the resulting exact vector field is \begin_inset Formula \begin{equation} -\dot{X}(t)=\left[W(X,t),V(t),A(X,t)\right]=\left[R(t)\Skew{\omega^{b}(t)},V(t),g+R(t)a^{b}(t)\right]\label{eq:bodyField} +\dot{X}(t)=\left[\dot{R}(X,t),V(t),\dot{V}(X,t)\right]=\left[R(t)\Skew{\omega^{b}(t)},V(t),g+R(t)a^{b}(t)\right]\label{eq:bodyField} \end{equation} \end_inset @@ -613,7 +624,11 @@ R(t)=\Phi_{R_{0}}(\theta(t)) \end_inset -We can create a trajectory +To find an expression for +\begin_inset Formula $\dot{\theta}(t)$ +\end_inset + +, create a trajectory \begin_inset Formula $\gamma(\delta)$ \end_inset @@ -625,7 +640,15 @@ We can create a trajectory \begin_inset Formula $\delta=0$ \end_inset - +, and moves +\begin_inset Formula $\theta(t)$ +\end_inset + + along the direction +\begin_inset Formula $\dot{\theta}(t)$ +\end_inset + +: \begin_inset Formula \[ \gamma(\delta)=R(t+\delta)=\Phi_{R_{0}}\left(\theta(t)+\dot{\theta}(t)\delta\right)\approx\Phi_{R(t)}\left(H(\theta)\dot{\theta}(t)\delta\right) @@ -633,7 +656,7 @@ We can create a trajectory \end_inset -and taking the derivative for +Taking the derivative for \begin_inset Formula $\delta=0$ \end_inset @@ -1073,7 +1096,7 @@ Predict the NavState from \begin_inset Formula \[ -X_{j}=\mathcal{R}_{X_{j}}(\zeta(t_{ij}))=\left\{ \Phi_{R_{0}}\left(\theta(t_{ij})\right),P_{i}+V_{i}t_{ij}+\frac{gt_{ij}^{2}}{2}+R_{i}\, p_{v}(t_{ij}),V_{i}+gt_{ij}+R_{i}\, v_{a}(t_{ij})\right\} +X_{j}=\mathcal{R}_{X_{i}}(\zeta(t_{ij}))=\left\{ \Phi_{R_{0}}\left(\theta(t_{ij})\right),P_{i}+V_{i}t_{ij}+\frac{gt_{ij}^{2}}{2}+R_{i}\, p_{v}(t_{ij}),V_{i}+gt_{ij}+R_{i}\, v_{a}(t_{ij})\right\} \] \end_inset @@ -1090,12 +1113,12 @@ Note that the predicted NavState \begin_inset Formula $X_{i}$ \end_inset -, but the inrgrated quantities +, but the integrated quantities \begin_inset Formula $\theta(t)$ \end_inset , -\begin_inset Formula $p_{i}(t)$ +\begin_inset Formula $p_{v}(t)$ \end_inset , and @@ -1113,9 +1136,9 @@ A Simple Euler Scheme To solve the differential equation we can use a simple Euler scheme: \begin_inset Formula \begin{eqnarray} -\theta_{k+1}=\theta_{k}+\dot{\theta}(t_{k})\Delta_{t} & = & \theta_{k}+H(\theta_{k})^{-1}\,\omega_{k}^{b}\Delta_{t}\label{eq:euler_theta}\\ -p_{k+1}=p_{k}+\dot{p}_{v}(t_{k})\Delta_{t} & = & p_{k}+v_{k}\Delta_{t}\label{eq:euler_p}\\ -v_{k+1}=v_{k}+\dot{v}_{a}(t_{k})\Delta_{t} & = & v_{k}+\exp\left(\Skew{\theta_{k}}\right)a_{k}^{b}\Delta_{t}\label{eq:euler_v} +\theta_{k+1}=\theta_{k}+\dot{\theta}(t_{k})\Delta_{t} & = & \theta_{k}+H(\theta_{k})^{-1}\,\omega_{k}^{b}\Delta_{t}\label{eq:euler_theta-1}\\ +p_{k+1}=p_{k}+\dot{p}_{v}(t_{k})\Delta_{t} & = & p_{k}+v_{k}\Delta_{t}\label{eq:euler_p-1}\\ +v_{k+1}=v_{k}+\dot{v}_{a}(t_{k})\Delta_{t} & = & v_{k}+\exp\left(\Skew{\theta_{k}}\right)a_{k}^{b}\Delta_{t}\label{eq:euler_v-1} \end{eqnarray} \end_inset @@ -1132,6 +1155,26 @@ where \begin_inset Formula $v_{k}\define v_{a}(t_{k})$ \end_inset +. + However, the position propagation can be done more accurately, by using + exact integration of the zero-order hold acceleration +\begin_inset Formula $a_{k}^{b}$ +\end_inset + +: +\begin_inset Formula +\begin{eqnarray} +\theta_{k+1} & = & \theta_{k}+H(\theta_{k})^{-1}\,\omega_{k}^{b}\Delta_{t}\label{eq:euler_theta}\\ +p_{k+1} & = & p_{k}+v_{k}\Delta_{t}+R_{k}a_{k}^{b}\frac{\Delta_{t}^{2}}{2}\label{eq:euler_p}\\ +v_{k+1} & = & v_{k}+R_{k}a_{k}^{b}\Delta_{t}\label{eq:euler_v} +\end{eqnarray} + +\end_inset + +where we defined the rotation matrix +\begin_inset Formula $R_{k}=\exp\left(\Skew{\theta_{k}}\right)$ +\end_inset + . \end_layout @@ -1196,7 +1239,7 @@ Then the noise on propagates as \begin_inset Formula \begin{equation} -\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{gd}B_{k}+C_{k}\Sigma_{\eta}^{ad}C_{k}\label{eq:prop} +\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}+C_{k}\Sigma_{\eta}^{gd}C_{k}\label{eq:prop} \end{equation} \end_inset @@ -1230,65 +1273,84 @@ where \end_inset partial derivatives with respect to the measured quantities -\begin_inset Formula $\omega^{b}$ -\end_inset - - and \begin_inset Formula $a^{b}$ +\end_inset + + and +\begin_inset Formula $\omega^{b}$ \end_inset . - Noting that +\end_layout + +\begin_layout Standard \begin_inset Formula \[ -H(\theta)=\sum_{k=0}^{\infty}\frac{(-1)^{k}}{(k+1)!}\Skew{\theta}^{k}\approx I-\frac{1}{2}\Skew{\theta} +\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t} \] \end_inset -for small -\begin_inset Formula $\theta$ +It can be shown that for small +\begin_inset Formula $\theta_{k}$ \end_inset -, and + we have \begin_inset Formula \[ -\deriv{\Skew{\theta}\omega}{\theta}=\deriv{\left(\theta\times\omega\right)}{\theta}=-\deriv{\left(\omega\times\theta\right)}{\theta}=-\deriv{\Skew{\omega}\theta}{\theta}=-\Skew{\omega} +\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}-\frac{\Delta t}{2}\Skew{\omega_{k}^{b}} \] \end_inset -we have +For the derivatives of +\begin_inset Formula $p_{k+1}$ +\end_inset + + and +\begin_inset Formula $v_{k+1}$ +\end_inset + + we need the derivative \begin_inset Formula \[ -\deriv{H(\theta)\omega}{\theta}\approx\frac{1}{2}\Skew{\omega} +\deriv{R_{k}a_{k}^{b}}{\theta_{k}}=-R_{k}\Skew{a_{k}^{b}}\deriv{R_{k}}{\theta_{k}}=-R_{k}\Skew{a_{k}^{b}}H(\theta_{k}) \] \end_inset -Similarly, +where we used \begin_inset Formula \[ -\exp\left(\Skew{\theta}\right)=\sum_{k=0}^{\infty}\frac{1}{k!}\Skew{\theta}^{k}\approx I+\Skew{\theta} +\deriv{\left(Ra\right)}R\approx-R\Skew a \] \end_inset -and hence -\begin_inset Formula -\[ -\deriv{\exp\left(\Skew{\theta}\right)a}{\theta}\approx-\Skew a -\] - +and the fact that the dependence of the rotation +\begin_inset Formula $R_{k}$ \end_inset -so we finally obtain + on +\begin_inset Formula $\theta_{k}$ +\end_inset + + is the already computed +\begin_inset Formula $H(\theta_{k})$ +\end_inset + +. + +\end_layout + +\begin_layout Standard +Putting all this together, we finally obtain \begin_inset Formula \[ A_{k}\approx\left[\begin{array}{ccc} -I_{3\times3}+\frac{1}{2}\Skew{\omega_{k}^{b}}\Delta_{t}\\ - & I_{3\times3} & I_{3\times3}\Delta_{t}\\ --\Skew{a_{k}^{b}}\Delta_{t} & & I_{3\times3} +I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}}\\ +-R_{k}\Skew{a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t}\\ +-R_{k}\Skew{a_{k}^{b}}H(\theta_{k})\Delta_{t} & & I_{3\times3} \end{array}\right] \] @@ -1298,101 +1360,27 @@ The other partial derivatives are simply \begin_inset Formula \[ B_{k}=\left[\begin{array}{c} -H(\theta_{k})^{-1}\Delta^{t}\\ +0_{3\times3}\\ +R_{k}\frac{\Delta_{t}}{2}^{2}\\ +R_{k}\Delta_{t} +\end{array}\right],\,\,\,\, C_{k}=\left[\begin{array}{c} +H(\theta_{k})^{-1}\Delta_{t}\\ 0_{3\times3}\\ 0_{3\times3} -\end{array}\right],\,\,\,\, C_{k}=\left[\begin{array}{c} -0_{3\times3}\\ -0_{3\times3}\\ -\exp\left(\Skew{\theta_{k}}\right)\Delta_{t} \end{array}\right] \] \end_inset -Substituting these expressions into Eq. - -\begin_inset CommandInset ref -LatexCommand ref -reference "eq:prop" +\end_layout + +\begin_layout Standard +A more accurate partial derivative of +\begin_inset Formula $H(\theta_{k})^{-1}$ \end_inset - and dropping terms involving -\begin_inset Formula $\Delta_{t}^{2}$ -\end_inset - -, we obtain -\family roman -\series medium -\shape up -\size normal -\emph off -\bar no -\strikeout off -\uuline off -\uwave off -\noun off -\color none - -\begin_inset Formula -\[ -\Sigma_{k+1}=\Sigma_{k}+\left[\begin{array}{ccc} -\frac{1}{2}\Skew{\omega_{k}^{b}}\Sigma_{k}^{\theta\theta}-\Sigma_{k}^{\theta\theta}\frac{1}{2}\Skew{\omega_{k}^{b}} & \Sigma_{k}^{\theta v}+\frac{1}{2}\Skew{\omega_{k}^{b}}\Sigma_{k}^{\theta p} & \Sigma_{k}^{\theta\theta}\Skew{a_{k}^{b}}+\frac{1}{2}\Skew{\omega_{k}^{b}}\Sigma_{k}^{\theta v}\\ -. & \Sigma_{k}^{pv}+\Sigma_{k}^{vp} & \Sigma_{k}^{vv}+\Sigma_{k}^{p\theta}\Skew{a_{k}^{b}}\\ -. & . & \Sigma_{k}^{v\theta}\Skew{a_{k}^{b}}-\Skew{a_{k}^{b}}\Sigma_{k}^{\theta v} -\end{array}\right]\Delta^{t}+\Sigma_{k}^{\eta} -\] - -\end_inset - -where we only show the upper-triangular part (the matrix is symmetric) and - where -\begin_inset Formula -\[ -\Sigma_{k}^{\eta}=B_{k}\Sigma_{\eta}^{gd}B_{k}+C_{k}\Sigma_{\eta}^{ad}C_{k}=\left[\begin{array}{ccc} -\sigma^{g}I_{3\times3}\\ -\\ - & & \sigma^{a}I_{3\times3} -\end{array}\right]\Delta_{t} -\] - -\end_inset - -The equality in the last line holds in the case of isotropic Gaussian measuremen -t noise, in which case -\begin_inset Formula $\Sigma_{\eta}^{gd}$ -\end_inset - -= -\begin_inset Formula $\sigma^{g}I_{3\times3}/\Delta_{t}$ -\end_inset - - and -\begin_inset Formula $\Sigma_{\eta}^{ga}$ -\end_inset - -= -\begin_inset Formula $\sigma^{a}I_{3\times3}/\Delta_{t}$ -\end_inset - -, and used the identities -\begin_inset Formula $H(\theta)^{-1}H(\theta)^{-T}\approx I_{3\times3}$ -\end_inset - - for small -\begin_inset Formula $\theta$ -\end_inset - -, and -\begin_inset Formula $\exp\left(\Skew{\theta}\right)\exp\left(\Skew{\theta}\right)^{T}=I_{3\times3}$ -\end_inset - - for all -\begin_inset Formula $\theta$ -\end_inset - -. + can be used, as well. \end_layout \begin_layout Section From 43520265aae6b3bfecb691f4e9c395efb7437922 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 17 Jan 2016 14:44:03 -0800 Subject: [PATCH 3/3] Fixed all navigation tests that were still using deprecated methods/types --- gtsam/navigation/CombinedImuFactor.h | 2 +- gtsam/navigation/ImuFactor.cpp | 15 +- gtsam/navigation/ImuFactor.h | 2 + gtsam/navigation/ScenarioRunner.cpp | 6 +- gtsam/navigation/ScenarioRunner.h | 10 +- .../tests/testCombinedImuFactor.cpp | 76 ++++------- gtsam/navigation/tests/testImuFactor.cpp | 128 ++++++++---------- .../navigation/tests/testPoseVelocityBias.cpp | 3 + gtsam/navigation/tests/testScenarioRunner.cpp | 14 +- 9 files changed, 117 insertions(+), 139 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 3bc8176a2..f5ce1f3db 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -278,10 +278,10 @@ public: boost::optional H4 = boost::none, boost::optional H5 = boost::none, boost::optional H6 = boost::none) const; +#ifdef ALLOW_DEPRECATED_IN_GTSAM4 /// @deprecated typename typedef gtsam::PreintegratedCombinedMeasurements CombinedPreintegratedMeasurements; -#ifdef ALLOW_DEPRECATED_IN_GTSAM4 /// @deprecated constructor CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, const CombinedPreintegratedMeasurements& pim, diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 2104d1878..673706d58 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -29,7 +29,7 @@ namespace gtsam { using namespace std; //------------------------------------------------------------------------------ -// Inner class PreintegratedMeasurements +// Inner class PreintegratedImuMeasurements //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::print(const string& s) const { PreintegrationBase::print(s); @@ -156,14 +156,15 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, } //------------------------------------------------------------------------------ +#ifdef ALLOW_DEPRECATED_IN_GTSAM4 ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, - const PreintegratedMeasurements& pim, const Vector3& n_gravity, + const PreintegratedImuMeasurements& pim, const Vector3& n_gravity, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor, const bool use2ndOrderCoriolis) : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), _PIM_(pim) { - boost::shared_ptr p = boost::make_shared< - PreintegratedMeasurements::Params>(pim.p()); + boost::shared_ptr p = boost::make_shared< + PreintegratedImuMeasurements::Params>(pim.p()); p->n_gravity = n_gravity; p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; @@ -171,11 +172,9 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, _PIM_.p_ = p; } -//------------------------------------------------------------------------------ -#ifdef ALLOW_DEPRECATED_IN_GTSAM4 void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, - PreintegratedMeasurements& pim, const Vector3& n_gravity, + PreintegratedImuMeasurements& pim, const Vector3& n_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) { // use deprecated predict PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity, @@ -184,4 +183,6 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, vel_j = pvb.velocity; } #endif +//------------------------------------------------------------------------------ + } // namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 9be189d02..ac7aee797 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -223,6 +223,7 @@ public: boost::optional H3 = boost::none, boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; +#ifdef ALLOW_DEPRECATED_IN_GTSAM4 /// @deprecated typename typedef PreintegratedImuMeasurements PreintegratedMeasurements; @@ -239,6 +240,7 @@ public: Vector3& vel_j, const imuBias::ConstantBias& bias_i, PreintegratedMeasurements& pim, const Vector3& n_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); +#endif private: diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 888508835..48f649b07 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -25,10 +25,10 @@ namespace gtsam { static double intNoiseVar = 0.0000001; static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; -ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate( +PreintegratedImuMeasurements ScenarioRunner::integrate( double T, const imuBias::ConstantBias& estimatedBias, bool corrupted) const { - ImuFactor::PreintegratedMeasurements pim(p_, estimatedBias); + PreintegratedImuMeasurements pim(p_, estimatedBias); const double dt = imuSampleTime(); const size_t nrSteps = T / dt; @@ -45,7 +45,7 @@ ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate( } NavState ScenarioRunner::predict( - const ImuFactor::PreintegratedMeasurements& pim, + const PreintegratedImuMeasurements& pim, const imuBias::ConstantBias& estimatedBias) const { const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0)); return pim.predict(state_i, estimatedBias); diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 3f950d42c..c2e62384f 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -28,8 +28,7 @@ namespace gtsam { */ class ScenarioRunner { public: - typedef boost::shared_ptr - SharedParams; + typedef boost::shared_ptr SharedParams; ScenarioRunner(const Scenario* scenario, const SharedParams& p, double imuSampleTime = 1.0 / 100.0, const imuBias::ConstantBias& bias = imuBias::ConstantBias()) @@ -69,19 +68,18 @@ class ScenarioRunner { const double& imuSampleTime() const { return imuSampleTime_; } /// Integrate measurements for T seconds into a PIM - ImuFactor::PreintegratedMeasurements integrate( + PreintegratedImuMeasurements integrate( double T, const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias(), bool corrupted = false) const; /// Predict predict given a PIM - NavState predict(const ImuFactor::PreintegratedMeasurements& pim, + NavState predict(const PreintegratedImuMeasurements& pim, const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias()) const; /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately - Matrix6 poseCovariance( - const ImuFactor::PreintegratedMeasurements& pim) const { + Matrix6 poseCovariance(const PreintegratedImuMeasurements& pim) const { Matrix9 cov = pim.preintMeasCov(); Matrix6 poseCov; poseCov << cov.block<3, 3>(0, 0), cov.block<3, 3>(0, 3), // diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index d473207da..3d2a20915 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -44,10 +44,10 @@ namespace { // Auxiliary functions to test preintegrated Jacobians // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ /* ************************************************************************* */ -CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements( +PreintegratedCombinedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, const list& deltaTs) { - CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, I_3x3, + PreintegratedCombinedMeasurements result(bias, I_3x3, I_3x3, I_3x3, I_3x3, I_3x3, I_6x6); list::const_iterator itAcc = measuredAccs.begin(); @@ -94,12 +94,13 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) { double deltaT = 0.5; double tol = 1e-6; + auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81); + // Actual preintegrated values - ImuFactor::PreintegratedMeasurements expected1(bias, Z_3x3, Z_3x3, Z_3x3); + PreintegratedImuMeasurements expected1(p, bias); expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6); + PreintegratedCombinedMeasurements actual1(p, bias); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -119,38 +120,33 @@ TEST( CombinedImuFactor, ErrorWithBiases ) { Point3(5.5, 1.0, -50.0)); Vector3 v2(0.5, 0.0, 0.0); + auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81); + p->omegaCoriolis = Vector3(0,0.1,0.1); + PreintegratedImuMeasurements pim( + p, imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); + // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0.1, 0.1; Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0 + 0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() - + Vector3(0.2, 0.0, 0.0); + Vector3 measuredAcc = + x1.rotation().unrotate(-p->n_gravity) + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; double tol = 1e-6; - ImuFactor::PreintegratedMeasurements pim( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - I_3x3, I_3x3, I_3x3); - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedImuFactor::CombinedPreintegratedMeasurements combined_pim( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - I_3x3, I_3x3, I_3x3, I_3x3, 2 * I_3x3, I_6x6); + PreintegratedCombinedMeasurements combined_pim(p, + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim, gravity, - omegaCoriolis); + ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim); noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov()); CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), - combined_pim, gravity, omegaCoriolis); + combined_pim); Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias); Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias, @@ -197,7 +193,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { } // Actual preintegrated values - CombinedImuFactor::CombinedPreintegratedMeasurements pim = + PreintegratedCombinedMeasurements pim = evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); @@ -236,19 +232,16 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { TEST(CombinedImuFactor, PredictPositionAndVelocity) { imuBias::ConstantBias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) + auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81); + // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0, 0; Vector3 measuredOmega; measuredOmega << 0, 0.1, 0; //M_PI/10.0+0.3; Vector3 measuredAcc; measuredAcc << 0, 1.1, -9.81; double deltaT = 0.001; - CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3, - I_3x3, I_3x3, 2 * I_3x3, I_6x6); + PreintegratedCombinedMeasurements pim(p, bias); for (int i = 0; i < 1000; ++i) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -256,48 +249,39 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity) { // Create factor noiseModel::Gaussian::shared_ptr combinedmodel = noiseModel::Gaussian::Covariance(pim.preintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim, - gravity, omegaCoriolis); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocityBias = pim.predict(x1, v1, bias, gravity, - omegaCoriolis); + NavState actual = pim.predict(NavState(), bias); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity << 0, 1, 0; - EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); + EXPECT(assert_equal(expectedPose, actual.pose())); EXPECT( - assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); + assert_equal(Vector(expectedVelocity), Vector(actual.velocity()))); } /* ************************************************************************* */ TEST(CombinedImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) - CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3, - I_3x3, I_3x3, 2 * I_3x3, I_6x6); + auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81); + PreintegratedCombinedMeasurements pim(p, bias); Vector3 measuredAcc; measuredAcc << 0, 0, -9.81; - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0, 0; Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0; double deltaT = 0.001; double tol = 1e-4; for (int i = 0; i < 1000; ++i) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim, - gravity, omegaCoriolis); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); // Predict Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2; Vector3 v(0, 0, 0), v2; - CombinedImuFactor::Predict(x, v, x2, v2, bias, pim, gravity, omegaCoriolis); + NavState actual = pim.predict(NavState(x, v), bias); Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); - EXPECT(assert_equal(expectedPose, x2, tol)); + EXPECT(assert_equal(expectedPose, actual.pose(), tol)); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 878fc9f58..8a48acfc5 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -135,11 +135,10 @@ TEST(ImuFactor, Accelerating) { Vector3(a, 0, 0)); const double T = 3.0; // seconds - ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma, - kZeroBias, kGravityAlongNavZDown); + ScenarioRunner runner(&scenario, defaultParams(), T / 10); - ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); + PreintegratedImuMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); @@ -149,8 +148,8 @@ TEST(ImuFactor, Accelerating) { boost::function f = boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, T/10, boost::none, boost::none, boost::none); - const Vector3 measuredAcc = runner.actual_specific_force_b(T); - const Vector3 measuredOmega = runner.actual_omega_b(T); + const Vector3 measuredAcc = runner.actualSpecificForce(T); + const Vector3 measuredOmega = runner.actualAngularVelocity(T); pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1, aG2); EXPECT(assert_equal( numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7)); @@ -332,8 +331,11 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; + auto p = defaultParams(); + p->omegaCoriolis = kNonZeroOmegaCoriolis; + imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)); - PreintegratedImuMeasurements pim(defaultParams(), biasHat); + PreintegratedImuMeasurements pim(p, biasHat); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Make sure of biasCorrectedDelta @@ -345,8 +347,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { EXPECT(assert_equal(expectedH, actualH)); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, - kNonZeroOmegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); Values values; values.insert(X(1), x1); @@ -376,14 +377,17 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) { + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - PreintegratedImuMeasurements pim(defaultParams(), + auto p = defaultParams(); + p->omegaCoriolis = kNonZeroOmegaCoriolis; + p->use2ndOrderCoriolis = true; + + PreintegratedImuMeasurements pim(p, imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1))); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor bool use2ndOrderCoriolis = true; - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, - kNonZeroOmegaCoriolis, boost::none, use2ndOrderCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); Values values; values.insert(X(1), x1); @@ -472,8 +476,6 @@ TEST(ImuFactor, fistOrderExponential) { /* ************************************************************************* */ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); - // Measurements list measuredAccs, measuredOmegas; list deltaTs; @@ -544,11 +546,16 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { const AcceleratingScenario scenario(nRb, p1, v1, a, Vector3(0, 0, M_PI / 10.0 + 0.3)); - const double T = 3.0; // seconds - ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma, - kZeroBias, kGravityAlongNavZDown); + auto p = defaultParams(); + p->body_P_sensor =Pose3(Rot3::Expmap(Vector3(0, M_PI/2, 0)), Point3(0.1, 0.05, 0.01)); + p->omegaCoriolis = kNonZeroOmegaCoriolis; - // ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); + + const double T = 3.0; // seconds + ScenarioRunner runner(&scenario, p, T / 10); + + // PreintegratedImuMeasurements pim = runner.integrate(T); // EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); // // Matrix6 estimatedCov = runner.estimatePoseCovariance(T); @@ -558,18 +565,13 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { Pose3 x1(nRb, p1); // Measurements - Vector3 measuredOmega = runner.actual_omega_b(0); - Vector3 measuredAcc = runner.actual_specific_force_b(0); - - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, M_PI/2, 0)), Point3(0.1, 0.05, 0.01)); - imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); + Vector3 measuredOmega = runner.actualAngularVelocity(0); + Vector3 measuredAcc = runner.actualSpecificForce(0); // Get mean prediction from "ground truth" measurements - const Vector3 accNoiseVar2(0.01, 0.02, 0.03); - const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02); - PreintegratedImuMeasurements pim(biasHat, accNoiseVar2.asDiagonal(), - omegaNoiseVar2.asDiagonal(), Z_3x3, true); // MonteCarlo does not sample integration noise - pim.set_body_P_sensor(body_P_sensor); + const Vector3 accNoiseVar2(0.01, 0.02, 0.03); + const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02); + PreintegratedImuMeasurements pim(p, biasHat); // Check updatedDeltaXij derivatives Matrix3 D_correctedAcc_measuredOmega = Matrix3::Zero(); @@ -601,12 +603,11 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { // integrate at least twice to get position information // otherwise factor cov noise from preint_cov is not positive definite - pim.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor); - pim.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor); + pim.integrateMeasurement(measuredAcc, measuredOmega, dt); + pim.integrateMeasurement(measuredAcc, measuredOmega, dt); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, - kNonZeroOmegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), Point3(5.5, 1.0, -50.0)); @@ -690,10 +691,9 @@ TEST(ImuFactor, PredictArbitrary) { Vector3(M_PI / 10, M_PI / 10, M_PI / 10)); const double T = 3.0; // seconds - ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma, - kZeroBias, kGravityAlongNavZDown); + ScenarioRunner runner(&scenario, defaultParams(), T / 10); // - // ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + // PreintegratedImuMeasurements pim = runner.integrate(T); // EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); // // Matrix6 estimatedCov = runner.estimatePoseCovariance(T); @@ -703,12 +703,12 @@ TEST(ImuFactor, PredictArbitrary) { imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); // Measurements - Vector3 measuredOmega = runner.actual_omega_b(0); - Vector3 measuredAcc = runner.actual_specific_force_b(0); + Vector3 measuredOmega = runner.actualAngularVelocity(0); + Vector3 measuredAcc = runner.actualSpecificForce(0); auto p = defaultParams(); p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise - ImuFactor::PreintegratedMeasurements pim(p, biasHat); + PreintegratedImuMeasurements pim(p, biasHat); imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega, // Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000)); @@ -738,10 +738,12 @@ TEST(ImuFactor, PredictArbitrary) { TEST(ImuFactor, bodyPSensorNoBias) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) + // Rotate sensor (z-down) to body (same as navigation) i.e. z-up + auto p = defaultParams(); + p->n_gravity = Vector3(0, 0, -kGravity); // z-up nav frame + p->body_P_sensor = Pose3(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0)); + // Measurements - Vector3 n_gravity(0, 0, -kGravity); // z-up nav frame - Vector3 omegaCoriolis(0, 0, 0); - // Sensor frame is z-down // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame Vector3 s_omegaMeas_ns(0, 0.1, M_PI / 10); // Acc measurement is acceleration of sensor in the sensor frame, when stationary, @@ -749,28 +751,22 @@ TEST(ImuFactor, bodyPSensorNoBias) { Vector3 s_accMeas(0, 0, -kGravity); double dt = 0.001; - // Rotate sensor (z-down) to body (same as navigation) i.e. z-up - Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0)); - - ImuFactor::PreintegratedMeasurements pim(bias, Z_3x3, Z_3x3, Z_3x3, true); + PreintegratedImuMeasurements pim(p, bias); for (int i = 0; i < 1000; ++i) - pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt, body_P_sensor); + pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, n_gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); // Predict - Pose3 x1; - Vector3 v1(0, 0, 0); - PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, n_gravity, - omegaCoriolis); + NavState actual = pim.predict(NavState(), bias); Pose3 expectedPose(Rot3().ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0)); - EXPECT(assert_equal(expectedPose, poseVelocity.pose)); + EXPECT(assert_equal(expectedPose, actual.pose())); Vector3 expectedVelocity(0, 0, 0); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity()))); } /* ************************************************************************* */ @@ -791,9 +787,6 @@ TEST(ImuFactor, bodyPSensorWithBias) { SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma); // Measurements - Vector3 n_gravity(0, 0, -kGravity); - Vector3 omegaCoriolis(0, 0, 0); - // Sensor frame is z-down // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame Vector3 measuredOmega(0, 0.01, 0); @@ -801,11 +794,12 @@ TEST(ImuFactor, bodyPSensorWithBias) { // table exerts an equal and opposite force w.r.t gravity Vector3 measuredAcc(0, 0, -kGravity); - Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3()); - - Matrix3 accCov = 1e-7 * I_3x3; - Matrix3 gyroCov = 1e-8 * I_3x3; - Matrix3 integrationCov = 1e-9 * I_3x3; + auto p = defaultParams(); + p->n_gravity = Vector3(0, 0, -kGravity); + p->body_P_sensor = Pose3(Rot3::ypr(0, 0, M_PI), Point3()); + p->accelerometerCovariance = 1e-7 * I_3x3; + p->gyroscopeCovariance = 1e-8 * I_3x3; + p->integrationCovariance = 1e-9 * I_3x3; double deltaT = 0.005; // Specify noise values on priors @@ -842,17 +836,13 @@ TEST(ImuFactor, bodyPSensorWithBias) { // Now add IMU factors and bias noise models Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0)); for (int i = 1; i < numFactors; i++) { - ImuFactor::PreintegratedMeasurements pim = - ImuFactor::PreintegratedMeasurements(priorBias, accCov, gyroCov, - integrationCov, true); + PreintegratedImuMeasurements pim = + PreintegratedImuMeasurements(p, priorBias); for (int j = 0; j < 200; ++j) - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT, - body_P_sensor); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factors - graph.add( - ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim, n_gravity, - omegaCoriolis)); + graph.add(ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim)); graph.add(BetweenFactor(B(i - 1), B(i), zeroBias, biasNoiseModel)); values.insert(X(i), Pose3()); diff --git a/gtsam/navigation/tests/testPoseVelocityBias.cpp b/gtsam/navigation/tests/testPoseVelocityBias.cpp index ce419fdd0..877769c2e 100644 --- a/gtsam/navigation/tests/testPoseVelocityBias.cpp +++ b/gtsam/navigation/tests/testPoseVelocityBias.cpp @@ -25,6 +25,8 @@ using namespace std; using namespace gtsam; +#ifdef ALLOW_DEPRECATED_IN_GTSAM4 + // Should be seen as between(pvb1,pvb2), i.e., written as pvb2 \omin pvb1 Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) { Matrix3 R1 = pvb1.pose.rotation().matrix(); @@ -55,6 +57,7 @@ TEST(PoseVelocityBias, error) { expected << 0.0, -0.5, 6.0, 4.0, 0.0, 3.0, 0.1, 0.2, 0.3; EXPECT(assert_equal(expected, actual, 1e-9)); } +#endif /* ************************************************************************************************/ int main() { diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index d48856214..a6aa80b71 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -51,7 +51,7 @@ TEST(ScenarioRunner, Forward) { ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); const double T = 0.1; // seconds - ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + PreintegratedImuMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); @@ -64,7 +64,7 @@ TEST(ScenarioRunner, ForwardWithBias) { ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); const double T = 0.1; // seconds - ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias); + PreintegratedImuMeasurements pim = runner.integrate(T, kNonZeroBias); Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); } @@ -78,7 +78,7 @@ TEST(ScenarioRunner, Circle) { ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); const double T = 0.1; // seconds - ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + PreintegratedImuMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); @@ -95,7 +95,7 @@ TEST(ScenarioRunner, Loop) { ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); const double T = 0.1; // seconds - ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + PreintegratedImuMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); @@ -126,7 +126,7 @@ TEST(ScenarioRunner, Accelerating) { using namespace accelerating; ScenarioRunner runner(&scenario, defaultParams(), T / 10); - ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + PreintegratedImuMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); @@ -140,7 +140,7 @@ TEST(ScenarioRunner, Accelerating) { // ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma, // kNonZeroBias); // -// ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, +// PreintegratedImuMeasurements pim = runner.integrate(T, // kNonZeroBias); // Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000, // kNonZeroBias); @@ -157,7 +157,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating) { const double T = 3; // seconds ScenarioRunner runner(&scenario, defaultParams(), T / 10); - ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + PreintegratedImuMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T);