/**
* Make tansform matrix by ICP algorism.
* i_prev_result parameter is not effective. should set 0.
*/
public bool transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARDoubleMatrix44 i_prev_result, double i_prev_err,
NyARDoubleMatrix44 o_result, NyARTransMatResultParam o_param)
{
if(this._icpp.icpPoint(i_square.sqvertex,i_offset.vertex,4,o_result,o_result,o_param)){
return true;
}
return false;
}
/**
* Make tansform matrix by ICP algorism.
* i_prev_result parameter is not effective. should set 0.
*/
public bool transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARDoubleMatrix44 i_prev_result, double i_prev_err,
NyARDoubleMatrix44 o_result, NyARTransMatResultParam o_param)
{
if (this._icpp.icpPoint(i_square.sqvertex, i_offset.vertex, 4, o_result, o_result, o_param))
{
return(true);
}
return(false);
}
/**
* Make tansform matrix by ICP algorism.
*/
public bool transMat(NyARSquare i_square, NyARRectOffset i_offset,NyARDoubleMatrix44 i_result,NyARTransMatResultParam o_param)
{
if(this._icpc.icpGetInitXw2Xc_from_PlanarData(i_square.sqvertex,i_offset.vertex,4,i_result)){
if(this._icpp.icpPoint(i_square.sqvertex,i_offset.vertex,4,i_result,i_result,o_param)){
return true;
}
}
return false;
}
public override bool icpPoint(NyARDoublePoint2d[] screenCoord,
NyARDoublePoint3d[] worldCoord, int num,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc, NyARTransMatResultParam o_result_param)
{
double err0 = 0, err1;
System.Diagnostics.Debug.Assert(num >= 4);
NyARIcpUtils.DeltaS dS = this.__dS;
NyARIcpUtils.U u = this.__u;
//ワークオブジェクトのリセット
if (this.__jus.getArraySize() < num)
{
this.__jus = new NyARIcpUtils.JusStack(num);
this.__du = NyARDoublePoint2d.createArray(num);
}
NyARIcpUtils.JusStack jus = this.__jus;
NyARDoublePoint2d[] du = this.__du;
o_matxw2xc.setValue(initMatXw2Xc);
double breakLoopErrorThresh = this.getBreakLoopErrorThresh();
double breakLoopErrorThresh2 = this.getBreakLoopErrorThresh2();
double breakLoopErrorRatioThresh = this.getBreakLoopErrorRatioThresh();
double maxLoop = this.getMaxLoop();
NyARDoubleMatrix44 matXw2U = this.__matXw2U;
for (int i = 0; ; i++)
{
matXw2U.mul(this._ref_matXc2U, o_matxw2xc);
err1 = 0.0;
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;
err1 += dx * dx + dy * dy;
du[j].x = dx;
du[j].y = dy;
}
err1 /= num;
if (err1 < breakLoopErrorThresh)
{
break;
}
if ((i > 0) && (err1 < breakLoopErrorThresh2) && (err1 / err0 > breakLoopErrorRatioThresh))
{
break;
}
if (i == maxLoop)
{
break;
}
err0 = err1;
jus.clear();
for (int j = 0; j < num; j++)
{
if(!jus.push(this._ref_matXc2U,o_matxw2xc, worldCoord[j],du[j],1.0))
{
return false;
}
}
if (!dS.setJusArray(jus))
{
return false;
}
dS.makeMat(o_matxw2xc);
}
if (o_result_param != null)
{
o_result_param.last_error = err1;
}
// *err = err1;
return true;
}
/** * ICPアルゴリズムによる姿勢推定を行います。 * * @param screenCoord * @param worldCoord * @param num * @param initMatXw2Xc * @param o_matxw2xc * @param o_result_param * 結果パラメータを受け取るオブジェクト。不要な場合はnullを指定可能。 * @return * @throws NyARException */ public abstract bool icpPoint(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int num, NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc,NyARTransMatResultParam o_result_param);
/**
* AR2Trackingの出力した頂点セットについて、変換行列を求めます。
* @param initConv
* @param i_pos2d
* 理想座標点セット
* @param i_pos3d
* 姿勢情報セット。i_pos2dに対応している必要があります。
* @param i_num
* 点セットの個数
* @param conv
* 計算結果の出力行列
* @param o_ret_param
* 返却値のパラメータ
* @return
* @throws NyARException
*/
public bool surfaceTrackingTransmat(NyARDoubleMatrix44 initConv, NyARDoublePoint2d[] i_pos2d, NyARDoublePoint3d[] i_pos3d, int i_num, NyARDoubleMatrix44 conv, NyARTransMatResultParam o_ret_param)
{
this._icp.setInlierProbability(this._last_inliner_probability);
if (!this._icp.icpPoint(i_pos2d, i_pos3d, i_num, initConv, conv, o_ret_param))
{
if (i_num < 4)
{
return(false);
}
}
if (o_ret_param.last_error > this._surface_threshold)
{
this._icp_r.setInlierProbability(0.8);
this._icp_r.icpPoint(i_pos2d, i_pos3d, i_num, conv, conv, o_ret_param);
if (o_ret_param.last_error > this._surface_threshold)
{
this._icp_r.setInlierProbability(0.6);
this._icp_r.icpPoint(i_pos2d, i_pos3d, i_num, conv, conv, o_ret_param);
if (o_ret_param.last_error > this._surface_threshold)
{
this._icp_r.setInlierProbability(0.4);
this._icp_r.icpPoint(i_pos2d, i_pos3d, i_num, conv, conv, o_ret_param);
if (o_ret_param.last_error > this._surface_threshold)
{
this._icp_r.setInlierProbability(0.0);
this._icp_r.icpPoint(i_pos2d, i_pos3d, i_num, conv, conv, o_ret_param);
if (o_ret_param.last_error > this._surface_threshold)
{
this._last_inliner_probability = 0;
return(false);
}
this._last_inliner_probability = 0;
}
this._last_inliner_probability = 0.4;
}
this._last_inliner_probability = 0.6;
}
this._last_inliner_probability = 0.8;
}
return(true);
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 計算に過去の履歴を使う点が、{@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;
//平行移動量計算機に、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);
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
//回転行列を計算
this._rotmatrix.initRotByPrevResult(i_prev_result);
//回転後の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);
// マトリクスの保存
o_result.setValue(this._rotmatrix, trans);
// エラー値が許容範囲でなければTransMatをやり直し
if (err > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR)
{
return(false);
}
// マトリクスの保存
o_result.setValue(this._rotmatrix, trans);
//エラー値保存
if (o_param != null)
{
o_param.last_error = err;
}
return(true);
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 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;
//平行移動量計算機に、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);
//回転行列を計算
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);
// マトリクスの保存
o_result.setValue(this._rotmatrix, trans);
if (o_param != null)
{
o_param.last_error = err;
}
return(true);
}
/**
* Make tansform matrix by ICP algorism.
*/
public bool transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARDoubleMatrix44 i_result, NyARTransMatResultParam o_param)
{
if (this._icpc.icpGetInitXw2Xc_from_PlanarData(i_square.sqvertex, i_offset.vertex, 4, i_result))
{
if (this._icpp.icpPoint(i_square.sqvertex, i_offset.vertex, 4, i_result, i_result, o_param))
{
return(true);
}
}
return(false);
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 計算に過去の履歴を使う点が、{@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;
//平行移動量計算機に、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);
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
//回転行列を計算
this._rotmatrix.initRotByPrevResult(i_prev_result);
//回転後の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);
// マトリクスの保存
o_result.setValue(this._rotmatrix, trans);
// エラー値が許容範囲でなければTransMatをやり直し
if (err > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR)
{
return false;
}
// マトリクスの保存
o_result.setValue(this._rotmatrix, trans);
//エラー値保存
if (o_param != null)
{
o_param.last_error = err;
}
return true;
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 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;
//平行移動量計算機に、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);
//回転行列を計算
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);
// マトリクスの保存
o_result.setValue(this._rotmatrix, trans);
if (o_param != null)
{
o_param.last_error = err;
}
return true;
}
public override bool icpPoint(NyARDoublePoint2d[] screenCoord,
NyARDoublePoint3d[] worldCoord, int num,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc, NyARTransMatResultParam o_result_param)
{
double err0 = 0, err1;
System.Diagnostics.Debug.Assert(num >= 4);
NyARIcpUtils.DeltaS dS = this.__dS;
NyARIcpUtils.U u = this.__u;
//ワークオブジェクトのリセット
if (this.__jus.getArraySize() < num)
{
this.__jus = new NyARIcpUtils.JusStack(num);
this.__du = NyARDoublePoint2d.createArray(num);
}
NyARIcpUtils.JusStack jus = this.__jus;
NyARDoublePoint2d[] du = this.__du;
o_matxw2xc.setValue(initMatXw2Xc);
double breakLoopErrorThresh = this.getBreakLoopErrorThresh();
double breakLoopErrorThresh2 = this.getBreakLoopErrorThresh2();
double breakLoopErrorRatioThresh = this.getBreakLoopErrorRatioThresh();
double maxLoop = this.getMaxLoop();
NyARDoubleMatrix44 matXw2U = this.__matXw2U;
for (int i = 0; ; i++)
{
matXw2U.mul(this._ref_matXc2U, o_matxw2xc);
err1 = 0.0;
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;
err1 += dx * dx + dy * dy;
du[j].x = dx;
du[j].y = dy;
}
err1 /= num;
if (err1 < breakLoopErrorThresh)
{
break;
}
if ((i > 0) && (err1 < breakLoopErrorThresh2) && (err1 / err0 > breakLoopErrorRatioThresh))
{
break;
}
if (i == maxLoop)
{
break;
}
err0 = err1;
jus.clear();
for (int j = 0; j < num; j++)
{
if (!jus.push(this._ref_matXc2U, o_matxw2xc, worldCoord[j], du[j], 1.0))
{
return(false);
}
}
if (!dS.setJusArray(jus))
{
return(false);
}
dS.makeMat(o_matxw2xc);
}
if (o_result_param != null)
{
o_result_param.last_error = err1;
}
// *err = err1;
return(true);
}
/**
* ICPアルゴリズムによる姿勢推定を行います。
*
* @param screenCoord
* @param worldCoord
* @param num
* @param initMatXw2Xc
* @param o_matxw2xc
* @param o_result_param
* 結果パラメータを受け取るオブジェクト。不要な場合はnullを指定可能。
* @return
* @throws NyARException
*/
public abstract bool icpPoint(NyARDoublePoint2d[] screenCoord,
NyARDoublePoint3d[] worldCoord, int num,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc, NyARTransMatResultParam o_result_param);
