core/vnl/algo/vnl_sparse_lm.h
Go to the documentation of this file.
00001 // This is core/vnl/algo/vnl_sparse_lm.h
00002 #ifndef vnl_sparse_lm_h_
00003 #define vnl_sparse_lm_h_
00004 #ifdef VCL_NEEDS_PRAGMA_INTERFACE
00005 #pragma interface
00006 #endif
00007 //:
00008 // \file
00009 // \brief Sparse Levenberg Marquardt nonlinear least squares
00010 // \author Matt Leotta (Brown)
00011 // \date   April 14, 2005
00012 //
00013 // \verbatim
00014 //  Modifications
00015 //   Mar 15, 2010  MJL - Modified to handle 'c' parameters (globals)
00016 // \endverbatim
00017 //
00018 
00019 #include <vcl_iosfwd.h>
00020 #include <vcl_vector.h>
00021 #include <vnl/vnl_vector.h>
00022 #include <vnl/vnl_matrix.h>
00023 #include <vnl/vnl_nonlinear_minimizer.h>
00024 
00025 class vnl_sparse_lst_sqr_function;
00026 
00027 //: Sparse Levenberg Marquardt nonlinear least squares
00028 //  Unlike vnl_levenberg_marquardt this does not use the MINPACK routines.
00029 //  This class implements sparse Levenberg Marquardt as described in
00030 //  the Hartley and Zisserman "Multiple View Geometry" book and further
00031 //  described in a technical report on sparse bundle adjustment available
00032 //  at http://www.ics.forth.gr/~lourakis/sba
00033 class vnl_sparse_lm : public vnl_nonlinear_minimizer
00034 {
00035  public:
00036 
00037   //: Initialize with the function object that is to be minimized.
00038   vnl_sparse_lm(vnl_sparse_lst_sqr_function& f);
00039 
00040   //: Destructor
00041   ~vnl_sparse_lm();
00042 
00043   //: Minimize the function supplied in the constructor until convergence or failure.
00044   //  On return, a, b, and c are such that f(a,b,c) is the lowest value achieved.
00045   //  Returns true for convergence, false for failure.
00046   //  If use_gradient is set to false, a finite difference approximation will be used,
00047   //  even if the Jacobian functions have been provided.
00048   //  If use_weights is set to false, weights will not be computed even if a
00049   //  weighting function has been provided.
00050   bool minimize(vnl_vector<double>& a,
00051                 vnl_vector<double>& b,
00052                 vnl_vector<double>& c,
00053                 bool use_gradient = true,
00054                 bool use_weights = true);
00055 
00056   // Coping with failure-------------------------------------------------------
00057 
00058   //: Provide an ASCII diagnosis of the last minimization on vcl_ostream.
00059   void diagnose_outcome(/*vcl_cerr*/) const;
00060   void diagnose_outcome(vcl_ostream&) const;
00061 
00062   //: Return J'*J computed at last minimum.
00063   //  it is an approximation of inverse of covariance
00064   vnl_matrix<double> const& get_JtJ();
00065 
00066   //: Access the final weights after optimization
00067   const vnl_vector<double>& get_weights() const { return weights_; }
00068 
00069 protected:
00070 
00071   //: used to compute the initial damping
00072   double tau_;
00073   //: the function to minimize
00074   vnl_sparse_lst_sqr_function* f_;
00075 
00076   vnl_matrix<double> inv_covar_;
00077   bool set_covariance_; // Set if covariance_ holds J'*J
00078 
00079   void init(vnl_sparse_lst_sqr_function* f);
00080 
00081 private:
00082 
00083   //: allocate matrix memory by setting all the matrix sizes
00084   void allocate_matrices();
00085 
00086   //: check vector sizes and verify that they match the problem size
00087   bool check_vector_sizes(vnl_vector<double> const& a,
00088                           vnl_vector<double> const& b,
00089                           vnl_vector<double> const& c);
00090 
00091   //: compute the blocks making up the the normal equations: Jt J d = Jt e
00092   void compute_normal_equations();
00093 
00094   //: extract the vector on the diagonal of Jt J
00095   vnl_vector<double> extract_diagonal() const;
00096 
00097   //: set the vector on the diagonal of Jt J
00098   void set_diagonal(const vnl_vector<double>& diag);
00099 
00100   //: compute all inv(Vi) and Yij
00101   void compute_invV_Y();
00102 
00103   //: compute Z and Sa
00104   void compute_Z_Sa(vnl_matrix<double>& Sa);
00105 
00106   //: compute Ma
00107   void compute_Ma(const vnl_matrix<double>& H);
00108 
00109   //: compute Mb
00110   void compute_Mb();
00111 
00112   //: solve for dc
00113   void solve_dc(vnl_vector<double>& dc);
00114 
00115   //: compute sea using ea, Z, dc, Y, and eb
00116   void compute_sea(vnl_vector<double> const& dc,
00117                    vnl_vector<double>& sea);
00118 
00119   //: compute Sa and sea
00120   // only used when size_c_ == 0
00121   void compute_Sa_sea(vnl_matrix<double>& Sa, vnl_vector<double>& sea);
00122 
00123   //: back solve to find db using da and dc
00124   void backsolve_db(vnl_vector<double> const& da,
00125                     vnl_vector<double> const& dc,
00126                     vnl_vector<double>& db);
00127 
00128   const int num_a_;
00129   const int num_b_;
00130   const int num_e_;
00131   const int num_nz_;
00132 
00133   const int size_a_;
00134   const int size_b_;
00135   const int size_c_;
00136   const int size_e_;
00137 
00138   //: Storage for each of the Jacobians A_ij, B_ij, and C_ij
00139   vcl_vector<vnl_matrix<double> > A_;
00140   vcl_vector<vnl_matrix<double> > B_;
00141   vcl_vector<vnl_matrix<double> > C_;
00142 
00143   //: Storage for normal equation blocks
00144   // diagonals of JtJ
00145   vcl_vector<vnl_matrix<double> > U_;
00146   vcl_vector<vnl_matrix<double> > V_;
00147   vnl_matrix<double>              T_;
00148   // off-diagonals of JtJ
00149   vcl_vector<vnl_matrix<double> > W_;
00150   vcl_vector<vnl_matrix<double> > R_;
00151   vcl_vector<vnl_matrix<double> > Q_;
00152   // vectors Jte
00153   vnl_vector<double> ea_;
00154   vnl_vector<double> eb_;
00155   vnl_vector<double> ec_;
00156 
00157   // Storage for residual vector
00158   vnl_vector<double> e_;
00159 
00160   // Storage for weight vector
00161   vnl_vector<double> weights_;
00162 
00163   // Storage for intermediate results
00164   vcl_vector<vnl_matrix<double> > inv_V_;
00165   vcl_vector<vnl_matrix<double> > Y_;
00166   vcl_vector<vnl_matrix<double> > Z_;
00167   vcl_vector<vnl_matrix<double> > Ma_;
00168   vcl_vector<vnl_matrix<double> > Mb_;
00169 
00170 };
00171 
00172 
00173 #endif // vnl_sparse_lm_h_