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