コード例 #1
0
        /// <summary>
        /// 通过新旧坐标表构造,三个点以上
        /// </summary>
        /// <param name="pold">旧坐标表</param>
        /// <param name="pnew">新坐标表</param>
        public CoordinateTQ(List <_3D_Point> pold, List <_3D_Point> pnew)
        {
            para = new TransParam();
            Matrix B  = new Matrix(pold.Count * 3, 8);
            Matrix l  = new Matrix(B.Row, 1);
            Matrix Bx = new Matrix(1, 8);
            Matrix w  = new Matrix(1, 1);
            Matrix x;

            do
            {
                double q0     = para.q0,
                       q1     = para.q1,
                       q2     = para.q2,
                       q3     = para.q3,
                       lambda = para.lambda,
                       X0     = para.X0,
                       Y0     = para.Y0,
                       Z0     = para.Z0,
                       a1     = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3,
                       a2     = 2 * (q1 * q2 - q0 * q3),
                       a3     = 2 * (q1 * q3 + q0 * q2),
                       b1     = 2 * (q1 * q2 + q0 * q3),
                       b2     = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3,
                       b3     = 2 * (q2 * q3 - q0 * q1),
                       c1     = 2 * (q1 * q3 - q0 * q2),
                       c2     = 2 * (q2 * q3 + q0 * q1),
                       c3     = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
                Bx[0, 0] = q0;
                Bx[0, 1] = q1;
                Bx[0, 2] = q2;
                Bx[0, 3] = q3;
                w[0, 0]  = (1 - (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3)) / 2;
                for (int i = 0; i < pold.Count; i++)
                {
                    int    row0 = 3 * i;
                    double X    = pold[i].X,
                           Y    = pold[i].Y,
                           Z    = pold[i].Z;
                    B[row0, 0] = 2 * lambda * (q0 * X - q3 * Y + q2 * Z);
                    B[row0, 1] = 2 * lambda * (q1 * X + q2 * Y + q3 * Z);
                    B[row0, 2] = 2 * lambda * (-q2 * X + q1 * Y + q0 * Z);
                    B[row0, 3] = 2 * lambda * (-q3 * X - q0 * Y + q1 * Z);
                    B[row0, 4] = a1 * X + a2 * Y + a3 * Z;
                    B[row0, 5] = 1;

                    B[row0 + 1, 0] = 2 * lambda * (q3 * X + q0 * Y - q1 * Z);
                    B[row0 + 1, 1] = 2 * lambda * (q2 * X - q1 * Y - q0 * Z);
                    B[row0 + 1, 2] = 2 * lambda * (q1 * X + q2 * Y + q3 * Z);
                    B[row0 + 1, 3] = 2 * lambda * (q0 * X - q3 * Y + q2 * Z);
                    B[row0 + 1, 4] = b1 * X + b2 * Y + b3 * Z;
                    B[row0 + 1, 6] = 1;

                    B[row0 + 2, 0] = 2 * lambda * (-q2 * X + q1 * Y + q0 * Z);
                    B[row0 + 2, 1] = 2 * lambda * (q3 * X + q0 * Y - q1 * Z);
                    B[row0 + 2, 2] = 2 * lambda * (-q0 * X + q3 * Y - q2 * Z);
                    B[row0 + 2, 3] = 2 * lambda * (q1 * X + q2 * Y + q3 * Z);
                    B[row0 + 2, 4] = c1 * X + c2 * Y + c3 * Z;
                    B[row0 + 2, 7] = 1;

                    l[row0, 0]     = pnew[i].X - (X0 + lambda * (a1 * X + a2 * Y + a3 * Z));
                    l[row0 + 1, 0] = pnew[i].Y - (Y0 + lambda * (b1 * X + b2 * Y + b3 * Z));
                    l[row0 + 2, 0] = pnew[i].Z - (Z0 + lambda * (c1 * X + c2 * Y + c3 * Z));
                }
                var N1 = Matrix.ColumnCombine(B.T() * B, Bx.T());
                var N2 = Matrix.ColumnCombine(Bx, Matrix.Zeros(Bx.Row, Bx.Row));
                var N  = Matrix.RowCombine(N1, N2);
                var W  = Matrix.RowCombine(B.T() * l, w);
                var xx = N.Inverse() * W;
                x            = xx.SubRMatrix(0, B.Column - 1);
                para.q0     += x[0, 0];
                para.q1     += x[1, 0];
                para.q2     += x[2, 0];
                para.q3     += x[3, 0];
                para.lambda += x[4, 0];
                para.X0     += x[5, 0];
                para.Y0     += x[6, 0];
                para.Z0     += x[7, 0];
            } while (!Terminate(x));
        }
コード例 #2
0
 /// <summary>
 /// 通过变换参数构造
 /// </summary>
 /// <param name="p">变换参数</param>
 public CoordinateTQ(TransParam p)
 {
     para = p;
 }