/**
* @param i_width
* @param i_height
*/
public NyARColorPatt_PseudoAffine(int i_width, int i_height)
{
this._size = new NyARIntSize(i_width, i_height);
this._patdata = new int[i_height * i_width];
this._pixelreader = new NyARRgbPixelReader_INT1D_X8R8G8B8_32(this._patdata, this._size);
//疑似アフィン変換のパラメタマトリクスを計算します。
//長方形から計算すると、有効要素がm00,m01,m02,m03,m10,m11,m20,m23,m30になります。
NyARDoubleMatrix44 mat = this._invmat;
mat.m00 = 0;
mat.m01 = 0;
mat.m02 = 0;
mat.m03 = 1.0;
mat.m10 = 0;
mat.m11 = i_width - 1;
mat.m12 = 0;
mat.m13 = 1.0;
mat.m20 = (i_width - 1) * (i_height - 1);
mat.m21 = i_width - 1;
mat.m22 = i_height - 1;
mat.m23 = 1.0;
mat.m30 = 0;
mat.m31 = 0;
mat.m32 = i_height - 1;
mat.m33 = 1.0;
mat.inverse(mat);
return;
}
/**
* {@link #icpGetInitXw2XcSub}関数のmat_dを計算します。
*/
private static NyARDoubleMatrix33 makeMatD(NyARDoubleMatrix44 i_cp, NyARDoublePoint2d[] pos2d, int i_num, NyARDoubleMatrix33 o_mat)
{
double m02 = 0;
double m12 = 0;
double m20 = 0;
double m21 = 0;
double m22 = 0;
for (int i = 0; i < i_num; i++)
{
double cx = i_cp.m02 - pos2d[i].x;
double cy = i_cp.m12 - pos2d[i].y;
m02 += (cx) * i_cp.m00;
m12 += (cx) * i_cp.m01 + (cy) * i_cp.m11;
m20 += i_cp.m00 * (cx);
m21 += i_cp.m01 * (cx) + i_cp.m11 * (cy);
m22 += (cx) * (cx) + (cy) * (cy);
}
o_mat.m00 = (i_cp.m00 * i_cp.m00) * i_num;
o_mat.m01 = (i_cp.m00 * i_cp.m01) * i_num;
o_mat.m02 = m02;
o_mat.m10 = (i_cp.m01 * i_cp.m00) * i_num;
o_mat.m11 = (i_cp.m01 * i_cp.m01 + i_cp.m11 * i_cp.m11) * i_num;
o_mat.m12 = m12;
o_mat.m20 = m20;
o_mat.m21 = m21;
o_mat.m22 = m22;
o_mat.inverse(o_mat);
return(o_mat);
}
public Form1()
{
InitializeComponent();
//ARの設定
//AR用カメラパラメタファイルをロード
NyARParam ap = NyARParam.createFromARParamFile(new StreamReader(AR_CAMERA_FILE));
ap.changeScreenSize(320, 240);
//AR用のパターンコードを読み出し
NyARCode code = NyARCode.createFromARPattFile(new StreamReader(AR_CODE_FILE),16, 16);
NyARDoubleMatrix44 result_mat = new NyARDoubleMatrix44();
//計算モードの設定
//キャプチャを作る
/**************************************************
このコードは、0番目（一番初めに見つかったキャプチャデバイス）
を使用するようにされています。
複数のキャプチャデバイスを持つシステムの場合、うまく動作しないかもしれません。
n番目のデバイスを使いたいときには、CaptureDevice cap=cl[0];←ここの0を変えてください。
手動で選択させる方法は、SimpleLiteDirect3Dを参考にしてください。
**************************************************/
CaptureDeviceList cl=new CaptureDeviceList();
CaptureDevice cap=cl[0];
cap.SetCaptureListener(this);
cap.PrepareCapture(320, 240,30);
this.m_cap = cap;
//ラスタを作る。
this.m_raster = new DsRgbRaster(cap.video_width, cap.video_height,NyARBufferType.OBJECT_CS_Bitmap);
//１パターンのみを追跡するクラスを作成
this.m_ar = NyARSingleDetectMarker.createInstance(ap, code, 80.0);
this.m_ar.setContinueMode(false);
}
/**
* 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);
}
/**
* {@link #icpGetInitXw2XcSub}関数のmat_eを計算します。
*/
private static double[] makeMatE(NyARDoubleMatrix44 i_cp, NyARDoubleMatrix44 rot, NyARDoublePoint2d[] pos2d, NyARDoublePoint3d[] pos3d, NyARDoublePoint3d offset, int i_num, double[] o_val)
{
double v0 = 0;
double v1 = 0;
double v2 = 0;
for (int j = 0; j < i_num; j++)
{
double p3x = pos3d[j].x + offset.x;
double p3y = pos3d[j].y + offset.y;
double p3z = pos3d[j].z + offset.z;
double wx = rot.m00 * p3x + rot.m01 * p3y + rot.m02 * p3z;
double wy = rot.m10 * p3x + rot.m11 * p3y + rot.m12 * p3z;
double wz = rot.m20 * p3x + rot.m21 * p3y + rot.m22 * p3z;
double c1 = wz * pos2d[j].x - i_cp.m00 * wx - i_cp.m01 * wy - i_cp.m02 * wz;
double c2 = wz * pos2d[j].y - i_cp.m11 * wy - i_cp.m12 * wz;
v0 += i_cp.m00 * c1;
v1 += i_cp.m01 * c1 + i_cp.m11 * c2;
v2 += (i_cp.m02 - pos2d[j].x) * c1 + (i_cp.m12 - pos2d[j].y) * c2;
}
o_val[0] = v0;
o_val[1] = v1;
o_val[2] = v2;
return(o_val);
}
/**
* コンストラクタです。
* @param i_width
* このラスタの幅
* @param i_height
* このラスタの高さ
* @
*/
public NyARColorPatt_PseudoAffine(int i_width, int i_height)
: base(i_width, i_height, true)
{
//疑似アフィン変換のパラメタマトリクスを計算します。
//長方形から計算すると、有効要素がm00,m01,m02,m03,m10,m11,m20,m23,m30になります。
NyARDoubleMatrix44 mat = this._invmat;
mat.m00 = 0;
mat.m01 = 0;
mat.m02 = 0;
mat.m03 = 1.0;
mat.m10 = 0;
mat.m11 = i_width - 1;
mat.m12 = 0;
mat.m13 = 1.0;
mat.m20 = (i_width - 1) * (i_height - 1);
mat.m21 = i_width - 1;
mat.m22 = i_height - 1;
mat.m23 = 1.0;
mat.m30 = 0;
mat.m31 = 0;
mat.m32 = i_height - 1;
mat.m33 = 1.0;
mat.inverse(mat);
return;
}
/**
* 基準点の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;
}
/**
* この関数は、行列同士の掛け算をして、インスタンスに格納します。
* i_mat_lとi_mat_rには、thisを指定しないでください。
* @param i_mat_l
* 左成分の行列
* @param i_mat_r
* 右成分の行列
*/
public void mul(NyARDoubleMatrix44 i_mat_l, NyARDoubleMatrix44 i_mat_r)
{
Debug.Assert(this != i_mat_l);
Debug.Assert(this != i_mat_r);
this.m00 = i_mat_l.m00 * i_mat_r.m00 + i_mat_l.m01 * i_mat_r.m10 + i_mat_l.m02 * i_mat_r.m20 + i_mat_l.m03 * i_mat_r.m30;
this.m01 = i_mat_l.m00 * i_mat_r.m01 + i_mat_l.m01 * i_mat_r.m11 + i_mat_l.m02 * i_mat_r.m21 + i_mat_l.m03 * i_mat_r.m31;
this.m02 = i_mat_l.m00 * i_mat_r.m02 + i_mat_l.m01 * i_mat_r.m12 + i_mat_l.m02 * i_mat_r.m22 + i_mat_l.m03 * i_mat_r.m32;
this.m03 = i_mat_l.m00 * i_mat_r.m03 + i_mat_l.m01 * i_mat_r.m13 + i_mat_l.m02 * i_mat_r.m23 + i_mat_l.m03 * i_mat_r.m33;
this.m10 = i_mat_l.m10 * i_mat_r.m00 + i_mat_l.m11 * i_mat_r.m10 + i_mat_l.m12 * i_mat_r.m20 + i_mat_l.m13 * i_mat_r.m30;
this.m11 = i_mat_l.m10 * i_mat_r.m01 + i_mat_l.m11 * i_mat_r.m11 + i_mat_l.m12 * i_mat_r.m21 + i_mat_l.m13 * i_mat_r.m31;
this.m12 = i_mat_l.m10 * i_mat_r.m02 + i_mat_l.m11 * i_mat_r.m12 + i_mat_l.m12 * i_mat_r.m22 + i_mat_l.m13 * i_mat_r.m32;
this.m13 = i_mat_l.m10 * i_mat_r.m03 + i_mat_l.m11 * i_mat_r.m13 + i_mat_l.m12 * i_mat_r.m23 + i_mat_l.m13 * i_mat_r.m33;
this.m20 = i_mat_l.m20 * i_mat_r.m00 + i_mat_l.m21 * i_mat_r.m10 + i_mat_l.m22 * i_mat_r.m20 + i_mat_l.m23 * i_mat_r.m30;
this.m21 = i_mat_l.m20 * i_mat_r.m01 + i_mat_l.m21 * i_mat_r.m11 + i_mat_l.m22 * i_mat_r.m21 + i_mat_l.m23 * i_mat_r.m31;
this.m22 = i_mat_l.m20 * i_mat_r.m02 + i_mat_l.m21 * i_mat_r.m12 + i_mat_l.m22 * i_mat_r.m22 + i_mat_l.m23 * i_mat_r.m32;
this.m23 = i_mat_l.m20 * i_mat_r.m03 + i_mat_l.m21 * i_mat_r.m13 + i_mat_l.m22 * i_mat_r.m23 + i_mat_l.m23 * i_mat_r.m33;
this.m30 = i_mat_l.m30 * i_mat_r.m00 + i_mat_l.m31 * i_mat_r.m10 + i_mat_l.m32 * i_mat_r.m20 + i_mat_l.m33 * i_mat_r.m30;
this.m31 = i_mat_l.m30 * i_mat_r.m01 + i_mat_l.m31 * i_mat_r.m11 + i_mat_l.m32 * i_mat_r.m21 + i_mat_l.m33 * i_mat_r.m31;
this.m32 = i_mat_l.m30 * i_mat_r.m02 + i_mat_l.m31 * i_mat_r.m12 + i_mat_l.m32 * i_mat_r.m22 + i_mat_l.m33 * i_mat_r.m32;
this.m33 = i_mat_l.m30 * i_mat_r.m03 + i_mat_l.m31 * i_mat_r.m13 + i_mat_l.m32 * i_mat_r.m23 + i_mat_l.m33 * i_mat_r.m33;
return;
}
/**
* 変換行列と頂点座標から、パラメータを計算
* o_paramの[0..3]にはXのパラメタ、[4..7]にはYのパラメタを格納する。
* @param i_vertex
* @param pa
* @param pb
*/
private void calcPara(NyARIntPoint2d[] i_vertex, double[] o_cparam)
{
NyARDoubleMatrix44 invmat = this._invmat;
double v1, v2, v4;
//変換行列とベクトルの積から、変換パラメタを計算する。
v1 = i_vertex[0].x;
v2 = i_vertex[1].x;
v4 = i_vertex[3].x;
o_cparam[0] = invmat.m00 * v1 + invmat.m01 * v2 + invmat.m02 * i_vertex[2].x + invmat.m03 * v4;
o_cparam[1] = invmat.m10 * v1 + invmat.m11 * v2; //m12,m13は0;
o_cparam[2] = invmat.m20 * v1 + invmat.m23 * v4; //m21,m22は0;
o_cparam[3] = v1; //m30は1.0で、m31,m32,m33は0
v1 = i_vertex[0].y;
v2 = i_vertex[1].y;
v4 = i_vertex[3].y;
o_cparam[4] = invmat.m00 * v1 + invmat.m01 * v2 + invmat.m02 * i_vertex[2].y + invmat.m03 * v4;
o_cparam[5] = invmat.m10 * v1 + invmat.m11 * v2; //m12,m13は0;
o_cparam[6] = invmat.m20 * v1 + invmat.m23 * v4; //m21,m22は0;
o_cparam[7] = v1; //m30は1.0で、m31,m32,m33は0
return;
}
/**
* この関数は、オブジェクトの配列を生成して返します。
* @param i_number
* 配列の長さ
* @return
* 新しいオブジェクト配列
*/
public static NyARDoubleMatrix44[] createArray(int i_number)
{
NyARDoubleMatrix44[] ret = new NyARDoubleMatrix44[i_number];
for (int i = 0; i < i_number; i++)
{
ret[i] = new NyARDoubleMatrix44();
}
return ret;
}
/**
* この関数は、オブジェクトの配列を生成して返します。
* @param i_number
* 配列の長さ
* @return
* 新しいオブジェクト配列
*/
public static NyARDoubleMatrix44[] createArray(int i_number)
{
NyARDoubleMatrix44[] ret = new NyARDoubleMatrix44[i_number];
for (int i = 0; i < i_number; i++)
{
ret[i] = new NyARDoubleMatrix44();
}
return(ret);
}
/**
* なんだろうこれ。Z位置？
* @param mx
* @param my
* @return
*/
public double calculateVd(double mx, double my)
{
NyARDoubleMatrix44 t = this.trans;
double vd0 = t.m00 * mx + t.m01 * my + t.m03;
double vd1 = t.m10 * mx + t.m11 * my + t.m13;
double vd2 = t.m20 * mx + t.m21 * my + t.m23;
return((vd0 * t.m02 + vd1 * t.m12 + vd2 * t.m22) / Math.Sqrt(vd0 * vd0 + vd1 * vd1 + vd2 * vd2));
}
/**
* 現在の特徴点セットから、
* @param i_keymap
* @param i_result
* @return
*/
public bool kpmMatching(KeyframeMap i_keymap, NyARDoubleMatrix44 i_transmat)
{
FeaturePairStack result = new FeaturePairStack(kMaxNumFeatures);
if (!this.query(this.mQueryKeyframe, i_keymap, result))
{
return(false);
}
return(kpmUtilGetPose_binary(this._ref_cparam, result, i_transmat, this._result_param));
}
/**
* 理想点を計算する。
* @param i_coord
* 変換元の座標(理想点)
*/
public void calculate2dPos(double i_x, double i_y, NyARDoublePoint2d o_idepos)
{
NyARDoubleMatrix44 t = this.ctrans;
double h = (t.m20 * i_x + t.m21 * i_y + t.m23);
double hx = (t.m00 * i_x + t.m01 * i_y + t.m03) / h;
double hy = (t.m10 * i_x + t.m11 * i_y + t.m13) / h;
o_idepos.x = hx;
o_idepos.y = hy;
}
public PerspectiveParam getPerspectiveParam(PerspectiveParam o_value)
{
NyARDoubleMatrix44 mat = this._frustum_rh;
o_value.far = mat.m23 / (mat.m22 + 1);
o_value.near = mat.m23 / (mat.m22 - 1);
o_value.aspect = mat.m11 / mat.m00;
o_value.fovy = 2 * Math.Atan(1 / (mat.m00 * o_value.aspect));
return(o_value);
}
/**
* カメラ座標系の点を、スクリーン座標の点へ変換します。
* @param i_x
* カメラ座標系の点
* @param i_y
* カメラ座標系の点
* @param i_z
* カメラ座標系の点
* @param o_pos2d
* 結果を受け取るオブジェクトです。
*/
public void project(double i_x, double i_y, double i_z, NyARDoublePoint2d o_pos2d)
{
NyARDoubleMatrix44 m = this._frustum_rh;
double v3_1 = 1 / i_z * m.m32;
double w = this._screen_size.w;
double h = this._screen_size.h;
o_pos2d.x = w - (1 + (i_x * m.m00 + i_z * m.m02) * v3_1) * w / 2;
o_pos2d.y = h - (1 + (i_y * m.m11 + i_z * m.m12) * v3_1) * h / 2;
return;
}
/**
* 元ar2GenTemplate関数。
* 与えられた座標を中心に、テンプレート画像を生成する。
* 座標は観察座標点。
* @param i_x
* @param i_y
* @param i_scale
* @param o_template
* @return
* @throws NyARException
*/
public void makeFromReferenceImage(double i_x, double i_y, NyARDoubleMatrix44 i_ref_ctrans, INyARCameraDistortionFactor i_ref_dist_factor, NyARNftIsetFile.ReferenceImage i_source)
{
int[] img = this.img;
int img1_ptr = 0;
int k = 0;
int sum2 = 0;
int sum = 0;
NyARDoublePoint2d ideal = this.__in;
for (int j = -(this.yts); j <= this.yts; j++)
{
for (int i = -(this.xts); i <= this.xts; i++)
{
i_ref_dist_factor.observ2Ideal(i_x + i * AR2_TEMP_SCALE, i_y + j * AR2_TEMP_SCALE, ideal);
double ideal_x = ideal.x;
double ideal_y = ideal.y;
//ar2ScreenCoord2MarkerCoord(in.x,in.y,i_ref_ctrans,in);の展開
double c11 = i_ref_ctrans.m20 * ideal_x - i_ref_ctrans.m00;
double c12 = i_ref_ctrans.m21 * ideal_x - i_ref_ctrans.m01;
double c21 = i_ref_ctrans.m20 * ideal_y - i_ref_ctrans.m10;
double c22 = i_ref_ctrans.m21 * ideal_y - i_ref_ctrans.m11;
double b1 = i_ref_ctrans.m03 - i_ref_ctrans.m23 * ideal_x;
double b2 = i_ref_ctrans.m13 - i_ref_ctrans.m23 * ideal_y;
double m = c11 * c22 - c12 * c21;
//public int ar2GetImageValue(double sx, double sy) throws NyARExceptionの展開
{
int ix = (int)((((c22 * b1 - c12 * b2) / m) * i_source.dpi / 25.4f) + 0.5);
int iy = (int)((i_source.height - (((c11 * b2 - c21 * b1) / m) * i_source.dpi) / 25.4f) + 0.5);
//座標計算と値取得は分けよう。
if (ix < 0 || ix >= i_source.width || iy < 0 || iy >= i_source.height)
{
img[img1_ptr] = AR2_TEMPLATE_NULL_PIXEL;
}
else
{
int ret = img[img1_ptr] = i_source.img[iy * i_source.width + ix];
sum2 += ret * ret;
sum += ret;
k++;
}
//byte値はint化
}
img1_ptr++;
}
}
int vlen = sum2 - sum * sum / k;
this.vlen = (int)Math.Sqrt((double)(vlen));
this.sum_of_img = sum;
this.valid_pixels = k;
return;
}
/**
* 透視変換パラメータを行列から復元します。
* @param o_value
* @return
* 値をセットしたo_valueを返します。
*/
public FrustumParam getFrustumParam(FrustumParam o_value)
{
double near;
NyARDoubleMatrix44 mat = this._frustum_rh;
o_value.far = mat.m23 / (mat.m22 + 1);
o_value.near = near = mat.m23 / (mat.m22 - 1);
o_value.left = (mat.m02 - 1) * near / mat.m00;
o_value.right = (mat.m02 + 1) * near / mat.m00;
o_value.bottom = (mat.m12 - 1) * near / mat.m11;
o_value.top = (mat.m12 + 1) * near / mat.m11;
return(o_value);
}
/**
* インスタンスの値とi_matの値が同一かを返します。
*/
override public bool Equals(Object i_mat)
{
NyARDoubleMatrix44 m = (NyARDoubleMatrix44)i_mat;
if (m.m00 == this.m00 && m.m01 == this.m01 && m.m02 == this.m02 && m.m03 == this.m03 &&
m.m10 == this.m10 && m.m11 == this.m11 && m.m12 == this.m12 && m.m13 == this.m13 &&
m.m20 == this.m20 && m.m21 == this.m21 && m.m22 == this.m22 && m.m23 == this.m23 &&
m.m30 == this.m30 && m.m31 == this.m31 && m.m32 == this.m32 && m.m33 == this.m33)
{
return(true);
}
return(false);
}
/**
* icpGetU_from_X_by_MatX2U関数
* @param matX2U
* @param coord3d
* @return
*/
public bool setXbyMatX2U(NyARDoubleMatrix44 matX2U, NyARDoublePoint3d coord3d)
{
double hx = matX2U.m00 * coord3d.x + matX2U.m01 * coord3d.y + matX2U.m02 * coord3d.z + matX2U.m03;
double hy = matX2U.m10 * coord3d.x + matX2U.m11 * coord3d.y + matX2U.m12 * coord3d.z + matX2U.m13;
double h = matX2U.m20 * coord3d.x + matX2U.m21 * coord3d.y + matX2U.m22 * coord3d.z + matX2U.m23;
if (h == 0.0)
{
return(false);
}
this.x = hx / h;
this.y = hy / h;
return(true);
}
/**
* この関数は、スクリーン座標を撮像点座標に変換します。
* 撮像点の座標系は、カメラ座標系になります。
* <p>公式 -
* この関数は、gluUnprojectのビューポートとモデルビュー行列を固定したものです。
* 公式は、以下の物使用しました。
* http://www.opengl.org/sdk/docs/man/xhtml/gluUnProject.xml
* ARToolKitの座標系に合せて計算するため、OpenGLのunProjectとはix,iyの与え方が違います。画面上の座標をそのまま与えてください。
* </p>
* @param ix
* スクリーン上の座標
* @param iy
* 画像上の座標
* @param o_point_on_screen
* 撮像点座標
*/
public void unProject(double ix, double iy, NyARDoublePoint3d o_point_on_screen)
{
double n = (this._frustum_rh.m23 / (this._frustum_rh.m22 - 1));
NyARDoubleMatrix44 m44 = this._inv_frustum_rh;
double v1 = (this._screen_size.w - ix - 1) * 2 / this._screen_size.w - 1.0;//ARToolKitのFrustramに合せてる。
double v2 = (this._screen_size.h - iy - 1) * 2 / this._screen_size.h - 1.0;
double v3 = 2 * n - 1.0;
double b = 1 / (m44.m30 * v1 + m44.m31 * v2 + m44.m32 * v3 + m44.m33);
o_point_on_screen.x = (m44.m00 * v1 + m44.m01 * v2 + m44.m02 * v3 + m44.m03) * b;
o_point_on_screen.y = (m44.m10 * v1 + m44.m11 * v2 + m44.m12 * v3 + m44.m13) * b;
o_point_on_screen.z = (m44.m20 * v1 + m44.m21 * v2 + m44.m22 * v3 + m44.m23) * b;
return;
}
/**
* 画面上の点と原点を結ぶ直線と任意姿勢の平面の交差点を、平面の座標系で取得します。
* ARToolKitの本P175周辺の実装と同じです。
* <p>
* このAPIは繰り返し使用には最適化されていません。同一なi_matに繰り返しアクセスするときは、展開してください。
* </p>
* @param ix
* スクリーン上の座標
* @param iy
* スクリーン上の座標
* @param i_mat
* 平面の姿勢行列です。
* @param o_pos
* 結果を受け取るオブジェクトです。
* @return
* 計算に成功すると、trueを返します。
*/
public bool unProjectOnMatrix(double ix, double iy, NyARDoubleMatrix44 i_mat, NyARDoublePoint3d o_pos)
{
//交点をカメラ座標系で計算
unProjectOnCamera(ix, iy, i_mat, o_pos);
//座標系の変換
NyARDoubleMatrix44 m = new NyARDoubleMatrix44();
if (!m.inverse(i_mat))
{
return(false);
}
m.transform3d(o_pos, o_pos);
return(true);
}
/**
* この関数は、{@link NyARTransMatResult}の内容を、回転行列にセットします。
* @param i_prev_result
* セットする姿勢変換行列。
*/
public virtual void initRotByPrevResult(NyARDoubleMatrix44 i_prev_result)
{
this.m00 = i_prev_result.m00;
this.m01 = i_prev_result.m01;
this.m02 = i_prev_result.m02;
this.m10 = i_prev_result.m10;
this.m11 = i_prev_result.m11;
this.m12 = i_prev_result.m12;
this.m20 = i_prev_result.m20;
this.m21 = i_prev_result.m21;
this.m22 = i_prev_result.m22;
return;
}
/**
* この関数は、{@link NyARTransMatResult}の内容を、回転行列にセットします。
* @param i_prev_result
* セットする姿勢変換行列。
*/
public virtual void initRotByPrevResult(NyARDoubleMatrix44 i_prev_result)
{
this.m00 = i_prev_result.m00;
this.m01 = i_prev_result.m01;
this.m02 = i_prev_result.m02;
this.m10 = i_prev_result.m10;
this.m11 = i_prev_result.m11;
this.m12 = i_prev_result.m12;
this.m20 = i_prev_result.m20;
this.m21 = i_prev_result.m21;
this.m22 = i_prev_result.m22;
return;
}
public void setFromMatrix(NyARDoubleMatrix44 i_mat)
{
// 最大成分を検索
double elem0 = i_mat.m00 - i_mat.m11 - i_mat.m22 + 1.0;
double elem1 = -i_mat.m00 + i_mat.m11 - i_mat.m22 + 1.0;
double elem2 = -i_mat.m00 - i_mat.m11 + i_mat.m22 + 1.0;
double elem3 = i_mat.m00 + i_mat.m11 + i_mat.m22 + 1.0;
if (elem0 > elem1 && elem0 > elem2 && elem0 > elem3)
{
double v = Math.Sqrt(elem0) * 0.5;
double mult = 0.25 / v;
this.x = v;
this.y = ((i_mat.m10 + i_mat.m01) * mult);
this.z = ((i_mat.m02 + i_mat.m20) * mult);
this.w = ((i_mat.m21 - i_mat.m12) * mult);
}
else if (elem1 > elem2 && elem1 > elem3)
{
double v = Math.Sqrt(elem1) * 0.5;
double mult = 0.25 / v;
this.x = ((i_mat.m10 + i_mat.m01) * mult);
this.y = (v);
this.z = ((i_mat.m21 + i_mat.m12) * mult);
this.w = ((i_mat.m02 - i_mat.m20) * mult);
}
else if (elem2 > elem3)
{
double v = Math.Sqrt(elem2) * 0.5;
double mult = 0.25 / v;
this.x = ((i_mat.m02 + i_mat.m20) * mult);
this.y = ((i_mat.m21 + i_mat.m12) * mult);
this.z = (v);
this.w = ((i_mat.m10 - i_mat.m01) * mult);
}
else
{
double v = Math.Sqrt(elem3) * 0.5;
double mult = 0.25 / v;
this.x = ((i_mat.m21 - i_mat.m12) * mult);
this.y = ((i_mat.m02 - i_mat.m20) * mult);
this.z = ((i_mat.m10 - i_mat.m01) * mult);
this.w = v;
}
return;
}
public void Test()
{
//AR用カメラパラメタファイルをロード
NyARParam ap = NyARParam.createFromARParamFile(new StreamReader(camera_file));
ap.changeScreenSize(320, 240);
//AR用のパターンコードを読み出し
NyARCode code = NyARCode.createFromARPattFile(new StreamReader(code_file), 16, 16);
//試験イメージの読み出し(320x240 BGRAのRAWデータ)
StreamReader sr = new StreamReader(data_file);
BinaryReader bs = new BinaryReader(sr.BaseStream);
byte[] raw = bs.ReadBytes(320 * 240 * 4);
// NyARBitmapRaster ra = new NyARBitmapRaster(320, 240);
// Graphics g = Graphics.FromImage(ra.getBitmap());
// g.DrawImage(new Bitmap("../../../../../data/320x240ABGR.png"), 0, 0);
NyARRgbRaster ra = new NyARRgbRaster(320, 240,NyARBufferType.BYTE1D_B8G8R8X8_32,false);
ra.wrapBuffer(raw);
//１パターンのみを追跡するクラスを作成
NyARSingleDetectMarker ar = NyARSingleDetectMarker.createInstance(ap, code, 80.0,NyARSingleDetectMarker.PF_NYARTOOLKIT);
NyARDoubleMatrix44 result_mat = new NyARDoubleMatrix44();
ar.setContinueMode(false);
ar.detectMarkerLite(ra, 100);
ar.getTransmationMatrix(result_mat);
//マーカーを検出
Stopwatch sw = new Stopwatch();
sw.Start();
for (int i = 0; i < 1000; i++)
{
//変換行列を取得
ar.detectMarkerLite(ra, 100);
ar.getTransmationMatrix(result_mat);
}
Console.WriteLine(result_mat.m00 + "," + result_mat.m01 + ","+result_mat.m02+","+result_mat.m03);
Console.WriteLine(result_mat.m10 + "," + result_mat.m11 + ","+result_mat.m12+","+result_mat.m13);
Console.WriteLine(result_mat.m20 + "," + result_mat.m21 + ","+result_mat.m22+","+result_mat.m23);
Console.WriteLine(result_mat.m30 + "," + result_mat.m31 + ","+result_mat.m32+","+result_mat.m33);
sw.Stop();
Console.WriteLine(sw.ElapsedMilliseconds + "[ms]");
return;
}
/**
* この関数は、スクリーン上の点と原点を結ぶ直線と、任意姿勢の平面の交差点を、カメラの座標系で取得します。
* この座標は、カメラ座標系です。
* @param ix
* スクリーン上の座標
* @param iy
* スクリーン上の座標
* @param i_mat
* 平面の姿勢行列です。
* @param o_pos
* 結果を受け取るオブジェクトです。
*/
public void unProjectOnCamera(double ix, double iy, NyARDoubleMatrix44 i_mat, NyARDoublePoint3d o_pos)
{
//画面→撮像点
this.unProject(ix, iy, o_pos);
//撮像点→カメラ座標系
double nx = i_mat.m02;
double ny = i_mat.m12;
double nz = i_mat.m22;
double mx = i_mat.m03;
double my = i_mat.m13;
double mz = i_mat.m23;
double t = (nx * mx + ny * my + nz * mz) / (nx * o_pos.x + ny * o_pos.y + nz * o_pos.z);
o_pos.x = t * o_pos.x;
o_pos.y = t * o_pos.y;
o_pos.z = t * o_pos.z;
}
public void setFromMatrix(NyARDoubleMatrix44 i_mat)
{
// 最大成分を検索
double elem0 = i_mat.m00 - i_mat.m11 - i_mat.m22 + 1.0;
double elem1 = -i_mat.m00 + i_mat.m11 - i_mat.m22 + 1.0;
double elem2 = -i_mat.m00 - i_mat.m11 + i_mat.m22 + 1.0;
double elem3 = i_mat.m00 + i_mat.m11 + i_mat.m22 + 1.0;
if (elem0 > elem1 && elem0 > elem2 && elem0 > elem3)
{
double v = Math.Sqrt(elem0) * 0.5;
double mult = 0.25 / v;
this.x = v;
this.y = ((i_mat.m10 + i_mat.m01) * mult);
this.z = ((i_mat.m02 + i_mat.m20) * mult);
this.w = ((i_mat.m21 - i_mat.m12) * mult);
}
else if (elem1 > elem2 && elem1 > elem3)
{
double v = Math.Sqrt(elem1) * 0.5;
double mult = 0.25 / v;
this.x = ((i_mat.m10 + i_mat.m01) * mult);
this.y = (v);
this.z = ((i_mat.m21 + i_mat.m12) * mult);
this.w = ((i_mat.m02 - i_mat.m20) * mult);
}
else if (elem2 > elem3)
{
double v = Math.Sqrt(elem2) * 0.5;
double mult = 0.25 / v;
this.x = ((i_mat.m02 + i_mat.m20) * mult);
this.y = ((i_mat.m21 + i_mat.m12) * mult);
this.z = (v);
this.w = ((i_mat.m10 - i_mat.m01) * mult);
}
else
{
double v = Math.Sqrt(elem3) * 0.5;
double mult = 0.25 / v;
this.x = ((i_mat.m21 - i_mat.m12) * mult);
this.y = ((i_mat.m02 - i_mat.m20) * mult);
this.z = ((i_mat.m10 - i_mat.m01) * mult);
this.w = v;
}
return;
}
/**
* icpUpdateMat関数です。matXw2Xcへ値を出力します。
* @param matXw2Xc
* @param dS
* double[6]
* @return
*/
public void makeMat(NyARDoubleMatrix44 matXw2Xc)
{
IcpMat44 mat = this.__mat;
mat.setDeltaS(this);
double w0, w1, w2, w3;
w0 = matXw2Xc.m00;
w1 = matXw2Xc.m01;
w2 = matXw2Xc.m02;
w3 = matXw2Xc.m03;
//ji
matXw2Xc.m00 = w0 * mat.m00 + w1 * mat.m10 + w2 * mat.m20;
matXw2Xc.m01 = w0 * mat.m01 + w1 * mat.m11 + w2 * mat.m21;
matXw2Xc.m02 = w0 * mat.m02 + w1 * mat.m12 + w2 * mat.m22;
matXw2Xc.m03 = w0 * mat.m03 + w1 * mat.m13 + w2 * mat.m23 + w3;
w0 = matXw2Xc.m10;
w1 = matXw2Xc.m11;
w2 = matXw2Xc.m12;
w3 = matXw2Xc.m13;
matXw2Xc.m10 = w0 * mat.m00 + w1 * mat.m10 + w2 * mat.m20;
matXw2Xc.m11 = w0 * mat.m01 + w1 * mat.m11 + w2 * mat.m21;
matXw2Xc.m12 = w0 * mat.m02 + w1 * mat.m12 + w2 * mat.m22;
matXw2Xc.m13 = w0 * mat.m03 + w1 * mat.m13 + w2 * mat.m23 + w3;
w0 = matXw2Xc.m20;
w1 = matXw2Xc.m21;
w2 = matXw2Xc.m22;
w3 = matXw2Xc.m23;
matXw2Xc.m20 = w0 * mat.m00 + w1 * mat.m10 + w2 * mat.m20;
matXw2Xc.m21 = w0 * mat.m01 + w1 * mat.m11 + w2 * mat.m21;
matXw2Xc.m22 = w0 * mat.m02 + w1 * mat.m12 + w2 * mat.m22;
matXw2Xc.m23 = w0 * mat.m03 + w1 * mat.m13 + w2 * mat.m23 + w3;
matXw2Xc.m30 = 0;
matXw2Xc.m31 = 0;
matXw2Xc.m32 = 0;
matXw2Xc.m33 = 1;
return;
}
private static bool kpmUtilGetPose_binary(NyARParam i_cparam, FeaturePairStack matchData, NyARDoubleMatrix44 i_transmat, NyARTransMatResultParam i_resultparam)
{
NyARDoubleMatrix44 initMatXw2Xc = new NyARDoubleMatrix44();
// ARdouble err;
int i;
if (matchData.getLength() < 4)
{
return(false);
}
NyARDoublePoint2d[] sCoord = NyARDoublePoint2d.createArray(matchData.getLength());
NyARDoublePoint3d[] wCoord = NyARDoublePoint3d.createArray(matchData.getLength());
for (i = 0; i < matchData.getLength(); i++)
{
sCoord[i].x = matchData.getItem(i).query.x;
sCoord[i].y = matchData.getItem(i).query.y;
wCoord[i].x = matchData.getItem(i).ref_.pos3d.x;
wCoord[i].y = matchData.getItem(i).ref_.pos3d.y;
wCoord[i].z = 0.0;
}
NyARIcpPlane icp_planer = new NyARIcpPlane(i_cparam.getPerspectiveProjectionMatrix());
if (!icp_planer.icpGetInitXw2Xc_from_PlanarData(sCoord, wCoord, matchData.getLength(), initMatXw2Xc))
{
return(false);
}
/*
* printf("--- Init pose ---\n"); for( int j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) printf(" %8.3f",
* initMatXw2Xc[j][i]); printf("\n"); }
*/
NyARIcpPoint icp_point = new NyARIcpPoint(i_cparam.getPerspectiveProjectionMatrix());
icp_point.icpPoint(sCoord, wCoord, matchData.getLength(), initMatXw2Xc, i_transmat, i_resultparam);
if (i_resultparam.last_error > 10.0f)
{
return(false);
}
return(true);
}
/**
* アプリケーションフレームワークのハンドラ（マーカ出現）
*/
protected override void onEnterHandler(INyIdMarkerData i_code)
{
NyIdMarkerData_RawBit code = (NyIdMarkerData_RawBit)i_code;
if (code.length > 4)
{
//4バイト以上の時はint変換しない。
this.current_id = -1;//undefined_id
}
else
{
this.current_id = 0;
//最大4バイト繋げて１個のint値に変換
for (int i = 0; i < code.length; i++)
{
this.current_id = (this.current_id << 8) | code.packet[i];
}
}
this.transmat = null;
}
/**
* この関数は、ARToolKitのcheck_dir関数に相当します。
* 詳細は不明です。(ベクトルの開始/終了座標を指定して、ベクトルの方向を調整？)
* @param i_start_vertex
* 開始位置？
* @param i_end_vertex
* 終了位置？
* @throws NyARException
*/
public bool checkVectorByVertex(NyARDoublePoint2d i_start_vertex, NyARDoublePoint2d i_end_vertex)
{
double h;
NyARDoubleMatrix44 inv_cpara = this._inv_cpara;
//final double[] world = __checkVectorByVertex_world;// [2][3];
double world0 = inv_cpara.m00 * i_start_vertex.x * 10.0 + inv_cpara.m01 * i_start_vertex.y * 10.0 + inv_cpara.m02 * 10.0; // mat_a->m[0]*st[0]*10.0+
double world1 = inv_cpara.m10 * i_start_vertex.x * 10.0 + inv_cpara.m11 * i_start_vertex.y * 10.0 + inv_cpara.m12 * 10.0; // mat_a->m[3]*st[0]*10.0+
double world2 = inv_cpara.m20 * i_start_vertex.x * 10.0 + inv_cpara.m21 * i_start_vertex.y * 10.0 + inv_cpara.m22 * 10.0; // mat_a->m[6]*st[0]*10.0+
double world3 = world0 + this.v1;
double world4 = world1 + this.v2;
double world5 = world2 + this.v3;
// </Optimize>
NyARPerspectiveProjectionMatrix cmat = this._projection_mat_ref;
h = cmat.m20 * world0 + cmat.m21 * world1 + cmat.m22 * world2;
if (h == 0.0)
{
return(false);
}
double camera0 = (cmat.m00 * world0 + cmat.m01 * world1 + cmat.m02 * world2) / h;
double camera1 = (cmat.m10 * world0 + cmat.m11 * world1 + cmat.m12 * world2) / h;
//h = cpara[2 * 4 + 0] * world3 + cpara[2 * 4 + 1] * world4 + cpara[2 * 4 + 2] * world5;
h = cmat.m20 * world3 + cmat.m21 * world4 + cmat.m22 * world5;
if (h == 0.0)
{
return(false);
}
double camera2 = (cmat.m00 * world3 + cmat.m01 * world4 + cmat.m02 * world5) / h;
double camera3 = (cmat.m10 * world3 + cmat.m11 * world4 + cmat.m12 * world5) / h;
double v = (i_end_vertex.x - i_start_vertex.x) * (camera2 - camera0) + (i_end_vertex.y - i_start_vertex.y) * (camera3 - camera1);
if (v < 0)
{
this.v1 = -this.v1;
this.v2 = -this.v2;
this.v3 = -this.v3;
}
return(true);
}
/**
* この関数は、オブジェクトの内容をインスタンスにコピーします。
* @param i_value
* コピー元のオブジェクト
*/
public void setValue(NyARDoubleMatrix44 i_value)
{
this.m00 = i_value.m00;
this.m01 = i_value.m01;
this.m02 = i_value.m02;
this.m03 = i_value.m03;
this.m10 = i_value.m10;
this.m11 = i_value.m11;
this.m12 = i_value.m12;
this.m13 = i_value.m13;
this.m20 = i_value.m20;
this.m21 = i_value.m21;
this.m22 = i_value.m22;
this.m23 = i_value.m23;
this.m30 = i_value.m30;
this.m31 = i_value.m31;
this.m32 = i_value.m32;
this.m33 = i_value.m33;
return;
}
public void icpGetInitXw2XcSub(NyARDoubleMatrix44 rot,
NyARDoublePoint2d[] pos2d, NyARDoublePoint3d[] ppos3d, int num,
NyARDoubleMatrix44 conv)
{
NyARDoublePoint3d off = makeOffset(ppos3d, num, this.__off);
NyARDoubleMatrix33 matd = makeMatD(this._cparam, pos2d, num, this.__matd);
double[] mate = makeMatE(this._cparam, rot, pos2d, ppos3d, off, num, this.__mate);
conv.setValue(rot);
conv.m03 = matd.m00 * mate[0] + matd.m01 * mate[1] + matd.m02 * mate[2];
conv.m13 = matd.m10 * mate[0] + matd.m11 * mate[1] + matd.m12 * mate[2];
conv.m23 = matd.m20 * mate[0] + matd.m21 * mate[1] + matd.m22 * mate[2];
conv.m03 = conv.m00 * off.x + conv.m01 * off.y + conv.m02 * off.z + conv.m03;
conv.m13 = conv.m10 * off.x + conv.m11 * off.y + conv.m12 * off.z + conv.m13;
conv.m23 = conv.m20 * off.x + conv.m21 * off.y + conv.m22 * off.z + conv.m23;
return;
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 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);
}
/**
* この関数は、ARToolKitスタイルのProjectionMatrixから、 CameraFrustamを計算します。
* @param i_promat
* @param i_size
* スクリーンサイズを指定します。
* @param i_scale
* {@link #toCameraFrustumRH(NyARParam i_arparam,double i_scale,double i_near,double i_far,double[] o_gl_projection)}を参照。
* @param i_near
* {@link #toCameraFrustumRH(NyARParam i_arparam,double i_scale,double i_near,double i_far,double[] o_gl_projection)}を参照。
* @param i_far
* {@link #toCameraFrustumRH(NyARParam i_arparam,double i_scale,double i_near,double i_far,double[] o_gl_projection)}を参照。
* @param o_gl_projection
* {@link #toCameraFrustumRH(NyARParam i_arparam,double i_scale,double i_near,double i_far,double[] o_gl_projection)}を参照。
*/
public static void toCameraFrustumRH(NyARPerspectiveProjectionMatrix i_promat,NyARIntSize i_size,double i_scale,double i_near,double i_far,ref Matrix4x4 o_mat)
{
NyARDoubleMatrix44 m=new NyARDoubleMatrix44();
i_promat.makeCameraFrustumRH(i_size.w,i_size.h,i_near*i_scale,i_far*i_scale,m);
o_mat.m00=(float)m.m00;
o_mat.m01=(float)m.m01;
o_mat.m02=(float)m.m02;
o_mat.m03=(float)m.m03;
o_mat.m10=(float)m.m10;
o_mat.m11=(float)m.m11;
o_mat.m12=(float)m.m12;
o_mat.m13=(float)m.m13;
o_mat.m20=(float)m.m20;
o_mat.m21=(float)m.m21;
o_mat.m22=(float)m.m22;
o_mat.m23=(float)m.m23;
o_mat.m30=(float)m.m30;
o_mat.m31=(float)m.m31;
o_mat.m32=(float)m.m32;
o_mat.m33=(float)m.m33;
return;
}
/**
* icpGetJ_U_Xc
* @param J_U_Xc
* @param matXc2U
* @param cameraCoord
* @return
*/
public bool setXc2UXc(NyARDoubleMatrix44 matXc2U, double Xcx, double Xcy, double Xcz)
{
double w1, w2, w3, w3_w3;
w1 = matXc2U.m00 * Xcx + matXc2U.m01 * Xcy + matXc2U.m02 * Xcz + matXc2U.m03;
w2 = matXc2U.m10 * Xcx + matXc2U.m11 * Xcy + matXc2U.m12 * Xcz + matXc2U.m13;
w3 = matXc2U.m20 * Xcx + matXc2U.m21 * Xcy + matXc2U.m22 * Xcz + matXc2U.m23;
if (w3 == 0.0)
{
return(false);
}
w3_w3 = w3 * w3;
this.m00 = (matXc2U.m00 * w3 - matXc2U.m20 * w1) / w3_w3;
this.m01 = (matXc2U.m01 * w3 - matXc2U.m21 * w1) / w3_w3;
this.m02 = (matXc2U.m02 * w3 - matXc2U.m22 * w1) / w3_w3;
this.m10 = (matXc2U.m10 * w3 - matXc2U.m20 * w2) / w3_w3;
this.m11 = (matXc2U.m11 * w3 - matXc2U.m21 * w2) / w3_w3;
this.m12 = (matXc2U.m12 * w3 - matXc2U.m22 * w2) / w3_w3;
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;
}
/**
* この関数は、スクリーン上の点と原点を結ぶ直線と、任意姿勢の平面の交差点を、カメラの座標系で取得します。
* この座標は、カメラ座標系です。
* @param ix
* スクリーン上の座標
* @param iy
* スクリーン上の座標
* @param i_mat
* 平面の姿勢行列です。
* @param o_pos
* 結果を受け取るオブジェクトです。
*/
public void unProjectOnCamera(double ix, double iy, NyARDoubleMatrix44 i_mat, NyARDoublePoint3d o_pos)
{
//画面→撮像点
this.unProject(ix, iy, o_pos);
//撮像点→カメラ座標系
double nx = i_mat.m02;
double ny = i_mat.m12;
double nz = i_mat.m22;
double mx = i_mat.m03;
double my = i_mat.m13;
double mz = i_mat.m23;
double t = (nx * mx + ny * my + nz * mz) / (nx * o_pos.x + ny * o_pos.y + nz * o_pos.z);
o_pos.x = t * o_pos.x;
o_pos.y = t * o_pos.y;
o_pos.z = t * o_pos.z;
}
/** * 自己コールバック関数です。 * 継承したクラスで、マーカ更新時の処理を実装してください。 * 引数の値の有効期間は、関数が終了するまでです。 * @param i_square * 現在のマーカ検出位置です。 * @param o_result * 現在の姿勢変換行列です。 */ protected abstract void onUpdateHandler(NyARSquare i_square, NyARDoubleMatrix44 o_result);
public override bool icpPoint(NyARDoublePoint2d[] screenCoord,
NyARDoublePoint3d[] worldCoord, int num,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc, NyARTransMatResultParam o_result_param)
{
Debug.Assert(num >= 4);
double err0 = 0, err1;
NyARIcpUtils.U u = this.__u;
NyARIcpUtils.DeltaS dS = this.__dS;
//ワークオブジェクトのリセット
if (this.__jus.getArraySize() < num)
{
this.__jus = new NyARIcpUtils.JusStack(num);
this.__E = new double[num];
this.__E2 = new double[num];
this.__du = NyARDoublePoint2d.createArray(num);
}
NyARIcpUtils.JusStack jus = this.__jus;
double[] E = this.__E;
double[] E2 = this.__E2;
NyARDoublePoint2d[] du = this.__du;
NyARDoubleMatrix44 matXw2U = this.__matXw2U;
int inlierNum = (int)(num * this.getInlierProbability()) - 1;
if (inlierNum < 3)
{
inlierNum = 3;
}
o_matxw2xc.setValue(initMatXw2Xc);
for (int i = 0;; i++) {
matXw2U.mul(this._ref_matXc2U, o_matxw2xc);
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;
du[j].x = dx;
du[j].y = dy;
E[j] = E2[j] = dx * dx + dy * dy;
}
Array.Sort(E2, 0, num);// qsort(E2, data->num, sizeof(ARdouble), compE);
double K2 = E2[inlierNum] * K2_FACTOR;
if (K2 < 16.0)
{
K2 = 16.0;
}
err1 = 0.0;
for (int j = 0; j < num; j++)
{
if (E2[j] > K2)
{
err1 += K2 / 6.0;
}
else
{
err1 += K2 / 6.0 * (1.0 - (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2));
}
}
err1 /= num;
if (err1 < this.breakLoopErrorThresh)
{
break;
}
if (i > 0 && err1 < this.breakLoopErrorThresh2 && err1 / err0 > this.breakLoopErrorRatioThresh)
{
break;
}
if (i == this._maxLoop)
{
break;
}
err0 = err1;
jus.clear();
for (int j = 0; j < num; j++)
{
if (E[j] <= K2)
{
double W = (1.0 - E[j] / K2) * (1.0 - E[j] / K2);
if(!jus.push(this._ref_matXc2U,o_matxw2xc, worldCoord[j],du[j],W))
{
return false;
}
}
}
if (jus.getLength() < 3)
{
return false;
}
if (!dS.setJusArray(jus))
{
return false;
}
dS.makeMat(o_matxw2xc);
}
if(o_result_param!=null){
o_result_param.last_error=err1;
}
return true;
}
/**
* この関数は、カメラパラメータから右手系の視錐台を作ります。
* <p>注意 -
* この処理は低速です。繰り返しの使用はできるだけ避けてください。
* </p>
* @param i_dist_min
* 視錐台のnear point(mm指定)
* @param i_dist_max
* 視錐台のfar point(mm指定)
* @param o_frustum
* 視錐台を受け取る配列。
* @see NyARPerspectiveProjectionMatrix#makeCameraFrustumRH
*/
public void makeCameraFrustumRH(double i_dist_min, double i_dist_max, NyARDoubleMatrix44 o_frustum)
{
this._projection_matrix.makeCameraFrustumRH(this._screen_size.w, this._screen_size.h, i_dist_min, i_dist_max, o_frustum);
return;
}
/**
* この関数は、行列同士の掛け算をして、インスタンスに格納します。
* i_mat_lとi_mat_rには、thisを指定しないでください。
* @param i_mat_l
* 左成分の行列
* @param i_mat_r
* 右成分の行列
*/
public void mul(NyARDoubleMatrix44 i_mat_l, NyARDoubleMatrix44 i_mat_r)
{
Debug.Assert(this != i_mat_l);
Debug.Assert(this != i_mat_r);
this.m00 = i_mat_l.m00 * i_mat_r.m00 + i_mat_l.m01 * i_mat_r.m10 + i_mat_l.m02 * i_mat_r.m20 + i_mat_l.m03 * i_mat_r.m30;
this.m01 = i_mat_l.m00 * i_mat_r.m01 + i_mat_l.m01 * i_mat_r.m11 + i_mat_l.m02 * i_mat_r.m21 + i_mat_l.m03 * i_mat_r.m31;
this.m02 = i_mat_l.m00 * i_mat_r.m02 + i_mat_l.m01 * i_mat_r.m12 + i_mat_l.m02 * i_mat_r.m22 + i_mat_l.m03 * i_mat_r.m32;
this.m03 = i_mat_l.m00 * i_mat_r.m03 + i_mat_l.m01 * i_mat_r.m13 + i_mat_l.m02 * i_mat_r.m23 + i_mat_l.m03 * i_mat_r.m33;
this.m10 = i_mat_l.m10 * i_mat_r.m00 + i_mat_l.m11 * i_mat_r.m10 + i_mat_l.m12 * i_mat_r.m20 + i_mat_l.m13 * i_mat_r.m30;
this.m11 = i_mat_l.m10 * i_mat_r.m01 + i_mat_l.m11 * i_mat_r.m11 + i_mat_l.m12 * i_mat_r.m21 + i_mat_l.m13 * i_mat_r.m31;
this.m12 = i_mat_l.m10 * i_mat_r.m02 + i_mat_l.m11 * i_mat_r.m12 + i_mat_l.m12 * i_mat_r.m22 + i_mat_l.m13 * i_mat_r.m32;
this.m13 = i_mat_l.m10 * i_mat_r.m03 + i_mat_l.m11 * i_mat_r.m13 + i_mat_l.m12 * i_mat_r.m23 + i_mat_l.m13 * i_mat_r.m33;
this.m20 = i_mat_l.m20 * i_mat_r.m00 + i_mat_l.m21 * i_mat_r.m10 + i_mat_l.m22 * i_mat_r.m20 + i_mat_l.m23 * i_mat_r.m30;
this.m21 = i_mat_l.m20 * i_mat_r.m01 + i_mat_l.m21 * i_mat_r.m11 + i_mat_l.m22 * i_mat_r.m21 + i_mat_l.m23 * i_mat_r.m31;
this.m22 = i_mat_l.m20 * i_mat_r.m02 + i_mat_l.m21 * i_mat_r.m12 + i_mat_l.m22 * i_mat_r.m22 + i_mat_l.m23 * i_mat_r.m32;
this.m23 = i_mat_l.m20 * i_mat_r.m03 + i_mat_l.m21 * i_mat_r.m13 + i_mat_l.m22 * i_mat_r.m23 + i_mat_l.m23 * i_mat_r.m33;
this.m30 = i_mat_l.m30 * i_mat_r.m00 + i_mat_l.m31 * i_mat_r.m10 + i_mat_l.m32 * i_mat_r.m20 + i_mat_l.m33 * i_mat_r.m30;
this.m31 = i_mat_l.m30 * i_mat_r.m01 + i_mat_l.m31 * i_mat_r.m11 + i_mat_l.m32 * i_mat_r.m21 + i_mat_l.m33 * i_mat_r.m31;
this.m32 = i_mat_l.m30 * i_mat_r.m02 + i_mat_l.m31 * i_mat_r.m12 + i_mat_l.m32 * i_mat_r.m22 + i_mat_l.m33 * i_mat_r.m32;
this.m33 = i_mat_l.m30 * i_mat_r.m03 + i_mat_l.m31 * i_mat_r.m13 + i_mat_l.m32 * i_mat_r.m23 + i_mat_l.m33 * i_mat_r.m33;
return;
}
/**
*
* @param iw_rotmat
* @param iw_transvec
* @param i_solver
* @param i_offset_3d
* @param i_2d_vertex
* @param i_err_threshold
* @param o_result
* @return
* @
*/
private double optimize(NyARRotMatrix iw_rotmat,NyARDoublePoint3d iw_transvec,INyARTransportVectorSolver i_solver,NyARDoublePoint3d[] i_offset_3d,NyARDoublePoint2d[] i_2d_vertex,double i_err_threshold,NyARDoubleMatrix44 o_result)
{
//System.out.println("START");
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
//初期のエラー値を計算
double min_err = errRate(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4, vertex_3d);
o_result.setValue(iw_rotmat, iw_transvec);
for (int i = 0; i < 5; i++)
{
//変換行列の最適化
this._mat_optimize.modifyMatrix(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4);
double err = errRate(iw_rotmat, iw_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, iw_transvec);
o_result.setValue(iw_rotmat, iw_transvec);
min_err = err;
}
//System.out.println("END");
return min_err;
}
/**
* カメラ座標系の4頂点でかこまれる領域から、RGB画像をo_rasterに取得します。
* @param i_vertex
* @param i_matrix
* i_vertexに適応する変換行列。
* ターゲットの姿勢行列を指定すると、ターゲット座標系になります。不要ならばnullを設定してください。
* @param i_resolution
* @param o_raster
* @return
* @throws NyARException
*/
public bool getRgbPatt3d(NyARRealitySource i_src, NyARDoublePoint3d[] i_vertex, NyARDoubleMatrix44 i_matrix, int i_resolution, INyARRgbRaster o_raster)
{
NyARDoublePoint2d[] vx=NyARDoublePoint2d.createArray(4);
if(i_matrix!=null){
//姿勢変換してから射影変換
NyARDoublePoint3d v3d=new NyARDoublePoint3d();
for(int i=3;i>=0;i--){
i_matrix.transform3d(i_vertex[i],v3d);
this._ref_prjmat.project(v3d,vx[i]);
}
}else{
//射影変換のみ
for(int i=3;i>=0;i--){
this._ref_prjmat.project(i_vertex[i],vx[i]);
}
}
//パターンの取得
return i_src.refPerspectiveRasterReader().copyPatt(vx, 0, 0, i_resolution, o_raster);
}
/**
* この関数は、逆行列を計算して、インスタンスにセットします。
* @param i_src
* 逆行列を計算するオブジェクト。thisを指定できます。
* @return
* 逆行列を得られると、trueを返します。
*/
public bool inverse(NyARDoubleMatrix44 i_src)
{
double a11, a12, a13, a14, a21, a22, a23, a24, a31, a32, a33, a34, a41, a42, a43, a44;
double b11, b12, b13, b14, b21, b22, b23, b24, b31, b32, b33, b34, b41, b42, b43, b44;
double t1, t2, t3, t4, t5, t6;
a11 = i_src.m00; a12 = i_src.m01; a13 = i_src.m02; a14 = i_src.m03;
a21 = i_src.m10; a22 = i_src.m11; a23 = i_src.m12; a24 = i_src.m13;
a31 = i_src.m20; a32 = i_src.m21; a33 = i_src.m22; a34 = i_src.m23;
a41 = i_src.m30; a42 = i_src.m31; a43 = i_src.m32; a44 = i_src.m33;
t1 = a33 * a44 - a34 * a43;
t2 = a34 * a42 - a32 * a44;
t3 = a32 * a43 - a33 * a42;
t4 = a34 * a41 - a31 * a44;
t5 = a31 * a43 - a33 * a41;
t6 = a31 * a42 - a32 * a41;
b11 = a22 * t1 + a23 * t2 + a24 * t3;
b21 = -(a23 * t4 + a24 * t5 + a21 * t1);
b31 = a24 * t6 - a21 * t2 + a22 * t4;
b41 = -(a21 * t3 - a22 * t5 + a23 * t6);
t1 = a43 * a14 - a44 * a13;
t2 = a44 * a12 - a42 * a14;
t3 = a42 * a13 - a43 * a12;
t4 = a44 * a11 - a41 * a14;
t5 = a41 * a13 - a43 * a11;
t6 = a41 * a12 - a42 * a11;
b12 = -(a32 * t1 + a33 * t2 + a34 * t3);
b22 = a33 * t4 + a34 * t5 + a31 * t1;
b32 = -(a34 * t6 - a31 * t2 + a32 * t4);
b42 = a31 * t3 - a32 * t5 + a33 * t6;
t1 = a13 * a24 - a14 * a23;
t2 = a14 * a22 - a12 * a24;
t3 = a12 * a23 - a13 * a22;
t4 = a14 * a21 - a11 * a24;
t5 = a11 * a23 - a13 * a21;
t6 = a11 * a22 - a12 * a21;
b13 = a42 * t1 + a43 * t2 + a44 * t3;
b23 = -(a43 * t4 + a44 * t5 + a41 * t1);
b33 = a44 * t6 - a41 * t2 + a42 * t4;
b43 = -(a41 * t3 - a42 * t5 + a43 * t6);
t1 = a23 * a34 - a24 * a33;
t2 = a24 * a32 - a22 * a34;
t3 = a22 * a33 - a23 * a32;
t4 = a24 * a31 - a21 * a34;
t5 = a21 * a33 - a23 * a31;
t6 = a21 * a32 - a22 * a31;
b14 = -(a12 * t1 + a13 * t2 + a14 * t3);
b24 = a13 * t4 + a14 * t5 + a11 * t1;
b34 = -(a14 * t6 - a11 * t2 + a12 * t4);
b44 = a11 * t3 - a12 * t5 + a13 * t6;
double det_1 = (a11 * b11 + a21 * b12 + a31 * b13 + a41 * b14);
if (det_1 == 0)
{
return false;
}
det_1 = 1 / det_1;
this.m00 = b11 * det_1;
this.m01 = b12 * det_1;
this.m02 = b13 * det_1;
this.m03 = b14 * det_1;
this.m10 = b21 * det_1;
this.m11 = b22 * det_1;
this.m12 = b23 * det_1;
this.m13 = b24 * det_1;
this.m20 = b31 * det_1;
this.m21 = b32 * det_1;
this.m22 = b33 * det_1;
this.m23 = b34 * det_1;
this.m30 = b41 * det_1;
this.m31 = b42 * det_1;
this.m32 = b43 * det_1;
this.m33 = b44 * det_1;
return true;
}
/**
* この関数は、検出したマーカーの変換行列を計算して、o_resultへ値を返します。
* 直前に実行した{@link #detectMarkerLite}が成功していないと使えません。
* @param o_result
* 変換行列を受け取るオブジェクト。
* @
*/
public void getTransmat(NyARDoubleMatrix44 o_result)
{
// 一番一致したマーカーの位置とかその辺を計算
if (this._is_continue){
//履歴が使えそうか判定
if(this._last_input_mat==o_result){
if(this._transmat.transMatContinue(this._square,this._offset,o_result, this._last_result_param.last_error,o_result, this._last_result_param)){
return;
}
}
}
//履歴使えないor継続認識失敗
this._transmat.transMat(this._square,this._offset,o_result,this._last_result_param);
this._last_input_mat=o_result;
return;
}
/**
* アプリケーションフレームワークのハンドラ（マーカ消滅）
*/
protected override void onLeaveHandler()
{
this.current_id = -1;
this.transmat = null;
return;
}
/* カメラのプロジェクションMatrix(RH)を返します。
* このMatrixはMicrosoft.DirectX.Direct3d.Device.Transform.Projectionに設定できます。
*/
public static void toCameraFrustumRH(NyARPerspectiveProjectionMatrix i_promat, NyARIntSize i_size, double i_scale, double i_near, double i_far, ref Matrix o_d3d_projection)
{
NyARDoubleMatrix44 m = new NyARDoubleMatrix44();
i_promat.makeCameraFrustumRH(i_size.w, i_size.h, i_near * i_scale, i_far * i_scale, m);
NyARD3dUtil.mat44ToD3dMatrixT(m, ref o_d3d_projection);
return;
}
/**
* アプリケーションフレームワークのハンドラ（マーカ更新）
*/
protected override void onUpdateHandler(NyARSquare i_square, NyARDoubleMatrix44 result)
{
NyARD3dUtil.toD3dCameraView(result, 1f, ref this.transmat);
}
/**
* Direct3d形式のカメラビュー行列に変換します。
*/
public static void toD3dCameraView(NyARDoubleMatrix44 i_ny_result, float i_scale, ref Matrix o_result)
{
Matrix m;
m.M11 = (float)i_ny_result.m00;
m.M12 = -(float)i_ny_result.m10;
m.M13 = -(float)i_ny_result.m20;
m.M14 = 0;
m.M21 = (float)i_ny_result.m01;
m.M22 = -(float)i_ny_result.m11;
m.M23 = -(float)i_ny_result.m21;
m.M24 = 0;
m.M31 = (float)i_ny_result.m02;
m.M32 = -(float)i_ny_result.m12;
m.M33 = -(float)i_ny_result.m22;
m.M34 = 0;
float scale =(1 / i_scale);
m.M41 = (float)i_ny_result.m03 * scale;
m.M42 = -(float)i_ny_result.m13 * scale;
m.M43 = -(float)i_ny_result.m23 * scale;
m.M44 = 1;
o_result = m;
return;
}
public static void mat44ToD3dMatrixT(NyARDoubleMatrix44 i_src, ref Matrix o_dst)
{
o_dst.M11 = (float)i_src.m00;
o_dst.M21 = (float)i_src.m01;
o_dst.M31 = (float)i_src.m02;
o_dst.M41 = (float)i_src.m03;
o_dst.M12 = (float)i_src.m10;
o_dst.M22 = (float)i_src.m11;
o_dst.M32 = (float)i_src.m12;
o_dst.M42 = (float)i_src.m13;
o_dst.M13 = (float)i_src.m20;
o_dst.M23 = (float)i_src.m21;
o_dst.M33 = (float)i_src.m22;
o_dst.M43 = (float)i_src.m23;
o_dst.M14 = (float)i_src.m30;
o_dst.M24 = (float)i_src.m31;
o_dst.M34 = (float)i_src.m32;
o_dst.M44 = (float)i_src.m33;
return;
}
/**
* この関数は、視錐台行列をインスタンスにセットします。
* @param i_projection
* ARToolKitスタイルの射影変換行列
* @param i_width
* スクリーンサイズです。
* @param i_height
* スクリーンサイズです。
*/
public void setValue(NyARDoubleMatrix44 i_projection_mat, int i_width, int i_height)
{
this._frustum_rh.setValue(i_projection_mat);
this._inv_frustum_rh.inverse(this._frustum_rh);
this._screen_size.setValue(i_width, i_height);
}
/**
* この関数は、オブジェクトの内容をインスタンスにコピーします。
* @param i_value
* コピー元のオブジェクト
*/
public void setValue(NyARDoubleMatrix44 i_value)
{
this.m00 = i_value.m00;
this.m01 = i_value.m01;
this.m02 = i_value.m02;
this.m03 = i_value.m03;
this.m10 = i_value.m10;
this.m11 = i_value.m11;
this.m12 = i_value.m12;
this.m13 = i_value.m13;
this.m20 = i_value.m20;
this.m21 = i_value.m21;
this.m22 = i_value.m22;
this.m23 = i_value.m23;
this.m30 = i_value.m30;
this.m31 = i_value.m31;
this.m32 = i_value.m32;
this.m33 = i_value.m33;
return;
}
/**
* @deprecated
* {@link #getTransmat}
*/
public void getTransmationMatrix(NyARDoubleMatrix44 o_result)
{
this.getTransmat(o_result);
return;
}
/**
* ターゲット座標系の4頂点でかこまれる領域を射影した平面から、RGB画像をo_rasterに取得します。
* @param i_vertex
* ターゲットのオフセットを基準値とした、頂点座標。要素数は4であること。(mm単位)
* @param i_matrix
* i_vertexに適応する変換行列。
* ターゲットの姿勢行列を指定すると、ターゲット座標系になります。不要ならばnullを設定してください。
* @param i_resolution
* 1ピクセルあたりのサンプリング値(n^2表現)
* @param o_raster
* 出力ラスタ
* @return
* @throws NyARException
* <p>メモ:この関数にはnewが残ってるので注意</p>
*/
public bool getRgbPatt3d(NyARRealitySource i_src, NyARDoublePoint3d[] i_vertex, NyARDoubleMatrix44 i_matrix, int i_resolution, INyARRgbRaster o_raster)
{
Debug.Assert(this._target_type==RT_KNOWN);
NyARDoublePoint2d[] da4=this._ref_pool._wk_da2_4;
NyARDoublePoint3d v3d=new NyARDoublePoint3d();
if(i_matrix!=null){
//姿勢変換してから射影変換
for(int i=3;i>=0;i--){
//姿勢を変更して射影変換
i_matrix.transform3d(i_vertex[i],v3d);
this._transform_matrix.transform3d(v3d,v3d);
this._ref_pool._ref_prj_mat.project(v3d,da4[i]);
}
}else{
//射影変換のみ
for(int i=3;i>=0;i--){
//姿勢を変更して射影変換
this._transform_matrix.transform3d(i_vertex[i],v3d);
this._ref_pool._ref_prj_mat.project(v3d,da4[i]);
}
}
//パターンの取得
return i_src.refPerspectiveRasterReader().copyPatt(da4,0,0,i_resolution, o_raster);
}
/**
* 画面上の点と原点を結ぶ直線と任意姿勢の平面の交差点を、平面の座標系で取得します。
* ARToolKitの本P175周辺の実装と同じです。
* <p>
* このAPIは繰り返し使用には最適化されていません。同一なi_matに繰り返しアクセスするときは、展開してください。
* </p>
* @param ix
* スクリーン上の座標
* @param iy
* スクリーン上の座標
* @param i_mat
* 平面の姿勢行列です。
* @param o_pos
* 結果を受け取るオブジェクトです。
* @return
* 計算に成功すると、trueを返します。
*/
public bool unProjectOnMatrix(double ix, double iy, NyARDoubleMatrix44 i_mat, NyARDoublePoint3d o_pos)
{
//交点をカメラ座標系で計算
unProjectOnCamera(ix, iy, i_mat, o_pos);
//座標系の変換
NyARDoubleMatrix44 m = new NyARDoubleMatrix44();
if (!m.inverse(i_mat))
{
return false;
}
m.transform3d(o_pos, o_pos);
return true;
}
/**
* 右手系の視錐台を作ります。
* この視錐台は、ARToolKitのarglCameraViewRHの作る視錐台と同じです。
* @param i_screen_width
* スクリーンサイズを指定します。
* @param i_screen_height
* スクリーンサイズを指定します。
* @param i_dist_min
* near pointを指定します(mm単位)
* @param i_dist_max
* far pointを指定します(mm単位)
* @param o_frustum
* 視錐台の格納先オブジェクトを指定します。
*/
public void makeCameraFrustumRH(double i_screen_width, double i_screen_height, double i_dist_min, double i_dist_max, NyARDoubleMatrix44 o_frustum)
{
NyARMat trans_mat = new NyARMat(3, 4);
NyARMat icpara_mat = new NyARMat(3, 4);
double[][] p = ArrayUtils.newDouble2dArray(3, 3);
int i;
this.decompMat(icpara_mat, trans_mat);
double[][] icpara = icpara_mat.getArray();
double[][] trans = trans_mat.getArray();
for (i = 0; i < 4; i++)
{
icpara[1][i] = (i_screen_height - 1) * (icpara[2][i]) - icpara[1][i];
}
p[0][0] = icpara[0][0] / icpara[2][2];
p[0][1] = icpara[0][1] / icpara[2][2];
p[0][2] = icpara[0][2] / icpara[2][2];
p[1][0] = icpara[1][0] / icpara[2][2];
p[1][1] = icpara[1][1] / icpara[2][2];
p[1][2] = icpara[1][2] / icpara[2][2];
p[2][0] = icpara[2][0] / icpara[2][2];
p[2][1] = icpara[2][1] / icpara[2][2];
p[2][2] = icpara[2][2] / icpara[2][2];
double q00, q01, q02, q03, q10, q11, q12, q13, q20, q21, q22, q23, q30, q31, q32, q33;
//視錐台への変換
q00 = (2.0 * p[0][0] / (i_screen_width - 1));
q01 = (2.0 * p[0][1] / (i_screen_width - 1));
q02 = -((2.0 * p[0][2] / (i_screen_width - 1)) - 1.0);
q03 = 0.0;
o_frustum.m00 = q00 * trans[0][0] + q01 * trans[1][0] + q02 * trans[2][0];
o_frustum.m01 = q00 * trans[0][1] + q01 * trans[1][1] + q02 * trans[2][1];
o_frustum.m02 = q00 * trans[0][2] + q01 * trans[1][2] + q02 * trans[2][2];
o_frustum.m03 = q00 * trans[0][3] + q01 * trans[1][3] + q02 * trans[2][3] + q03;
q10 = 0.0;
q11 = -(2.0 * p[1][1] / (i_screen_height - 1));
q12 = -((2.0 * p[1][2] / (i_screen_height - 1)) - 1.0);
q13 = 0.0;
o_frustum.m10 = q10 * trans[0][0] + q11 * trans[1][0] + q12 * trans[2][0];
o_frustum.m11 = q10 * trans[0][1] + q11 * trans[1][1] + q12 * trans[2][1];
o_frustum.m12 = q10 * trans[0][2] + q11 * trans[1][2] + q12 * trans[2][2];
o_frustum.m13 = q10 * trans[0][3] + q11 * trans[1][3] + q12 * trans[2][3] + q13;
q20 = 0.0;
q21 = 0.0;
q22 = (i_dist_max + i_dist_min) / (i_dist_min - i_dist_max);
q23 = 2.0 * i_dist_max * i_dist_min / (i_dist_min - i_dist_max);
o_frustum.m20 = q20 * trans[0][0] + q21 * trans[1][0] + q22 * trans[2][0];
o_frustum.m21 = q20 * trans[0][1] + q21 * trans[1][1] + q22 * trans[2][1];
o_frustum.m22 = q20 * trans[0][2] + q21 * trans[1][2] + q22 * trans[2][2];
o_frustum.m23 = q20 * trans[0][3] + q21 * trans[1][3] + q22 * trans[2][3] + q23;
q30 = 0.0;
q31 = 0.0;
q32 = -1.0;
q33 = 0.0;
o_frustum.m30 = q30 * trans[0][0] + q31 * trans[1][0] + q32 * trans[2][0];
o_frustum.m31 = q30 * trans[0][1] + q31 * trans[1][1] + q32 * trans[2][1];
o_frustum.m32 = q30 * trans[0][2] + q31 * trans[1][2] + q32 * trans[2][2];
o_frustum.m33 = q30 * trans[0][3] + q31 * trans[1][3] + q32 * trans[2][3] + q33;
return;
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 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;
}
/**
* アプリケーションフレームワークのハンドラ（マーカ更新）
*/
protected override void onUpdateHandler(NyARSquare i_square, NyARDoubleMatrix44 result)
{
this.transmat = result;
}
