ARFloat Tracker::rppMultiGetTransMat(ARMarkerInfo *marker_info, int marker_num, ARMultiMarkerInfoT *config) { rpp_float err = 1e+20; rpp_mat R, R_init; rpp_vec t; std::map<int, int> marker_id_freq; for (int i = 0; i < marker_num; i++) { const int m_patt_id = marker_info[i].id; if (m_patt_id >= 0) { std::map<int, int>::iterator iter = marker_id_freq.find(m_patt_id); if (iter == marker_id_freq.end()) { marker_id_freq.insert(std::make_pair(int(m_patt_id), 1)); } else { ((*iter).second)++; } } } std::deque<std::pair<int, int> > config_patt_id; for (int j = 0; j < config->marker_num; j++) config_patt_id.push_back(std::make_pair(int(j), int(config->marker[j].patt_id))); std::map<int, int> m2c_idx; for (int m = 0; m < marker_num; m++) { const int m_patt_id = marker_info[m].id; bool ignore_marker = (m_patt_id < 0); std::map<int, int>::iterator m_iter = marker_id_freq.find(m_patt_id); if (m_iter != marker_id_freq.end()) ignore_marker |= ((*m_iter).second > 1); if (!ignore_marker) { std::deque<std::pair<int, int> >::iterator c_iter = config_patt_id.begin(); if (c_iter != config_patt_id.end()) do { const int patt_id = (*c_iter).second; if (marker_info[m].id == patt_id) { m2c_idx.insert(std::make_pair(int(m), int((*c_iter).first))); config_patt_id.erase(c_iter); c_iter = config_patt_id.end(); continue; } else { c_iter++; } } while (c_iter != config_patt_id.end()); } } // ---------------------------------------------------------------------- const unsigned int n_markers = (unsigned int) m2c_idx.size(); const unsigned int n_pts = 4 * n_markers; if (n_markers == 0) return (-1); rpp_vec *ppos2d = NULL, *ppos3d = NULL; arMalloc( ppos2d, rpp_vec, n_pts); arMalloc( ppos3d, rpp_vec, n_pts); memset(ppos2d, 0, sizeof(rpp_vec) * n_pts); memset(ppos3d, 0, sizeof(rpp_vec) * n_pts); const rpp_float iprts_z = 1; int p = 0; for (std::map<int, int>::iterator iter = m2c_idx.begin(); iter != m2c_idx.end(); iter++) { const int m = (*iter).first; const int c = (*iter).second; const int dir = marker_info[m].dir; const int v_idx[4] = { (4 - dir) % 4, (5 - dir) % 4, (6 - dir) % 4, (7 - dir) % 4 }; for (int i = 0; i < 4; i++) for (int j = 0; j < 3; j++) { ppos2d[p + i][j] = (rpp_float) (j == 2 ? iprts_z : marker_info[m].vertex[v_idx[i]][j]); ppos3d[p + i][j] = (rpp_float) config->marker[c].pos3d[i][j]; } p += 4; } 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] }; robustPlanarPose(err, R, t, cc, fc, ppos3d, ppos2d, n_pts, R_init, true, 0, 0, 0); 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]; } if (ppos2d != NULL) free(ppos2d); if (ppos3d != NULL) free(ppos3d); if (err > 1e+10) return (-1); // an actual error has occurred in robustPlanarPose() return (ARFloat(err)); // NOTE: err is a real number from the interval [0,1e+10] }
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; }
ARFloat Tracker::rppGetTransMat(ARMarkerInfo *marker_info, ARFloat center[2], ARFloat width, ARFloat conv[3][4]) { const bool initial_estimate_with_arGetInitRot = false; // only for testing rpp_float err = 1e+20; rpp_mat R, R_init; rpp_vec t; if(initial_estimate_with_arGetInitRot ) { ARFloat rot[3][3]; if( arGetInitRot( marker_info, arCamera->mat, rot ) < 0 ) return -1; for(int i=0; i<3; i++) for(int j=0; j<3; j++) R_init[i][j] = (rpp_float)rot[i][j]; } int dir = marker_info->dir; rpp_vec ppos2d[4]; rpp_vec ppos3d[4]; const unsigned int n_pts = 4; const rpp_float model_z = 0; const rpp_float iprts_z = 1; ppos2d[0][0] = marker_info->vertex[(4-dir)%4][0]; ppos2d[0][1] = marker_info->vertex[(4-dir)%4][1]; ppos2d[0][2] = iprts_z; ppos2d[1][0] = marker_info->vertex[(5-dir)%4][0]; ppos2d[1][1] = marker_info->vertex[(5-dir)%4][1]; ppos2d[1][2] = iprts_z; ppos2d[2][0] = marker_info->vertex[(6-dir)%4][0]; ppos2d[2][1] = marker_info->vertex[(6-dir)%4][1]; ppos2d[2][2] = iprts_z; ppos2d[3][0] = marker_info->vertex[(7-dir)%4][0]; ppos2d[3][1] = marker_info->vertex[(7-dir)%4][1]; ppos2d[3][2] = iprts_z; ppos3d[0][0] = center[0] - width*(ARFloat)0.5; ppos3d[0][1] = center[1] + width*(ARFloat)0.5; ppos3d[0][2] = model_z; ppos3d[1][0] = center[0] + width*(ARFloat)0.5; ppos3d[1][1] = center[1] + width*(ARFloat)0.5; ppos3d[1][2] = model_z; ppos3d[2][0] = center[0] + width*(ARFloat)0.5; ppos3d[2][1] = center[1] - width*(ARFloat)0.5; ppos3d[2][2] = model_z; ppos3d[3][0] = center[0] - width*(ARFloat)0.5; ppos3d[3][1] = center[1] - width*(ARFloat)0.5; ppos3d[3][2] = model_z; 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]}; robustPlanarPose(err,R,t,cc,fc,ppos3d,ppos2d,n_pts,R_init, !initial_estimate_with_arGetInitRot,0,0,0); for(int i=0; i<3; i++) { conv[i][3] = (ARFloat)t[i]; for(int j=0; j<3; j++) conv[i][j] = (ARFloat)R[i][j]; } if(err > 1e+10) return(-1); // an actual error has occurred in robustPlanarPose() return(ARFloat(err)); // NOTE: err is a real number from the interval [0,1e+10] }