glc.cpp
Go to the documentation of this file.
00001 
00029 #include <vector>
00030 
00031 #include "isam/glc.h"
00032 #include "isam/util.h"
00033 #include "isam/Rot3d.h"
00034 #include "isam/ChowLiuTree.h"
00035 
00036 #define GLC_EPS 1e-8
00037 #define GLC_INCLUDE_IC_FACTORS
00038 
00039 using namespace std;
00040 using namespace Eigen;
00041 
00042 namespace isam {
00043 
00044 #ifdef USE_QUATERNIONS
00045 // code to account for angle axis to euler state represention change when using quaternions
00046 // this would evaluate to identity if exmap wasn't mapping between euler and angle axis
00047 // something similar can be use to trasform covariance recovery output when using quats
00048 MatrixXd node_exmap_jacobian (Node *n) {
00049   // TODO replace with analytical jacobian
00050   const double eps = 0.0001;
00051   const double eps2 = 0.0002;
00052   int dim_n = n->dim();
00053   MatrixXd J(dim_n, dim_n);
00054   for(int col=0; col<dim_n; col++){
00055     VectorXd d(dim_n);
00056     d.setZero();
00057     // remember original value
00058     VectorXd o = n->vector0();
00059     // evaluate positive delta
00060     d(col) = eps;
00061     n->self_exmap(d);
00062     VectorXd yp = n->vector0();
00063     n->update0(o);
00064     // evaluate negative delta
00065     d(col) = -eps;
00066     n->self_exmap(d);
00067     VectorXd ym = n->vector0();
00068     n->update0(o);
00069     // calculte diff
00070     VectorXd diff = (yp - ym) / (eps2);
00071     // wrap angular difference
00072     VectorXb is_angle = n->is_angle();
00073     for (int k=0; k<dim_n; k++) {
00074       if (is_angle(k))
00075         diff(k) = standardRad(diff(k));
00076     }
00077     J.col(col) = diff;
00078   }
00079   return J;
00080 }
00081 
00082 MatrixXd exmap_jacobian (const vector<Node*>& nodes) {
00083   int dim = 0;
00084   for (size_t i=0; i<nodes.size(); i++) {
00085     dim += nodes[i]->dim();;
00086   }
00087   MatrixXd J = MatrixXd::Identity(dim, dim);
00088   int off=0;
00089   for (size_t i=0; i<nodes.size(); i++) {
00090     int dim_n = nodes[i]->dim();
00091     J.block(off,off,dim_n,dim_n) = node_exmap_jacobian (nodes[i]);
00092     off += dim_n;
00093   }
00094   return J;
00095 }
00096 #endif // USE_QUATERNIONS
00097 
00098 MatrixXd glc_cholcov (MatrixXd A, double eps = numeric_limits<float>::epsilon()) {
00099     
00100   A = (A.transpose() + A) / 2.0;
00101   
00102   SelfAdjointEigenSolver<MatrixXd> eigensolver(A);
00103   if (eigensolver.info() != Success) {
00104     cout << "ERROR: Failed to compute eigenvalue decomposition!" << endl;
00105     cout << "A:" << endl << A << endl;
00106   }
00107        
00108   VectorXd D = eigensolver.eigenvalues();
00109   MatrixXd U = eigensolver.eigenvectors();
00110   
00111   double tol = eps * D.size();
00112   
00113   vector<int> inds_pos;
00114   for (int i=0; i<D.size(); i++) {
00115     if (D(i) > tol) 
00116       inds_pos.push_back(i);
00117   }
00118   
00119   if (0 == inds_pos.size()) {
00120       return MatrixXd();
00121   }
00122   
00123   VectorXd Dsub_sqrt(inds_pos.size());
00124   MatrixXd Usub(U.rows(), inds_pos.size());
00125   for (size_t i=0; i<inds_pos.size(); i++) {
00126     Dsub_sqrt(i) = sqrt(D(inds_pos[i]));
00127     Usub.col(i) = U.col(inds_pos[i]);
00128   }
00129   
00130   MatrixXd B;
00131   if (inds_pos.size() == (unsigned int)D.size()) {
00132     B = A.llt().matrixL().transpose(); 
00133   } else {
00134     B = Dsub_sqrt.asDiagonal() * Usub.transpose();
00135   }
00136   
00137 #ifdef GLC_DEBUG
00138   double recreate_test = (B.transpose()*B - A).array().abs().matrix().lpNorm<Infinity>();
00139   cout << "[glc]\tCholcov Recreate Test: " << recreate_test << endl;
00140   if (recreate_test > 10) {
00141     cout << "ERROR: Cholcov Recreate Test Failed!" << endl;
00142     cout << "A" << endl << A << endl;
00143   }
00144 #endif
00145 
00146   return B;   
00147 }
00148 
00149 MatrixXd glc_get_weighted_jacobian (isam::Factor *f) {
00150 
00151     Jacobian jac = f->jacobian();
00152     // get jacobian size
00153     std::vector<Node*>& f_nodes = f->nodes();
00154     int n_measure = f->dim();
00155     int n_var = 0;
00156     for (size_t i=0; i<f_nodes.size(); i++) {
00157         n_var += f_nodes[i]->dim();
00158     }
00159         
00160     // fill jacobian matrix
00161     MatrixXd H (n_measure, n_var);
00162     int offset = 0;
00163     for (Terms::const_iterator it=jac.terms().begin(); it!=jac.terms().end(); it++) {
00164         int ncols = it->term().cols();
00165         H.block(0, offset, n_measure, ncols) = it->term();
00166         offset += ncols;
00167     }
00168     
00169     return H;
00170 
00171 }
00172 
00173 
00174 std::vector<isam::Node*> glc_elim_clique_nodes (Node *node) {
00175     
00176   vector<isam::Node*> node_vector;
00177   
00178   int id_e = node->unique_id();
00179   
00180   std::list< isam::Node* > node_list;
00181   const list<Factor*>& factors = node->factors();
00182   for (list<Factor*>::const_iterator it = factors.begin(); it!=factors.end(); it++) {
00183       
00184     std::vector<Node*>& f_nodes = (*it)->nodes();
00185     
00186     for (size_t i=0; i<f_nodes.size(); i++) {
00187     
00188       int id_i = f_nodes[i]->unique_id();
00189       
00190       if (id_e != id_i) {
00191           
00192         // try to add second node in factor
00193         if (node_list.end() == find (node_list.begin(), node_list.end(), f_nodes[i])) { // not already added
00194           node_list.push_back(f_nodes[i]);
00195           node_vector.push_back (f_nodes[i]);
00196         }       
00197       }   
00198     }        
00199   }
00200   
00201   return node_vector;
00202 }
00203 
00204 vector<Factor*>
00205 glc_intra_clique_factors (vector<Node*> clique_nodes, Node *node) {
00206 
00207   vector<Factor*> ic_factors;
00208 
00209   // loop over each node
00210   for (size_t i=0; i<clique_nodes.size(); i++) {
00211     // get this nodes factors
00212     const std::list<Factor*> factors = clique_nodes[i]->factors();
00213 
00214     for (list<Factor*>::const_iterator it = factors.begin(); it!=factors.end(); it++) {
00215 
00216       // make sure factor hasnt already been added to the list
00217       if (ic_factors.end() != find (ic_factors.begin(), ic_factors.end(), (*it)))
00218           continue;
00219 
00220       std::vector<Node*>& f_nodes = (*it)->nodes();
00221 
00222       // nodes in these factors can be: the marg node, nodes in the clique, or nodes ouside the clique
00223       // we wish to return factors strictly between nodes in the clique, not outside nor margnode
00224       // is the marg node in this factor
00225       if (f_nodes.end() != find (f_nodes.begin(), f_nodes.end(), node))
00226           continue; // node found
00227 
00228       // strctly include in clique
00229       bool ic = true;
00230       for (size_t j=0; j<f_nodes.size() && ic; j++) {
00231         // if we find a factor node that is not in the clique
00232         if (clique_nodes.end() == find (clique_nodes.begin(), clique_nodes.end(), f_nodes[j]))
00233           ic = false;
00234       }
00235 
00236       if (ic)
00237         ic_factors.push_back(*it);
00238 
00239     }
00240   }
00241 
00242   return ic_factors;
00243 
00244 }
00245 
00246 MatrixXd glc_target_info (Node *node, vector<Node*>& clique_nodes, vector<Factor*>& ic_factors){
00247 
00248   vector<Node*> all_nodes (clique_nodes);
00249   all_nodes.push_back(node);
00250   
00251   int n_full = 0;
00252   for (size_t i=0; i<all_nodes.size(); i++) n_full += all_nodes[i]->dim();
00253   MatrixXd L (n_full, n_full); // clique nodes first then marg node at end
00254   L.setZero();
00255   
00256   const list<Factor*>& tmp = node->factors();
00257   list<Factor*> factors (tmp);
00258   for (size_t i=0; i<ic_factors.size(); i++)
00259     factors.push_back(ic_factors[i]);
00260   
00261   for (list<Factor*>::iterator it = factors.begin(); it!=factors.end(); it++) {
00262     
00263     Factor *f = *it;
00264 
00266     //if (0 == strcmp("Pose3d_z_Factor", f->name())) {
00267     //  continue;
00268     //}
00269 
00270     MatrixXd H = glc_get_weighted_jacobian (f);
00271     MatrixXd dL = H.transpose()*H;
00272     
00273     // there is probably a better/faster way to do this
00274     vector<Node*>& f_nodes = f->nodes();
00275     int ioff = 0;
00276     for (size_t i=0; i<f_nodes.size(); i++) {
00277       Node *ni = f_nodes[i];
00278       int joff = ioff;
00279       for (size_t j=i; j<f_nodes.size(); j++) {
00280         Node *nj = f_nodes[j];
00281         
00282         // find the position(s) of this block in the target information
00283         int koff = 0;
00284         Node *nk = NULL;
00285         for (size_t k=0; k<all_nodes.size(); k++) {
00286           if (ni == all_nodes[k]) {
00287             nk = all_nodes[k];
00288             break;
00289           }
00290           koff += all_nodes[k]->dim();
00291         }
00292         int loff = 0;
00293         Node *nl = NULL;
00294         for (size_t l=0; l<all_nodes.size(); l++) {
00295           if (nj == all_nodes[l]) {
00296             nl = all_nodes[l];
00297             break;
00298           }
00299           loff += all_nodes[l]->dim();
00300         }
00301         // add information to the target informaiton
00302         L.block(koff, loff, nk->dim(), nl->dim()) += dL.block(ioff, joff, ni->dim(), nj->dim());
00303         if (ni != nj) // off diagonal elements 
00304           L.block(loff, koff, nl->dim(), nk->dim()) += dL.block(joff, ioff, nj->dim(), ni->dim());
00305         
00306         joff += nj->dim();  
00307       }
00308       ioff += ni->dim();
00309     }
00310     
00311 #ifdef GLC_DEBUG
00312     cout << "[glc]\tAdding info from " << f->name() << " factor between nodes : ";
00313     for (size_t n=0; n<f_nodes.size(); n++)
00314         cout << f_nodes[n]->unique_id() << " ";
00315     cout << endl;
00316     //cout << "L" << endl << L << endl;
00317 #endif
00318 
00319   }
00320   
00321   // marginalization
00322   int n = n_full - node->dim();
00323   MatrixXd Lbb = L.bottomRightCorner(node->dim(), node->dim());
00324   MatrixXd L_marg = L.topLeftCorner(n,n) - (L.topRightCorner(n,node->dim()) * posdef_pinv(Lbb, GLC_EPS) * L.bottomLeftCorner(node->dim(),n));
00325   //cout << "L_marg" << endl << L_marg << endl;
00326   return L_marg;
00327 
00328 }
00329 
00330 Factor* glc_factor (const MatrixXd& L, const vector<Node*>& clique_nodes, GLC_Reparam* rp) {
00331     
00332     int np = clique_nodes.size();
00333     int n = 0;
00334     for (int i=0; i<np; i++) n += clique_nodes[i]->dim();
00335 
00336     MatrixXd Lp = L;
00337 #ifdef USE_QUATERNIONS
00338     MatrixXd Jexmap = exmap_jacobian (clique_nodes);
00339     MatrixXd Jpinv = pinv(Jexmap, GLC_EPS);
00340     Lp = Jpinv.transpose() * L * Jpinv;
00341 #endif
00342         
00343     Factor *f_add = NULL;
00344     if (rp == NULL) { // H with linearization on world frame xfms
00345 
00346       MatrixXd G = glc_cholcov(Lp, GLC_EPS);
00347 
00348       VectorXd x(n);
00349       x.setZero();
00350       int off = 0;
00351       for (int i=0; i<np; i++) {
00352         x.segment(off, clique_nodes[i]->dim()) = clique_nodes[i]->vector(LINPOINT);
00353         off += clique_nodes[i]->dim();
00354       }
00355 
00356       if (G.rows() != 0 && G.cols() != 0) {
00357         f_add = new GLC_Factor(clique_nodes, x, G, NULL);
00358       } else {
00359         return NULL; // can happen when removing node at end of chain
00360       }
00361     
00362     } else { // H with linearization on root shifted xfms
00363 
00364       GLC_Reparam *rptmp = rp->clone();
00365       rptmp->set_nodes (clique_nodes);
00366       VectorXd x_rp = rptmp->reparameterize(LINPOINT);
00367       MatrixXd F = rptmp->jacobian();
00368       delete rptmp;
00369 
00370       MatrixXd Fpinv = pinv(F, GLC_EPS);
00371       MatrixXd UTU = Fpinv.transpose() * Lp * Fpinv;
00372 
00373       MatrixXd G;
00374       G = glc_cholcov(UTU, GLC_EPS);
00375 
00376       if(G.rows() == 0) {
00377 #ifdef GLC_DEBUG
00378         cout << "[GLC]\t\tComputed No Rank Factor! Skipping" << endl;
00379 #endif
00380         if (Lp.rows() <= 6) {
00381           return NULL;
00382         } else {
00383           cout << "Lp" << endl << Lp << endl;
00384           exit(0);
00385         }
00386       }
00387 
00388 #ifdef GLC_DEBUG
00389       MatrixXd H = G*F;
00390       double recreate_test = (H.transpose()*H - Lp).array().abs().matrix().lpNorm<Infinity>();
00391       cout << "[glc]\tTarget Recreate Test: " << recreate_test << endl;
00392       if (recreate_test > 10) {
00393         cout << "[ERROR]\tRecreate Test Failed!" << endl;
00394         cout << "Lp" << endl << Lp << endl;
00395         cout << "F" << endl << F << endl;
00396         exit(0);
00397       }
00398 #endif
00399 
00400       f_add = new GLC_Factor(clique_nodes, x_rp, G, rp);
00401     }
00402 
00403 #ifdef GLC_DEBUG
00404     if (f_add != NULL) {
00405       cout << "[glc]\t\tComputed GLC_Factor " << f_add->unique_id() << " between nodes ";
00406       for (int i=0; i<np; i++)
00407           cout << clique_nodes[i]->unique_id() << " ";
00408       cout << endl;
00409     }
00410 #endif
00411     
00412     return f_add;
00413     
00414 }
00415 
00416 vector<Factor*> glc_lift_factors (const MatrixXd& L, const vector<Node*>& clique_nodes, bool sparse,
00417                                   GLC_Reparam* rp) {
00418   
00419   vector<Factor*> new_glc_factors;    
00420 
00421   if (sparse) {
00422 
00423     ChowLiuTree clt(L, clique_nodes);
00424     
00425     map<int, ChowLiuTreeNode>::iterator it;
00426     for (it=clt.tree.begin(); it!=clt.tree.end(); it++) {
00427       
00428       if (it->second.is_root()) {
00429         MatrixXd La = it->second.marginal;
00430         double mag_test = La.array().abs().matrix().lpNorm<Infinity>();
00431         if (mag_test > GLC_EPS) {
00432           vector<Node*> f_nodes;
00433           f_nodes.push_back(clique_nodes[it->second.id]);
00434           Factor * f_add = glc_factor (La, f_nodes, rp);
00435           if (f_add != NULL) {
00436             new_glc_factors.push_back (f_add);
00437           }
00438         }
00439       } else {
00440         vector<Node*> f_nodes;
00441         f_nodes.push_back(clique_nodes[it->second.id]);
00442         f_nodes.push_back(clique_nodes[it->second.pid]);
00443         int adim = f_nodes[0]->dim();
00444         int bdim = f_nodes[1]->dim();
00445         MatrixXd Lagb = it->second.conditional;
00446         MatrixXd Lab = it->second.joint;
00447         MatrixXd Hagb(adim, adim+bdim);
00448         Hagb.block(0,0,adim,adim) = MatrixXd::Identity (adim,adim);
00449         MatrixXd Laa = Lab.block(0,0,adim,adim);
00450         Hagb.block(0,adim,adim,bdim) = posdef_pinv(Laa) * Lab.block(0,adim,adim,bdim);
00451         MatrixXd Lt = Hagb.transpose() * Lagb * Hagb;
00452         Factor * f_add = glc_factor (Lt, f_nodes, rp);
00453         if (f_add != NULL)
00454             new_glc_factors.push_back (f_add);
00455       }
00456     }
00457     
00458   } else {
00459     Factor * f_add = glc_factor (L, clique_nodes, rp);
00460     if (f_add != NULL)
00461       new_glc_factors.push_back (f_add);
00462   }
00463       
00464   return new_glc_factors;
00465   
00466 }
00467 
00468 vector<Factor*> glc_elim_factors (Node* node) {
00469 
00470   vector<Factor*> elim_factors (node->factors().begin(), node->factors().end());
00471 #ifdef GLC_INCLUDE_IC_FACTORS
00472   vector<Factor*> ic_factors;
00473   ic_factors = glc_intra_clique_factors (glc_elim_clique_nodes (node), node);
00474   elim_factors.insert(elim_factors.end(), ic_factors.begin(), ic_factors.end());
00475 #endif
00476   return elim_factors;
00477 
00478 }
00479 
00480 
00481 
00482 vector<Factor*> glc_remove_node(Slam& slam, Node* node, bool sparse, GLC_Reparam* rp) {
00483   
00484   // get the nodes in the elimination clique, new glc factor(s) will span these nodes
00485   vector<Node*> clique_nodes = glc_elim_clique_nodes (node);
00486     
00487 #ifdef GLC_DEBUG
00488   cout << "[glc]\tRemoving Node: " << node->unique_id() << endl;
00489   cout << "[glc]\tElimination Clique Nodes: ";
00490   for (size_t i=0; i<clique_nodes.size(); i++)
00491     cout << clique_nodes[i]->unique_id() << " ";
00492   cout << endl;
00493 #endif
00494   
00495   // intra-clique factors, those in the clique that are not directly connected to the marg node
00496   // not strictly required, initial icra paper included them
00497   vector<Factor*> ic_factors;
00498 #ifdef GLC_INCLUDE_IC_FACTORS
00499   ic_factors = glc_intra_clique_factors (clique_nodes, node);
00500 #endif
00501   MatrixXd L = glc_target_info (node, clique_nodes, ic_factors);
00502 
00503   // lift glc factors
00504   vector<Factor*> new_glc_factors;
00505   new_glc_factors = glc_lift_factors (L, clique_nodes, sparse, rp);
00506 
00507   // remove node and delete all adjacent factors
00508   slam.remove_node(node);  
00509 
00510   // remove all ic factors
00511   for(size_t i=0; i<ic_factors.size(); i++) {
00512     slam.remove_factor(ic_factors[i]);
00513   }
00514   
00515   // add glc factors
00516   for(size_t i=0; i<new_glc_factors.size(); i++) {
00517     slam.add_factor(new_glc_factors[i]);
00518 #ifdef GLC_DEBUG
00519     cout << "[glc]\tAdded GLC Factor: " << new_glc_factors[i]->unique_id() << endl;
00520 #endif
00521   }
00522   
00523 
00524   return new_glc_factors;
00525 
00526 }
00527 
00528 } // namespace isam


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