Blame |
Last modification |
View Log
| RSS feed
package jp.nyatla.nyartoolkit.base;
import jp.nyatla.nyartoolkit.NyARException;
import jp.nyatla.nyartoolkit.core.NyARMat;
public class Param {
private static final int AR_PARAM_NMIN=6;//#define AR_PARAM_NMIN 6
private static final int AR_PARAM_NMAX=1000;//#define AR_PARAM_NMAX 1000
private static final double AR_PARAM_C34=100.0;//#define AR_PARAM_C34 100.0
/*
typedef struct {
int xsize, ysize;
double matL[3][4];
double matR[3][4];
double matL2R[3][4];
double dist_factorL[4];
double dist_factorR[4];
} ARSParam;*/
/* static class ARSParam{
int xsize, ysize;
Double2dArray matL=new Double2dArray(3,4);
Double2dArray matR=new Double2dArray(3,4);
Double2dArray matL2R=new Double2dArray(3,4);
double[] dist_factorL=new double[4];
double[] dist_factorR=new double[4];
}*/
private static final int arParamGet_AR_PARAM_CDMIN = 12;
/*int arParamGet( double global[][3], double screen[][2], int data_num,double mat[3][4] );*/
public static int arParamGet( double global[][], double[][] screen, int num,double[][] mat) throws NyARException
{
NyARMat mat_a, mat_at, mat_r;//ARMat *mat_a, *mat_at, *mat_r, mat_cpara;
NyARMat mat_wm1,mat_wm2;//ARMat *mat_wm1, *mat_wm2;
int i;
if(num < AR_PARAM_NMIN){
return( -1 );
}
if(num > AR_PARAM_NMAX){
return( -1 );
}
NyARException.trap("未チェックのパス");{
mat_a = new NyARMat(2*num,arParamGet_AR_PARAM_CDMIN-1 );//mat_a = arMatrixAlloc( 2*num, AR_PARAM_CDMIN-1 );
mat_at =new NyARMat(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_at = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );
mat_r = new NyARMat(2*num,1);//mat_r = arMatrixAlloc( 2*num, 1 );
mat_wm1 =new NyARMat(arParamGet_AR_PARAM_CDMIN-1, arParamGet_AR_PARAM_CDMIN-1 );//mat_wm1 = arMatrixAlloc( AR_PARAM_CDMIN-1, AR_PARAM_CDMIN-1 );
mat_wm2 =new NyARMat(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_wm2 = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );
}
/*
mat_a = Matrix.arMatrixAlloc( 2*num, arParamGet_AR_PARAM_CDMIN-1 );//mat_a = arMatrixAlloc( 2*num, AR_PARAM_CDMIN-1 );
if( mat_a == null){
return -1;
}
mat_at =Matrix.arMatrixAlloc(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_at = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );
if(mat_at == null){
return -1;
}
mat_r = Matrix.arMatrixAlloc( 2*num, 1 );//mat_r = arMatrixAlloc( 2*num, 1 );
if(mat_r ==null){
return -1;
}
mat_wm1 = Matrix.arMatrixAlloc(arParamGet_AR_PARAM_CDMIN-1, arParamGet_AR_PARAM_CDMIN-1 );//mat_wm1 = arMatrixAlloc( AR_PARAM_CDMIN-1, AR_PARAM_CDMIN-1 );
if( mat_wm1 == null) {
return -1;
}
mat_wm2 = Matrix.arMatrixAlloc(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_wm2 = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );
if( mat_wm2 == null ) {
return -1;
}
*/
/* Initializing array */
mat_a.zeroClear();//Javaではゼロクリアされるので不要
// pa1=mat_a.getPointer();//pa1 = mat_a->m;
// for(i = 0; i < 2 * num * (arParamGet_AR_PARAM_CDMIN-1); i++){
// {//*pa1++ = 0.0;
// pa1.set(0.0);
// pa1.incPtr();}
// }
double[][] pa1 =mat_a.getArray();
double[][] pa2 =mat_a.getArray();
/* Calculate A,R matrix */
double[][] pr=mat_r.getArray();
int pr_ptr_col=0;
for(i = 0; i < num; i++) {//for(i = 0, pr = mat_r->m; i < num; i++) {
int pa1_ptr_row =2*i; //pa1 = &(mat_a->m[ (2*i) * (AR_PARAM_CDMIN-1)
int pa2_ptr_row =2*i+1;//pa2 = &(mat_a->m[ (2*i+1) * (AR_PARAM_CDMIN-1) + 4]);
// pa1=mat_a.getPointer((2*i)*(arParamGet_AR_PARAM_CDMIN-1)); ]);
// pa2=mat_a.getPointer((2*i+1)*(arParamGet_AR_PARAM_CDMIN-1) + 4);
//*pa1++ = global[i][0]; *pa1++ = global[i][1];
pa1[pa1_ptr_row][ 0]=global[i][0];
pa1[pa1_ptr_row][ 1]=global[i][1];
//*pa1++ = global[i][2]; *pa1++ = 1.0;
pa1[pa1_ptr_row][ 2]=global[i][2];
pa1[pa1_ptr_row][ 3]=1.0;
//*pa2++ = global[i][0]; *pa2++ = global[i][1];
pa2[pa2_ptr_row][ 4]=global[i][0];
pa2[pa2_ptr_row][ 5]=global[i][1];
//*pa2++ = global[i][2]; *pa2++ = 1.0;
pa2[pa2_ptr_row][ 6]=global[i][2];
pa2[pa2_ptr_row][ 7]=1.0;
//pa1 += 4;
//*pa1++ = -global[i][0] * screen[i][0];
pa1[pa1_ptr_row][ 8]=-global[i][0] * screen[i][0];
//*pa1++ = -global[i][1] * screen[i][0];
pa1[pa1_ptr_row][ 9]=-global[i][1] * screen[i][0];
//*pa1 = -global[i][2] * screen[i][0];
pa1[pa1_ptr_row][10]=-global[i][2]* screen[i][0];
//*pa2++ = -global[i][0] * screen[i][1];
pa2[pa2_ptr_row][ 8]=-global[i][0] * screen[i][1];
//*pa2++ = -global[i][1] * screen[i][1];
pa2[pa2_ptr_row][ 9]=-global[i][1] * screen[i][1];
//*pa2 = -global[i][2] * screen[i][1];
pa2[pa2_ptr_row][10]=-global[i][2] * screen[i][1];
//*pr++ = screen[i][0] * AR_PARAM_C34;
pr[0][pr_ptr_col]=screen[i][0] * AR_PARAM_C34;pr_ptr_col++;
//*pr++ = screen[i][1] * AR_PARAM_C34;
pr[0][pr_ptr_col]=screen[i][1] * AR_PARAM_C34;pr_ptr_col++;
}
NyARException.trap("未チェックのパス");
NyARMat.matrixTrans(mat_at, mat_a );//if( arMatrixTrans( mat_at, mat_a ) < 0 ){
NyARException.trap("未チェックのパス");
mat_wm1.matrixMul(mat_at, mat_a );//if( arMatrixMul( mat_wm1, mat_at, mat_a ) < 0 ) {
NyARException.trap("未チェックのパス");
mat_wm1.matrixSelfInv();//if( arMatrixSelfInv( mat_wm1 ) < 0 ) {
NyARException.trap("未チェックのパス");
mat_wm2.matrixMul(mat_wm1, mat_at );//if( arMatrixMul( mat_wm2, mat_wm1, mat_at ) < 0 ) {
//mat_cpara.row = AR_PARAM_CDMIN-1;//mat_cpara.row = AR_PARAM_CDMIN-1;
//mat_cpara.clm = 1;
//mat_cpara.m = &(mat[0][0]);
/*1次元行列から3x4行列に転写。高負荷なところじゃないから地道に転写でOK*/
NyARMat mat_cpara=new NyARMat(arParamGet_AR_PARAM_CDMIN-1,1);
double[][] mat_cpara_array=mat_cpara.getArray();
NyARException.trap("未チェックのパス");
mat_cpara.matrixMul(mat_wm2, mat_r );//if( arMatrixMul( &mat_cpara, mat_wm2, mat_r ) < 0 ) {
for(int i2=0;i<3;i++){
for(int i3=0;i3<4;i3++){
mat[i2][i3]=mat_cpara_array[i2*4+i3][0];
}
}
//ARMat.wrap(mat.array(),arParamGet_AR_PARAM_CDMIN-1,1);}
mat[2][3]=AR_PARAM_C34;//mat[2][3] = AR_PARAM_C34;
return 0;
}
/* //int arsParamChangeSize( ARSParam *source, int xsize, int ysize, ARSParam *newparam );
public static int arsParamChangeSize( ARSParam source, int xsize, int ysize, ARSParam newparam)
{
double scale;
newparam.xsize=xsize;//newparam->xsize = xsize;
newparam.ysize=ysize;//newparam->ysize = ysize;
scale=(double)xsize/ (double)(source.xsize);//scale = (double)xsize / (double)(source->xsize);
for(int i = 0; i < 4; i++ ){
newparam.matL.set(0,i,source.matL.get(0,i)*scale);//newparam->matL[0][i] = source->matL[0][i] * scale;
newparam.matL.set(1,i,source.matL.get(1,i)*scale);//newparam->matL[1][i] = source->matL[1][i] * scale;
newparam.matL.set(2,i,source.matL.get(2,i));//newparam->matL[2][i] = source->matL[2][i];
}
for(int i = 0; i < 4; i++ ) {
newparam.matR.set(0,i,source.matR.get(0,i)*scale);//newparam->matR[0][i] = source->matR[0][i] * scale;
newparam.matR.set(1,i,source.matR.get(1,i)*scale);//newparam->matR[1][i] = source->matR[1][i] * scale;
newparam.matR.set(2,i,source.matR.get(2,i));//newparam->matR[2][i] = source->matR[2][i];
}
for(int i = 0; i < 4; i++ ) {
newparam.matL2R.set(0,i,source.matL2R.get(0,i));//newparam->matL2R[0][i] = source->matL2R[0][i];
newparam.matL2R.set(1,i,source.matL2R.get(1,i));//newparam->matL2R[1][i] = source->matL2R[1][i];
newparam.matL2R.set(2,i,source.matL2R.get(2,i));//newparam->matL2R[2][i] = source->matL2R[2][i];
}
newparam.dist_factorL[0] = source.dist_factorL[0] * scale;//newparam->dist_factorL[0] = source->dist_factorL[0] * scale;
newparam.dist_factorL[1] = source.dist_factorL[1] * scale;//newparam->dist_factorL[1] = source->dist_factorL[1] * scale;
newparam.dist_factorL[2] = source.dist_factorL[2] / (scale*scale);//newparam->dist_factorL[2] = source->dist_factorL[2] / (scale*scale);
newparam.dist_factorL[3] = source.dist_factorL[3];//newparam->dist_factorL[3] = source->dist_factorL[3];
newparam.dist_factorR[0] = source.dist_factorR[0] * scale;//newparam->dist_factorR[0] = source->dist_factorR[0] * scale;
newparam.dist_factorR[1] = source.dist_factorR[1] * scale;//newparam->dist_factorR[1] = source->dist_factorR[1] * scale;
newparam.dist_factorR[2] = source.dist_factorR[2] / (scale*scale);//newparam->dist_factorR[2] = source->dist_factorR[2] / (scale*scale);
newparam.dist_factorR[3] = source.dist_factorR[3];//newparam->dist_factorR[3] = source->dist_factorR[3];
return 0;
}*/
/* //int arsParamSave( char *filename, ARSParam *sparam );
public static int arsParamSave(String filename, ARSParam sparam) throws Exception
{
//int xsize, ysize;
//Double2dArray matL=new Double2dArray(3,4);
//Double2dArray matR=new Double2dArray(3,4);
//Double2dArray matL2R=new Double2dArray(3,4);
//double dist_factorL[]=new double[4];
//double dist_factorR[]=new double[4];
byte[] buf=new byte[(4+4+(3*4*8)*3+(4*8)*2)];
//バッファをラップ
ByteBuffer bb = ByteBuffer.wrap(buf);
bb.order(ByteOrder.BIG_ENDIAN);
//書き込み
bb.putInt(sparam.xsize);
bb.putInt(sparam.ysize);
for(int i=0;i<3;i++){
for(int i2=0;i2<4;i2++){
bb.putDouble(sparam.matL.get(i, i2));
}
}
for(int i=0;i<3;i++){
for(int i2=0;i2<4;i2++){
bb.putDouble(sparam.matR.get(i, i2));
}
}
for(int i=0;i<3;i++){
for(int i2=0;i2<4;i2++){
bb.putDouble(sparam.matL2R.get(i, i2));
}
}
for(int i=0;i<4;i++){
bb.putDouble(sparam.dist_factorL[i]);
}
for(int i=0;i<4;i++){
bb.putDouble(sparam.dist_factorR[i]);
}
//ファイルに保存
FileOutputStream fs=new FileOutputStream(filename);
fs.write(buf);
fs.close();
return 0;
}*/
/* //int arsParamLoad( char *filename, ARSParam *sparam );
public static int arsParamLoad(String filename, ARSParam sparam ) throws Exception
{
//ファイルを読んどく
FileInputStream fs=new FileInputStream(filename);
File f=new File(filename);
int file_size=(int)f.length();
byte[] buf=new byte[file_size];
fs.read(buf);
fs.close();
//バッファを加工
ByteBuffer bb = ByteBuffer.wrap(buf);
bb.order(ByteOrder.BIG_ENDIAN);
//固定回数パースして配列に格納
sparam.xsize=bb.getInt();
sparam.ysize=bb.getInt();
for(int i=0;i<3;i++){
for(int i2=0;i2<4;i2++){
sparam.matL.set(i,i2,bb.getDouble());
}
}
for(int i=0;i<3;i++){
for(int i2=0;i2<4;i2++){
sparam.matR.set(i,i2,bb.getDouble());
}
}
for(int i=0;i<3;i++){
for(int i2=0;i2<4;i2++){
sparam.matL2R.set(i,i2,bb.getDouble());
}
}
for(int i=0;i<3;i++){
sparam.dist_factorL[i]=bb.getDouble();
}
for(int i=0;i<3;i++){
sparam.dist_factorR[i]=bb.getDouble();
}
return 0;
}*/
/*
// int arsParamGetMat( double matL[3][4], double matR[3][4],double cparaL[3][4], double cparaR[3][4], double matL2R[3][4] );
public static int arsParamGetMat(double[][] matL, double[][] matR,double[][] cparaL, double[][] cparaR, double[][] matL2R) throws JartkException
{
ARMat t1,t2,t3;//ARMat *t1, *t2, *t3;
//double transL[3][4], transR[3][4];
Double2dArray transL=new Double2dArray(3,4);
Double2dArray transR=new Double2dArray(3,4);
arParamDecompMat( matL,cparaL,transL);
arParamDecompMat( matR,cparaR,transR);
JartkException.trap("未チェックパス");{
t1=new ARMat(4,4);//t1 = arMatrixAlloc( 4, 4 );
t2=new ARMat(4,4);//t2 = arMatrixAlloc( 4, 4 );
}
double[][] t1_array=t1.getArray();
double[][] t2_array=t2.getArray();
for(int j = 0; j < 3; j++ ){
for(int i = 0; i < 4; i++ ) {
JartkException.trap("未チェックパス");{
t1_array[j][i]=transL.get(j,i);//t1->m[j*4+i] = transL[j][i];
t2_array[j][i]=transL.get(j,i);//t2->m[j*4+i] = transR[j][i];
}
}
}
JartkException.trap("未チェックパス");{
t1_array[3][0]=t1_array[3][1]=t1_array[3][2]=0.0;//t1->m[12] = t1->m[13] = t1->m[14] = 0.0;
t1_array[3][3]=1.0;//t1->m[15] = 1.0;
t2_array[3][0]=t2_array[3][1]=t2_array[3][2]=0.0;//t2->m[12] = t2->m[13] = t2->m[14] = 0.0;
t2_array[3][3]=1.0;//t2->m[15] = 1.0;
}
JartkException.trap("未チェックのパス");
t1.matrixSelfInv();//if( arMatrixSelfInv(t1) != 0 ) {
JartkException.trap("未チェックのパス");
t3 =ARMat.matrixAllocMul(t2, t1);//t3 = arMatrixAllocMul(t2, t1);
double[][] t3_array=t3.getArray();
if(t3==null){
return -1;
}
for(int j = 0; j < 3; j++ ) {
for(int i = 0; i < 4; i++ ) {
JartkException.trap("未チェックパス");
matL2R[j][i]=t3_array[j][i];//matL2R[j][i] = t3->m[j*4+i];
}
}
return 0;
}
*/ //int arsParamDisp( ARSParam *sparam )
/* public static int arsParamDisp( ARSParam sparam)
{
System.out.println("--------------------------------------");//printf("--------------------------------------\n");
System.out.println("SIZE = "+sparam.xsize+", "+sparam.ysize);// printf("SIZE = %d, %d\n", sparam->xsize, sparam->ysize);
System.out.println("-- Left --");//printf("-- Left --\n");
System.out.println("Distotion factor = "+sparam.dist_factorL[0]+" "+sparam.dist_factorL[1]+" "+sparam.dist_factorL[2]+" "+sparam.dist_factorL[3]);//printf("Distotion factor = %f %f %f %f\n", sparam->dist_factorL[0],sparam->dist_factorL[1], sparam->dist_factorL[2], sparam->dist_factorL[3] );
for(int j = 0; j < 3; j++ ) {
for(int i = 0; i < 4; i++ ){
System.out.print(sparam.matL.get(j,i)+" ");//printf("%7.5f ", sparam->matL[j][i]);
}
System.out.println();//printf("\n");
}
System.out.println("-- Right --");//printf("-- Right --\n");
System.out.println("Distotion factor = "+sparam.dist_factorR[0]+" "+sparam.dist_factorR[1]+" "+sparam.dist_factorR[2]+" "+sparam.dist_factorR[3]);//printf("Distotion factor = %f %f %f %f\n", sparam->dist_factorR[0],sparam->dist_factorR[1], sparam->dist_factorR[2], sparam->dist_factorR[3]);
for(int j = 0; j < 3; j++ ){
for(int i = 0; i < 4; i++ ){
System.out.println(sparam.matR.get(j,i)+" ");//printf("%7.5f ", sparam->matR[j][i]);
}
System.out.println();//printf("\n");
}
System.out.println("-- Left => Right --");//printf("-- Left => Right --\n");
for(int j = 0; j < 3; j++ ) {
for(int i = 0; i < 4; i++ ){
//printf("%7.5f ", sparam->matL2R[j][i]);
}
System.out.println();//printf("\n");
}
System.out.println("--------------------------------------");//printf("--------------------------------------\n");
return 0;
}*/
}