/**
* この関数は、頂点同士の距離から、頂点のシフト量（回転量）を返します。
* よく似た２つの矩形の頂点同士の、頂点の対応を取るために使用します。
* @param i_square
* 比較対象の矩形
* @return
* シフト量を数値で返します。
* シフト量はthis-i_squareです。1の場合、this.sqvertex[0]とi_square.sqvertex[1]が対応点になる(shift量1)であることを示します。
*/
public int checkVertexShiftValue(NyARSquare i_square)
{
NyARDoublePoint2d[] a = this.sqvertex;
NyARDoublePoint2d[] b = i_square.sqvertex;
//3-0番目
int min_dist = int.MaxValue;
int min_index = 0;
int xd, yd;
for (int i = 3; i >= 0; i--)
{
int d = 0;
for (int i2 = 3; i2 >= 0; i2--)
{
xd = (int)(a[i2].x - b[(i2 + i) % 4].x);
yd = (int)(a[i2].y - b[(i2 + i) % 4].y);
d += xd * xd + yd * yd;
}
if (min_dist > d)
{
min_dist = d;
min_index = i;
}
}
return min_index;
}
/**
* この関数は、頂点同士の距離から、頂点のシフト量（回転量）を返します。
* よく似た２つの矩形の頂点同士の、頂点の対応を取るために使用します。
* @param i_square
* 比較対象の矩形
* @return
* シフト量を数値で返します。
* シフト量はthis-i_squareです。1の場合、this.sqvertex[0]とi_square.sqvertex[1]が対応点になる(shift量1)であることを示します。
*/
public int checkVertexShiftValue(NyARSquare i_square)
{
NyARDoublePoint2d[] a = this.sqvertex;
NyARDoublePoint2d[] b = i_square.sqvertex;
//3-0番目
int min_dist = int.MaxValue;
int min_index = 0;
int xd, yd;
for (int i = 3; i >= 0; i--)
{
int d = 0;
for (int i2 = 3; i2 >= 0; i2--)
{
xd = (int)(a[i2].x - b[(i2 + i) % 4].x);
yd = (int)(a[i2].y - b[(i2 + i) % 4].y);
d += xd * xd + yd * yd;
}
if (min_dist > d)
{
min_dist = d;
min_index = i;
}
}
return min_index;
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 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;
double err_threshold = makeErrThreshold(i_square.sqvertex);
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;
}
//平行移動量計算機に、2D座標系をセット
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, err_threshold, o_result);
//必要なら計算パラメータを返却
if (o_param != null)
{
o_param.last_error = err;
}
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;
//最適化計算の閾値を決定
double err_threshold = makeErrThreshold(i_square.sqvertex);
//平行移動量計算機に、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);
//回転行列を計算
NyARRotMatrix rot = this._rotmatrix;
rot.initRotByPrevResult(i_prev_result);
//回転後の3D座標系から、平行移動量を計算
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
rot.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//現在のエラーレートを計算
double min_err = errRate(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
//エラーレートの判定
if (min_err < i_prev_err + err_threshold)
{
//save initial result
o_result.setValue(rot, trans);
// System.out.println("TR:ok");
//最適化してみる。
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);
o_result.setValue(rot, trans);
min_err = err;
}
//継続計算成功
if (o_param != null)
{
o_param.last_error = min_err;
}
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;
//最適化計算の閾値を決定
double err_threshold = makeErrThreshold(i_square.sqvertex);
//平行移動量計算機に、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);
//回転行列を計算
NyARRotMatrix rot = this._rotmatrix;
rot.initRotByPrevResult(i_prev_result);
//回転後の3D座標系から、平行移動量を計算
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
rot.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//現在のエラーレートを計算
double min_err = errRate(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
//エラーレートの判定
if(min_err<i_prev_err+err_threshold){
//save initial result
o_result.setValue(rot,trans);
// System.out.println("TR:ok");
//最適化してみる。
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);
o_result.setValue(rot, trans);
min_err = err;
}
//継続計算成功
if (o_param != null)
{
o_param.last_error = min_err;
}
return true;
}
//継続計算失敗
return false;
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 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;
double err_threshold = makeErrThreshold(i_square.sqvertex);
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;
}
//平行移動量計算機に、2D座標系をセット
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, err_threshold, o_result);
//必要なら計算パラメータを返却
if (o_param != null)
{
o_param.last_error = err;
}
return true;
}
/** オブジェクトのステータスを更新し、必要に応じて自己コールバック関数を駆動します。
* 戻り値は、「実際にマーカを発見する事ができたか」を示す真偽値です。クラスの状態とは異なります。
*/
private bool updateStatus(NyARSquare i_square, int i_code_index)
{
if (this._current_arcode_index < 0) {// 未認識中
if (i_code_index < 0) {// 未認識から未認識の遷移
// なにもしないよーん。
return false;
}
else {// 未認識から認識の遷移
this._current_arcode_index = i_code_index;
// イベント生成
// OnEnter
this.onEnterHandler(i_code_index);
// 変換行列を作成
this._transmat.transMat(i_square, this._offset, this._transmat_result, this._last_result_param);
// OnUpdate
this.onUpdateHandler(i_square, this._transmat_result);
this._lost_delay_count = 0;
return true;
}
}
else {// 認識中
if (i_code_index < 0) {// 認識から未認識の遷移
this._lost_delay_count++;
if (this._lost_delay < this._lost_delay_count) {
// OnLeave
this._current_arcode_index = -1;
this.onLeaveHandler();
}
return false;
}
else if (i_code_index == this._current_arcode_index) {// 同じARCodeの再認識
// イベント生成
// 変換行列を作成
if (!this._transmat.transMatContinue(i_square, this._offset, this._transmat_result, this._last_result_param.last_error, this._transmat_result, this._last_result_param)) {
this._transmat.transMat(i_square, this._offset, this._transmat_result, this._last_result_param);
}
// OnUpdate
this.onUpdateHandler(i_square, this._transmat_result);
this._lost_delay_count = 0;
return true;
}
else {// 異なるコードの認識→今はサポートしない。
throw new NyARException();
}
}
}
/** * 自己コールバック関数です。 * 継承したクラスで、マーカ更新時の処理を実装してください。 * 引数の値の有効期間は、関数が終了するまでです。 * @param i_square * 現在のマーカ検出位置です。 * @param o_result * 現在の姿勢変換行列です。 */ protected abstract void onUpdateHandler(NyARSquare i_square, NyARDoubleMatrix44 o_result);
/**オブジェクトのステータスを更新し、必要に応じて自己コールバック関数を駆動します。
*/
private bool UpdateStatus(NyARSquare i_square, INyIdMarkerData i_marker_data)
{
bool is_id_found = false;
if (!this._is_active) {// 未認識中
if (i_marker_data == null) {// 未認識から未認識の遷移
// なにもしないよーん。
this._is_active = false;
}
else {// 未認識から認識の遷移
this._data_current.copyFrom(i_marker_data);
// イベント生成
// OnEnter
this.OnEnterHandler(this._data_current);
// 変換行列を作成
this._transmat.transMat(i_square, this._offset, this._transmat_result, this._last_result_param);
// OnUpdate
this.OnUpdateHandler(i_square, this._transmat_result);
this._lost_delay_count = 0;
this._is_active = true;
is_id_found = true;
}
}
else {// 認識中
if (i_marker_data == null) {
// 認識から未認識の遷移
this._lost_delay_count++;
if (this._lost_delay < this._lost_delay_count) {
// OnLeave
this.OnLeaveHandler();
this._is_active = false;
}
}
else if (this._data_current.isEqual(i_marker_data)) {
//同じidの再認識
if (!this._transmat.transMatContinue(i_square, this._offset, this._transmat_result, this._last_result_param.last_error, this._transmat_result, this._last_result_param)) {
this._transmat.transMat(i_square, this._offset, this._transmat_result, this._last_result_param);
}
// OnUpdate
this.OnUpdateHandler(i_square, this._transmat_result);
this._lost_delay_count = 0;
is_id_found = true;
}
else {// 異なるコードの認識→今はサポートしない。
throw new NyARException();
}
}
return is_id_found;
}
/**
* 頂点データをNyARSquareにセットする関数です。
* 初期位置セットには使わないこと。
* @param i_vx
* @param i_s
*/
private void SetSquare(NyARDoublePoint2d[] i_vx, NyARSquare i_s)
{
NyARLinear l = this.__tmp_l;
//線分を平滑化。（ノイズが多いソースを使う時は線分の平滑化。ほんとは使いたくない。）
for (int i = 3; i >= 0; i--) {
i_s.sqvertex[i].setValue(i_vx[i]);
l.makeLinearWithNormalize(i_vx[i], i_vx[(i + 1) % 4]);
i_s.line[i].a = i_s.line[i].a * 0.6 + l.a * 0.4;
i_s.line[i].b = i_s.line[i].b * 0.6 + l.b * 0.4;
i_s.line[i].c = i_s.line[i].c * 0.6 + l.c * 0.4;
}
for (int i = 3; i >= 0; i--) {
i_s.line[i].crossPos(i_s.line[(i + 3) % 4], i_s.sqvertex[i]);
}
}