ARCore中获取相对姿态：基于自定义原点的姿态转换技巧

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

import com.google.ar.core.Pose;

public class ArPoseCalculator {

    private Pose neutralPose = null; // 用于存储自定义的原点姿态

    /**
     * 获取对象的姿态，并将其转换为相对于中立姿态的相对姿态。
     *
     * @param currentObjectPose 当前对象的相机相对姿态。
     * @return 如果中立姿态已设置，返回对象相对于中立姿态的相对姿态；
     *         否则，返回当前对象的相机相对姿态，并将其设置为中立姿态。
     */
    public Pose getRelativePose(Pose currentObjectPose) {
        // 如果中立姿态尚未设置，则将当前姿态设为中立姿态
        if (neutralPose == null) {
            neutralPose = currentObjectPose;
            // 此时，因为没有参考点，所以返回的姿态就是其自身（相机相对）
            // 或者可以返回一个单位姿态，取决于具体业务需求
            return Pose.makeIdentity(); // 返回单位姿态表示相对于自身
        }

        // 计算当前对象姿态相对于中立姿态的相对姿态
        // 步骤1: 获取中立姿态的反向变换
        Pose inverseNeutralPose = neutralPose.inverse();
        // 步骤2: 将反向中立姿态的变换与当前对象姿态的变换进行组合
        // 结果是：从 neutralPose 到 currentObjectPose 的变换
        Pose relativePose = inverseNeutralPose.compose(currentObjectPose);

        return relativePose;
    }

    /**
     * 重置中立姿态。
     */
    public void resetNeutralPose() {
        neutralPose = null;
    }

    // 示例用法
    public static void main(String[] args) {
        ArPoseCalculator calculator = new ArPoseCalculator();

        // 模拟第一次获取的姿态（作为中立姿态）
        // 假设 neutralPose 在 (0,0,0) 处，无旋转
        Pose initialCameraPose = Pose.makeTranslation(0f, 0f, 0f);
        System.out.println("第一次获取姿态（作为中立姿态）: " + initialCameraPose);
        Pose relative1 = calculator.getRelativePose(initialCameraPose);
        System.out.println("相对于中立姿态1（期望单位姿态）: " + relative1); // 期望是单位姿态 (0,0,0)

        // 模拟第二次获取的姿态，向X轴平移1米
        Pose secondCameraPose = Pose.makeTranslation(1f, 0f, 0f);
        System.out.println("第二次获取姿态（相机相对）: " + secondCameraPose);
        Pose relative2 = calculator.getRelativePose(secondCameraPose);
        System.out.println("相对于中立姿态1（期望X轴平移1米）: " + relative2); // 期望是 (1,0,0)

        // 模拟第三次获取的姿态，向X轴平移-0.5米
        Pose thirdCameraPose = Pose.makeTranslation(-0.5f, 0f, 0f);
        System.out.println("第三次获取姿态（相机相对）: " + thirdCameraPose);
        Pose relative3 = calculator.getRelativePose(thirdCameraPose);
        System.out.println("相对于中立姿态1（期望X轴平移-0.5米）: " + relative3); // 期望是 (-0.5,0,0)

        // 模拟重置中立姿态，并设置新的中立姿态在X轴平移2米处
        calculator.resetNeutralPose();
        Pose newInitialCameraPose = Pose.makeTranslation(2f, 0f, 0f);
        System.out.println("\n重置中立姿态，并设置新的中立姿态在 (2,0,0)");
        Pose relative4 = calculator.getRelativePose(newInitialCameraPose);
        System.out.println("相对于新的中立姿态（期望单位姿态）: " + relative4);

        // 模拟新的姿态在X轴平移3米
        Pose fourthCameraPose = Pose.makeTranslation(3f, 0f, 0f);
        System.out.println("第四次获取姿态（相机相对）: " + fourthCameraPose);
        Pose relative5 = calculator.getRelativePose(fourthCameraPose);
        System.out.println("相对于新的中立姿态（期望X轴平移1米）: " + relative5); // 期望是 (1,0,0)
    }
}

第一次获取姿态（作为中立姿态）: Pose{t=[0.0, 0.0, 0.0], q=[0.0, 0.0, 0.0, 1.0]}
相对于中立姿态1（期望单位姿态）: Pose{t=[0.0, 0.0, 0.0], q=[0.0, 0.0, 0.0, 1.0]}
第二次获取姿态（相机相对）: Pose{t=[1.0, 0.0, 0.0], q=[0.0, 0.0, 0.0, 1.0]}
相对于中立姿态1（期望X轴平移1米）: Pose{t=[1.0, 0.0, 0.0], q=[0.0, 0.0, 0.0, 1.0]}
第三次获取姿态（相机相对）: Pose{t=[-0.5, 0.0, 0.0], q=[0.0, 0.0, 0.0, 1.0]}
相对于中立姿态1（期望X轴平移-0.5米）: Pose{t=[-0.5, 0.0, 0.0], q=[0.0, 0.0, 0.0, 1.0]}

重置中立姿态，并设置新的中立姿态在 (2,0,0)
相对于新的中立姿态（期望单位姿态）: Pose{t=[0.0, 0.0, 0.0], q=[0.0, 0.0, 0.0, 1.0]}
第四次获取姿态（相机相对）: Pose{t=[3.0, 0.0, 0.0], q=[0.0, 0.0, 0.0, 1.0]}
相对于新的中立姿态（期望X轴平移1米）: Pose{t=[1.0, 0.0, 0.0], q=[0.0, 0.0, 0.0, 1.0]}