My Project 1.0
Holistic Fusion: A Generic Framework for Multimodal Sensor Fusion
Loading...
Searching...
No Matches
NavState.cpp
Go to the documentation of this file.
1/*
2Copyright 2023 by Julian Nubert, Robotic Systems Lab, ETH Zurich.
3All rights reserved.
4This file is released under the "BSD-3-Clause License".
5Please see the LICENSE file that has been included as part of this package.
6 */
7
8// Implementation
10
11// GTSAM
12#include <gtsam/geometry/Pose3.h>
13
14namespace holistic_fusion {
15
16// NavState -------------------------------------------------------------------
17void NavState::updateInWorld(const Eigen::Isometry3d& T_W_Ik_new, const Eigen::Vector3d& I_v_W_I, const Eigen::Vector3d& I_w_W_I,
18 const double timeK) {
19 // Transformation
20 T_W_Ik_ = T_W_Ik_new;
21
22 // Velocities in body frame
23 I_v_W_I_ = I_v_W_I;
24 I_w_W_I_ = I_w_W_I;
25
26 // Time
27 timeK_ = timeK;
28
29 // // Poses
30 // // Sanity check --> Make sure that roll and pitch is in odometry frame and not world to odometry frame
31 // gtsam::Pose3 T_W_O_gtsam(getT_W_O().matrix());
32 // if (T_W_O_gtsam.rotation().roll() > 1e-2 || T_W_O_gtsam.rotation().pitch() > 1e-2) {
33 // std::cout << YELLOW_START << "GMsf" << RED_START << " T_W_O roll: " << T_W_O_gtsam.rotation().roll()
34 // << ", pitch: " << T_W_O_gtsam.rotation().pitch() << ", which is supposed to be close to zero!" << COLOR_END << std::endl;
35 // }
36 // // Always update roll and pitch in odometry frame
37 // gtsam::Pose3 T_W_Ik_new_gtsam(T_W_Ik_new.matrix());
38 // double globalRoll = T_W_Ik_new_gtsam.rotation().roll();
39 // double globalPitch = T_W_Ik_new_gtsam.rotation().pitch();
40 // // Decide on relocalization
41 // if (odomNotJump) {
42 // std::cout << YELLOW_START << "GMsf" << GREEN_START << " Relocalization is needed. Publishing to world->map." << COLOR_END <<
43 // std::endl;
44 // // For this computation step assume T_O_Ik ~ T_O_Ikm1 --> there will be no robot translation in the odometry frame
45 // gtsam::Rot3 R_O_Ik_gravityAligned(T_O_Ik_gravityAligned_.rotation().matrix());
46 // R_O_Ik_gravityAligned = gtsam::Rot3::Ypr(R_O_Ik_gravityAligned.yaw(), globalPitch, globalRoll);
47 // T_O_Ik_gravityAligned_.matrix().block<3, 3>(0, 0) = R_O_Ik_gravityAligned.matrix();
48 // T_W_M_ = T_W_Ik_new * T_O_Ik_gravityAligned_.inverse() * T_M_O_.inverse();
49 // } else {
50 // T_O_Ik_gravityAligned_ = T_M_O_.inverse() * T_W_M_.inverse() * T_W_Ik_new;
51 // }
52}
53
54void NavState::updateOrientationInWorld(const Eigen::Matrix3d& R_W_Ik) {
55 T_W_Ik_.linear() = R_W_Ik;
56}
57
58void NavState::updatePositionInWorld(const Eigen::Vector3d& W_t_W_Ik) {
59 T_W_Ik_.translation() = W_t_W_Ik;
60}
61
63 timeK_ = timeK;
64}
65
66// SafeIntegratedNavState -----------------------------------------------------------------------------------------------
67void SafeIntegratedNavState::update(const Eigen::Isometry3d& T_W_O, const Eigen::Isometry3d& T_O_Ik_gravityAligned,
68 const Eigen::Vector3d& I_v_W_I, const Eigen::Vector3d& I_w_W_I, const double timeK,
69 const bool odomNotJump) {
70 // Mutex for safety
71 std::lock_guard<std::mutex> updateLock(stateUpdateMutex_);
72 // Assign
73 T_W_Ik_ = T_W_O * T_O_Ik_gravityAligned;
74 T_W_O_ = T_W_O;
75 T_O_Ik_gravityAligned_ = T_O_Ik_gravityAligned;
76 I_v_W_I_ = I_v_W_I;
77 I_w_W_I_ = I_w_W_I;
78 timeK_ = timeK;
79}
80
81void SafeIntegratedNavState::updateInWorld(const Eigen::Isometry3d& T_W_Ik, const Eigen::Vector3d& I_v_W_I, const Eigen::Vector3d& I_w_W_I,
82 const double timeK, const bool odomNotJump) {
83 // Mutex for safety
84 std::lock_guard<std::mutex> updateLock(stateUpdateMutex_);
85 // Parent class
86 NavState::updateInWorld(T_W_Ik, I_v_W_I, I_w_W_I, timeK);
88}
89
90void SafeIntegratedNavState::updateOrientationInWorld(const Eigen::Matrix3d& R_W_Ik, const bool odomNotJump) {
91 // Mutex for safety
92 std::lock_guard<std::mutex> updateLock(stateUpdateMutex_);
93 // Parent class
95 // Update Variables of this child class
96 if (odomNotJump) {
98 } else {
100 }
101}
102
103void SafeIntegratedNavState::updatePositionInWorld(const Eigen::Vector3d& W_t_W_Ik, const bool odomNotJump) {
104 // Mutex for safety
105 std::lock_guard<std::mutex> updateLock(stateUpdateMutex_);
106 // Parent class
108 // Update Variables of this child class
109 if (odomNotJump) {
111 } else {
113 }
114}
115
117 // Mutex for safety
118 std::lock_guard<std::mutex> updateLock(stateUpdateMutex_);
119 // Parent class
121}
122
123} // namespace holistic_fusion
virtual void updateLatestMeasurementTimestamp(double timeK)
Definition NavState.cpp:62
Eigen::Vector3d I_v_W_I_
Definition NavState.h:54
void updateOrientationInWorld(const Eigen::Matrix3d &R_W_Ik)
Definition NavState.cpp:54
void updatePositionInWorld(const Eigen::Vector3d &W_t_W_Ik)
Definition NavState.cpp:58
Eigen::Isometry3d T_W_Ik_
Definition NavState.h:52
Eigen::Vector3d I_w_W_I_
Definition NavState.h:55
void updateInWorld(const Eigen::Isometry3d &T_W_Ik, const Eigen::Vector3d &I_v_W_I, const Eigen::Vector3d &I_w_W_I, double timeK)
Definition NavState.cpp:17
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)
Definition NavState.cpp:67
void updateOrientationInWorld(const Eigen::Matrix3d &R_W_Ik, bool odomNotJump)
Definition NavState.cpp:90
void updateLatestMeasurementTimestamp(const double timeK) override
Definition NavState.cpp:116
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)
Definition NavState.cpp:81
void updatePositionInWorld(const Eigen::Vector3d &W_t_W_Ik, bool odomNotJump)
Definition NavState.cpp:103