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::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] }