/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 計算に過去の履歴を使う点が、{@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;
}
public void getMatXc2U(NyARDoubleMatrix44 o_ret)
{
o_ret.setValue(this._ref_matXc2U);
}
public bool icpStereoPoint(NyARDoublePoint2d[] screenCoord_l,
NyARDoublePoint3d[] worldCoord_l, int num_l,
NyARDoublePoint2d[] screenCoord_r,
NyARDoublePoint3d[] worldCoord_r, int num_r,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 matXw2Xc)
{
System.Diagnostics.Debug.Assert(num_l + num_r >= 3);
double err0 = 0, err1;
// 6*2*num?
NyARIcpUtils.DeltaS dS = this.__dS;
//ワークオブジェクトのリセット
if (this.__jus.getArraySize() < num_l + num_r)
{
this.__jus = new NyARIcpUtils.JusStack(num_l + num_r);
this.__du = NyARDoublePoint2d.createArray(num_l + num_r);
}
NyARIcpUtils.JusStack jus = this.__jus;
NyARDoublePoint2d[] du = this.__du;
NyARIcpUtils.U u = this.__u;
NyARDoubleMatrix44 matXc2Ul = new NyARDoubleMatrix44();
NyARDoubleMatrix44 matXc2Ur = new NyARDoubleMatrix44();
NyARDoubleMatrix44 matXw2Ul = new NyARDoubleMatrix44();
NyARDoubleMatrix44 matXw2Ur = new NyARDoubleMatrix44();
matXw2Xc.setValue(initMatXw2Xc);
matXc2Ul.mul(this._ref_matXcl2Ul, this._matC2L);
matXc2Ur.mul(this._ref_matXcr2Ur, this._matC2R);
for (int i = 0; ; i++)
{
matXw2Ul.mul(matXc2Ul, matXw2Xc);
matXw2Ur.mul(matXc2Ur, matXw2Xc);
err1 = 0.0;
for (int j = 0; j < num_l; j++)
{
if (!u.setXbyMatX2U(matXw2Ul, worldCoord_l[j]))
{
return false;
}
double dx = screenCoord_l[j].x - u.x;
double dy = screenCoord_l[j].y - u.y;
err1 += dx * dx + dy * dy;
du[j].x = dx;
du[j].y = dy;
}
for (int j = 0; j < num_r; j++)
{
if (!u.setXbyMatX2U(matXw2Ur, worldCoord_r[j]))
{
return false;
}
double dx = screenCoord_r[j].x - u.x;
double dy = screenCoord_r[j].y - u.y;
err1 += dx * dx + dy * dy;
du[j + num_l].x = dx;
du[j + num_l].y = dy;
}
err1 /= (num_l + num_r);
if (err1 < this.breakLoopErrorThresh)
{
break;
}
if (i > 0 && err1 < ICP_BREAK_LOOP_ERROR_THRESH2
&& err1 / err0 > this.breakLoopErrorRatioThresh)
{
break;
}
if (i == this.maxLoop)
{
break;
}
err0 = err1;
for (int j = 0; j < num_l; j++)
{
if (!jus.push(this._ref_matXc2U, matXw2Xc, worldCoord_l[j], du[j], 1.0))
{
return false;
}
}
for (int j = 0; j < num_r; j++)
{
if (!jus.push(this._ref_matXc2U, matXw2Xc, worldCoord_r[j], du[j], 1.0))
{
return false;
}
}
if (!dS.setJusArray(jus))
{
return false;
}
dS.makeMat(matXw2Xc);
}
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;
}
static void Main(string[] args)
{
NyARDoubleMatrix44 DEST_MAT = new NyARDoubleMatrix44(
new double[] {
0.9832165682361184, 0.004789697223621061, -0.18237945710280384, -190.59060790299358,
0.012860184615056927, -0.9989882709616935, 0.04309419210331572, 64.04490277502563,
-0.18198852802987958, -0.044716355753573425, -0.9822833548209547, 616.6427596804766,
0, 0, 0, 1
});
NyARDoubleMatrix44 SRC_MAT = new NyARDoubleMatrix44(new double[] {
0.984363556, 0.00667689135, -0.176022261, -191.179672,
0.0115975942, -0.999569774, 0.0269410834, 63.0028076,
-0.175766647, -0.0285612550, -0.984017432, 611.758728,
0, 0, 0, 1
});
String img_file = "../../../../../data/testcase/test.raw";
String cparam = "../../../../../data/testcase/camera_para5.dat";
String fsetfile = "../../../../../data/testcase/pinball.fset";
String isetfile = "../../../../../data/testcase/pinball.iset5";
//カメラパラメータ
NyARParam param = NyARParam.loadFromARParamFile(File.OpenRead(cparam), 640, 480, NyARParam.DISTFACTOR_LT_ARTK5);
INyARGrayscaleRaster gs = NyARGrayscaleRaster.createInstance(640, 480);
//試験画像の準備
{
INyARRgbRaster rgb = NyARRgbRaster.createInstance(640, 480, NyARBufferType.BYTE1D_B8G8R8X8_32);
Stream fs = File.OpenRead(img_file);
byte[] b = (byte[])rgb.getBuffer();
fs.Read(b, 0, b.Length);
INyARRgb2GsFilterRgbAve filter = (INyARRgb2GsFilterRgbAve)rgb.createInterface(typeof(INyARRgb2GsFilterRgbAve));
filter.convert(gs);
}
NyARNftFsetFile fset = NyARNftFsetFile.loadFromFsetFile(File.OpenRead(fsetfile));
NyARNftIsetFile iset = NyARNftIsetFile.loadFromIsetFile(File.OpenRead(isetfile));
NyARSurfaceTracker st = new NyARSurfaceTracker(param, 16, 0.5);
NyARSurfaceDataSet sd = new NyARSurfaceDataSet(iset, fset);
NyARDoubleMatrix44 sret = new NyARDoubleMatrix44();
NyARDoublePoint2d[] o_pos2d = NyARDoublePoint2d.createArray(16);
NyARDoublePoint3d[] o_pos3d = NyARDoublePoint3d.createArray(16);
NyARSurfaceTrackingTransmatUtils tmat = new NyARSurfaceTrackingTransmatUtils(param, 5.0);
NyARDoubleMatrix44 tret = new NyARDoubleMatrix44();
for (int j = 0; j < 10; j++)
{
Stopwatch s = new Stopwatch();
s.Reset();
s.Start();
for (int i = 0; i < 3000; i++)
{
sret.setValue(SRC_MAT);
int nop = st.tracking(gs, sd, sret, o_pos2d, o_pos3d, 16);
//Transmatの試験
NyARDoublePoint3d off = NyARSurfaceTrackingTransmatUtils.centerOffset(o_pos3d, nop, new NyARDoublePoint3d());
NyARSurfaceTrackingTransmatUtils.modifyInputOffset(sret, o_pos3d, nop, off);
tmat.surfaceTrackingTransmat(sret, o_pos2d, o_pos3d, nop, tret, new NyARTransMatResultParam());
NyARSurfaceTrackingTransmatUtils.restoreOutputOffset(tret, off);
System.Console.WriteLine(tret.Equals(DEST_MAT));
}
s.Stop();
System.Console.WriteLine(s.ElapsedMilliseconds);
}
return;
}
public bool icpStereoPoint(NyARDoublePoint2d[] screenCoord_l,
NyARDoublePoint3d[] worldCoord_l, int num_l,
NyARDoublePoint2d[] screenCoord_r,
NyARDoublePoint3d[] worldCoord_r, int num_r,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 matXw2Xc)
{
System.Diagnostics.Debug.Assert(num_l + num_r >= 3);
double err0 = 0, err1;
// 6*2*num?
NyARIcpUtils.DeltaS dS = this.__dS;
//ワークオブジェクトのリセット
if (this.__jus.getArraySize() < num_l + num_r)
{
this.__jus = new NyARIcpUtils.JusStack(num_l + num_r);
this.__du = NyARDoublePoint2d.createArray(num_l + num_r);
}
NyARIcpUtils.JusStack jus = this.__jus;
NyARDoublePoint2d[] du = this.__du;
NyARIcpUtils.U u = this.__u;
NyARDoubleMatrix44 matXc2Ul = new NyARDoubleMatrix44();
NyARDoubleMatrix44 matXc2Ur = new NyARDoubleMatrix44();
NyARDoubleMatrix44 matXw2Ul = new NyARDoubleMatrix44();
NyARDoubleMatrix44 matXw2Ur = new NyARDoubleMatrix44();
matXw2Xc.setValue(initMatXw2Xc);
matXc2Ul.mul(this._ref_matXcl2Ul, this._matC2L);
matXc2Ur.mul(this._ref_matXcr2Ur, this._matC2R);
for (int i = 0; ; i++)
{
matXw2Ul.mul(matXc2Ul, matXw2Xc);
matXw2Ur.mul(matXc2Ur, matXw2Xc);
err1 = 0.0;
for (int j = 0; j < num_l; j++)
{
if (!u.setXbyMatX2U(matXw2Ul, worldCoord_l[j]))
{
return(false);
}
double dx = screenCoord_l[j].x - u.x;
double dy = screenCoord_l[j].y - u.y;
err1 += dx * dx + dy * dy;
du[j].x = dx;
du[j].y = dy;
}
for (int j = 0; j < num_r; j++)
{
if (!u.setXbyMatX2U(matXw2Ur, worldCoord_r[j]))
{
return(false);
}
double dx = screenCoord_r[j].x - u.x;
double dy = screenCoord_r[j].y - u.y;
err1 += dx * dx + dy * dy;
du[j + num_l].x = dx;
du[j + num_l].y = dy;
}
err1 /= (num_l + num_r);
if (err1 < this.breakLoopErrorThresh)
{
break;
}
if (i > 0 && err1 < ICP_BREAK_LOOP_ERROR_THRESH2 &&
err1 / err0 > this.breakLoopErrorRatioThresh)
{
break;
}
if (i == this.maxLoop)
{
break;
}
err0 = err1;
for (int j = 0; j < num_l; j++)
{
if (!jus.push(this._ref_matXc2U, matXw2Xc, worldCoord_l[j], du[j], 1.0))
{
return(false);
}
}
for (int j = 0; j < num_r; j++)
{
if (!jus.push(this._ref_matXc2U, matXw2Xc, worldCoord_r[j], du[j], 1.0))
{
return(false);
}
}
if (!dS.setJusArray(jus))
{
return(false);
}
dS.makeMat(matXw2Xc);
}
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);
}
public void getMatXc2U(NyARDoubleMatrix44 o_ret)
{
o_ret.setValue(this._ref_matXc2U);
}