MapMaker.cc

Go to the documentation of this file.

// Copyright 2008 Isis Innovation Limited
#include "ptam/MapMaker.h"
#include "ptam/MapPoint.h"
#include "ptam/Bundle.h"
#include "ptam/PatchFinder.h"
#include "ptam/SmallMatrixOpts.h"
#include "ptam/HomographyInit.h"

#include <cvd/vector_image_ref.h>
#include <cvd/vision.h>
#include <cvd/image_interpolate.h>

#include <TooN/SVD.h>
#include <TooN/SymEigen.h>

#include <gvars3/instances.h>
#include <fstream>
#include <algorithm>

#include <ptam/Params.h>

#ifdef WIN32
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#endif

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

//Weiss{ feature statistics
//static unsigned int outcount[5]={0,0,0,0,0};
//static unsigned int addcount[5]={0,0,0,0,0};
//static unsigned int kfid=0;
//}

// Constructor sets up internal reference variable to Map.
// Most of the intialisation is done by Reset()..
MapMaker::MapMaker(Map& m, const ATANCamera &cam, ros::NodeHandle& nh)
: mMap(m), mCamera(cam), octomap_interface(nh)
{
  mbResetRequested = false;
  Reset();
  start(); // This CVD::thread func starts the map-maker thread with function run()
  GUI.RegisterCommand("SaveMap", GUICommandCallBack, this);
  //GV3::Register(mgvdWiggleScale, "MapMaker.WiggleScale", 0.1, SILENT); // Default to 10cm between keyframes
};


void MapMaker::Reset()
{
  // This is only called from within the mapmaker thread...
  mMap.Reset();
  mvFailureQueue.clear();
  while(!mqNewQueue.empty()) mqNewQueue.pop();
  mMap.vpKeyFrames.clear(); // TODO: actually erase old keyframes
  mvpKeyFrameQueue.clear(); // TODO: actually erase old keyframes
  mbBundleRunning = false;
  mbBundleConverged_Full = true;
  mbBundleConverged_Recent = true;
  mbResetDone = true;
  mbResetRequested = false;
  mbBundleAbortRequested = false;
}

// CHECK_RESET is a handy macro which makes the mapmaker thread stop
// what it's doing and reset, if required.
#define CHECK_RESET if(mbResetRequested) {Reset(); continue;};

void MapMaker::run()
{

#ifdef WIN32
  // For some reason, I get tracker thread starvation on Win32 when
  // adding key-frames. Perhaps this will help:
  SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_LOWEST);
#endif

  
  const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();

  while(!shouldStop())  // ShouldStop is a CVD::Thread func which return true if the thread is told to exit.
  {
    CHECK_RESET;
    sleep(1); // Sleep not really necessary, especially if mapmaker is busy
    CHECK_RESET;

    // Handle any GUI commands encountered..
    while(!mvQueuedCommands.empty())
    {
      GUICommandHandler(mvQueuedCommands.begin()->sCommand, mvQueuedCommands.begin()->sParams);
      mvQueuedCommands.erase(mvQueuedCommands.begin());
    }

    if(!mMap.IsGood())  // Nothing to do if there is no map yet!
      continue;

    // From here on, mapmaker does various map-maintenance jobs in a certain priority
    // Hierarchy. For example, if there's a new key-frame to be added (QueueSize() is >0)
    // then that takes high priority.

    CHECK_RESET;
    // Should we run local bundle adjustment?
    if(!mbBundleConverged_Recent && QueueSize() == 0)
      //Weiss{
      if (pPars.BundleMethod=="LOCAL" || pPars.BundleMethod=="LOCAL_GLOBAL")
        BundleAdjustRecent();
    //}

    CHECK_RESET;
    // Are there any newly-made map points which need more measurements from older key-frames?
    if(mbBundleConverged_Recent && QueueSize() == 0)
      ReFindNewlyMade();

    CHECK_RESET;
    // Run global bundle adjustment?
    if(mbBundleConverged_Recent && !mbBundleConverged_Full && QueueSize() == 0)
      //Weiss{
      if (pPars.BundleMethod=="GLOBAL" || pPars.BundleMethod=="LOCAL_GLOBAL")
        BundleAdjustAll();

    //}

    CHECK_RESET;
    // Very low priorty: re-find measurements marked as outliers
    if(mbBundleConverged_Recent && mbBundleConverged_Full && rand()%20 == 0 && QueueSize() == 0)
      ReFindFromFailureQueue();

    CHECK_RESET;
    HandleBadPoints();

    CHECK_RESET;
    // Any new key-frames to be added?
    if(QueueSize() > 0)
      AddKeyFrameFromTopOfQueue(); // Integrate into map data struct, and process
  }
}


// Tracker calls this to demand a reset
void MapMaker::RequestReset()
{
  mbResetDone = false;
  mbResetRequested = true;
}

bool MapMaker::ResetDone()
{
  return mbResetDone;
}

// HandleBadPoints() Does some heuristic checks on all points in the map to see if 
// they should be flagged as bad, based on tracker feedback.
void MapMaker::HandleBadPoints()
{
  // Did the tracker see this point as an outlier more often than as an inlier?
  for(unsigned int i=0; i<mMap.vpPoints.size(); i++)
  {
    MapPoint &p = *mMap.vpPoints[i];
    if(p.nMEstimatorOutlierCount > 20 && p.nMEstimatorOutlierCount > p.nMEstimatorInlierCount)
      p.bBad = true;
  }

  // All points marked as bad will be erased - erase all records of them
  // from keyframes in which they might have been measured.
  for(unsigned int i=0; i<mMap.vpPoints.size(); i++)
    if(mMap.vpPoints[i]->bBad)
    {
      //slynen octomap_interface{
      octomap_interface.deletePoint(mMap.vpPoints[i]);
      //}
      MapPoint::Ptr p = mMap.vpPoints[i];
      for(unsigned int j=0; j<mMap.vpKeyFrames.size(); j++)
      {
        KeyFrame &k = *mMap.vpKeyFrames[j];
        if(k.mMeasurements.count(p)){
          //slynen{ reprojection
#ifdef KF_REPROJ
          //if the mapmaker wants to move a point to the trash which we use for keyframe reproj. we select a new point
          for(unsigned int ibest=0;ibest<k.iBestPointsCount;ibest++){ //check all best points
            //slynen copy this
            if(k.apCurrentBestPoints[ibest]==p) //is the point out of range or to be deleted one of our bestPoints?
            {
              k.apCurrentBestPoints[ibest].reset();
              int mindist = 9999;
              int tempdist = 0;
              Vector<2> ptlev0 = LevelZeroPos(p->irCenter, p->nSourceLevel); //to level zero coords
              for(int iP = k.vpPoints.size()-1; iP>=0; iP--){ //search a nearby point as new bestPoint
                Vector<2> pt2lev0 = LevelZeroPos(k.vpPoints[iP]->irCenter, k.vpPoints[iP]->nSourceLevel);
                tempdist = abs(ptlev0[0] - pt2lev0[0]) + abs(ptlev0[1] - pt2lev0[1]);   //calc simple dist
                if(tempdist < mindist && ! k.vpPoints[iP]->bBad){                                       //if closer and not bad
                  mindist = tempdist;
                  k.apCurrentBestPoints[ibest] = k.vpPoints[iP]; //closest point as new point
                }
              }
            }
          }
#endif
          //}
          k.mMeasurements.erase(p);
        }
      }
    }
  // Move bad points to the trash list.
  mMap.MoveBadPointsToTrash();
}

MapMaker::~MapMaker()
{
  mbBundleAbortRequested = true;
  stop(); // makes shouldStop() return true
  cout << "Waiting for mapmaker to die.." << endl;
  join();
  cout << " .. mapmaker has died." << endl;
}


// Finds 3d coords of point in reference frame B from two z=1 plane projections
Vector<3> MapMaker::ReprojectPoint(SE3<> se3AfromB, const Vector<2> &v2A, const Vector<2> &v2B)
{
  Matrix<3,4> PDash;
  PDash.slice<0,0,3,3>() = se3AfromB.get_rotation().get_matrix();
  PDash.slice<0,3,3,1>() = se3AfromB.get_translation().as_col();

  Matrix<4> A;
  A[0][0] = -1.0; A[0][1] =  0.0; A[0][2] = v2B[0]; A[0][3] = 0.0;
  A[1][0] =  0.0; A[1][1] = -1.0; A[1][2] = v2B[1]; A[1][3] = 0.0;
  A[2] = v2A[0] * PDash[2] - PDash[0];
  A[3] = v2A[1] * PDash[2] - PDash[1];

  SVD<4,4> svd(A);
  Vector<4> v4Smallest = svd.get_VT()[3];
  if(v4Smallest[3] == 0.0)
    v4Smallest[3] = 0.00001;
  return project(v4Smallest);
}



// InitFromStereo() generates the initial match from two keyframes
// and a vector of image correspondences. Uses the 
bool MapMaker::InitFromStereo(KeyFrame::Ptr kF,
                              KeyFrame::Ptr kS,
                              vector<pair<ImageRef, ImageRef> > &vTrailMatches,
                              SE3<> &se3TrackerPose)
{
  //Weiss{
  //mdWiggleScale = 0.1;
  const FixParams& pPars = PtamParameters::fixparams();
  mdWiggleScale=pPars.WiggleScale;
  //mdWiggleScale = *mgvdWiggleScale; // Cache this for the new map.
  //}

  mCamera.SetImageSize(kF->aLevels[0].im.size());

//Weiss{
        if(vTrailMatches.size()<4)
        {
                ROS_WARN_STREAM("Too few matches to init.");
                return false;
        }


  //    vector<HomographyMatch> vMatches;
  //    TooN::Matrix<> X(vTrailMatches.size(), 3);
  //    SO3<> rotmat = SO3<>::exp(rotvec);
  //
  //    for(unsigned int i=0; i<vTrailMatches.size(); i++)
  //    {
  //            HomographyMatch m;
  //            m.v2CamPlaneFirst = mCamera.UnProject(vTrailMatches[i].first);
  //            m.v2CamPlaneSecond = mCamera.UnProject(vTrailMatches[i].second);
  //            Vector<3> p1 = makeVector(m.v2CamPlaneFirst[0],m.v2CamPlaneFirst[1],1);
  //            p1=p1/norm(p1);
  //            Vector<3> p2 = rotmat.inverse()*makeVector(m.v2CamPlaneSecond[0],m.v2CamPlaneSecond[1],1);
  //            p2=p2/norm(p2);
  //            Vector<3> of= p2-p1;
  //
  //            X[i]=makeVector(of[2]*p1[1]-of[1]*p1[2], of[0]*p1[2]-of[2]*p1[0], of[1]*p1[0]-of[0]*p1[1]);
  //
  //            vMatches.push_back(m);
  //    }
  //
  //    SVD<> svdX(X);
  //    SE3<> se3;
  //    se3.get_rotation()=rotmat;
  //    Vector<3> trans = makeVector(svdX.get_VT()[2][0],svdX.get_VT()[2][1],svdX.get_VT()[2][2]);
  //    se3.get_translation()= -trans*mdWiggleScale/norm(trans);




  vector<HomographyMatch> vMatches;
  for(unsigned int i=0; i<vTrailMatches.size(); i++)
  {
    HomographyMatch m;
    m.v2CamPlaneFirst = mCamera.UnProject(vTrailMatches[i].first);
    m.v2CamPlaneSecond = mCamera.UnProject(vTrailMatches[i].second);
    m.m2PixelProjectionJac = mCamera.GetProjectionDerivs();
    vMatches.push_back(m);
  }


  //Weiss{
  if(vMatches.size()<4)
    return false;
  //}

  SE3<> se3;
  bool bGood;
  HomographyInit HomographyInit;

  bGood = HomographyInit.Compute(vMatches, 5.0, se3);

  if(!bGood)
  {
    mMessageForUser << "  Could not init from stereo pair, try again." << endl;
    return false;
  }

  // Check that the initialiser estimated a non-zero baseline
  double dTransMagn = sqrt(se3.get_translation() * se3.get_translation());
  if(dTransMagn == 0)
  {
    mMessageForUser << "  Estimated zero baseline from stereo pair, try again." << endl;
    return false;
  }
  // change the scale of the map so the second camera is wiggleScale away from the first
  se3.get_translation() *= mdWiggleScale/dTransMagn;


  KeyFrame::Ptr pkFirst = kF;
  KeyFrame::Ptr pkSecond = kS;

  pkFirst->bFixed = true;
  pkFirst->se3CfromW = SE3<>();

  pkSecond->bFixed = false;
  pkSecond->se3CfromW = se3;

  // Construct map from the stereo matches.
  PatchFinder finder;

  for(unsigned int i=0; i<vMatches.size(); i++)
  {
    MapPoint::Ptr p(new MapPoint());

    // Patch source stuff:
    p->pPatchSourceKF = pkFirst;
    p->nSourceLevel = 0;
    p->v3Normal_NC = makeVector( 0,0,-1);
    p->irCenter = vTrailMatches[i].first;
    p->v3Center_NC = unproject(mCamera.UnProject(p->irCenter));
    p->v3OneDownFromCenter_NC = unproject(mCamera.UnProject(p->irCenter + ImageRef(0,1)));
    p->v3OneRightFromCenter_NC = unproject(mCamera.UnProject(p->irCenter + ImageRef(1,0)));
    normalize(p->v3Center_NC);
    normalize(p->v3OneDownFromCenter_NC);
    normalize(p->v3OneRightFromCenter_NC);
    p->RefreshPixelVectors();

    // Do sub-pixel alignment on the second image
    finder.MakeTemplateCoarseNoWarp(p);
    finder.MakeSubPixTemplate();
    finder.SetSubPixPos(vec(vTrailMatches[i].second));
    bool bGood = finder.IterateSubPixToConvergence(*pkSecond,10);
    if(!bGood)
    {
      continue;
    }

    // Triangulate point:
    Vector<2> v2SecondPos = finder.GetSubPixPos();
    p->v3WorldPos = ReprojectPoint(se3, mCamera.UnProject(v2SecondPos), vMatches[i].v2CamPlaneFirst);
    if(p->v3WorldPos[2] < 0.0)
    {
      continue;
    }

    // Not behind map? Good, then add to map.
    p->pMMData = new MapMakerData();
    mMap.vpPoints.push_back(p);

    // Construct first two measurements and insert into relevant DBs:
    Measurement mFirst;
    mFirst.nLevel = 0;
    mFirst.Source = Measurement::SRC_ROOT;
    mFirst.v2RootPos = vec(vTrailMatches[i].first);
    mFirst.bSubPix = true;
    pkFirst->mMeasurements[p] = mFirst;
    p->pMMData->sMeasurementKFs.insert(pkFirst);

    Measurement mSecond;
    mSecond.nLevel = 0;
    mSecond.Source = Measurement::SRC_TRAIL;
    mSecond.v2RootPos = finder.GetSubPixPos();
    mSecond.bSubPix = true;
    pkSecond->mMeasurements[p] = mSecond;
    p->pMMData->sMeasurementKFs.insert(pkSecond);
    //slynen{ reprojection
#ifdef KF_REPROJ
    pkFirst->AddKeyMapPoint(p);
    pkSecond->AddKeyMapPoint(p);
#endif
    //}
  }

//Weiss{
        if(mMap.vpPoints.size()<4)
        {
                ROS_WARN_STREAM("Too few map points to init.");
                return false;
        }
//}

  mMap.vpKeyFrames.push_back(pkFirst);
  mMap.vpKeyFrames.push_back(pkSecond);
  pkFirst->MakeKeyFrame_Rest();
  pkSecond->MakeKeyFrame_Rest();
  //Weiss{
  //    for(int i=0; i<5; i++)
  /*}*/ BundleAdjustAll();

  // Estimate the feature depth distribution in the first two key-frames
  // (Needed for epipolar search)

//Weiss{
        if(!RefreshSceneDepth(pkFirst))
        {
                ROS_WARN_STREAM("Something is seriously wrong with the first KF.");
                return false;
        }
        if(!RefreshSceneDepth(pkSecond))
        {
                ROS_WARN_STREAM("Something is seriously wrong with the second KF.");
                return false;
        }
        mdWiggleScaleDepthNormalized = mdWiggleScale / pkFirst->dSceneDepthMean;


  //check if point have been added
  const VarParams& pParams = PtamParameters::varparams();
  bool addedsome=false;
  addedsome |= AddSomeMapPoints(3);
  if(!pParams.NoLevelZeroMapPoints)
    addedsome |= AddSomeMapPoints(0);
  addedsome |= AddSomeMapPoints(1);
  addedsome |= AddSomeMapPoints(2);
  if(!addedsome)
        {
                ROS_WARN_STREAM("Could not add any map points on any level - abort init.");
                return false;
        }
  //}

  mbBundleConverged_Full = false;
  mbBundleConverged_Recent = false;
  //Weiss{
  double nloops=0;
  while(!mbBundleConverged_Full & (nloops<pParams.MaxStereoInitLoops))
  {
    BundleAdjustAll();
    if(mbResetRequested | (nloops>=pParams.MaxStereoInitLoops))
      return false;
    nloops++;
  }

  //sanity check: if the point variance is too large assume init is crap --> very hacky, assumes flat init scene!!
  if(((pkFirst->dSceneDepthSigma+pkSecond->dSceneDepthSigma)/2.0>0.5) && pParams.CheckInitMapVar)
  {
    ROS_WARN_STREAM("Initial map rejected because of too large point variance. Point sigma: " << ((pkFirst->dSceneDepthSigma+pkSecond->dSceneDepthSigma)/2.0));
    return false;
  }

  //}

  // Rotate and translate the map so the dominant plane is at z=0:
  ApplyGlobalTransformationToMap(CalcPlaneAligner());
  mMap.bGood = true;
  se3TrackerPose = pkSecond->se3CfromW;

  mMessageForUser << "  MapMaker: made initial map with " << mMap.vpPoints.size() << " points." << endl;

  //slynen octomap_interface{
    octomap_interface.addKeyFrame(pkFirst);
    octomap_interface.addKeyFrame(pkSecond);
  //}

  return true;
}

// ThinCandidates() Thins out a key-frame's candidate list.
// Candidates are those salient corners where the mapmaker will attempt 
// to make a new map point by epipolar search. We don't want to make new points
// where there are already existing map points, this routine erases such candidates.
// Operates on a single level of a keyframe.
void MapMaker::ThinCandidates(KeyFrame::Ptr k, int nLevel)
{
  vector<Candidate> &vCSrc = k->aLevels[nLevel].vCandidates;
  vector<Candidate> vCGood;
  vector<ImageRef> irBusyLevelPos;
  // Make a list of `busy' image locations, which already have features at the same level
  // or at one level higher.
  for(meas_it it = k->mMeasurements.begin(); it!=k->mMeasurements.end(); it++)
  {
    if(!(it->second.nLevel == nLevel || it->second.nLevel == nLevel + 1))
      continue;
    irBusyLevelPos.push_back(ir_rounded(it->second.v2RootPos / LevelScale(nLevel)));
  }

  // Only keep those candidates further than 10 pixels away from busy positions.
  unsigned int nMinMagSquared = 10*10;
  for(unsigned int i=0; i<vCSrc.size(); i++)
  {
    ImageRef irC = vCSrc[i].irLevelPos;
    bool bGood = true;
    for(unsigned int j=0; j<irBusyLevelPos.size(); j++)
    {
      ImageRef irB = irBusyLevelPos[j];
      if((irB - irC).mag_squared() < nMinMagSquared)
      {
        bGood = false;
        break;
      }
    }
    if(bGood)
      vCGood.push_back(vCSrc[i]);
  }
  vCSrc = vCGood;
}

// Adds map points by epipolar search to the last-added key-frame, at a single
// specified pyramid level. Does epipolar search in the target keyframe as closest by
// the ClosestKeyFrame function.
bool MapMaker::AddSomeMapPoints(int nLevel)
{
  //Weiss{
  bool addedsome=false;
  //}
  KeyFrame::Ptr kSrc = (mMap.vpKeyFrames[mMap.vpKeyFrames.size() - 1]); // The new keyframe
  KeyFrame::Ptr kTarget = ClosestKeyFrame(kSrc);
  Level &l = kSrc->aLevels[nLevel];

  ThinCandidates(kSrc, nLevel);
  for(unsigned int i = 0; i<l.vCandidates.size(); i++)
    addedsome|=AddPointEpipolar(kSrc, kTarget, nLevel, i);
  return addedsome;
};

// Rotates/translates the whole map and all keyframes
void MapMaker::ApplyGlobalTransformationToMap(SE3<> se3NewFromOld)
{
  for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
    mMap.vpKeyFrames[i]->se3CfromW = mMap.vpKeyFrames[i]->se3CfromW * se3NewFromOld.inverse();

//  SO3<> so3Rot = se3NewFromOld.get_rotation();
  for(unsigned int i=0; i<mMap.vpPoints.size(); i++)
  {
    mMap.vpPoints[i]->v3WorldPos =
        se3NewFromOld * mMap.vpPoints[i]->v3WorldPos;
    mMap.vpPoints[i]->RefreshPixelVectors();
  }
}

// Applies a global scale factor to the map
void MapMaker::ApplyGlobalScaleToMap(double dScale)
{
  for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
    mMap.vpKeyFrames[i]->se3CfromW.get_translation() *= dScale;

  for(unsigned int i=0; i<mMap.vpPoints.size(); i++)
  {
    (*mMap.vpPoints[i]).v3WorldPos *= dScale;
    (*mMap.vpPoints[i]).v3PixelRight_W *= dScale;
    (*mMap.vpPoints[i]).v3PixelDown_W *= dScale;
    (*mMap.vpPoints[i]).RefreshPixelVectors();
  }
}

// The tracker entry point for adding a new keyframe;
// the tracker thread doesn't want to hang about, so 
// just dumps it on the top of the mapmaker's queue to 
// be dealt with later, and return.
void MapMaker::AddKeyFrame(KeyFrame::Ptr k)
{
  k->pSBI = NULL; // Mapmaker uses a different SBI than the tracker, so will re-gen its own
  mvpKeyFrameQueue.push_back(k);
  if(mbBundleRunning)   // Tell the mapmaker to stop doing low-priority stuff and concentrate on this KF first.
    mbBundleAbortRequested = true;
}

// Mapmaker's code to handle incoming key-frames.
void MapMaker::AddKeyFrameFromTopOfQueue()
{
  if(mvpKeyFrameQueue.size() == 0)
    return;

  KeyFrame::Ptr pK = mvpKeyFrameQueue[0];
  mvpKeyFrameQueue.erase(mvpKeyFrameQueue.begin());

  //Weiss{

  
  const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
  if (pPars.MaxKF>1)    // MaxKF<2 means keep all KFs
  {
    while (mMap.vpKeyFrames.size()>(unsigned int)pPars.MaxKF)
    {
      // find farthest KF
      double dFarthestDist = 0;
      vector<KeyFrame::Ptr>::iterator itFarthest = mMap.vpKeyFrames.begin();

      for(vector<KeyFrame::Ptr>::iterator it = mMap.vpKeyFrames.begin();it!=mMap.vpKeyFrames.end();++it)
      {
        double dDist = KeyFrameLinearDist(pK, *it);
        if(dDist > dFarthestDist)
        {
          dFarthestDist = dDist;
          itFarthest = it;
        }
      }

      for (unsigned int i=0;i<mMap.vpPoints.size();i++)
        if (mMap.vpPoints[i]->pPatchSourceKF==*itFarthest)
          mMap.vpPoints[i]->bBad=true;

      if ((*itFarthest)->bFixed)        // if we delete the a fix KF, fix the oldest one
      {
        mMap.vpKeyFrames.erase(itFarthest);
        mMap.vpKeyFrames[0]->bFixed=true;
      }
      else
        mMap.vpKeyFrames.erase(itFarthest);
    }
  }
  //}

  pK->MakeKeyFrame_Rest();
  //Weiss{ feature statistics
  //    kfid = pK->ID;
  //}
  mMap.vpKeyFrames.push_back(pK);
  // Any measurements? Update the relevant point's measurement counter status map
  for(meas_it it = pK->mMeasurements.begin();
      it!=pK->mMeasurements.end();
      it++)
  {
    it->first->pMMData->sMeasurementKFs.insert(pK);
    it->second.Source = Measurement::SRC_TRACKER;
  }
  // And maybe we missed some - this now adds to the map itself, too.
  ReFindInSingleKeyFrame(pK);

  bool addedsome=false;
  addedsome |= AddSomeMapPoints(3);       // .. and add more map points by epipolar search.
  if(!pPars.NoLevelZeroMapPoints)
    addedsome |= AddSomeMapPoints(0);
  addedsome |= AddSomeMapPoints(1);
  addedsome |= AddSomeMapPoints(2);

  //Weiss{ feature statistics
  //    ROS_WARN_STREAM("\ntotall : " << addcount[4] << " totlvl0: " << addcount[0]  << " totlvl1: " <<  addcount[1]  << " totlvl2: " << addcount[2]  << " totlvl3: " <<  addcount[3]);
  //    ROS_WARN_STREAM("\noutall : " << outcount[4] << " outlvl0: " << outcount[0]  << " outlvl1: " <<  outcount[1]  << " outlvl2: " << outcount[2]  << " outlvl3: " <<  outcount[3]);
  //}

  mbBundleConverged_Full = false;
  mbBundleConverged_Recent = false;

  //slynen octomap_interface{
  octomap_interface.addKeyFrame(pK);
  //}
}

// Tries to make a new map point out of a single candidate point
// by searching for that point in another keyframe, and triangulating
// if a match is found.
bool MapMaker::AddPointEpipolar(KeyFrame::Ptr kSrc,
                                KeyFrame::Ptr kTarget,
                                int nLevel,
                                int nCandidate)
{
  static Image<Vector<2> > imUnProj;
  static bool bMadeCache = false;
  if(!bMadeCache)
  {
    imUnProj.resize(kSrc->aLevels[0].im.size());
    ImageRef ir;
    do imUnProj[ir] = mCamera.UnProject(ir);
    while(ir.next(imUnProj.size()));
    bMadeCache = true;
  }

  int nLevelScale = LevelScale(nLevel);
  Candidate &candidate = kSrc->aLevels[nLevel].vCandidates[nCandidate];
  ImageRef irLevelPos = candidate.irLevelPos;
  Vector<2> v2RootPos = LevelZeroPos(irLevelPos, nLevel);

  Vector<3> v3Ray_SC = unproject(mCamera.UnProject(v2RootPos)); // direction vector from current camera to the feature? [SW]
  normalize(v3Ray_SC);
  Vector<3> v3LineDirn_TC = kTarget->se3CfromW.get_rotation() * (kSrc->se3CfromW.get_rotation().inverse() * v3Ray_SC); // direction vector from target KF camer to the feature? [SW]

  // Restrict epipolar search to a relatively narrow depth range
  // to increase reliability
  double dMean = kSrc->dSceneDepthMean;
  double dSigma = kSrc->dSceneDepthSigma;
  //Weiss{  comments marked with [SW]
  //    double dStartDepth = max(mdWiggleScale, dMean - dSigma); // estimated minimal distance of the feature in the current KF? [SW]
  //    double dEndDepth = min(40 * mdWiggleScale, dMean + dSigma); // estimated maximal distance of the feature in the current KF? [SW]
  //    double dStartDepth = max(mdWiggleScale, dMean - dSigma); // estimated minimal distance of the feature in the current KF? [SW]
  double dStartDepth = max(0.0, dMean - dSigma); // estimated minimal distance of the feature in the current KF? [SW]
  double dEndDepth = dMean + dSigma; // estimated maximal distance of the feature in the current KF? [SW]
  //}
  Vector<3> v3CamCenter_TC = kTarget->se3CfromW * kSrc->se3CfromW.inverse().get_translation(); // The camera of the current camera seen from the target KF? [SW]
  Vector<3> v3RayStart_TC = v3CamCenter_TC + dStartDepth * v3LineDirn_TC;                           // start point on the 3D epi-line expressed in the target KF? [SW]
  Vector<3> v3RayEnd_TC = v3CamCenter_TC + dEndDepth * v3LineDirn_TC;                               // end point on the 3d epi-line expressed in the target KF? [SW]

  //Weiss{
  // it is mainly to speed up calculation, but breaks on large means (dMean>40), since then v3RayEnd_TC[2] <= v3RayStart_TC[2]
  // is always true
  // Note: v3RayEnd_TC[2] <= v3RayStart_TC[2] is still true if the KFs have an angle of 90° or more w.r.t. each other...
  // this happens rarely though...
  //    if(v3RayEnd_TC[2] <= v3RayStart_TC[2])  // it's highly unlikely that we'll manage to get anything out if we're facing backwards wrt the other camera's view-ray
  //            return false;
  //}
  if(v3RayEnd_TC[2] <= 0.0 )  return false;
  if(v3RayStart_TC[2] <= 0.0)
    v3RayStart_TC += v3LineDirn_TC * (0.001 - v3RayStart_TC[2] / v3LineDirn_TC[2]);

  Vector<2> v2A = project(v3RayStart_TC);
  Vector<2> v2B = project(v3RayEnd_TC);
  Vector<2> v2AlongProjectedLine = v2A-v2B;

  if(v2AlongProjectedLine * v2AlongProjectedLine < 0.00000001)
  {
    mMessageForUser << "v2AlongProjectedLine too small." << endl;
    return false;
  }
  normalize(v2AlongProjectedLine);
  Vector<2> v2Normal;
  v2Normal[0] = v2AlongProjectedLine[1];
  v2Normal[1] = -v2AlongProjectedLine[0];

  double dNormDist = v2A * v2Normal;
  if(fabs(dNormDist) > mCamera.LargestRadiusInImage() )
    return false;

  double dMinLen = min(v2AlongProjectedLine * v2A, v2AlongProjectedLine * v2B) - 0.05;
  double dMaxLen = max(v2AlongProjectedLine * v2A, v2AlongProjectedLine * v2B) + 0.05;
  if(dMinLen < -2.0)  dMinLen = -2.0;
  if(dMaxLen < -2.0)  dMaxLen = -2.0;
  if(dMinLen > 2.0)   dMinLen = 2.0;
  if(dMaxLen > 2.0)   dMaxLen = 2.0;

  // Find current-frame corners which might match this
  PatchFinder Finder;
  Finder.MakeTemplateCoarseNoWarp(kSrc, nLevel, irLevelPos);
  if(Finder.TemplateBad())  return false;

  vector<Vector<2> > &vv2Corners = kTarget->aLevels[nLevel].vImplaneCorners;
  vector<ImageRef> &vIR = kTarget->aLevels[nLevel].vCorners;
  if(!kTarget->aLevels[nLevel].bImplaneCornersCached)
  {
    for(unsigned int i=0; i<vIR.size(); i++)   // over all corners in target img..
      vv2Corners.push_back(imUnProj[ir(LevelZeroPos(vIR[i], nLevel))]);
    kTarget->aLevels[nLevel].bImplaneCornersCached = true;
  }

  int nBest = -1;
  int nBestZMSSD = Finder.mnMaxSSD + 1;
  double dMaxDistDiff = mCamera.OnePixelDist() * (4.0 + 1.0 * nLevelScale);
  double dMaxDistSq = dMaxDistDiff * dMaxDistDiff;

  for(unsigned int i=0; i<vv2Corners.size(); i++)   // over all corners in target img..
  {
    Vector<2> v2Im = vv2Corners[i];
    double dDistDiff = dNormDist - v2Im * v2Normal;
    if(dDistDiff * dDistDiff > dMaxDistSq)      continue; // skip if not along epi line
    if(v2Im * v2AlongProjectedLine < dMinLen)   continue; // skip if not far enough along line
    if(v2Im * v2AlongProjectedLine > dMaxLen)   continue; // or too far
    int nZMSSD = Finder.ZMSSDAtPoint(kTarget->aLevels[nLevel].im, vIR[i]);
    if(nZMSSD < nBestZMSSD)
    {
      nBest = i;
      nBestZMSSD = nZMSSD;
    }
  }

  if(nBest == -1)   return false;   // Nothing found.

  //  Found a likely candidate along epipolar ray
  Finder.MakeSubPixTemplate();
  Finder.SetSubPixPos(LevelZeroPos(vIR[nBest], nLevel));
  bool bSubPixConverges = Finder.IterateSubPixToConvergence(*kTarget,10);
  if(!bSubPixConverges)
    return false;

  // Now triangulate the 3d point...
  Vector<3> v3New;
  v3New = kTarget->se3CfromW.inverse() *
      ReprojectPoint(kSrc->se3CfromW * kTarget->se3CfromW.inverse(),
                     mCamera.UnProject(v2RootPos),
                     mCamera.UnProject(Finder.GetSubPixPos()));

  MapPoint::Ptr pNew(new MapPoint());
  pNew->v3WorldPos = v3New;
  pNew->pMMData = new MapMakerData();

  // Patch source stuff:
  pNew->pPatchSourceKF = kSrc;
  pNew->nSourceLevel = nLevel;
  pNew->v3Normal_NC = makeVector( 0,0,-1);
  pNew->irCenter = irLevelPos;
  pNew->v3Center_NC = unproject(mCamera.UnProject(v2RootPos));
  pNew->v3OneRightFromCenter_NC = unproject(mCamera.UnProject(v2RootPos + vec(ImageRef(nLevelScale,0))));
  pNew->v3OneDownFromCenter_NC  = unproject(mCamera.UnProject(v2RootPos + vec(ImageRef(0,nLevelScale))));

  normalize(pNew->v3Center_NC);
  normalize(pNew->v3OneDownFromCenter_NC);
  normalize(pNew->v3OneRightFromCenter_NC);

  pNew->RefreshPixelVectors();

  mMap.vpPoints.push_back(pNew);
  mqNewQueue.push(pNew);
  Measurement m;
  m.Source = Measurement::SRC_ROOT;
  m.v2RootPos = v2RootPos;
  m.nLevel = nLevel;
  m.bSubPix = true;
  kSrc->mMeasurements[pNew] = m;

  m.Source = Measurement::SRC_EPIPOLAR;
  m.v2RootPos = Finder.GetSubPixPos();
  kTarget->mMeasurements[pNew] = m;
  pNew->pMMData->sMeasurementKFs.insert(kSrc);
  pNew->pMMData->sMeasurementKFs.insert(kTarget);
  //slynen{ reprojection
#ifdef KF_REPROJ
  kSrc->AddKeyMapPoint(pNew);
  kTarget->AddKeyMapPoint(pNew);
#endif
  //}

  return true;
}

double MapMaker::KeyFrameLinearDist(KeyFrame::Ptr k1, KeyFrame::Ptr k2)
{
  Vector<3> v3KF1_CamPos = k1->se3CfromW.inverse().get_translation();
  Vector<3> v3KF2_CamPos = k2->se3CfromW.inverse().get_translation();
  Vector<3> v3Diff = v3KF2_CamPos - v3KF1_CamPos;
  double dDist = sqrt(v3Diff * v3Diff);
  return dDist;
}

vector<KeyFrame::Ptr> MapMaker::NClosestKeyFrames(KeyFrame::Ptr k, unsigned int N)
{
  vector<pair<double, KeyFrame::Ptr > > vKFandScores;
  for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
  {
    if(mMap.vpKeyFrames[i] == k)
      continue;
    double dDist = KeyFrameLinearDist(k, mMap.vpKeyFrames[i]);
    vKFandScores.push_back(make_pair(dDist, mMap.vpKeyFrames[i]));
  }
  if(N > vKFandScores.size())
    N = vKFandScores.size();
  partial_sort(vKFandScores.begin(), vKFandScores.begin() + N, vKFandScores.end());

  vector<KeyFrame::Ptr> vResult;
  for(unsigned int i=0; i<N; i++)
    vResult.push_back(vKFandScores[i].second);
  return vResult;
}

KeyFrame::Ptr MapMaker::ClosestKeyFrame(KeyFrame::Ptr k)
{
  double dClosestDist = 9999999999.9;
  int nClosest = -1;
  for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
  {
    if(mMap.vpKeyFrames[i] == k)
      continue;
    double dDist = KeyFrameLinearDist(k, mMap.vpKeyFrames[i]);
    if(dDist < dClosestDist)
    {
      dClosestDist = dDist;
      nClosest = i;
    }
  }
  assert(nClosest != -1);
  return mMap.vpKeyFrames[nClosest];
}

double MapMaker::DistToNearestKeyFrame(KeyFrame::Ptr kCurrent)
{
  KeyFrame::Ptr pClosest = ClosestKeyFrame(kCurrent);
  double dDist = KeyFrameLinearDist(kCurrent, pClosest);
  return dDist;
}

bool MapMaker::NeedNewKeyFrame(KeyFrame::Ptr kCurrent)
{
  KeyFrame::Ptr pClosest = ClosestKeyFrame(kCurrent);
  double dDist = KeyFrameLinearDist(kCurrent, pClosest);

  //Weiss{
  
  const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();

  std::vector<double> medianpixdist;
  Vector<3> veccam2;
  Vector<2> cam2,cam1;
  for(std::map<MapPoint::Ptr, Measurement>::iterator it = kCurrent->mMeasurements.begin();it != kCurrent->mMeasurements.end(); it++)
  {
    veccam2 = pClosest->se3CfromW*it->first->v3WorldPos;        // rotate in closest KF
    veccam2 = (kCurrent->se3CfromW*pClosest->se3CfromW.inverse()).get_rotation()*veccam2;       // derot in current frame
    cam2 = mCamera.Project(makeVector(veccam2[0]/veccam2[2],veccam2[1]/veccam2[2]));
    cam1 = it->second.v2RootPos;
    medianpixdist.push_back(sqrt((cam2[0]-cam1[0])*(cam2[0]-cam1[0]) + (cam2[1]-cam1[1])*(cam2[1]-cam1[1])));
  }
  std::vector<double>::iterator first = medianpixdist.begin();
  std::vector<double>::iterator last = medianpixdist.end();
  std::vector<double>::iterator middle = first + std::floor((last - first) / 2);
  std::nth_element(first, middle, last); // can specify comparator as optional 4th arg
  double mediandist = *middle;

  dDist *= (1.0 / kCurrent->dSceneDepthMean);

  //    
  //    const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
  //if(dDist > GV2.GetDouble("MapMaker.MaxKFDistWiggleMult",1.0,SILENT) * mdWiggleScaleDepthNormalized)
  if(pPars.UseKFPixelDist & (mediandist>pPars.AutoInitPixel))
    return true;

  if(dDist > pPars.MaxKFDistWiggleMult * mdWiggleScaleDepthNormalized)
  {
    return true;
  }
  return false;
  //}
}

// Perform bundle adjustment on all keyframes, all map points
void MapMaker::BundleAdjustAll()
{
  // construct the sets of kfs/points to be adjusted:
  // in this case, all of them
  set<KeyFrame::Ptr> sAdj;
  set<KeyFrame::Ptr> sFixed;
  for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
    if(mMap.vpKeyFrames[i]->bFixed)
      sFixed.insert(mMap.vpKeyFrames[i]);
    else
      sAdj.insert(mMap.vpKeyFrames[i]);

  set<MapPoint::Ptr> sMapPoints;
  for(unsigned int i=0; i<mMap.vpPoints.size();i++)
    sMapPoints.insert(mMap.vpPoints[i]);

  BundleAdjust(sAdj, sFixed, sMapPoints, false);
}

// Peform a local bundle adjustment which only adjusts
// recently added key-frames
void MapMaker::BundleAdjustRecent()
{
  if(mMap.vpKeyFrames.size() < 8)
  { // Ignore this unless map is big enough
    mbBundleConverged_Recent = true;
    return;
  }


  // First, make a list of the keyframes we want adjusted in the adjuster.
  // This will be the last keyframe inserted, and its four nearest neighbors
  set<KeyFrame::Ptr> sAdjustSet;

  //Weiss{ if we only keep N KFs only insert most recently added KF as loose
  unsigned char nloose = 4;
  
  const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
  if (pPars.MaxKF>1)    // MaxKF<2 means keep all KFs
    nloose=0;

  KeyFrame::Ptr pkfNewest = mMap.vpKeyFrames.back();
  sAdjustSet.insert(pkfNewest);
  vector<KeyFrame::Ptr> vClosest = NClosestKeyFrames(pkfNewest, 4);
  for(int i=0; i<nloose; i++)
    if(vClosest[i]->bFixed == false)
      sAdjustSet.insert(vClosest[i]);
  //}

  // Now we find the set of features which they contain.
  set<MapPoint::Ptr> sMapPoints;
  for(set<KeyFrame::Ptr>::iterator iter = sAdjustSet.begin();
      iter!=sAdjustSet.end();
      iter++)
  {
    map<MapPoint::Ptr,Measurement> &mKFMeas = (*iter)->mMeasurements;
    for(meas_it jiter = mKFMeas.begin(); jiter!= mKFMeas.end(); jiter++)
      sMapPoints.insert(jiter->first);
  };

  // Finally, add all keyframes which measure above points as fixed keyframes
  set<KeyFrame::Ptr> sFixedSet;
  for(vector<KeyFrame::Ptr>::iterator it = mMap.vpKeyFrames.begin(); it!=mMap.vpKeyFrames.end(); it++)
  {
    if(sAdjustSet.count(*it))
      continue;
    bool bInclude = false;
    for(meas_it jiter = (*it)->mMeasurements.begin(); jiter!= (*it)->mMeasurements.end(); jiter++)
      if(sMapPoints.count(jiter->first))
      {
        bInclude = true;
        break;
      }
    if(bInclude)
      sFixedSet.insert(*it);
  }

  BundleAdjust(sAdjustSet, sFixedSet, sMapPoints, true);
}

// Common bundle adjustment code. This creates a bundle-adjust instance, populates it, and runs it.
void MapMaker::BundleAdjust(set<KeyFrame::Ptr> sAdjustSet, set<KeyFrame::Ptr> sFixedSet, set<MapPoint::Ptr> sMapPoints, bool bRecent)
{
  Bundle b(mCamera);   // Our bundle adjuster
  mbBundleRunning = true;
  mbBundleRunningIsRecent = bRecent;

  // The bundle adjuster does different accounting of keyframes and map points;
  // Translation maps are stored:
  map<MapPoint::Ptr, int> mPoint_BundleID;
  map<int, MapPoint::Ptr> mBundleID_Point;
  map<KeyFrame::Ptr, int> mView_BundleID;
  map<int, KeyFrame::Ptr> mBundleID_View;

  // Add the keyframes' poses to the bundle adjuster. Two parts: first nonfixed, then fixed.
  for(set<KeyFrame::Ptr>::iterator it = sAdjustSet.begin(); it!= sAdjustSet.end(); it++)
  {
    int nBundleID = b.AddCamera((*it)->se3CfromW, (*it)->bFixed);
    mView_BundleID[*it] = nBundleID;
    mBundleID_View[nBundleID] = *it;
  }
  for(set<KeyFrame::Ptr>::iterator it = sFixedSet.begin(); it!= sFixedSet.end(); it++)
  {
    int nBundleID = b.AddCamera((*it)->se3CfromW, true);
    mView_BundleID[*it] = nBundleID;
    mBundleID_View[nBundleID] = *it;
  }

  // Add the points' 3D position
  for(set<MapPoint::Ptr>::iterator it = sMapPoints.begin(); it!=sMapPoints.end(); it++)
  {
    int nBundleID = b.AddPoint((*it)->v3WorldPos);
    mPoint_BundleID[*it] = nBundleID;
    mBundleID_Point[nBundleID] = *it;

    //Weiss{ feature statistics
    //          if(kfid==(*it)->pPatchSourceKF->ID)
    //          {
    //                  addcount[(*it)->nSourceLevel]++;
    //                  addcount[4]++;
    //          }
    //}
  }

  // Add the relevant point-in-keyframe measurements
  for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
  {
    if(mView_BundleID.count(mMap.vpKeyFrames[i]) == 0)
      continue;

    int nKF_BundleID = mView_BundleID[mMap.vpKeyFrames[i]];
    for(meas_it it= mMap.vpKeyFrames[i]->mMeasurements.begin();
        it!= mMap.vpKeyFrames[i]->mMeasurements.end();
        it++)
    {

      if(mPoint_BundleID.count(it->first) == 0)
        continue;
      int nPoint_BundleID = mPoint_BundleID[it->first];
      b.AddMeas(nKF_BundleID, nPoint_BundleID, it->second.v2RootPos, LevelScale(it->second.nLevel) * LevelScale(it->second.nLevel));
    }
  }

  // Run the bundle adjuster. This returns the number of successful iterations
  int nAccepted = b.Compute(&mbBundleAbortRequested);

  if(nAccepted < 0)
  {
    // Crap: - LM Ran into a serious problem!
    // This is probably because the initial stereo was messed up.
    // Get rid of this map and start again!
    mMessageForUser << "!! MapMaker: Cholesky failure in bundle adjust. " << endl
        << "   The map is probably corrupt: Ditching the map. " << endl;
    mbResetRequested = true;
    return;
  }

  // Bundle adjustment did some updates, apply these to the map
  if(nAccepted > 0)
  {
    //slynen{ pcl interface
    std::set<MapPoint::Ptr> mapPointsToUpdate; //copy the data
    for(map<MapPoint::Ptr,int>::iterator itr = mPoint_BundleID.begin();
        itr!=mPoint_BundleID.end();
        itr++){
      itr->first->v3WorldPos = b.GetPoint(itr->second);
      mapPointsToUpdate.insert(itr->first);
    }
    octomap_interface.updatePoints(mapPointsToUpdate);
    //}

    for(map<KeyFrame::Ptr,int>::iterator itr = mView_BundleID.begin();
        itr!=mView_BundleID.end();
        itr++)
      itr->first->se3CfromW = b.GetCamera(itr->second);
    if(bRecent)
      mbBundleConverged_Recent = false;
    mbBundleConverged_Full = false;
  };

  if(b.Converged())
  {
    mbBundleConverged_Recent = true;
    if(!bRecent)
      mbBundleConverged_Full = true;
  }

  mbBundleRunning = false;
  mbBundleAbortRequested = false;

  // Handle outlier measurements:
  vector<pair<int,int> > vOutliers_PC_pair = b.GetOutlierMeasurements();

  for(unsigned int i=0; i<vOutliers_PC_pair.size(); i++)
  {
    MapPoint::Ptr pp = mBundleID_Point[vOutliers_PC_pair[i].first];
    KeyFrame::Ptr pk = mBundleID_View[vOutliers_PC_pair[i].second];
    Measurement &m = pk->mMeasurements[pp];

    if(pp->pMMData->GoodMeasCount() <= 2 || m.Source == Measurement::SRC_ROOT)   // Is the original source kf considered an outlier? That's bad.
      pp->bBad = true;
    else
    {
      // Do we retry it? Depends where it came from!!
      if(m.Source == Measurement::SRC_TRACKER || m.Source == Measurement::SRC_EPIPOLAR)
        mvFailureQueue.push_back(pair<KeyFrame::Ptr,MapPoint::Ptr>(pk,pp));
      else
        pp->pMMData->sNeverRetryKFs.insert(pk);
      pk->mMeasurements.erase(pp);
      pp->pMMData->sMeasurementKFs.erase(pk);
      //Weiss{ feature statistics
      //                        if(kfid==pp->pPatchSourceKF->ID)
      //                        {
      //                                outcount[pp->nSourceLevel]++;
      //                                outcount[4]++;
      //                        }
      //}
    }
  }
}

// Mapmaker's try-to-find-a-point-in-a-keyframe code. This is used to update
// data association if a bad measurement was detected, or if a point
// was never searched for in a keyframe in the first place. This operates
// much like the tracker! So most of the code looks just like in 
// TrackerData.h.
bool MapMaker::ReFind_Common(KeyFrame::Ptr k, MapPoint::Ptr p)
{
  // abort if either a measurement is already in the map, or we've
  // decided that this point-kf combo is beyond redemption
  if(p->pMMData->sMeasurementKFs.count(k)
      || p->pMMData->sNeverRetryKFs.count(k))
    return false;

  static PatchFinder Finder;
  Vector<3> v3Cam = k->se3CfromW * p->v3WorldPos;
  if(v3Cam[2] < 0.001)
  {
    p->pMMData->sNeverRetryKFs.insert(k);
    return false;
  }
  Vector<2> v2ImPlane = project(v3Cam);
  if(v2ImPlane* v2ImPlane > mCamera.LargestRadiusInImage() * mCamera.LargestRadiusInImage())
  {
    p->pMMData->sNeverRetryKFs.insert(k);
    return false;
  }

  Vector<2> v2Image = mCamera.Project(v2ImPlane);
  if(mCamera.Invalid())
  {
    p->pMMData->sNeverRetryKFs.insert(k);
    return false;
  }

  ImageRef irImageSize = k->aLevels[0].im.size();
  if(v2Image[0] < 0 || v2Image[1] < 0 || v2Image[0] > irImageSize[0] || v2Image[1] > irImageSize[1])
  {
    p->pMMData->sNeverRetryKFs.insert(k);
    return false;
  }

  Matrix<2> m2CamDerivs = mCamera.GetProjectionDerivs();
  Finder.MakeTemplateCoarse(p, k->se3CfromW, m2CamDerivs);

  if(Finder.TemplateBad())
  {
    p->pMMData->sNeverRetryKFs.insert(k);
    return false;
  }
  bool bFound = Finder.FindPatchCoarse(ir(v2Image), k, 4);  // Very tight search radius!
  if(!bFound)
  {
    p->pMMData->sNeverRetryKFs.insert(k);
    return false;
  }

  // If we found something, generate a measurement struct and put it in the map
  Measurement m;
  m.nLevel = Finder.GetLevel();
  m.Source = Measurement::SRC_REFIND;

  if(Finder.GetLevel() > 0)
  {
    Finder.MakeSubPixTemplate();
    Finder.IterateSubPixToConvergence(*k,8);
    m.v2RootPos = Finder.GetSubPixPos();
    m.bSubPix = true;
  }
  else
  {
    m.v2RootPos = Finder.GetCoarsePosAsVector();
    m.bSubPix = false;
  };

  if(k->mMeasurements.count(p))
  {
    assert(0); // This should never happen, we checked for this at the start.
  }
  k->mMeasurements[p] = m;
  p->pMMData->sMeasurementKFs.insert(k);
  //slynen{ reprojection
#ifdef KF_REPROJ
  k->AddKeyMapPoint(p);
#endif
  //}
  //slynen octomap_interface{
  octomap_interface.updatePoint(p);
  //}
  return true;
}

// A general data-association update for a single keyframe
// Do this on a new key-frame when it's passed in by the tracker
int MapMaker::ReFindInSingleKeyFrame(KeyFrame::Ptr k)
{
  vector<MapPoint::Ptr> vToFind;
  for(unsigned int i=0; i<mMap.vpPoints.size(); i++)
    vToFind.push_back(mMap.vpPoints[i]);

  int nFoundNow = 0;
  for(unsigned int i=0; i<vToFind.size(); i++)
    if(ReFind_Common(k,vToFind[i]))
      nFoundNow++;

  return nFoundNow;
};

// When new map points are generated, they're only created from a stereo pair
// this tries to make additional measurements in other KFs which they might
// be in.
void MapMaker::ReFindNewlyMade()
{
  if(mqNewQueue.empty())
    return;
  int nFound = 0;
  int nBad = 0;
  while(!mqNewQueue.empty() && mvpKeyFrameQueue.size() == 0)
  {
    MapPoint::Ptr pNew = mqNewQueue.front();
    mqNewQueue.pop();
    if(pNew->bBad)
    {
      nBad++;
      continue;
    }
    for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
      if(ReFind_Common(mMap.vpKeyFrames[i], pNew))
        nFound++;
  }
};

// Dud measurements get a second chance.
void MapMaker::ReFindFromFailureQueue()
{
  if(mvFailureQueue.size() == 0)
    return;
  sort(mvFailureQueue.begin(), mvFailureQueue.end());
  vector<pair<KeyFrame::Ptr, MapPoint::Ptr> >::iterator it;
  int nFound=0;
  for(it = mvFailureQueue.begin(); it!=mvFailureQueue.end(); it++)
    if(ReFind_Common(it->first, it->second))
      nFound++;

  mvFailureQueue.erase(mvFailureQueue.begin(), it);
};

// Is the tracker's camera pose in cloud-cuckoo land?
bool MapMaker::IsDistanceToNearestKeyFrameExcessive(KeyFrame::Ptr kCurrent)
{
  return DistToNearestKeyFrame(kCurrent) > mdWiggleScale * 10.0;
}

// Find a dominant plane in the map, find an SE3<> to put it as the z=0 plane
SE3<> MapMaker::CalcPlaneAligner()
{
  unsigned int nPoints = mMap.vpPoints.size();
  if(nPoints < 10)
  {
    mMessageForUser << "  MapMaker: CalcPlane: too few points to calc plane." << endl;
    return SE3<>();
  };
  //Weiss{
  
  const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
  int nRansacs = pPars.PlaneAlignerRansacs;
  //int nRansacs = GV2.GetInt("MapMaker.PlaneAlignerRansacs", 100, HIDDEN|SILENT);
  //}
  Vector<3> v3BestMean=makeVector(0,0,0);
  Vector<3> v3BestNormal=makeVector(0,0,0);
  double dBestDistSquared = 9999999999999999.9;

  for(int i=0; i<nRansacs; i++)
  {
    int nA = rand()%nPoints;
    int nB = nA;
    int nC = nA;
    while(nB == nA)
      nB = rand()%nPoints;
    while(nC == nA || nC==nB)
      nC = rand()%nPoints;

    Vector<3> v3Mean = 0.33333333 * (mMap.vpPoints[nA]->v3WorldPos +
        mMap.vpPoints[nB]->v3WorldPos +
        mMap.vpPoints[nC]->v3WorldPos);

    Vector<3> v3CA = mMap.vpPoints[nC]->v3WorldPos  - mMap.vpPoints[nA]->v3WorldPos;
    Vector<3> v3BA = mMap.vpPoints[nB]->v3WorldPos  - mMap.vpPoints[nA]->v3WorldPos;
    Vector<3> v3Normal = v3CA ^ v3BA;
    if(v3Normal * v3Normal  == 0)
      continue;
    normalize(v3Normal);

    double dSumError = 0.0;
    for(unsigned int i=0; i<nPoints; i++)
    {
      Vector<3> v3Diff = mMap.vpPoints[i]->v3WorldPos - v3Mean;
      double dDistSq = v3Diff * v3Diff;
      if(dDistSq == 0.0)
        continue;
      double dNormDist = fabs(v3Diff * v3Normal);

      if(dNormDist > 0.05)
        dNormDist = 0.05;
      dSumError += dNormDist;
    }
    if(dSumError < dBestDistSquared)
    {
      dBestDistSquared = dSumError;
      v3BestMean = v3Mean;
      v3BestNormal = v3Normal;
    }
  }

  // Done the ransacs, now collect the supposed inlier set
  vector<Vector<3> > vv3Inliers;
  for(unsigned int i=0; i<nPoints; i++)
  {
    Vector<3> v3Diff = mMap.vpPoints[i]->v3WorldPos - v3BestMean;
    double dDistSq = v3Diff * v3Diff;
    if(dDistSq == 0.0)
      continue;
    double dNormDist = fabs(v3Diff * v3BestNormal);
    if(dNormDist < 0.05)
      vv3Inliers.push_back(mMap.vpPoints[i]->v3WorldPos);
  }

  // With these inliers, calculate mean and cov
  Vector<3> v3MeanOfInliers = Zeros;
  for(unsigned int i=0; i<vv3Inliers.size(); i++)
    v3MeanOfInliers+=vv3Inliers[i];
  v3MeanOfInliers *= (1.0 / vv3Inliers.size());

  Matrix<3> m3Cov = Zeros;
  for(unsigned int i=0; i<vv3Inliers.size(); i++)
  {
    Vector<3> v3Diff = vv3Inliers[i] - v3MeanOfInliers;
    m3Cov += v3Diff.as_col() * v3Diff.as_row();
  };

  // Find the principal component with the minimal variance: this is the plane normal
  SymEigen<3> sym(m3Cov);
  Vector<3> v3Normal = sym.get_evectors()[0];

  // Use the version of the normal which points towards the cam center
  if(v3Normal[2] > 0)
    v3Normal *= -1.0;

  Matrix<3> m3Rot = Identity;
  m3Rot[2] = v3Normal;
  m3Rot[0] = m3Rot[0] - (v3Normal * (m3Rot[0] * v3Normal));
  normalize(m3Rot[0]);
  m3Rot[1] = m3Rot[2] ^ m3Rot[0];

  SE3<> se3Aligner;
  se3Aligner.get_rotation() = m3Rot;
  Vector<3> v3RMean = se3Aligner * v3MeanOfInliers;
  se3Aligner.get_translation() = -v3RMean;

  return se3Aligner;
}

// Calculates the depth(z-) distribution of map points visible in a keyframe
// This function is only used for the first two keyframes - all others
// get this filled in by the tracker
bool MapMaker::RefreshSceneDepth(KeyFrame::Ptr pKF)
{
  double dSumDepth = 0.0;
  double dSumDepthSquared = 0.0;
  int nMeas = 0;
  for(meas_it it = pKF->mMeasurements.begin(); it!=pKF->mMeasurements.end(); it++)
  {
    MapPoint &point = *it->first;
    Vector<3> v3PosK = pKF->se3CfromW * point.v3WorldPos;
    dSumDepth += v3PosK[2];
    dSumDepthSquared += v3PosK[2] * v3PosK[2];
    nMeas++;
  }
  //Weiss{
  //    assert(nMeas > 2); // If not then something is seriously wrong with this KF!!
  //agree, but do not kill whole PTAM just because of that...
  if (nMeas<2)
    return false;
  pKF->dSceneDepthMean = dSumDepth / nMeas;
  pKF->dSceneDepthSigma = sqrt((dSumDepthSquared / nMeas) - (pKF->dSceneDepthMean) * (pKF->dSceneDepthMean));
  return true;
}

void MapMaker::GUICommandCallBack(void* ptr, string sCommand, string sParams)
{
  Command c;
  c.sCommand = sCommand;
  c.sParams = sParams;
  ((MapMaker*) ptr)->mvQueuedCommands.push_back(c);
}

void MapMaker::GUICommandHandler(string sCommand, string sParams)  // Called by the callback func..
{
  if(sCommand=="SaveMap")
  {
    mMessageForUser << "  MapMaker: Saving the map.... " << endl;
    ofstream ofs("map.dump");
    for(unsigned int i=0; i<mMap.vpPoints.size(); i++)
    {
      ofs << mMap.vpPoints[i]->v3WorldPos << "  ";
      ofs << mMap.vpPoints[i]->nSourceLevel << endl;
    }
    ofs.close();

    for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
    {
      ostringstream ost1;
      ost1 << "keyframes/" << i << ".jpg";
      //          img_save(mMap.vpKeyFrames[i]->aLevels[0].im, ost1.str());

      ostringstream ost2;
      ost2 << "keyframes/" << i << ".info";
      ofstream ofs2;
      ofs2.open(ost2.str().c_str());
      ofs2 << mMap.vpKeyFrames[i]->se3CfromW << endl;
      ofs2.close();
    }
    mMessageForUser << "  ... done saving map." << endl;
    return;
  }

  mMessageForUser << "! MapMaker::GUICommandHandler: unhandled command "<< sCommand << endl;
  exit(1);
};