Example #1
0
 private static NyARMat makeMatAtA(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtA)
 {
     o_matAtA.loadZero();
     double[][] t = o_matAtA.getArray();
     for (int i = 0; i < i_num; i++)
     {
         //0
         double wx = worldCoord[i].x;
         double wy = worldCoord[i].y;
         double sx = screenCoord[i].x;
         double sy = screenCoord[i].y;
         double wxwx = wx * wx;
         double wywy = wy * wy;
         double wxwy = wx * wy;
         t[0][0] += wxwx;
         t[0][1] += wxwy;
         t[0][2] += wx;
         t[1][2] += wy;
         t[0][6] += (-wxwx) * (sx);
         t[0][7] += (-wxwy) * (sx);
         t[1][1] += wywy;
         t[1][7] += (-wywy) * (sx);
         t[2][6] += (-wx) * (sx);
         t[2][7] += (-wy) * (sx);
         t[3][6] += (-wxwx) * (sy);
         t[3][7] += (-wxwy) * (sy);
         t[4][7] += (-wywy) * (sy);
         t[5][6] += (-wx) * (sy);
         t[5][7] += (-wy) * (sy);
         t[6][6] += (wxwx) * (sx) * (sx) + (wxwx) * (sy) * (sy);
         t[6][7] += (wxwy) * (sx) * (sx) + (wxwy) * (sy) * (sy);
         t[7][7] += (wywy) * (sx) * (sx) + (wywy) * (sy) * (sy);
     }
     t[1][0] = t[3][4] = t[4][3] = t[0][1];
     t[2][0] = t[3][5] = t[5][3] = t[0][2];
     t[2][1] = t[5][4] = t[4][5] = t[1][2];
     t[7][0] = t[6][1] = t[1][6] = t[0][7];
     t[4][6] = t[6][4] = t[7][3] = t[3][7];
     t[2][2] = t[5][5] = i_num;
     t[3][3] = t[0][0];
     t[4][4] = t[1][1];
     t[6][0] = t[0][6];
     t[6][2] = t[2][6];
     t[6][3] = t[3][6];
     t[6][5] = t[5][6];
     t[7][1] = t[1][7];
     t[7][2] = t[2][7];
     t[7][4] = t[4][7];
     t[7][5] = t[5][7];
     t[7][6] = t[6][7];
     //先頭でゼロクリアしない場合。
     //t[0][3]=t[0][4]=t[0][5]=t[1][3]=t[1][4]=t[1][5]=t[2][3]=t[2][4]=t[2][5]=t[3][0]=t[3][1]=t[3][2]=t[4][0]=t[4][1]=t[4][2]=t[5][0]=t[5][1]=t[5][2]=0;
     return o_matAtA;
 }
 private static NyARMat makeMatAtA(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtA)
 {
     o_matAtA.loadZero();
     double[][] t = o_matAtA.getArray();
     for (int i = 0; i < i_num; i++)
     {
         //0
         double wx   = worldCoord[i].x;
         double wy   = worldCoord[i].y;
         double sx   = screenCoord[i].x;
         double sy   = screenCoord[i].y;
         double wxwx = wx * wx;
         double wywy = wy * wy;
         double wxwy = wx * wy;
         t[0][0] += wxwx;
         t[0][1] += wxwy;
         t[0][2] += wx;
         t[1][2] += wy;
         t[0][6] += (-wxwx) * (sx);
         t[0][7] += (-wxwy) * (sx);
         t[1][1] += wywy;
         t[1][7] += (-wywy) * (sx);
         t[2][6] += (-wx) * (sx);
         t[2][7] += (-wy) * (sx);
         t[3][6] += (-wxwx) * (sy);
         t[3][7] += (-wxwy) * (sy);
         t[4][7] += (-wywy) * (sy);
         t[5][6] += (-wx) * (sy);
         t[5][7] += (-wy) * (sy);
         t[6][6] += (wxwx) * (sx) * (sx) + (wxwx) * (sy) * (sy);
         t[6][7] += (wxwy) * (sx) * (sx) + (wxwy) * (sy) * (sy);
         t[7][7] += (wywy) * (sx) * (sx) + (wywy) * (sy) * (sy);
     }
     t[1][0] = t[3][4] = t[4][3] = t[0][1];
     t[2][0] = t[3][5] = t[5][3] = t[0][2];
     t[2][1] = t[5][4] = t[4][5] = t[1][2];
     t[7][0] = t[6][1] = t[1][6] = t[0][7];
     t[4][6] = t[6][4] = t[7][3] = t[3][7];
     t[2][2] = t[5][5] = i_num;
     t[3][3] = t[0][0];
     t[4][4] = t[1][1];
     t[6][0] = t[0][6];
     t[6][2] = t[2][6];
     t[6][3] = t[3][6];
     t[6][5] = t[5][6];
     t[7][1] = t[1][7];
     t[7][2] = t[2][7];
     t[7][4] = t[4][7];
     t[7][5] = t[5][7];
     t[7][6] = t[6][7];
     //先頭でゼロクリアしない場合。
     //t[0][3]=t[0][4]=t[0][5]=t[1][3]=t[1][4]=t[1][5]=t[2][3]=t[2][4]=t[2][5]=t[3][0]=t[3][1]=t[3][2]=t[4][0]=t[4][1]=t[4][2]=t[5][0]=t[5][1]=t[5][2]=0;
     return(o_matAtA);
 }
            public NyARMat makeJtJ(NyARMat o_dst)
            {
                o_dst.loadZero();
                Item[]     buf = this._items;
                double[][] b   = o_dst.getArray();
                for (int k = 0; k < this._length; k++)
                {
                    Item ptr = buf[k];
                    b[0][0] += (ptr.m00 * ptr.m00) + (ptr.m10 * ptr.m10);
                    b[0][1] += (ptr.m00 * ptr.m01) + (ptr.m10 * ptr.m11);
                    b[0][2] += (ptr.m00 * ptr.m02) + (ptr.m10 * ptr.m12);
                    b[0][3] += (ptr.m00 * ptr.m03) + (ptr.m10 * ptr.m13);
                    b[0][4] += (ptr.m00 * ptr.m04) + (ptr.m10 * ptr.m14);
                    b[0][5] += (ptr.m00 * ptr.m05) + (ptr.m10 * ptr.m15);

                    b[1][0] += (ptr.m01 * ptr.m00) + (ptr.m11 * ptr.m10);
                    b[1][1] += (ptr.m01 * ptr.m01) + (ptr.m11 * ptr.m11);
                    b[1][2] += (ptr.m01 * ptr.m02) + (ptr.m11 * ptr.m12);
                    b[1][3] += (ptr.m01 * ptr.m03) + (ptr.m11 * ptr.m13);
                    b[1][4] += (ptr.m01 * ptr.m04) + (ptr.m11 * ptr.m14);
                    b[1][5] += (ptr.m01 * ptr.m05) + (ptr.m11 * ptr.m15);

                    b[2][0] += (ptr.m02 * ptr.m00) + (ptr.m12 * ptr.m10);
                    b[2][1] += (ptr.m02 * ptr.m01) + (ptr.m12 * ptr.m11);
                    b[2][2] += (ptr.m02 * ptr.m02) + (ptr.m12 * ptr.m12);
                    b[2][3] += (ptr.m02 * ptr.m03) + (ptr.m12 * ptr.m13);
                    b[2][4] += (ptr.m02 * ptr.m04) + (ptr.m12 * ptr.m14);
                    b[2][5] += (ptr.m02 * ptr.m05) + (ptr.m12 * ptr.m15);

                    b[3][0] += (ptr.m03 * ptr.m00) + (ptr.m13 * ptr.m10);
                    b[3][1] += (ptr.m03 * ptr.m01) + (ptr.m13 * ptr.m11);
                    b[3][2] += (ptr.m03 * ptr.m02) + (ptr.m13 * ptr.m12);
                    b[3][3] += (ptr.m03 * ptr.m03) + (ptr.m13 * ptr.m13);
                    b[3][4] += (ptr.m03 * ptr.m04) + (ptr.m13 * ptr.m14);
                    b[3][5] += (ptr.m03 * ptr.m05) + (ptr.m13 * ptr.m15);

                    b[4][0] += (ptr.m04 * ptr.m00) + (ptr.m14 * ptr.m10);
                    b[4][1] += (ptr.m04 * ptr.m01) + (ptr.m14 * ptr.m11);
                    b[4][2] += (ptr.m04 * ptr.m02) + (ptr.m14 * ptr.m12);
                    b[4][3] += (ptr.m04 * ptr.m03) + (ptr.m14 * ptr.m13);
                    b[4][4] += (ptr.m04 * ptr.m04) + (ptr.m14 * ptr.m14);
                    b[4][5] += (ptr.m04 * ptr.m05) + (ptr.m14 * ptr.m15);

                    b[5][0] += (ptr.m05 * ptr.m00) + (ptr.m15 * ptr.m10);
                    b[5][1] += (ptr.m05 * ptr.m01) + (ptr.m15 * ptr.m11);
                    b[5][2] += (ptr.m05 * ptr.m02) + (ptr.m15 * ptr.m12);
                    b[5][3] += (ptr.m05 * ptr.m03) + (ptr.m15 * ptr.m13);
                    b[5][4] += (ptr.m05 * ptr.m04) + (ptr.m15 * ptr.m14);
                    b[5][5] += (ptr.m05 * ptr.m05) + (ptr.m15 * ptr.m15);
                }
                return(o_dst);
            }