spa2d.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 //
36 // Sparse Pose Adjustment classes and functions, 2D version
37 //
38 
39 #include <stdio.h>
41 #include <Eigen/Cholesky>
42 #include <chrono>
43 
44 using namespace Eigen;
45 using namespace std;
46 
47 #include <iostream>
48 #include <iomanip>
49 #include <fstream>
50 
51 // elapsed time in microseconds
52 static long long utime()
53 {
54  auto duration = std::chrono::system_clock::now().time_since_epoch();
55  return std::chrono::duration_cast<std::chrono::microseconds>(duration).count();
56 }
57 
58 namespace sba
59 {
60 
61  // R = [[c -s][s c]]
62  // [R' | R't]
63  void Node2d::setTransform()
64  {
65  w2n(0,0) = w2n(1,1) = cos(arot);
66  w2n(0,1) = sin(arot);
67  w2n(1,0) = -w2n(0,1);
68  w2n.col(2).setZero();
69  w2n.col(2) = -w2n*trans;
70  }
71 
72 
73  //
74  // sets angle derivatives dR'/dth
75  //
76  void Node2d::setDr()
77  {
78  dRdx(0,0) = dRdx(1,1) = w2n(1,0); // -sin(th)
79  dRdx(0,1) = w2n(0,0); // cos(th)
80  dRdx(1,0) = -w2n(0,0); // -cos(th)
81  }
82 
83 
84  // set up Jacobians
85 
86  void Con2dP2::setJacobians(std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &nodes)
87  {
88  // node references
89  Node2d &nr = nodes[ndr];
90  Matrix<double,3,1> &tr = nr.trans;
91  Node2d &n1 = nodes[nd1];
92  Matrix<double,3,1> &t1 = n1.trans;
93 
94  // first get the second frame in first frame coords
95  Eigen::Matrix<double,2,1> pc = nr.w2n * t1;
96 
97  // Jacobians wrt first frame parameters
98 
99  // translational part of 0p1 wrt translational vars of p0
100  // this is just -R0' [from 0t1 = R0'(t1 - t0)]
101  J0.block<2,2>(0,0) = -nr.w2n.block<2,2>(0,0);
102 
103 
104  // translational part of 0p1 wrt rotational vars of p0
105  // dR'/dq * [pw - t]
106  Eigen::Matrix<double,2,1> pwt;
107  pwt = (t1-tr).head(2); // transform translations
108 
109  // dx
110  Eigen::Matrix<double,2,1> dp = nr.dRdx * pwt; // dR'/dq * [pw - t]
111  J0.block<2,1>(0,2) = dp;
112 
113  // rotational part of 0p1 wrt translation vars of p0 => zero
114  J0.block<1,2>(2,0).setZero();
115 
116  // rotational part of 0p1 wrt rotational vars of p0
117  // from 0q1 = (q1-q0)
118  J0(2,2) = -1.0;
119  J0t = J0.transpose();
120 
121  // cout << endl << "J0 " << ndr << endl << J0 << endl;
122 
123  // Jacobians wrt second frame parameters
124  // translational part of 0p1 wrt translational vars of p1
125  // this is just R0' [from 0t1 = R0'(t1 - t0)]
126  J1.block<2,2>(0,0) = nr.w2n.block<2,2>(0,0);
127 
128  // translational part of 0p1 wrt rotational vars of p1: zero
129  J1.block<2,1>(0,2).setZero();
130 
131  // rotational part of 0p1 wrt translation vars of p0 => zero
132  J1.block<1,2>(2,0).setZero();
133 
134 
135  // rotational part of 0p1 wrt rotational vars of p0
136  // from 0q1 = (q1-q0)
137  J1(2,2) = 1.0;
138  J1t = J1.transpose();
139 
140  // cout << endl << "J1 " << nd1 << endl << J1 << endl;
141 
142  };
143 
144 
145 
146  // error function
147  // NOTE: this is h(x) - z, not z - h(x)
148  inline double Con2dP2::calcErr(const Node2d &nd0, const Node2d &nd1)
149  {
150  err.block<2,1>(0,0) = nd0.w2n * nd1.trans - tmean;
151  double aerr = (nd1.arot - nd0.arot) - amean;
152  if (aerr > M_PI) aerr -= 2.0*M_PI;
153  if (aerr < -M_PI) aerr += 2.0*M_PI;
154  err(2) = aerr;
155 
156  // cout << err.transpose() << endl;
157 
158  return err.dot(prec * err);
159  }
160 
161 
162  // error function for distance cost
163  double Con2dP2::calcErrDist(const Node2d &nd0, const Node2d &nd1)
164  {
165  Vector2d derr = nd0.w2n * nd1.trans - tmean;
166  return derr.dot(derr);
167  }
168 
169 
170  // error measure, squared
171  // assumes node transforms have already been calculated
172  // <tcost> is true if we just want the distance offsets
173  double SysSPA2d::calcCost(bool tcost)
174  {
175  double cost = 0.0;
176 
177  // do distance offset
178  if (tcost)
179  {
180  for(size_t i=0; i<p2cons.size(); i++)
181  {
182  Con2dP2 &con = p2cons[i];
183  double err = con.calcErrDist(nodes[con.ndr],nodes[con.nd1]);
184  cost += err;
185  }
186  }
187 
188  // full cost
189  else
190  {
191  for(size_t i=0; i<p2cons.size(); i++)
192  {
193  Con2dP2 &con = p2cons[i];
194  double err = con.calcErr(nodes[con.ndr],nodes[con.nd1]);
195  cost += err;
196  }
197  errcost = cost;
198  }
199 
200  return cost;
201  }
202 
203 
204  // add a node at a pose
205  // <pos> is x,y,th, with th in radians
206  // returns node position
207  int SysSPA2d::addNode(const Vector3d &pos, int id)
208  {
209  Node2d nd;
210  nd.nodeId = id;
211 
212  nd.arot = pos(2);
213  nd.trans.head(2) = pos.head(2);
214  nd.trans(2) = 1.0;
215 
216  // add in to system
217  nd.setTransform(); // set up world2node transform
218  nd.setDr();
219  int ndi = nodes.size();
220  nodes.push_back(nd);
221  return ndi;
222  }
223 
224  // add a constraint
225  // <nd0>, <nd1> are node id's
226  // <mean> is x,y,th, with th in radians
227  // <prec> is a 3x3 precision matrix (inverse covariance
228  // returns true if nodes are found
229  // TODO: make node lookup more efficient
230  bool SysSPA2d::addConstraint(int ndi0, int ndi1, const Vector3d &mean,
231  const Matrix3d &prec)
232  {
233  int ni0 = -1, ni1 = -1;
234  for (int i=0; i<(int)nodes.size(); i++)
235  {
236  if (nodes[i].nodeId == ndi0)
237  ni0 = i;
238  if (nodes[i].nodeId == ndi1)
239  ni1 = i;
240  }
241  if (ni0 < 0 || ni1 < 0) return false;
242 
243  Con2dP2 con;
244  con.ndr = ni0;
245  con.nd1 = ni1;
246 
247  con.tmean = mean.head(2);
248  con.amean = mean(2);
249  con.prec = prec;
250  p2cons.push_back(con);
251  return true;
252  }
253 
254 
255  // Set up linear system
256  // Use dense matrices
257 
258  void SysSPA2d::setupSys(double sLambda)
259  {
260  // set matrix sizes and clear
261  // assumes scales vars are all free
262  int nFree = nodes.size() - nFixed;
263  A.setZero(3*nFree,3*nFree);
264  B.setZero(3*nFree);
265  VectorXi dcnt(nFree);
266  dcnt.setZero(nFree);
267 
268  // lambda augmentation
269  double lam = 1.0 + sLambda;
270 
271  // loop over P2 constraints
272  for(size_t pi=0; pi<p2cons.size(); pi++)
273  {
274  Con2dP2 &con = p2cons[pi];
275  con.setJacobians(nodes);
276 
277  // add in 4 blocks of A
278  int i0 = 3*(con.ndr-nFixed); // will be negative if fixed
279  int i1 = 3*(con.nd1-nFixed); // will be negative if fixed
280 
281  if (i0>=0)
282  {
283  A.block<3,3>(i0,i0) += con.J0t * con.prec * con.J0;
284  dcnt(con.ndr - nFixed)++;
285  }
286  if (i1>=0)
287  {
288  dcnt(con.nd1 - nFixed)++;
289  Matrix<double,3,3> tp = con.prec * con.J1;
290  A.block<3,3>(i1,i1) += con.J1t * tp;
291  if (i0>=0)
292  {
293  A.block<3,3>(i0,i1) += con.J0t * con.prec * con.J1;
294  A.block<3,3>(i1,i0) += con.J1t * con.prec * con.J0;
295  }
296  }
297 
298  // add in 2 blocks of B
299  if (i0>=0)
300  B.block<3,1>(i0,0) -= con.J0t * con.prec * con.err;
301  if (i1>=0)
302  B.block<3,1>(i1,0) -= con.J1t * con.prec * con.err;
303  } // finish P2 constraints
304 
305 
306  // augment diagonal
307  A.diagonal() *= lam;
308 
309  // check the matrix and vector
310  for (int i=0; i<3*nFree; i++)
311  for (int j=0; j<3*nFree; j++)
312  if (std::isnan(A(i,j)) ) { printf("[SetupSys] NaN in A\n"); *(int *)0x0 = 0; }
313 
314  for (int j=0; j<3*nFree; j++)
315  if (std::isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }
316 
317  int ndc = 0;
318  for (int i=0; i<nFree; i++)
319  if (dcnt(i) == 0) ndc++;
320 
321  if (ndc > 0)
322  cout << "[SetupSys] " << ndc << " disconnected nodes" << endl;
323  }
324 
325 
326  // Set up sparse linear system; see setupSys for algorithm.
327  // Currently doesn't work with scale variables
328  void SysSPA2d::setupSparseSys(double sLambda, int iter, int sparseType)
329  {
330  // set matrix sizes and clear
331  // assumes scales vars are all free
332  int nFree = nodes.size() - nFixed;
333 
334  long long t0, t1, t2, t3;
335  t0 = utime();
336 
337  if (iter == 0)
338  csp.setupBlockStructure(nFree); // initialize CSparse structures
339  else
340  csp.setupBlockStructure(0); // zero out CSparse structures
341 
342  t1 = utime();
343 
344  VectorXi dcnt(nFree);
345  dcnt.setZero(nFree);
346 
347  // lambda augmentation
348  double lam = 1.0 + sLambda;
349 
350  // loop over P2 constraints
351  for(size_t pi=0; pi<p2cons.size(); pi++)
352  {
353  Con2dP2 &con = p2cons[pi];
354  con.setJacobians(nodes);
355 
356  // add in 4 blocks of A; actually just need upper triangular
357  int i0 = con.ndr-nFixed; // will be negative if fixed
358  int i1 = con.nd1-nFixed; // will be negative if fixed
359 
360  if (i0>=0)
361  {
362  Matrix<double,3,3> m = con.J0t*con.prec*con.J0;
363  csp.addDiagBlock(m,i0);
364  dcnt(con.ndr - nFixed)++;
365  }
366  if (i1>=0)
367  {
368  dcnt(con.nd1 - nFixed)++;
369  Matrix<double,3,3> tp = con.prec * con.J1;
370  Matrix<double,3,3> m = con.J1t * tp;
371  csp.addDiagBlock(m,i1);
372  if (i0>=0)
373  {
374  Matrix<double,3,3> m2 = con.J0t * tp;
375  if (i1 < i0)
376  {
377  m = m2.transpose();
378  csp.addOffdiagBlock(m,i1,i0);
379  }
380  else
381  {
382  csp.addOffdiagBlock(m2,i0,i1);
383  }
384  }
385  }
386 
387  // add in 2 blocks of B
388  if (i0>=0)
389  csp.B.block<3,1>(i0*3,0) -= con.J0t * con.prec * con.err;
390  if (i1>=0)
391  csp.B.block<3,1>(i1*3,0) -= con.J1t * con.prec * con.err;
392  } // finish P2 constraints
393 
394  t2 = utime();
395 
396  // set up sparse matrix structure from blocks
397  if (sparseType == SBA_BLOCK_JACOBIAN_PCG)
398  csp.incDiagBlocks(lam); // increment diagonal block
399  else
400  csp.setupCSstructure(lam,iter==0);
401  t3 = utime();
402 
403  if (verbose)
404  printf("\n[SetupSparseSys] Block: %0.1f Cons: %0.1f CS: %0.1f\n",
405  (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
406 
407  int ndc = 0;
408  for (int i=0; i<nFree; i++)
409  if (dcnt(i) == 0) ndc++;
410 
411  if (ndc > 0)
412  cout << "[SetupSparseSys] " << ndc << " disconnected nodes" << endl;
413  }
414 
415 
424 
425  int SysSPA2d::doSPA(int niter, double sLambda, int useCSparse, double initTol,
426  int maxCGiters)
427  {
428  // number of nodes
429  int ncams = nodes.size();
430 
431  // set number of constraints
432  int ncons = p2cons.size();
433 
434  // check for fixed frames
435  if (nFixed <= 0)
436  {
437  cout << "[doSPA2d] No fixed frames" << endl;
438  return 0;
439  }
440  for (int i=0; i<ncams; i++)
441  {
442  Node2d &nd = nodes[i];
443  if (i >= nFixed)
444  nd.isFixed = false;
445  else
446  nd.isFixed = true;
447  nd.setTransform(); // set up world-to-node transform for cost calculation
448  nd.setDr(); // always use local angles
449  }
450 
451  // initialize vars
452  if (sLambda > 0.0) // do we initialize lambda?
453  lambda = sLambda;
454 
455  double laminc = 2.0; // how much to increment lambda if we fail
456  double lamdec = 0.5; // how much to decrement lambda if we succeed
457  int iter = 0; // iterations
458  sqMinDelta = 1e-8 * 1e-8;
459  double cost = calcCost();
460  if (verbose)
461  cout << iter << " Initial squared cost: " << cost << " which is "
462  << sqrt(cost/ncons) << " rms error" << endl;
463 
464  int good_iter = 0;
465  double cumTime = 0;
466  for (; iter<niter; iter++) // loop at most <niter> times
467  {
468  // set up and solve linear system
469  // NOTE: shouldn't need to redo all calcs in setupSys if we
470  // got here from a bad update
471 
472  long long t0, t1, t2, t3;
473  t0 = utime();
474  if (useCSparse)
475  setupSparseSys(lambda,iter,useCSparse); // set up sparse linear system
476  else
477  setupSys(lambda); // set up linear system
478 
479  // cout << "[SPA] Solving...";
480  t1 = utime();
481 
482  // use appropriate linear solver
483  if (useCSparse == SBA_BLOCK_JACOBIAN_PCG)
484  {
485  if (csp.B.rows() != 0)
486  {
487  int iters = csp.doBPCG(maxCGiters,initTol,iter);
488  if (verbose)
489  cout << "[Block PCG] " << iters << " iterations" << endl;
490  }
491  }
492 #ifdef SBA_DSIF
493  // PCG with incomplete Cholesky
494  else if (useCSparse == 3)
495  {
496  int res = csp.doPCG(maxCGiters);
497  if (res > 1)
498  cout << "[DoSPA] Sparse PCG failed with error " << res << endl;
499  }
500 #endif
501  else if (useCSparse > 0)
502  {
503  if (csp.B.rows() != 0)
504  {
505  bool ok = csp.doChol();
506  if (!ok)
507  cout << "[DoSBA] Sparse Cholesky failed!" << endl;
508  }
509  }
510 
511  // Dense direct Cholesky
512  else
513  A.ldlt().solveInPlace(B); // Cholesky decomposition and solution
514 
515  t2 = utime();
516  // cout << "solved" << endl;
517 
518  // get correct result vector
519  VectorXd &BB = useCSparse ? csp.B : B;
520 
521  // check for convergence
522  // this is a pretty crummy convergence measure...
523  double sqDiff = BB.squaredNorm();
524  if (sqDiff < sqMinDelta) // converged, done...
525  {
526  if (verbose)
527  cout << "Converged with delta: " << sqrt(sqDiff) << endl;
528  break;
529  }
530 
531  // update the frames
532  int ci = 0;
533  for(int i=0; i < ncams; i++)
534  {
535  Node2d &nd = nodes[i];
536  if (nd.isFixed) continue; // not to be updated
537  nd.oldtrans = nd.trans; // save in case we don't improve the cost
538  nd.oldarot = nd.arot;
539  nd.trans.head<2>() += BB.segment<2>(ci);
540 
541  nd.arot += BB(ci+2);
542  nd.normArot();
543  nd.setTransform(); // set up projection matrix for cost calculation
544  nd.setDr(); // set rotational derivatives
545  ci += 3; // advance B index
546  }
547 
548  // new cost
549  double newcost = calcCost();
550  if (verbose)
551  cout << iter << " Updated squared cost: " << newcost << " which is "
552  << sqrt(newcost/ncons) << " rms error" << endl;
553 
554  // check if we did good
555  if (newcost < cost) // && iter != 0) // NOTE: iter==0 case is for checking
556  {
557  cost = newcost;
558  lambda *= lamdec; // decrease lambda
559  // laminc = 2.0; // reset bad lambda factor; not sure if this is a good idea...
560  good_iter++;
561  }
562  else
563  {
564  lambda *= laminc; // increase lambda
565  laminc *= 2.0; // increase the increment
566 
567  // reset nodes
568  for(int i=0; i<ncams; i++)
569  {
570  Node2d &nd = nodes[i];
571  if (nd.isFixed) continue; // not to be updated
572  nd.trans = nd.oldtrans;
573  nd.arot = nd.oldarot;
574  nd.setTransform(); // set up projection matrix for cost calculation
575  nd.setDr();
576  }
577 
578  cost = calcCost(); // need to reset errors
579  if (verbose)
580  cout << iter << " Downdated cost: " << cost << endl;
581  // NOTE: shouldn't need to redo all calcs in setupSys
582  }
583 
584  t3 = utime();
585  if (iter == 0 && verbose)
586  {
587  printf("[SPA] Setup: %0.2f ms Solve: %0.2f ms Update: %0.2f ms\n",
588  0.001*(double)(t1-t0),
589  0.001*(double)(t2-t1),
590  0.001*(double)(t3-t2));
591  }
592 
593  double dt=1e-6*(double)(t3-t0);
594  cumTime+=dt;
595  if (print_iros_stats){
596  cerr << "iteration= " << iter
597  << "\t chi2= " << cost
598  << "\t time= " << dt
599  << "\t cumTime= " << cumTime
600  << "\t kurtChi2= " << cost
601  << endl;
602  }
603 
604  }
605 
606  // return number of iterations performed
607  return good_iter;
608 
609  }
610 
611 
618 
619  static inline int getind(std::map<int,int> &m, int ind)
620  {
621  std::map<int,int>::iterator it;
622  it = m.find(ind);
623  if (it == m.end())
624  return -1;
625  else
626  return it->second;
627  }
628 
629  int SysSPA2d::doSPAwindowed(int window, int niter, double sLambda, int useCSparse)
630  {
631  // number of nodes
632  int nnodes = nodes.size();
633  if (nnodes < 2) return 0;
634 
635  int nlow = nnodes - window;
636  if (nlow < 1) nlow = 1; // always have one fixed node
637 
638  if (verbose)
639  cout << "[SPA Window] From " << nlow << " to " << nnodes << endl;
640 
641  // number of constraints
642  int ncons = p2cons.size();
643 
644  // set up SPA
645  SysSPA2d spa;
646  spa.verbose = verbose;
647 
648  // node, constraint vectors and index mapping
649  std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &wnodes = spa.nodes;
650  std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> > &wp2cons = spa.p2cons;
651  std::map<int,int> inds;
652  std::vector<int> rinds; // reverse indices
653 
654  // loop through all constraints and set up fixed nodes and constraints
655  for (int i=0; i<ncons; i++)
656  {
657  Con2dP2 &con = p2cons[i];
658  if (con.ndr >= nlow || con.nd1 >= nlow)
659  wp2cons.push_back(con);
660 
661  if (con.ndr >= nlow && con.nd1 < nlow) // have a winner
662  {
663  int j = getind(inds,con.nd1); // corresponding index
664  if (j < 0) // not present, add it in
665  {
666  inds.insert(std::pair<int,int>(con.nd1,wnodes.size()));
667  wnodes.push_back(nodes[con.nd1]);
668  }
669  rinds.push_back(con.nd1);
670  }
671  else if (con.nd1 >= nlow && con.ndr < nlow)
672  {
673  int j = getind(inds,con.ndr); // corresponding index
674  if (j < 0) // not present, add it in
675  {
676  inds.insert(std::pair<int,int>(con.ndr,wnodes.size()));
677  wnodes.push_back(nodes[con.ndr]);
678  }
679  rinds.push_back(con.ndr);
680  }
681  }
682 
683  spa.nFixed = wnodes.size();
684  if (verbose)
685  cout << "[SPA Window] Fixed node count: " << spa.nFixed << endl;
686 
687  // add in variable nodes
688  for (int i=0; i<(int)wp2cons.size(); i++)
689  {
690  Con2dP2 &con = wp2cons[i];
691  if (con.nd1 >= nlow && con.ndr >= nlow) // have a winner
692  {
693  int n0 = getind(inds,con.ndr);
694  if (n0 < 0)
695  {
696  inds.insert(std::pair<int,int>(con.ndr,wnodes.size()));
697  wnodes.push_back(nodes[con.ndr]);
698  rinds.push_back(con.ndr);
699  }
700  int n1 = getind(inds,con.nd1);
701  if (n1 < 0)
702  {
703  inds.insert(std::pair<int,int>(con.nd1,wnodes.size()));
704  wnodes.push_back(nodes[con.nd1]);
705  rinds.push_back(con.nd1);
706  }
707  }
708  }
709 
710  if (verbose)
711  {
712  cout << "[SPA Window] Variable node count: " << spa.nodes.size() - spa.nFixed << endl;
713  cout << "[SPA Window] Constraint count: " << spa.p2cons.size() << endl;
714  }
715 
716  // new constraint indices
717  for (int i=0; i<(int)wp2cons.size(); i++)
718  {
719  Con2dP2 &con = wp2cons[i];
720  con.ndr = getind(inds,con.ndr);
721  con.nd1 = getind(inds,con.nd1);
722  }
723 
724  // run spa
725  niter = spa.doSPA(niter,sLambda,useCSparse);
726 
727  // reset constraint indices
728  for (int i=0; i<(int)wp2cons.size(); i++)
729  {
730  Con2dP2 &con = wp2cons[i];
731  con.ndr = rinds[con.ndr];
732  con.nd1 = rinds[con.nd1];
733  }
734  return niter;
735  }
736 
737 
738 #ifdef SBA_DSIF
739 
743  // Set up sparse linear system for Delayed Sparse Information Filter
744  void SysSPA2d::setupSparseDSIF(int newnode)
745  {
746  // set matrix sizes and clear
747  // assumes scales vars are all free
748  int nFree = nodes.size() - nFixed;
749 
750  // long long t0, t1, t2, t3;
751  // t0 = utime();
752 
753  // don't erase old stuff here, the delayed filter just grows in size
754  csp.setupBlockStructure(nFree,false); // initialize CSparse structures
755 
756  // t1 = utime();
757 
758  // loop over P2 constraints
759  for(size_t pi=0; pi<p2cons.size(); pi++)
760  {
761  Con2dP2 &con = p2cons[pi];
762 
763  // don't consider old constraints
764  if (con.ndr < newnode && con.nd1 < newnode)
765  continue;
766 
767  con.setJacobians(nodes);
768 
769  // add in 4 blocks of A; actually just need upper triangular
770  int i0 = con.ndr-nFixed; // will be negative if fixed
771  int i1 = con.nd1-nFixed; // will be negative if fixed
772 
773  // DSIF will not diverge on standard datasets unless
774  // we reduce the precision of the constraints
775  double fact = 1.0;
776  if (i0 != i1-1) fact = 0.99; // what should we use????
777 
778  if (i0>=0)
779  {
780  Matrix<double,3,3> m = con.J0t*con.prec*con.J0;
781  csp.addDiagBlock(m,i0);
782  }
783  if (i1>=0)
784  {
785  Matrix<double,3,3> m = con.J1t*con.prec*con.J1;
786  csp.addDiagBlock(m,i1);
787 
788  if (i0>=0)
789  {
790  Matrix<double,3,3> m2 = con.J0t*con.prec*con.J1;
791  m2 = m2 * fact * fact;
792  if (i1 < i0)
793  {
794  m = m2.transpose();
795  csp.addOffdiagBlock(m,i1,i0);
796  }
797  else
798  {
799  csp.addOffdiagBlock(m2,i0,i1);
800  }
801  }
802  }
803 
804  // add in 2 blocks of B
805  if (i0>=0)
806  csp.B.block<3,1>(i0*3,0) -= con.J0t * con.prec * con.err;
807  if (i1>=0)
808  csp.B.block<3,1>(i1*3,0) -= con.J1t * con.prec * con.err;
809 
810  } // finish P2 constraints
811 
812  // t2 = utime();
813 
814  csp.Bprev = csp.B; // save for next iteration
815 
816  // set up sparse matrix structure from blocks
817  csp.setupCSstructure(1.0,true);
818 
819  // t3 = utime();
820 
821  // printf("\n[SetupSparseSys] Block: %0.1f Cons: %0.1f CS: %0.1f\n",
822  // (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
823 
824  }
825 
826 
829  void SysSPA2d::doDSIF(int newnode)
830  {
831  // number of nodes
832  int nnodes = nodes.size();
833 
834  // set number of constraints
835  int ncons = p2cons.size();
836 
837  // check for fixed frames
838  if (nFixed <= 0)
839  {
840  cout << "[doDSIF] No fixed frames" << endl;
841  return;
842  }
843 
844  // check for newnode being ok
845  if (newnode >= nnodes)
846  {
847  cout << "[doDSIF] no new nodes to add" << endl;
848  return;
849  }
850  else // set up saved mean value of pose
851  {
852  for (int i=newnode; i<nnodes; i++)
853  {
854  nodes[i].oldtrans = nodes[i].trans;
855  nodes[i].oldarot = nodes[i].arot;
856  }
857  }
858 
859  for (int i=0; i<nnodes; i++)
860  {
861  Node2d &nd = nodes[i];
862  if (i >= nFixed)
863  nd.isFixed = false;
864  else
865  nd.isFixed = true;
866  nd.setTransform(); // set up world-to-node transform for cost calculation
867  nd.setDr(); // always use local angles
868  }
869 
870  // initialize vars
871  double cost = calcCost();
872  if (verbose)
873  cout << " Initial squared cost: " << cost << " which is "
874  << sqrt(cost/ncons) << " rms error" << endl;
875 
876  // set up and solve linear system
877  long long t0, t1, t2, t3;
878  t0 = utime();
879  setupSparseDSIF(newnode); // set up sparse linear system
880 
881 #if 0
882  cout << "[doDSIF] B = " << csp.B.transpose() << endl;
883  csp.uncompress(A);
884  cout << "[doDSIF] A = " << endl << A << endl;
885 #endif
886 
887  // cout << "[SPA] Solving...";
888  t1 = utime();
889  bool ok = csp.doChol();
890  if (!ok)
891  cout << "[doDSIF] Sparse Cholesky failed!" << endl;
892  t2 = utime();
893  // cout << "solved" << endl;
894 
895  // get correct result vector
896  VectorXd &BB = csp.B;
897 
898  // cout << "[doDSIF] RES = " << BB.transpose() << endl;
899 
900  // update the frames
901  int ci = 0;
902  for(int i=0; i < nnodes; i++)
903  {
904  Node2d &nd = nodes[i];
905  if (nd.isFixed) continue; // not to be updated
906  nd.trans.head(2) = nd.oldtrans.head(2)+BB.segment<2>(ci);
907  nd.arot = nd.oldarot+BB(ci+2);
908  nd.normArot();
909  nd.setTransform(); // set up projection matrix for cost calculation
910  nd.setDr(); // set rotational derivatives
911  ci += 3; // advance B index
912  }
913 
914  // new cost
915  double newcost = calcCost();
916  if (verbose)
917  cout << " Updated squared cost: " << newcost << " which is "
918  << sqrt(newcost/ncons) << " rms error" << endl;
919 
920  t3 = utime();
921  }
922 
923 
924 
925  // write out the precision matrix for CSparse
926  void SysSPA2d::writeSparseA(char *fname, bool useCSparse)
927  {
928  ofstream ofs(fname);
929  if (ofs == NULL)
930  {
931  cout << "Can't open file " << fname << endl;
932  return;
933  }
934 
935  // cameras
936  if (useCSparse)
937  {
938  setupSparseSys(0.0,0);
939 
940  int *Ai = csp.A->i;
941  int *Ap = csp.A->p;
942  double *Ax = csp.A->x;
943 
944  for (int i=0; i<csp.csize; i++)
945  for (int j=Ap[i]; j<Ap[i+1]; j++)
946  if (Ai[j] <= i)
947  ofs << Ai[j] << " " << i << setprecision(16) << " " << Ax[j] << endl;
948  }
949  else
950  {
951  Eigen::IOFormat pfmt(16);
952 
953  int nrows = A.rows();
954  int ncols = A.cols();
955 
956  for (int i=0; i<nrows; i++)
957  for (int j=i; j<ncols; j++)
958  {
959  double a = A(i,j);
960  if (A(i,j) != 0.0)
961  ofs << i << " " << j << setprecision(16) << " " << a << endl;
962  }
963  }
964 
965  ofs.close();
966  }
967 #endif
968 
969 
970  // return vector of connections
971  // 4 entries for each connection: x,y,x',y'
972  void SysSPA2d::getGraph(std::vector<float> &graph)
973  {
974  for (int i=0; i<(int)p2cons.size(); i++)
975  {
976  Con2dP2 &con = p2cons[i];
977  Node2d &nd0 = nodes[con.ndr];
978  Node2d &nd1 = nodes[con.nd1];
979  graph.push_back(nd0.trans(0));
980  graph.push_back(nd0.trans(1));
981  graph.push_back(nd1.trans(0));
982  graph.push_back(nd1.trans(1));
983  }
984  }
985 
986 
987 } // namespace vo
988 
989 
990 
993 
995 
996 #if 0
997 
998 // this was a failed attempt at Ryan's augmentation of the info matrix
999 
1000 
1001  // Set up sparse linear system for Delayed Sparse Information Filter
1002  void SysSPA2d::setupSparseDSIF(int newnode)
1003  {
1004  cout << "[SetupDSIF] at " << newnode << endl;
1005 
1006  // set matrix sizes and clear
1007  // assumes scales vars are all free
1008  int nFree = nodes.size() - nFixed;
1009 
1010  // long long t0, t1, t2, t3;
1011  // t0 = utime();
1012 
1013  // don't erase old stuff here, the delayed filter just grows in size
1014  csp.setupBlockStructure(nFree,false); // initialize CSparse structures
1015 
1016  // t1 = utime();
1017 
1018  // loop over P2 constraints
1019  for(size_t pi=0; pi<p2cons.size(); pi++)
1020  {
1021  Con2dP2 &con = p2cons[pi];
1022 
1023  // don't consider old constraints
1024  if (con.ndr < newnode && con.nd1 < newnode)
1025  continue;
1026 
1027  con.setJacobians(nodes);
1028  Matrix3d J;
1029  J.setIdentity();
1030  Vector2d dRdth = nodes[con.ndr].dRdx.transpose() * con.tmean;
1031  J(0,2) = dRdth(0);
1032  J(1,2) = dRdth(1);
1033  Matrix3d Jt = J.transpose();
1034  Vector3d u;
1035  u.head(2) = nodes[con.ndr].trans.head(2);
1036  u(2) = nodes[con.ndr].arot;
1037  Vector3d f;
1038  f.head(2) = u.head(2) + nodes[con.ndr].w2n.transpose().block<2,2>(0,0) * con.tmean;
1039  f(2) = u(2) + con.amean;
1040  if (f(2) > M_PI) f(2) -= 2.0*M_PI;
1041  if (f(2) < M_PI) f(2) += 2.0*M_PI;
1042 
1043 
1044  cout << "[SetupDSIF] u = " << u.transpose() << endl;
1045  cout << "[SetupDSIF] f = " << f.transpose() << endl;
1046  cout << "[SetupDSIF] fo = " << nodes[con.nd1].trans.transpose() << endl;
1047 
1048 
1049  // add in 4 blocks of A; actually just need upper triangular
1050  int i0 = con.ndr-nFixed; // will be negative if fixed
1051  int i1 = con.nd1-nFixed; // will be negative if fixed
1052 
1053  if (i0 != i1-1) continue; // just sequential nodes for now
1054 
1055  if (i0>=0)
1056  {
1057  Matrix<double,3,3> m = Jt*con.prec*J;
1058  csp.addDiagBlock(m,i0);
1059  }
1060  if (i1>=0)
1061  {
1062  Matrix<double,3,3> m = con.prec;
1063  csp.addDiagBlock(m,i1);
1064 
1065  if (i0>=0)
1066  {
1067  Matrix<double,3,3> m2 = -con.prec * J;
1068 
1069 
1070  if (i1 < i0)
1071  {
1072  m = m2.transpose();
1073  csp.addOffdiagBlock(m,i1,i0);
1074  }
1075  else
1076  {
1077  csp.addOffdiagBlock(m2,i0,i1);
1078  }
1079  }
1080  }
1081 
1082  // add in 2 blocks of B
1083  // Jt Gamma (J u - e)
1084  Vector3d Juf = J*u - f;
1085  if (Juf(2) > M_PI) Juf(2) -= 2.0*M_PI;
1086  if (Juf(2) < M_PI) Juf(2) += 2.0*M_PI;
1087  if (i0>=0)
1088  {
1089  csp.B.block<3,1>(i0*3,0) += Jt * con.prec * Juf;
1090  }
1091  if (i1>=0)
1092  {
1093  csp.B.block<3,1>(i1*3,0) -= con.prec * Juf;
1094  }
1095 
1096  } // finish P2 constraints
1097 
1098  // t2 = utime();
1099 
1100  csp.Bprev = csp.B; // save for next iteration
1101 
1102  // set up sparse matrix structure from blocks
1103  csp.setupCSstructure(1.0,true);
1104 
1105  // t3 = utime();
1106 
1107  // printf("\n[SetupSparseSys] Block: %0.1f Cons: %0.1f CS: %0.1f\n",
1108  // (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
1109 
1110  }
1111 
1112 
1116  void SysSPA2d::doDSIF(int newnode, bool useCSparse)
1117  {
1118  // number of nodes
1119  int nnodes = nodes.size();
1120 
1121  // set number of constraints
1122  int ncons = p2cons.size();
1123 
1124  // check for fixed frames
1125  if (nFixed <= 0)
1126  {
1127  cout << "[doDSIF] No fixed frames" << endl;
1128  return;
1129  }
1130 
1131  // check for newnode being ok
1132  if (newnode >= nnodes)
1133  {
1134  cout << "[doDSIF] no new nodes to add" << endl;
1135  return;
1136  }
1137 
1138  nFixed = 0;
1139 
1140  for (int i=0; i<nnodes; i++)
1141  {
1142  Node2d &nd = nodes[i];
1143  if (i >= nFixed)
1144  nd.isFixed = false;
1145  else
1146  nd.isFixed = true;
1147  nd.setTransform(); // set up world-to-node transform for cost calculation
1148  nd.setDr(); // always use local angles
1149  }
1150 
1151  // initialize vars
1152  double cost = calcCost();
1153  if (verbose)
1154  cout << " Initial squared cost: " << cost << " which is "
1155  << sqrt(cost/ncons) << " rms error" << endl;
1156 
1157  if (newnode == 1)
1158  {
1159  // set up first system with node 0
1160  csp.setupBlockStructure(1,false); // initialize CSparse structures
1161  Matrix3d m;
1162  m.setIdentity();
1163  m = m*1000000;
1164  csp.addDiagBlock(m,0);
1165  Vector3d u;
1166  u.head(2) = nodes[0].trans.head(2);
1167  u(2) = nodes[0].arot;
1168  csp.B.head(3) = u*1000000;
1169  csp.Bprev = csp.B; // save for next iteration
1170  cout << "[doDSIF] B = " << csp.B.transpose() << endl;
1171  }
1172 
1173  // set up and solve linear system
1174  // NOTE: shouldn't need to redo all calcs in setupSys if we
1175  // got here from a bad update
1176 
1177  long long t0, t1, t2, t3;
1178  t0 = utime();
1179  if (useCSparse)
1180  setupSparseDSIF(newnode); // set up sparse linear system
1181  else
1182  {}
1183 
1184 #if 1
1185  cout << "[doDSIF] B = " << csp.B.transpose() << endl;
1186  csp.uncompress(A);
1187  cout << "[doDSIF] A = " << endl << A << endl;
1188 #endif
1189 
1190  // cout << "[SPA] Solving...";
1191  t1 = utime();
1192  if (useCSparse)
1193  {
1194  bool ok = csp.doChol();
1195  if (!ok)
1196  cout << "[doDSIF] Sparse Cholesky failed!" << endl;
1197  }
1198  else
1199  A.ldlt().solveInPlace(B); // Cholesky decomposition and solution
1200  t2 = utime();
1201  // cout << "solved" << endl;
1202 
1203  // get correct result vector
1204  VectorXd &BB = useCSparse ? csp.B : B;
1205 
1206  cout << "[doDSIF] RES = " << BB.transpose() << endl;
1207 
1208  // update the frames
1209  int ci = 0;
1210  for(int i=0; i < nnodes; i++)
1211  {
1212  Node2d &nd = nodes[i];
1213  if (nd.isFixed) continue; // not to be updated
1214  nd.trans.head<2>() = BB.segment<2>(ci);
1215  nd.arot = BB(ci+2);
1216  nd.normArot();
1217  nd.setTransform(); // set up projection matrix for cost calculation
1218  nd.setDr(); // set rotational derivatives
1219  ci += 3; // advance B index
1220  }
1221 
1222  // new cost
1223  double newcost = calcCost();
1224  if (verbose)
1225  cout << " Updated squared cost: " << newcost << " which is "
1226  << sqrt(newcost/ncons) << " rms error" << endl;
1227 
1228  t3 = utime();
1229  }
1230 
1231 
1235 
1236  // Set up sparse linear system for Delayed Sparse Information Filter
1237  void SysSPA2d::setupSparseDSIF(int newnode)
1238  {
1239  cout << "[SetupDSIF] at " << newnode << endl;
1240 
1241  // set matrix sizes and clear
1242  // assumes scales vars are all free
1243  int nFree = nodes.size() - nFixed;
1244 
1245  // long long t0, t1, t2, t3;
1246  // t0 = utime();
1247 
1248  // don't erase old stuff here, the delayed filter just grows in size
1249  csp.setupBlockStructure(nFree,false); // initialize CSparse structures
1250 
1251  // t1 = utime();
1252 
1253  // loop over P2 constraints
1254  for(size_t pi=0; pi<p2cons.size(); pi++)
1255  {
1256  Con2dP2 &con = p2cons[pi];
1257 
1258  // don't consider old constraints
1259  if (con.ndr < newnode && con.nd1 < newnode)
1260  continue;
1261 
1262  con.setJacobians(nodes);
1263 
1264  // add in 4 blocks of A; actually just need upper triangular
1265  int i0 = con.ndr-nFixed; // will be negative if fixed
1266  int i1 = con.nd1-nFixed; // will be negative if fixed
1267 
1268  if (i0 != i1-1) continue; // just sequential nodes for now
1269 
1270  if (i0>=0)
1271  {
1272  Matrix<double,3,3> m = con.J0t*con.prec*con.J0;
1273  csp.addDiagBlock(m,i0);
1274  }
1275  if (i1>=0)
1276  {
1277  Matrix<double,3,3> m = con.J1t*con.prec*con.J1;
1278  csp.addDiagBlock(m,i1);
1279 
1280  if (i0>=0)
1281  {
1282  Matrix<double,3,3> m2 = con.J0t*con.prec*con.J1;
1283  if (i1 < i0)
1284  {
1285  m = m2.transpose();
1286  csp.addOffdiagBlock(m,i1,i0);
1287  }
1288  else
1289  {
1290  csp.addOffdiagBlock(m2,i0,i1);
1291  }
1292  }
1293  }
1294 
1295  // add in 2 blocks of B
1296  // (J u + e)^T G J
1297 
1298  if (i0>=0)
1299  {
1300  Vector3d u;
1301  u.head(2) = nodes[con.ndr].trans.head(2);
1302  u(2) = nodes[con.ndr].arot;
1303  Vector3d bm = con.err + con.J0 * u;
1304  csp.B.block<3,1>(i0*3,0) += (bm.transpose() * con.prec * con.J0).transpose();
1305  }
1306  if (i1>=0)
1307  {
1308  Vector3d u;
1309  u.head(2) = nodes[con.nd1].trans.head(2);
1310  u(2) = nodes[con.nd1].arot;
1311  Vector3d bm = con.err + con.J1 * u;
1312  csp.B.block<3,1>(i1*3,0) += (bm.transpose() * con.prec * con.J1).transpose();
1313  }
1314 
1315  } // finish P2 constraints
1316 
1317  // t2 = utime();
1318 
1319  csp.Bprev = csp.B; // save for next iteration
1320 
1321  // set up sparse matrix structure from blocks
1322  csp.setupCSstructure(1.0,true);
1323 
1324  // t3 = utime();
1325 
1326  // printf("\n[SetupSparseSys] Block: %0.1f Cons: %0.1f CS: %0.1f\n",
1327  // (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
1328 
1329  }
1330 
1331 
1335  void SysSPA2d::doDSIF(int newnode, bool useCSparse)
1336  {
1337  // number of nodes
1338  int nnodes = nodes.size();
1339 
1340  // set number of constraints
1341  int ncons = p2cons.size();
1342 
1343  // check for fixed frames
1344  if (nFixed <= 0)
1345  {
1346  cout << "[doDSIF] No fixed frames" << endl;
1347  return;
1348  }
1349 
1350  // check for newnode being ok
1351  if (newnode >= nnodes)
1352  {
1353  cout << "[doDSIF] no new nodes to add" << endl;
1354  return;
1355  }
1356 
1357  for (int i=0; i<nnodes; i++)
1358  {
1359  Node2d &nd = nodes[i];
1360  if (i >= nFixed)
1361  nd.isFixed = false;
1362  else
1363  nd.isFixed = true;
1364  nd.setTransform(); // set up world-to-node transform for cost calculation
1365  nd.setDr(); // always use local angles
1366  }
1367 
1368  // initialize vars
1369  double cost = calcCost();
1370  if (verbose)
1371  cout << " Initial squared cost: " << cost << " which is "
1372  << sqrt(cost/ncons) << " rms error" << endl;
1373 
1374  // set up and solve linear system
1375  // NOTE: shouldn't need to redo all calcs in setupSys if we
1376  // got here from a bad update
1377 
1378  long long t0, t1, t2, t3;
1379  t0 = utime();
1380  if (useCSparse)
1381  setupSparseDSIF(newnode); // set up sparse linear system
1382  else
1383  {}
1384 
1385 #if 1
1386  cout << "[doDSIF] B = " << csp.B.transpose() << endl;
1387  csp.uncompress(A);
1388  cout << "[doDSIF] A = " << endl << A << endl;
1389 #endif
1390 
1391  // cout << "[SPA] Solving...";
1392  t1 = utime();
1393  if (useCSparse)
1394  {
1395  bool ok = csp.doChol();
1396  if (!ok)
1397  cout << "[doDSIF] Sparse Cholesky failed!" << endl;
1398  }
1399  else
1400  A.ldlt().solveInPlace(B); // Cholesky decomposition and solution
1401  t2 = utime();
1402  // cout << "solved" << endl;
1403 
1404  // get correct result vector
1405  VectorXd &BB = useCSparse ? csp.B : B;
1406 
1407  cout << "[doDSIF] RES = " << BB.transpose() << endl;
1408 
1409  // update the frames
1410  int ci = 0;
1411  for(int i=0; i < nnodes; i++)
1412  {
1413  Node2d &nd = nodes[i];
1414  if (nd.isFixed) continue; // not to be updated
1415  nd.trans.head<2>() = BB.segment<2>(ci);
1416  nd.arot = BB(ci+2);
1417  nd.normArot();
1418  nd.setTransform(); // set up projection matrix for cost calculation
1419  nd.setDr(); // set rotational derivatives
1420  ci += 3; // advance B index
1421  }
1422 
1423  // new cost
1424  double newcost = calcCost();
1425  if (verbose)
1426  cout << " Updated squared cost: " << newcost << " which is "
1427  << sqrt(newcost/ncons) << " rms error" << endl;
1428 
1429  t3 = utime();
1430 
1431  } // namespace sba
1432 
1433 
1436  void SysSPA2d::removeNode(int id)
1437  {
1438  int ind = -1;
1439  for (int i=0; i<(int)nodes.size(); i++)
1440  {
1441  if (nodes[i].nodeId == ind)
1442  ind = i;
1443  }
1444  if (ind < 0) return;
1445 
1446  // remove node
1447  nodes.erase(nodes.begin() + ind);
1448 
1449  // remove all constraints referring to node
1450  // and adjust indices of all nodes with indices
1451  // greater than 'ind'
1452  int i=0;
1453  while (i <(int)p2cons.size())
1454  {
1455  std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> >::iterator iter = p2cons.begin() + i;
1456  if (iter->ndr == ind || iter->nd1 == ind)
1457  {
1458  p2cons.erase(iter);
1459  }
1460  else
1461  {
1462  if (iter->ndr > ind) iter->ndr--;
1463  if (iter->nd1 > ind) iter->nd1--;
1464  i++;
1465  }
1466  }
1467  }
1468 
1470  // <nd0>, <nd1> are node id's
1471  bool SysSPA2d::removeConstraint(int ndi0, int ndi1)
1472  {
1473  int ni0 = -1, ni1 = -1;
1474  for (int i=0; i<(int)nodes.size(); i++)
1475  {
1476  if (nodes[i].nodeId == ndi0)
1477  ni0 = i;
1478  if (nodes[i].nodeId == ndi1)
1479  ni1 = i;
1480  }
1481  if (ni0 < 0 || ni1 < 0) return false;
1482 
1483  int i=0;
1484  while (i <(int)p2cons.size())
1485  {
1486  std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> >::iterator iter = p2cons.begin() + i;
1487  if (iter->ndr == ni0 && iter->nd1 == ni1)
1488  {
1489  p2cons.erase(iter);
1490  }
1491  else
1492  {
1493  i++;
1494  }
1495  }
1496 
1497  return true;
1498  }
1499 
1500 
1501 
1502 #endif
1503 
1504 
sba::SysSPA2d::doSPA
int doSPA(int niter, double sLambda=1.0e-4, int useCSparse=SBA_SPARSE_CHOLESKY, double initTol=1.0e-8, int CGiters=50)
Definition: spa2d.cpp:425
sba::getind
static int getind(std::map< int, int > &m, int ind)
Definition: spa2d.cpp:619
sba::Con2dP2
Definition: spa2d.h:141
Eigen
Definition: sba_file_io.h:18
sba::Con2dP2::tmean
Eigen::Vector2d tmean
Mean vector, quaternion (inverse) and precision matrix for this constraint.
Definition: spa2d.h:153
sba::Node2d::arot
double arot
Definition: spa2d.h:104
sba::SysSPA2d::verbose
bool verbose
if we want statistics
Definition: spa2d.h:274
sba::Node2d::w2n
Eigen::Matrix< double, 2, 3 > w2n
Resultant transform from world to node coordinates;.
Definition: spa2d.h:113
sba::Con2dP2::J1
Eigen::Matrix< double, 3, 3 > J1
Definition: spa2d.h:167
sba::Con2dP2::amean
double amean
Definition: spa2d.h:154
sba::Node2d::dRdx
Eigen::Matrix2d dRdx
Definition: spa2d.h:123
sba::Node2d::setTransform
void setTransform()
Definition: spa2d.cpp:63
sba::Node2d::isFixed
bool isFixed
For SPA, is this camera fixed or free?
Definition: spa2d.h:128
sba::writeSparseA
void writeSparseA(const char *fname, SysSBA &sba)
Definition: sba_file_io.cpp:418
ok
ROSCPP_DECL bool ok()
sba::Con2dP2::setJacobians
void setJacobians(std::vector< Node2d, Eigen::aligned_allocator< Node2d > > &nodes)
Definition: spa2d.cpp:86
f
f
sba::Con2dP2::J0t
Eigen::Matrix< double, 3, 3 > J0t
Definition: spa2d.h:167
sba::SysSPA2d::nFixed
int nFixed
Number of fixed nodes.
Definition: spa2d.h:222
sba::Con2dP2::err
Eigen::Matrix< double, 3, 1 > err
error
Definition: spa2d.h:158
sba::SysSPA2d::p2cons
std::vector< Con2dP2, Eigen::aligned_allocator< Con2dP2 > > p2cons
Set of P2 constraints.
Definition: spa2d.h:225
sba::Con2dP2::prec
Eigen::Matrix< double, 3, 3 > prec
Definition: spa2d.h:155
sba
Definition: bpcg.h:60
utime
static long long utime()
Definition: spa2d.cpp:52
sba::Node2d::setDr
void setDr()
Definition: spa2d.cpp:76
sba::Con2dP2::J1t
Eigen::Matrix< double, 3, 3 > J1t
Definition: spa2d.h:167
SBA_BLOCK_JACOBIAN_PCG
#define SBA_BLOCK_JACOBIAN_PCG
Definition: sba.h:69
sba::Node2d
Definition: spa2d.h:94
sba::Con2dP2::nd1
int nd1
Node2d index for the second node.
Definition: spa2d.h:150
sba::Con2dP2::calcErr
double calcErr(const Node2d &nd0, const Node2d &nd1)
calculates projection error and stores it in <err>
Definition: spa2d.cpp:148
sba::SysSPA2d
SysSPA2d holds a set of nodes and constraints for sparse pose adjustment.
Definition: spa2d.h:192
std
sba::Node2d::trans
Eigen::Matrix< double, 3, 1 > trans
6DOF pose as a unit quaternion and translation vector
Definition: spa2d.h:103
sba::Con2dP2::J0
Eigen::Matrix< double, 3, 3 > J0
jacobian with respect to frames; uses dR'/dq from Node2d calculation
Definition: spa2d.h:167
sba::Node2d::oldarot
double oldarot
Definition: spa2d.h:133
sba::SysSPA2d::nodes
std::vector< Node2d, Eigen::aligned_allocator< Node2d > > nodes
set of nodes (camera frames) for SPA system, indexed by position;
Definition: spa2d.h:214
sba::Node2d::normArot
void normArot()
Normalize to [-pi,+pi].
Definition: spa2d.h:106
sba::Node2d::oldtrans
Eigen::Matrix< double, 3, 1 > oldtrans
Definition: spa2d.h:132
sba::Con2dP2::calcErrDist
double calcErrDist(const Node2d &nd0, const Node2d &nd1)
calculate error in distance only, no weighting
Definition: spa2d.cpp:163
sba::Node2d::nodeId
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int nodeId
node id - somewhat redundant, but can be useful, e.g., in KARTO links
Definition: spa2d.h:100
spa2d.h
sba::Con2dP2::ndr
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int ndr
Reference pose index.
Definition: spa2d.h:147


sparse_bundle_adjustment
Author(s):
autogenerated on Wed Mar 2 2022 01:03:04