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); } }