Relocaliser.cc

Go to the documentation of this file.

// Copyright 2008 Isis Innovation Limited
#include "ptam/Relocaliser.h"
#include "ptam/SmallBlurryImage.h"
#include <cvd/utility.h>
//#include <gvars3/instances.h>

using namespace CVD;
using namespace std;
//using namespace GVars3;

Relocaliser::Relocaliser(Map &map, ATANCamera &camera)
: mMap(map),
  mCamera(camera)
{
};

SE3<> Relocaliser::BestPose()
{
  return mse3Best;
}

bool Relocaliser::AttemptRecovery(KeyFrame& kCurrent)
{
  // Ensure the incoming frame has a SmallBlurryImage attached
  if(!kCurrent.pSBI)
    kCurrent.pSBI = new SmallBlurryImage(kCurrent);
  else
    kCurrent.pSBI->MakeFromKF(kCurrent);

  // Find the best ZMSSD match from all keyframes in map
  ScoreKFs(kCurrent);

  // And estimate a camera rotation from a 3DOF image alignment
  pair<SE2<>, double> result_pair = kCurrent.pSBI->IteratePosRelToTarget(*mMap.vpKeyFrames[mnBest]->pSBI, 6);
  mse2 = result_pair.first;
  double dScore =result_pair.second;

  SE3<> se3KeyFramePos = mMap.vpKeyFrames[mnBest]->se3CfromW;
  mse3Best = SmallBlurryImage::SE3fromSE2(mse2, mCamera) * se3KeyFramePos;

  //Weiss{
  
  const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
  //if(dScore < GV2.GetDouble("Reloc2.MaxScore", 9e6, SILENT))
  if(dScore < pPars.RelocMaxScore)
    return true;
  else 
    return false;
  //}
};

// Compare current KF to all KFs stored in map by
// Zero-mean SSD
void Relocaliser::ScoreKFs(KeyFrame& kCurrent)
{
  mdBestScore = 99999999999999.9;
  mnBest = -1;

  for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
  {
    double dSSD = kCurrent.pSBI->ZMSSD(*mMap.vpKeyFrames[i]->pSBI);
    if(dSSD < mdBestScore)
    {
      mdBestScore = dSSD;
      mnBest = i;
    }
  }
}