int NewMapPtsNCC::getSeedsBetween(MapPointList& mapPts, int iCam, int jCam,
Mat_d& seed1, Mat_d& seed2) {
MapPoint* pHead = mapPts.getHead();
if (!pHead)
repErr("SLAM failed - No map point can be found!\n");
std::vector<FeaturePoint*> vecFeatPts1, vecFeatPts2;
for (MapPoint* p = pHead; p; p = p->next) {
//if ((p->iLocalType == TYPE_MAPPOINT_STATIC || p->iLocalType == TYPE_MAPPOINT_DYNAMIC) && p->numVisCam >= 2 && p->pFeatures[iCam] && p->pFeatures[iCam]->f == m_curFrame && p->pFeatures[jCam] && p->pFeatures[jCam]->f == m_curFrame) {
if (!p->isFalse() && !p->isUncertain() && p->numVisCam >= 2
&& p->pFeatures[iCam] && p->pFeatures[iCam]->f == m_curFrame
&& p->pFeatures[jCam] && p->pFeatures[jCam]->f == m_curFrame) {
vecFeatPts1.push_back(p->pFeatures[iCam]);
vecFeatPts2.push_back(p->pFeatures[jCam]);
}
}
if (vecFeatPts1.empty())
return 0;
seed1.resize(vecFeatPts1.size(), 2);
seed2.resize(vecFeatPts2.size(), 2);
for (size_t i = 0; i < vecFeatPts1.size(); i++) {
seed1.data[2 * i] = vecFeatPts1[i]->x;
seed1.data[2 * i + 1] = vecFeatPts1[i]->y;
seed2.data[2 * i] = vecFeatPts2[i]->x;
seed2.data[2 * i + 1] = vecFeatPts2[i]->y;
}
return seed1.rows;
}
void NewMapPts::decidePointType() {
using namespace std;
assert(pCoSLAM);
const int hw = 20;
//generate dynamic masks
int numCams = pCoSLAM->numCams;
Mat_uc mask[SLAM_MAX_NUM];
for (int c = 0; c < numCams; c++) {
int camId = m_camGroup.camIds[c];
mask[camId].resize(pCoSLAM->slam[camId].H, pCoSLAM->slam[camId].W);
mask[camId].fill(0);
}
for (int c = 0; c < numCams; c++) {
int camId = m_camGroup.camIds[c];
vector<FeaturePoint*> vecDynMapPts;
pCoSLAM->slam[camId].getMappedDynPts(vecDynMapPts);
int W = mask[camId].cols;
int H = mask[camId].rows;
for (size_t i = 0; i < vecDynMapPts.size(); i++) {
FeaturePoint* fp = vecDynMapPts[i];
int x = static_cast<int>(fp->x + 0.5);
int y = static_cast<int>(fp->y + 0.5);
for (int s = -hw; s <= hw; s++) {
for (int t = -hw; t <= hw; t++) {
int x1 = x + t;
int y1 = y + s;
if (x1 < 0 || x1 >= W || y1 < 0 || y1 >= H)
continue;
mask[camId].data[y1 * W + x1] = 1;
}
}
}
}
for (size_t i = 0; i < newMapPts.size(); i++) {
MapPoint* mpt = newMapPts[i];
if (mpt->isUncertain()) {
bool isStatic = true;
for (int c = 0; c < numCams; c++) {
int camId = m_camGroup.camIds[c];
int W = mask[camId].cols;
int H = mask[camId].rows;
FeaturePoint* fp = mpt->pFeatures[camId];
if (fp) {
int x = static_cast<int>(fp->x + 0.5);
int y = static_cast<int>(fp->y + 0.5);
if (x >= 0 && x < W && y >= 0 && y < H) {
if (mask[camId][y * W + x] > 0) {
isStatic = false;
break;
}
}
}
}
if (isStatic) {
mpt->setLocalStatic();
}
}
}
}
