Factor.h
Go to the documentation of this file.
00001 
00028 #pragma once
00029 
00030 #include <vector>
00031 #include <string>
00032 
00033 #include <math.h> // for sqrt
00034 #include <Eigen/Dense>
00035 
00036 #include "util.h"
00037 #include "Jacobian.h"
00038 #include "Element.h"
00039 #include "Node.h"
00040 #include "Noise.h"
00041 #include "numericalDiff.h"
00042 
00043 namespace isam {
00044 
00045 //typedef double (*cost_func_t)(double);
00046 
00047 
00048 // Factor of the graph of measurements between Nodes.
00049 class Factor : public Element, Function {
00050   friend std::ostream& operator<<(std::ostream& output, const Factor& e) {
00051     e.write(output);
00052     return output;
00053   }
00054 
00055   cost_func_t *ptr_cost_func;
00056 
00057   static int _next_id;
00058   bool _deleted;
00059 protected:
00060 
00061   const Noise _noise;
00062 
00063   std::vector<Node*> _nodes; // list of nodes affected by measurement
00064 
00065 public:
00066 
00067   virtual Eigen::VectorXd error(Selector s = ESTIMATE) const {
00068     Eigen::VectorXd err = _noise.sqrtinf() * basic_error(s);
00069     // optional modified cost function
00070     if (*ptr_cost_func) {
00071       for (int i=0; i<err.size(); i++) {
00072         double val = err(i);
00073         err(i) = ((val>=0)?1.:(-1.)) * sqrt((*ptr_cost_func)(val));
00074       }
00075     }
00076     return err;
00077   }
00078 
00079   std::vector<Node*>& nodes() {return _nodes;}
00080 
00081   Factor(const char* name, int dim, const Noise& noise)
00082     : Element(name, dim), ptr_cost_func(NULL), _deleted(false), _noise(noise) {
00083 #ifndef NDEBUG
00084     // all lower triagular entries below the diagonal must be 0
00085     for (int r=0; r<_noise.sqrtinf().rows(); r++) {
00086       for (int c=0; c<r; c++) {
00087         requireDebug(_noise.sqrtinf()(r,c)==0, "Factor::Factor: sqrtinf must be upper triangular!");
00088       }
00089     }
00090 #endif
00091     _id = _next_id++;
00092   }
00093 
00094   virtual ~Factor() {}
00095 
00096   virtual void initialize() = 0;
00097 
00098   virtual void initialize_internal() {
00099     for (unsigned int i=0; i<_nodes.size(); i++) {
00100       _nodes[i]->add_factor(this);
00101     }
00102     initialize();
00103   }
00104 
00105   virtual void set_cost_function(cost_func_t* ptr) {ptr_cost_func = ptr;}
00106 
00107   virtual Eigen::VectorXd basic_error(Selector s = ESTIMATE) const = 0;
00108 
00109   virtual const Eigen::MatrixXd& sqrtinf() const {return _noise.sqrtinf();}
00110 
00111   Eigen::VectorXd evaluate() const {
00112     return error(LINPOINT);
00113   }
00114 
00115   virtual Jacobian jacobian_internal(bool force_numerical) {
00116     if (force_numerical) {
00117       // ignore any symbolic derivative provided by user
00118       return Factor::jacobian();
00119     } else {
00120       return jacobian();
00121     }
00122   }
00123 
00124   // can be replaced by symbolic derivative by user
00125   virtual Jacobian jacobian() {
00126     Eigen::MatrixXd H = numerical_jacobian();
00127     Eigen::VectorXd r = error(LINPOINT);
00128     Jacobian jac(r);
00129     int position = 0;
00130     int n_measure = dim();
00131     for (unsigned int i=0; i<_nodes.size(); i++) {
00132       int n_var = _nodes[i]->dim();
00133       Eigen::MatrixXd Hi = H.block(0, position, n_measure, n_var);
00134       position += n_var;
00135       jac.add_term(_nodes[i], Hi);
00136     }
00137     return jac;
00138   }
00139 
00140   int num_measurements() const {
00141     return dim();
00142   }
00143 
00144   void mark_deleted() { _deleted = true; }
00145   bool deleted() const { return _deleted; }
00146 
00147   virtual void write(std::ostream &out) const {
00148     Element::write(out);
00149     for (unsigned int i=0; i<_nodes.size(); i++) {
00150       if (_nodes[i]) {
00151         out << " " << _nodes[i]->unique_id();
00152       }
00153     }
00154   }
00155 
00156 private:
00157 
00158   virtual Eigen::MatrixXd numerical_jacobian() {
00159     return numericalDiff(*this);
00160   }
00161 
00162 };
00163 
00168 inline std::string noise_to_string(const Noise& noise) {
00169   int nrows = noise.sqrtinf().rows();
00170   int ncols = noise.sqrtinf().cols();
00171   require(nrows==ncols, "slam2d::sqrtinf_to_string: matrix must be square");
00172   std::stringstream s;
00173   s << "{";
00174   bool first = true;
00175   for (int r=0; r<nrows; r++) {
00176     for (int c=r; c<ncols; c++) {
00177       if (first) {
00178         first = false;
00179       } else {
00180         s << ",";
00181       }
00182       s << noise.sqrtinf()(r,c);
00183     }
00184   }
00185   s << "}";
00186   return s.str();
00187 }
00188 
00189 // Generic template for easy instantiation of new factors
00190 template <class T>
00191 class FactorT : public Factor {
00192 
00193 protected:
00194 
00195   const T _measure;
00196   
00197 public:
00198   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00199 
00200   FactorT(const char* name, int dim, const Noise& noise, const T& measure) : Factor(name, dim, noise), _measure(measure) {}
00201 
00202   const T& measurement() const {return _measure;}
00203 
00204   void write(std::ostream &out) const {
00205     Factor::write(out);
00206     out << " " << _measure << " " << noise_to_string(_noise);
00207   }
00208 
00209 };
00210 
00211 
00212 }


demo_lidar
Author(s): Ji Zhang
autogenerated on Sun Mar 1 2015 11:30:50