Main-Repo/SourceCode/AIReconstruction/AI.Reconstruction/TransducerImgPlaneToWorkCoordMatrix.cs

using System;
using AI.Common;
using AI.Common.Tools;


namespace AI.Reconstruction
{
    /// <summary>
    /// 探头位姿
    /// 以探头的中心点为原点，探头深度方向为y轴正方向，探头标记点方向为x轴负方向，建立坐标系
    /// 该位姿表示的是将该成像平面上的点转换到目标坐标系下的转换矩阵
    /// 图像像素坐标，应该 (x - w/2) * spacingx，-y * spacingy 才能转换到成像平面
    /// </summary>
    public class TransducerImgPlaneToWorkCoordMatrix
    {

        #region private

        private float _tx;
        private float _ty;
        private float _tz;
        private float _r11;
        private float _r12;
        private float _r13;
        private float _r21;
        private float _r22;
        private float _r23;
        private float _r31;
        private float _r32;
        private float _r33;
        private float[] _translationVector;
        private float[] _rotationMatrix;


        #endregion


        #region property

        /// <summary>
        /// x方向的平移
        /// </summary>
        public float TransX { get => _tx; set => _tx = value; }

        /// <summary>
        /// y方向的平移
        /// </summary>
        public float TransY { get => _ty; set => _ty = value; }

        /// <summary>
        /// z方向的平移
        /// </summary>
        public float TransZ { get => _tz; set => _tz = value; }

        /// <summary>
        /// 旋转矩阵的第一行第一列
        /// </summary>
        public float Rotate11 { get => _r11; set => _r11 = value; }

        /// <summary>
        /// 旋转矩阵的第一行第二列
        /// </summary>
        public float Rotate12 { get => _r12; set => _r12 = value; }

        /// <summary>
        /// 旋转矩阵的第一行第三列
        /// </summary>
        public float Rotate13 { get => _r13; set => _r13 = value; }

        /// <summary>
        /// 旋转矩阵的第二行第一列
        /// </summary>
        public float Rotate21 { get => _r21; set => _r21 = value; }

        /// <summary>
        /// 旋转矩阵的第二行第二列
        /// </summary>
        public float Rotate22 { get => _r22; set => _r22 = value; }

        /// <summary>
        /// 旋转矩阵的第二行第三列
        /// </summary>
        public float Rotate23 { get => _r23; set => _r23 = value; }

        /// <summary>
        /// 旋转矩阵的第三行第一列
        /// </summary>
        public float Rotate31 { get => _r31; set => _r31 = value; }

        /// <summary>
        /// 旋转矩阵的第三行第二列
        /// </summary>
        public float Rotate32 { get => _r32; set => _r32 = value; }

        /// <summary>
        /// 旋转矩阵的第三行第三列
        /// </summary>
        public float Rotate33 { get => _r33; set => _r33 = value; }

        /// <summary>
        /// 平移向量
        /// </summary>
        public float[] TranslationVector
        {
            get => _translationVector;
            set => _translationVector = value;
        }

        /// <summary>
        /// 旋转矩阵
        /// </summary>
        public float[] RotationMatrix
        {
            get => _rotationMatrix;
            set => _rotationMatrix = value;
        }

        #endregion


        #region constructor

        /// <summary>
        /// 构造函数，只有平移
        /// </summary>
        /// <param name="tx"></param>
        /// <param name="ty"></param>
        /// <param name="tz"></param>
        public TransducerImgPlaneToWorkCoordMatrix(float tx, float ty, float tz)
        {
            _tx = tx;
            _ty = ty;
            _tz = tz;
            _r11 = 1;
            _r12 = 0;
            _r13 = 0;
            _r21 = 0;
            _r22 = 1;
            _r23 = 0;
            _r31 = 0;
            _r32 = 0;
            _r33 = 1;
            GenRotationMatrix();
            GenTranslationVector();
        }

        /// <summary>
        /// 构造函数，只有旋转
        /// </summary>
        /// <param name="r11"></param>
        /// <param name="r12"></param>
        /// <param name="r13"></param>
        /// <param name="r21"></param>
        /// <param name="r22"></param>
        /// <param name="r23"></param>
        /// <param name="r31"></param>
        /// <param name="r32"></param>
        /// <param name="r33"></param>
        public TransducerImgPlaneToWorkCoordMatrix(float r11, float r12, float r13,
            float r21, float r22, float r23, float r31, float r32, float r33)
        {
            _tx = 0;
            _ty = 0;
            _tz = 0;
            _r11 = r11;
            _r12 = r12;
            _r13 = r13;
            _r21 = r21;
            _r22 = r22;
            _r23 = r23;
            _r31 = r31;
            _r32 = r32;
            _r33 = r33;
            GenRotationMatrix();
            GenTranslationVector();
        }

        /// <summary>
        /// 构造函数，旋转和平移都有
        /// </summary>
        /// <param name="tx"></param>
        /// <param name="ty"></param>
        /// <param name="tz"></param>
        /// <param name="r11"></param>
        /// <param name="r12"></param>
        /// <param name="r13"></param>
        /// <param name="r21"></param>
        /// <param name="r22"></param>
        /// <param name="r23"></param>
        /// <param name="r31"></param>
        /// <param name="r32"></param>
        /// <param name="r33"></param>
        public TransducerImgPlaneToWorkCoordMatrix(float tx, float ty, float tz,
            float r11, float r12, float r13, float r21, float r22, float r23,
            float r31, float r32, float r33)
        {
            _tx = tx;
            _ty = ty;
            _tz = tz;
            _r11 = r11;
            _r12 = r12;
            _r13 = r13;
            _r21 = r21;
            _r22 = r22;
            _r23 = r23;
            _r31 = r31;
            _r32 = r32;
            _r33 = r33;
            GenRotationMatrix();
            GenTranslationVector();
        }

        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="matrix"></param>
        /// <param name="len"></param>
        public TransducerImgPlaneToWorkCoordMatrix(float[] matrix, int len)
        {
            if (matrix.Length != len)
            {
                throw new ArgumentException("the matrix length(" + matrix.Length + ") is not equal to " + len + " .");
            }
            if (len == 3)
            {
                _tx = matrix[0];
                _ty = matrix[1];
                _tz = matrix[2];
                _r11 = 1;
                _r12 = 0;
                _r13 = 0;
                _r21 = 0;
                _r22 = 1;
                _r23 = 0;
                _r31 = 0;
                _r32 = 0;
                _r33 = 1;
            }
            else if (len == 12)
            {
                _tx = matrix[0];
                _ty = matrix[1];
                _tz = matrix[2];
                _r11 = matrix[3];
                _r12 = matrix[4];
                _r13 = matrix[5];
                _r21 = matrix[6];
                _r22 = matrix[7];
                _r23 = matrix[8];
                _r31 = matrix[9];
                _r32 = matrix[10];
                _r33 = matrix[11];
            }
            else
            {
                throw new ArgumentException("unexpected matrix length(" + matrix.Length + "), the matrix length should be 3 or 12.");
            }
            GenRotationMatrix();
            GenTranslationVector();
        }

        #endregion


        #region public 

        /// <summary>
        /// 将某个点从成像平面坐标系转换到真实物理坐标系下
        /// </summary>
        /// <param name="point"></param>
        /// <returns></returns>
        public Point3DF Transform(Point3DF point)
        {
            float x = point.X;
            float y = point.Y;
            float z = point.Z;
            float xdst = _r11 * x + _r12 * y + _r13 * z + _tx;
            float ydst = _r21 * x + _r22 * y + _r23 * z + _ty;
            float zdst = _r31 * x + _r32 * y + _r33 * z + _tz;
            return new Point3DF(xdst, ydst, zdst);
        }

        /// <summary>
        /// 判断三个变换矩阵所代表的探头成像平面平行
        /// </summary>
        /// <param name="matrix1"></param>
        /// <param name="matrix2"></param>
        /// <returns></returns>
        public bool ImagePlaneParallels(TransducerImgPlaneToWorkCoordMatrix matrix1, TransducerImgPlaneToWorkCoordMatrix matrix2)
        {
            if (!MathHelper.AlmostEqual(matrix1.Rotate11, _r11) || !MathHelper.AlmostEqual(matrix2.Rotate11, _r11))
            {
                return false;
            }
            if (!MathHelper.AlmostEqual(matrix1.Rotate12, _r12) || !MathHelper.AlmostEqual(matrix2.Rotate12, _r12))
            {
                return false;
            }
            if (!MathHelper.AlmostEqual(matrix1.Rotate13, _r13) || !MathHelper.AlmostEqual(matrix2.Rotate13, _r13))
            {
                return false;
            }
            if (!MathHelper.AlmostEqual(matrix1.Rotate21, _r21) || !MathHelper.AlmostEqual(matrix2.Rotate21, _r21))
            {
                return false;
            }
            if (!MathHelper.AlmostEqual(matrix1.Rotate22, _r22) || !MathHelper.AlmostEqual(matrix2.Rotate22, _r22))
            {
                return false;
            }
            if (!MathHelper.AlmostEqual(matrix1.Rotate23, _r23) || !MathHelper.AlmostEqual(matrix2.Rotate23, _r23))
            {
                return false;
            }
            if (!MathHelper.AlmostEqual(matrix1.Rotate31, _r31) || !MathHelper.AlmostEqual(matrix2.Rotate31, _r31))
            {
                return false;
            }
            if (!MathHelper.AlmostEqual(matrix1.Rotate32, _r32) || !MathHelper.AlmostEqual(matrix2.Rotate32, _r32))
            {
                return false;
            }
            if (!MathHelper.AlmostEqual(matrix1.Rotate33, _r33) || !MathHelper.AlmostEqual(matrix2.Rotate33, _r33))
            {
                return false;
            }
            return true;
        }

        #endregion


        #region private 

        private void GenRotationMatrix()
        {
            _rotationMatrix = new float[9];
            _rotationMatrix[0] = _r11;
            _rotationMatrix[1] = _r12;
            _rotationMatrix[2] = _r13;
            _rotationMatrix[3] = _r21;
            _rotationMatrix[4] = _r22;
            _rotationMatrix[5] = _r23;
            _rotationMatrix[6] = _r31;
            _rotationMatrix[7] = _r32;
            _rotationMatrix[8] = _r33;
        }

        private void GenTranslationVector()
        {
            _translationVector = new float[3];
            _translationVector[0] = _tx;
            _translationVector[1] = _ty;
            _translationVector[2] = _tz;
        }


        #endregion

    }
}