Java实现TF类矩阵变换--三维坐标转换

---

前言

工作中需要使用Java实现对三维坐标的转换，因为没有现成的库实现，所以封装了一个类实现了基本的矩阵变换。

---

一、引入math下的矩阵基本库

        <dependency>
            <groupId>org.saintandreas</groupId>
            <artifactId>math</artifactId>
            <version>1.0.4</version>
        </dependency>

二、定义Transform类

import lombok.AllArgsConstructor;
import lombok.Data;
import lombok.NoArgsConstructor;
import lombok.experimental.Accessors;
import org.saintandreas.math.Matrix3f;
import org.saintandreas.math.Quaternion;
import org.saintandreas.math.Vector3f;

/**
 * @Author wangzeming
 * @Date 2022/1/6 9:09
 */
@Data
@Accessors(chain = true)
@AllArgsConstructor
@NoArgsConstructor
public class Transform {

    private Matrix3f m_basis;

    private Vector3f m_origin;

    @Data
    @Accessors(chain = true)
    public static class Pose {
        private float x;
        private float y;
        private float z;
        private float yaw;
    }

    public void setM_basis(Quaternion quaternion) {
        this.m_basis = quaternion.toRotationMatrix();
    }

    public Transform inverse() {
        Matrix3f inv = this.m_basis.transpose();
        Vector3f negative = getNegative(this.getM_origin());
        return new Transform(inv, inv.mult(negative));
    }

    public static Pose getPose(Transform transform) {
        Vector3f m_origin = transform.getM_origin();
        Quaternion quaternion = new Quaternion().apply(transform.getM_basis());
        float yaw = getYaw(quaternion);
        return new Pose().setX(m_origin.getX())
                .setY(m_origin.getY())
                .setZ(m_origin.getZ())
                .setYaw(yaw);
    }

    public Vector3f getNegative(Vector3f vector3f) {
        return new Vector3f(-vector3f.getX(), -vector3f.getY(), -vector3f.getZ());
    }

    /**
     * Transform * Transform
     */
    public static Transform multiplyTransform (Transform t1, Transform t2) {
        return new Transform(t1.getM_basis().mult(t2.getM_basis()), multiplyVector3f(t1, t2.getM_origin()));
    }

    /**
     * Transform * Vector3f
     */
    public static Vector3f multiplyVector3f (Transform transform, Vector3f x) {
        return new Vector3f(transform.getM_basis().getRow(0).dot(x) + transform.getM_origin().getX(),
                        transform.getM_basis().getRow(1).dot(x) + transform.getM_origin().getY(),
                        transform.getM_basis().getRow(2).dot(x) + transform.getM_origin().getZ());
    }

    public static float getYaw(Quaternion q) {
        float w = q.getW();
        float x = q.getX();
        float y = q.getY();
        float z = q.getZ();
        float siny_cosp = 2 * (w * z + x * y);
        float cosy_cosp = 1 - 2 * (y * y + z * z);
        return (float) Math.atan2(siny_cosp, cosy_cosp);
    }

    /**
     * tOne: 绝对坐标
     * tTwo: 绝对坐标
     * getRelativePose: tTwo相对于tOne的相对坐标
     */
    public static Transform.Pose getRelativePose(Transform tOne, Transform tTwo) {
        Transform tOneInverse = tOne.inverse();
        Transform multiplyTransform = Transform.multiplyTransform(tOneInverse, tTwo);
        return Transform.getPose(multiplyTransform);
    }

    /**
     * absoluteTF: 绝对坐标
     * relativeTF: 相对坐标
     */
    public static Transform.Pose getAbsolutePose(Transform absoluteTF, Transform relativeTF) {
        Transform multiplyTransform = Transform.multiplyTransform(absoluteTF, relativeTF);
        return Transform.getPose(multiplyTransform);
    }

    public static Transform getTransform(Double x, Double y, Double r) {
        Vector3f vector3f = new Vector3f(x.floatValue(), y.floatValue(), 0);
        Quaternion quaternion = Quaternion.fromAngles(0, 0, r.floatValue());
        Transform transform = new Transform();
        transform.setM_origin(vector3f)
                .setM_basis(quaternion);
        return transform;
    }
}

三、如何使用？

1. A点坐标

Transform pointA = getTransform(x1, y1, r1);

2. B点坐标

Transform pointB = getTransform(x, y, r);

3. 计算A点相对于B点的相对坐标？

Transform.Pose relativeA = getRelativePose(pointB , pointA);

4. 根据相对坐标计算绝对坐标？

Pose pose= getAbsolutePose(pointB , relativeA);

总结

揣到兜里的才是自己的！