Read aprilrobotics rotation in row-major order
Before we were getting the transpose by reading it in column-major
order.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: If2c920663edbccf40711e092b29b767cbddb578a
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 4c07034..03e7f7a 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -103,7 +103,9 @@
const auto position_offset =
frc971::vision::CreatePosition(*fbb, T.x(), T.y(), T.z());
- const auto orientation = Eigen::Quaterniond(Eigen::Matrix3d(pose.R->data));
+ // Aprilrobotics stores the rotation matrix in row-major order
+ const auto orientation = Eigen::Quaterniond(
+ Eigen::Matrix<double, 3, 3, Eigen::RowMajor>(pose.R->data));
const auto orientation_offset = frc971::vision::CreateQuaternion(
*fbb, orientation.w(), orientation.x(), orientation.y(), orientation.z());