NyARToolKit

2008.05.24:0.39.2
・nyar_NyARTransMat_O2_transMatCont関数を追加
　NyARToolKit 1.2.0ベースのコードに修正

Index: nyar_NyARTransMat.h
===================================================================
--- nyar_NyARTransMat.h (revision 201)
+++ nyar_NyARTransMat.h (revision 202)
@@ -65,7 +65,7 @@
nyar_NyARTransMat_O2_t* i_inst);

/*!
- nyar_NyARTransMat_O2インスタンスのデストラクタです。
+ arGetTransMat互換の関数です。
\param i_inst
nyar_NyARTransMat_O2インスタンスを渡します。
このインスタンスは、nyar_NyARTransMat_O2_create関数で生成したものである必要があります。
@@ -87,6 +87,32 @@
const double i_width,
double o_conv[3][4]);

+/*!
+ arGetTransMatCont互換の関数です。
+ \param i_inst
+ nyar_NyARTransMat_O2インスタンスを渡します。
+ このインスタンスは、nyar_NyARTransMat_O2_create関数で生成したものである必要があります。
+ \param i_marker_info
+ arGetTransMatと同じです。
+ \param i_prev_conv
+ arGetTransMatと同じです。
+ \param i_center
+ arGetTransMatと同じです。
+ \param i_width
+ arGetTransMatと同じです。
+ \param o_conv
+ arGetTransMatと同じです。
+ \return
+ arGetTransMatと同じです。
+*/
+double nyar_NyARTransMat_O2_transMatCont(
+ nyar_NyARTransMat_O2_t* i_inst,
+ const ARMarkerInfo* i_marker_info,
+ const double i_prev_conv[3][4],
+ const double i_center[2],
+ const double i_width,
+ double o_conv[3][4]);
+
#ifdef __cplusplus
}
#endif
Index: nyar_NyARTransMat.c
===================================================================
--- nyar_NyARTransMat.c (revision 201)
+++ nyar_NyARTransMat.c (revision 202)
@@ -36,9 +36,9 @@
#include <math.h>
#include <assert.h>
#include <AR/nyar_NyARTransMat.h>
+#include <string.h>


-
#define nyar_FALSE (0!=0)
#define nyar_TRUE (0==0)

@@ -86,11 +86,11 @@
}

static int check_dir(
- nyar_NyARTransRot_O3_t* i_inst,
- double dir[],
- const double st[],
- const double ed[],
- const double cpara[])
+ nyar_NyARTransRot_O3_t* i_inst,
+ double dir[],
+ const double st[],
+ const double ed[],
+ const double cpara[])
{
double h;
int i, j;
@@ -141,7 +141,7 @@

/*int check_rotation( double rot[2][3] )*/
static int check_rotation(
- double rot[2][3])
+ double rot[2][3])
{
double v1[3],v2[3],v3[3];
double ca, cb, k1, k2, k3, k4;
@@ -379,7 +379,7 @@
const double a,
const double b,
const double c,
- double o_rot[])
+ double o_rot[9])
{
double sina, sinb, sinc;
double cosa, cosb, cosc;
@@ -411,7 +411,7 @@

static int nyar_NyARTransRot_O3_arGetAngle(
nyar_NyARTransRot_O3_t* i_inst,
- double o_abc[])
+ double o_abc[3])
{
double a, b, c,tmp;
double sina, cosa, sinb, cosb, sinc, cosc;
@@ -521,8 +521,8 @@
}

static int nyar_NyARTransRot_O3_arGetInitRot(
- nyar_NyARTransRot_O3_t* i_inst,
- const ARMarkerInfo* marker_info)
+ nyar_NyARTransRot_O3_t* i_inst,
+ const ARMarkerInfo* marker_info)
{
const double* cpara=&(i_inst->cparam->mat[0][0]);
double wdir[3][3];
@@ -591,8 +591,8 @@
static double nyar_NyARTransRot_O3_modifyMatrix(
nyar_NyARTransRot_O3_t* i_inst,
const double trans[],
- const double* vertex,//[][3],
- const double* pos2d)//[][2])
+ const double vertex[][3],
+ const double pos2d[][2])
{
double factor;
double a2, b2, c2;
@@ -674,18 +674,18 @@
combo12 = cpara[4] * CASB+ cpara[5] * SASB+ cpara[6] * wcos;
combo22 = cpara[8] * CASB+ cpara[9] * SASB+ cpara[10] * wcos;

- combo02_2 =combo02 * vertex[0*3+2] + combo03;
- combo02_5 =combo02 * vertex[1*3+2] + combo03;
- combo02_8 =combo02 * vertex[2*3+2] + combo03;
- combo02_11=combo02 * vertex[3*3+2] + combo03;
- combo12_2 =combo12 * vertex[0*3+2] + combo13;
- combo12_5 =combo12 * vertex[1*3+2] + combo13;
- combo12_8 =combo12 * vertex[2*3+2] + combo13;
- combo12_11=combo12 * vertex[3*3+2] + combo13;
- combo22_2 =combo22 * vertex[0*3+2] + combo23;
- combo22_5 =combo22 * vertex[1*3+2] + combo23;
- combo22_8 =combo22 * vertex[2*3+2] + combo23;
- combo22_11=combo22 * vertex[3*3+2] + combo23;
+ combo02_2 =combo02 * vertex[0][2] + combo03;
+ combo02_5 =combo02 * vertex[1][2] + combo03;
+ combo02_8 =combo02 * vertex[2][2] + combo03;
+ combo02_11=combo02 * vertex[3][2] + combo03;
+ combo12_2 =combo12 * vertex[0][2] + combo13;
+ combo12_5 =combo12 * vertex[1][2] + combo13;
+ combo12_8 =combo12 * vertex[2][2] + combo13;
+ combo12_11=combo12 * vertex[3][2] + combo13;
+ combo22_2 =combo22 * vertex[0][2] + combo23;
+ combo22_5 =combo22 * vertex[1][2] + combo23;
+ combo22_8 =combo22 * vertex[2][2] + combo23;
+ combo22_11=combo22 * vertex[3][2] + combo23;
for(t3=0;t3<3;t3++){
wsin=sinc[t3];
wcos=cosc[t3];
@@ -710,21 +710,21 @@
combo21 = cpara[8] * rot1+ cpara[9] * rot4+ cpara[10] * rot7;
//
err = 0.0;
- h = combo20 * vertex[0*3+0]+ combo21 * vertex[0*3+1]+ combo22_2;
- x = pos2d[0*2+0] - (combo00 * vertex[0*3+0]+ combo01 * vertex[0*3+1]+ combo02_2) / h;
- y = pos2d[0*2+1] - (combo10 * vertex[0*3+0]+ combo11 * vertex[0*3+1]+ combo12_2) / h;
+ h = combo20 * vertex[0][0]+ combo21 * vertex[0][1]+ combo22_2;
+ x = pos2d[0][0] - (combo00 * vertex[0][0]+ combo01 * vertex[0][1]+ combo02_2) / h;
+ y = pos2d[0][1] - (combo10 * vertex[0][0]+ combo11 * vertex[0][1]+ combo12_2) / h;
err += x*x+y*y;
- h = combo20 * vertex[1*3+0]+ combo21 * vertex[1*3+1]+ combo22_5;
- x = pos2d[1*2+0] - (combo00 * vertex[1*3+0]+ combo01 * vertex[1*3+1]+ combo02_5) / h;
- y = pos2d[1*2+1] - (combo10 * vertex[1*3+0]+ combo11 * vertex[1*3+1]+ combo12_5) / h;
+ h = combo20 * vertex[1][0]+ combo21 * vertex[1][1]+ combo22_5;
+ x = pos2d[1][0] - (combo00 * vertex[1][0]+ combo01 * vertex[1][1]+ combo02_5) / h;
+ y = pos2d[1][1] - (combo10 * vertex[1][0]+ combo11 * vertex[1][1]+ combo12_5) / h;
err += x*x+y*y;
- h = combo20 * vertex[2*3+0]+ combo21 * vertex[2*3+1]+ combo22_8;
- x = pos2d[2*2+0] - (combo00 * vertex[2*3+0]+ combo01 * vertex[2*3+1]+ combo02_8) / h;
- y = pos2d[2*2+1] - (combo10 * vertex[2*3+0]+ combo11 * vertex[2*3+1]+ combo12_8) / h;
+ h = combo20 * vertex[2][0]+ combo21 * vertex[2][1]+ combo22_8;
+ x = pos2d[2][0] - (combo00 * vertex[2][0]+ combo01 * vertex[2][1]+ combo02_8) / h;
+ y = pos2d[2][1] - (combo10 * vertex[2][0]+ combo11 * vertex[2][1]+ combo12_8) / h;
err += x*x+y*y;
- h = combo20 * vertex[3*3+0]+ combo21 * vertex[3*3+1]+ combo22_11;
- x = pos2d[3*2+0] - (combo00 * vertex[3*3+0]+ combo01 * vertex[3*3+1]+ combo02_11) / h;
- y = pos2d[3*2+1] - (combo10 * vertex[3*3+0]+ combo11 * vertex[3*3+1]+ combo12_11) / h;
+ h = combo20 * vertex[3][0]+ combo21 * vertex[3][1]+ combo22_11;
+ x = pos2d[3][0] - (combo00 * vertex[3][0]+ combo01 * vertex[3][1]+ combo02_11) / h;
+ y = pos2d[3][1] - (combo10 * vertex[3][0]+ combo11 * vertex[3][1]+ combo12_11) / h;
err += x*x+y*y;
if( err < minerr ) {
minerr = err;
@@ -750,8 +750,56 @@
return minerr/4;
}

+static void nyar_NyARTransRot_O3_initRotByPrevResult(
+ nyar_NyARTransRot_O3_t* i_inst,
+ const double i_prev_result[3][4])
+{
+ double* L_rot=i_inst->array;
+
+ L_rot[0*3+0]=i_prev_result[0][0];
+ L_rot[0*3+1]=i_prev_result[0][1];
+ L_rot[0*3+2]=i_prev_result[0][2];
+
+ L_rot[1*3+0]=i_prev_result[1][0];
+ L_rot[1*3+1]=i_prev_result[1][1];
+ L_rot[1*3+2]=i_prev_result[1][2];
+
+ L_rot[2*3+0]=i_prev_result[2][0];
+ L_rot[2*3+1]=i_prev_result[2][1];
+ L_rot[2*3+2]=i_prev_result[2][2];
+}
/*********************************************************************************

+nyar_NyARTransMatResult擬似クラス
+
+*********************************************************************************/
+
+
+void nyar_NyARTransMatResult_updateMatrixValue(
+ const double i_rot[9],
+ const double i_off[3],
+ const double i_trans[3],
+ double o_conv[3][4])
+{
+ o_conv[0][0] = i_rot[0*3+0];
+ o_conv[0][1] = i_rot[0*3+1];
+ o_conv[0][2] = i_rot[0*3+2];
+ o_conv[0][3] = i_rot[0*3+0]*i_off[0] + i_rot[0*3+1]*i_off[1] + i_rot[0*3+2]*i_off[2] + i_trans[0];
+
+ o_conv[1][0] = i_rot[1*3+0];
+ o_conv[1][1] = i_rot[1*3+1];
+ o_conv[1][2] = i_rot[1*3+2];
+ o_conv[1][3] = i_rot[1*3+0]*i_off[0] + i_rot[1*3+1]*i_off[1] + i_rot[1*3+2]*i_off[2] + i_trans[1];
+
+ o_conv[2][0] = i_rot[2*3+0];
+ o_conv[2][1] = i_rot[2*3+1];
+ o_conv[2][2] = i_rot[2*3+2];
+ o_conv[2][3] = i_rot[2*3+0]*i_off[0] + i_rot[2*3+1]*i_off[1] + i_rot[2*3+2]*i_off[2] + i_trans[2];
+ return;
+}
+
+/*********************************************************************************
+
nyar_NyARTransMat_O2擬似クラス

*********************************************************************************/
@@ -762,17 +810,27 @@
double o_off[]);

static double nyar_NyARTransMat_O2_arGetTransMatSub(
+ const nyar_NyARTransMat_O2_t* i_inst,
+ const double i_pos2d[][2],
+ const double i_pos3d[][3],
+ ARMat* i_mat_b,
+ ARMat* i_mat_d,
+ double o_trans[]);
+
+static void nyar_NyARTransMat_O2_arGetTransMat3_initTransMat(
nyar_NyARTransMat_O2_t* i_inst,
- const double* i_ppos2d,
- double* i_pos3d,
- ARMat* i_mat_b,
- ARMat* i_mat_d,
- double o_trans[]);
+ const double i_ppos3d[][2],
+ const double i_ppos2d[][2],
+ double o_pos2d[][2],
+ double o_pos3d[][3],
+ double o_off[3],
+ ARMat* o_mat_b,
+ ARMat* o_mat_d);


#define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5
#define AR_GET_TRANS_MAT_MAX_FIT_ERROR 1.0
-#define P_MAX 10
+#define P_MAX 4
#define NUMBER_OF_VERTEX 4


@@ -780,8 +838,6 @@
nyar_NyARTransRot_O3_t* transrot;
const ARParam* param;
struct{
- double transMat_pos3d[P_MAX][3];
- double transMat_pos2d[P_MAX][2];
ARMat* transMat_mat_a;
ARMat* transMat_mat_b;
ARMat* transMat_mat_c;
@@ -820,167 +876,247 @@
}


-// private final double[][] wk_transMat_pos3d=new double[P_MAX][3];//pos3d[P_MAX][3];
-// private final double[][] wk_transMat_ppos2d=new double[4][2];
-// private final double[][] wk_transMat_ppos3d=new double[4][2];
-// private final double[] wk_transMat_off=new double[3];
-// private final double[][] wk_transMat_pos2d=new double[P_MAX][2];//pos2d[P_MAX][2];

-/*
-private final DoubleValue wk_arGetTransMatSub_a1=new DoubleValue();
-private final DoubleValue wk_arGetTransMatSub_a2=new DoubleValue();
-private final NyARMat wk_transMat_mat_a=new NyARMat(NUMBER_OF_VERTEX*2,3);
-private final NyARMat wk_transMat_mat_b=new NyARMat(3,NUMBER_OF_VERTEX*2);
-private final NyARMat wk_transMat_mat_d=new NyARMat( 3, 3 );
-private final double[] wk_transMat_mat_trans=new double[3];
-*/

+static void nyar_NyARTransMat_O2_init_transMat_ppos(
+ const ARMarkerInfo* i_marker_info,
+ const double i_width,
+ const double* i_center,
+ double o_ppos2d[4][2],
+ double o_ppos3d[4][2])
+{
+ double c0,c1,w_2;
+ int dir=i_marker_info->dir;
+ o_ppos2d[0][0] = i_marker_info->vertex[(4-dir)%4][0];
+ o_ppos2d[0][1] = i_marker_info->vertex[(4-dir)%4][1];
+ o_ppos2d[1][0] = i_marker_info->vertex[(5-dir)%4][0];
+ o_ppos2d[1][1] = i_marker_info->vertex[(5-dir)%4][1];
+ o_ppos2d[2][0] = i_marker_info->vertex[(6-dir)%4][0];
+ o_ppos2d[2][1] = i_marker_info->vertex[(6-dir)%4][1];
+ o_ppos2d[3][0] = i_marker_info->vertex[(7-dir)%4][0];
+ o_ppos2d[3][1] = i_marker_info->vertex[(7-dir)%4][1];

+
+ c0=i_center[0];
+ c1=i_center[1];
+ w_2 =i_width/2.0;
+
+ o_ppos3d[0][0] = c0 - w_2;//center[0] - w/2.0;
+ o_ppos3d[0][1] = c1 + w_2;//center[1] + w/2.0;
+ o_ppos3d[1][0] = c0 + w_2;//center[0] + w/2.0;
+ o_ppos3d[1][1] = c1 + w_2;//center[1] + w/2.0;
+ o_ppos3d[2][0] = c0 + w_2;//center[0] + w/2.0;
+ o_ppos3d[2][1] = c1 - w_2;//center[1] - w/2.0;
+ o_ppos3d[3][0] = c0 - w_2;//center[0] - w/2.0;
+ o_ppos3d[3][1] = c1 - w_2;//center[1] - w/2.0;
+ return;
+}
+
double nyar_NyARTransMat_O2_transMat(
nyar_NyARTransMat_O2_t* i_inst,
const ARMarkerInfo* i_marker_info,
- const double center[2],
+ const double i_center[2],
const double i_width,
double o_conv[3][4])
{
double ppos2d[4][2];
double ppos3d[4][2];
double off[3];
- double* pos3d=&(i_inst->wk.transMat_pos3d[0][0]);
- int dir=i_marker_info->dir;
- double* pos2d=&(i_inst->wk.transMat_pos2d[0][0]);
- int i,x2;
+ double pos3d[P_MAX][3];
+ double pos2d[P_MAX][2];
+
+
+ int i;
/**/
- const double* cpara=&(i_inst->param->mat[0][0]);
- ARMat* mat_a =i_inst->wk.transMat_mat_a;
+
ARMat* mat_b =i_inst->wk.transMat_mat_b;
ARMat* mat_d =i_inst->wk.transMat_mat_d;
/**/
double err=-1;
- const double* rot=i_inst->transrot->array;
+
double trans[3];
- /**/

- ppos2d[0][0] = i_marker_info->vertex[(4-dir)%4][0];
- ppos2d[0][1] = i_marker_info->vertex[(4-dir)%4][1];
- ppos2d[1][0] = i_marker_info->vertex[(5-dir)%4][0];
- ppos2d[1][1] = i_marker_info->vertex[(5-dir)%4][1];
- ppos2d[2][0] = i_marker_info->vertex[(6-dir)%4][0];
- ppos2d[2][1] = i_marker_info->vertex[(6-dir)%4][1];
- ppos2d[3][0] = i_marker_info->vertex[(7-dir)%4][0];
- ppos2d[3][1] = i_marker_info->vertex[(7-dir)%4][1];
- ppos3d[0][0] = center[0] - i_width/2.0;
- ppos3d[0][1] = center[1] + i_width/2.0;
- ppos3d[1][0] = center[0] + i_width/2.0;
- ppos3d[1][1] = center[1] + i_width/2.0;
- ppos3d[2][0] = center[0] + i_width/2.0;
- ppos3d[2][1] = center[1] - i_width/2.0;
- ppos3d[3][0] = center[0] - i_width/2.0;
- ppos3d[3][1] = center[1] - i_width/2.0;
+
+ //rotationの初期化
nyar_NyARTransRot_O3_arGetInitRot(i_inst->transrot,i_marker_info);

+ //ppos2dとppos3dの初期化
+ nyar_NyARTransMat_O2_init_transMat_ppos(i_marker_info,i_width,i_center,ppos2d,ppos3d);
+
//arGetTransMat3の前段処理(pos3dとoffを初期化)
- nyar_NyARTransMat_O2_initPos3d(&(ppos3d[0][0]),pos3d,off);
+ nyar_NyARTransMat_O2_arGetTransMat3_initTransMat(i_inst,ppos2d,ppos3d,pos2d,pos3d,off,mat_b,mat_d);

-
- //arGetTransMatSubにあった処理。毎回おなじっぽい。pos2dに変換座標を格納する。
-
- if(nyar_FittingMode == AR_FITTING_TO_INPUT ){
- for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {
- arParamIdeal2Observ(i_inst->param->dist_factor,ppos2d[i][0],ppos2d[i][1],&pos2d[i*2+0],&pos2d[i*2+1]);
- }
- }else{
- for(i = 0; i < NUMBER_OF_VERTEX; i++ ){
- pos2d[i*2+0] = ppos2d[i][0];
- pos2d[i*2+1] = ppos2d[i][1];
- }
- }
-
- //変換マトリクスdとbの準備(arGetTransMatSubの一部)
- for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {
- x2=i*2;
- //</Optimize>
- mat_a->m[x2*3+0]=mat_b->m[0*NUMBER_OF_VERTEX*2+x2]=cpara[0*4+0];//mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0];
- mat_a->m[x2*3+1]=mat_b->m[1*NUMBER_OF_VERTEX*2+x2]=cpara[0*4+1];//mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1];
- mat_a->m[x2*3+2]=mat_b->m[2*NUMBER_OF_VERTEX*2+x2]=cpara[0*4+2]-pos2d[i*2+0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];
- mat_a->m[(x2+1)*3+0]=mat_b->m[0*NUMBER_OF_VERTEX*2+x2+1]=0.0;//mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0;
- mat_a->m[(x2+1)*3+1]=mat_b->m[1*NUMBER_OF_VERTEX*2+x2+1]=cpara[1*4+1];//mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1];
- mat_a->m[(x2+1)*3+2]=mat_b->m[2*NUMBER_OF_VERTEX*2+x2+1]=cpara[1*4+2] - pos2d[i*2+1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];
- }
- arMatrixMul(mat_d,mat_b,mat_a);
- arMatrixSelfInv(mat_d);
-
for(i=0;i<AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ){
//<arGetTransMat3>
- err = nyar_NyARTransMat_O2_arGetTransMatSub(i_inst,pos2d, pos3d,mat_b,mat_d,trans);
- o_conv[0][0] = rot[0*3+0];
- o_conv[0][1] = rot[0*3+1];
- o_conv[0][2] = rot[0*3+2];
- o_conv[1][0] = rot[1*3+0];
- o_conv[1][1] = rot[1*3+1];
- o_conv[1][2] = rot[1*3+2];
- o_conv[2][0] = rot[2*3+0];
- o_conv[2][1] = rot[2*3+1];
- o_conv[2][2] = rot[2*3+2];
- o_conv[0][3] = rot[0*3+0]*off[0] + rot[0*3+1]*off[1] + rot[0*3+2]*off[2] + trans[0];
- o_conv[1][3] = rot[1*3+0]*off[0] + rot[1*3+1]*off[1] + rot[1*3+2]*off[2] + trans[1];
- o_conv[2][3] = rot[2*3+0]*off[0] + rot[2*3+1]*off[1] + rot[2*3+2]*off[2] + trans[2];
-
- //</arGetTransMat3>
+ err = nyar_NyARTransMat_O2_arGetTransMatSub(i_inst,pos2d,pos3d,mat_b,mat_d,trans);
+ // //</arGetTransMat3>
if( err < AR_GET_TRANS_MAT_MAX_FIT_ERROR ){
break;
}
}
+ //マトリクスの保存
+ nyar_NyARTransMatResult_updateMatrixValue(i_inst->transrot->array,off,trans,o_conv);
return err;
}

-static void nyar_NyARTransMat_O2_initPos3d(
- const double* i_ppos3d,//[][2]
- double* o_pos3d,//[][3]
- double o_off[])
+double nyar_NyARTransMat_O2_transMatCont(
+ nyar_NyARTransMat_O2_t* i_inst,
+ const ARMarkerInfo* i_marker_info,
+ const double i_prev_conv[3][4],
+ const double i_center[2],
+ const double i_width,
+ double o_conv[3][4])
+/*
+double nyar_NyARTransMat_O2_transMatCont(
+ NyARSquare i_square,int i_direction, double i_width,NyARTransMatResult io_result_conv)throws NyARException*/
{
- double pmax[3];
- double pmin[3];
+ double ppos2d[4][2];
+ double ppos3d[4][2];
+ double pos3d[P_MAX][3];
+ double pos2d[P_MAX][2];
+ double off[3];
+
+ ARMat* mat_b =i_inst->wk.transMat_mat_b;
+ ARMat* mat_d =i_inst->wk.transMat_mat_d;
+ double err1,err2;
+ int i;
+ double trans[3];
+ double conv2[3][4];
+
+// double[][] ppos2d=wk_transMat_ppos2d;
+// double[][] ppos3d=wk_transMat_ppos3d;
+// double[] off =wk_transMat_off;
+// double[][] pos3d =wk_transMat_pos3d;
+
+ // arGetTransMatContSub計算部分
+ nyar_NyARTransRot_O3_initRotByPrevResult(i_inst->transrot,i_prev_conv);
+
+ //ppos2dとppos3dの初期化
+ nyar_NyARTransMat_O2_init_transMat_ppos(i_marker_info,i_width,i_center,ppos2d,ppos3d);
+
+ //arGetTransMat3の前段処理(pos3dとoffを初期化)
+ // double[][] pos2d=this.wk_transMat_pos2d;
+
+
+ //transMatに必要な初期値を計算
+ nyar_NyARTransMat_O2_arGetTransMat3_initTransMat(i_inst,ppos2d,ppos3d,pos2d,pos3d,off,mat_b,mat_d);
+
+ err1=err2=-1;
+ for( i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ){
+ err1 = nyar_NyARTransMat_O2_arGetTransMatSub(i_inst,pos2d,pos3d,mat_b,mat_d,trans);
+ if( err1 < AR_GET_TRANS_MAT_MAX_FIT_ERROR ){
+ //十分な精度を達成できたらブレーク
+ break;
+ }
+ }
+ //値を保存
+ nyar_NyARTransMatResult_updateMatrixValue(i_inst->transrot->array,off,trans,o_conv);
+
+ //エラー値が許容範囲でなければTransMatをやり直し
+ if(err1>AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR){
+ //transMatを実行(初期化値は共用)
+ nyar_NyARTransRot_O3_arGetInitRot(i_inst->transrot,i_marker_info);
+ err2=nyar_NyARTransMat_O2_transMat(i_inst,i_marker_info,i_center,i_width,conv2);
+ //transmMatここまで
+ if(err2<err1){
+ //良い値が取れたら、差換え
+ memcpy(o_conv,conv2,sizeof(double)*3*4);
+ err1 = err2;
+ }
+ }
+ return err1;
+}
+
+
+/* Java版の不具合でi_ppos2dとi_ppos3dの順番がひっくり返ってたのを直してある。
+
+
+*/
+static void nyar_NyARTransMat_O2_arGetTransMat3_initTransMat(
+ nyar_NyARTransMat_O2_t* i_inst,
+ const double i_ppos2d[][2],
+ const double i_ppos3d[][2],
+ double o_pos2d[][2],
+ double o_pos3d[][3],
+ double o_off[3],
+ ARMat* o_mat_b,
+ ARMat* o_mat_d)
+{
+ double pmax0,pmax1,pmax2,pmin0,pmin1,pmin2;
int i;
- pmax[0]=pmax[1]=pmax[2] = -10000000000.0;
- pmin[0]=pmin[1]=pmin[2] = 10000000000.0;
+ ARMat* mat_a =i_inst->wk.transMat_mat_a;
+ const double* cpara=&(i_inst->param->mat[0][0]);
+ int x2;
+
+ pmax0=pmax1=pmax2 = -10000000000.0;
+ pmin0=pmin1=pmin2 = 10000000000.0;
for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {
- if( i_ppos3d[i*2+0] > pmax[0] ){
- pmax[0] = i_ppos3d[i*2+0];
+ if( i_ppos3d[i][0] > pmax0 ){
+ pmax0 = i_ppos3d[i][0];
+ }
+ if( i_ppos3d[i][0] < pmin0 ){
+ pmin0 = i_ppos3d[i][0];
+ }
+ if( i_ppos3d[i][1] > pmax1 ){
+ pmax1 = i_ppos3d[i][1];
+ }
+ if( i_ppos3d[i][1] < pmin1 ){
+ pmin1 = i_ppos3d[i][1];
+ }
+ /* オリジナルでもコメントアウト
+ if( ppos3d[i][2] > pmax[2] ) pmax[2] = ppos3d[i][2];
+ if( ppos3d[i][2] < pmin[2] ) pmin[2] = ppos3d[i][2];
+ */
+ }
+ o_off[0] = -(pmax0 + pmin0) / 2.0;
+ o_off[1] = -(pmax1 + pmin1) / 2.0;
+ o_off[2] = -(pmax2 + pmin2) / 2.0;
+
+
+ for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {
+ o_pos3d[i][0] = i_ppos3d[i][0] + o_off[0];
+ o_pos3d[i][1] = i_ppos3d[i][1] + o_off[1];
+ o_pos3d[i][2] = 0.0;
+ }
+ //ココから先でarGetTransMatSubの初期化処理
+ //arGetTransMatSubにあった処理。毎回おなじっぽい。pos2dに変換座標を格納する。
+
+ if(arFittingMode == AR_FITTING_TO_INPUT ){
+ for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {
+ arParamIdeal2Observ(i_inst->param->dist_factor,i_ppos2d[i][0],i_ppos2d[i][1],&o_pos2d[i][0],&o_pos2d[i][1]);
}
- if( i_ppos3d[i*2+0] < pmin[0] ){
- pmin[0] = i_ppos3d[i*2+0];
- }
- if( i_ppos3d[i*2+1] > pmax[1] ){
- pmax[1] = i_ppos3d[i*2+1];
- }
- if( i_ppos3d[i*2+1] < pmin[1] ){
- pmin[1] = i_ppos3d[i*2+1];
- }
+ }else{
+ for(i = 0; i < NUMBER_OF_VERTEX; i++ ){
+ o_pos2d[i][0] = i_ppos2d[i][0];
+ o_pos2d[i][1] = i_ppos2d[i][1];
+ }
}
- o_off[0] = -(pmax[0] + pmin[0]) / 2.0;
- o_off[1] = -(pmax[1] + pmin[1]) / 2.0;
- o_off[2] = -(pmax[2] + pmin[2]) / 2.0;


-
- for(i = 0; i < NUMBER_OF_VERTEX; i++ ){
- o_pos3d[i*3+0] = i_ppos3d[i*2+0] + o_off[0];
- o_pos3d[i*3+1] = i_ppos3d[i*2+1] + o_off[1];
- o_pos3d[i*3+2] = 0.0;
+ for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {
+ x2=i*2;
+ //</Optimize>
+ mat_a->m[x2*3+0]=o_mat_b->m[0*NUMBER_OF_VERTEX*2+x2]=cpara[0*4+0];//mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0];
+ mat_a->m[x2*3+1]=o_mat_b->m[1*NUMBER_OF_VERTEX*2+x2]=cpara[0*4+1];//mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1];
+ mat_a->m[x2*3+2]=o_mat_b->m[2*NUMBER_OF_VERTEX*2+x2]=cpara[0*4+2]-o_pos2d[i][0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];
+ mat_a->m[(x2+1)*3+0]=o_mat_b->m[0*NUMBER_OF_VERTEX*2+x2+1]=0.0;//mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0;
+ mat_a->m[(x2+1)*3+1]=o_mat_b->m[1*NUMBER_OF_VERTEX*2+x2+1]=cpara[1*4+1];//mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1];
+ mat_a->m[(x2+1)*3+2]=o_mat_b->m[2*NUMBER_OF_VERTEX*2+x2+1]=cpara[1*4+2] - o_pos2d[i][1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];
}
-}
+
+ //mat_d
+ arMatrixMul(o_mat_d,o_mat_b,mat_a);
+ arMatrixSelfInv(o_mat_d);
+}

-
static double nyar_NyARTransMat_O2_arGetTransMatSub(
- nyar_NyARTransMat_O2_t* i_inst,
- const double* i_ppos2d,//[][2],
- double* i_pos3d,//[][3],
- ARMat* i_mat_b,
- ARMat* i_mat_d,
- double o_trans[])
+ const nyar_NyARTransMat_O2_t* i_inst,
+ const double i_pos2d[][2],
+ const double i_pos3d[][3],
+ ARMat* i_mat_b,
+ ARMat* i_mat_d,
+ double o_trans[])
{
- // double cpara[]=param.get34Array();
ARMat* mat_c=i_inst->wk.transMat_mat_c;
ARMat* mat_e=i_inst->wk.transMat_mat_e;
ARMat* mat_f=i_inst->wk.transMat_mat_f;
@@ -989,30 +1125,31 @@
double ret;
int i;
double* rot=i_inst->transrot->array;
+
int x2;
for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {
- x2=i*2;
- wx = rot[0] * i_pos3d[i*3+0]+ rot[1] * i_pos3d[i*3+1]+ rot[2] * i_pos3d[i*3+2];
- wy = rot[3] * i_pos3d[i*3+0]+ rot[4] * i_pos3d[i*3+1]+ rot[5] * i_pos3d[i*3+2];
- wz = rot[6] * i_pos3d[i*3+0]+ rot[7] * i_pos3d[i*3+1]+ rot[8] * i_pos3d[i*3+2];
- mat_c->m[x2+0] =wz * i_ppos2d[i*2+0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;
- mat_c->m[x2+1]=wz * i_ppos2d[i*2+1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;
+ x2=i*2;
+ wx = rot[0] * i_pos3d[i][0]+ rot[1] * i_pos3d[i][1]+ rot[2] * i_pos3d[i][2];
+ wy = rot[3] * i_pos3d[i][0]+ rot[4] * i_pos3d[i][1]+ rot[5] * i_pos3d[i][2];
+ wz = rot[6] * i_pos3d[i][0]+ rot[7] * i_pos3d[i][1]+ rot[8] * i_pos3d[i][2];
+ mat_c->m[x2+0]=wz * i_pos2d[i][0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;
+ mat_c->m[x2+1]=wz * i_pos2d[i][1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;
}
+
arMatrixMul(mat_e,i_mat_b, mat_c );
arMatrixMul(mat_f,i_mat_d, mat_e );
-
- // double[] trans=wk_arGetTransMatSub_trans;//double trans[3];
o_trans[0] = mat_f->m[0];//trans[0] = mat_f->m[0];
o_trans[1] = mat_f->m[1];
o_trans[2] = mat_f->m[2];//trans[2] = mat_f->m[2];
- ret =nyar_NyARTransRot_O3_modifyMatrix(i_inst->transrot,o_trans, i_pos3d, i_ppos2d);
+
+ ret =nyar_NyARTransRot_O3_modifyMatrix(i_inst->transrot,o_trans, i_pos3d, i_pos2d);
for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {
- x2=i*2;
- wx = rot[0] * i_pos3d[i*3+0]+ rot[1] * i_pos3d[i*3+1]+ rot[2] * i_pos3d[i*3+2];
- wy = rot[3] * i_pos3d[i*3+0]+ rot[4] * i_pos3d[i*3+1]+ rot[5] * i_pos3d[i*3+2];
- wz = rot[6] * i_pos3d[i*3+0]+ rot[7] * i_pos3d[i*3+1]+ rot[8] * i_pos3d[i*3+2];
- mat_c->m[x2+0] =wz * i_ppos2d[i*2+0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;
- mat_c->m[x2+1]=wz * i_ppos2d[i*2+1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;
+ x2=i*2;
+ wx = rot[0] * i_pos3d[i][0]+ rot[1] * i_pos3d[i][1]+ rot[2] * i_pos3d[i][2];
+ wy = rot[3] * i_pos3d[i][0]+ rot[4] * i_pos3d[i][1]+ rot[5] * i_pos3d[i][2];
+ wz = rot[6] * i_pos3d[i][0]+ rot[7] * i_pos3d[i][1]+ rot[8] * i_pos3d[i][2];
+ mat_c->m[x2+0]=wz * i_pos2d[i][0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;
+ mat_c->m[x2+1]=wz * i_pos2d[i][1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;
}

arMatrixMul(mat_e,i_mat_b, mat_c );
@@ -1020,19 +1157,8 @@
o_trans[0] = mat_f->m[0];//trans[0] = mat_f->m[0];
o_trans[1] = mat_f->m[1];
o_trans[2] = mat_f->m[2];//trans[2] = mat_f->m[2];
- ret =nyar_NyARTransRot_O3_modifyMatrix(i_inst->transrot,o_trans, i_pos3d, i_ppos2d);
-
- // double[][] conv=result_mat.getArray();
- // for( i = 2; i >=0; i-- ) {//<Optimize/>for( j = 0; j < 3; j++ ) {
- // //<Optimize>
- // //for( i = 0; i < 3; i++ ){
- // // conv[j][i] = rot[j][i];
- // //}
- // conv[i][0] = rot[i*3+0];
- // conv[i][1] = rot[i*3+1];
- // conv[i][2] = rot[i*3+2];
- // //</Optimize>
- // conv[i][3] = trans[i];
- // }
+ ret =nyar_NyARTransRot_O3_modifyMatrix(i_inst->transrot,o_trans, i_pos3d, i_pos2d);
return ret;
}
+
+