trying to fix transform issue

SEESAI · May 14, 2024 · 07fcc14 · 07fcc14
1 parent 5d76ad8
commit 07fcc14
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/AirLib/include/sensors/imu/ImuSimple.hpp b/AirLib/include/sensors/imu/ImuSimple.hpp
@@ -67,8 +67,11 @@ namespace airlib
             output.orientation = ground_truth.kinematics->pose.orientation;
 
             //acceleration is in world frame so transform to body frame
-//            output.linear_acceleration = VectorMath::rotateVectorReverse(output.linear_acceleration, ground_truth.kinematics->pose.orientation, true);
-            //VectorMath::transformToBodyFrame(output.linear_acceleration, ground_truth.kinematics->pose.orientation, true);
+            std::cout << "X accel: untransformed = " << output.linear_acceleration.x();
+            output.linear_acceleration = VectorMath::transformToBodyFrame(output.linear_acceleration,
+                                                                          ground_truth.kinematics->pose.orientation,
+                                                                          true);
+            std::cout << ", transformed = " << output.linear_acceleration.x() << "\n";
 
             //add noise
             addNoise(output.linear_acceleration, output.angular_velocity);