remodify the bug for 'chang_x()' functions

Huguet57 · Apr 4, 2021 · 90e5408 · 90e5408
1 parent a885ffe
commit 90e5408
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 6 deletions.
diff --git a/IKFoM_toolkit/esekfom/esekfom.hpp b/IKFoM_toolkit/esekfom/esekfom.hpp
@@ -1795,9 +1795,12 @@ class esekf{
 	{
 		x_ = input_state;
 
-		x_.build_S2_state();
-		x_.build_SO3_state();
-		x_.build_vect_state();
+		if((!x_.vect_state.size())&&(!x_.SO3_state.size())&&(!x_.S2_state.size()))
+		{
+			x_.build_S2_state();
+			x_.build_SO3_state();
+			x_.build_vect_state();
+		}
 	}
 
 	void change_P(cov &input_cov)

diff --git a/Samples/FAST_LIO-stable/include/esekfom/esekfom.hpp b/Samples/FAST_LIO-stable/include/esekfom/esekfom.hpp
@@ -1794,9 +1794,12 @@ class esekf{
 	void change_x(state &input_state)
 	{
 		x_ = input_state;
-		x_.build_S2_state();
-		x_.build_SO3_state();
-		x_.build_vect_state();
+		if((!x_.vect_state.size())&&(!x_.SO3_state.size())&&(!x_.S2_state.size()))
+		{
+			x_.build_S2_state();
+			x_.build_SO3_state();
+			x_.build_vect_state();
+		}
 	}
 
 	void change_P(cov &input_cov)