/**
*
* @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;
}
/**
* この関数は、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 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);
}
/**
* 座標値を射影変換します。
* @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;
}
/**
* この関数は、回転行列を最適化します。
* 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;
}
/**
* この関数は、入力した３次元頂点を回転して返します。
* @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;
}
/**
* この関数は、入力した３次元頂点を回転して返します。
* @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;
}
/**
* 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);
}
/**
* この関数は、スクリーン座標を撮像点座標に変換します。
* 撮像点の座標系は、カメラ座標系になります。
* <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;
}
/**
* この関数は、回転角を最適化します。
* 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;
}
/**
* この関数は、入力した複数の３次元頂点を回転して返します。
* @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;
}
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;
}
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;
}
/**
* この関数は、平行移動量と回転行列をセットして、インスタンスのパラメータを更新します。
* 拡大率は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;
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* ARToolKitのarGetTransMatに該当します。
* @see INyARTransMat#transMatContinue
*/
public bool transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARDoubleMatrix44 o_result, NyARTransMatResultParam o_param)
{
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);
//回転行列を計算
if (!this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex))
{
return(false);
}
//回転後の3D座標系から、平行移動量を計算
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//計算結果の最適化(平行移動量と回転行列の最適化)
double err = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold, o_result);
//必要なら計算パラメータを返却
if (o_param != null)
{
o_param.last_error = err;
}
return(true);
}
/**
* この関数は、行列の回転成分から、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);
}
}
/**
* {@link #icpGetInitXw2XcSub}関数のmat_eを計算します。
*/
private static double[] makeMatE(NyARDoubleMatrix44 i_cp, NyARDoubleMatrix44 rot, NyARDoublePoint2d[] pos2d, NyARDoublePoint3d[] pos3d, NyARDoublePoint3d offset, int i_num, double[] o_val)
{
double v0 = 0;
double v1 = 0;
double v2 = 0;
for (int j = 0; j < i_num; j++)
{
double p3x = pos3d[j].x + offset.x;
double p3y = pos3d[j].y + offset.y;
double p3z = pos3d[j].z + offset.z;
double wx = rot.m00 * p3x + rot.m01 * p3y + rot.m02 * p3z;
double wy = rot.m10 * p3x + rot.m11 * p3y + rot.m12 * p3z;
double wz = rot.m20 * p3x + rot.m21 * p3y + rot.m22 * p3z;
double c1 = wz * pos2d[j].x - i_cp.m00 * wx - i_cp.m01 * wy - i_cp.m02 * wz;
double c2 = wz * pos2d[j].y - i_cp.m11 * wy - i_cp.m12 * wz;
v0 += i_cp.m00 * c1;
v1 += i_cp.m01 * c1 + i_cp.m11 * c2;
v2 += (i_cp.m02 - pos2d[j].x) * c1 + (i_cp.m12 - pos2d[j].y) * c2;
}
o_val[0] = v0;
o_val[1] = v1;
o_val[2] = v2;
return o_val;
}
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;
}
/**
* この関数は、行列を回転行列として、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);
}
}
/**
* この関数は、回転行列を最適化します。
* 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);
}
/**
* 画面上の点と原点を結ぶ直線と任意姿勢の平面の交差点を、平面の座標系で取得します。
* 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;
}
/**
* この関数は、行列を回転行列として、ZXY系の角度値をセットします。
* @param i_angle
* セットする角度値です。
*/
public void setZXYAngle(NyARDoublePoint3d i_angle)
{
setZXYAngle(i_angle.x, i_angle.y, i_angle.z);
return;
}
/**
* この関数は、スクリーン上の点と原点を結ぶ直線と、任意姿勢の平面の交差点を、カメラの座標系で取得します。
* この座標は、カメラ座標系です。
* @param ix
* スクリーン上の座標
* @param iy
* スクリーン上の座標
* @param i_mat
* 平面の姿勢行列です。
* @param o_pos
* 結果を受け取るオブジェクトです。
*/
public void unProjectOnCamera(double ix, double iy, NyARDoubleMatrix44 i_mat, NyARDoublePoint3d o_pos)
{
//画面→撮像点
this.unProject(ix, iy, o_pos);
//撮像点→カメラ座標系
double nx = i_mat.m02;
double ny = i_mat.m12;
double nz = i_mat.m22;
double mx = i_mat.m03;
double my = i_mat.m13;
double mz = i_mat.m23;
double t = (nx * mx + ny * my + nz * mz) / (nx * o_pos.x + ny * o_pos.y + nz * o_pos.z);
o_pos.x = t * o_pos.x;
o_pos.y = t * o_pos.y;
o_pos.z = t * o_pos.z;
}
/**
*
* @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;
}
/**
* この関数は、姿勢行列のエラーレートを計算します。
* エラーレートは、回転行列、平行移動量、オフセット、観察座標から計算します。
* 通常、ユーザが使うことはありません。
* @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;
}
/**
* 座標値を射影変換します。
* @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;
}
/**
* この関数は、3次元座標を座標変換します。
* @param i_x
* 変換する三次元座標(X)
* @param i_y
* 変換する三次元座標(Y)
* @param i_z
* 変換する三次元座標(Z)
* @param o_out
* 変換後の座標を受け取るオブジェクト
*/
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;
o_out.y = this.m10 * i_x + this.m11 * i_y + this.m12 * i_z;
o_out.z = this.m20 * i_x + this.m21 * i_y + this.m22 * i_z;
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;
}
/**
* この関数は、行列を回転行列として、ZXY系の角度値をセットします。
* @param i_angle
* セットする角度値です。
*/
public void setZXYAngle(NyARDoublePoint3d i_angle)
{
setZXYAngle(i_angle.x, i_angle.y, i_angle.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);
}
/**
* コンストラクタです。
* 参照する射影変換オブジェクトを指定して、インスタンスを生成します。
* @param i_matrix
* 参照する射影変換オブジェクト
* @
*/
public NyARRotMatrix_ARToolKit(NyARPerspectiveProjectionMatrix i_matrix)
: base(i_matrix)
{
this._angle = new NyARDoublePoint3d();
return;
}
/**
* カメラ座標系の点を、スクリーン座標の点へ変換します。
* @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);
}
/**
* i_bufに{@link #getAngle()}の結果をコピーして返します。
* @return
* 複製した{@link #getAngle()}の値
*/
public NyARDoublePoint3d getAngle(NyARDoublePoint3d i_buf)
{
i_buf.setValue(this._angle);
return i_buf;
}
/**
* この関数は、スクリーン上の点と原点を結ぶ直線と、任意姿勢の平面の交差点を、カメラの座標系で取得します。
* この座標は、カメラ座標系です。
* @param ix
* スクリーン上の座標
* @param iy
* スクリーン上の座標
* @param i_mat
* 平面の姿勢行列です。
* @param o_pos
* 結果を受け取るオブジェクトです。
*/
public void unProjectOnCamera(double ix, double iy, NyARDoubleMatrix44 i_mat, NyARDoublePoint3d o_pos)
{
//画面→撮像点
this.unProject(ix, iy, o_pos);
//撮像点→カメラ座標系
double nx = i_mat.m02;
double ny = i_mat.m12;
double nz = i_mat.m22;
double mx = i_mat.m03;
double my = i_mat.m13;
double mz = i_mat.m23;
double t = (nx * mx + ny * my + nz * mz) / (nx * o_pos.x + ny * o_pos.y + nz * o_pos.z);
o_pos.x = t * o_pos.x;
o_pos.y = t * o_pos.y;
o_pos.z = t * o_pos.z;
}
/**
* この関数は、ZXY系の角度値を、回転行列にセットします。
* @param i_angle
* ZXY系のラジアン値
*/
public void initRotByAngle(NyARDoublePoint3d i_angle)
{
this.setZXYAngle(i_angle);
}
/**
*
* @param J_U_S
* double[2][6]
* @return
*/
public bool push(NyARDoubleMatrix44 matXc2U, NyARDoubleMatrix44 matXw2Xc, NyARDoublePoint3d worldCoord, NyARDoublePoint2d du, double W)
{
double[][] J_Xc_S = this.__J_Xc_S;
J_U_Xc juxc = this.__juxc;
double wx = worldCoord.x;
double wy = worldCoord.y;
double wz = worldCoord.z;
//icpGetJ_Xc_S1
if (!juxc.setXc2UXc(
matXc2U,
matXw2Xc.m00 * wx + matXw2Xc.m01 * wy + matXw2Xc.m02 * wz + matXw2Xc.m03,
matXw2Xc.m10 * wx + matXw2Xc.m11 * wy + matXw2Xc.m12 * wz + matXw2Xc.m13,
matXw2Xc.m20 * wx + matXw2Xc.m21 * wy + matXw2Xc.m22 * wz + matXw2Xc.m23))
{
return(false);
}
//icpGetJ_Xc_S2
J_Xc_S[0][0] = (-matXw2Xc.m01 * wz) + (matXw2Xc.m02 * wy);
J_Xc_S[0][1] = (matXw2Xc.m00 * wz) + (-matXw2Xc.m02 * wx);
J_Xc_S[0][2] = (-matXw2Xc.m00 * wy) + (matXw2Xc.m01 * wx);
J_Xc_S[0][3] = (matXw2Xc.m00);
J_Xc_S[0][4] = (matXw2Xc.m01);
J_Xc_S[0][5] = (matXw2Xc.m02);
J_Xc_S[1][0] = (-matXw2Xc.m11 * wz) + (matXw2Xc.m12 * wy);
J_Xc_S[1][1] = (matXw2Xc.m10 * wz) + (-matXw2Xc.m12 * wx);
J_Xc_S[1][2] = (-matXw2Xc.m10 * wy) + (matXw2Xc.m11 * wx);
J_Xc_S[1][3] = (matXw2Xc.m10);
J_Xc_S[1][4] = (matXw2Xc.m11);
J_Xc_S[1][5] = (matXw2Xc.m12);
J_Xc_S[2][0] = (-matXw2Xc.m21 * wz) + (matXw2Xc.m22 * wy);
J_Xc_S[2][1] = (matXw2Xc.m20 * wz) + (-matXw2Xc.m22 * wx);
J_Xc_S[2][2] = (-matXw2Xc.m20 * wy) + (matXw2Xc.m21 * wx);
J_Xc_S[2][3] = (matXw2Xc.m20);
J_Xc_S[2][4] = (matXw2Xc.m21);
J_Xc_S[2][5] = (matXw2Xc.m22);
Item item = this.prePush();
if (item == null)
{
return(false);
}
item.m00 = ((juxc.m00 * J_Xc_S[0][0]) + (juxc.m01 * J_Xc_S[1][0]) + (juxc.m02 * J_Xc_S[2][0])) * W;
item.m01 = ((juxc.m00 * J_Xc_S[0][1]) + (juxc.m01 * J_Xc_S[1][1]) + (juxc.m02 * J_Xc_S[2][1])) * W;
item.m02 = ((juxc.m00 * J_Xc_S[0][2]) + (juxc.m01 * J_Xc_S[1][2]) + (juxc.m02 * J_Xc_S[2][2])) * W;
item.m03 = ((juxc.m00 * J_Xc_S[0][3]) + (juxc.m01 * J_Xc_S[1][3]) + (juxc.m02 * J_Xc_S[2][3])) * W;
item.m04 = ((juxc.m00 * J_Xc_S[0][4]) + (juxc.m01 * J_Xc_S[1][4]) + (juxc.m02 * J_Xc_S[2][4])) * W;
item.m05 = ((juxc.m00 * J_Xc_S[0][5]) + (juxc.m01 * J_Xc_S[1][5]) + (juxc.m02 * J_Xc_S[2][5])) * W;
item.m10 = ((juxc.m10 * J_Xc_S[0][0]) + (juxc.m11 * J_Xc_S[1][0]) + (juxc.m12 * J_Xc_S[2][0])) * W;
item.m11 = ((juxc.m10 * J_Xc_S[0][1]) + (juxc.m11 * J_Xc_S[1][1]) + (juxc.m12 * J_Xc_S[2][1])) * W;
item.m12 = ((juxc.m10 * J_Xc_S[0][2]) + (juxc.m11 * J_Xc_S[1][2]) + (juxc.m12 * J_Xc_S[2][2])) * W;
item.m13 = ((juxc.m10 * J_Xc_S[0][3]) + (juxc.m11 * J_Xc_S[1][3]) + (juxc.m12 * J_Xc_S[2][3])) * W;
item.m14 = ((juxc.m10 * J_Xc_S[0][4]) + (juxc.m11 * J_Xc_S[1][4]) + (juxc.m12 * J_Xc_S[2][4])) * W;
item.m15 = ((juxc.m10 * J_Xc_S[0][5]) + (juxc.m11 * J_Xc_S[1][5]) + (juxc.m12 * J_Xc_S[2][5])) * W;
item.ux = du.x * W;
item.uy = du.y * W;
return(true);
}
/**
* この関数は、回転行列を最適化します。
* 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;
}
/**
* 画面座標群と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;
}
/**
* この関数は、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);
}
/**
* {@link #icpGetInitXw2XcSub}関数のオフセットを計算します。
*/
private static NyARDoublePoint3d makeOffset(NyARDoublePoint3d[] ppos3d, int num, NyARDoublePoint3d o_off)
{
double minx, miny, minz;
double maxx, maxy, maxz;
minx = miny = minz = Double.PositiveInfinity;
maxx = maxy = maxz = Double.NegativeInfinity;
for (int i = 0; i < num; i++)
{
if (ppos3d[i].x > maxx)
{
maxx = ppos3d[i].x;
}
if (ppos3d[i].x < minx)
{
minx = ppos3d[i].x;
}
if (ppos3d[i].y > maxy)
{
maxy = ppos3d[i].y;
}
if (ppos3d[i].y < miny)
{
miny = ppos3d[i].y;
}
if (ppos3d[i].z > maxz)
{
maxz = ppos3d[i].z;
}
if (ppos3d[i].z < minz)
{
minz = ppos3d[i].z;
}
}
o_off.x = -(maxx + minx) / 2.0;
o_off.y = -(maxy + miny) / 2.0;
o_off.z = -(maxz + minz) / 2.0;
return(o_off);
}
private double optimizeParamZ(double sina, double cosa, double sinb, double cosb, 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 * (-sina * sinb * ix - cosa * iy + sina * cosb * iz) + cp01 * (ix * cosb + sinb * iz));
double X1 = (cp01 * (sina * ix * sinb + cosa * iy - sina * iz * cosb) + cp00 * (cosb * ix + sinb * iz));
double X2 = cp00 * i_trans.x + cp01 * (i_trans.y) + cp02 * (-cosa * sinb) * ix + cp02 * (sina) * iy + cp02 * ((cosb * cosa) * iz + i_trans.z);
double Y0 = cp11 * (ix * cosb + sinb * iz);
double Y1 = cp11 * (sina * ix * sinb + cosa * iy - sina * iz * cosb);
double Y2 = (cp11 * i_trans.y + cp12 * (-cosa * sinb) * ix + cp12 * ((sina) * iy + (cosb * cosa) * iz + i_trans.z));
double H0 = 0;
double H1 = 0;
double H2 = ((-cosa * sinb) * ix + (sina) * iy + (cosb * cosa) * iz + 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);
}
/**
* この関数は、ZXY系の角度値を、回転行列にセットします。
* @param i_angle
* ZXY系のラジアン値
*/
public void initRotByAngle(NyARDoublePoint3d i_angle)
{
this.setZXYAngle(i_angle);
}
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;
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 計算に過去の履歴を使う点が、{@link #transMat}と異なります。
* @see INyARTransMat#transMatContinue
*/
public bool transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARDoubleMatrix44 i_prev_result, double i_prev_err, NyARDoubleMatrix44 o_result, NyARTransMatResultParam o_param)
{
NyARDoublePoint3d trans = this.__transMat_trans;
//最適化計算の閾値を決定
double err_threshold = makeErrThreshold(i_square.sqvertex);
//平行移動量計算機に、2D座標系をセット
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;
}
this._transsolver.set2dVertex(vertex_2d, 4);
//回転行列を計算
NyARRotMatrix rot = this._rotmatrix;
rot.initRotByPrevResult(i_prev_result);
//回転後の3D座標系から、平行移動量を計算
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
rot.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//現在のエラーレートを計算
double min_err = errRate(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
//エラーレートの判定
if (min_err < i_prev_err + err_threshold)
{
//save initial result
o_result.setValue(rot, trans);
// System.out.println("TR:ok");
//最適化してみる。
for (int i = 0; i < 5; i++)
{
//変換行列の最適化
this._mat_optimize.modifyMatrix(rot, trans, i_offset.vertex, vertex_2d, 4);
double err = errRate(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
//System.out.println("E:"+err);
if (min_err - err < err_threshold / 2)
{
//System.out.println("BREAK");
break;
}
this._transsolver.solveTransportVector(vertex_3d, trans);
o_result.setValue(rot, trans);
min_err = err;
}
//継続計算成功
if (o_param != null)
{
o_param.last_error = min_err;
}
return(true);
}
//継続計算失敗
return(false);
}
/**
* {@link #icpGetInitXw2XcSub}関数のオフセットを計算します。
*/
private static NyARDoublePoint3d makeOffset(NyARDoublePoint3d[] ppos3d, int num, NyARDoublePoint3d o_off)
{
double minx, miny, minz;
double maxx, maxy, maxz;
minx = miny = minz = Double.PositiveInfinity;
maxx = maxy = maxz = Double.NegativeInfinity;
for (int i = 0; i < num; i++)
{
if (ppos3d[i].x > maxx)
{
maxx = ppos3d[i].x;
}
if (ppos3d[i].x < minx)
{
minx = ppos3d[i].x;
}
if (ppos3d[i].y > maxy)
{
maxy = ppos3d[i].y;
}
if (ppos3d[i].y < miny)
{
miny = ppos3d[i].y;
}
if (ppos3d[i].z > maxz)
{
maxz = ppos3d[i].z;
}
if (ppos3d[i].z < minz)
{
minz = ppos3d[i].z;
}
}
o_off.x = -(maxx + minx) / 2.0;
o_off.y = -(maxy + miny) / 2.0;
o_off.z = -(maxz + minz) / 2.0;
return o_off;
}
/*
* 射影変換式 基本式 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));
}
public bool icpGetInitXw2Xc_from_PlanarData(
NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord,
int i_num, NyARDoubleMatrix44 initMatXw2Xc)
{
if (i_num < 4)
{
throw new NyARException();
}
// nを元に配列の準備
NyARMat matAtA = this.__matAtA;
NyARMat matAtB = this.__matAtB;
NyARMat matC = this.__matC;
makeMatAtA(screenCoord, worldCoord, i_num, matAtA);
makeMatAtB(screenCoord, worldCoord, i_num, matAtB);
if (!matAtA.inverse())
{
return false;
}
matC.mul(matAtA, matAtB);
double[][] bufc = matC.getArray();
double t0, t1, t2;
NyARRotVector vec0 = this.__vec0;
NyARRotVector vec1 = this.__vec1;
NyARDoubleMatrix44 matxc = this._cparam;
vec0.v3 = (bufc[6][0]);
vec0.v2 = (bufc[3][0] - matxc.m12 * vec0.v3) / matxc.m11;
vec0.v1 = (bufc[0][0] - matxc.m02 * vec0.v3 - matxc.m01 * vec0.v2) / matxc.m00;
vec1.v3 = (bufc[7][0]);
vec1.v2 = (bufc[4][0] - matxc.m12 * vec1.v3) / matxc.m11;
vec1.v1 = (bufc[1][0] - matxc.m02 * vec1.v3 - matxc.m01 * vec1.v2) / matxc.m00;
t2 = 1.0;
t1 = (bufc[5][0] - matxc.m12 * t2) / matxc.m11;
t0 = (bufc[2][0] - matxc.m02 * t2 - matxc.m01 * t1) / matxc.m00;
double l1 = Math.Sqrt(vec0.v1 * vec0.v1 + vec0.v2 * vec0.v2 + vec0.v3 * vec0.v3);
double l2 = Math.Sqrt(vec1.v1 * vec1.v1 + vec1.v2 * vec1.v2 + vec1.v3 * vec1.v3);
vec0.v1 /= l1;
vec0.v2 /= l1;
vec0.v3 /= l1;
vec1.v1 /= l2;
vec1.v2 /= l2;
vec1.v3 /= l2;
t0 /= (l1 + l2) / 2.0;
t1 /= (l1 + l2) / 2.0;
t2 /= (l1 + l2) / 2.0;
if (t2 < 0.0)
{
vec0.v1 = -vec0.v1;
vec0.v2 = -vec0.v2;
vec0.v3 = -vec0.v3;
vec1.v1 = -vec1.v1;
vec1.v2 = -vec1.v2;
vec1.v3 = -vec1.v3;
t0 = -t0;
t1 = -t1;
t1 = -t2;
}
// ここまで
if (!NyARRotVector.checkRotation(vec0, vec1))
{
return false;
}
double v20 = vec0.v2 * vec1.v3 - vec0.v3 * vec1.v2;
double v21 = vec0.v3 * vec1.v1 - vec0.v1 * vec1.v3;
double v22 = vec0.v1 * vec1.v2 - vec0.v2 * vec1.v1;
l1 = Math.Sqrt(v20 * v20 + v21 * v21 + v22 * v22);
v20 /= l1;
v21 /= l1;
v22 /= l1;
initMatXw2Xc.m00 = vec0.v1;
initMatXw2Xc.m10 = vec0.v2;
initMatXw2Xc.m20 = vec0.v3;
initMatXw2Xc.m01 = vec1.v1;
initMatXw2Xc.m11 = vec1.v2;
initMatXw2Xc.m21 = vec1.v3;
initMatXw2Xc.m02 = v20;
initMatXw2Xc.m12 = v21;
initMatXw2Xc.m22 = v22;
initMatXw2Xc.m03 = t0;
initMatXw2Xc.m13 = t1;
initMatXw2Xc.m23 = t2;
initMatXw2Xc.m30 = initMatXw2Xc.m31 = initMatXw2Xc.m32 = 0;
initMatXw2Xc.m33 = 1;
icpGetInitXw2XcSub(initMatXw2Xc, screenCoord, worldCoord, i_num, initMatXw2Xc);
return true;
}
/**
* この関数は、回転行列を最適化します。
* 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;
}
/**
* この関数は、スクリーン座標点をマーカ平面の点に変換します。
* {@link #isExistMarker(int)}がtrueの時にだけ使用できます。
* @param i_id
* マーカID（ハンドル）値。
* @param i_x
* 変換元のスクリーン座標
* @param i_y
* 変換元のスクリーン座標
* @param i_out
* 結果を格納するオブジェクト
* @return
* 結果を格納したi_outに設定したオブジェクト
*/
public NyARDoublePoint3d getMarkerPlanePos(int i_id, int i_x, int i_y, NyARDoublePoint3d i_out)
{
this._frustum.unProjectOnMatrix(i_x, i_y, this.getMarkerMatrix(i_id), i_out);
return i_out;
}
/**
* カメラ座標系の点を、スクリーン座標の点へ変換します。
* @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);
}
/**
* 画面上の点と原点を結ぶ直線と任意姿勢の平面の交差点を、平面の座標系で取得します。
* 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);
}
/**
* この関数は、スクリーン座標を撮像点座標に変換します。
* 撮像点の座標系は、カメラ座標系になります。
* <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;
}
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;
}
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;
}
/**
* {@link #icpGetInitXw2XcSub}関数のmat_eを計算します。
*/
private static double[] makeMatE(NyARDoubleMatrix44 i_cp, NyARDoubleMatrix44 rot, NyARDoublePoint2d[] pos2d, NyARDoublePoint3d[] pos3d, NyARDoublePoint3d offset, int i_num, double[] o_val)
{
double v0 = 0;
double v1 = 0;
double v2 = 0;
for (int j = 0; j < i_num; j++)
{
double p3x = pos3d[j].x + offset.x;
double p3y = pos3d[j].y + offset.y;
double p3z = pos3d[j].z + offset.z;
double wx = rot.m00 * p3x + rot.m01 * p3y + rot.m02 * p3z;
double wy = rot.m10 * p3x + rot.m11 * p3y + rot.m12 * p3z;
double wz = rot.m20 * p3x + rot.m21 * p3y + rot.m22 * p3z;
double c1 = wz * pos2d[j].x - i_cp.m00 * wx - i_cp.m01 * wy - i_cp.m02 * wz;
double c2 = wz * pos2d[j].y - i_cp.m11 * wy - i_cp.m12 * wz;
v0 += i_cp.m00 * c1;
v1 += i_cp.m01 * c1 + i_cp.m11 * c2;
v2 += (i_cp.m02 - pos2d[j].x) * c1 + (i_cp.m12 - pos2d[j].y) * c2;
}
o_val[0] = v0;
o_val[1] = v1;
o_val[2] = v2;
return(o_val);
}
/**
* i_bufに{@link #getAngle()}の結果をコピーして返します。
* @return
* 複製した{@link #getAngle()}の値
*/
public NyARDoublePoint3d getAngle(NyARDoublePoint3d i_buf)
{
i_buf.setValue(this._angle);
return(i_buf);
}
