GLCReparam.cpp
Go to the documentation of this file.
00001 
00029 #include <vector>
00030 
00031 #include "isam/GLCReparam.h"
00032 
00033 #include "isam/Pose3d.h"
00034 #include "isam/Point3d.h"
00035 #include "isam/Pose2d.h"
00036 #include "isam/Point2d.h"
00037 #include "isam/slam2d.h"
00038 #include "isam/slam3d.h"
00039 
00040 using namespace std;
00041 using namespace Eigen;
00042 
00043 namespace isam {
00044 
00045 VectorXd GLC_RootShift::reparameterize (Selector s) {
00046 
00047   VectorXd x;
00048 
00049   int np = _nodes.size();
00050 
00051   if (np == 1) {
00052     x = _nodes[0]->vector(s);
00053     return x;
00054   }
00055 
00056   x.resize(_dim);
00057   x.setZero();
00058 
00059   // decide on the root for the root shift
00060   // for now just use the first pose2d or pose3d
00061   int ir=0;
00062   for (ir=0; ir<np; ir++) {
00063     if (0 == strcmp(_nodes[ir]->name(), "Pose3d") ||
00064         0 == strcmp(_nodes[ir]->name(), "Pose2d"))
00065       break;
00066   }
00067   assert (ir != np); // assumes a pose3d or pose2d node is avaliable
00068   Node *node_i = _nodes[ir];
00069 
00070   int ioff = 0;
00071   for (int j=0; j<np; j++) {
00072     if (j == ir) { // root node
00073       x.segment(ioff, node_i->dim()) = node_i->vector(s);
00074       ioff += node_i->dim();
00075     } else {
00076       Node *node_j = _nodes[j];
00077       VectorXd x_i_j = root_shift (node_i, node_j, s);
00078       assert(node_j->dim() == x_i_j.size());
00079       x.segment(ioff, node_j->dim()) = x_i_j;
00080       ioff += node_j->dim();
00081     }
00082   }
00083 
00084   return x;
00085 }
00086 
00087 VectorXd GLC_RootShift::root_shift (Node* node_i, Node* node_j, Selector s) {
00088 
00089   VectorXd x_i_j;
00090   if (0 == strcmp(node_i->name(), "Pose3d")) {
00091     Pose3d pose3d_i = dynamic_cast<Pose3d_Node*>(node_i)->value(s);
00092     if (0 == strcmp(node_j->name(), "Pose3d")) {
00093       x_i_j = dynamic_cast<Pose3d_Node*>(node_j)->value(s).ominus(pose3d_i).vector();
00094     } else if (0 == strcmp(node_j->name(), "Point3d")) {
00095       x_i_j = pose3d_i.transform_to(dynamic_cast<Point3d_Node*>(node_j)->value(s)).vector();
00096     } else {
00097       std::cout << "Warning: root shift undefined for:" << node_i->name() << " and " << node_j->name() << std::endl;
00098       x_i_j = node_j->vector(s); // identity
00099     }
00100   } else if (0 == strcmp(node_i->name(), "Pose2d")) {
00101     Pose2d pose2d_i = dynamic_cast<Pose2d_Node*>(node_i)->value(s);
00102     if (0 == strcmp(node_j->name(), "Pose2d")) {
00103       x_i_j = dynamic_cast<Pose2d_Node*>(node_j)->value(s).ominus(pose2d_i).vector();
00104     } else if (0 == strcmp(node_j->name(), "Point2d")) {
00105       x_i_j = pose2d_i.transform_to(dynamic_cast<Point2d_Node*>(node_j)->value(s)).vector();
00106     } else {
00107       std::cout << "Warning: root shift undefined for:" << node_i->name() << " and " << node_j->name() << std::endl;
00108       x_i_j = node_j->vector(s); // identity
00109     }
00110   }
00111 
00112   return x_i_j;
00113 }
00114 
00115 const double epsilon = 0.0001;
00116 
00117 // TODO replace with analytical jacobians
00118 MatrixXd GLC_RootShift::jacobian() {
00119 
00120   MatrixXd J(_dim,_dim);
00121 
00122   int col = 0;
00123   // for each node...
00124   for (vector<Node*>::iterator it = _nodes.begin(); it!=_nodes.end(); it++) {
00125     Node* node = *it;
00126     int dim_n = node->dim();
00127     // for each dimension of the node...
00128     for (int j=0; j<dim_n; j++, col++) {
00129       VectorXd delta(dim_n);
00130       delta.setZero();
00131       // remember original value
00132       VectorXd original = node->vector0();
00133       // evaluate positive delta
00134       delta(j) = epsilon;
00135       //node->self_exmap(delta); // taken care of in exmap_jacobian() in glc.[h/cpp]
00136       node->update0(node->vector0() + delta);
00137       VectorXd y_plus = reparameterize(LINPOINT);
00138       node->update0(original);
00139       // evaluate negative delta
00140       delta(j) = -epsilon;
00141       //node->self_exmap(delta);  // taken care of in exmap_jacobian() in glc.[h/cpp]
00142       node->update0(node->vector0() + delta);
00143       VectorXd y_minus = reparameterize(LINPOINT);
00144       node->update0(original);
00145       // store column
00146       VectorXd diff = (y_plus - y_minus) / (epsilon + epsilon);
00147       // wrap angular difference
00148       for (int k=0; k<_dim; k++) {
00149         if (_is_angle(k))
00150           diff(k) = standardRad(diff(k));
00151       }
00152       J.col(col) = diff;
00153     }
00154   }
00155 
00156   return J;
00157 }
00158 
00159 } // namespace isam


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