12#include <gtsam/geometry/Pose3.h>
17void NavState::updateInWorld(
const Eigen::Isometry3d& T_W_Ik_new,
const Eigen::Vector3d& I_v_W_I,
const Eigen::Vector3d& I_w_W_I,
59 T_W_Ik_.translation() = W_t_W_Ik;
68 const Eigen::Vector3d& I_v_W_I,
const Eigen::Vector3d& I_w_W_I,
const double timeK,
69 const bool odomNotJump) {
73 T_W_Ik_ = T_W_O * T_O_Ik_gravityAligned;
82 const double timeK,
const bool odomNotJump) {
virtual void updateLatestMeasurementTimestamp(double timeK)
void updateOrientationInWorld(const Eigen::Matrix3d &R_W_Ik)
void updatePositionInWorld(const Eigen::Vector3d &W_t_W_Ik)
Eigen::Isometry3d T_W_Ik_
void updateInWorld(const Eigen::Isometry3d &T_W_Ik, const Eigen::Vector3d &I_v_W_I, const Eigen::Vector3d &I_w_W_I, double timeK)
Eigen::Isometry3d T_O_Ik_gravityAligned_
void update(const Eigen::Isometry3d &T_W_O, const Eigen::Isometry3d &T_O_Ik_gravityAligned, const Eigen::Vector3d &I_v_W_I, const Eigen::Vector3d &I_w_W_I, double timeK, bool odomNotJump)
void updateOrientationInWorld(const Eigen::Matrix3d &R_W_Ik, bool odomNotJump)
void updateLatestMeasurementTimestamp(const double timeK) override
void updateInWorld(const Eigen::Isometry3d &T_W_Ik, const Eigen::Vector3d &I_v_W_I, const Eigen::Vector3d &I_w_W_I, const double timeK, bool odomNotJump)
std::mutex stateUpdateMutex_
void updatePositionInWorld(const Eigen::Vector3d &W_t_W_Ik, bool odomNotJump)