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); }
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; }