numericalDiff.cpp
Go to the documentation of this file.
00001 
00028 #include <iostream>
00029 
00030 #include "isam/numericalDiff.h"
00031 
00032 #define SYMMETRIC
00033 
00034 const double epsilon = 0.0001;
00035 
00036 using namespace std;
00037 using namespace Eigen;
00038 
00039 namespace isam {
00040 
00041 MatrixXd numericalDiff(Function& func) {
00042 #ifndef SYMMETRIC
00043   VectorXd y0 = func.evaluate();
00044 #endif
00045   // number of measurement rows
00046   int m = func.num_measurements();
00047   // number of variables
00048   int n = 0;
00049   vector<Node*>& nodes = func.nodes();
00050   for (vector<Node*>::iterator it = nodes.begin(); it!=nodes.end(); it++) {
00051     n += (*it)->dim();
00052   }
00053   // result has one column per variable
00054   MatrixXd Jacobian(m,n);
00055   int col = 0;
00056   // for each node...
00057   for (vector<Node*>::iterator it = nodes.begin(); it!=nodes.end(); it++) {
00058     Node* node = *it;
00059     int dim_n = node->dim();
00060     // for each dimension of the node...
00061     for (int j=0; j<dim_n; j++, col++) {
00062       VectorXd delta(dim_n);
00063       delta.setZero();
00064       // remember original value
00065       VectorXd original = node->vector0();
00066       // evaluate positive delta
00067       delta(j) = epsilon;
00068       node->self_exmap(delta);
00069       VectorXd y_plus = func.evaluate();
00070       node->update0(original);
00071 #ifdef SYMMETRIC
00072       // evaluate negative delta
00073       delta(j) = -epsilon;
00074       node->self_exmap(delta);
00075       VectorXd y_minus = func.evaluate();
00076       node->update0(original);
00077       // store column
00078       VectorXd diff = (y_plus - y_minus) / (epsilon + epsilon);
00079 #else
00080       VectorXd diff = (y_plus - y0) / epsilon;
00081 #endif
00082       Jacobian.col(col) = diff;
00083     }
00084   }
00085 
00086   return Jacobian;
00087 }
00088 
00089 }


demo_lidar
Author(s): Ji Zhang
autogenerated on Sun Oct 5 2014 23:22:43