From d231ce01ae9b46d3ed3339d5eb497027da2d33b0 Mon Sep 17 00:00:00 2001 From: John Turner <7strbass@gmail.com> Date: Wed, 8 Jan 2025 12:31:52 -0500 Subject: [PATCH] --remove unnecessary quaternion conversions --- .../datasets/rearrange/samplers/receptacle.py | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py index 462075b3c9..635ada2a00 100644 --- a/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py +++ b/habitat-lab/habitat/datasets/rearrange/samplers/receptacle.py @@ -22,8 +22,6 @@ from habitat.datasets.rearrange.navmesh_utils import is_accessible from habitat.sims.habitat_simulator.debug_visualizer import dblr_draw_bb from habitat.utils.geometry_utils import random_triangle_point -from habitat_sim.utils.common import quat_from_two_vectors as qf2v -from habitat_sim.utils.common import quat_to_magnum as qtm # global module singleton for mesh importing instantiated upon first import _manager = mn.trade.ImporterManager() @@ -279,19 +277,20 @@ def get_global_transform(self, sim: habitat_sim.Simulator) -> mn.Matrix4: # this is a global stage receptacle # TODO: add an API query or other method to avoid reconstructing the stage frame here stage_config = sim.get_stage_initialization_template() - r_frameup_worldup = qf2v( - np.array(habitat_sim.geo.UP), np.array(stage_config.orient_up) + + r_frameup_worldup = mn.Quaternion.rotation( + habitat_sim.geo.UP, stage_config.orient_up.normalized() ) - v_prime = qtm(r_frameup_worldup).transform_vector( + v_prime = r_frameup_worldup.transform_vector_normalized( habitat_sim.geo.FRONT - ) + ).normalized() world_to_local = ( - qf2v(np.array(v_prime), np.array(stage_config.orient_front)) + mn.Quaternion.rotation( + v_prime, stage_config.orient_front.normalized() + ) * r_frameup_worldup - ) - world_to_local = habitat_sim.utils.common.quat_to_magnum( - world_to_local - ) + ).normalized() + local_to_world = world_to_local.inverted() l2w4 = mn.Matrix4.from_(local_to_world.to_matrix(), mn.Vector3())