/**
* 基準点のdpiを推定する。
* @param i_cptrans
* 射影変換行列. [cparam]*[trans]
* @param trans
* @param pos
* [2]
* @param o_dpi
* x,y方向それぞれの推定dpi
* @return
*/
public void ar2GetResolution2d(NyARNftFsetFile.NyAR2FeatureCoord i_pos, NyARDoublePoint2d o_dpi)
{
NyARDoubleMatrix44 t = this.ctrans;
//基点
double mx0 = i_pos.mx;
double my0 = i_pos.my;
double h0 = t.m20 * mx0 + t.m21 * my0 + t.m23;
double hx0 = t.m00 * mx0 + t.m01 * my0 + t.m03;
double hy0 = t.m10 * mx0 + t.m11 * my0 + t.m13;
double x0 = hx0 / h0;
double y0 = hy0 / h0;
double h, sx, sy;
//+X
h = h0 + t.m20 * DPI_BOX;
sx = ((hx0 + DPI_BOX * t.m00) / h) - x0;
sy = ((hy0 + DPI_BOX * t.m10) / h) - y0;
//dpi -x
o_dpi.x = Math.Sqrt(sx * sx + sy * sy) * 2.54;
//+Y
h = h0 + t.m21 * DPI_BOX;
sx = ((hx0 + DPI_BOX * t.m01) / h) - x0;
sy = ((hy0 + DPI_BOX * t.m11) / h) - y0;
//dpi -y
o_dpi.y = Math.Sqrt(sx * sx + sy * sy) * 2.54;
return;
}
/**
* x,yのうち小さい方の解像度を返す。
* @param i_pos
* 理想系座標
* @return
* 推定したdpi
*/
public double ar2GetMinResolution(NyARNftFsetFile.NyAR2FeatureCoord i_pos)
{
NyARDoubleMatrix44 t = this.ctrans;
//基点
double mx0 = i_pos.mx;
double my0 = i_pos.my;
double h0 = t.m20 * mx0 + t.m21 * my0 + t.m23;
double hx0 = t.m00 * mx0 + t.m01 * my0 + t.m03;
double hy0 = t.m10 * mx0 + t.m11 * my0 + t.m13;
double x0 = hx0 / h0;
double y0 = hy0 / h0;
double h, sx, sy;
//+X
h = h0 + t.m20 * DPI_BOX;
sx = ((hx0 + DPI_BOX * t.m00) / h) - x0;
sy = ((hy0 + DPI_BOX * t.m10) / h) - y0;
//dpi -x
double dx = Math.Sqrt(sx * sx + sy * sy) * 2.54;
//+Y
h = h0 + t.m21 * DPI_BOX;
sx = ((hx0 + DPI_BOX * t.m01) / h) - x0;
sy = ((hy0 + DPI_BOX * t.m11) / h) - y0;
//dpi -y
double dy = Math.Sqrt(sx * sx + sy * sy) * 2.54;
return(dx < dy ? dx : dy);
}
private static int selectHistory(NyARSurfaceFeatures candidate, NyARFeatureCoordPtrList i_prev_log)
{
int j;
for (int i = 0; i < i_prev_log.getLength(); i++)
{
NyARNftFsetFile.NyAR2FeatureCoord prev_item = i_prev_log.getItem(i);
for (j = 0; j < candidate.getLength(); j++)
{
NyARSurfaceFeatureItem item = candidate.getItem(j);
// 過去ログでも検出した形跡があったものを選択する。
if (prev_item == item.ref_feature)
{
return(j);
}
}
}
//残ってない
if (candidate.getLength() == 0)
{
return(-1);
}
//適当に返す
int k = (int)((double)candidate.getLength() * _rand.rand() / (RepeatedRandomizer.RAND_MAX + 1.0f));
for (int i = j = 0; i < candidate.getLength(); i++)
{
if (j == k)
{
return(i);
}
j++;
}
return(-1);
}
/**
*
* @param i_dpi
* @param ts1
* @param ts2
* @param search_size2
* @param i_occ_size
* @param max_sim_thresh
* @param min_sim_thresh
* @param sd_thresh
* @return
* @throws InterruptedException
* @throws NyARException
*/
public NyARNftFsetFile.NyAR2FeatureCoord[] ar2SelectFeature(double i_dpi, int search_size2, int i_occ_size, double max_sim_thresh, double min_sim_thresh, double sd_thresh)
{
i_occ_size *= 2;
int xsize = this._refimage.getWidth();
int ysize = this._refimage.getHeight();
double[] fimage2 = new double[xsize * ysize];
System.Array.Copy(this.fimage, fimage2, fimage2.Length);
// System.arraycopy(this.fimage, 0, fimage2, 0, fimage2.length);
TemplateImage_O1 ti = this._tmpimg;
int div_size = (ti._ts1 + ti._ts2 + 1) * 3;
int xdiv = xsize / div_size;
int ydiv = ysize / div_size;
//特徴点の最大数で配列を生成
NyARNftFsetFile.NyAR2FeatureCoord[] ret = NyARNftFsetFile.NyAR2FeatureCoord.createArray((xsize / i_occ_size) * (ysize / i_occ_size) + xdiv * ydiv);
int num_of_ret = 0;
for (; ;)
{
//長時間かかるループなので割り込み監視はブレーク可能。
// if (Thread.interrupted())
// {
// throw new InterruptedException();
// }
double min_sim = max_sim_thresh;
int fp2 = 0;
int cx = -1;
int cy = -1;
cx = cy = -1;
for (int j = 0; j < ysize; j++)
{
for (int i = 0; i < xsize; i++)
{
if (fimage2[fp2] < min_sim)
{
min_sim = fimage2[fp2];
cx = i;
cy = j;
}
fp2++;
}
}
if (cx == -1)
{
break;
}
if (!ti.make_template(this._refimage, cx, cy, 0.0f))
{
fimage2[cy * xsize + cx] = 1.0f;
continue;
}
if (ti.vlen / (ti._ts1 + ti._ts2 + 1) < sd_thresh)
{
fimage2[cy * xsize + cx] = 1.0f;
continue;
}
double min = 1.0f;
double max = -1.0f;
for (int j = -search_size2; j <= search_size2; j++)
{
for (int i = -search_size2; i <= search_size2; i++)
{
if (i * i + j * j > search_size2 * search_size2)
{
continue;
}
if (i == 0 && j == 0)
{
continue;
}
double sim = ti.get_similarity(this._refimage, cx + i, cy + j);
if (sim < 0)
{
continue;
}
if (sim < min)
{
min = sim;
if (min < min_sim_thresh && min < min_sim)
{
break;
}
}
if (sim > max)
{
max = sim;
if (max > 0.99)
{
break;
}
}
}
if ((min < min_sim_thresh && min < min_sim) || max > 0.99f)
{
break;
}
}
if ((min < min_sim_thresh && min < min_sim) || max > 0.99f)
{
fimage2[cy * xsize + cx] = 1.0f;
continue;
}
NyARNftFsetFile.NyAR2FeatureCoord p = ret[num_of_ret];
p.x = cx;
p.y = cy;
p.maxSim = min_sim;
p.mx = (float)cx / i_dpi * 25.4f;
p.my = (float)(ysize - cy) / i_dpi * 25.4f;
// System.out.println(String.format("%3d: (%3d,%3d) : %f min=%f max=%f",num_of_ret, cx, cy, min_sim, min, max));
num_of_ret++;
for (int j = -i_occ_size; j <= i_occ_size; j++)
{
for (int i = -i_occ_size; i <= i_occ_size; i++)
{
if (cy + j < 0 || cy + j >= ysize || cx + i < 0 || cx + i >= xsize)
{
continue;
}
fimage2[(cy + j) * xsize + (cx + i)] = 1.0f;
}
}
}
//Resize
NyARNftFsetFile.NyAR2FeatureCoord[] resize_ret = new NyARNftFsetFile.NyAR2FeatureCoord[num_of_ret];
System.Array.Copy(ret, resize_ret, num_of_ret);
// System.arraycopy(ret, 0, resize_ret, 0, num_of_ret);
return(resize_ret);
}
public void extractVisibleFeatures(
NyARNftFsetFile i_fset, NyARSurfaceTransMatrixSet i_ctrans,
NyARSurfaceFeatures candidate, NyARSurfaceFeatures candidate2)
{
//get work objects
NyARDoublePoint2d ide2d = this.__ide2d;
NyARDoublePoint2d obs2d = this.__obs2d;
NyARDoublePoint2d rideal2d = this.__rideal2d;
// trans1.setCoefficient(i_cparam.getPerspectiveProjectionMatrix(), trans1);
candidate.clear();
candidate2.clear();
int xsize = this._ref_size.w;
int ysize = this._ref_size.h;
INyARCameraDistortionFactor df = this._ref_dist_factor;
for (int j = 0; j < i_fset.list.Length; j++)
{
NyARNftFsetFile.NyAR2FeaturePoints fpoint_ptr = i_fset.list[j];
for (int k = 0; k < fpoint_ptr.coord.Length; k++)
{
NyARNftFsetFile.NyAR2FeatureCoord coord_ptr = fpoint_ptr.coord[k];
//理想画面点を計算
i_ctrans.calculate2dPos(coord_ptr.mx, coord_ptr.my, ide2d);
df.ideal2Observ(ide2d.x, ide2d.y, obs2d);
//観察座標に変換後、画面内にあるか確認
if (obs2d.x < 0 || obs2d.x >= xsize)
{
continue;
}
if (obs2d.y < 0 || obs2d.y >= ysize)
{
continue;
}
//逆変換可能か確認
df.observ2Ideal(obs2d, rideal2d);
if (ide2d.sqDist(rideal2d) > 1.0)
{
continue;
}
//原点からのベクトルを計算
//Z軸が+だとつかえないので判定？
if (i_ctrans.calculateVd(coord_ptr.mx, coord_ptr.my) > -0.1)
{
continue;
}
// double vd0 = trans1.m00 * coord_ptr.mx+ trans1.m01 * coord_ptr.my+ trans1.m03;
// double vd1 = trans1.m10 * coord_ptr.mx+ trans1.m11 * coord_ptr.my+ trans1.m13;
// double vd2 = trans1.m20 * coord_ptr.mx+ trans1.m21 * coord_ptr.my+ trans1.m23;
// if( (vd0*trans1.m02 + vd1*trans1.m12 + vd2*trans1.m22)/Math.sqrt( vd0*vd0 + vd1*vd1 + vd2*vd2 ) > -0.1 ){
// continue;
// }
//撮影箇所のdpiを計算(x,y方向で計算して、大・小の順番で格納？)
double dpi = i_ctrans.ar2GetMinResolution(coord_ptr);
//dpiによってコレクトする候補を分離
if (dpi <= fpoint_ptr.maxdpi && dpi >= fpoint_ptr.mindpi)
{
NyARSurfaceFeatureItem item = candidate.prePush();
if (item == null)
{
return;
}
item.ref_feature = coord_ptr;
item.scale = fpoint_ptr.scale;
item.x = obs2d.x;
item.y = obs2d.y;
}
else if (dpi <= fpoint_ptr.maxdpi * 2 && dpi >= fpoint_ptr.mindpi / 2)
{
NyARSurfaceFeatureItem item = candidate2.prePush();
if (item == null)
{
return;
}
item.ref_feature = coord_ptr;
item.scale = fpoint_ptr.scale;
item.x = obs2d.x;
item.y = obs2d.y;
}
}
}
return;
}