From 6f3446dbd0dc9a30c023f8524140a964e9d91336 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 7 Mar 2024 14:37:03 -0500 Subject: [PATCH] Make No Sense -YoungBoy Never Broke again --- src/teleoperation/lander_align/lander_align.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 7f918795a..a3591d174 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -226,15 +226,21 @@ namespace mrover { rot << n.x(),0,0, n.y(),0,1, n.z(),1,0; - std::cout << "rot matrix " << rot << std::endl; + std::cout << "rot matrix 2 s" << std::endl << rot << std::endl; + Eigen::HouseholderQR qr{rot}; Eigen::Matrix3d q = qr.householderQ(); rot.col(0) = q.col(0); rot.col(1) = q.col(2); rot.col(2) = q.col(1); + std::cout << "rot matrix " << std::endl << rot << std::endl; + rot << 1,0,0, + 0,-1,0, + 0,0,1; + } - //Calculate the plane location in the world frame` + //Calculate the plane location in the world frame manif::SE3d plane_loc_in_world = {{mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}}; plane_loc_in_world = zedToMap * plane_loc_in_world; manif::SE3d plane_loc_in_world_final = {{plane_loc_in_world.translation().x(),plane_loc_in_world.translation().y(),plane_loc_in_world.translation().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};