/**
* この関数は、姿勢行列のエラーレートを計算します。
* エラーレートは、回転行列、平行移動量、オフセット、観察座標から計算します。
* 通常、ユーザが使うことはありません。
* @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_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;
}
/**
* この関数は、オブジェクトの内容をインスタンスにコピーします。
* @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;
}
/**
* この関数は、行列同士の掛け算をして、インスタンスに格納します。
* 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_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());
}
/**
* この関数は、姿勢行列のエラーレートを計算します。
* エラーレートは、回転行列、平行移動量、オフセット、観察座標から計算します。
* 通常、ユーザが使うことはありません。
* @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_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_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;
}
/**
* この関数は、行列同士の掛け算をして、インスタンスに格納します。
* 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;
}
/**
* この関数は、回転行列を最適化します。
* 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;
}
/**
* この関数は、逆行列を計算して、インスタンスにセットします。
* @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;
}
/**
* {@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);
}
/**
* この関数は、平行移動量と回転行列をセットして、インスタンスのパラメータを更新します。
* 拡大率は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;
}
