AR_TEMPL_FUNC ARFloat AR_TEMPL_TRACKER::arGetTransMatContSub(ARMarkerInfo *marker_info, ARFloat prev_conv[3][4], ARFloat center[2], ARFloat width, ARFloat conv[3][4]) { ARFloat rot[3][3]; ARFloat ppos2d[4][2]; ARFloat ppos3d[4][2]; int dir; ARFloat err; int i, j; for( i = 0; i < 3; i++ ) { for( j = 0; j < 3; j++ ) { rot[i][j] = prev_conv[i][j]; } } dir = marker_info->dir; ppos2d[0][0] = marker_info->vertex[(4-dir)%4][0]; ppos2d[0][1] = marker_info->vertex[(4-dir)%4][1]; ppos2d[1][0] = marker_info->vertex[(5-dir)%4][0]; ppos2d[1][1] = marker_info->vertex[(5-dir)%4][1]; ppos2d[2][0] = marker_info->vertex[(6-dir)%4][0]; ppos2d[2][1] = marker_info->vertex[(6-dir)%4][1]; ppos2d[3][0] = marker_info->vertex[(7-dir)%4][0]; ppos2d[3][1] = marker_info->vertex[(7-dir)%4][1]; ppos3d[0][0] = center[0] - width*(ARFloat)0.5; ppos3d[0][1] = center[1] + width*(ARFloat)0.5; ppos3d[1][0] = center[0] + width*(ARFloat)0.5; ppos3d[1][1] = center[1] + width*(ARFloat)0.5; ppos3d[2][0] = center[0] + width*(ARFloat)0.5; ppos3d[2][1] = center[1] - width*(ARFloat)0.5; ppos3d[3][0] = center[0] - width*(ARFloat)0.5; ppos3d[3][1] = center[1] - width*(ARFloat)0.5; for( i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ) { err = arGetTransMat3( rot, ppos2d, ppos3d, 4, conv, arCamera); if( err < AR_GET_TRANS_MAT_MAX_FIT_ERROR ) break; } return err; }
ARFloat Tracker::arGetTransMat(ARMarkerInfo *marker_info, ARFloat center[2], ARFloat width, ARFloat conv[3][4]) { ARFloat rot[3][3]; ARFloat ppos2d[4][2]; ARFloat ppos3d[4][2]; int dir; ARFloat err; int i; if (arGetInitRot(marker_info, arCamera->mat, rot) < 0) { return -1; } dir = marker_info->dir; ppos2d[0][0] = marker_info->vertex[(4 - dir) % 4][0]; ppos2d[0][1] = marker_info->vertex[(4 - dir) % 4][1]; ppos2d[1][0] = marker_info->vertex[(5 - dir) % 4][0]; ppos2d[1][1] = marker_info->vertex[(5 - dir) % 4][1]; ppos2d[2][0] = marker_info->vertex[(6 - dir) % 4][0]; ppos2d[2][1] = marker_info->vertex[(6 - dir) % 4][1]; ppos2d[3][0] = marker_info->vertex[(7 - dir) % 4][0]; ppos2d[3][1] = marker_info->vertex[(7 - dir) % 4][1]; ppos3d[0][0] = center[0] - width * (ARFloat) 0.5; ppos3d[0][1] = center[1] + width * (ARFloat) 0.5; ppos3d[1][0] = center[0] + width * (ARFloat) 0.5; ppos3d[1][1] = center[1] + width * (ARFloat) 0.5; ppos3d[2][0] = center[0] + width * (ARFloat) 0.5; ppos3d[2][1] = center[1] - width * (ARFloat) 0.5; ppos3d[3][0] = center[0] - width * (ARFloat) 0.5; ppos3d[3][1] = center[1] - width * (ARFloat) 0.5; for (i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++) { err = arGetTransMat3(rot, ppos2d, ppos3d, 4, conv, arCamera); if (err < AR_GET_TRANS_MAT_MAX_FIT_ERROR) break; } return err; }
double arGetTransMat( ARMarkerInfo *marker_info, double center[2], double width, double conv[3][4] ) { double rot[3][3]; double ppos2d[4][2]; double ppos3d[4][2]; int dir; double err; int i; if( arGetInitRot( marker_info, arParam.mat, rot ) < 0 ) return -1; dir = marker_info->dir; ppos2d[0][0] = marker_info->vertex[(4-dir)%4][0]; ppos2d[0][1] = marker_info->vertex[(4-dir)%4][1]; ppos2d[1][0] = marker_info->vertex[(5-dir)%4][0]; ppos2d[1][1] = marker_info->vertex[(5-dir)%4][1]; ppos2d[2][0] = marker_info->vertex[(6-dir)%4][0]; ppos2d[2][1] = marker_info->vertex[(6-dir)%4][1]; ppos2d[3][0] = marker_info->vertex[(7-dir)%4][0]; ppos2d[3][1] = marker_info->vertex[(7-dir)%4][1]; ppos3d[0][0] = center[0] - width/2.0; ppos3d[0][1] = center[1] + width/2.0; ppos3d[1][0] = center[0] + width/2.0; ppos3d[1][1] = center[1] + width/2.0; ppos3d[2][0] = center[0] + width/2.0; ppos3d[2][1] = center[1] - width/2.0; ppos3d[3][0] = center[0] - width/2.0; ppos3d[3][1] = center[1] - width/2.0; for( i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ) { err = arGetTransMat3( rot, ppos2d, ppos3d, 4, conv, arParam.dist_factor, arParam.mat ); if( err < AR_GET_TRANS_MAT_MAX_FIT_ERROR ) break; } return err; }
ARFloat Tracker::arMultiGetTransMatHull(ARMarkerInfo *marker_info, int marker_num, ARMultiMarkerInfoT *config) { //return arMultiGetTransMat(marker_info, marker_num, config); int numInPoints=0; std::vector<int> trackedMarkers; int trackedCenterX=-1,trackedCenterY=-1; //const int indices[4] = {idx0,idx1,idx2,idx3}; //rpp_vec ppos2d[4]; //rpp_vec ppos3d[4]; const int maxHullPoints = 16; int indices[maxHullPoints]; rpp_vec ppos2d[maxHullPoints]; rpp_vec ppos3d[maxHullPoints]; // create an array of 2D points and keep references // to the source points // for(int i=0; i<marker_num; i++) { int mId = marker_info[i].id; int configIdx = -1; for(int j=0; j<config->marker_num; j++) if(config->marker[j].patt_id==mId) { configIdx = j; break; } if(configIdx==-1) continue; trackedMarkers.push_back(i); for(int c=0; c<4; c++) { int dir = marker_info[i].dir; int cornerIdx = (c+4-dir)%4; hullInPoints[numInPoints].x = (MarkerPoint::coord_type)marker_info[i].vertex[cornerIdx][0]; hullInPoints[numInPoints].y = (MarkerPoint::coord_type)marker_info[i].vertex[cornerIdx][1]; hullInPoints[numInPoints].cornerIdx = c; hullInPoints[numInPoints].markerIdx = configIdx; numInPoints++; } if(numInPoints>=MAX_HULL_POINTS) break; } // next get the convex hull of all points // (decrease amount by one to ignore last point, which is identical to first point) // int numHullPoints = nearHull_2D(hullInPoints, numInPoints, numInPoints, hullOutPoints)-1; int idx0,idx1,idx2,idx3; if(hullTrackingMode==HULL_FOUR && numHullPoints != 0) { // find those points with furthest distance and that lie on // opposite parts of the hull. this fixes the first two points of our quad. // findLongestDiameter(hullOutPoints, numHullPoints, idx0,idx1); assert(iabs(idx0-idx1)>0); // find the point that is furthest away of the line // of our first two points. this fixes the third point of the quad findFurthestAway(hullOutPoints, numHullPoints, idx0,idx1, idx2); sortIntegers(idx0,idx1, idx2); // of all other points find the one that results in // a quad with the largest area. maximizeArea(hullOutPoints, numHullPoints, idx0,idx1,idx2,idx3); // now that we have all four points we must sort them... // sortInLastInteger(idx0,idx1,idx2, idx3); numHullPoints = 4; indices[0] = idx0; indices[1] = idx1; indices[2] = idx2; indices[3] = idx3; } else { if(numHullPoints>maxHullPoints) numHullPoints = maxHullPoints; for(int i=0; i<numHullPoints; i++) indices[i] = i; } assert(numHullPoints<=maxHullPoints); // create arrays of vertices for the 2D and 3D positions // //const int indices[4] = {idx0,idx1,idx2,idx3}; //rpp_vec ppos2d[4]; //rpp_vec ppos3d[4]; for(int i=0; i<numHullPoints; i++) { //int idx = indices[(i+1)%4]; int idx = indices[i]; const MarkerPoint& pt = hullOutPoints[idx]; trackedCorners.push_back(CornerPoint(pt.x,pt.y)); trackedCenterX += pt.x; trackedCenterY += pt.y; ppos2d[i][0] = pt.x; ppos2d[i][1] = pt.y; ppos2d[i][2] = 1.0f; assert(pt.markerIdx < config->marker_num); const ARMultiEachMarkerInfoT& markerInfo = config->marker[pt.markerIdx]; int cornerIdx = pt.cornerIdx; ppos3d[i][0] = markerInfo.pos3d[cornerIdx][0]; ppos3d[i][1] = markerInfo.pos3d[cornerIdx][1]; ppos3d[i][2] = 0; } trackedCenterX /= 4; trackedCenterY /= 4; // prepare structures and data we need for input and output // parameters of the rpp functions const rpp_float cc[2] = {arCamera->mat[0][2],arCamera->mat[1][2]}; const rpp_float fc[2] = {arCamera->mat[0][0],arCamera->mat[1][1]}; rpp_float err = 1e+20; rpp_mat R, R_init; rpp_vec t; if(poseEstimator==POSE_ESTIMATOR_RPP) { robustPlanarPose(err,R,t,cc,fc,ppos3d,ppos2d,numHullPoints,R_init, true,0,0,0); if(err>1e+10) return(-1); // an actual error has occurred in robustPlanarPose() for(int k=0; k<3; k++) { config->trans[k][3] = (ARFloat)t[k]; for(int j=0; j<3; j++) config->trans[k][j] = (ARFloat)R[k][j]; } } else { ARFloat rot[3][3]; int minIdx=-1, minDist=0x7fffffff; for(size_t i=0; i<trackedMarkers.size(); i++) { assert(trackedMarkers[i]>=0 && trackedMarkers[i]<marker_num); int idx = trackedMarkers[i]; const ARMarkerInfo& mInfo = marker_info[idx]; int dx = trackedCenterX-(int)mInfo.pos[0], dy = trackedCenterY-(int)mInfo.pos[1]; int d = dx*dx + dy*dy; if(d<minDist) { minDist = d; minIdx = (int)idx; } } //trackedCorners.push_back(CornerPoint((int)marker_info[minIdx].pos[0],(int)marker_info[minIdx].pos[1])); if(minIdx >= 0) { return -1; } if(arGetInitRot(marker_info+minIdx, arCamera->mat, rot )<0) return -1; // finally use the normal pose estimator to get the pose // ARFloat tmp_pos2d[maxHullPoints][2], tmp_pos3d[maxHullPoints][2]; for(int i=0; i<numHullPoints; i++) { tmp_pos2d[i][0] = (ARFloat)ppos2d[i][0]; tmp_pos2d[i][1] = (ARFloat)ppos2d[i][1]; tmp_pos3d[i][0] = (ARFloat)ppos3d[i][0]; tmp_pos3d[i][1] = (ARFloat)ppos3d[i][1]; } for(int i=0; i<AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ) { err = arGetTransMat3(rot, tmp_pos2d, tmp_pos3d, numHullPoints, config->trans, arCamera); if(err<AR_GET_TRANS_MAT_MAX_FIT_ERROR) break; } } return (ARFloat)err; }
double arGetTransMat2( double rot[3][3], double ppos2d[][2], double ppos3d[][2], int num, double conv[3][4] ) { return arGetTransMat3( rot, ppos2d, ppos3d, num, conv, arParam.dist_factor, arParam.mat ); }