public void transformVertex(double i_x, double i_y, double i_z, NyARDoublePoint3d o_out)
{
o_out.x = this.m00 * i_x + this.m01 * i_y + this.m02 * i_z + this.m03;
o_out.y = this.m10 * i_x + this.m11 * i_y + this.m12 * i_z + this.m13;
o_out.z = this.m20 * i_x + this.m21 * i_y + this.m22 * i_z + this.m23;
return;
}
/**
* double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )
*
* @param i_square
* 計算対象のNyARSquareオブジェクト
* @param i_direction
* @param i_width
* @return
* @throws NyARException
*/
public void transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
{
NyARDoublePoint3d trans = this.__transMat_trans;
double err_threshold = makeErrThreshold(i_square.sqvertex);
//平行移動量計算機に、2D座標系をセット
NyARDoublePoint2d[] vertex_2d = this.__transMat_vertex_2d;
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
this._transsolver.set2dVertex(vertex_2d, 4);
//回転行列を計算
this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);
//回転後の3D座標系から、平行移動量を計算
this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//計算結果の最適化(平行移動量と回転行列の最適化)
o_result_conv.error = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold);
// マトリクスの保存
this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
return;
}
/**
* この関数は、3次元座標を座標変換します。
* 4列目は1と仮定します。
* @param i_x
* 変換する三次元座標(X)
* @param i_y
* 変換する三次元座標(Y)
* @param i_z
* 変換する三次元座標(Z)
* @param o_out
* 変換後の座標を受け取るオブジェクト
*/
public void transform3d(double i_x, double i_y, double i_z, NyARDoublePoint3d o_out)
{
o_out.x = this.m00 * i_x + this.m01 * i_y + this.m02 * i_z + this.m03;
o_out.y = this.m10 * i_x + this.m11 * i_y + this.m12 * i_z + this.m13;
o_out.z = this.m20 * i_x + this.m21 * i_y + this.m22 * i_z + this.m23;
return;
}
/**
*
* @param iw_rotmat
* @param iw_transvec
* @param i_solver
* @param i_offset_3d
* @param i_2d_vertex
* @param i_err_threshold
* @param o_result
* @return
* @
*/
private double optimize(NyARRotMatrix iw_rotmat, NyARDoublePoint3d iw_transvec, INyARTransportVectorSolver i_solver, NyARDoublePoint3d[] i_offset_3d, NyARDoublePoint2d[] i_2d_vertex, double i_err_threshold, NyARDoubleMatrix44 o_result)
{
//System.out.println("START");
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
//初期のエラー値を計算
double min_err = errRate(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4, vertex_3d);
o_result.setValue(iw_rotmat, iw_transvec);
for (int i = 0; i < 5; i++)
{
//変換行列の最適化
this._mat_optimize.modifyMatrix(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4);
double err = errRate(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4, vertex_3d);
//System.out.println("E:"+err);
if (min_err - err < i_err_threshold)
{
//System.out.println("BREAK");
break;
}
i_solver.solveTransportVector(vertex_3d, iw_transvec);
o_result.setValue(iw_rotmat, iw_transvec);
min_err = err;
}
//System.out.println("END");
return(min_err);
}
/**
* 画面座標群と3次元座標群から、平行移動量を計算します。
* 2d座標系は、直前に実行した{@link #set2dVertex}のものを使用します。
*/
public void solveTransportVector(NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint3d o_transfer)
{
double[][] matc = this._mat_c.getArray();
double cpara00 = this._projection_mat.m00;
double cpara01 = this._projection_mat.m01;
double cpara02 = this._projection_mat.m02;
double cpara11 = this._projection_mat.m11;
double cpara12 = this._projection_mat.m12;
double[] cx = this._cx;
double[] cy = this._cy;
//（3D座標？）を一括請求
for (int i = 0; i < 4; i++)
{
int x2 = i + i;
NyARDoublePoint3d point3d_ptr = i_vertex3d[i];
//透視変換？
matc[x2][0] = point3d_ptr.z * cx[i] - cpara00 * point3d_ptr.x - cpara01 * point3d_ptr.y - cpara02 * point3d_ptr.z; // mat_c->m[j*2+0] = wz*pos2d[j][0]-cpara[0][0]*wx-cpara[0][1]*wy-cpara[0][2]*wz;
matc[x2 + 1][0] = point3d_ptr.z * cy[i] - cpara11 * point3d_ptr.y - cpara12 * point3d_ptr.z; // mat_c->m[j*2+1]= wz*pos2d[j][1]-cpara[1][1]*wy-cpara[1][2]*wz;
}
this._mat_e.mul(this._mat_at, this._mat_c);
this._mat_f.mul(this._mat_t, this._mat_e);
double[][] matf = this._mat_f.getArray();
o_transfer.x = matf[0][0]; // trans[0] = mat_f->m[0];
o_transfer.y = matf[1][0];
o_transfer.z = matf[2][0]; // trans[2] = mat_f->m[2];
return;
}
private void rotation2Sincos_ZXY(NyARDoubleMatrix33 i_rot_matrix, TSinCosValue[] o_out, NyARDoublePoint3d o_ang)
{
double x, y, z;
double sina = i_rot_matrix.m21;
if (sina >= 1.0)
{
x = Math.PI / 2;
y = 0;
z = Math.Atan2(-i_rot_matrix.m10, i_rot_matrix.m00);
}
else if (sina <= -1.0)
{
x = -Math.PI / 2;
y = 0;
z = Math.Atan2(-i_rot_matrix.m10, i_rot_matrix.m00);
}
else
{
x = Math.Asin(sina);
y = Math.Atan2(-i_rot_matrix.m20, i_rot_matrix.m22);
z = Math.Atan2(-i_rot_matrix.m01, i_rot_matrix.m11);
}
o_ang.x = x;
o_ang.y = y;
o_ang.z = z;
o_out[0].sin_val = Math.Sin(x);
o_out[0].cos_val = Math.Cos(x);
o_out[1].sin_val = Math.Sin(y);
o_out[1].cos_val = Math.Cos(y);
o_out[2].sin_val = Math.Sin(z);
o_out[2].cos_val = Math.Cos(z);
return;
}
private static NyARMat makeMatAtB(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtB)
{
double v0, v1, v2, v3, v4, v5, v6, v7;
v0 = v1 = v2 = v3 = v4 = v5 = v6 = v7 = 0;
for (int i = 0; i < i_num; i++)
{
double wx = worldCoord[i].x;
double wy = worldCoord[i].y;
double sx = screenCoord[i].x;
double sy = screenCoord[i].y;
v0 += wx * sx;
v1 += wy * sx;
v2 += sx;
v3 += wx * sy;
v4 += wy * sy;
v5 += sy;
v6 += -wx * sx * sx - wx * sy * sy;
v7 += -wy * sx * sx - wy * sy * sy;
}
double[][] t = o_matAtB.getArray();
t[0][0] = v0;
t[1][0] = v1;
t[2][0] = v2;
t[3][0] = v3;
t[4][0] = v4;
t[5][0] = v5;
t[6][0] = v6;
t[7][0] = v7;
return o_matAtB;
}
public void setValue(NyARDoublePoint3d i_in)
{
this.x = i_in.x;
this.y = i_in.y;
this.z = i_in.z;
return;
}
public void setValue(NyARDoublePoint3d i_in)
{
this.x=i_in.x;
this.y=i_in.y;
this.z=i_in.z;
return;
}
/**
* この関数は、姿勢行列のエラーレートを計算します。
* エラーレートは、回転行列、平行移動量、オフセット、観察座標から計算します。
* 通常、ユーザが使うことはありません。
* @param i_rot
* 回転行列
* @param i_trans
* 平行移動量
* @param i_vertex3d
* オフセット位置
* @param i_vertex2d
* 理想座標
* @param i_number_of_vertex
* 評価する頂点数
* @param o_rot_vertex
* 計算過程で得られた、各頂点の三次元座標
* @return
* エラーレート(Σ(理想座標と計算座標の距離[n]^2))
* @
*/
public double errRate(NyARDoubleMatrix33 i_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, NyARDoublePoint3d[] o_rot_vertex)
{
NyARPerspectiveProjectionMatrix cp = this._ref_projection_mat;
double cp00 = cp.m00;
double cp01 = cp.m01;
double cp02 = cp.m02;
double cp11 = cp.m11;
double cp12 = cp.m12;
double err = 0;
for (int i = 0; i < i_number_of_vertex; i++)
{
double x3d, y3d, z3d;
o_rot_vertex[i].x = x3d = i_rot.m00 * i_vertex3d[i].x + i_rot.m01 * i_vertex3d[i].y + i_rot.m02 * i_vertex3d[i].z;
o_rot_vertex[i].y = y3d = i_rot.m10 * i_vertex3d[i].x + i_rot.m11 * i_vertex3d[i].y + i_rot.m12 * i_vertex3d[i].z;
o_rot_vertex[i].z = z3d = i_rot.m20 * i_vertex3d[i].x + i_rot.m21 * i_vertex3d[i].y + i_rot.m22 * i_vertex3d[i].z;
x3d += i_trans.x;
y3d += i_trans.y;
z3d += i_trans.z;
//射影変換
double x2d = x3d * cp00 + y3d * cp01 + z3d * cp02;
double y2d = y3d * cp11 + z3d * cp12;
double h2d = z3d;
//エラーレート計算
double t1 = i_vertex2d[i].x - x2d / h2d;
double t2 = i_vertex2d[i].y - y2d / h2d;
err += t1 * t1 + t2 * t2;
}
return(err / i_number_of_vertex);
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* ARToolKitのarGetTransMatに該当します。
* @see INyARTransMat#transMatContinue
*/
public bool transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
{
NyARDoublePoint3d trans = this.__transMat_trans;
double err_threshold = makeErrThreshold(i_square.sqvertex);
NyARDoublePoint2d[] vertex_2d;
if (this._ref_dist_factor != null)
{
//歪み復元必要
vertex_2d = this.__transMat_vertex_2d;
this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
}
else
{
//歪み復元は不要
vertex_2d = i_square.sqvertex;
}
//平行移動量計算機に、2D座標系をセット
this._transsolver.set2dVertex(vertex_2d, 4);
//回転行列を計算
this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);
//回転後の3D座標系から、平行移動量を計算
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//計算結果の最適化(平行移動量と回転行列の最適化)
this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold, o_result_conv);
return(true);
}
public void getMarkerPlanePos(int id,int i_x,int i_y,ref Vector3 i_out)
{
NyARDoublePoint3d p=new NyARDoublePoint3d();
this.getMarkerPlanePos(id,i_x,i_y,p);
i_out.x=-(float)p.x;
i_out.y=(float)p.y;
i_out.z=(float)p.z;
}
/**
* p2-p1ベクトルのsquare normを計算する。
* @param i_p1
* @param i_p2
* @return
*/
public static double sqNorm(NyARDoublePoint3d i_p1, NyARDoublePoint3d i_p2)
{
double x, y, z;
x = i_p2.x - i_p1.x;
y = i_p2.y - i_p1.y;
z = i_p2.z - i_p1.z;
return x * x + y * y + z * z;
}
/**
* i_pointとのベクトルから距離を計算します。
* @return
*/
public double dist(NyARDoublePoint3d i_point)
{
double x, y, z;
x = this.x - i_point.x;
y = this.y - i_point.y;
z = this.z - i_point.z;
return Math.Sqrt(x * x + y * y + z * z);
}
/**
* 座標値を射影変換します。
* @param i_3dvertex
* 変換元の座標値
* @param o_2d
* 変換後の座標値を受け取るオブジェクト
*/
public void project(NyARDoublePoint3d i_3dvertex, NyARIntPoint2d o_2d)
{
double w = 1 / (i_3dvertex.z * this.m22);
o_2d.x = (int)((i_3dvertex.x * this.m00 + i_3dvertex.y * this.m01 + i_3dvertex.z * this.m02) * w);
o_2d.y = (int)((i_3dvertex.y * this.m11 + i_3dvertex.z * this.m12) * w);
return;
}
/**
* 現在の行列で３次元座標を射影変換します。
* @param i_3dvertex
* @param o_2d
*/
public void projectionConvert(NyARDoublePoint3d i_3dvertex, NyARDoublePoint2d o_2d)
{
double w = i_3dvertex.z * this.m22;
o_2d.x = (i_3dvertex.x * this.m00 + i_3dvertex.y * this.m01 + i_3dvertex.z * this.m02) / w;
o_2d.y = (i_3dvertex.y * this.m11 + i_3dvertex.z * this.m12) / w;
return;
}
/**
* 配列ファクトリ
* @param i_number
* @return
*/
public static NyARDoublePoint3d[] createArray(int i_number)
{
NyARDoublePoint3d[] ret = new NyARDoublePoint3d[i_number];
for (int i = 0; i < i_number; i++)
{
ret[i] = new NyARDoublePoint3d();
}
return ret;
}
/**
* 配列ファクトリ
* @param i_number
* @return
*/
public static NyARDoublePoint3d[] createArray(int i_number)
{
NyARDoublePoint3d[] ret = new NyARDoublePoint3d[i_number];
for (int i = 0; i < i_number; i++)
{
ret[i] = new NyARDoublePoint3d();
}
return(ret);
}
/**
* i_pointとのベクトルから距離を計算します。
* @return
*/
public double dist(NyARDoublePoint3d i_point)
{
double x, y, z;
x = this.x - i_point.x;
y = this.y - i_point.y;
z = this.z - i_point.z;
return(Math.Sqrt(x * x + y * y + z * z));
}
/**
* p2-p1ベクトルのsquare normを計算する。
* @param i_p1
* @param i_p2
* @return
*/
public static double sqNorm(NyARDoublePoint3d i_p1, NyARDoublePoint3d i_p2)
{
double x, y, z;
x = i_p2.x - i_p1.x;
y = i_p2.y - i_p1.y;
z = i_p2.z - i_p1.z;
return(x * x + y * y + z * z);
}
/**
* p2-p1間の距離の二乗値を計算します。
* @param i_p1
* @return
*/
public double sqDist(NyARDoublePoint3d i_p1)
{
double x, y, z;
x = this.x - i_p1.x;
y = this.y - i_p1.y;
z = this.z - i_p1.z;
return(x * x + y * y + z * z);
}
/**
* この関数は、入力した３次元頂点を回転して返します。
* @param i_in_point
* 回転する三次元座標
* @param i_out_point
* 回転した三次元座標
*/
public void getPoint3d(NyARDoublePoint3d i_in_point, NyARDoublePoint3d i_out_point)
{
double x = i_in_point.x;
double y = i_in_point.y;
double z = i_in_point.z;
i_out_point.x = this.m00 * x + this.m01 * y + this.m02 * z;
i_out_point.y = this.m10 * x + this.m11 * y + this.m12 * z;
i_out_point.z = this.m20 * x + this.m21 * y + this.m22 * z;
return;
}
/**
* この関数は、回転行列を最適化します。
* i_vertex3dのオフセット値を、io_rotとi_transで座標変換後に射影変換した２次元座標と、i_vertex2dが最も近くなるように、io_rotを調整します。
* io_rot,i_transの値は、ある程度の精度で求められている必要があります。
* @param io_rot
* 調整する回転行列
* @param i_trans
* 平行移動量
* @param i_vertex3d
* 三次元オフセット座標
* @param i_vertex2d
* 理想座標系の頂点座標
* @param i_number_of_vertex
* 頂点数
* @
*/
public void modifyMatrix(NyARDoubleMatrix33 io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex)
{
NyARDoublePoint3d ang = this.__ang;
// ZXY系のsin/cos値を抽出
io_rot.getZXYAngle(ang);
modifyMatrix(ang, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang);
io_rot.setZXYAngle(ang.x, ang.y, ang.z);
return;
}
/*
* 射影変換式 基本式 ox=(cosc * cosb - sinc * sina * sinb)*ix+(-sinc * cosa)*iy+(cosc * sinb + sinc * sina * cosb)*iz+i_trans.x; oy=(sinc * cosb + cosc * sina *
* sinb)*ix+(cosc * cosa)*iy+(sinc * sinb - cosc * sina * cosb)*iz+i_trans.y; oz=(-cosa * sinb)*ix+(sina)*iy+(cosb * cosa)*iz+i_trans.z;
*
* double ox=(cosc * cosb)*ix+(-sinc * sina * sinb)*ix+(-sinc * cosa)*iy+(cosc * sinb)*iz + (sinc * sina * cosb)*iz+i_trans.x; double oy=(sinc * cosb)*ix
* +(cosc * sina * sinb)*ix+(cosc * cosa)*iy+(sinc * sinb)*iz+(- cosc * sina * cosb)*iz+i_trans.y; double oz=(-cosa * sinb)*ix+(sina)*iy+(cosb *
* cosa)*iz+i_trans.z;
*
* sina,cosaについて解く cx=(cp00*(-sinc*sinb*ix+sinc*cosb*iz)+cp01*(cosc*sinb*ix-cosc*cosb*iz)+cp02*(iy))*sina
* +(cp00*(-sinc*iy)+cp01*((cosc*iy))+cp02*(-sinb*ix+cosb*iz))*cosa
* +(cp00*(i_trans.x+cosc*cosb*ix+cosc*sinb*iz)+cp01*((i_trans.y+sinc*cosb*ix+sinc*sinb*iz))+cp02*(i_trans.z));
* cy=(cp11*(cosc*sinb*ix-cosc*cosb*iz)+cp12*(iy))*sina +(cp11*((cosc*iy))+cp12*(-sinb*ix+cosb*iz))*cosa
* +(cp11*((i_trans.y+sinc*cosb*ix+sinc*sinb*iz))+cp12*(i_trans.z)); ch=(iy)*sina +(-sinb*ix+cosb*iz)*cosa +i_trans.z; sinb,cosb hx=(cp00*(-sinc *
* sina*ix+cosc*iz)+cp01*(cosc * sina*ix+sinc*iz)+cp02*(-cosa*ix))*sinb +(cp01*(sinc*ix-cosc * sina*iz)+cp00*(cosc*ix+sinc * sina*iz)+cp02*(cosa*iz))*cosb
* +(cp00*(i_trans.x+(-sinc*cosa)*iy)+cp01*(i_trans.y+(cosc * cosa)*iy)+cp02*(i_trans.z+(sina)*iy)); double hy=(cp11*(cosc *
* sina*ix+sinc*iz)+cp12*(-cosa*ix))*sinb +(cp11*(sinc*ix-cosc * sina*iz)+cp12*(cosa*iz))*cosb +(cp11*(i_trans.y+(cosc *
* cosa)*iy)+cp12*(i_trans.z+(sina)*iy)); double h =((-cosa*ix)*sinb +(cosa*iz)*cosb +i_trans.z+(sina)*iy); パラメータ返還式 L=2*Σ(d[n]*e[n]+a[n]*b[n])
* J=2*Σ(d[n]*f[n]+a[n]*c[n])/L K=2*Σ(-e[n]*f[n]+b[n]*c[n])/L M=Σ(-e[n]^2+d[n]^2-b[n]^2+a[n]^2)/L 偏微分式 +J*cos(x) +K*sin(x) -sin(x)^2 +cos(x)^2
* +2*M*cos(x)*sin(x)
*/
private double optimizeParamX(double sinb, double cosb, double sinc, double cosc, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle)
{
NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;
double L, J, K, M, N, O;
L = J = K = M = N = O = 0;
double cp00 = cp.m00;
double cp01 = cp.m01;
double cp02 = cp.m02;
double cp11 = cp.m11;
double cp12 = cp.m12;
for (int i = 0; i < i_number_of_vertex; i++)
{
double ix, iy, iz;
ix = i_vertex3d[i].x;
iy = i_vertex3d[i].y;
iz = i_vertex3d[i].z;
double X0 = (cp00 * (-sinc * sinb * ix + sinc * cosb * iz) + cp01 * (cosc * sinb * ix - cosc * cosb * iz) + cp02 * (iy));
double X1 = (cp00 * (-sinc * iy) + cp01 * ((cosc * iy)) + cp02 * (-sinb * ix + cosb * iz));
double X2 = (cp00 * (i_trans.x + cosc * cosb * ix + cosc * sinb * iz) + cp01 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp02 * (i_trans.z));
double Y0 = (cp11 * (cosc * sinb * ix - cosc * cosb * iz) + cp12 * (iy));
double Y1 = (cp11 * ((cosc * iy)) + cp12 * (-sinb * ix + cosb * iz));
double Y2 = (cp11 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp12 * (i_trans.z));
double H0 = (iy);
double H1 = (-sinb * ix + cosb * iz);
double H2 = i_trans.z;
double VX = i_vertex2d[i].x;
double VY = i_vertex2d[i].y;
double a, b, c, d, e, f;
a = (VX * H0 - X0);
b = (VX * H1 - X1);
c = (VX * H2 - X2);
d = (VY * H0 - Y0);
e = (VY * H1 - Y1);
f = (VY * H2 - Y2);
L += d * e + a * b;
N += d * d + a * a;
J += d * f + a * c;
M += e * e + b * b;
K += e * f + b * c;
O += f * f + c * c;
}
L *= 2;
J *= 2;
K *= 2;
return getMinimumErrorAngleFromParam(L, J, K, M, N, O, i_hint_angle);
}
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;
}
/**
* i_in_pointを変換行列で座標変換する。
* @param i_in_point
* @param i_out_point
*/
public void getPoint3d(NyARDoublePoint3d i_in_point, NyARDoublePoint3d i_out_point)
{
double x = i_in_point.x;
double y = i_in_point.y;
double z = i_in_point.z;
i_out_point.x = this.m00 * x + this.m01 * y + this.m02 * z;
i_out_point.y = this.m10 * x + this.m11 * y + this.m12 * z;
i_out_point.z = this.m20 * x + this.m21 * y + this.m22 * z;
return;
}
/**
* 先にセットした2次元座標群と3次元座標群から、平行移動量を計算します。
* 2d座標系は、直前に実行したset2dVertexのものを使用します。
* @param i_vertex3d
* 3次元空間の座標群を設定します。頂点の順番は、画面座標群と同じ順序で格納してください。
* @param o_transfer
* @throws NyARException
*/
public void solveTransportVector(NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint3d o_transfer)
{
int number_of_vertex = this._nmber_of_vertex;
double p00 = this._projection_mat.m00;
double p01 = this._projection_mat.m01;
double p02 = this._projection_mat.m02;
double p11 = this._projection_mat.m11;
double p12 = this._projection_mat.m12;
//行列[A]の3列目のキャッシュ
double[] cx = this._cx;
double[] cy = this._cy;
//回転行列を元座標の頂点群に適応
//[A]T*[b]を計算
double b1 = 0, b2 = 0, b3 = 0;
for (int i = 0; i < number_of_vertex; i++)
{
double w1 = i_vertex3d[i].z * cx[i] - p00 * i_vertex3d[i].x - p01 * i_vertex3d[i].y - p02 * i_vertex3d[i].z;
double w2 = i_vertex3d[i].z * cy[i] - p11 * i_vertex3d[i].y - p12 * i_vertex3d[i].z;
b1 += w1;
b2 += w2;
b3 += cx[i] * w1 + cy[i] * w2;
}
//[A]T*[b]を計算
b3 = p02 * b1 + p12 * b2 - b3;//順番変えたらダメよ
b2 = p01 * b1 + p11 * b2;
b1 = p00 * b1;
//([A]T*[A])*[T]=[A]T*[b]を方程式で解く。
//a01とa10を0と仮定しても良いんじゃないかな？
double a00 = this._a00;
double a01 = this._a01_10;
double a02 = this._a02_20;
double a11 = this._a11;
double a12 = this._a12_21;
double a22 = this._a22;
double t1 = a22 * b2 - a12 * b3;
double t2 = a12 * b2 - a11 * b3;
double t3 = a01 * b3 - a02 * b2;
double t4 = a12 * a12 - a11 * a22;
double t5 = a02 * a12 - a01 * a22;
double t6 = a02 * a11 - a01 * a12;
double det = a00 * t4 - a01 * t5 + a02 * t6;
o_transfer.x = (a01 * t1 - a02 * t2 + b1 * t4) / det;
o_transfer.y = -(a00 * t1 + a02 * t3 + b1 * t5) / det;
o_transfer.z = (a00 * t2 + a01 * t3 + b1 * t6) / det;
return;
}
public void modifyMatrix(NyARDoubleMatrix33 io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex)
{
TSinCosValue[] angles_in = this.__angles_in;// x,y,z
NyARDoublePoint3d ang = this.__ang;
// ZXY系のsin/cos値を抽出
rotation2Sincos_ZXY(io_rot, angles_in, ang);
ang.x += optimizeParamX(angles_in[1], angles_in[2], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.x);
ang.y += optimizeParamY(angles_in[0], angles_in[2], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.y);
ang.z += optimizeParamZ(angles_in[0], angles_in[1], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.z);
io_rot.setZXYAngle(ang.x, ang.y, ang.z);
return;
}
/**
* 複数の頂点を一括して変換する
* @param i_in_point
* @param i_out_point
* @param i_number_of_vertex
*/
public void getPoint3dBatch(NyARDoublePoint3d[] i_in_point, NyARDoublePoint3d[] i_out_point, int i_number_of_vertex)
{
for (int i = i_number_of_vertex - 1; i >= 0; i--)
{
NyARDoublePoint3d out_ptr = i_out_point[i];
NyARDoublePoint3d in_ptr = i_in_point[i];
double x = in_ptr.x;
double y = in_ptr.y;
double z = in_ptr.z;
out_ptr.x = this.m00 * x + this.m01 * y + this.m02 * z;
out_ptr.y = this.m10 * x + this.m11 * y + this.m12 * z;
out_ptr.z = this.m20 * x + this.m21 * y + this.m22 * z;
}
}
/**
* この関数は、スクリーン座標を撮像点座標に変換します。
* 撮像点の座標系は、カメラ座標系になります。
* <p>公式 -
* この関数は、gluUnprojectのビューポートとモデルビュー行列を固定したものです。
* 公式は、以下の物使用しました。
* http://www.opengl.org/sdk/docs/man/xhtml/gluUnProject.xml
* ARToolKitの座標系に合せて計算するため、OpenGLのunProjectとはix,iyの与え方が違います。画面上の座標をそのまま与えてください。
* </p>
* @param ix
* スクリーン上の座標
* @param iy
* 画像上の座標
* @param o_point_on_screen
* 撮像点座標
*/
public void unProject(double ix, double iy, NyARDoublePoint3d o_point_on_screen)
{
double n = (this._frustum_rh.m23 / (this._frustum_rh.m22 - 1));
NyARDoubleMatrix44 m44 = this._inv_frustum_rh;
double v1 = (this._screen_size.w - ix - 1) * 2 / this._screen_size.w - 1.0;//ARToolKitのFrustramに合せてる。
double v2 = (this._screen_size.h - iy - 1) * 2 / this._screen_size.h - 1.0;
double v3 = 2 * n - 1.0;
double b = 1 / (m44.m30 * v1 + m44.m31 * v2 + m44.m32 * v3 + m44.m33);
o_point_on_screen.x = (m44.m00 * v1 + m44.m01 * v2 + m44.m02 * v3 + m44.m03) * b;
o_point_on_screen.y = (m44.m10 * v1 + m44.m11 * v2 + m44.m12 * v3 + m44.m13) * b;
o_point_on_screen.z = (m44.m20 * v1 + m44.m21 * v2 + m44.m22 * v3 + m44.m23) * b;
return;
}
/**
* icpGetU_from_X_by_MatX2U関数
* @param matX2U
* @param coord3d
* @return
*/
public bool setXbyMatX2U(NyARDoubleMatrix44 matX2U, NyARDoublePoint3d coord3d)
{
double hx = matX2U.m00 * coord3d.x + matX2U.m01 * coord3d.y + matX2U.m02 * coord3d.z + matX2U.m03;
double hy = matX2U.m10 * coord3d.x + matX2U.m11 * coord3d.y + matX2U.m12 * coord3d.z + matX2U.m13;
double h = matX2U.m20 * coord3d.x + matX2U.m21 * coord3d.y + matX2U.m22 * coord3d.z + matX2U.m23;
if (h == 0.0)
{
return(false);
}
this.x = hx / h;
this.y = hy / h;
return(true);
}
/**
* この関数は、回転角を最適化します。
* i_vertex3dのオフセット値を、i_angleとi_transで座標変換後に射影変換した２次元座標と、i_vertex2dが最も近くなる値を、o_angleへ返します。
* io_rot,i_transの値は、ある程度の精度で求められている必要があります。
* @param i_angle
* 回転角
* @param i_trans
* 平行移動量
* @param i_vertex3d
* 三次元オフセット座標
* @param i_vertex2d
* 理想座標系の頂点座標
* @param i_number_of_vertex
* 頂点数
* @param o_angle
* 調整した回転角を受け取る配列
* @
*/
public void modifyMatrix(NyARDoublePoint3d i_angle, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, NyARDoublePoint3d o_angle)
{
// ZXY系のsin/cos値を抽出
double sinx = Math.Sin(i_angle.x);
double cosx = Math.Cos(i_angle.x);
double siny = Math.Sin(i_angle.y);
double cosy = Math.Cos(i_angle.y);
double sinz = Math.Sin(i_angle.z);
double cosz = Math.Cos(i_angle.z);
o_angle.x = i_angle.x + optimizeParamX(siny, cosy, sinz, cosz, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.x);
o_angle.y = i_angle.y + optimizeParamY(sinx, cosx, sinz, cosz, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.y);
o_angle.z = i_angle.z + optimizeParamZ(sinx, cosx, siny, cosy, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.z);
return;
}
private static bool kpmUtilGetPose_binary(NyARParam i_cparam, FeaturePairStack matchData, NyARDoubleMatrix44 i_transmat, NyARTransMatResultParam i_resultparam)
{
NyARDoubleMatrix44 initMatXw2Xc = new NyARDoubleMatrix44();
// ARdouble err;
int i;
if (matchData.getLength() < 4)
{
return(false);
}
NyARDoublePoint2d[] sCoord = NyARDoublePoint2d.createArray(matchData.getLength());
NyARDoublePoint3d[] wCoord = NyARDoublePoint3d.createArray(matchData.getLength());
for (i = 0; i < matchData.getLength(); i++)
{
sCoord[i].x = matchData.getItem(i).query.x;
sCoord[i].y = matchData.getItem(i).query.y;
wCoord[i].x = matchData.getItem(i).ref_.pos3d.x;
wCoord[i].y = matchData.getItem(i).ref_.pos3d.y;
wCoord[i].z = 0.0;
}
NyARIcpPlane icp_planer = new NyARIcpPlane(i_cparam.getPerspectiveProjectionMatrix());
if (!icp_planer.icpGetInitXw2Xc_from_PlanarData(sCoord, wCoord, matchData.getLength(), initMatXw2Xc))
{
return(false);
}
/*
* printf("--- Init pose ---\n"); for( int j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) printf(" %8.3f",
* initMatXw2Xc[j][i]); printf("\n"); }
*/
NyARIcpPoint icp_point = new NyARIcpPoint(i_cparam.getPerspectiveProjectionMatrix());
icp_point.icpPoint(sCoord, wCoord, matchData.getLength(), initMatXw2Xc, i_transmat, i_resultparam);
if (i_resultparam.last_error > 10.0f)
{
return(false);
}
return(true);
}
/**
* この関数は、平行移動量と回転行列をセットして、インスタンスのパラメータを更新します。
* 拡大率は1倍にセットします。
* @param i_rot
* 設定する回転行列
* @param i_trans
* 設定する平行移動量
*/
public void setValue(NyARDoubleMatrix33 i_rot, NyARDoublePoint3d i_trans)
{
this.m00=i_rot.m00;
this.m01=i_rot.m01;
this.m02=i_rot.m02;
this.m03=i_trans.x;
this.m10 =i_rot.m10;
this.m11 =i_rot.m11;
this.m12 =i_rot.m12;
this.m13 =i_trans.y;
this.m20 = i_rot.m20;
this.m21 = i_rot.m21;
this.m22 = i_rot.m22;
this.m23 = i_trans.z;
this.m30=this.m31=this.m32=0;
this.m33=1.0;
return;
}
/**
* パラメータで変換行列を更新します。
*
* @param i_rot
* @param i_off
* @param i_trans
*/
public void updateMatrixValue(NyARRotMatrix i_rot, NyARDoublePoint3d i_trans, NyARTransMatResult o_result)
{
o_result.m00 = i_rot.m00;
o_result.m01 = i_rot.m01;
o_result.m02 = i_rot.m02;
o_result.m03 = i_trans.x;
o_result.m10 = i_rot.m10;
o_result.m11 = i_rot.m11;
o_result.m12 = i_rot.m12;
o_result.m13 = i_trans.y;
o_result.m20 = i_rot.m20;
o_result.m21 = i_rot.m21;
o_result.m22 = i_rot.m22;
o_result.m23 = i_trans.z;
o_result.has_value = true;
return;
}
public void icpGetInitXw2XcSub(NyARDoubleMatrix44 rot,
NyARDoublePoint2d[] pos2d, NyARDoublePoint3d[] ppos3d, int num,
NyARDoubleMatrix44 conv)
{
NyARDoublePoint3d off = makeOffset(ppos3d, num, this.__off);
NyARDoubleMatrix33 matd = makeMatD(this._cparam, pos2d, num, this.__matd);
double[] mate = makeMatE(this._cparam, rot, pos2d, ppos3d, off, num, this.__mate);
conv.setValue(rot);
conv.m03 = matd.m00 * mate[0] + matd.m01 * mate[1] + matd.m02 * mate[2];
conv.m13 = matd.m10 * mate[0] + matd.m11 * mate[1] + matd.m12 * mate[2];
conv.m23 = matd.m20 * mate[0] + matd.m21 * mate[1] + matd.m22 * mate[2];
conv.m03 = conv.m00 * off.x + conv.m01 * off.y + conv.m02 * off.z + conv.m03;
conv.m13 = conv.m10 * off.x + conv.m11 * off.y + conv.m12 * off.z + conv.m13;
conv.m23 = conv.m20 * off.x + conv.m21 * off.y + conv.m22 * off.z + conv.m23;
return;
}
/**
* この関数は、行列の回転成分から、ZXY系の角度値を計算します。
* @param o_out
* 角度値を受け取るオブジェクトです。
* 角度値の範囲は、0-PI(要確認)です。
*/
public void getZXYAngle(NyARDoublePoint3d o_out)
{
double sina = this.m21;
if (sina >= 1.0)
{
o_out.x = Math.PI / 2;
o_out.y = 0;
o_out.z = Math.Atan2(-this.m10, this.m00);
}
else if (sina <= -1.0)
{
o_out.x = -Math.PI / 2;
o_out.y = 0;
o_out.z = Math.Atan2(-this.m10, this.m00);
}
else
{
o_out.x = Math.Asin(sina);
o_out.z = Math.Atan2(-this.m01, this.m11);
o_out.y = Math.Atan2(-this.m20, this.m22);
}
}
/**
* この関数は、0-PIの間で値を返します。
* @param o_out
*/
public void getZXYAngle(NyARDoublePoint3d o_out)
{
double sina = this.m21;
if (sina >= 1.0)
{
o_out.x = Math.PI / 2;
o_out.y = 0;
o_out.z = Math.Atan2(-this.m10, this.m00);
}
else if (sina <= -1.0)
{
o_out.x = -Math.PI / 2;
o_out.y = 0;
o_out.z = Math.Atan2(-this.m10, this.m00);
}
else
{
o_out.x = Math.Asin(sina);
o_out.z = Math.Atan2(-this.m01, this.m11);
o_out.y = Math.Atan2(-this.m20, this.m22);
}
}
/**
* 平行移動量と回転行列をセットします。この関数は、INyARTransmatインタフェイスのクラスが結果を保存するために使います。
* @param i_rot
* @param i_trans
*/
public void setValue(NyARDoubleMatrix33 i_rot, NyARDoublePoint3d i_trans, double i_error)
{
this.m00 = i_rot.m00;
this.m01 = i_rot.m01;
this.m02 = i_rot.m02;
this.m03 = i_trans.x;
this.m10 = i_rot.m10;
this.m11 = i_rot.m11;
this.m12 = i_rot.m12;
this.m13 = i_trans.y;
this.m20 = i_rot.m20;
this.m21 = i_rot.m21;
this.m22 = i_rot.m22;
this.m23 = i_trans.z;
this.m30 = this.m31 = this.m32 = 0;
this.m33 = 1.0;
this.has_value = true;
this.last_error = i_error;
return;
}
/**
* この関数は、回転行列を最適化します。
* i_vertex3dのオフセット値を、io_rotとi_transで座標変換後に射影変換した２次元座標と、i_vertex2dが最も近くなるように、io_rotを調整します。
* io_rot,i_transの値は、ある程度の精度で求められている必要があります。
* @param io_rot
* 調整する回転行列
* @param i_trans
* 平行移動量
* @param i_vertex3d
* 三次元オフセット座標
* @param i_vertex2d
* 理想座標系の頂点座標
* @param i_number_of_vertex
* 頂点数
* @
*/
public void modifyMatrix(NyARDoubleMatrix33 io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex)
{
NyARDoublePoint3d ang = this.__ang;
// ZXY系のsin/cos値を抽出
io_rot.getZXYAngle(ang);
modifyMatrix(ang, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang);
io_rot.setZXYAngle(ang.x, ang.y, ang.z);
return;
}
/**
* 画面上の点と原点を結ぶ直線と任意姿勢の平面の交差点を、平面の座標系で取得します。
* ARToolKitの本P175周辺の実装と同じです。
* <p>
* このAPIは繰り返し使用には最適化されていません。同一なi_matに繰り返しアクセスするときは、展開してください。
* </p>
* @param ix
* スクリーン上の座標
* @param iy
* スクリーン上の座標
* @param i_mat
* 平面の姿勢行列です。
* @param o_pos
* 結果を受け取るオブジェクトです。
* @return
* 計算に成功すると、trueを返します。
*/
public bool unProjectOnMatrix(double ix, double iy, NyARDoubleMatrix44 i_mat, NyARDoublePoint3d o_pos)
{
//交点をカメラ座標系で計算
unProjectOnCamera(ix, iy, i_mat, o_pos);
//座標系の変換
NyARDoubleMatrix44 m = new NyARDoubleMatrix44();
if (!m.inverse(i_mat))
{
return(false);
}
m.transform3d(o_pos, o_pos);
return(true);
}
/**
* カメラ座標系の点を、スクリーン座標の点へ変換します。
* @param i_pos
* カメラ座標系の点
* @param o_pos2d
* 結果を受け取るオブジェクトです。
*/
public void project(NyARDoublePoint3d i_pos, NyARDoublePoint2d o_pos2d)
{
this.project(i_pos.x, i_pos.y, i_pos.z, o_pos2d);
}
/**
* この関数は、行列を回転行列として、ZXY系の角度値をセットします。
* @param i_angle
* セットする角度値です。
*/
public void setZXYAngle(NyARDoublePoint3d i_angle)
{
setZXYAngle(i_angle.x, i_angle.y, i_angle.z);
return;
}
/**
* 画面座標群と3次元座標群から、平行移動量を計算します。
* 2d座標系は、直前に実行した{@link #set2dVertex}のものを使用します。
*/
public void solveTransportVector(NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint3d o_transfer)
{
double[][] matc = this._mat_c.getArray();
double cpara00 = this._projection_mat.m00;
double cpara01 = this._projection_mat.m01;
double cpara02 = this._projection_mat.m02;
double cpara11 = this._projection_mat.m11;
double cpara12 = this._projection_mat.m12;
double[] cx = this._cx;
double[] cy = this._cy;
//（3D座標？）を一括請求
for (int i = 0; i < 4; i++)
{
int x2 = i + i;
NyARDoublePoint3d point3d_ptr = i_vertex3d[i];
//透視変換？
matc[x2][0] = point3d_ptr.z * cx[i] - cpara00 * point3d_ptr.x - cpara01 * point3d_ptr.y - cpara02 * point3d_ptr.z;// mat_c->m[j*2+0] = wz*pos2d[j][0]-cpara[0][0]*wx-cpara[0][1]*wy-cpara[0][2]*wz;
matc[x2 + 1][0] = point3d_ptr.z * cy[i] - cpara11 * point3d_ptr.y - cpara12 * point3d_ptr.z;// mat_c->m[j*2+1]= wz*pos2d[j][1]-cpara[1][1]*wy-cpara[1][2]*wz;
}
this._mat_e.mul(this._mat_at, this._mat_c);
this._mat_f.mul(this._mat_t, this._mat_e);
double[][] matf = this._mat_f.getArray();
o_transfer.x = matf[0][0];// trans[0] = mat_f->m[0];
o_transfer.y = matf[1][0];
o_transfer.z = matf[2][0];// trans[2] = mat_f->m[2];
return;
}
/**
* この関数は、3次元座標を座標変換します。
* @param i_position
* 変換する三次元座標
* @param o_out
* 変換後の座標を受け取るオブジェクト
*/
public void transformVertex(NyARDoublePoint3d i_position, NyARDoublePoint3d o_out)
{
transformVertex(i_position.x, i_position.y, i_position.z, o_out);
return;
}
/**
* コンストラクタです。
* 参照する射影変換オブジェクトを指定して、インスタンスを生成します。
* @param i_matrix
* 参照する射影変換オブジェクト
* @
*/
public NyARRotMatrix_ARToolKit(NyARPerspectiveProjectionMatrix i_matrix)
: base(i_matrix)
{
this._angle = new NyARDoublePoint3d();
return;
}
public void modifyMatrix(NyARDoubleMatrix33 io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex)
{
TSinCosValue[] angles_in = this.__angles_in;// x,y,z
NyARDoublePoint3d ang = this.__ang;
// ZXY系のsin/cos値を抽出
rotation2Sincos_ZXY(io_rot, angles_in, ang);
ang.x += optimizeParamX(angles_in[1], angles_in[2], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.x);
ang.y += optimizeParamY(angles_in[0], angles_in[2], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.y);
ang.z += optimizeParamZ(angles_in[0], angles_in[1], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.z);
io_rot.setZXYAngle(ang.x, ang.y, ang.z);
return;
}
/**
* 画面上の点と原点を結ぶ直線と任意姿勢の平面の交差点を、平面の座標系で取得します。
* ARToolKitの本P175周辺の実装と同じです。
* <p>
* このAPIは繰り返し使用には最適化されていません。同一なi_matに繰り返しアクセスするときは、展開してください。
* </p>
* @param ix
* スクリーン上の座標
* @param iy
* スクリーン上の座標
* @param i_mat
* 平面の姿勢行列です。
* @param o_pos
* 結果を受け取るオブジェクトです。
* @return
* 計算に成功すると、trueを返します。
*/
public bool unProjectOnMatrix(double ix, double iy, NyARDoubleMatrix44 i_mat, NyARDoublePoint3d o_pos)
{
//交点をカメラ座標系で計算
unProjectOnCamera(ix, iy, i_mat, o_pos);
//座標系の変換
NyARDoubleMatrix44 m = new NyARDoubleMatrix44();
if (!m.inverse(i_mat))
{
return false;
}
m.transform3d(o_pos, o_pos);
return true;
}
/**
* この関数は、3次元座標を座標変換します。
* 4列目は1と仮定します。
* @param i_in
* 返還前する座標値
* @param o_out
* 変換後の座標を受け取るオブジェクト
*/
public void transform3d(NyARDoublePoint3d i_in, NyARDoublePoint3d o_out)
{
transform3d(i_in.x, i_in.y, i_in.z, o_out);
}
/**
* この関数は、入力した複数の３次元頂点を回転して返します。
* @param i_in_point
* 回転する三次元座標の配列
* @param i_out_point
* 回転した三次元座標の配列
* @param i_number_of_vertex
* 回転する座標の個数
*/
public void getPoint3dBatch(NyARDoublePoint3d[] i_in_point, NyARDoublePoint3d[] i_out_point, int i_number_of_vertex)
{
for (int i = i_number_of_vertex - 1; i >= 0; i--)
{
NyARDoublePoint3d out_ptr = i_out_point[i];
NyARDoublePoint3d in_ptr = i_in_point[i];
double x = in_ptr.x;
double y = in_ptr.y;
double z = in_ptr.z;
out_ptr.x = this.m00 * x + this.m01 * y + this.m02 * z;
out_ptr.y = this.m10 * x + this.m11 * y + this.m12 * z;
out_ptr.z = this.m20 * x + this.m21 * y + this.m22 * z;
}
return;
}
/**
* この関数は、回転角を最適化します。
* i_vertex3dのオフセット値を、i_angleとi_transで座標変換後に射影変換した２次元座標と、i_vertex2dが最も近くなる値を、o_angleへ返します。
* io_rot,i_transの値は、ある程度の精度で求められている必要があります。
* @param i_angle
* 回転角
* @param i_trans
* 平行移動量
* @param i_vertex3d
* 三次元オフセット座標
* @param i_vertex2d
* 理想座標系の頂点座標
* @param i_number_of_vertex
* 頂点数
* @param o_angle
* 調整した回転角を受け取る配列
* @
*/
public void modifyMatrix(NyARDoublePoint3d i_angle, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, NyARDoublePoint3d o_angle)
{
// ZXY系のsin/cos値を抽出
double sinx = Math.Sin(i_angle.x);
double cosx = Math.Cos(i_angle.x);
double siny = Math.Sin(i_angle.y);
double cosy = Math.Cos(i_angle.y);
double sinz = Math.Sin(i_angle.z);
double cosz = Math.Cos(i_angle.z);
o_angle.x = i_angle.x + optimizeParamX(siny, cosy, sinz, cosz, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.x);
o_angle.y = i_angle.y + optimizeParamY(sinx, cosx, sinz, cosz, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.y);
o_angle.z = i_angle.z + optimizeParamZ(sinx, cosx, siny, cosy, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.z);
return;
}
/**
* ターゲット座標系の4頂点でかこまれる領域を射影した平面から、RGB画像をo_rasterに取得します。
* @param i_vertex
* ターゲットのオフセットを基準値とした、頂点座標。要素数は4であること。(mm単位)
* @param i_matrix
* i_vertexに適応する変換行列。
* ターゲットの姿勢行列を指定すると、ターゲット座標系になります。不要ならばnullを設定してください。
* @param i_resolution
* 1ピクセルあたりのサンプリング値(n^2表現)
* @param o_raster
* 出力ラスタ
* @return
* @throws NyARException
* <p>メモ:この関数にはnewが残ってるので注意</p>
*/
public bool getRgbPatt3d(NyARRealitySource i_src, NyARDoublePoint3d[] i_vertex, NyARDoubleMatrix44 i_matrix, int i_resolution, INyARRgbRaster o_raster)
{
Debug.Assert(this._target_type==RT_KNOWN);
NyARDoublePoint2d[] da4=this._ref_pool._wk_da2_4;
NyARDoublePoint3d v3d=new NyARDoublePoint3d();
if(i_matrix!=null){
//姿勢変換してから射影変換
for(int i=3;i>=0;i--){
//姿勢を変更して射影変換
i_matrix.transform3d(i_vertex[i],v3d);
this._transform_matrix.transform3d(v3d,v3d);
this._ref_pool._ref_prj_mat.project(v3d,da4[i]);
}
}else{
//射影変換のみ
for(int i=3;i>=0;i--){
//姿勢を変更して射影変換
this._transform_matrix.transform3d(i_vertex[i],v3d);
this._ref_pool._ref_prj_mat.project(v3d,da4[i]);
}
}
//パターンの取得
return i_src.refPerspectiveRasterReader().copyPatt(da4,0,0,i_resolution, o_raster);
}
/**
* i_bufに{@link #getAngle()}の結果をコピーして返します。
* @return
* 複製した{@link #getAngle()}の値
*/
public NyARDoublePoint3d getAngle(NyARDoublePoint3d i_buf)
{
i_buf.setValue(this._angle);
return i_buf;
}
/**
* この関数は、回転行列を最適化します。
* ARToolKitのarGetRotに相当します。
*/
public double modifyMatrix(NyARRotMatrix_ARToolKit io_rot, NyARDoublePoint3d trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d)
{
double factor;
double a2, b2, c2;
double h, x, y;
double err, minerr = 0;
int t1, t2, t3;
int best_idx = 0;
factor = 10.0 * Math.PI / 180.0;
double rot0, rot1, rot2;
double combo00, combo01, combo02, combo03, combo10, combo11, combo12, combo13, combo20, combo21, combo22, combo23;
double combo02_2, combo02_5, combo02_8, combo02_11;
double combo22_2, combo22_5, combo22_8, combo22_11;
double combo12_2, combo12_5, combo12_8, combo12_11;
// vertex展開
double VX00, VX01, VX02, VX10, VX11, VX12, VX20, VX21, VX22, VX30, VX31, VX32;
VX00 = i_vertex3d[0].x;
VX01 = i_vertex3d[0].y;
VX02 = i_vertex3d[0].z;
VX10 = i_vertex3d[1].x;
VX11 = i_vertex3d[1].y;
VX12 = i_vertex3d[1].z;
VX20 = i_vertex3d[2].x;
VX21 = i_vertex3d[2].y;
VX22 = i_vertex3d[2].z;
VX30 = i_vertex3d[3].x;
VX31 = i_vertex3d[3].y;
VX32 = i_vertex3d[3].z;
double P2D00, P2D01, P2D10, P2D11, P2D20, P2D21, P2D30, P2D31;
P2D00 = i_vertex2d[0].x;
P2D01 = i_vertex2d[0].y;
P2D10 = i_vertex2d[1].x;
P2D11 = i_vertex2d[1].y;
P2D20 = i_vertex2d[2].x;
P2D21 = i_vertex2d[2].y;
P2D30 = i_vertex2d[3].x;
P2D31 = i_vertex2d[3].y;
NyARPerspectiveProjectionMatrix prjmat = this._projection_mat_ref;
double CP0 = prjmat.m00, CP1 = prjmat.m01, CP2 = prjmat.m02, CP4 = prjmat.m10, CP5 = prjmat.m11, CP6 = prjmat.m12, CP8 = prjmat.m20, CP9 = prjmat.m21, CP10 = prjmat.m22;
combo03 = CP0 * trans.x + CP1 * trans.y + CP2 * trans.z + prjmat.m03;
combo13 = CP4 * trans.x + CP5 * trans.y + CP6 * trans.z + prjmat.m13;
combo23 = CP8 * trans.x + CP9 * trans.y + CP10 * trans.z + prjmat.m23;
double CACA, SASA, SACA, CA, SA;
double CACACB, SACACB, SASACB, CASB, SASB;
double SACASC, SACACBSC, SACACBCC, SACACC;
double[][] double1D = this.__modifyMatrix_double1D;
double[] a_factor = double1D[1];
double[] sinb = double1D[2];
double[] cosb = double1D[3];
double[] b_factor = double1D[4];
double[] sinc = double1D[5];
double[] cosc = double1D[6];
double[] c_factor = double1D[7];
double w, w2;
double wsin, wcos;
NyARDoublePoint3d angle = io_rot.getAngle();
a2 = angle.x;
b2 = angle.y;
c2 = angle.z;
// comboの3行目を先に計算
for (int i = 0; i < 10; i++)
{
minerr = 1000000000.0;
// sin-cosテーブルを計算(これが外に出せるとは…。)
for (int j = 0; j < 3; j++)
{
w2 = factor * (j - 1);
w = a2 + w2;
a_factor[j] = w;
w = b2 + w2;
b_factor[j] = w;
sinb[j] = Math.Sin(w);
cosb[j] = Math.Cos(w);
w = c2 + w2;
c_factor[j] = w;
sinc[j] = Math.Sin(w);
cosc[j] = Math.Cos(w);
}
//
for (t1 = 0; t1 < 3; t1++)
{
SA = Math.Sin(a_factor[t1]);
CA = Math.Cos(a_factor[t1]);
// Optimize
CACA = CA * CA;
SASA = SA * SA;
SACA = SA * CA;
for (t2 = 0; t2 < 3; t2++)
{
wsin = sinb[t2];
wcos = cosb[t2];
CACACB = CACA * wcos;
SACACB = SACA * wcos;
SASACB = SASA * wcos;
CASB = CA * wsin;
SASB = SA * wsin;
// comboの計算1
combo02 = CP0 * CASB + CP1 * SASB + CP2 * wcos;
combo12 = CP4 * CASB + CP5 * SASB + CP6 * wcos;
combo22 = CP8 * CASB + CP9 * SASB + CP10 * wcos;
combo02_2 = combo02 * VX02 + combo03;
combo02_5 = combo02 * VX12 + combo03;
combo02_8 = combo02 * VX22 + combo03;
combo02_11 = combo02 * VX32 + combo03;
combo12_2 = combo12 * VX02 + combo13;
combo12_5 = combo12 * VX12 + combo13;
combo12_8 = combo12 * VX22 + combo13;
combo12_11 = combo12 * VX32 + combo13;
combo22_2 = combo22 * VX02 + combo23;
combo22_5 = combo22 * VX12 + combo23;
combo22_8 = combo22 * VX22 + combo23;
combo22_11 = combo22 * VX32 + combo23;
for (t3 = 0; t3 < 3; t3++)
{
wsin = sinc[t3];
wcos = cosc[t3];
SACASC = SACA * wsin;
SACACC = SACA * wcos;
SACACBSC = SACACB * wsin;
SACACBCC = SACACB * wcos;
rot0 = CACACB * wcos + SASA * wcos + SACACBSC - SACASC;
rot1 = SACACBCC - SACACC + SASACB * wsin + CACA * wsin;
rot2 = -CASB * wcos - SASB * wsin;
combo00 = CP0 * rot0 + CP1 * rot1 + CP2 * rot2;
combo10 = CP4 * rot0 + CP5 * rot1 + CP6 * rot2;
combo20 = CP8 * rot0 + CP9 * rot1 + CP10 * rot2;
rot0 = -CACACB * wsin - SASA * wsin + SACACBCC - SACACC;
rot1 = -SACACBSC + SACASC + SASACB * wcos + CACA * wcos;
rot2 = CASB * wsin - SASB * wcos;
combo01 = CP0 * rot0 + CP1 * rot1 + CP2 * rot2;
combo11 = CP4 * rot0 + CP5 * rot1 + CP6 * rot2;
combo21 = CP8 * rot0 + CP9 * rot1 + CP10 * rot2;
//
err = 0.0;
h = combo20 * VX00 + combo21 * VX01 + combo22_2;
x = P2D00 - (combo00 * VX00 + combo01 * VX01 + combo02_2) / h;
y = P2D01 - (combo10 * VX00 + combo11 * VX01 + combo12_2) / h;
err += x * x + y * y;
h = combo20 * VX10 + combo21 * VX11 + combo22_5;
x = P2D10 - (combo00 * VX10 + combo01 * VX11 + combo02_5) / h;
y = P2D11 - (combo10 * VX10 + combo11 * VX11 + combo12_5) / h;
err += x * x + y * y;
h = combo20 * VX20 + combo21 * VX21 + combo22_8;
x = P2D20 - (combo00 * VX20 + combo01 * VX21 + combo02_8) / h;
y = P2D21 - (combo10 * VX20 + combo11 * VX21 + combo12_8) / h;
err += x * x + y * y;
h = combo20 * VX30 + combo21 * VX31 + combo22_11;
x = P2D30 - (combo00 * VX30 + combo01 * VX31 + combo02_11) / h;
y = P2D31 - (combo10 * VX30 + combo11 * VX31 + combo12_11) / h;
err += x * x + y * y;
if (err < minerr)
{
minerr = err;
a2 = a_factor[t1];
b2 = b_factor[t2];
c2 = c_factor[t3];
best_idx = t1 + t2 * 3 + t3 * 9;
}
}
}
}
if (best_idx == (1 + 3 + 9))
{
factor *= 0.5;
}
}
io_rot.setAngle(a2, b2, c2);
/* printf("factor = %10.5f\n", factor*180.0/MD_PI); */
return minerr / 4;
}
/**
* 座標値を射影変換します。
* @param i_3dvertex
* 変換元の座標値
* @param o_2d
* 変換後の座標値を受け取るオブジェクト
*/
public void project(NyARDoublePoint3d i_3dvertex, NyARIntPoint2d o_2d)
{
double w = 1 / (i_3dvertex.z * this.m22);
o_2d.x = (int)((i_3dvertex.x * this.m00 + i_3dvertex.y * this.m01 + i_3dvertex.z * this.m02) * w);
o_2d.y = (int)((i_3dvertex.y * this.m11 + i_3dvertex.z * this.m12) * w);
return;
}
public override bool icpPoint(NyARDoublePoint2d[] screenCoord,
NyARDoublePoint3d[] worldCoord, int num,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc, NyARTransMatResultParam o_result_param)
{
Debug.Assert(num >= 4);
double err0 = 0, err1;
NyARIcpUtils.U u = this.__u;
NyARIcpUtils.DeltaS dS = this.__dS;
//ワークオブジェクトのリセット
if (this.__jus.getArraySize() < num)
{
this.__jus = new NyARIcpUtils.JusStack(num);
this.__E = new double[num];
this.__E2 = new double[num];
this.__du = NyARDoublePoint2d.createArray(num);
}
NyARIcpUtils.JusStack jus = this.__jus;
double[] E = this.__E;
double[] E2 = this.__E2;
NyARDoublePoint2d[] du = this.__du;
NyARDoubleMatrix44 matXw2U = this.__matXw2U;
int inlierNum = (int)(num * this.getInlierProbability()) - 1;
if (inlierNum < 3)
{
inlierNum = 3;
}
o_matxw2xc.setValue(initMatXw2Xc);
for (int i = 0;; i++) {
matXw2U.mul(this._ref_matXc2U, o_matxw2xc);
for (int j = 0; j < num; j++)
{
if (!u.setXbyMatX2U(matXw2U, worldCoord[j]))
{
return false;
}
double dx = screenCoord[j].x - u.x;
double dy = screenCoord[j].y - u.y;
du[j].x = dx;
du[j].y = dy;
E[j] = E2[j] = dx * dx + dy * dy;
}
Array.Sort(E2, 0, num);// qsort(E2, data->num, sizeof(ARdouble), compE);
double K2 = E2[inlierNum] * K2_FACTOR;
if (K2 < 16.0)
{
K2 = 16.0;
}
err1 = 0.0;
for (int j = 0; j < num; j++)
{
if (E2[j] > K2)
{
err1 += K2 / 6.0;
}
else
{
err1 += K2 / 6.0 * (1.0 - (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2));
}
}
err1 /= num;
if (err1 < this.breakLoopErrorThresh)
{
break;
}
if (i > 0 && err1 < this.breakLoopErrorThresh2 && err1 / err0 > this.breakLoopErrorRatioThresh)
{
break;
}
if (i == this._maxLoop)
{
break;
}
err0 = err1;
jus.clear();
for (int j = 0; j < num; j++)
{
if (E[j] <= K2)
{
double W = (1.0 - E[j] / K2) * (1.0 - E[j] / K2);
if(!jus.push(this._ref_matXc2U,o_matxw2xc, worldCoord[j],du[j],W))
{
return false;
}
}
}
if (jus.getLength() < 3)
{
return false;
}
if (!dS.setJusArray(jus))
{
return false;
}
dS.makeMat(o_matxw2xc);
}
if(o_result_param!=null){
o_result_param.last_error=err1;
}
return true;
}
/**
* この関数は、ZXY系の角度値を、回転行列にセットします。
* @param i_angle
* ZXY系のラジアン値
*/
public void initRotByAngle(NyARDoublePoint3d i_angle)
{
this.setZXYAngle(i_angle);
}
public void transformVertex(NyARDoublePoint3d i_in,NyARDoublePoint3d o_out)
{
transformVertex(i_in.x,i_in.y,i_in.z,o_out);
}
/**
* カメラ座標系の4頂点でかこまれる領域から、RGB画像をo_rasterに取得します。
* @param i_vertex
* @param i_matrix
* i_vertexに適応する変換行列。
* ターゲットの姿勢行列を指定すると、ターゲット座標系になります。不要ならばnullを設定してください。
* @param i_resolution
* @param o_raster
* @return
* @throws NyARException
*/
public bool getRgbPatt3d(NyARRealitySource i_src, NyARDoublePoint3d[] i_vertex, NyARDoubleMatrix44 i_matrix, int i_resolution, INyARRgbRaster o_raster)
{
NyARDoublePoint2d[] vx=NyARDoublePoint2d.createArray(4);
if(i_matrix!=null){
//姿勢変換してから射影変換
NyARDoublePoint3d v3d=new NyARDoublePoint3d();
for(int i=3;i>=0;i--){
i_matrix.transform3d(i_vertex[i],v3d);
this._ref_prjmat.project(v3d,vx[i]);
}
}else{
//射影変換のみ
for(int i=3;i>=0;i--){
this._ref_prjmat.project(i_vertex[i],vx[i]);
}
}
//パターンの取得
return i_src.refPerspectiveRasterReader().copyPatt(vx, 0, 0, i_resolution, o_raster);
}