KeyPose* SingleSLAM::addKeyPose(bool bSelfMotion) {
KeyPose* pose = 0;
CamPoseItem* cam = m_camPos.current();
pose = m_keyPose.add(currentFrame(), cam);
pose->setNumMappedPoints(m_nMappedStaticPts);
//test (for debugging group merge)
//pose->setImage(m_img);
pose->setSmallImage(m_smallImg, m_smallScale);
pose->setCameraIntrinsic(K.data);
pose->setFeatPoints(m_featPts.getFrameHead(currentFrame()),
m_featPts.getFrameTail(currentFrame()));
if (bSelfMotion) {
pose->bSelfMotion = true;
m_selfKeyPose.push_back(pose);
m_prevKeyPos = m_lastKeyPos;
m_lastKeyPos = pose;
}
for (FeaturePoint* fp = pose->pHead; fp != pose->pTail->next;
fp = fp->next) {
fp->bKeyFrm = true;
}
return pose;
}
void Relocalizer::cacheNewKeyPose(SingleSLAM& slam){
KeyPose* pose = 0;
CamPoseItem* cam = slam.m_camPos.current();
pose = new KeyPose(slam.currentFrame(), cam);
pose->setNumMappedPoints(slam.m_nMappedStaticPts);
pose->setImage(slam.m_img);
pose->setSmallImage(slam.m_smallImg, slam.m_smallScale);
pose->setThumbImage(slam.m_smallImg, slam.maxThumbW);
pose->setCameraIntrinsic(slam.K.data);
pose->setFeatPoints(slam.m_featPts.getFrameHead(slam.currentFrame()),
slam.m_featPts.getFrameTail(slam.currentFrame()));
if( _pastNKeyPoses.num < RELOC_MAX_CACHE_FRAME)
_pastNKeyPoses.push_back(pose);
else{
delete _pastNKeyPoses.pop_front();
_pastNKeyPoses.push_back(pose);
}
}
