void getValidRowsCols(const Mat_d& distMat, double invalidVal, Mat_c& rowFlag,
Mat_c& colFlag) {
int m = distMat.rows;
int n = distMat.cols;
rowFlag.resize(m, 1);
colFlag.resize(n, 1);
for (int i = 0; i < m; i++) {
bool allNone = true;
for (int j = 0; j < n; j++) {
if (distMat.data[i * n + j] != invalidVal) {
allNone = false;
break;
}
}
rowFlag.data[i] = allNone ? 0 : 1;
}
for (int j = 0; j < n; j++) {
bool allNone = true;
for (int i = 0; i < m; i++) {
if (distMat.data[i * n + j] != invalidVal) {
allNone = false;
break;
}
}
colFlag.data[j] = allNone ? 0 : 1;
}
}
void TracksToMeasurments(const double invK[][9], std::vector<const Track2D*> vecPTracks, int nviews, Mat_d& meas, Mat_c& vmask) {
int nTrack = vecPTracks.size();
int nMeas = 0;
for (int i = 0; i < nTrack; i++) {
nMeas += vecPTracks[i]->length();
}
meas.resize(nMeas, 2);
vmask.resize(nTrack, nviews);
vmask.fill(0);
double tmpPt[2];
int k = 0;
for (int i = 0; i < nTrack; i++) {
Track2DNode* p = vecPTracks[i]->head.next;
Track2DNode* tail = vecPTracks[i]->tail->next;
while (p != tail) {
int f = p->f;
tmpPt[0] = p->x;
tmpPt[1] = p->y;
normPoint(invK[f], tmpPt, meas + 2 * k);
vmask.data[i * nviews + f] = true;
k++;
p = p->next;
}
}
}
