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