Relocaliser.cc
Go to the documentation of this file.
00001 // Copyright 2008 Isis Innovation Limited
00002 #include "ptam/Relocaliser.h"
00003 #include "ptam/SmallBlurryImage.h"
00004 #include <cvd/utility.h>
00005 //#include <gvars3/instances.h>
00006 
00007 using namespace CVD;
00008 using namespace std;
00009 //using namespace GVars3;
00010 
00011 Relocaliser::Relocaliser(Map &map, ATANCamera &camera)
00012 : mMap(map),
00013   mCamera(camera)
00014 {
00015 };
00016 
00017 SE3<> Relocaliser::BestPose()
00018 {
00019   return mse3Best;
00020 }
00021 
00022 bool Relocaliser::AttemptRecovery(KeyFrame& kCurrent)
00023 {
00024   // Ensure the incoming frame has a SmallBlurryImage attached
00025   if(!kCurrent.pSBI)
00026     kCurrent.pSBI = new SmallBlurryImage(kCurrent);
00027   else
00028     kCurrent.pSBI->MakeFromKF(kCurrent);
00029 
00030   // Find the best ZMSSD match from all keyframes in map
00031   ScoreKFs(kCurrent);
00032 
00033   // And estimate a camera rotation from a 3DOF image alignment
00034   pair<SE2<>, double> result_pair = kCurrent.pSBI->IteratePosRelToTarget(*mMap.vpKeyFrames[mnBest]->pSBI, 6);
00035   mse2 = result_pair.first;
00036   double dScore =result_pair.second;
00037 
00038   SE3<> se3KeyFramePos = mMap.vpKeyFrames[mnBest]->se3CfromW;
00039   mse3Best = SmallBlurryImage::SE3fromSE2(mse2, mCamera) * se3KeyFramePos;
00040 
00041   //Weiss{
00042   
00043   const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
00044   //if(dScore < GV2.GetDouble("Reloc2.MaxScore", 9e6, SILENT))
00045   if(dScore < pPars.RelocMaxScore)
00046     return true;
00047   else 
00048     return false;
00049   //}
00050 };
00051 
00052 // Compare current KF to all KFs stored in map by
00053 // Zero-mean SSD
00054 void Relocaliser::ScoreKFs(KeyFrame& kCurrent)
00055 {
00056   mdBestScore = 99999999999999.9;
00057   mnBest = -1;
00058 
00059   for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
00060   {
00061     double dSSD = kCurrent.pSBI->ZMSSD(*mMap.vpKeyFrames[i]->pSBI);
00062     if(dSSD < mdBestScore)
00063     {
00064       mdBestScore = dSSD;
00065       mnBest = i;
00066     }
00067   }
00068 }
00069 


ptam
Author(s): Markus Achtelik , Stephan Weiss , Simon Lynen
autogenerated on Sun Oct 5 2014 23:52:33