diff --git a/src/XRFeitoriaUnreal/Source/XRFeitoriaUnreal/Private/Annotator.cpp b/src/XRFeitoriaUnreal/Source/XRFeitoriaUnreal/Private/Annotator.cpp index 8cf12e4b..1268de80 100644 --- a/src/XRFeitoriaUnreal/Source/XRFeitoriaUnreal/Private/Annotator.cpp +++ b/src/XRFeitoriaUnreal/Source/XRFeitoriaUnreal/Private/Annotator.cpp @@ -226,8 +226,11 @@ void AAnnotator::ExportSkeletalMeshParameters(int FrameNumber) // Save Actor Info (location, rotation, stencil value) { - FVector ActorLocation = SkeletalMeshComponent->GetOwner()->GetActorLocation(); - FRotator ActorRotation = SkeletalMeshComponent->GetOwner()->GetActorRotation(); + FTransform ActorTransform = SkeletalMeshComponent->GetOwner()->GetActorTransform(); + UXF_BlueprintFunctionLibrary::ConvertUnrealToOpenCV(ActorTransform); + UE_LOG(LogTemp, Warning, TEXT("ActorTransform: %s"), *ActorTransform.ToString()); + FVector ActorLocation = ActorTransform.GetLocation(); + FRotator ActorRotation = ActorTransform.Rotator(); int StencilValue = SkeletalMeshComponent->CustomDepthStencilValue; TArray ActorInfo; diff --git a/src/XRFeitoriaUnreal/Source/XRFeitoriaUnreal/Private/XF_BlueprintFunctionLibrary.cpp b/src/XRFeitoriaUnreal/Source/XRFeitoriaUnreal/Private/XF_BlueprintFunctionLibrary.cpp index 2880c307..17c45607 100644 --- a/src/XRFeitoriaUnreal/Source/XRFeitoriaUnreal/Private/XF_BlueprintFunctionLibrary.cpp +++ b/src/XRFeitoriaUnreal/Source/XRFeitoriaUnreal/Private/XF_BlueprintFunctionLibrary.cpp @@ -1075,3 +1075,34 @@ void UXF_BlueprintFunctionLibrary::SaveTextToFile(const FString StringToWrite, F TEXT("FileManipulation: Failed to write FString to file.")); } } + +void UXF_BlueprintFunctionLibrary::ConvertCoordinateSystem(FTransform& Transform, const EAxis SrcXInDstAxis, const EAxis SrcYInDstAxis, const EAxis SrcZInDstAxis) +{ + // Unreal Engine: + // Front : X + // Right : Y + // Up : Z + // + // OpenCV: + // Front : Z + // Right : X + // Up : Yn + + FMatrix M12 = FMatrix::Identity; + + M12.SetColumn(0, UnitVectorFromAxisEnum(SrcXInDstAxis)); + M12.SetColumn(1, UnitVectorFromAxisEnum(SrcYInDstAxis)); + M12.SetColumn(2, UnitVectorFromAxisEnum(SrcZInDstAxis)); + + Transform.SetFromMatrix(M12.GetTransposed() * Transform.ToMatrixWithScale() * M12); +} + +void UXF_BlueprintFunctionLibrary::ConvertUnrealToOpenCV(FTransform& Transform) +{ + ConvertCoordinateSystem(Transform, EAxis::Y, EAxis::Zn, EAxis::X); +} + +void UXF_BlueprintFunctionLibrary::ConvertOpenCVToUnreal(FTransform& Transform) +{ + ConvertCoordinateSystem(Transform, EAxis::Z, EAxis::X, EAxis::Yn); +} diff --git a/src/XRFeitoriaUnreal/Source/XRFeitoriaUnreal/Public/XF_BlueprintFunctionLibrary.h b/src/XRFeitoriaUnreal/Source/XRFeitoriaUnreal/Public/XF_BlueprintFunctionLibrary.h index e7bc47c2..a1daa6af 100644 --- a/src/XRFeitoriaUnreal/Source/XRFeitoriaUnreal/Public/XF_BlueprintFunctionLibrary.h +++ b/src/XRFeitoriaUnreal/Source/XRFeitoriaUnreal/Public/XF_BlueprintFunctionLibrary.h @@ -194,6 +194,39 @@ class XRFEITORIAUNREAL_API UXF_BlueprintFunctionLibrary : public UBlueprintFunct FVector& CenterLoc, bool& bIsHit); +public: + /** Enumeration to specify any cartesian axis in positive or negative directions */ + enum class EAxis + { + X, Y, Z, + Xn, Yn, Zn, + }; + + // These axes must match the order in which they are declared in EAxis + inline static const TArray UnitVectors = + { + { 1, 0, 0 }, // X + { 0, 1, 0 }, // Y + { 0, 0, 1 }, // Z + { -1, 0, 0 }, // -X + { 0, -1, 0 }, // -Y + { 0, 0, -1 }, // -Z + }; + + static const FVector& UnitVectorFromAxisEnum(const EAxis Axis) + { + return UnitVectors[std::underlying_type_t(Axis)]; + }; + + /** Converts in-place the coordinate system of the given FTransform by specifying the source axes in terms of the destination axes */ + static void ConvertCoordinateSystem(FTransform& Transform, const EAxis DstXInSrcAxis, const EAxis DstYInSrcAxis, const EAxis DstZInSrcAxis); + + /** Converts in-place an FTransform in Unreal coordinates to OpenCV coordinates */ + static void ConvertUnrealToOpenCV(FTransform& Transform); + + /** Converts in-place an FTransform in OpenCV coordinates to Unreal coordinates */ + static void ConvertOpenCVToUnreal(FTransform& Transform); + private: static void SaveTextToFile( const FString StringToWrite, diff --git a/xrfeitoria/utils/converter.py b/xrfeitoria/utils/converter.py index 8f3cf9f5..7e32d24f 100644 --- a/xrfeitoria/utils/converter.py +++ b/xrfeitoria/utils/converter.py @@ -191,8 +191,7 @@ def vec_amass2humandata(cls, vector: np.ndarray) -> np.ndarray: class ConverterUnreal: - UNITS_SCALE = 100.0 # 1 meter = 100 cm - ROTATION_OFFSET = [0, 0, -90.0] # (x, y, z) in degrees, around z-axis (left-handed) + units_scale = 100.0 # 1 meter = 100 cm @classmethod def rotation_camera_from_ue(cls, euler, degrees=True) -> np.ndarray: @@ -210,7 +209,7 @@ def rotation_camera_from_ue(cls, euler, degrees=True) -> np.ndarray: return rotation_matrix([x, y, z], order='xyz', degrees=degrees) @classmethod - def rotation_from_ue(cls, euler, offset=ROTATION_OFFSET, degrees=True) -> np.ndarray: + def rotation_from_ue(cls, euler, degrees=True) -> np.ndarray: """Convert from ue camera space to opencv camera space convention. Args: @@ -221,8 +220,12 @@ def rotation_from_ue(cls, euler, offset=ROTATION_OFFSET, degrees=True) -> np.nda Returns: np.ndarray: Rotation matrix 3x3. """ - _euler = np.array(euler) + np.array(offset) - return rotation_matrix(_euler, 'zxy', degrees=degrees) + from scipy.spatial.transform import Rotation + + rot = Rotation.from_euler('xyz', euler, degrees=True).as_rotvec() + rot[2] *= -1 + rot = Rotation.from_euler('xyz', [0, 90.0, 0.0], degrees=degrees) * Rotation.from_rotvec(rot) + return rot.as_matrix().T @classmethod def location_from_ue(cls, vector: np.ndarray) -> np.ndarray: @@ -239,9 +242,9 @@ def location_from_ue(cls, vector: np.ndarray) -> np.ndarray: if isinstance(vector, list): vector = np.array(vector) if vector.shape == (3,): - ret = np.array([vector[1], -vector[2], vector[0]]) / cls.UNITS_SCALE + ret = np.array([vector[1], -vector[2], vector[0]]) / cls.units_scale elif vector.ndim >= 2 and vector.shape[-1] == 3: - ret = np.stack([vector[..., 1], -vector[..., 2], vector[..., 0]], axis=-1) / cls.UNITS_SCALE + ret = np.stack([vector[..., 1], -vector[..., 2], vector[..., 0]], axis=-1) / cls.units_scale return ret