12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix ImuFactor examples to show correct prediction values

release/4.3a0
Varun Agrawal 2023-03-01 15:40:31 -05:00
parent 2e1c51478c
commit e4b1505e38
2 changed files with 17 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
python/gtsam/examples/CombinedImuFactorExample.py
View File

@ -5,7 +5,7 @@ All Rights Reserved
See LICENSE for the license information See LICENSE for the license information
A script validating and demonstrating the CobiendImuFactor inference. A script validating and demonstrating inference with the CombinedImuFactor.
Author: Varun Agrawal Author: Varun Agrawal
""" """
@ -17,15 +17,15 @@ from __future__ import print_function
import argparse import argparse
import math import math
import gtsam
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import numpy as np import numpy as np
from gtsam.symbol_shorthand import B, V, X from gtsam.symbol_shorthand import B, V, X
from gtsam.utils.plot import plot_pose3 from gtsam.utils.plot import plot_pose3
from mpl_toolkits.mplot3d import Axes3D from mpl_toolkits.mplot3d import Axes3D
from PreintegrationExample import POSES_FIG, PreintegrationExample from PreintegrationExample import POSES_FIG, PreintegrationExample
import gtsam
GRAVITY = 9.81 GRAVITY = 9.81
np.set_printoptions(precision=3, suppress=True) np.set_printoptions(precision=3, suppress=True)
@ -192,7 +192,9 @@ class CombinedImuFactorExample(PreintegrationExample):
if verbose: if verbose:
print(factor) print(factor)
print(pim.predict(initial_state_i, self.actualBias)) print("Predicted state at {0}:\n{1}".format(
t + self.dt,
pim.predict(initial_state_i, self.actualBias)))
pim.resetIntegration() pim.resetIntegration()
@ -204,6 +206,9 @@ class CombinedImuFactorExample(PreintegrationExample):
print("Actual state at {0}:\n{1}".format( print("Actual state at {0}:\n{1}".format(
t + self.dt, actual_state_i)) t + self.dt, actual_state_i))
# Set initial state to current
initial_state_i = actual_state_i
noisy_state_i = gtsam.NavState( noisy_state_i = gtsam.NavState(
actual_state_i.pose().compose(poseNoise), actual_state_i.pose().compose(poseNoise),
actual_state_i.velocity() + np.random.randn(3) * 0.1) actual_state_i.velocity() + np.random.randn(3) * 0.1)

11
python/gtsam/examples/ImuFactorExample.py
View File

@ -17,15 +17,15 @@ from __future__ import print_function
import argparse import argparse
import math import math
import gtsam
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import numpy as np import numpy as np
from gtsam.symbol_shorthand import B, V, X from gtsam.symbol_shorthand import B, V, X
from gtsam.utils.plot import plot_pose3 from gtsam.utils.plot import plot_pose3
from mpl_toolkits.mplot3d import Axes3D from mpl_toolkits.mplot3d import Axes3D
from PreintegrationExample import POSES_FIG, PreintegrationExample from PreintegrationExample import POSES_FIG, PreintegrationExample
import gtsam
BIAS_KEY = B(0) BIAS_KEY = B(0)
GRAVITY = 9.81 GRAVITY = 9.81
@ -185,7 +185,9 @@ class ImuFactorExample(PreintegrationExample):
if verbose: if verbose:
print(factor) print(factor)
print(pim.predict(initial_state_i, self.actualBias)) print("Predicted state at {0}:\n{1}".format(
t + self.dt,
pim.predict(initial_state_i, self.actualBias)))
pim.resetIntegration() pim.resetIntegration()
@ -197,6 +199,9 @@ class ImuFactorExample(PreintegrationExample):
print("Actual state at {0}:\n{1}".format( print("Actual state at {0}:\n{1}".format(
t + self.dt, actual_state_i)) t + self.dt, actual_state_i))
# Set initial state to current
initial_state_i = actual_state_i
noisy_state_i = gtsam.NavState( noisy_state_i = gtsam.NavState(
actual_state_i.pose().compose(poseNoise), actual_state_i.pose().compose(poseNoise),
actual_state_i.velocity() + np.random.randn(3) * 0.1) actual_state_i.velocity() + np.random.randn(3) * 0.1)