Merge remote-tracking branch 'origin/feature/ImuFactorPush2' into manifold
Conflicts: gtsam/navigation/ScenarioRunner.cpp gtsam/navigation/ScenarioRunner.h gtsam/navigation/tests/testScenarioRunner.cpprelease/4.3a0
Frank Dellaert
commit
1790d1dc7e
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -133,6 +133,17 @@ TEST (Point3, distance) {
|
|||
EXPECT(assert_equal(numH2, H2, 1e-8));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Point3, cross) {
|
||||
Matrix aH1, aH2;
|
||||
boost::function<Point3(const Point3&, const Point3&)> 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;
|
||||
|
|
|
|||
|
|
@ -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<Rot3, Vector3>(
|
||||
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<Rot3, Vector3>(
|
||||
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); };
|
||||
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@ namespace gtsam {
|
|||
class AggregateImuReadings {
|
||||
public:
|
||||
typedef imuBias::ConstantBias Bias;
|
||||
typedef ImuFactor::PreintegratedMeasurements::Params Params;
|
||||
typedef PreintegrationBase::Params Params;
|
||||
|
||||
private:
|
||||
const boost::shared_ptr<Params> p_;
|
||||
|
|
|
|||
|
|
@ -278,10 +278,10 @@ public:
|
|||
boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
|
||||
boost::none, boost::optional<Matrix&> 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,
|
||||
|
|
|
|||
|
|
@ -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<Pose3>& 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<PreintegratedMeasurements::Params> p = boost::make_shared<
|
||||
PreintegratedMeasurements::Params>(pim.p());
|
||||
boost::shared_ptr<PreintegratedImuMeasurements::Params> 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
|
||||
|
|
|
|||
|
|
@ -223,6 +223,7 @@ public:
|
|||
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
|
||||
boost::none, boost::optional<Matrix&> 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:
|
||||
|
||||
|
|
|
|||
|
|
@ -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<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
|
||||
CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, I_3x3,
|
||||
PreintegratedCombinedMeasurements result(bias, I_3x3,
|
||||
I_3x3, I_3x3, I_3x3, I_3x3, I_6x6);
|
||||
|
||||
list<Vector3>::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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -135,22 +135,21 @@ 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));
|
||||
AggregateImuReadings 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));
|
||||
Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||
|
||||
// Check G1 and G2 derivatives of pim.update
|
||||
Matrix93 aG1,aG2;
|
||||
boost::function<NavState(const Vector3&, const Vector3&)> 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<Vector3> measuredAccs, measuredOmegas;
|
||||
list<double> 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<Bias>(B(i - 1), B(i), zeroBias, biasNoiseModel));
|
||||
|
||||
values.insert(X(i), Pose3());
|
||||
|
|
|
|||
|
|
@ -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() {
|
||||
|
|
|
|||
Loading…
Reference in New Issue