Skip to content

Commit

Permalink
Not as cooked
Browse files Browse the repository at this point in the history
  • Loading branch information
jbrhm committed Mar 7, 2024
1 parent a5fcd3f commit ace3eca
Showing 1 changed file with 2 additions and 2 deletions.
4 changes: 2 additions & 2 deletions src/teleoperation/lander_align/lander_align.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -236,12 +236,12 @@ namespace mrover {
}
//Calculate the plane location in the world frame`
manif::SE3d plane_loc_in_world = {{mBestLocationInWorld.value().x(), mBestLocationInWorld.value().y(), mBestLocationInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};
mBestLocationInWorld = std::make_optional<Eigen::Vector3d>(zedToMap * plane_loc_in_world);
plane_loc_in_world = zedToMap * plane_loc_in_world;
plane_loc_in_world.rotation().matrix() = rot;

//Calculate the offset location in the world frame
manif::SE3d offset_loc_in_world = {{mBestOffsetInWorld.value().x(), mBestOffsetInWorld.value().y(), mBestOffsetInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};
mBestOffsetInWorld = std::make_optional<Eigen::Vector3d>(zedToMap * offset_loc_in_world);
offset_loc_in_world = zedToMap * offset_loc_in_world;
offset_loc_in_world.rotation().matrix() = rot;


Expand Down

0 comments on commit ace3eca

Please sign in to comment.