artoolkit: arGetTransMatCont.c Source File

00001 /*******************************************************
00002  *
00003  * Author: Hirokazu Kato
00004  *
00005  *         kato@sys.im.hiroshima-cu.ac.jp
00006  *
00007  * Date: 01/09/10
00008  *
00009 *******************************************************/
00010 
00011 #include <AR/ar.h>
00012 
00013 static double arGetTransMatContSub( ARMarkerInfo *marker_info, double prev_conv[3][4],
00014                                     double center[2], double width, double conv[3][4] );
00015 
00016 double arGetTransMatCont( ARMarkerInfo *marker_info, double prev_conv[3][4],
00017                           double center[2], double width, double conv[3][4] )
00018 {
00019     double  err1, err2;
00020     double  wtrans[3][4];
00021     int     i, j;
00022 
00023     err1 = arGetTransMatContSub(marker_info, prev_conv, center, width, conv);
00024     if( err1 > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR ) {
00025         err2 = arGetTransMat(marker_info, center, width, wtrans);
00026         if( err2 < err1 ) {
00027             for( j = 0; j < 3; j++ ) {
00028                 for( i = 0; i < 4; i++ ) conv[j][i] = wtrans[j][i];
00029             }
00030             err1 = err2;
00031         }
00032     }
00033 
00034     return err1;
00035 }
00036 
00037 
00038 static double arGetTransMatContSub( ARMarkerInfo *marker_info, double prev_conv[3][4],
00039                                     double center[2], double width, double conv[3][4] )
00040 {
00041     double  rot[3][3];
00042     double  ppos2d[4][2];
00043     double  ppos3d[4][2];
00044     int     dir;
00045     double  err;
00046     int     i, j;
00047 
00048     for( i = 0; i < 3; i++ ) {
00049         for( j = 0; j < 3; j++ ) {
00050             rot[i][j] = prev_conv[i][j];
00051         }
00052     }
00053 
00054     dir = marker_info->dir;
00055     ppos2d[0][0] = marker_info->vertex[(4-dir)%4][0];
00056     ppos2d[0][1] = marker_info->vertex[(4-dir)%4][1];
00057     ppos2d[1][0] = marker_info->vertex[(5-dir)%4][0];
00058     ppos2d[1][1] = marker_info->vertex[(5-dir)%4][1];
00059     ppos2d[2][0] = marker_info->vertex[(6-dir)%4][0];
00060     ppos2d[2][1] = marker_info->vertex[(6-dir)%4][1];
00061     ppos2d[3][0] = marker_info->vertex[(7-dir)%4][0];
00062     ppos2d[3][1] = marker_info->vertex[(7-dir)%4][1];
00063     ppos3d[0][0] = center[0] - width/2.0;
00064     ppos3d[0][1] = center[1] + width/2.0;
00065     ppos3d[1][0] = center[0] + width/2.0;
00066     ppos3d[1][1] = center[1] + width/2.0;
00067     ppos3d[2][0] = center[0] + width/2.0;
00068     ppos3d[2][1] = center[1] - width/2.0;
00069     ppos3d[3][0] = center[0] - width/2.0;
00070     ppos3d[3][1] = center[1] - width/2.0;
00071 
00072     for( i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ) {
00073         err = arGetTransMat3( rot, ppos2d, ppos3d, 4, conv,
00074                                    arParam.dist_factor, arParam.mat );
00075         if( err < AR_GET_TRANS_MAT_MAX_FIT_ERROR ) break;
00076     }
00077     return err;
00078 }