帮我改写下面的函数,功能相同,但不要使用递归的方法bool OtsTrack::BackTrackForArray(int startSearchingID, int *inner_id, std::vector<int> &array_temp,
std::vector<int> &inner_temp, double **distancesOfAllPoints, VecPoint3D &pts3d_, VecPoint3D &arrayPoints,
int &inner_pointer, std::vector<int> &inner_id_in_array, std::vector<int> &indexes)
{
std::string realArrayID = "";
int pointsSize = -1;
int virtualID = array_temp[indexes[0]];
if (virtualID != -1)
{
int virtualArrayID = m_pointsMapArrayID[virtualID];
realArrayID = m_arrayMarkerIds[virtualArrayID];
pointsSize = m_mapToolSetting.at(realArrayID).FaceData[m_pointsMapFaceID[virtualID]].FaceMarkerNum;
if (inner_pointer >= pointsSize)
return true;
}
// find a point
Point3D p1;
int index = 0;
// the the distance between first point and current point
double distance = 0.0;
for (size_t i = startSearchingID; i < pts3d_.size(); i++)
{
bool hasSameIndex = false;
for (auto in : indexes)
if (in == i)
hasSameIndex = true;
if (hasSameIndex || array_temp[i] != -1)
continue;
p1 = pts3d_[i];
index = i;
if (fabs(distancesOfAllPoints[indexes[0]][i]) < 1e-5)
{
distance = (arrayPoints[0] - p1).Norm2();
distancesOfAllPoints[indexes[0]][i] = distance;
}
else
distance = distancesOfAllPoints[indexes[0]][i];
if (distance > m_fMaxRefDistance)
{
continue;
}
// set
indexes.emplace_back(index);
arrayPoints.emplace_back(p1);
if (inner_pointer == 1)
{
std::vector<int> spareArray;
std::vector<int> inner_wrongConfirmed_id_1; //<0,1,2,3
std::vector<int> inner_wrongConfirmed_id_2;
for (int k = 0; k < m_pointsMapArrayID.size(); k++)
{
for (int j1 = 0; j1 < m_AllRefDistances[k].size(); j1++)
for (int j2 = j1 + 1; j2 < m_AllRefDistances[k].size(); j2++)
{
double diff_12 = fabs(m_AllRefDistances[k][j1][j2] - distance);
if (diff_12 < m_OTSParam.fMatchingDistanceThreshold)
{ // less than m_OTSParam.fMatchingDistanceThreshold mm
spareArray.emplace_back(k);
inner_wrongConfirmed_id_1.emplace_back(j1);
inner_wrongConfirmed_id_2.emplace_back(j2);
}
}
}
for (size_t h = 0; h < spareArray.size(); h++)
{
//<first and second point
// set
inner_id[0] = inner_wrongConfirmed_id_1[h];
inner_id[1] = inner_wrongConfirmed_id_2[h];
array_temp[indexes[0]] = spareArray[h];
array_temp[indexes[inner_pointer++]] = spareArray[h];
if (BackTrackForArray(i + 1, inner_id, array_temp, inner_temp, distancesOfAllPoints, pts3d_,
arrayPoints, inner_pointer, inner_id_in_array, indexes))
{
return true;
}
// resume
inner_id[0] = -1;
inner_id[1] = -1;
array_temp[indexes[0]] = -1;
array_temp[indexes[--inner_pointer]] = -1;
}
// resume
arrayPoints.pop_back();
indexes.pop_back();
}
// confirm the inner id
else if (inner_pointer == 2)
{
std::vector<int> inner_wrongConfirmed_id_1; //<0,1,2,3
std::vector<int> numberofInner;
std::vector<int> inner_wrongConfirmed_id_2;
for (int q = 0; q < 2; q++)
{
for (int j2 = 0; j2 < pointsSize; j2++)
{
bool haveSameInnerId = false;
for (int ind = 0; ind < inner_pointer; ind++)
{
if (inner_id[ind] == j2)
{
haveSameInnerId = true;
break;
}
}
if (haveSameInnerId) //
continue;
double diff_13 = fabs(m_AllRefDistances[virtualID][inner_id[q]][j2] - distance);
if (diff_13 < m_OTSParam.fMatchingDistanceThreshold)
{
numberofInner.emplace_back(q);
inner_wrongConfirmed_id_1.emplace_back(inner_id[q]);
inner_wrongConfirmed_id_2.emplace_back(j2);
}
}
}
if (numberofInner.empty())
{
arrayPoints.pop_back();
indexes.pop_back();
continue;
}
// bool matched = true;
Point3D p2 = arrayPoints[1]; //<second point
double distance_p1p2 = 0.0;
if (fabs(distancesOfAllPoints[indexes[1]][index]) < 1e-5)
{
distance_p1p2 = (p2 - p1).Norm2();
distancesOfAllPoints[indexes[1]][index] = distance_p1p2;
}
else
distance_p1p2 = distancesOfAllPoints[indexes[1]][index];
for (size_t h = 0; h < numberofInner.size(); h++)
{
int index_array_2 = numberofInner[h] == 0 ? 1 : 0;
double diff_23 =
fabs(m_AllRefDistances[virtualID][inner_id[index_array_2]][inner_wrongConfirmed_id_2[h]] -
distance_p1p2);
if (diff_23 < m_OTSParam.fMatchingDistanceThreshold)
{
inner_id_in_array.emplace_back(inner_id[numberofInner[h]]); // A
inner_id_in_array.emplace_back(inner_id[index_array_2]); // B
inner_id_in_array.emplace_back(inner_wrongConfirmed_id_2[h]);
inner_temp[index] = inner_wrongConfirmed_id_2[h];
inner_id[inner_pointer++] = inner_wrongConfirmed_id_2[h];
array_temp[index] = virtualID;
inner_temp[indexes[0]] = inner_id_in_array[0]; // first point
inner_temp[indexes[1]] = inner_id_in_array[1]; // second point
}
else
continue;
bool angleMatched = AngleMatching(arrayPoints, inner_id_in_array, pointsSize, virtualID);
if (!angleMatched)
{
// resume
inner_temp[indexes[1]] = -1;
inner_temp[indexes[0]] = -1;
inner_temp[index] = -1;
inner_id[--inner_pointer] = -1;
array_temp[index] = -1;
inner_id_in_array.pop_back();
inner_id_in_array.pop_back();
inner_id_in_array.pop_back();
// arrayPoints.pop_back();
// indexes.pop_back();
continue;
}
if (BackTrackForArray(i + 1, inner_id, array_temp, inner_temp, distancesOfAllPoints, pts3d_,
arrayPoints, inner_pointer, inner_id_in_array, indexes))
{
return true;
}
// resume
inner_temp[indexes[1]] = -1;
inner_temp[indexes[0]] = -1;
inner_temp[index] = -1;
inner_id[--inner_pointer] = -1;
array_temp[index] = -1;
inner_id_in_array.pop_back();
inner_id_in_array.pop_back();
inner_id_in_array.pop_back();
}
arrayPoints.pop_back();
indexes.pop_back();
}
else if (inner_pointer >= 3)
{
// if(inner_temp[indexes[0]] == -1){
// return false;
// }
for (int j2 = 0; j2 < pointsSize; j2++)
{
bool haveSameInnerId = false;
for (int ind = 0; ind < inner_pointer; ind++)
{
if (inner_id_in_array[ind] == j2)
{
haveSameInnerId = true;
break;
}
}
if (haveSameInnerId) //
continue;
double diff = fabs(m_AllRefDistances[virtualID][inner_temp[indexes[0]]][j2] - distance);
if (diff < m_OTSParam.fMatchingDistanceThreshold)
{
//< third point
// set
inner_id[inner_pointer++] = j2; //<inner_pointer = 4
inner_temp[index] = j2;
array_temp[index] = virtualID;
inner_id_in_array.emplace_back(j2); // D
break;
}
}
if (inner_pointer < arrayPoints.size())
{
// this is no any operation on inner_id, inner_temp,inner_id_in_array
arrayPoints.pop_back();
indexes.pop_back();
continue;
}
// cross validation
bool matched = true;
for (int j = 1; j < inner_pointer - 1; j++)
{
Point3D p2 = arrayPoints[j];
double distance_p1p2 = 0.0;
if (fabs(distancesOfAllPoints[indexes[j]][index]) < 1e-5)
{
distance_p1p2 = (p2 - p1).Norm2();
distancesOfAllPoints[indexes[j]][index] = distance_p1p2;
}
else
distance_p1p2 = distancesOfAllPoints[indexes[j]][index];
double diff =
fabs(m_AllRefDistances[virtualID][inner_temp[indexes[j]]][inner_temp[index]] - distance_p1p2);
if (diff > m_OTSParam.fMatchingDistanceThreshold)
{
matched = false;
break;
}
}
// resume
if (!matched)
{
//< recover the data
inner_pointer--; // inner_pointer = 3
/*
As for disambiguity and array matching, there is no need to match every edges and angles.
*/
inner_id[inner_pointer] = -1;
inner_temp[index] = -1;
array_temp[index] = -1;
inner_id_in_array.pop_back(); // D
arrayPoints.pop_back();
indexes.pop_back();
continue;
}
if (BackTrackForArray(i + 1, inner_id, array_temp, inner_temp, distancesOfAllPoints, pts3d_, arrayPoints,
inner_pointer, inner_id_in_array, indexes))
{
return true;
}
// resume
inner_pointer--;
inner_id[inner_pointer] = -1;
inner_temp[index] = -1;
array_temp[index] = -1;
inner_id_in_array.pop_back();
arrayPoints.pop_back();
indexes.pop_back();
}
}
if (arrayPoints.size() >= 3 && !realArrayID.empty())
{
if (inner_pointer >= m_mapToolSetting.at(realArrayID).MinFaceMarkerNum)
return true;
}
return false;
}
最新发布