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
00046
00047
00048 MatrixXd node_exmap_jacobian (Node *n) {
00049
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
00058 VectorXd o = n->vector0();
00059
00060 d(col) = eps;
00061 n->self_exmap(d);
00062 VectorXd yp = n->vector0();
00063 n->update0(o);
00064
00065 d(col) = -eps;
00066 n->self_exmap(d);
00067 VectorXd ym = n->vector0();
00068 n->update0(o);
00069
00070 VectorXd diff = (yp - ym) / (eps2);
00071
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
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
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
00193 if (node_list.end() == find (node_list.begin(), node_list.end(), f_nodes[i])) {
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
00210 for (size_t i=0; i<clique_nodes.size(); i++) {
00211
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
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
00223
00224
00225 if (f_nodes.end() != find (f_nodes.begin(), f_nodes.end(), node))
00226 continue;
00227
00228
00229 bool ic = true;
00230 for (size_t j=0; j<f_nodes.size() && ic; j++) {
00231
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);
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
00267
00268
00269
00270 MatrixXd H = glc_get_weighted_jacobian (f);
00271 MatrixXd dL = H.transpose()*H;
00272
00273
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
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
00302 L.block(koff, loff, nk->dim(), nl->dim()) += dL.block(ioff, joff, ni->dim(), nj->dim());
00303 if (ni != nj)
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
00317 #endif
00318
00319 }
00320
00321
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
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) {
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;
00360 }
00361
00362 } else {
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
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
00496
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
00504 vector<Factor*> new_glc_factors;
00505 new_glc_factors = glc_lift_factors (L, clique_nodes, sparse, rp);
00506
00507
00508 slam.remove_node(node);
00509
00510
00511 for(size_t i=0; i<ic_factors.size(); i++) {
00512 slam.remove_factor(ic_factors[i]);
00513 }
00514
00515
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 }