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);
}
/**
* 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());
}
/**
* 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;
}
/**
* 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());
}
/*
* (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;
}