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


sparse_bundle_adjustment
Author(s):
autogenerated on Fri Apr 3 2020 03:30:54