V3 VIO correction (#1346)

Author: Serafadam

src/basalt/BasaltVIO.cpp

@@ -31,12 +31,12 @@ void BasaltVIO::buildInternal() {
     imu.addCallback(std::bind(&BasaltVIO::imuCB, this, std::placeholders::_1));
 
     basalt::PoseState<double>::SE3 initTrans(Eigen::Quaterniond::Identity(), Eigen::Vector3d(0, 0, 0));
-    Eigen::Matrix<double, 3, 3> R180;
-    R180 << -1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0;
-    Eigen::Quaterniond q180(R180);
-    basalt::PoseState<double>::SE3 opticalTransform180(q180, Eigen::Vector3d(0, 0, 0));
+    Eigen::Matrix<double, 3, 3> R;
+    R << 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0;
+    Eigen::Quaterniond q(R);
+    basalt::PoseState<double>::SE3 initialRotation(q, Eigen::Vector3d(0, 0, 0));
     // to output pose in FLU world coordinates
-    localTransform = std::make_shared<basalt::PoseState<double>::SE3>(initTrans * opticalTransform180.inverse());
+    localTransform = std::make_shared<basalt::PoseState<double>::SE3>(initTrans * initialRotation.inverse());
     setDefaultVIOConfig();
 }