ARCore姿态变换：计算相对姿态的正确方法

Pose centerPose = face.getCenterPose(); // 当前姿态，相机相对
if (neutralPose == null) {
  neutralPose = centerPose; // 设定初始中立姿态
}
// 尝试计算相对姿态
Pose relativePoseAttempt = centerPose.compose(neutralPose.inverse());

Pose relativePose = neutralPose.inverse().compose(centerPose);

import com.google.ar.core.Pose;
import com.google.ar.core.Trackable;
import com.google.ar.core.TrackingState;
import com.google.ar.core.AugmentedFace; // 假设是处理AugmentedFace的场景

public class ArPoseRelativeCalculator {

    private Pose neutralPose = null; // 用于存储初始的中立姿态

    /**
     * 初始化中立姿态。通常在第一次获取到有效姿态时调用。
     * @param initialPose 初始的中立姿态，通常是相机相对姿态。
     */
    public void setNeutralPose(Pose initialPose) {
        if (initialPose != null) {
            this.neutralPose = initialPose;
            System.out.println("Neutral Pose set: " + initialPose);
        }
    }

    /**
     * 计算当前姿态相对于已设置的中立姿态的相对姿态。
     * @param currentPose 当前的世界坐标系姿态（例如，来自AugmentedFace.getCenterPose()）。
     * @return 相对于中立姿态的相对姿态，如果中立姿态未设置则返回null。
     */
    public Pose getRelativePose(Pose currentPose) {
        if (neutralPose == null) {
            System.err.println("Error: Neutral Pose not set. Cannot calculate relative pose.");
            return null;
        }
        if (currentPose == null) {
            System.err.println("Error: Current Pose is null.");
            return null;
        }

        // 正确的相对姿态计算方法
        Pose relativePose = neutralPose.inverse().compose(currentPose);
        return relativePose;
    }

    // 假设在ARCore帧更新循环中使用
    public void onUpdate(AugmentedFace face) {
        if (face.getTrackingState() == TrackingState.TRACKING) {
            Pose centerPose = face.getCenterPose();

            // 如果中立姿态尚未设置，则将其设置为当前姿态
            if (neutralPose == null) {
                setNeutralPose(centerPose);
            }

            // 计算当前面部姿态相对于中立姿态的变换
            Pose relativeFacePose = getRelativePose(centerPose);

            if (relativeFacePose != null) {
                // 现在可以使用 relativeFacePose 来渲染相对于中立姿态的AR内容
                // 例如，根据 relativeFacePose 的平移和旋转来更新模型矩阵
                System.out.println("Relative Face Pose: " + relativeFacePose);
            }
        }
    }

    // 可以在需要时重置中立姿态
    public void resetNeutralPose() {
        this.neutralPose = null;
        System.out.println("Neutral Pose reset.");
    }

    // 示例用法（非ARCore环境，仅为说明逻辑）
    public static void main(String[] args) {
        ArPoseRelativeCalculator calculator = new ArPoseRelativeCalculator();

        // 模拟一个初始中立姿态 (例如，位于原点，无旋转)
        Pose initialNeutral = Pose.makeTranslation(0f, 0f, 0f);
        calculator.setNeutralPose(initialNeutral);

        // 模拟一个当前姿态 (例如，向右移动0.5米)
        Pose currentPose1 = Pose.makeTranslation(0.5f, 0f, 0f);
        Pose relative1 = calculator.getRelativePose(currentPose1);
        // 预期 relative1 也是 (0.5, 0, 0) - 因为中立姿态在原点
        System.out.println("Relative Pose 1 (expected 0.5,0,0): " + relative1);

        // 模拟一个带有旋转的初始中立姿态 (例如，绕Y轴旋转90度)
        Pose rotatedNeutral = Pose.makeRotation(0f, 0.707f, 0f, 0.707f); // 绕Y轴旋转90度
        calculator.setNeutralPose(rotatedNeutral);

        // 模拟一个当前姿态 (例如，世界坐标系中向X轴正方向移动0.5米)
        Pose currentPose2 = Pose.makeTranslation(0.5f, 0f, 0f);
        Pose relative2 = calculator.getRelativePose(currentPose2);
        // 预期 relative2 是 (0, 0, -0.5) - 因为中立姿态旋转了，X轴在它的局部坐标系中变成了-Z轴
        System.out.println("Relative Pose 2 (expected 0,0,-0.5): " + relative2);
    }
}