MapMaker.cc
Go to the documentation of this file.
00001 // Copyright 2008 Isis Innovation Limited
00002 #include "ptam/MapMaker.h"
00003 #include "ptam/MapPoint.h"
00004 #include "ptam/Bundle.h"
00005 #include "ptam/PatchFinder.h"
00006 #include "ptam/SmallMatrixOpts.h"
00007 #include "ptam/HomographyInit.h"
00008 
00009 #include <cvd/vector_image_ref.h>
00010 #include <cvd/vision.h>
00011 #include <cvd/image_interpolate.h>
00012 
00013 #include <TooN/SVD.h>
00014 #include <TooN/SymEigen.h>
00015 
00016 #include <gvars3/instances.h>
00017 #include <fstream>
00018 #include <algorithm>
00019 
00020 #include <ptam/Params.h>
00021 
00022 #ifdef WIN32
00023 #define WIN32_LEAN_AND_MEAN
00024 #include <windows.h>
00025 #endif
00026 
00027 using namespace CVD;
00028 using namespace std;
00029 using namespace GVars3;
00030 
00031 //Weiss{ feature statistics
00032 //static unsigned int outcount[5]={0,0,0,0,0};
00033 //static unsigned int addcount[5]={0,0,0,0,0};
00034 //static unsigned int kfid=0;
00035 //}
00036 
00037 // Constructor sets up internal reference variable to Map.
00038 // Most of the intialisation is done by Reset()..
00039 MapMaker::MapMaker(Map& m, const ATANCamera &cam, ros::NodeHandle& nh)
00040 : mMap(m), mCamera(cam), octomap_interface(nh)
00041 {
00042   mbResetRequested = false;
00043   Reset();
00044   start(); // This CVD::thread func starts the map-maker thread with function run()
00045   GUI.RegisterCommand("SaveMap", GUICommandCallBack, this);
00046   //GV3::Register(mgvdWiggleScale, "MapMaker.WiggleScale", 0.1, SILENT); // Default to 10cm between keyframes
00047 };
00048 
00049 
00050 void MapMaker::Reset()
00051 {
00052   // This is only called from within the mapmaker thread...
00053   mMap.Reset();
00054   mvFailureQueue.clear();
00055   while(!mqNewQueue.empty()) mqNewQueue.pop();
00056   mMap.vpKeyFrames.clear(); // TODO: actually erase old keyframes
00057   mvpKeyFrameQueue.clear(); // TODO: actually erase old keyframes
00058   mbBundleRunning = false;
00059   mbBundleConverged_Full = true;
00060   mbBundleConverged_Recent = true;
00061   mbResetDone = true;
00062   mbResetRequested = false;
00063   mbBundleAbortRequested = false;
00064 }
00065 
00066 // CHECK_RESET is a handy macro which makes the mapmaker thread stop
00067 // what it's doing and reset, if required.
00068 #define CHECK_RESET if(mbResetRequested) {Reset(); continue;};
00069 
00070 void MapMaker::run()
00071 {
00072 
00073 #ifdef WIN32
00074   // For some reason, I get tracker thread starvation on Win32 when
00075   // adding key-frames. Perhaps this will help:
00076   SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_LOWEST);
00077 #endif
00078 
00079   
00080   const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
00081 
00082   while(!shouldStop())  // ShouldStop is a CVD::Thread func which return true if the thread is told to exit.
00083   {
00084     CHECK_RESET;
00085     sleep(1); // Sleep not really necessary, especially if mapmaker is busy
00086     CHECK_RESET;
00087 
00088     // Handle any GUI commands encountered..
00089     while(!mvQueuedCommands.empty())
00090     {
00091       GUICommandHandler(mvQueuedCommands.begin()->sCommand, mvQueuedCommands.begin()->sParams);
00092       mvQueuedCommands.erase(mvQueuedCommands.begin());
00093     }
00094 
00095     if(!mMap.IsGood())  // Nothing to do if there is no map yet!
00096       continue;
00097 
00098     // From here on, mapmaker does various map-maintenance jobs in a certain priority
00099     // Hierarchy. For example, if there's a new key-frame to be added (QueueSize() is >0)
00100     // then that takes high priority.
00101 
00102     CHECK_RESET;
00103     // Should we run local bundle adjustment?
00104     if(!mbBundleConverged_Recent && QueueSize() == 0)
00105       //Weiss{
00106       if (pPars.BundleMethod=="LOCAL" || pPars.BundleMethod=="LOCAL_GLOBAL")
00107         BundleAdjustRecent();
00108     //}
00109 
00110     CHECK_RESET;
00111     // Are there any newly-made map points which need more measurements from older key-frames?
00112     if(mbBundleConverged_Recent && QueueSize() == 0)
00113       ReFindNewlyMade();
00114 
00115     CHECK_RESET;
00116     // Run global bundle adjustment?
00117     if(mbBundleConverged_Recent && !mbBundleConverged_Full && QueueSize() == 0)
00118       //Weiss{
00119       if (pPars.BundleMethod=="GLOBAL" || pPars.BundleMethod=="LOCAL_GLOBAL")
00120         BundleAdjustAll();
00121 
00122     //}
00123 
00124     CHECK_RESET;
00125     // Very low priorty: re-find measurements marked as outliers
00126     if(mbBundleConverged_Recent && mbBundleConverged_Full && rand()%20 == 0 && QueueSize() == 0)
00127       ReFindFromFailureQueue();
00128 
00129     CHECK_RESET;
00130     HandleBadPoints();
00131 
00132     CHECK_RESET;
00133     // Any new key-frames to be added?
00134     if(QueueSize() > 0)
00135       AddKeyFrameFromTopOfQueue(); // Integrate into map data struct, and process
00136   }
00137 }
00138 
00139 
00140 // Tracker calls this to demand a reset
00141 void MapMaker::RequestReset()
00142 {
00143   mbResetDone = false;
00144   mbResetRequested = true;
00145 }
00146 
00147 bool MapMaker::ResetDone()
00148 {
00149   return mbResetDone;
00150 }
00151 
00152 // HandleBadPoints() Does some heuristic checks on all points in the map to see if 
00153 // they should be flagged as bad, based on tracker feedback.
00154 void MapMaker::HandleBadPoints()
00155 {
00156   // Did the tracker see this point as an outlier more often than as an inlier?
00157   for(unsigned int i=0; i<mMap.vpPoints.size(); i++)
00158   {
00159     MapPoint &p = *mMap.vpPoints[i];
00160     if(p.nMEstimatorOutlierCount > 20 && p.nMEstimatorOutlierCount > p.nMEstimatorInlierCount)
00161       p.bBad = true;
00162   }
00163 
00164   // All points marked as bad will be erased - erase all records of them
00165   // from keyframes in which they might have been measured.
00166   for(unsigned int i=0; i<mMap.vpPoints.size(); i++)
00167     if(mMap.vpPoints[i]->bBad)
00168     {
00169       //slynen octomap_interface{
00170       octomap_interface.deletePoint(mMap.vpPoints[i]);
00171       //}
00172       MapPoint::Ptr p = mMap.vpPoints[i];
00173       for(unsigned int j=0; j<mMap.vpKeyFrames.size(); j++)
00174       {
00175         KeyFrame &k = *mMap.vpKeyFrames[j];
00176         if(k.mMeasurements.count(p)){
00177           //slynen{ reprojection
00178 #ifdef KF_REPROJ
00179           //if the mapmaker wants to move a point to the trash which we use for keyframe reproj. we select a new point
00180           for(unsigned int ibest=0;ibest<k.iBestPointsCount;ibest++){ //check all best points
00181             //slynen copy this
00182             if(k.apCurrentBestPoints[ibest]==p) //is the point out of range or to be deleted one of our bestPoints?
00183             {
00184               k.apCurrentBestPoints[ibest].reset();
00185               int mindist = 9999;
00186               int tempdist = 0;
00187               Vector<2> ptlev0 = LevelZeroPos(p->irCenter, p->nSourceLevel); //to level zero coords
00188               for(int iP = k.vpPoints.size()-1; iP>=0; iP--){ //search a nearby point as new bestPoint
00189                 Vector<2> pt2lev0 = LevelZeroPos(k.vpPoints[iP]->irCenter, k.vpPoints[iP]->nSourceLevel);
00190                 tempdist = abs(ptlev0[0] - pt2lev0[0]) + abs(ptlev0[1] - pt2lev0[1]);   //calc simple dist
00191                 if(tempdist < mindist && ! k.vpPoints[iP]->bBad){                                       //if closer and not bad
00192                   mindist = tempdist;
00193                   k.apCurrentBestPoints[ibest] = k.vpPoints[iP]; //closest point as new point
00194                 }
00195               }
00196             }
00197           }
00198 #endif
00199           //}
00200           k.mMeasurements.erase(p);
00201         }
00202       }
00203     }
00204   // Move bad points to the trash list.
00205   mMap.MoveBadPointsToTrash();
00206 }
00207 
00208 MapMaker::~MapMaker()
00209 {
00210   mbBundleAbortRequested = true;
00211   stop(); // makes shouldStop() return true
00212   cout << "Waiting for mapmaker to die.." << endl;
00213   join();
00214   cout << " .. mapmaker has died." << endl;
00215 }
00216 
00217 
00218 // Finds 3d coords of point in reference frame B from two z=1 plane projections
00219 Vector<3> MapMaker::ReprojectPoint(SE3<> se3AfromB, const Vector<2> &v2A, const Vector<2> &v2B)
00220 {
00221   Matrix<3,4> PDash;
00222   PDash.slice<0,0,3,3>() = se3AfromB.get_rotation().get_matrix();
00223   PDash.slice<0,3,3,1>() = se3AfromB.get_translation().as_col();
00224 
00225   Matrix<4> A;
00226   A[0][0] = -1.0; A[0][1] =  0.0; A[0][2] = v2B[0]; A[0][3] = 0.0;
00227   A[1][0] =  0.0; A[1][1] = -1.0; A[1][2] = v2B[1]; A[1][3] = 0.0;
00228   A[2] = v2A[0] * PDash[2] - PDash[0];
00229   A[3] = v2A[1] * PDash[2] - PDash[1];
00230 
00231   SVD<4,4> svd(A);
00232   Vector<4> v4Smallest = svd.get_VT()[3];
00233   if(v4Smallest[3] == 0.0)
00234     v4Smallest[3] = 0.00001;
00235   return project(v4Smallest);
00236 }
00237 
00238 
00239 
00240 // InitFromStereo() generates the initial match from two keyframes
00241 // and a vector of image correspondences. Uses the 
00242 bool MapMaker::InitFromStereo(KeyFrame::Ptr kF,
00243                               KeyFrame::Ptr kS,
00244                               vector<pair<ImageRef, ImageRef> > &vTrailMatches,
00245                               SE3<> &se3TrackerPose)
00246 {
00247   //Weiss{
00248   //mdWiggleScale = 0.1;
00249   const FixParams& pPars = PtamParameters::fixparams();
00250   mdWiggleScale=pPars.WiggleScale;
00251   //mdWiggleScale = *mgvdWiggleScale; // Cache this for the new map.
00252   //}
00253 
00254   mCamera.SetImageSize(kF->aLevels[0].im.size());
00255 
00256 //Weiss{
00257         if(vTrailMatches.size()<4)
00258         {
00259                 ROS_WARN_STREAM("Too few matches to init.");
00260                 return false;
00261         }
00262 
00265 
00266   //    vector<HomographyMatch> vMatches;
00267   //    TooN::Matrix<> X(vTrailMatches.size(), 3);
00268   //    SO3<> rotmat = SO3<>::exp(rotvec);
00269   //
00270   //    for(unsigned int i=0; i<vTrailMatches.size(); i++)
00271   //    {
00272   //            HomographyMatch m;
00273   //            m.v2CamPlaneFirst = mCamera.UnProject(vTrailMatches[i].first);
00274   //            m.v2CamPlaneSecond = mCamera.UnProject(vTrailMatches[i].second);
00275   //            Vector<3> p1 = makeVector(m.v2CamPlaneFirst[0],m.v2CamPlaneFirst[1],1);
00276   //            p1=p1/norm(p1);
00277   //            Vector<3> p2 = rotmat.inverse()*makeVector(m.v2CamPlaneSecond[0],m.v2CamPlaneSecond[1],1);
00278   //            p2=p2/norm(p2);
00279   //            Vector<3> of= p2-p1;
00280   //
00281   //            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]);
00282   //
00283   //            vMatches.push_back(m);
00284   //    }
00285   //
00286   //    SVD<> svdX(X);
00287   //    SE3<> se3;
00288   //    se3.get_rotation()=rotmat;
00289   //    Vector<3> trans = makeVector(svdX.get_VT()[2][0],svdX.get_VT()[2][1],svdX.get_VT()[2][2]);
00290   //    se3.get_translation()= -trans*mdWiggleScale/norm(trans);
00291 
00293 
00294 
00295 
00296   vector<HomographyMatch> vMatches;
00297   for(unsigned int i=0; i<vTrailMatches.size(); i++)
00298   {
00299     HomographyMatch m;
00300     m.v2CamPlaneFirst = mCamera.UnProject(vTrailMatches[i].first);
00301     m.v2CamPlaneSecond = mCamera.UnProject(vTrailMatches[i].second);
00302     m.m2PixelProjectionJac = mCamera.GetProjectionDerivs();
00303     vMatches.push_back(m);
00304   }
00305 
00306 
00307   //Weiss{
00308   if(vMatches.size()<4)
00309     return false;
00310   //}
00311 
00312   SE3<> se3;
00313   bool bGood;
00314   HomographyInit HomographyInit;
00315 
00316   bGood = HomographyInit.Compute(vMatches, 5.0, se3);
00317 
00318   if(!bGood)
00319   {
00320     mMessageForUser << "  Could not init from stereo pair, try again." << endl;
00321     return false;
00322   }
00323 
00324   // Check that the initialiser estimated a non-zero baseline
00325   double dTransMagn = sqrt(se3.get_translation() * se3.get_translation());
00326   if(dTransMagn == 0)
00327   {
00328     mMessageForUser << "  Estimated zero baseline from stereo pair, try again." << endl;
00329     return false;
00330   }
00331   // change the scale of the map so the second camera is wiggleScale away from the first
00332   se3.get_translation() *= mdWiggleScale/dTransMagn;
00333 
00334 
00335   KeyFrame::Ptr pkFirst = kF;
00336   KeyFrame::Ptr pkSecond = kS;
00337 
00338   pkFirst->bFixed = true;
00339   pkFirst->se3CfromW = SE3<>();
00340 
00341   pkSecond->bFixed = false;
00342   pkSecond->se3CfromW = se3;
00343 
00344   // Construct map from the stereo matches.
00345   PatchFinder finder;
00346 
00347   for(unsigned int i=0; i<vMatches.size(); i++)
00348   {
00349     MapPoint::Ptr p(new MapPoint());
00350 
00351     // Patch source stuff:
00352     p->pPatchSourceKF = pkFirst;
00353     p->nSourceLevel = 0;
00354     p->v3Normal_NC = makeVector( 0,0,-1);
00355     p->irCenter = vTrailMatches[i].first;
00356     p->v3Center_NC = unproject(mCamera.UnProject(p->irCenter));
00357     p->v3OneDownFromCenter_NC = unproject(mCamera.UnProject(p->irCenter + ImageRef(0,1)));
00358     p->v3OneRightFromCenter_NC = unproject(mCamera.UnProject(p->irCenter + ImageRef(1,0)));
00359     normalize(p->v3Center_NC);
00360     normalize(p->v3OneDownFromCenter_NC);
00361     normalize(p->v3OneRightFromCenter_NC);
00362     p->RefreshPixelVectors();
00363 
00364     // Do sub-pixel alignment on the second image
00365     finder.MakeTemplateCoarseNoWarp(p);
00366     finder.MakeSubPixTemplate();
00367     finder.SetSubPixPos(vec(vTrailMatches[i].second));
00368     bool bGood = finder.IterateSubPixToConvergence(*pkSecond,10);
00369     if(!bGood)
00370     {
00371       continue;
00372     }
00373 
00374     // Triangulate point:
00375     Vector<2> v2SecondPos = finder.GetSubPixPos();
00376     p->v3WorldPos = ReprojectPoint(se3, mCamera.UnProject(v2SecondPos), vMatches[i].v2CamPlaneFirst);
00377     if(p->v3WorldPos[2] < 0.0)
00378     {
00379       continue;
00380     }
00381 
00382     // Not behind map? Good, then add to map.
00383     p->pMMData = new MapMakerData();
00384     mMap.vpPoints.push_back(p);
00385 
00386     // Construct first two measurements and insert into relevant DBs:
00387     Measurement mFirst;
00388     mFirst.nLevel = 0;
00389     mFirst.Source = Measurement::SRC_ROOT;
00390     mFirst.v2RootPos = vec(vTrailMatches[i].first);
00391     mFirst.bSubPix = true;
00392     pkFirst->mMeasurements[p] = mFirst;
00393     p->pMMData->sMeasurementKFs.insert(pkFirst);
00394 
00395     Measurement mSecond;
00396     mSecond.nLevel = 0;
00397     mSecond.Source = Measurement::SRC_TRAIL;
00398     mSecond.v2RootPos = finder.GetSubPixPos();
00399     mSecond.bSubPix = true;
00400     pkSecond->mMeasurements[p] = mSecond;
00401     p->pMMData->sMeasurementKFs.insert(pkSecond);
00402     //slynen{ reprojection
00403 #ifdef KF_REPROJ
00404     pkFirst->AddKeyMapPoint(p);
00405     pkSecond->AddKeyMapPoint(p);
00406 #endif
00407     //}
00408   }
00409 
00410 //Weiss{
00411         if(mMap.vpPoints.size()<4)
00412         {
00413                 ROS_WARN_STREAM("Too few map points to init.");
00414                 return false;
00415         }
00416 //}
00417 
00418   mMap.vpKeyFrames.push_back(pkFirst);
00419   mMap.vpKeyFrames.push_back(pkSecond);
00420   pkFirst->MakeKeyFrame_Rest();
00421   pkSecond->MakeKeyFrame_Rest();
00422   //Weiss{
00423   //    for(int i=0; i<5; i++)
00424   /*}*/ BundleAdjustAll();
00425 
00426   // Estimate the feature depth distribution in the first two key-frames
00427   // (Needed for epipolar search)
00428 
00429 //Weiss{
00430         if(!RefreshSceneDepth(pkFirst))
00431         {
00432                 ROS_WARN_STREAM("Something is seriously wrong with the first KF.");
00433                 return false;
00434         }
00435         if(!RefreshSceneDepth(pkSecond))
00436         {
00437                 ROS_WARN_STREAM("Something is seriously wrong with the second KF.");
00438                 return false;
00439         }
00440         mdWiggleScaleDepthNormalized = mdWiggleScale / pkFirst->dSceneDepthMean;
00441 
00442 
00443   //check if point have been added
00444   const VarParams& pParams = PtamParameters::varparams();
00445   bool addedsome=false;
00446   addedsome |= AddSomeMapPoints(3);
00447   if(!pParams.NoLevelZeroMapPoints)
00448     addedsome |= AddSomeMapPoints(0);
00449   addedsome |= AddSomeMapPoints(1);
00450   addedsome |= AddSomeMapPoints(2);
00451   if(!addedsome)
00452         {
00453                 ROS_WARN_STREAM("Could not add any map points on any level - abort init.");
00454                 return false;
00455         }
00456   //}
00457 
00458   mbBundleConverged_Full = false;
00459   mbBundleConverged_Recent = false;
00460   //Weiss{
00461   double nloops=0;
00462   while(!mbBundleConverged_Full & (nloops<pParams.MaxStereoInitLoops))
00463   {
00464     BundleAdjustAll();
00465     if(mbResetRequested | (nloops>=pParams.MaxStereoInitLoops))
00466       return false;
00467     nloops++;
00468   }
00469 
00470   //sanity check: if the point variance is too large assume init is crap --> very hacky, assumes flat init scene!!
00471   if(((pkFirst->dSceneDepthSigma+pkSecond->dSceneDepthSigma)/2.0>0.5) && pParams.CheckInitMapVar)
00472   {
00473     ROS_WARN_STREAM("Initial map rejected because of too large point variance. Point sigma: " << ((pkFirst->dSceneDepthSigma+pkSecond->dSceneDepthSigma)/2.0));
00474     return false;
00475   }
00476 
00477   //}
00478 
00479   // Rotate and translate the map so the dominant plane is at z=0:
00480   ApplyGlobalTransformationToMap(CalcPlaneAligner());
00481   mMap.bGood = true;
00482   se3TrackerPose = pkSecond->se3CfromW;
00483 
00484   mMessageForUser << "  MapMaker: made initial map with " << mMap.vpPoints.size() << " points." << endl;
00485 
00486   //slynen octomap_interface{
00487     octomap_interface.addKeyFrame(pkFirst);
00488     octomap_interface.addKeyFrame(pkSecond);
00489   //}
00490 
00491   return true;
00492 }
00493 
00494 // ThinCandidates() Thins out a key-frame's candidate list.
00495 // Candidates are those salient corners where the mapmaker will attempt 
00496 // to make a new map point by epipolar search. We don't want to make new points
00497 // where there are already existing map points, this routine erases such candidates.
00498 // Operates on a single level of a keyframe.
00499 void MapMaker::ThinCandidates(KeyFrame::Ptr k, int nLevel)
00500 {
00501   vector<Candidate> &vCSrc = k->aLevels[nLevel].vCandidates;
00502   vector<Candidate> vCGood;
00503   vector<ImageRef> irBusyLevelPos;
00504   // Make a list of `busy' image locations, which already have features at the same level
00505   // or at one level higher.
00506   for(meas_it it = k->mMeasurements.begin(); it!=k->mMeasurements.end(); it++)
00507   {
00508     if(!(it->second.nLevel == nLevel || it->second.nLevel == nLevel + 1))
00509       continue;
00510     irBusyLevelPos.push_back(ir_rounded(it->second.v2RootPos / LevelScale(nLevel)));
00511   }
00512 
00513   // Only keep those candidates further than 10 pixels away from busy positions.
00514   unsigned int nMinMagSquared = 10*10;
00515   for(unsigned int i=0; i<vCSrc.size(); i++)
00516   {
00517     ImageRef irC = vCSrc[i].irLevelPos;
00518     bool bGood = true;
00519     for(unsigned int j=0; j<irBusyLevelPos.size(); j++)
00520     {
00521       ImageRef irB = irBusyLevelPos[j];
00522       if((irB - irC).mag_squared() < nMinMagSquared)
00523       {
00524         bGood = false;
00525         break;
00526       }
00527     }
00528     if(bGood)
00529       vCGood.push_back(vCSrc[i]);
00530   }
00531   vCSrc = vCGood;
00532 }
00533 
00534 // Adds map points by epipolar search to the last-added key-frame, at a single
00535 // specified pyramid level. Does epipolar search in the target keyframe as closest by
00536 // the ClosestKeyFrame function.
00537 bool MapMaker::AddSomeMapPoints(int nLevel)
00538 {
00539   //Weiss{
00540   bool addedsome=false;
00541   //}
00542   KeyFrame::Ptr kSrc = (mMap.vpKeyFrames[mMap.vpKeyFrames.size() - 1]); // The new keyframe
00543   KeyFrame::Ptr kTarget = ClosestKeyFrame(kSrc);
00544   Level &l = kSrc->aLevels[nLevel];
00545 
00546   ThinCandidates(kSrc, nLevel);
00547   for(unsigned int i = 0; i<l.vCandidates.size(); i++)
00548     addedsome|=AddPointEpipolar(kSrc, kTarget, nLevel, i);
00549   return addedsome;
00550 };
00551 
00552 // Rotates/translates the whole map and all keyframes
00553 void MapMaker::ApplyGlobalTransformationToMap(SE3<> se3NewFromOld)
00554 {
00555   for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
00556     mMap.vpKeyFrames[i]->se3CfromW = mMap.vpKeyFrames[i]->se3CfromW * se3NewFromOld.inverse();
00557 
00558 //  SO3<> so3Rot = se3NewFromOld.get_rotation();
00559   for(unsigned int i=0; i<mMap.vpPoints.size(); i++)
00560   {
00561     mMap.vpPoints[i]->v3WorldPos =
00562         se3NewFromOld * mMap.vpPoints[i]->v3WorldPos;
00563     mMap.vpPoints[i]->RefreshPixelVectors();
00564   }
00565 }
00566 
00567 // Applies a global scale factor to the map
00568 void MapMaker::ApplyGlobalScaleToMap(double dScale)
00569 {
00570   for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
00571     mMap.vpKeyFrames[i]->se3CfromW.get_translation() *= dScale;
00572 
00573   for(unsigned int i=0; i<mMap.vpPoints.size(); i++)
00574   {
00575     (*mMap.vpPoints[i]).v3WorldPos *= dScale;
00576     (*mMap.vpPoints[i]).v3PixelRight_W *= dScale;
00577     (*mMap.vpPoints[i]).v3PixelDown_W *= dScale;
00578     (*mMap.vpPoints[i]).RefreshPixelVectors();
00579   }
00580 }
00581 
00582 // The tracker entry point for adding a new keyframe;
00583 // the tracker thread doesn't want to hang about, so 
00584 // just dumps it on the top of the mapmaker's queue to 
00585 // be dealt with later, and return.
00586 void MapMaker::AddKeyFrame(KeyFrame::Ptr k)
00587 {
00588   k->pSBI = NULL; // Mapmaker uses a different SBI than the tracker, so will re-gen its own
00589   mvpKeyFrameQueue.push_back(k);
00590   if(mbBundleRunning)   // Tell the mapmaker to stop doing low-priority stuff and concentrate on this KF first.
00591     mbBundleAbortRequested = true;
00592 }
00593 
00594 // Mapmaker's code to handle incoming key-frames.
00595 void MapMaker::AddKeyFrameFromTopOfQueue()
00596 {
00597   if(mvpKeyFrameQueue.size() == 0)
00598     return;
00599 
00600   KeyFrame::Ptr pK = mvpKeyFrameQueue[0];
00601   mvpKeyFrameQueue.erase(mvpKeyFrameQueue.begin());
00602 
00603   //Weiss{
00604 
00605   
00606   const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
00607   if (pPars.MaxKF>1)    // MaxKF<2 means keep all KFs
00608   {
00609     while (mMap.vpKeyFrames.size()>(unsigned int)pPars.MaxKF)
00610     {
00611       // find farthest KF
00612       double dFarthestDist = 0;
00613       vector<KeyFrame::Ptr>::iterator itFarthest = mMap.vpKeyFrames.begin();
00614 
00615       for(vector<KeyFrame::Ptr>::iterator it = mMap.vpKeyFrames.begin();it!=mMap.vpKeyFrames.end();++it)
00616       {
00617         double dDist = KeyFrameLinearDist(pK, *it);
00618         if(dDist > dFarthestDist)
00619         {
00620           dFarthestDist = dDist;
00621           itFarthest = it;
00622         }
00623       }
00624 
00625       for (unsigned int i=0;i<mMap.vpPoints.size();i++)
00626         if (mMap.vpPoints[i]->pPatchSourceKF==*itFarthest)
00627           mMap.vpPoints[i]->bBad=true;
00628 
00629       if ((*itFarthest)->bFixed)        // if we delete the a fix KF, fix the oldest one
00630       {
00631         mMap.vpKeyFrames.erase(itFarthest);
00632         mMap.vpKeyFrames[0]->bFixed=true;
00633       }
00634       else
00635         mMap.vpKeyFrames.erase(itFarthest);
00636     }
00637   }
00638   //}
00639 
00640   pK->MakeKeyFrame_Rest();
00641   //Weiss{ feature statistics
00642   //    kfid = pK->ID;
00643   //}
00644   mMap.vpKeyFrames.push_back(pK);
00645   // Any measurements? Update the relevant point's measurement counter status map
00646   for(meas_it it = pK->mMeasurements.begin();
00647       it!=pK->mMeasurements.end();
00648       it++)
00649   {
00650     it->first->pMMData->sMeasurementKFs.insert(pK);
00651     it->second.Source = Measurement::SRC_TRACKER;
00652   }
00653   // And maybe we missed some - this now adds to the map itself, too.
00654   ReFindInSingleKeyFrame(pK);
00655 
00656   bool addedsome=false;
00657   addedsome |= AddSomeMapPoints(3);       // .. and add more map points by epipolar search.
00658   if(!pPars.NoLevelZeroMapPoints)
00659     addedsome |= AddSomeMapPoints(0);
00660   addedsome |= AddSomeMapPoints(1);
00661   addedsome |= AddSomeMapPoints(2);
00662 
00663   //Weiss{ feature statistics
00664   //    ROS_WARN_STREAM("\ntotall : " << addcount[4] << " totlvl0: " << addcount[0]  << " totlvl1: " <<  addcount[1]  << " totlvl2: " << addcount[2]  << " totlvl3: " <<  addcount[3]);
00665   //    ROS_WARN_STREAM("\noutall : " << outcount[4] << " outlvl0: " << outcount[0]  << " outlvl1: " <<  outcount[1]  << " outlvl2: " << outcount[2]  << " outlvl3: " <<  outcount[3]);
00666   //}
00667 
00668   mbBundleConverged_Full = false;
00669   mbBundleConverged_Recent = false;
00670 
00671   //slynen octomap_interface{
00672   octomap_interface.addKeyFrame(pK);
00673   //}
00674 }
00675 
00676 // Tries to make a new map point out of a single candidate point
00677 // by searching for that point in another keyframe, and triangulating
00678 // if a match is found.
00679 bool MapMaker::AddPointEpipolar(KeyFrame::Ptr kSrc,
00680                                 KeyFrame::Ptr kTarget,
00681                                 int nLevel,
00682                                 int nCandidate)
00683 {
00684   static Image<Vector<2> > imUnProj;
00685   static bool bMadeCache = false;
00686   if(!bMadeCache)
00687   {
00688     imUnProj.resize(kSrc->aLevels[0].im.size());
00689     ImageRef ir;
00690     do imUnProj[ir] = mCamera.UnProject(ir);
00691     while(ir.next(imUnProj.size()));
00692     bMadeCache = true;
00693   }
00694 
00695   int nLevelScale = LevelScale(nLevel);
00696   Candidate &candidate = kSrc->aLevels[nLevel].vCandidates[nCandidate];
00697   ImageRef irLevelPos = candidate.irLevelPos;
00698   Vector<2> v2RootPos = LevelZeroPos(irLevelPos, nLevel);
00699 
00700   Vector<3> v3Ray_SC = unproject(mCamera.UnProject(v2RootPos)); // direction vector from current camera to the feature? [SW]
00701   normalize(v3Ray_SC);
00702   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]
00703 
00704   // Restrict epipolar search to a relatively narrow depth range
00705   // to increase reliability
00706   double dMean = kSrc->dSceneDepthMean;
00707   double dSigma = kSrc->dSceneDepthSigma;
00708   //Weiss{  comments marked with [SW]
00709   //    double dStartDepth = max(mdWiggleScale, dMean - dSigma); // estimated minimal distance of the feature in the current KF? [SW]
00710   //    double dEndDepth = min(40 * mdWiggleScale, dMean + dSigma); // estimated maximal distance of the feature in the current KF? [SW]
00711   //    double dStartDepth = max(mdWiggleScale, dMean - dSigma); // estimated minimal distance of the feature in the current KF? [SW]
00712   double dStartDepth = max(0.0, dMean - dSigma); // estimated minimal distance of the feature in the current KF? [SW]
00713   double dEndDepth = dMean + dSigma; // estimated maximal distance of the feature in the current KF? [SW]
00714   //}
00715   Vector<3> v3CamCenter_TC = kTarget->se3CfromW * kSrc->se3CfromW.inverse().get_translation(); // The camera of the current camera seen from the target KF? [SW]
00716   Vector<3> v3RayStart_TC = v3CamCenter_TC + dStartDepth * v3LineDirn_TC;                           // start point on the 3D epi-line expressed in the target KF? [SW]
00717   Vector<3> v3RayEnd_TC = v3CamCenter_TC + dEndDepth * v3LineDirn_TC;                               // end point on the 3d epi-line expressed in the target KF? [SW]
00718 
00719   //Weiss{
00721   // it is mainly to speed up calculation, but breaks on large means (dMean>40), since then v3RayEnd_TC[2] <= v3RayStart_TC[2]
00722   // is always true
00723   // 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...
00724   // this happens rarely though...
00725   //    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
00726   //            return false;
00728   //}
00729   if(v3RayEnd_TC[2] <= 0.0 )  return false;
00730   if(v3RayStart_TC[2] <= 0.0)
00731     v3RayStart_TC += v3LineDirn_TC * (0.001 - v3RayStart_TC[2] / v3LineDirn_TC[2]);
00732 
00733   Vector<2> v2A = project(v3RayStart_TC);
00734   Vector<2> v2B = project(v3RayEnd_TC);
00735   Vector<2> v2AlongProjectedLine = v2A-v2B;
00736 
00737   if(v2AlongProjectedLine * v2AlongProjectedLine < 0.00000001)
00738   {
00739     mMessageForUser << "v2AlongProjectedLine too small." << endl;
00740     return false;
00741   }
00742   normalize(v2AlongProjectedLine);
00743   Vector<2> v2Normal;
00744   v2Normal[0] = v2AlongProjectedLine[1];
00745   v2Normal[1] = -v2AlongProjectedLine[0];
00746 
00747   double dNormDist = v2A * v2Normal;
00748   if(fabs(dNormDist) > mCamera.LargestRadiusInImage() )
00749     return false;
00750 
00751   double dMinLen = min(v2AlongProjectedLine * v2A, v2AlongProjectedLine * v2B) - 0.05;
00752   double dMaxLen = max(v2AlongProjectedLine * v2A, v2AlongProjectedLine * v2B) + 0.05;
00753   if(dMinLen < -2.0)  dMinLen = -2.0;
00754   if(dMaxLen < -2.0)  dMaxLen = -2.0;
00755   if(dMinLen > 2.0)   dMinLen = 2.0;
00756   if(dMaxLen > 2.0)   dMaxLen = 2.0;
00757 
00758   // Find current-frame corners which might match this
00759   PatchFinder Finder;
00760   Finder.MakeTemplateCoarseNoWarp(kSrc, nLevel, irLevelPos);
00761   if(Finder.TemplateBad())  return false;
00762 
00763   vector<Vector<2> > &vv2Corners = kTarget->aLevels[nLevel].vImplaneCorners;
00764   vector<ImageRef> &vIR = kTarget->aLevels[nLevel].vCorners;
00765   if(!kTarget->aLevels[nLevel].bImplaneCornersCached)
00766   {
00767     for(unsigned int i=0; i<vIR.size(); i++)   // over all corners in target img..
00768       vv2Corners.push_back(imUnProj[ir(LevelZeroPos(vIR[i], nLevel))]);
00769     kTarget->aLevels[nLevel].bImplaneCornersCached = true;
00770   }
00771 
00772   int nBest = -1;
00773   int nBestZMSSD = Finder.mnMaxSSD + 1;
00774   double dMaxDistDiff = mCamera.OnePixelDist() * (4.0 + 1.0 * nLevelScale);
00775   double dMaxDistSq = dMaxDistDiff * dMaxDistDiff;
00776 
00777   for(unsigned int i=0; i<vv2Corners.size(); i++)   // over all corners in target img..
00778   {
00779     Vector<2> v2Im = vv2Corners[i];
00780     double dDistDiff = dNormDist - v2Im * v2Normal;
00781     if(dDistDiff * dDistDiff > dMaxDistSq)      continue; // skip if not along epi line
00782     if(v2Im * v2AlongProjectedLine < dMinLen)   continue; // skip if not far enough along line
00783     if(v2Im * v2AlongProjectedLine > dMaxLen)   continue; // or too far
00784     int nZMSSD = Finder.ZMSSDAtPoint(kTarget->aLevels[nLevel].im, vIR[i]);
00785     if(nZMSSD < nBestZMSSD)
00786     {
00787       nBest = i;
00788       nBestZMSSD = nZMSSD;
00789     }
00790   }
00791 
00792   if(nBest == -1)   return false;   // Nothing found.
00793 
00794   //  Found a likely candidate along epipolar ray
00795   Finder.MakeSubPixTemplate();
00796   Finder.SetSubPixPos(LevelZeroPos(vIR[nBest], nLevel));
00797   bool bSubPixConverges = Finder.IterateSubPixToConvergence(*kTarget,10);
00798   if(!bSubPixConverges)
00799     return false;
00800 
00801   // Now triangulate the 3d point...
00802   Vector<3> v3New;
00803   v3New = kTarget->se3CfromW.inverse() *
00804       ReprojectPoint(kSrc->se3CfromW * kTarget->se3CfromW.inverse(),
00805                      mCamera.UnProject(v2RootPos),
00806                      mCamera.UnProject(Finder.GetSubPixPos()));
00807 
00808   MapPoint::Ptr pNew(new MapPoint());
00809   pNew->v3WorldPos = v3New;
00810   pNew->pMMData = new MapMakerData();
00811 
00812   // Patch source stuff:
00813   pNew->pPatchSourceKF = kSrc;
00814   pNew->nSourceLevel = nLevel;
00815   pNew->v3Normal_NC = makeVector( 0,0,-1);
00816   pNew->irCenter = irLevelPos;
00817   pNew->v3Center_NC = unproject(mCamera.UnProject(v2RootPos));
00818   pNew->v3OneRightFromCenter_NC = unproject(mCamera.UnProject(v2RootPos + vec(ImageRef(nLevelScale,0))));
00819   pNew->v3OneDownFromCenter_NC  = unproject(mCamera.UnProject(v2RootPos + vec(ImageRef(0,nLevelScale))));
00820 
00821   normalize(pNew->v3Center_NC);
00822   normalize(pNew->v3OneDownFromCenter_NC);
00823   normalize(pNew->v3OneRightFromCenter_NC);
00824 
00825   pNew->RefreshPixelVectors();
00826 
00827   mMap.vpPoints.push_back(pNew);
00828   mqNewQueue.push(pNew);
00829   Measurement m;
00830   m.Source = Measurement::SRC_ROOT;
00831   m.v2RootPos = v2RootPos;
00832   m.nLevel = nLevel;
00833   m.bSubPix = true;
00834   kSrc->mMeasurements[pNew] = m;
00835 
00836   m.Source = Measurement::SRC_EPIPOLAR;
00837   m.v2RootPos = Finder.GetSubPixPos();
00838   kTarget->mMeasurements[pNew] = m;
00839   pNew->pMMData->sMeasurementKFs.insert(kSrc);
00840   pNew->pMMData->sMeasurementKFs.insert(kTarget);
00841   //slynen{ reprojection
00842 #ifdef KF_REPROJ
00843   kSrc->AddKeyMapPoint(pNew);
00844   kTarget->AddKeyMapPoint(pNew);
00845 #endif
00846   //}
00847 
00848   return true;
00849 }
00850 
00851 double MapMaker::KeyFrameLinearDist(KeyFrame::Ptr k1, KeyFrame::Ptr k2)
00852 {
00853   Vector<3> v3KF1_CamPos = k1->se3CfromW.inverse().get_translation();
00854   Vector<3> v3KF2_CamPos = k2->se3CfromW.inverse().get_translation();
00855   Vector<3> v3Diff = v3KF2_CamPos - v3KF1_CamPos;
00856   double dDist = sqrt(v3Diff * v3Diff);
00857   return dDist;
00858 }
00859 
00860 vector<KeyFrame::Ptr> MapMaker::NClosestKeyFrames(KeyFrame::Ptr k, unsigned int N)
00861 {
00862   vector<pair<double, KeyFrame::Ptr > > vKFandScores;
00863   for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
00864   {
00865     if(mMap.vpKeyFrames[i] == k)
00866       continue;
00867     double dDist = KeyFrameLinearDist(k, mMap.vpKeyFrames[i]);
00868     vKFandScores.push_back(make_pair(dDist, mMap.vpKeyFrames[i]));
00869   }
00870   if(N > vKFandScores.size())
00871     N = vKFandScores.size();
00872   partial_sort(vKFandScores.begin(), vKFandScores.begin() + N, vKFandScores.end());
00873 
00874   vector<KeyFrame::Ptr> vResult;
00875   for(unsigned int i=0; i<N; i++)
00876     vResult.push_back(vKFandScores[i].second);
00877   return vResult;
00878 }
00879 
00880 KeyFrame::Ptr MapMaker::ClosestKeyFrame(KeyFrame::Ptr k)
00881 {
00882   double dClosestDist = 9999999999.9;
00883   int nClosest = -1;
00884   for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
00885   {
00886     if(mMap.vpKeyFrames[i] == k)
00887       continue;
00888     double dDist = KeyFrameLinearDist(k, mMap.vpKeyFrames[i]);
00889     if(dDist < dClosestDist)
00890     {
00891       dClosestDist = dDist;
00892       nClosest = i;
00893     }
00894   }
00895   assert(nClosest != -1);
00896   return mMap.vpKeyFrames[nClosest];
00897 }
00898 
00899 double MapMaker::DistToNearestKeyFrame(KeyFrame::Ptr kCurrent)
00900 {
00901   KeyFrame::Ptr pClosest = ClosestKeyFrame(kCurrent);
00902   double dDist = KeyFrameLinearDist(kCurrent, pClosest);
00903   return dDist;
00904 }
00905 
00906 bool MapMaker::NeedNewKeyFrame(KeyFrame::Ptr kCurrent)
00907 {
00908   KeyFrame::Ptr pClosest = ClosestKeyFrame(kCurrent);
00909   double dDist = KeyFrameLinearDist(kCurrent, pClosest);
00910 
00911   //Weiss{
00912   
00913   const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
00914 
00915   std::vector<double> medianpixdist;
00916   Vector<3> veccam2;
00917   Vector<2> cam2,cam1;
00918   for(std::map<MapPoint::Ptr, Measurement>::iterator it = kCurrent->mMeasurements.begin();it != kCurrent->mMeasurements.end(); it++)
00919   {
00920     veccam2 = pClosest->se3CfromW*it->first->v3WorldPos;        // rotate in closest KF
00921     veccam2 = (kCurrent->se3CfromW*pClosest->se3CfromW.inverse()).get_rotation()*veccam2;       // derot in current frame
00922     cam2 = mCamera.Project(makeVector(veccam2[0]/veccam2[2],veccam2[1]/veccam2[2]));
00923     cam1 = it->second.v2RootPos;
00924     medianpixdist.push_back(sqrt((cam2[0]-cam1[0])*(cam2[0]-cam1[0]) + (cam2[1]-cam1[1])*(cam2[1]-cam1[1])));
00925   }
00926   std::vector<double>::iterator first = medianpixdist.begin();
00927   std::vector<double>::iterator last = medianpixdist.end();
00928   std::vector<double>::iterator middle = first + std::floor((last - first) / 2);
00929   std::nth_element(first, middle, last); // can specify comparator as optional 4th arg
00930   double mediandist = *middle;
00931 
00932   dDist *= (1.0 / kCurrent->dSceneDepthMean);
00933 
00934   //    
00935   //    const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
00936   //if(dDist > GV2.GetDouble("MapMaker.MaxKFDistWiggleMult",1.0,SILENT) * mdWiggleScaleDepthNormalized)
00937   if(pPars.UseKFPixelDist & (mediandist>pPars.AutoInitPixel))
00938     return true;
00939 
00940   if(dDist > pPars.MaxKFDistWiggleMult * mdWiggleScaleDepthNormalized)
00941   {
00942     return true;
00943   }
00944   return false;
00945   //}
00946 }
00947 
00948 // Perform bundle adjustment on all keyframes, all map points
00949 void MapMaker::BundleAdjustAll()
00950 {
00951   // construct the sets of kfs/points to be adjusted:
00952   // in this case, all of them
00953   set<KeyFrame::Ptr> sAdj;
00954   set<KeyFrame::Ptr> sFixed;
00955   for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
00956     if(mMap.vpKeyFrames[i]->bFixed)
00957       sFixed.insert(mMap.vpKeyFrames[i]);
00958     else
00959       sAdj.insert(mMap.vpKeyFrames[i]);
00960 
00961   set<MapPoint::Ptr> sMapPoints;
00962   for(unsigned int i=0; i<mMap.vpPoints.size();i++)
00963     sMapPoints.insert(mMap.vpPoints[i]);
00964 
00965   BundleAdjust(sAdj, sFixed, sMapPoints, false);
00966 }
00967 
00968 // Peform a local bundle adjustment which only adjusts
00969 // recently added key-frames
00970 void MapMaker::BundleAdjustRecent()
00971 {
00972   if(mMap.vpKeyFrames.size() < 8)
00973   { // Ignore this unless map is big enough
00974     mbBundleConverged_Recent = true;
00975     return;
00976   }
00977 
00978 
00979   // First, make a list of the keyframes we want adjusted in the adjuster.
00980   // This will be the last keyframe inserted, and its four nearest neighbors
00981   set<KeyFrame::Ptr> sAdjustSet;
00982 
00983   //Weiss{ if we only keep N KFs only insert most recently added KF as loose
00984   unsigned char nloose = 4;
00985   
00986   const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
00987   if (pPars.MaxKF>1)    // MaxKF<2 means keep all KFs
00988     nloose=0;
00989 
00990   KeyFrame::Ptr pkfNewest = mMap.vpKeyFrames.back();
00991   sAdjustSet.insert(pkfNewest);
00992   vector<KeyFrame::Ptr> vClosest = NClosestKeyFrames(pkfNewest, 4);
00993   for(int i=0; i<nloose; i++)
00994     if(vClosest[i]->bFixed == false)
00995       sAdjustSet.insert(vClosest[i]);
00996   //}
00997 
00998   // Now we find the set of features which they contain.
00999   set<MapPoint::Ptr> sMapPoints;
01000   for(set<KeyFrame::Ptr>::iterator iter = sAdjustSet.begin();
01001       iter!=sAdjustSet.end();
01002       iter++)
01003   {
01004     map<MapPoint::Ptr,Measurement> &mKFMeas = (*iter)->mMeasurements;
01005     for(meas_it jiter = mKFMeas.begin(); jiter!= mKFMeas.end(); jiter++)
01006       sMapPoints.insert(jiter->first);
01007   };
01008 
01009   // Finally, add all keyframes which measure above points as fixed keyframes
01010   set<KeyFrame::Ptr> sFixedSet;
01011   for(vector<KeyFrame::Ptr>::iterator it = mMap.vpKeyFrames.begin(); it!=mMap.vpKeyFrames.end(); it++)
01012   {
01013     if(sAdjustSet.count(*it))
01014       continue;
01015     bool bInclude = false;
01016     for(meas_it jiter = (*it)->mMeasurements.begin(); jiter!= (*it)->mMeasurements.end(); jiter++)
01017       if(sMapPoints.count(jiter->first))
01018       {
01019         bInclude = true;
01020         break;
01021       }
01022     if(bInclude)
01023       sFixedSet.insert(*it);
01024   }
01025 
01026   BundleAdjust(sAdjustSet, sFixedSet, sMapPoints, true);
01027 }
01028 
01029 // Common bundle adjustment code. This creates a bundle-adjust instance, populates it, and runs it.
01030 void MapMaker::BundleAdjust(set<KeyFrame::Ptr> sAdjustSet, set<KeyFrame::Ptr> sFixedSet, set<MapPoint::Ptr> sMapPoints, bool bRecent)
01031 {
01032   Bundle b(mCamera);   // Our bundle adjuster
01033   mbBundleRunning = true;
01034   mbBundleRunningIsRecent = bRecent;
01035 
01036   // The bundle adjuster does different accounting of keyframes and map points;
01037   // Translation maps are stored:
01038   map<MapPoint::Ptr, int> mPoint_BundleID;
01039   map<int, MapPoint::Ptr> mBundleID_Point;
01040   map<KeyFrame::Ptr, int> mView_BundleID;
01041   map<int, KeyFrame::Ptr> mBundleID_View;
01042 
01043   // Add the keyframes' poses to the bundle adjuster. Two parts: first nonfixed, then fixed.
01044   for(set<KeyFrame::Ptr>::iterator it = sAdjustSet.begin(); it!= sAdjustSet.end(); it++)
01045   {
01046     int nBundleID = b.AddCamera((*it)->se3CfromW, (*it)->bFixed);
01047     mView_BundleID[*it] = nBundleID;
01048     mBundleID_View[nBundleID] = *it;
01049   }
01050   for(set<KeyFrame::Ptr>::iterator it = sFixedSet.begin(); it!= sFixedSet.end(); it++)
01051   {
01052     int nBundleID = b.AddCamera((*it)->se3CfromW, true);
01053     mView_BundleID[*it] = nBundleID;
01054     mBundleID_View[nBundleID] = *it;
01055   }
01056 
01057   // Add the points' 3D position
01058   for(set<MapPoint::Ptr>::iterator it = sMapPoints.begin(); it!=sMapPoints.end(); it++)
01059   {
01060     int nBundleID = b.AddPoint((*it)->v3WorldPos);
01061     mPoint_BundleID[*it] = nBundleID;
01062     mBundleID_Point[nBundleID] = *it;
01063 
01064     //Weiss{ feature statistics
01065     //          if(kfid==(*it)->pPatchSourceKF->ID)
01066     //          {
01067     //                  addcount[(*it)->nSourceLevel]++;
01068     //                  addcount[4]++;
01069     //          }
01070     //}
01071   }
01072 
01073   // Add the relevant point-in-keyframe measurements
01074   for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
01075   {
01076     if(mView_BundleID.count(mMap.vpKeyFrames[i]) == 0)
01077       continue;
01078 
01079     int nKF_BundleID = mView_BundleID[mMap.vpKeyFrames[i]];
01080     for(meas_it it= mMap.vpKeyFrames[i]->mMeasurements.begin();
01081         it!= mMap.vpKeyFrames[i]->mMeasurements.end();
01082         it++)
01083     {
01084 
01085       if(mPoint_BundleID.count(it->first) == 0)
01086         continue;
01087       int nPoint_BundleID = mPoint_BundleID[it->first];
01088       b.AddMeas(nKF_BundleID, nPoint_BundleID, it->second.v2RootPos, LevelScale(it->second.nLevel) * LevelScale(it->second.nLevel));
01089     }
01090   }
01091 
01092   // Run the bundle adjuster. This returns the number of successful iterations
01093   int nAccepted = b.Compute(&mbBundleAbortRequested);
01094 
01095   if(nAccepted < 0)
01096   {
01097     // Crap: - LM Ran into a serious problem!
01098     // This is probably because the initial stereo was messed up.
01099     // Get rid of this map and start again!
01100     mMessageForUser << "!! MapMaker: Cholesky failure in bundle adjust. " << endl
01101         << "   The map is probably corrupt: Ditching the map. " << endl;
01102     mbResetRequested = true;
01103     return;
01104   }
01105 
01106   // Bundle adjustment did some updates, apply these to the map
01107   if(nAccepted > 0)
01108   {
01109     //slynen{ pcl interface
01110     std::set<MapPoint::Ptr> mapPointsToUpdate; //copy the data
01111     for(map<MapPoint::Ptr,int>::iterator itr = mPoint_BundleID.begin();
01112         itr!=mPoint_BundleID.end();
01113         itr++){
01114       itr->first->v3WorldPos = b.GetPoint(itr->second);
01115       mapPointsToUpdate.insert(itr->first);
01116     }
01117     octomap_interface.updatePoints(mapPointsToUpdate);
01118     //}
01119 
01120     for(map<KeyFrame::Ptr,int>::iterator itr = mView_BundleID.begin();
01121         itr!=mView_BundleID.end();
01122         itr++)
01123       itr->first->se3CfromW = b.GetCamera(itr->second);
01124     if(bRecent)
01125       mbBundleConverged_Recent = false;
01126     mbBundleConverged_Full = false;
01127   };
01128 
01129   if(b.Converged())
01130   {
01131     mbBundleConverged_Recent = true;
01132     if(!bRecent)
01133       mbBundleConverged_Full = true;
01134   }
01135 
01136   mbBundleRunning = false;
01137   mbBundleAbortRequested = false;
01138 
01139   // Handle outlier measurements:
01140   vector<pair<int,int> > vOutliers_PC_pair = b.GetOutlierMeasurements();
01141 
01142   for(unsigned int i=0; i<vOutliers_PC_pair.size(); i++)
01143   {
01144     MapPoint::Ptr pp = mBundleID_Point[vOutliers_PC_pair[i].first];
01145     KeyFrame::Ptr pk = mBundleID_View[vOutliers_PC_pair[i].second];
01146     Measurement &m = pk->mMeasurements[pp];
01147 
01148     if(pp->pMMData->GoodMeasCount() <= 2 || m.Source == Measurement::SRC_ROOT)   // Is the original source kf considered an outlier? That's bad.
01149       pp->bBad = true;
01150     else
01151     {
01152       // Do we retry it? Depends where it came from!!
01153       if(m.Source == Measurement::SRC_TRACKER || m.Source == Measurement::SRC_EPIPOLAR)
01154         mvFailureQueue.push_back(pair<KeyFrame::Ptr,MapPoint::Ptr>(pk,pp));
01155       else
01156         pp->pMMData->sNeverRetryKFs.insert(pk);
01157       pk->mMeasurements.erase(pp);
01158       pp->pMMData->sMeasurementKFs.erase(pk);
01159       //Weiss{ feature statistics
01160       //                        if(kfid==pp->pPatchSourceKF->ID)
01161       //                        {
01162       //                                outcount[pp->nSourceLevel]++;
01163       //                                outcount[4]++;
01164       //                        }
01165       //}
01166     }
01167   }
01168 }
01169 
01170 // Mapmaker's try-to-find-a-point-in-a-keyframe code. This is used to update
01171 // data association if a bad measurement was detected, or if a point
01172 // was never searched for in a keyframe in the first place. This operates
01173 // much like the tracker! So most of the code looks just like in 
01174 // TrackerData.h.
01175 bool MapMaker::ReFind_Common(KeyFrame::Ptr k, MapPoint::Ptr p)
01176 {
01177   // abort if either a measurement is already in the map, or we've
01178   // decided that this point-kf combo is beyond redemption
01179   if(p->pMMData->sMeasurementKFs.count(k)
01180       || p->pMMData->sNeverRetryKFs.count(k))
01181     return false;
01182 
01183   static PatchFinder Finder;
01184   Vector<3> v3Cam = k->se3CfromW * p->v3WorldPos;
01185   if(v3Cam[2] < 0.001)
01186   {
01187     p->pMMData->sNeverRetryKFs.insert(k);
01188     return false;
01189   }
01190   Vector<2> v2ImPlane = project(v3Cam);
01191   if(v2ImPlane* v2ImPlane > mCamera.LargestRadiusInImage() * mCamera.LargestRadiusInImage())
01192   {
01193     p->pMMData->sNeverRetryKFs.insert(k);
01194     return false;
01195   }
01196 
01197   Vector<2> v2Image = mCamera.Project(v2ImPlane);
01198   if(mCamera.Invalid())
01199   {
01200     p->pMMData->sNeverRetryKFs.insert(k);
01201     return false;
01202   }
01203 
01204   ImageRef irImageSize = k->aLevels[0].im.size();
01205   if(v2Image[0] < 0 || v2Image[1] < 0 || v2Image[0] > irImageSize[0] || v2Image[1] > irImageSize[1])
01206   {
01207     p->pMMData->sNeverRetryKFs.insert(k);
01208     return false;
01209   }
01210 
01211   Matrix<2> m2CamDerivs = mCamera.GetProjectionDerivs();
01212   Finder.MakeTemplateCoarse(p, k->se3CfromW, m2CamDerivs);
01213 
01214   if(Finder.TemplateBad())
01215   {
01216     p->pMMData->sNeverRetryKFs.insert(k);
01217     return false;
01218   }
01219   bool bFound = Finder.FindPatchCoarse(ir(v2Image), k, 4);  // Very tight search radius!
01220   if(!bFound)
01221   {
01222     p->pMMData->sNeverRetryKFs.insert(k);
01223     return false;
01224   }
01225 
01226   // If we found something, generate a measurement struct and put it in the map
01227   Measurement m;
01228   m.nLevel = Finder.GetLevel();
01229   m.Source = Measurement::SRC_REFIND;
01230 
01231   if(Finder.GetLevel() > 0)
01232   {
01233     Finder.MakeSubPixTemplate();
01234     Finder.IterateSubPixToConvergence(*k,8);
01235     m.v2RootPos = Finder.GetSubPixPos();
01236     m.bSubPix = true;
01237   }
01238   else
01239   {
01240     m.v2RootPos = Finder.GetCoarsePosAsVector();
01241     m.bSubPix = false;
01242   };
01243 
01244   if(k->mMeasurements.count(p))
01245   {
01246     assert(0); // This should never happen, we checked for this at the start.
01247   }
01248   k->mMeasurements[p] = m;
01249   p->pMMData->sMeasurementKFs.insert(k);
01250   //slynen{ reprojection
01251 #ifdef KF_REPROJ
01252   k->AddKeyMapPoint(p);
01253 #endif
01254   //}
01255   //slynen octomap_interface{
01256   octomap_interface.updatePoint(p);
01257   //}
01258   return true;
01259 }
01260 
01261 // A general data-association update for a single keyframe
01262 // Do this on a new key-frame when it's passed in by the tracker
01263 int MapMaker::ReFindInSingleKeyFrame(KeyFrame::Ptr k)
01264 {
01265   vector<MapPoint::Ptr> vToFind;
01266   for(unsigned int i=0; i<mMap.vpPoints.size(); i++)
01267     vToFind.push_back(mMap.vpPoints[i]);
01268 
01269   int nFoundNow = 0;
01270   for(unsigned int i=0; i<vToFind.size(); i++)
01271     if(ReFind_Common(k,vToFind[i]))
01272       nFoundNow++;
01273 
01274   return nFoundNow;
01275 };
01276 
01277 // When new map points are generated, they're only created from a stereo pair
01278 // this tries to make additional measurements in other KFs which they might
01279 // be in.
01280 void MapMaker::ReFindNewlyMade()
01281 {
01282   if(mqNewQueue.empty())
01283     return;
01284   int nFound = 0;
01285   int nBad = 0;
01286   while(!mqNewQueue.empty() && mvpKeyFrameQueue.size() == 0)
01287   {
01288     MapPoint::Ptr pNew = mqNewQueue.front();
01289     mqNewQueue.pop();
01290     if(pNew->bBad)
01291     {
01292       nBad++;
01293       continue;
01294     }
01295     for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
01296       if(ReFind_Common(mMap.vpKeyFrames[i], pNew))
01297         nFound++;
01298   }
01299 };
01300 
01301 // Dud measurements get a second chance.
01302 void MapMaker::ReFindFromFailureQueue()
01303 {
01304   if(mvFailureQueue.size() == 0)
01305     return;
01306   sort(mvFailureQueue.begin(), mvFailureQueue.end());
01307   vector<pair<KeyFrame::Ptr, MapPoint::Ptr> >::iterator it;
01308   int nFound=0;
01309   for(it = mvFailureQueue.begin(); it!=mvFailureQueue.end(); it++)
01310     if(ReFind_Common(it->first, it->second))
01311       nFound++;
01312 
01313   mvFailureQueue.erase(mvFailureQueue.begin(), it);
01314 };
01315 
01316 // Is the tracker's camera pose in cloud-cuckoo land?
01317 bool MapMaker::IsDistanceToNearestKeyFrameExcessive(KeyFrame::Ptr kCurrent)
01318 {
01319   return DistToNearestKeyFrame(kCurrent) > mdWiggleScale * 10.0;
01320 }
01321 
01322 // Find a dominant plane in the map, find an SE3<> to put it as the z=0 plane
01323 SE3<> MapMaker::CalcPlaneAligner()
01324 {
01325   unsigned int nPoints = mMap.vpPoints.size();
01326   if(nPoints < 10)
01327   {
01328     mMessageForUser << "  MapMaker: CalcPlane: too few points to calc plane." << endl;
01329     return SE3<>();
01330   };
01331   //Weiss{
01332   
01333   const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
01334   int nRansacs = pPars.PlaneAlignerRansacs;
01335   //int nRansacs = GV2.GetInt("MapMaker.PlaneAlignerRansacs", 100, HIDDEN|SILENT);
01336   //}
01337   Vector<3> v3BestMean=makeVector(0,0,0);
01338   Vector<3> v3BestNormal=makeVector(0,0,0);
01339   double dBestDistSquared = 9999999999999999.9;
01340 
01341   for(int i=0; i<nRansacs; i++)
01342   {
01343     int nA = rand()%nPoints;
01344     int nB = nA;
01345     int nC = nA;
01346     while(nB == nA)
01347       nB = rand()%nPoints;
01348     while(nC == nA || nC==nB)
01349       nC = rand()%nPoints;
01350 
01351     Vector<3> v3Mean = 0.33333333 * (mMap.vpPoints[nA]->v3WorldPos +
01352         mMap.vpPoints[nB]->v3WorldPos +
01353         mMap.vpPoints[nC]->v3WorldPos);
01354 
01355     Vector<3> v3CA = mMap.vpPoints[nC]->v3WorldPos  - mMap.vpPoints[nA]->v3WorldPos;
01356     Vector<3> v3BA = mMap.vpPoints[nB]->v3WorldPos  - mMap.vpPoints[nA]->v3WorldPos;
01357     Vector<3> v3Normal = v3CA ^ v3BA;
01358     if(v3Normal * v3Normal  == 0)
01359       continue;
01360     normalize(v3Normal);
01361 
01362     double dSumError = 0.0;
01363     for(unsigned int i=0; i<nPoints; i++)
01364     {
01365       Vector<3> v3Diff = mMap.vpPoints[i]->v3WorldPos - v3Mean;
01366       double dDistSq = v3Diff * v3Diff;
01367       if(dDistSq == 0.0)
01368         continue;
01369       double dNormDist = fabs(v3Diff * v3Normal);
01370 
01371       if(dNormDist > 0.05)
01372         dNormDist = 0.05;
01373       dSumError += dNormDist;
01374     }
01375     if(dSumError < dBestDistSquared)
01376     {
01377       dBestDistSquared = dSumError;
01378       v3BestMean = v3Mean;
01379       v3BestNormal = v3Normal;
01380     }
01381   }
01382 
01383   // Done the ransacs, now collect the supposed inlier set
01384   vector<Vector<3> > vv3Inliers;
01385   for(unsigned int i=0; i<nPoints; i++)
01386   {
01387     Vector<3> v3Diff = mMap.vpPoints[i]->v3WorldPos - v3BestMean;
01388     double dDistSq = v3Diff * v3Diff;
01389     if(dDistSq == 0.0)
01390       continue;
01391     double dNormDist = fabs(v3Diff * v3BestNormal);
01392     if(dNormDist < 0.05)
01393       vv3Inliers.push_back(mMap.vpPoints[i]->v3WorldPos);
01394   }
01395 
01396   // With these inliers, calculate mean and cov
01397   Vector<3> v3MeanOfInliers = Zeros;
01398   for(unsigned int i=0; i<vv3Inliers.size(); i++)
01399     v3MeanOfInliers+=vv3Inliers[i];
01400   v3MeanOfInliers *= (1.0 / vv3Inliers.size());
01401 
01402   Matrix<3> m3Cov = Zeros;
01403   for(unsigned int i=0; i<vv3Inliers.size(); i++)
01404   {
01405     Vector<3> v3Diff = vv3Inliers[i] - v3MeanOfInliers;
01406     m3Cov += v3Diff.as_col() * v3Diff.as_row();
01407   };
01408 
01409   // Find the principal component with the minimal variance: this is the plane normal
01410   SymEigen<3> sym(m3Cov);
01411   Vector<3> v3Normal = sym.get_evectors()[0];
01412 
01413   // Use the version of the normal which points towards the cam center
01414   if(v3Normal[2] > 0)
01415     v3Normal *= -1.0;
01416 
01417   Matrix<3> m3Rot = Identity;
01418   m3Rot[2] = v3Normal;
01419   m3Rot[0] = m3Rot[0] - (v3Normal * (m3Rot[0] * v3Normal));
01420   normalize(m3Rot[0]);
01421   m3Rot[1] = m3Rot[2] ^ m3Rot[0];
01422 
01423   SE3<> se3Aligner;
01424   se3Aligner.get_rotation() = m3Rot;
01425   Vector<3> v3RMean = se3Aligner * v3MeanOfInliers;
01426   se3Aligner.get_translation() = -v3RMean;
01427 
01428   return se3Aligner;
01429 }
01430 
01431 // Calculates the depth(z-) distribution of map points visible in a keyframe
01432 // This function is only used for the first two keyframes - all others
01433 // get this filled in by the tracker
01434 bool MapMaker::RefreshSceneDepth(KeyFrame::Ptr pKF)
01435 {
01436   double dSumDepth = 0.0;
01437   double dSumDepthSquared = 0.0;
01438   int nMeas = 0;
01439   for(meas_it it = pKF->mMeasurements.begin(); it!=pKF->mMeasurements.end(); it++)
01440   {
01441     MapPoint &point = *it->first;
01442     Vector<3> v3PosK = pKF->se3CfromW * point.v3WorldPos;
01443     dSumDepth += v3PosK[2];
01444     dSumDepthSquared += v3PosK[2] * v3PosK[2];
01445     nMeas++;
01446   }
01447   //Weiss{
01448   //    assert(nMeas > 2); // If not then something is seriously wrong with this KF!!
01449   //agree, but do not kill whole PTAM just because of that...
01450   if (nMeas<2)
01451     return false;
01452   pKF->dSceneDepthMean = dSumDepth / nMeas;
01453   pKF->dSceneDepthSigma = sqrt((dSumDepthSquared / nMeas) - (pKF->dSceneDepthMean) * (pKF->dSceneDepthMean));
01454   return true;
01455 }
01456 
01457 void MapMaker::GUICommandCallBack(void* ptr, string sCommand, string sParams)
01458 {
01459   Command c;
01460   c.sCommand = sCommand;
01461   c.sParams = sParams;
01462   ((MapMaker*) ptr)->mvQueuedCommands.push_back(c);
01463 }
01464 
01465 void MapMaker::GUICommandHandler(string sCommand, string sParams)  // Called by the callback func..
01466 {
01467   if(sCommand=="SaveMap")
01468   {
01469     mMessageForUser << "  MapMaker: Saving the map.... " << endl;
01470     ofstream ofs("map.dump");
01471     for(unsigned int i=0; i<mMap.vpPoints.size(); i++)
01472     {
01473       ofs << mMap.vpPoints[i]->v3WorldPos << "  ";
01474       ofs << mMap.vpPoints[i]->nSourceLevel << endl;
01475     }
01476     ofs.close();
01477 
01478     for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
01479     {
01480       ostringstream ost1;
01481       ost1 << "keyframes/" << i << ".jpg";
01482       //          img_save(mMap.vpKeyFrames[i]->aLevels[0].im, ost1.str());
01483 
01484       ostringstream ost2;
01485       ost2 << "keyframes/" << i << ".info";
01486       ofstream ofs2;
01487       ofs2.open(ost2.str().c_str());
01488       ofs2 << mMap.vpKeyFrames[i]->se3CfromW << endl;
01489       ofs2.close();
01490     }
01491     mMessageForUser << "  ... done saving map." << endl;
01492     return;
01493   }
01494 
01495   mMessageForUser << "! MapMaker::GUICommandHandler: unhandled command "<< sCommand << endl;
01496   exit(1);
01497 }; 
01498 
01499 
01500 
01501 
01502 
01503 
01504 
01505 
01506 
01507 
01508 
01509 


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