Bundle.h
Go to the documentation of this file.
00001 // -*- c++ -*-
00002 // Copyright 2008 Isis Innovation Limited
00003 #ifndef __BUNDLE_H
00004 #define __BUNDLE_H
00005 // Bundle.h
00006 // 
00007 // This file declares the Bundle class along with a few helper classes.
00008 // Bundle is the bundle adjustment core of the mapping system; instances
00009 // of Bundle are generated by MapMaker to adjust the positions of 
00010 // keyframes (called Cameras in this file) and map points.
00011 //
00012 // It's a pretty straight-forward Levenberg-Marquardt bundle adjustment 
00013 // implementation closely following Hartley and Zisserman's MVG book, with
00014 // the addition of a robust M-Estimator.
00015 //
00016 // Unfortunately, having undergone a few tweaks, the code is now
00017 // not the easiest to read!
00018 //
00019 // Basic operation: MapMaker creates a new Bundle object;
00020 // then adds map points and keyframes to adjust;
00021 // then adds measurements of map points in keyframes;
00022 // then calls Compute() to do bundle adjustment;
00023 // then reads results back to update the map.
00024 
00025 #include "ATANCamera.h"
00026 #include <TooN/TooN.h>
00027 using namespace TooN;
00028 #include <TooN/se3.h>
00029 #include <vector>
00030 #include <map>
00031 #include <set>
00032 #include <list>
00033 #include "ptam/Params.h"
00034 
00035 // An index into the big measurement map which stores all the measurements.
00036 
00037 // Camera struct holds the pose of a keyframe
00038 // and some computation intermediates
00039 struct Camera
00040 {
00041   bool bFixed;
00042   SE3<> se3CfW;
00043   SE3<> se3CfWNew;
00044   Matrix<6> m6U;          // Accumulator
00045   Vector<6> v6EpsilonA;   // Accumulator
00046   int nStartRow;
00047 };
00048 
00049 // Camera-camera pair index
00050 struct OffDiagScriptEntry
00051 {
00052   int j;
00053   int k;
00054 };
00055 
00056 // A map point, plus computation intermediates.
00057 struct Point
00058 {
00059   inline Point()
00060   { nMeasurements = 0; nOutliers = 0;}
00061   Vector<3> v3Pos;
00062   Vector<3> v3PosNew;
00063   Matrix<3> m3V;          // Accumulator
00064   Vector<3> v3EpsilonB;   // Accumulator 
00065   Matrix<3> m3VStarInv;
00066 
00067   int nMeasurements;
00068   int nOutliers;
00069   std::set<int> sCameras; // Which cameras observe this point?
00070   std::vector<OffDiagScriptEntry> vOffDiagonalScript; // A record of all camera-camera pairs observing this point
00071 };
00072 
00073 // A measurement of a point by a camera, plus
00074 // computation intermediates.
00075 struct Meas
00076 {
00077   inline Meas()
00078   {bBad = false;}
00079 
00080   // Which camera/point did this measurement come from?
00081   int p; // The point  - called i in MVG
00082   int c; // The camera - called j in MVG
00083 
00084   inline bool operator<(const Meas &rhs) const
00085   {  return(c<rhs.c ||(c==rhs.c && p < rhs.p)); }
00086 
00087   bool bBad;
00088 
00089   Vector<2> v2Found;
00090   Vector<2> v2Epsilon;
00091   Matrix<2,6> m26A;
00092   Matrix<2,3> m23B;
00093   Matrix<6,3> m63W; 
00094   Matrix<6,3> m63Y;
00095   double dSqrtInvNoise;
00096 
00097   // Temporary projection quantities
00098   Vector<3> v3Cam;
00099   double dErrorSquared;
00100   Matrix<2> m2CamDerivs;
00101 };
00102 
00103 // Core bundle adjustment class
00104 class Bundle
00105 {
00106 public:
00107 
00108   Bundle(const ATANCamera &TCam);   // We need the camera model because we do full distorting projection in the bundle adjuster. Could probably get away with a linear approximation.
00109   int AddCamera(SE3<> se3CamFromWorld, bool bFixed); // Add a viewpoint. bFixed signifies that this one is not to be adjusted.
00110   int AddPoint(Vector<3> v3Pos);       // Add a map point.
00111   void AddMeas(int nCam, int nPoint, Vector<2> v2Pos, double dSigmaSquared); // Add a measurement
00112   int Compute(bool *pbAbortSignal);    // Perform bundle adjustment. Aborts if *pbAbortSignal gets set to true. Returns number of accepted update iterations, or negative on error.
00113   inline bool Converged() { return mbConverged;}  // Has bundle adjustment converged?
00114   Vector<3> GetPoint(int n);       // Point coords after adjustment
00115   SE3<> GetCamera(int n);            // Camera pose after adjustment
00116   std::vector<std::pair<int,int> > GetOutlierMeasurements();  // Measurements flagged as outliers
00117   std::set<int> GetOutliers();                                // Points flagged as outliers
00118 
00119 protected:
00120 
00121   inline void ProjectAndFindSquaredError(Meas &meas); // Project a single point in a single view, compare to measurement
00122   template<class MEstimator> bool Do_LM_Step(bool *pbAbortSignal);
00123   template<class MEstimator> double FindNewError();
00124   void GenerateMeasLUTs();
00125   void GenerateOffDiagScripts();
00126   void ClearAccumulators(); // Zero temporary quantities stored in cameras and points
00127   void ModifyLambda_GoodStep();
00128   void ModifyLambda_BadStep();
00129 
00130   std::vector<Point> mvPoints;
00131   std::vector<Camera> mvCameras;
00132   std::list<Meas> mMeasList;
00133   std::vector<std::pair<int,int> > mvOutlierMeasurementIdx;  // p-c pair
00134   std::vector<std::vector<Meas*> > mvMeasLUTs;  //Each camera gets a per-point table of pointers to valid measurements
00135 
00136   ATANCamera mCamera;
00137   int mnCamsToUpdate;
00138   int mnStartRow;
00139   double mdSigmaSquared;
00140   double mdLambda;
00141   double mdLambdaFactor;
00142   bool mbConverged;
00143   bool mbHitMaxIterations;
00144   int mnCounter;
00145   int mnAccepted;
00146 
00147   //Weiss{
00148   //GVars3::gvar3<int> mgvnMaxIterations;
00149   //GVars3::gvar3<double> mgvdUpdateConvergenceLimit;
00150   //GVars3::gvar3<int> mgvnBundleCout;
00151   //}
00152   bool *mpbAbortSignal;
00153 };
00154 
00155 
00156 
00157 
00158 
00159 #endif


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