private double optimize(NyARRotMatrix io_rotmat, NyARDoublePoint3d io_transvec, INyARTransportVectorSolver i_solver, NyARDoublePoint3d[] i_offset_3d, NyARDoublePoint2d[] i_2d_vertex, double i_err_threshold)
{
//System.out.println("START");
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
//初期のエラー値を計算
double min_err = errRate(io_rotmat, io_transvec, i_offset_3d, i_2d_vertex, 4, vertex_3d);
NyARDoubleMatrix33 rot = this.__rot;
rot.setValue(io_rotmat);
for (int i = 0; i < 5; i++)
{
//変換行列の最適化
this._mat_optimize.modifyMatrix(rot, io_transvec, i_offset_3d, i_2d_vertex, 4);
double err = errRate(rot, io_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, io_transvec);
io_rotmat.setValue(rot);
min_err = err;
}
//System.out.println("END");
return(min_err);
}
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 bool solveHomography4PointsInhomogenous(NyARDoubleMatrix33 i_homography_mat,
NyARDoublePoint2d x1, NyARDoublePoint2d x2, NyARDoublePoint2d x3, NyARDoublePoint2d x4,
NyARDoublePoint2d xp1, NyARDoublePoint2d xp2, NyARDoublePoint2d xp3, NyARDoublePoint2d xp4)
{
double[][] _mat_A = ArrayUtils.newDouble2dArray(8, 9);
// x1.setValue(0, 0);x2.setValue(10, 0);x3.setValue(10, 10);x4.setValue(0, 10);
// xp1.setValue(10, 10);xp2.setValue(10, 0);xp3.setValue(0, 0);xp4.setValue(0, 10);
//Homography4PointsInhomogeneousConstraint
AddHomographyPointContraint(_mat_A, 0, x1, xp1);
AddHomographyPointContraint(_mat_A, 2, x2, xp2);
AddHomographyPointContraint(_mat_A, 4, x3, xp3);
AddHomographyPointContraint(_mat_A, 6, x4, xp4);
//SolveHomography4PointsInhomogenous
if (!this.solveNullVector8x9Destructive(i_homography_mat, _mat_A))
{
return(false);
}
if (Math.Abs(i_homography_mat.determinant()) < 1e-5)
{
return(false);
}
return(true);
}
/**
* この関数は、姿勢行列のエラーレートを計算します。
* エラーレートは、回転行列、平行移動量、オフセット、観察座標から計算します。
* 通常、ユーザが使うことはありません。
* @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);
}
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;
}
/**
* この関数は、オブジェクトの配列を生成して返します。
* @param i_number
* 配列の長さ
* @return
* 新しいオブジェクト配列
*/
public static NyARDoubleMatrix33[] createArray(int i_number)
{
NyARDoubleMatrix33[] ret = new NyARDoubleMatrix33[i_number];
for (int i = 0; i < i_number; i++)
{
ret[i] = new NyARDoubleMatrix33();
}
return ret;
}
/**
* この関数は、オブジェクトの配列を生成して返します。
* @param i_number
* 配列の長さ
* @return
* 新しいオブジェクト配列
*/
public static NyARDoubleMatrix33[] createArray(int i_number)
{
NyARDoubleMatrix33[] ret = new NyARDoubleMatrix33[i_number];
for (int i = 0; i < i_number; i++)
{
ret[i] = new NyARDoubleMatrix33();
}
return(ret);
}
/**
* この関数は、回転行列を最適化します。
* 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;
}
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;
}
private bool solveHomography4PointsInhomogenous(NyARDoubleMatrix33 i_homography_mat)
{
//SolveHomography4PointsInhomogenous
if (!this.solveNullVector8x9Destructive(i_homography_mat, this._mat_A))
{
return(false);
}
if (Math.Abs(i_homography_mat.determinant()) < 1e-5)
{
return(false);
}
return(true);
}
public void setValue(NyARDoubleMatrix33 i_value)
{
this.m00 = i_value.m00;
this.m01 = i_value.m01;
this.m02 = i_value.m02;
this.m10 = i_value.m10;
this.m11 = i_value.m11;
this.m12 = i_value.m12;
this.m20 = i_value.m20;
this.m21 = i_value.m21;
this.m22 = i_value.m22;
return;
}
public void copyFrom(NyARDoubleMatrix33 i_matrix)
{
this.m00 = (long)i_matrix.m00 * 0x1000000;
this.m01 = (long)i_matrix.m01 * 0x1000000;
this.m02 = (long)i_matrix.m02 * 0x1000000;
this.m10 = (long)i_matrix.m10 * 0x1000000;
this.m11 = (long)i_matrix.m11 * 0x1000000;
this.m12 = (long)i_matrix.m12 * 0x1000000;
this.m20 = (long)i_matrix.m20 * 0x1000000;
this.m21 = (long)i_matrix.m21 * 0x1000000;
this.m22 = (long)i_matrix.m22 * 0x1000000;
return;
}
/**
* この関数は、オブジェクトの内容をインスタンスにコピーします。
* @param i_value
* コピー元のオブジェクト
*/
public void setValue(NyARDoubleMatrix33 i_value)
{
this.m00 = i_value.m00;
this.m01 = i_value.m01;
this.m02 = i_value.m02;
this.m10 = i_value.m10;
this.m11 = i_value.m11;
this.m12 = i_value.m12;
this.m20 = i_value.m20;
this.m21 = i_value.m21;
this.m22 = i_value.m22;
return;
}
public void copyFrom(NyARDoubleMatrix33 i_matrix)
{
this.m00 = (long)i_matrix.m00 * 0x10000;
this.m01 = (long)i_matrix.m01 * 0x10000;
this.m02 = (long)i_matrix.m02 * 0x10000;
this.m10 = (long)i_matrix.m10 * 0x10000;
this.m11 = (long)i_matrix.m11 * 0x10000;
this.m12 = (long)i_matrix.m12 * 0x10000;
this.m20 = (long)i_matrix.m20 * 0x10000;
this.m21 = (long)i_matrix.m21 * 0x10000;
this.m22 = (long)i_matrix.m22 * 0x10000;
return;
}
/**
* インスタンスの値とi_matの値が同一かを返します。
*/
override public bool Equals(Object i_mat)
{
if (i_mat is NyARDoubleMatrix33)
{
NyARDoubleMatrix33 m = (NyARDoubleMatrix33)i_mat;
if (m.m00 == this.m00 && m.m01 == this.m01 && m.m02 == this.m02 &&
m.m10 == this.m10 && m.m11 == this.m11 && m.m12 == this.m12 &&
m.m20 == this.m20 && m.m21 == this.m21 && m.m22 == this.m22)
{
return(true);
}
}
return(false);
}
/**
* この関数は、行列同士の掛け算をして、インスタンスに格納します。
* i_mat_lとi_mat_rには、thisを指定しないでください。
* @param i_mat_l
* 左成分の行列
* @param i_mat_r
* 右成分の行列
*/
public void mul(NyARDoubleMatrix33 i_mat_l, NyARDoubleMatrix33 i_mat_r)
{
//assert(this!=i_mat_l);
//assert(this!=i_mat_r);
this.m00 = i_mat_l.m00 * i_mat_r.m00 + i_mat_l.m01 * i_mat_r.m10 + i_mat_l.m02 * i_mat_r.m20;
this.m01 = i_mat_l.m00 * i_mat_r.m01 + i_mat_l.m01 * i_mat_r.m11 + i_mat_l.m02 * i_mat_r.m21;
this.m02 = i_mat_l.m00 * i_mat_r.m02 + i_mat_l.m01 * i_mat_r.m12 + i_mat_l.m02 * i_mat_r.m22;
this.m10 = i_mat_l.m10 * i_mat_r.m00 + i_mat_l.m11 * i_mat_r.m10 + i_mat_l.m12 * i_mat_r.m20;
this.m11 = i_mat_l.m10 * i_mat_r.m01 + i_mat_l.m11 * i_mat_r.m11 + i_mat_l.m12 * i_mat_r.m21;
this.m12 = i_mat_l.m10 * i_mat_r.m02 + i_mat_l.m11 * i_mat_r.m12 + i_mat_l.m12 * i_mat_r.m22;
this.m20 = i_mat_l.m20 * i_mat_r.m00 + i_mat_l.m21 * i_mat_r.m10 + i_mat_l.m22 * i_mat_r.m20;
this.m21 = i_mat_l.m20 * i_mat_r.m01 + i_mat_l.m21 * i_mat_r.m11 + i_mat_l.m22 * i_mat_r.m21;
this.m22 = i_mat_l.m20 * i_mat_r.m02 + i_mat_l.m21 * i_mat_r.m12 + i_mat_l.m22 * i_mat_r.m22;
return;
}
public void sincos2Rotation_ZXY(TSinCosValue[] i_sincos, NyARDoubleMatrix33 i_rot_matrix)
{
double sina = i_sincos[0].sin_val;
double cosa = i_sincos[0].cos_val;
double sinb = i_sincos[1].sin_val;
double cosb = i_sincos[1].cos_val;
double sinc = i_sincos[2].sin_val;
double cosc = i_sincos[2].cos_val;
i_rot_matrix.m00 = cosc * cosb - sinc * sina * sinb;
i_rot_matrix.m01 = -sinc * cosa;
i_rot_matrix.m02 = cosc * sinb + sinc * sina * cosb;
i_rot_matrix.m10 = sinc * cosb + cosc * sina * sinb;
i_rot_matrix.m11 = cosc * cosa;
i_rot_matrix.m12 = sinc * sinb - cosc * sina * cosb;
i_rot_matrix.m20 = -cosa * sinb;
i_rot_matrix.m21 = sina;
i_rot_matrix.m22 = cosb * cosa;
}
public void sincos2Rotation_ZXY(TSinCosValue[] i_sincos, NyARDoubleMatrix33 i_rot_matrix)
{
double sina = i_sincos[0].sin_val;
double cosa = i_sincos[0].cos_val;
double sinb = i_sincos[1].sin_val;
double cosb = i_sincos[1].cos_val;
double sinc = i_sincos[2].sin_val;
double cosc = i_sincos[2].cos_val;
i_rot_matrix.m00 = cosc * cosb - sinc * sina * sinb;
i_rot_matrix.m01 = -sinc * cosa;
i_rot_matrix.m02 = cosc * sinb + sinc * sina * cosb;
i_rot_matrix.m10 = sinc * cosb + cosc * sina * sinb;
i_rot_matrix.m11 = cosc * cosa;
i_rot_matrix.m12 = sinc * sinb - cosc * sina * cosb;
i_rot_matrix.m20 = -cosa * sinb;
i_rot_matrix.m21 = sina;
i_rot_matrix.m22 = cosb * cosa;
}
/**
* この関数は、逆行列を計算して、インスタンスにセットします。
* @param i_src
* 逆行列を計算するオブジェクト。thisを指定できます。
* @return
* 逆行列を得られると、trueを返します。
*/
public bool inverse(NyARDoubleMatrix33 i_src)
{
double a11, a12, a13, a21, a22, a23, a31, a32, a33;
double b11, b12, b13, b21, b22, b23, b31, b32, b33;
a11 = i_src.m00; a12 = i_src.m01; a13 = i_src.m02;
a21 = i_src.m10; a22 = i_src.m11; a23 = i_src.m12;
a31 = i_src.m20; a32 = i_src.m21; a33 = i_src.m22;
b11 = a22 * a33 - a23 * a32;
b12 = a32 * a13 - a33 * a12;
b13 = a12 * a23 - a13 * a22;
b21 = a23 * a31 - a21 * a33;
b22 = a33 * a11 - a31 * a13;
b23 = a13 * a21 - a11 * a23;
b31 = a21 * a32 - a22 * a31;
b32 = a31 * a12 - a32 * a11;
b33 = a11 * a22 - a12 * a21;
double det_1 = a11 * b11 + a21 * b12 + a31 * b13;
if (det_1 == 0)
{
return(false);
}
det_1 = 1 / det_1;
this.m00 = b11 * det_1;
this.m01 = b12 * det_1;
this.m02 = b13 * det_1;
this.m10 = b21 * det_1;
this.m11 = b22 * det_1;
this.m12 = b23 * det_1;
this.m20 = b31 * det_1;
this.m21 = b32 * det_1;
this.m22 = b33 * det_1;
return(true);
}
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;
}
/**
* intrinsic_matrixとdistortion_coeffsからインスタンスを初期化する。
* @param i_w
* カメラパラメータ生成時の画面サイズ
* @param i_h
* カメラパラメータ生成時の画面サイズ
* @param i_intrinsic_matrix 3x3 matrix このパラメータは、OpenCVのcvCalibrateCamera2関数が出力するintrinsic_matrixの値と合致します。
* @param i_distortion_coeffs 4x1 vector このパラメータは、OpenCVのcvCalibrateCamera2関数が出力するdistortion_coeffsの値と合致します。
*/
public ParamLoader(int i_w, int i_h, double[] i_intrinsic_matrix, double[] i_distortion_coeffs)
{
this.size = new NyARIntSize(i_w, i_h);
//dist factor
NyARCameraDistortionFactorV4 v4dist = new NyARCameraDistortionFactorV4();
v4dist.setValue(this.size, i_intrinsic_matrix, i_distortion_coeffs);
double s = v4dist.getS();
this.dist_factor = v4dist;
//projection matrix
this.pmat = new NyARPerspectiveProjectionMatrix();
NyARDoubleMatrix33 r = new NyARDoubleMatrix33();
r.setValue(i_intrinsic_matrix);
r.m00 /= s;
r.m01 /= s;
r.m10 /= s;
r.m11 /= s;
this.pmat.setValue(r, new NyARDoublePoint3d());
}
/**
* Solve for the null vector x of an 8x9 matrix A such A*x=0. The matrix A is destroyed in the process. This system
* is solved using QR decomposition with Gram-Schmidt.
*/
private bool solveNullVector8x9Destructive(NyARDoubleMatrix33 x, double[][] A)
{
double[][] Q = ArrayUtils.newDouble2dArray(8, 9);
if (!OrthogonalizePivot8x9Basis0(Q, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis1(Q, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis2(Q, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis3(Q, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis4(Q, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis5(Q, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis6(Q, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis7(Q, A))
{
return(false);
}
return(OrthogonalizeIdentity8x9(x, Q));
}
/**
* Solve for the null vector x of an 8x9 matrix A such A*x=0. The matrix A is destroyed in the process. This system
* is solved using QR decomposition with Gram-Schmidt.
*/
private bool solveNullVector8x9Destructive(NyARDoubleMatrix33 x, double[][] A)
{
double[][] Q = this._solveNullVector8x9Destructive_Q;
if (!OrthogonalizePivot8x9Basis0(Q, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis(Q, 6, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis(Q, 5, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis(Q, 4, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis(Q, 3, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis(Q, 2, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis(Q, 1, A))
{
return(false);
}
if (!OrthogonalizePivot8x9Basis(Q, 0, A))
{
return(false);
}
return(OrthogonalizeIdentity8x9(x, Q));
}
/**
* 平行移動量と回転行列をセットします。この関数は、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;
}
/**
* intrinsic_matrixとdistortion_coeffsからインスタンスを初期化する。
* @param i_camera_width
* カメラパラメータ生成時の画面サイズ
* @param i_camera_height
* カメラパラメータ生成時の画面サイズ
* @param i_intrinsic_matrix 3x3 matrix このパラメータは、OpenCVのcvCalibrateCamera2関数が出力するintrinsic_matrixの値と合致します。
* @param i_distortion_coeffs 4x1 vector このパラメータは、OpenCVのcvCalibrateCamera2関数が出力するdistortion_coeffsの値と合致します。
*/
public ParamLoader(int i_camera_width, int i_camera_height, double[] i_intrinsic_matrix, double[] i_distortion_coeffs, int i_screen_width, int i_screen_height)
{
double x_scale = (double)i_screen_width / (double)(i_camera_width); // scale = (double)xsize / (double)(source->xsize);
double y_scale = (double)i_screen_height / (double)(i_camera_height); // scale = (double)ysize / (double)(source->ysize);
this.size = new NyARIntSize(i_camera_width, i_camera_height);
//dist factor(倍率1倍の基準点)
NyARCameraDistortionFactorV4 v4dist = new NyARCameraDistortionFactorV4(i_camera_width, i_camera_height, i_intrinsic_matrix, i_distortion_coeffs, x_scale, y_scale);
double s = v4dist.getS();
//projection matrix
NyARDoubleMatrix33 r = new NyARDoubleMatrix33();
r.setValue(i_intrinsic_matrix);
r.m00 /= s;
r.m01 /= s;
r.m10 /= s;
r.m11 /= s;
NyARPerspectiveProjectionMatrix pm = new NyARPerspectiveProjectionMatrix();
pm.setValue(r, new NyARDoublePoint3d());
pm.changeScale(x_scale, y_scale);
this.dist_factor = v4dist;
this.pmat = pm;
}
// boolean OrthogonalizeIdentity8x9(T x[9], const T Q[72]) {
private bool OrthogonalizeIdentity8x9(NyARDoubleMatrix33 x, double[][] Q)
{
double[] XX = this._OrthogonalizeIdentity8x9_X;
double max_w = 0;
for (int i = 8; i >= 0; i--)
{
double w = OrthogonalizeIdentity8x9(XX, Q, i);
if (w > max_w)
{
max_w = w;
x.m00 = XX[0];
x.m01 = XX[1];
x.m02 = XX[2];
x.m10 = XX[3];
x.m11 = XX[4];
x.m12 = XX[5];
x.m20 = XX[6];
x.m21 = XX[7];
x.m22 = XX[8];
}
}
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;
}
/**
* intrinsic_matrixとdistortion_coeffsからインスタンスを初期化する。
* @param i_w
* カメラパラメータ生成時の画面サイズ
* @param i_h
* カメラパラメータ生成時の画面サイズ
* @param i_intrinsic_matrix 3x3 matrix このパラメータは、OpenCVのcvCalibrateCamera2関数が出力するintrinsic_matrixの値と合致します。
* @param i_distortion_coeffs 4x1 vector このパラメータは、OpenCVのcvCalibrateCamera2関数が出力するdistortion_coeffsの値と合致します。
*/
public ParamLoader(int i_w, int i_h, double[] i_intrinsic_matrix, double[] i_distortion_coeffs)
{
this.size = new NyARIntSize(i_w, i_h);
//dist factor
NyARCameraDistortionFactorV4 v4dist = new NyARCameraDistortionFactorV4();
v4dist.setValue(this.size, i_intrinsic_matrix, i_distortion_coeffs);
double s = v4dist.getS();
this.dist_factor = v4dist;
//projection matrix
this.pmat = new NyARPerspectiveProjectionMatrix();
NyARDoubleMatrix33 r = new NyARDoubleMatrix33();
r.setValue(i_intrinsic_matrix);
r.m00 /= s;
r.m01 /= s;
r.m10 /= s;
r.m11 /= s;
this.pmat.setValue(r, new NyARDoublePoint3d());
}
/**
* {@link #icpGetInitXw2XcSub}関数のmat_dを計算します。
*/
private static NyARDoubleMatrix33 makeMatD(NyARDoubleMatrix44 i_cp, NyARDoublePoint2d[] pos2d, int i_num, NyARDoubleMatrix33 o_mat)
{
double m02 = 0;
double m12 = 0;
double m20 = 0;
double m21 = 0;
double m22 = 0;
for (int i = 0; i < i_num; i++)
{
double cx = i_cp.m02 - pos2d[i].x;
double cy = i_cp.m12 - pos2d[i].y;
m02 += (cx) * i_cp.m00;
m12 += (cx) * i_cp.m01 + (cy) * i_cp.m11;
m20 += i_cp.m00 * (cx);
m21 += i_cp.m01 * (cx) + i_cp.m11 * (cy);
m22 += (cx) * (cx) + (cy) * (cy);
}
o_mat.m00 = (i_cp.m00 * i_cp.m00) * i_num;
o_mat.m01 = (i_cp.m00 * i_cp.m01) * i_num;
o_mat.m02 = m02;
o_mat.m10 = (i_cp.m01 * i_cp.m00) * i_num;
o_mat.m11 = (i_cp.m01 * i_cp.m01 + i_cp.m11 * i_cp.m11) * i_num;
o_mat.m12 = m12;
o_mat.m20 = m20;
o_mat.m21 = m21;
o_mat.m22 = m22;
o_mat.inverse(o_mat);
return(o_mat);
}
/**
* Multiply an in-homogenous point by a similarity.
*/
private static void MultiplyPointHomographyInhomogenous(NyARDoublePoint2d xp, NyARDoubleMatrix33 H, NyARDoublePoint2d x)
{
double w = H.m20 * x.x + H.m21 * x.y + H.m22;
xp.x = (H.m00 * x.x + H.m01 * x.y + H.m02) / w;
xp.y = (H.m10 * x.x + H.m11 * x.y + H.m12) / w;
}
// private boolean ComputeSubpixelHessian(
// float H[9],float b[3],
// const Image& lap0,const Image& lap1,const Image& lap2,
// int x,int y)
private bool updateLocation(DogFeaturePoint kp, LaplacianImage lap0, LaplacianImage lap1, LaplacianImage lap2)
{
double[] tmp = new double[2];
double[] b = new double[3];
// Downsample the feature point to the detection octave
bilinear_downsample_point(tmp, kp.x, kp.y, kp.octave);
double xp = tmp[0];
double yp = tmp[1];
// Compute the discrete pixel location
int x = (int)(xp + 0.5f);
int y = (int)(yp + 0.5f);
double[] H = new double[9];
if (lap0.getWidth() == lap1.getWidth() && lap1.getWidth() == lap2.getWidth())
{
//すべての画像サイズが同じ
//assert lap0.getHeight() == lap1.getHeight() && lap1.getHeight() == lap2.getHeight();// "Width/height are not consistent");
ComputeSubpixelHessianSameOctave(H, b, lap0, lap1, lap2, x, y);
}
else if ((lap0.getWidth() == lap1.getWidth()) && ((lap1.getWidth() >> 1) == lap2.getWidth()))
{
//0,1が同じで2がその半分
//assert (lap0.getHeight() == lap1.getHeight()) && ((lap1.getHeight() >> 1) == lap2.getHeight());// Width/height are not consistent");
ComputeSubpixelHessianFineOctavePair(H, b, lap0, lap1, lap2, x, y);
}
else if (((lap0.getWidth() >> 1) == lap1.getWidth()) && (lap1.getWidth() == lap2.getWidth()))
{
//0の半分が1,2
//assert ((lap0.getWidth() >> 1) == lap1.getWidth()) && (lap1.getWidth() == lap2.getWidth());// Width/height are not consistent");
ComputeSubpixelHessianCoarseOctavePair(H, b, lap0, lap1, lap2, x, y);
}
else
{
// ASSERT(0, "Image sizes are inconsistent");
return(false);
}
// A*u=b // if (!SolveSymmetricLinearSystem3x3(u, H, b)) {
NyARDoubleMatrix33 m = new NyARDoubleMatrix33();
m.m00 = H[0];
m.m01 = H[1];
m.m02 = H[2];
m.m10 = H[3];
m.m11 = H[4];
m.m12 = H[5];
m.m20 = H[6];
m.m21 = H[7];
m.m22 = H[8];
if (!m.inverse(m))
{
return(false);
}
double u0 = (m.m00 * b[0] + m.m01 * b[1] + m.m02 * b[2]);
double u1 = (m.m10 * b[0] + m.m11 * b[1] + m.m12 * b[2]);
double u2 = (m.m20 * b[0] + m.m21 * b[1] + m.m22 * b[2]);
// If points move too much in the sub-pixel update, then the point probably unstable.
if ((u0 * u0) + (u1 * u1) > mMaxSubpixelDistanceSqr)
{
return(false);
}
// Compute the edge score
if (!ComputeEdgeScore(tmp, H))
{
return(false);
}
kp.edge_score = tmp[0];
// Compute a linear estimate of the intensity
// ASSERT(kp.score == lap1.get<float>(y)[x],
// "Score is not consistent with the DoG image");
double[] lap1_buf = (double[])lap1.getBuffer();
kp.score = lap1_buf[lap1.get(y) + x] - (b[0] * u0 + b[1] * u1 + b[2] * u2);
// Update the location:
// Apply the update on the downsampled location and then upsample
// the result.
// bilinear_upsample_point(kp.x, kp.y, xp+u[0], yp+u[1], kp.octave);
bilinear_upsample_point(tmp, xp + u0, yp + u1, kp.octave);
kp.x = tmp[0];
kp.y = tmp[1];
// Update the scale
kp.sp_scale = kp.scale + u2;
kp.sp_scale = ClipScalar(kp.sp_scale, 0, mLaplacianPyramid.numScalePerOctave());
return(true);
}
/**
* {@link #icpGetInitXw2XcSub}関数のmat_dを計算します。
*/
private static NyARDoubleMatrix33 makeMatD(NyARDoubleMatrix44 i_cp, NyARDoublePoint2d[] pos2d, int i_num, NyARDoubleMatrix33 o_mat)
{
double m02 = 0;
double m12 = 0;
double m20 = 0;
double m21 = 0;
double m22 = 0;
for (int i = 0; i < i_num; i++)
{
double cx = i_cp.m02 - pos2d[i].x;
double cy = i_cp.m12 - pos2d[i].y;
m02 += (cx) * i_cp.m00;
m12 += (cx) * i_cp.m01 + (cy) * i_cp.m11;
m20 += i_cp.m00 * (cx);
m21 += i_cp.m01 * (cx) + i_cp.m11 * (cy);
m22 += (cx) * (cx) + (cy) * (cy);
}
o_mat.m00 = (i_cp.m00 * i_cp.m00) * i_num;
o_mat.m01 = (i_cp.m00 * i_cp.m01) * i_num;
o_mat.m02 = m02;
o_mat.m10 = (i_cp.m01 * i_cp.m00) * i_num;
o_mat.m11 = (i_cp.m01 * i_cp.m01 + i_cp.m11 * i_cp.m11) * i_num;
o_mat.m12 = m12;
o_mat.m20 = m20;
o_mat.m21 = m21;
o_mat.m22 = m22;
o_mat.inverse(o_mat);
return o_mat;
}
/**
* この関数は、行列同士の掛け算をして、インスタンスに格納します。
* i_mat_lとi_mat_rには、thisを指定しないでください。
* @param i_mat_l
* 左成分の行列
* @param i_mat_r
* 右成分の行列
*/
public void mul(NyARDoubleMatrix33 i_mat_l, NyARDoubleMatrix33 i_mat_r)
{
//assert(this!=i_mat_l);
//assert(this!=i_mat_r);
this.m00 = i_mat_l.m00 * i_mat_r.m00 + i_mat_l.m01 * i_mat_r.m10 + i_mat_l.m02 * i_mat_r.m20;
this.m01 = i_mat_l.m00 * i_mat_r.m01 + i_mat_l.m01 * i_mat_r.m11 + i_mat_l.m02 * i_mat_r.m21;
this.m02 = i_mat_l.m00 * i_mat_r.m02 + i_mat_l.m01 * i_mat_r.m12 + i_mat_l.m02 * i_mat_r.m22;
this.m10 = i_mat_l.m10 * i_mat_r.m00 + i_mat_l.m11 * i_mat_r.m10 + i_mat_l.m12 * i_mat_r.m20;
this.m11 = i_mat_l.m10 * i_mat_r.m01 + i_mat_l.m11 * i_mat_r.m11 + i_mat_l.m12 * i_mat_r.m21;
this.m12 = i_mat_l.m10 * i_mat_r.m02 + i_mat_l.m11 * i_mat_r.m12 + i_mat_l.m12 * i_mat_r.m22;
this.m20 = i_mat_l.m20 * i_mat_r.m00 + i_mat_l.m21 * i_mat_r.m10 + i_mat_l.m22 * i_mat_r.m20;
this.m21 = i_mat_l.m20 * i_mat_r.m01 + i_mat_l.m21 * i_mat_r.m11 + i_mat_l.m22 * i_mat_r.m21;
this.m22 = i_mat_l.m20 * i_mat_r.m02 + i_mat_l.m21 * i_mat_r.m12 + i_mat_l.m22 * i_mat_r.m22;
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;
}
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_src
* 逆行列を計算するオブジェクト。thisを指定できます。
* @return
* 逆行列を得られると、trueを返します。
*/
public bool inverse(NyARDoubleMatrix33 i_src)
{
double a11, a12, a13, a21, a22, a23, a31, a32, a33;
double b11, b12, b13, b21, b22, b23, b31, b32, b33;
a11 = i_src.m00; a12 = i_src.m01; a13 = i_src.m02;
a21 = i_src.m10; a22 = i_src.m11; a23 = i_src.m12;
a31 = i_src.m20; a32 = i_src.m21; a33 = i_src.m22;
b11 = a22 * a33 - a23 * a32;
b12 = a32 * a13 - a33 * a12;
b13 = a12 * a23 - a13 * a22;
b21 = a23 * a31 - a21 * a33;
b22 = a33 * a11 - a31 * a13;
b23 = a13 * a21 - a11 * a23;
b31 = a21 * a32 - a22 * a31;
b32 = a31 * a12 - a32 * a11;
b33 = a11 * a22 - a12 * a21;
double det_1 = a11 * b11 + a21 * b12 + a31 * b13;
if (det_1 == 0)
{
return false;
}
det_1 = 1 / det_1;
this.m00 = b11 * det_1;
this.m01 = b12 * det_1;
this.m02 = b13 * det_1;
this.m10 = b21 * det_1;
this.m11 = b22 * det_1;
this.m12 = b23 * det_1;
this.m20 = b31 * det_1;
this.m21 = b32 * det_1;
this.m22 = b33 * det_1;
return true;
}
/*
* (non-Javadoc)
* @see jp.nyatla.nyartoolkit.core.transmat.INyARTransMat#transMatContinue(jp.nyatla.nyartoolkit.core.NyARSquare, int, double, jp.nyatla.nyartoolkit.core.transmat.NyARTransMatResult)
*/
public void transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
{
NyARDoublePoint3d trans = this.__transMat_trans;
// io_result_convが初期値なら、transMatで計算する。
if (!o_result_conv.has_value)
{
this.transMat(i_square, i_offset, o_result_conv);
return;
}
//最適化計算の閾値を決定
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.initRotByPrevResult(o_result_conv);
//回転後の3D座標系から、平行移動量を計算
this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//現在のエラーレートを計算しておく
double min_err = errRate(this._rotmatrix, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
NyARDoubleMatrix33 rot = this.__rot;
//エラーレートが前回のエラー値より閾値分大きかったらアゲイン
if (min_err < o_result_conv.error + err_threshold)
{
rot.setValue(this._rotmatrix);
//最適化してみる。
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);
this._rotmatrix.setValue(rot);
min_err = err;
}
this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
}
else
{
//回転行列を計算
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);
//計算結果の最適化(平行移動量と回転行列の最適化)
min_err = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold);
this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
}
o_result_conv.error = min_err;
return;
}