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 
00034 // An index into the big measurement map which stores all the measurements.
00035 
00036 // Camera struct holds the pose of a keyframe
00037 // and some computation intermediates
00038 struct Camera
00039 {
00040   bool bFixed;
00041   SE3<> se3CfW;
00042   SE3<> se3CfWNew;
00043   Matrix<6> m6U;          // Accumulator
00044   Vector<6> v6EpsilonA;   // Accumulator
00045   int nStartRow;
00046 };
00047 
00048 // Camera-camera pair index
00049 struct OffDiagScriptEntry
00050 {
00051   int j;
00052   int k;
00053 };
00054 
00055 // A map point, plus computation intermediates.
00056 struct Point
00057 {
00058   inline Point()
00059   { nMeasurements = 0; nOutliers = 0;}
00060   Vector<3> v3Pos;
00061   Vector<3> v3PosNew;
00062   Matrix<3> m3V;          // Accumulator
00063   Vector<3> v3EpsilonB;   // Accumulator 
00064   Matrix<3> m3VStarInv;
00065   
00066   int nMeasurements;
00067   int nOutliers;
00068   std::set<int> sCameras; // Which cameras observe this point?
00069   std::vector<OffDiagScriptEntry> vOffDiagonalScript; // A record of all camera-camera pairs observing this point
00070 };
00071 
00072 // A measurement of a point by a camera, plus
00073 // computation intermediates.
00074 struct Meas
00075 {
00076   inline Meas()
00077   {bBad = false;}
00078   
00079   // Which camera/point did this measurement come from?
00080   int p; // The point  - called i in MVG
00081   int c; // The camera - called j in MVG
00082 
00083   inline bool operator<(const Meas &rhs) const
00084   {  return(c<rhs.c ||(c==rhs.c && p < rhs.p)); }
00085   
00086   bool bBad;
00087   
00088   Vector<2> v2Found;
00089   Vector<2> v2Epsilon;
00090   Matrix<2,6> m26A;
00091   Matrix<2,3> m23B;
00092   Matrix<6,3> m63W; 
00093   Matrix<6,3> m63Y;
00094   double dSqrtInvNoise;
00095   
00096   // Temporary projection quantities
00097   Vector<3> v3Cam;
00098   double dErrorSquared;
00099   Matrix<2> m2CamDerivs;
00100 };
00101 
00102 // Core bundle adjustment class
00103 class Bundle
00104 {
00105 public:
00106 
00107   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.
00108   int AddCamera(SE3<> se3CamFromWorld, bool bFixed); // Add a viewpoint. bFixed signifies that this one is not to be adjusted.
00109   int AddPoint(Vector<3> v3Pos);       // Add a map point.
00110   void AddMeas(int nCam, int nPoint, Vector<2> v2Pos, double dSigmaSquared); // Add a measurement
00111   int Compute(bool *pbAbortSignal);    // Perform bundle adjustment. Aborts if *pbAbortSignal gets set to true. Returns number of accepted update iterations, or negative on error.
00112   inline bool Converged() { return mbConverged;}  // Has bundle adjustment converged?
00113   Vector<3> GetPoint(int n);       // Point coords after adjustment
00114   SE3<> GetCamera(int n);            // Camera pose after adjustment
00115   std::vector<std::pair<int,int> > GetOutlierMeasurements();  // Measurements flagged as outliers
00116   std::set<int> GetOutliers();                                // Points flagged as outliers
00117   
00118 protected:
00119 
00120   inline void ProjectAndFindSquaredError(Meas &meas); // Project a single point in a single view, compare to measurement
00121   template<class MEstimator> bool Do_LM_Step(bool *pbAbortSignal);
00122   template<class MEstimator> double FindNewError();
00123   void GenerateMeasLUTs();
00124   void GenerateOffDiagScripts();
00125   void ClearAccumulators(); // Zero temporary quantities stored in cameras and points
00126   void ModifyLambda_GoodStep();
00127   void ModifyLambda_BadStep();
00128   
00129   std::vector<Point> mvPoints;
00130   std::vector<Camera> mvCameras;
00131   std::list<Meas> mMeasList;
00132   std::vector<std::pair<int,int> > mvOutlierMeasurementIdx;  // p-c pair
00133   std::vector<std::vector<Meas*> > mvMeasLUTs;  //Each camera gets a per-point table of pointers to valid measurements
00134   
00135   ATANCamera mCamera;
00136   int mnCamsToUpdate;
00137   int mnStartRow;
00138   double mdSigmaSquared;
00139   double mdLambda;
00140   double mdLambdaFactor;
00141   bool mbConverged;
00142   bool mbHitMaxIterations;
00143   int mnCounter;
00144   int mnAccepted;
00145   
00146   GVars3::gvar3<int> mgvnMaxIterations;
00147   GVars3::gvar3<double> mgvdUpdateConvergenceLimit;
00148   GVars3::gvar3<int> mgvnBundleCout;
00149   
00150   bool *mpbAbortSignal;
00151 };
00152 
00153 
00154 
00155 
00156 
00157 #endif


tum_ardrone
Author(s):
autogenerated on Sat Jun 8 2019 20:27:22