iSAM
Jacobian.h
Go to the documentation of this file.
00001 
00028 #pragma once
00029 
00030 #include <Eigen/Dense>
00031 
00032 namespace isam {
00033 
00034 typedef double (*cost_func_t)(double);
00035 
00036 
00037 // Elementary Jacobian for one specific variable/node, potentially containing multiple measurement rows.
00038 class Term {
00039   friend std::ostream& operator<<(std::ostream& output, const Term& t) {
00040     t.write(output);
00041     return output;
00042   }
00043 
00044   Node* _node;
00045 
00046   Eigen::MatrixXd _term;
00047 
00048 public:
00049 
00050   const Node* node() const {return _node;}
00051 
00052   const Eigen::MatrixXd& term() const {return _term;}
00053 
00054   Term(Node* node, const Eigen::MatrixXd& term) : _node(node), _term(term) {}
00055 
00056   Term(Node* node, const double * term, int r, int c) : _node(node), _term(r,c) {
00057     int n = 0;
00058     for (int row=0; row<r; row++) {
00059       for (int col=0; col<c; col++) {
00060         _term(row,col) = term[n++];
00061       }
00062     }
00063   }
00064 
00065   void write(std::ostream &out) const {
00066     out << _term << std::endl;
00067   }
00068 };
00069 
00070 typedef std::list<Term> Terms;
00071 
00072 // Jacobian consisting of multiple blocks.
00073 class Jacobian {
00074   friend std::ostream& operator<<(std::ostream& output, const Jacobian& t) {
00075     t.write(output);
00076     return output;
00077   }
00078 
00079   int _dimtotal;
00080 
00081   Terms _terms;
00082 
00083   Eigen::VectorXd _residual;
00084 
00085 public:
00086 
00087   Jacobian() : _dimtotal(0), _residual()  {}
00088 
00089   Jacobian(Eigen::VectorXd& residual) : _dimtotal(0), _residual(residual) {}
00090 
00091   inline Jacobian(const double * residual, int size) : _dimtotal(0), _residual(size) {
00092     memcpy(_residual.data(), residual, size*sizeof(double));
00093   }
00094 
00095   const Terms& terms() const {return _terms;}
00096 
00097   // note: rhs for linear system Ax=b is negative of residual!
00098   Eigen::VectorXd rhs() const {return - _residual;}
00099 
00100   void add_term(Node* node, const Eigen::MatrixXd& term) {
00101     _terms.push_back(Term(node, term));
00102     _dimtotal += node->dim();
00103   }
00104 
00105   inline void add_term(Node* node, const double* term, int r, int c) {
00106     _terms.push_back(Term(node, term, r, c));
00107     _dimtotal += node->dim();
00108   }
00109   
00110   int dimtotal() const { return _dimtotal; }
00111 
00112   void write(std::ostream &out) const {
00113     int i=1;
00114     for (Terms::const_iterator it = _terms.begin(); it != _terms.end(); it++, i++) {
00115       out << "Term " << i << ":" << std::endl;
00116       out << *it;
00117     }
00118     out << "rhs: " << std::endl << _residual << std::endl;
00119   }
00120 };
00121 
00122 }
 All Classes Files Functions Variables