spa.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
37 //
38 
39 //
40 // Ok, we're going with 2-pose constraints, and added scale variables.
41 // Format for files:
42 // Format for files:
43 // #nodes #scales #p2p_constraints #scale_constraints
44 // x y z rx ry rz ; first node, vector part of unit quaternion
45 // ...
46 // p1 p2 Lambda_12 ; first p2p constraint
47 // ...
48 // p1 p2 s1 K_12 w ; first scale constraint,
49 // ; (p1-p2)^2 = s1*K_12 with weight w
50 
51 
52 #include <stdio.h>
54 #include <Eigen/Cholesky>
55 #include <chrono>
56 
57 using namespace Eigen;
58 using namespace std;
59 
60 #include <iostream>
61 #include <iomanip>
62 #include <fstream>
63 #include <utility>
64 
65 // elapsed time in microseconds
66 static long long utime()
67 {
68  auto duration = std::chrono::system_clock::now().time_since_epoch();
69  return std::chrono::duration_cast<std::chrono::microseconds>(duration).count();
70 }
71 
72 namespace sba
73 {
74  // reads in a file of pose constraints
75  bool readP2File(char *fname, SysSPA spa)
76  {
77  // Format for files:
78  // #nodes #scales #p2p_constraints #scale_constraints
79  // x y z rx ry rz ; first node, vector part of unit quaternion
80  // ...
81  // p1 p2 Lambda_12 ; first p2p constraint
82  // ...
83  // p1 p2 s1 K_12 w ; first scale constraint,
84  // ; (p1-p2)^2 = s1*K_12 with weight w
85 
86  ifstream ifs(fname);
87  if (!ifs)
88  {
89  cout << "Can't open file " << fname << endl;
90  return false;
91  }
92  ifs.precision(10); // what is this for???
93 
94  // read header
95  string line;
96  if (!getline(ifs,line) || line != "# P2 Constraint File")
97  {
98  cout << "Bad header" << endl;
99  return false;
100  }
101  cout << "Found P2 constraint file" << endl;
102 
103  // read number of cameras, scalers, and p2p constraints
104  int ncams, nss, nscs, np2s;
105  if (!(ifs >> ncams >> nss >> np2s >> nscs))
106  {
107  cout << "Bad entity count" << endl;
108  return false;
109  }
110  cout << "Number of cameras: " << ncams
111  << " Number of scalers: " << nss
112  << " Number of p2p constraints: " << np2s
113  << " Number of scale constraints: " << nscs << endl;
114 
115  cout << "Reading in camera data..." << flush;
116  std::vector<Node,Eigen::aligned_allocator<Node> > &nodes = spa.nodes;
117  Node nd;
118 
119  for (int i=0; i<ncams; i++)
120  {
121  double v1,v2,v3;
122  if (!(ifs >> v1 >> v2 >> v3))
123  {
124  cout << "Bad node translation params at number " << i << endl;
125  return false;
126  }
127  nd.trans = Vector4d(v1,v2,v3,1.0);
128 
129  if (!(ifs >> v1 >> v2 >> v3))
130  {
131  cout << "Bad node rotation quaternion at number " << i << endl;
132  return false;
133  }
134  Matrix3d m;
135  nd.qrot = Quaternion<double>(v1,v2,v2,0.0);
136  nd.normRot(); // normalize to unit vector and compute w
137 
138  nodes.push_back(nd);
139  }
140  cout << "done" << endl;
141 
142  // read in p2p constraints
143  cout << "Reading in constraint data..." << flush;
144  std::vector<ConP2,Eigen::aligned_allocator<ConP2> > &cons = spa.p2cons;
145  ConP2 con;
146 
147  for (int i=0; i<np2s; i++)
148  {
149  int p1, p2;
150  double vz[6]; // mean
151  double vv[36]; // prec
152 
153  if (!(ifs >> p1 >> p2))
154  {
155  cout << "Bad node indices at constraint " << i << endl;
156  return false;
157  }
158 
159  for (int i=0; i<6; i++)
160  {
161  if (!(ifs >> vz[i]))
162  {
163  cout << "Bad constraint mean at constraint " << i << endl;
164  return false;
165  }
166  }
167 
168  for (int i=0; i<36; i++)
169  {
170  if (!(ifs >> vv[i]))
171  {
172  cout << "Bad constraint precision at constraint " << i << endl;
173  return false;
174  }
175  }
176 
177  con.ndr = p1;
178  con.nd1 = p2;
179 
180  // con.setMean(Matrix<double,6,1>(vz));
181  con.prec = Matrix<double,6,6>(vv);
182 
183  cons.push_back(con);
184  }
185 
186 
187  // read in scale constraints
188  cout << "Reading in scale constraint data..." << flush;
189  std::vector<ConScale,Eigen::aligned_allocator<ConScale> > &scons = spa.scons;
190  ConScale scon;
191 
192  for (int i=0; i<nscs; i++)
193  {
194  int p1, p2, sv;
195  double ks, w;
196 
197  if (!(ifs >> p1 >> p2 >> sv >> ks >> w))
198  {
199  cout << "Bad scale constraint at constraint " << i << endl;
200  return false;
201  }
202 
203  scon.nd0 = p1;
204  scon.nd1 = p2;
205  scon.sv = sv;
206  scon.ks = ks;
207  scon.w = w;
208 
209  scons.push_back(scon);
210  }
211 
212  return true;
213  }
214 
215 // Do we want to normalize quaternion vectors based on the sign of w?
216 #define NORMALIZE_Q
217 
218  // set up Jacobians
219  // see Konolige RSS 2010 submission for details
220 
221  void ConP2::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes)
222  {
223  // node references
224  Node &nr = nodes[ndr];
225  Matrix<double,4,1> &tr = nr.trans;
226  Quaternion<double> &qr = nr.qrot;
227  Node &n1 = nodes[nd1];
228  Matrix<double,4,1> &t1 = n1.trans;
229  Quaternion<double> &q1 = n1.qrot;
230 
231  // first get the second frame in first frame coords
232  Eigen::Matrix<double,3,1> pc = nr.w2n * t1;
233 
234  // Jacobians wrt first frame parameters
235 
236  // translational part of 0p1 wrt translational vars of p0
237  // this is just -R0' [from 0t1 = R0'(t1 - t0)]
238  J0.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);
239 
240 
241  // translational part of 0p1 wrt rotational vars of p0
242  // dR'/dq * [pw - t]
243  Eigen::Matrix<double,3,1> pwt;
244  pwt = (t1-tr).head(3); // transform translations
245 
246  // dx
247  Eigen::Matrix<double,3,1> dp = nr.dRdx * pwt; // dR'/dq * [pw - t]
248  J0.block<3,1>(0,3) = dp;
249  // dy
250  dp = nr.dRdy * pwt; // dR'/dq * [pw - t]
251  J0.block<3,1>(0,4) = dp;
252  // dz
253  dp = nr.dRdz * pwt; // dR'/dq * [pw - t]
254  J0.block<3,1>(0,5) = dp;
255 
256  // rotational part of 0p1 wrt translation vars of p0 => zero
257  J0.block<3,3>(3,0).setZero();
258 
259  // rotational part of 0p1 wrt rotational vars of p0
260  // from 0q1 = qpmean * s0' * q0' * q1
261 
262  // dqdx
263  Eigen::Quaternion<double> qr0, qr1, qrn, qrd;
264  qr1.coeffs() = q1.coeffs();
265  qrn.coeffs() = Vector4d(-qpmean.w(),-qpmean.z(),qpmean.y(),qpmean.x()); // qpmean * ds0'/dx
266  qr0.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
267  qr0 = qr0*qr1; // rotate to zero mean
268  qrd = qpmean*qr0; // for normalization check
269  qrn = qrn*qr0;
270 
271 #ifdef NORMALIZE_Q
272  if (qrd.w() < 0.0)
273  qrn.vec() = -qrn.vec();
274 #endif
275 
276  J0.block<3,1>(3,3) = qrn.vec();
277 
278  // dqdy
279  qrn.coeffs() = Vector4d(qpmean.z(),-qpmean.w(),-qpmean.x(),qpmean.y()); // qpmean * ds0'/dy
280  qrn = qrn*qr0;
281 
282 #ifdef NORMALIZE_Q
283  if (qrd.w() < 0.0)
284  qrn.vec() = -qrn.vec();
285 #endif
286 
287  J0.block<3,1>(3,4) = qrn.vec();
288 
289  // dqdz
290  qrn.coeffs() = Vector4d(-qpmean.y(),qpmean.x(),-qpmean.w(),qpmean.z()); // qpmean * ds0'/dz
291  qrn = qrn*qr0;
292 
293 #ifdef NORMALIZE_Q
294  if (qrd.w() < 0.0)
295  qrn.vec() = -qrn.vec();
296 #endif
297 
298  J0.block<3,1>(3,5) = qrn.vec();
299 
300  // transpose
301  J0t = J0.transpose();
302 
303  // cout << endl << "J0 " << ndr << endl << J0 << endl;
304 
305  // Jacobians wrt second frame parameters
306  // translational part of 0p1 wrt translational vars of p1
307  // this is just R0' [from 0t1 = R0'(t1 - t0)]
308  J1.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);
309 
310  // translational part of 0p1 wrt rotational vars of p1: zero
311  J1.block<3,3>(0,3).setZero();
312 
313  // rotational part of 0p1 wrt translation vars of p0 => zero
314  J1.block<3,3>(3,0).setZero();
315 
316 
317  // rotational part of 0p1 wrt rotational vars of p0
318  // from 0q1 = q0'*s1*q1
319 
320  Eigen::Quaternion<double> qrc;
321  qrc.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
322  qrc = qpmean*qrc*qr1; // mean' * qr0' * qr1
323  qrc.normalize();
324 
325  // cout << endl << "QRC : " << qrc.coeffs().transpose() << endl;
326 
327  double dq = 1.0e-8;
328  double wdq = 1.0 - dq*dq;
329  qr1.coeffs() = Vector4d(dq,0,0,wdq);
330  // cout << "QRC+x: " << (qrc*qr1).coeffs().transpose() << endl;
331  // cout << "QRdx: " << ((qrc*qr1).coeffs().transpose() - qrc.coeffs().transpose())/dq << endl;
332 
333  // dqdx
334  qrn.coeffs() = Vector4d(1,0,0,0);
335  qrn = qrc*qrn;
336 
337  // cout << "J1dx: " << qrn.coeffs().transpose() << endl;
338 
339 #ifdef NORMALIZE_Q
340  if (qrc.w() < 0.0)
341  qrn.vec() = -qrn.vec();
342 #endif
343 
344  J1.block<3,1>(3,3) = qrn.vec();
345 
346  // dqdy
347  qrn.coeffs() = Vector4d(0,1,0,0);
348  qrn = qrc*qrn;
349 
350 #ifdef NORMALIZE_Q
351  if (qrc.w() < 0.0)
352  qrn.vec() = -qrn.vec();
353 #endif
354 
355  J1.block<3,1>(3,4) = qrn.vec();
356 
357  // dqdz
358  qrn.coeffs() = Vector4d(0,0,1,0);
359  qrn = qrc*qrn;
360 
361 #ifdef NORMALIZE_Q
362  if (qrc.w() < 0.0)
363  qrn.vec() = -qrn.vec();
364 #endif
365 
366  J1.block<3,1>(3,5) = qrn.vec();
367 
368  // transpose
369  J1t = J1.transpose();
370 
371  // cout << endl << "J1 " << nd1 << endl << J1 << endl;
372 
373  };
374 
375 
376 
377  // set up Jacobians
378  // see Konolige RSS 2010 submission for details
379 
380  void ConScale::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes)
381  {
382  // node references
383  Node &n0 = nodes[nd0];
384  Matrix<double,4,1> &t0 = n0.trans;
385  Node &n1 = nodes[nd1];
386  Matrix<double,4,1> &t1 = n1.trans;
387 
388  Eigen::Matrix<double,3,1> td = (t1-t0).head(3);
389 
390  // Jacobians wrt first frame parameters
391  // (ti - tj)^2 - a*kij
392 
393  // scale error wrt translational vars t0
394  J0 = -2.0 * td;
395 
396  // scale error wrt translational vars t1
397  J1 = 2.0 * td;
398 
399  // scale error wrt scale variable is just -kij
400  };
401 
402 
403 
404  //
405  // This hasn't been tested, and is probably wrong.
406  //
407 
408  void ConP3P::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > nodes)
409  {
410  // node references
411  Node nr = nodes[ndr];
412  Matrix<double,4,1> &tr = nr.trans;
413  Quaternion<double> &qr = nr.qrot;
414  Node n1 = nodes[nd1];
415  Matrix<double,4,1> &t1 = n1.trans;
416  Quaternion<double> &q1 = n1.qrot;
417  Node n2 = nodes[nd2];
418  Matrix<double,4,1> &t2 = n2.trans;
419  Quaternion<double> &q2 = n2.qrot;
420 
421  // calculate J10 -> d(0p1)/d(p0)
422  // rows are indexed by 0p1 parameters
423  // cols are indexed by p0 parameters
424 
425  // translational part of 0p1 wrt translational vars of p0
426  // this is just -R0' [from 0t1 = R0'(t1 - t0)]
427  J10.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);
428 
429  // translational part of 0p1 wrt rotational vars of p0
430  // dR'/dq * [pw - t]
431  Matrix<double,3,1> pwt = (t1-tr).head(3);
432  Matrix<double,3,1> dp = nr.dRdx * pwt; // dR'/dqx * [pw - t]
433  J10.block<3,1>(0,3) = dp;
434  dp = nr.dRdy * pwt; // dR'/dqy * [pw - t]
435  J10.block<3,1>(0,4) = dp;
436  dp = nr.dRdz * pwt; // dR'/dqz * [pw - t]
437  J10.block<3,1>(0,5) = dp;
438 
439  // rotational part of 0p1 wrt translation vars of p0 => zero
440  J10.block<3,3>(3,0).setZero();
441 
442  // rotational part of 0p1 wrt rotational vars of p0
443  // from 0q1 = q0'*q1
444 
445  // dqdx
446  double wn = -1.0/qr.w(); // need to switch params for small qr(3)
447  dp[0] = wn*t1[0]*qr.x() - t1[3];
448  dp[1] = wn*t1[1]*qr.x() + t1[2];
449  dp[2] = wn*t1[2]*qr.x() - t1[1];
450  J10.block<3,1>(3,3) = dp;
451 
452  // dqdy
453  dp[0] = wn*t1[0]*qr.y() - t1[2];
454  dp[1] = wn*t1[1]*qr.y() - t1[3];
455  dp[2] = wn*t1[2]*qr.y() + t1[0];
456  J10.block<3,1>(3,4) = dp;
457 
458  // dqdz
459  dp[0] = wn*t1[0]*qr.z() + t1[1];
460  dp[1] = wn*t1[1]*qr.z() - t1[0];
461  dp[2] = wn*t1[2]*qr.z() - t1[3];
462  J10.block<3,1>(3,5) = dp;
463 
464  // whew, J10 done...
465 
466  // J20 is similar, just using p2
467  // translational part of 0p2 wrt translational vars of p0
468  // this is just -R0' [from 0t2 = R0'(t2 - t0)]
469  J20.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);
470 
471  // translational part of 0p2 wrt rotational vars of p0
472  // dR'/dq * [pw - t]
473  pwt = (t2-tr).head(3);
474  dp = nr.dRdx * pwt; // dR'/dqx * [pw - t]
475  J20.block<3,1>(0,3) = dp;
476  dp = nr.dRdy * pwt; // dR'/dqy * [pw - t]
477  J20.block<3,1>(0,4) = dp;
478  dp = nr.dRdz * pwt; // dR'/dqz * [pw - t]
479  J20.block<3,1>(0,5) = dp;
480 
481  // rotational part of 0p1 wrt translation vars of p0 => zero
482  J20.block<3,3>(3,0).setZero();
483 
484  // rotational part of 0p2 wrt rotational vars of p0
485  // from 0q1 = q0'*q2
486 
487  // dqdx
488  wn = -1.0/qr.w(); // need to switch params for small qr(3)
489  dp[0] = wn*q2.x()*qr.x() - q2.w();
490  dp[1] = wn*q2.y()*qr.x() + q2.z();
491  dp[2] = wn*q2.z()*qr.x() - q2.y();
492  J20.block<3,1>(3,3) = dp;
493 
494  // dqdy
495  dp[0] = wn*q2.x()*qr.y() - q2.z();
496  dp[1] = wn*q2.y()*qr.y() - q2.w();
497  dp[2] = wn*q2.z()*qr.y() + q2.x();
498  J20.block<3,1>(3,4) = dp;
499 
500  // dqdz
501  dp[0] = wn*q2.x()*qr.z() + q2.y();
502  dp[1] = wn*q2.y()*qr.z() - q2.x();
503  dp[2] = wn*q2.z()*qr.z() - q2.w();
504  J20.block<3,1>(3,5) = dp;
505 
506  // calculate J11 -> d(0p1)/d(p1)
507  // rows are indexed by 0p1 parameters
508  // cols are indexed by p1 parameters
509 
510  // translational part of 0p1 wrt translational vars of p1
511  // this is just R0' [from 0t1 = R0'(t1 - t0)]
512  J11.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);
513 
514  // translational part of 0p1 wrt rotational vars of p1 => zero
515  J11.block<3,3>(0,3).setZero();
516 
517  // rotational part of 0p1 wrt translation vars of p1 => zero
518  J11.block<3,3>(3,0).setZero();
519 
520  // rotational part of 0p1 wrt rotational vars of p1
521  // from 0q1 = q0'*q1
522 
523  // dqdx1
524  wn = 1.0/q1.w(); // need to switch params for small q1(3)
525  dp[0] = wn*qr.x()*q1.x() + qr.w();
526  dp[1] = wn*qr.y()*q1.x() - qr.z();
527  dp[2] = wn*qr.z()*q1.x() + qr.y();
528  J11.block<3,1>(3,3) = dp;
529 
530  // dqdy1
531  dp[0] = wn*qr.x()*q1.y() + qr.z();
532  dp[1] = wn*qr.y()*q1.y() + qr.w();
533  dp[2] = wn*qr.z()*q1.y() - qr.x();
534  J11.block<3,1>(3,4) = dp;
535 
536  // dqdz1
537  dp[0] = wn*qr.x()*q1.z() - qr.y();
538  dp[1] = wn*qr.y()*q1.z() + qr.x();
539  dp[2] = wn*qr.z()*q1.z() + qr.w();
540  J11.block<3,1>(3,5) = dp;
541 
542  // whew, J11 done...
543 
544  // J22 is similar to J11
545  // rows are indexed by 0p2 parameters
546  // cols are indexed by p2 parameters
547 
548  // translational part of 0p2 wrt translational vars of p2
549  // this is just R0' [from 0t2 = R0'(t2 - t0)]
550  J22.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);
551 
552  // translational part of 0p2 wrt rotational vars of p2 => zero
553  J22.block<3,3>(0,3).setZero();
554 
555  // rotational part of 0p2 wrt translation vars of p2 => zero
556  J22.block<3,3>(3,0).setZero();
557 
558  // rotational part of 0p2 wrt rotational vars of p2
559  // from 0q2 = q0'*q2
560 
561  // dqdx2
562  wn = 1.0/q2.w(); // need to switch params for small q2.w()
563  dp[0] = wn*qr.x()*q2.x() + qr.w();
564  dp[1] = wn*qr.y()*q2.x() - qr.z();
565  dp[2] = wn*qr.z()*q2.x() + qr.y();
566  J22.block<3,1>(3,3) = dp;
567 
568  // dqdy1
569  dp[0] = wn*qr.x()*q2.y() + qr.z();
570  dp[1] = wn*qr.y()*q2.y() + qr.w();
571  dp[2] = wn*qr.z()*q2.y() - qr.x();
572  J22.block<3,1>(3,4) = dp;
573 
574  // dqdz1
575  dp[0] = wn*qr.x()*q2.z() - qr.y();
576  dp[1] = wn*qr.y()*q2.z() + qr.x();
577  dp[2] = wn*qr.z()*q2.z() + qr.w();
578  J22.block<3,1>(3,5) = dp;
579 
580  // whew, J22 done...
581 
582  };
583 
584 
585  // error function
586  inline double ConP2::calcErr(const Node &nd0, const Node &nd1)
587  {
588  Quaternion<double> q0p,q1;
589  q0p.vec() = -nd0.qrot.coeffs().head(3); // invert quaternion
590  q0p.w() = nd0.qrot.w();
591  q1 = nd1.qrot;
592  err.block<3,1>(0,0) = nd0.w2n * nd1.trans - tmean;
593 
594  // cout << endl;
595  // cout << q0p.coeffs().transpose() << endl;
596  // cout << q1.coeffs().transpose() << endl;
597  // cout << qpmean.coeffs().transpose() << endl;
598 
599  q1 = qpmean * q0p * q1;
600 
601  // cout << q1.coeffs().transpose() << endl << endl;
602 
603 // this seems to mess up convergence...
604 #ifdef NORMALIZE_Q
605  if (q1.w() < 0.0)
606  err.block<3,1>(3,0) = -q1.vec(); // normalized
607  else
608 #endif
609  err.block<3,1>(3,0) = q1.vec(); // do we have to normalize w???
610  // cout << endl << "Error: " << err.transpose() << endl << endl;
611  return err.dot(prec * err);
612  }
613 
614 
615  // error function for distance cost
616  double ConP2::calcErrDist(const Node &nd0, const Node &nd1)
617  {
618  Vector3d derr;
619  Quaternion<double> q0p,q1;
620  q0p.vec() = -nd0.qrot.vec(); // invert quaternion
621  q0p.w() = nd0.qrot.w();
622  q1 = nd1.qrot;
623  derr = nd0.w2n * nd1.trans - tmean;
624  return derr.dot(derr);
625  }
626 
627 
628  // error function
629  inline double ConScale::calcErr(const Node &nd0, const Node &nd1, double alpha)
630  {
631  err = (nd1.trans - nd0.trans).squaredNorm();
632  err -= ks*alpha;
633  // cout << "Scale err: " << err << endl;
634  return w*err*err;
635  }
636 
637 
638  // Adds a node to the system.
639  // \return the index of the node added.
640  int SysSPA::addNode(Eigen::Matrix<double,4,1> &trans,
641  Eigen::Quaternion<double> &qrot,
642  bool isFixed)
643  {
644  Node nd;
645  nd.trans = trans;
646  nd.qrot = qrot;
647  nd.isFixed = isFixed;
648  nd.setTransform(); // set up world2node transform
649  nd.setDr(true); // set rotational derivatives
650  // Should this be local or global?
651  nd.normRot();//Local();
652  nodes.push_back(nd);
653  return nodes.size()-1;
654  }
655 
656 
657  // add a constraint
658  // <nd0>, <nd1> are node id's
659  // <mean> is x,y,th, with th in radians
660  // <prec> is a 3x3 precision matrix (inverse covariance
661  // returns true if nodes are found
662  bool SysSPA::addConstraint(int nd0, int nd1,
663  Eigen::Vector3d &tmean,
664  Eigen::Quaterniond &qpmean,
665  Eigen::Matrix<double,6,6> &prec)
666  {
667  if (nd0 >= (int)nodes.size() || nd1 >= (int)nodes.size())
668  return false;
669 
670  ConP2 con;
671  con.ndr = nd0;
672  con.nd1 = nd1;
673 
674  con.tmean = tmean;
675  Quaternion<double> qr;
676  qr = qpmean;
677  qr.normalize();
678  con.qpmean = qr.inverse(); // inverse of the rotation measurement
679  con.prec = prec;
680 
681  p2cons.push_back(con);
682  return true;
683  }
684 
685 
686  // error measure, squared
687  // assumes node transforms have already been calculated
688  // <tcost> is true if we just want the distance offsets
689  double SysSPA::calcCost(bool tcost)
690  {
691  double cost = 0.0;
692 
693  // do distance offset
694  if (tcost)
695  {
696  for(size_t i=0; i<p2cons.size(); i++)
697  {
698  ConP2 &con = p2cons[i];
699  double err = con.calcErrDist(nodes[con.ndr],nodes[con.nd1]);
700  cost += err;
701  }
702  }
703 
704  // full cost
705  else
706  {
707  for(size_t i=0; i<p2cons.size(); i++)
708  {
709  ConP2 &con = p2cons[i];
710  double err = con.calcErr(nodes[con.ndr],nodes[con.nd1]);
711  cost += err;
712  }
713  if (scons.size() > 0) // can have zero scale constraints
714  for(size_t i=0; i<scons.size(); i++)
715  {
716  ConScale &con = scons[i];
717  double err = con.calcErr(nodes[con.nd0],nodes[con.nd1],scales[con.sv]);
718  cost += err;
719  }
720  }
721 
722  return cost;
723  }
724 
725 
726  // Set up linear system, from RSS submission (konolige 2010)
727  // This is a relatively compact version of the algorithm!
728  // but not Jacobians
729 
730  void SysSPA::setupSys(double sLambda)
731  {
732  // set matrix sizes and clear
733  // assumes scales vars are all free
734  int nFree = nodes.size() - nFixed;
735  int nscales = scales.size();
736  A.setZero(6*nFree+nscales,6*nFree+nscales);
737  B.setZero(6*nFree+nscales);
738  VectorXi dcnt(nFree);
739  dcnt.setZero(nFree);
740 
741  // lambda augmentation
742  double lam = 1.0 + sLambda;
743 
744  // loop over P2 constraints
745  for(size_t pi=0; pi<p2cons.size(); pi++)
746  {
747  ConP2 &con = p2cons[pi];
748  con.setJacobians(nodes);
749 
750  // add in 4 blocks of A; actually just need upper triangular
751  // i0 < i1
752  int i0 = 6*(con.ndr-nFixed); // will be negative if fixed
753  int i1 = 6*(con.nd1-nFixed); // will be negative if fixed
754 
755  if (i0>=0)
756  {
757  A.block<6,6>(i0,i0) += con.J0t * con.prec * con.J0;
758  dcnt(con.ndr - nFixed)++;
759  }
760  if (i1>=0)
761  {
762  dcnt(con.nd1 - nFixed)++;
763  Matrix<double,6,6> tp = con.prec * con.J1;
764  A.block<6,6>(i1,i1) += con.J1t * tp;
765  if (i0>=0)
766  {
767  A.block<6,6>(i0,i1) += con.J0t * con.prec * con.J1;
768  A.block<6,6>(i1,i0) += con.J1t * con.prec * con.J0;
769  }
770  }
771 
772  // add in 2 blocks of B
773  if (i0>=0)
774  B.block<6,1>(i0,0) -= con.J0t * con.prec * con.err;
775  if (i1>=0)
776  B.block<6,1>(i1,0) -= con.J1t * con.prec * con.err;
777  } // finish P2 constraints
778 
779  // loop over Scale constraints
780  if (scons.size() > 0) // could be zero
781  for(size_t pi=0; pi<scons.size(); pi++)
782  {
783  ConScale &con = scons[pi];
784  con.setJacobians(nodes);
785  // add in 4 blocks of A for t0, t1; actually just need upper triangular
786  // i0 < i1
787  int i0 = 6*(con.nd0-nFixed); // will be negative if fixed
788  int i1 = 6*(con.nd1-nFixed); // will be negative if fixed
789 
790  if (i0>=0)
791  {
792  A.block<3,3>(i0,i0) += con.w * con.J0 * con.J0.transpose();
793  }
794  if (i1>=0)
795  {
796  A.block<3,3>(i1,i1) += con.w * con.J1 * con.J1.transpose();
797  if (i0>=0)
798  {
799  A.block<3,3>(i0,i1) += con.w * con.J0 * con.J1.transpose();
800  A.block<3,3>(i1,i0) = A.block<3,3>(i0,i1).transpose();
801  }
802  }
803 
804  // add in 2 blocks of B
805  if (i0>=0)
806  B.block<3,1>(i0,0) -= con.w * con.J0 * con.err;
807  if (i1>=0)
808  B.block<3,1>(i1,0) -= con.w * con.J1 * con.err;
809 
810  // scale variable, add in 5 blocks; actually just need upper triangular
811  int is = 6*nFree+con.sv;
812  A(is,is) += con.w * con.ks * con.ks;
813  if (i0>=0)
814  {
815  A.block<3,1>(i0,is) += con.w * con.J0 * -con.ks;
816  A.block<1,3>(is,i0) = A.block<3,1>(i0,is).transpose();
817  }
818  if (i1>=0)
819  {
820  A.block<3,1>(i1,is) += con.w * con.J1 * -con.ks;
821  A.block<1,3>(is,i1) = A.block<3,1>(i1,is).transpose();
822  }
823 
824  // add in scale block of B
825  B(is) -= con.w * (-con.ks) * con.err;
826 
827  } // finish Scale constraints
828 
829 
830  // augment diagonal
831  A.diagonal() *= lam;
832 
833  // check the matrix and vector
834  for (int i=0; i<6*nFree; i++)
835  for (int j=0; j<6*nFree; j++)
836  if (std::isnan(A(i,j)) ) { printf("[SetupSys] NaN in A\n"); *(int *)0x0 = 0; }
837 
838  for (int j=0; j<6*nFree; j++)
839  if (std::isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }
840 
841  int ndc = 0;
842  for (int i=0; i<nFree; i++)
843  if (dcnt(i) == 0) ndc++;
844 
845  if (ndc > 0)
846  cout << "[SetupSys] " << ndc << " disconnected nodes" << endl;
847  }
848 
849 
850  // Set up sparse linear system; see setupSys for algorithm.
851  // Currently doesn't work with scale variables
852  void SysSPA::setupSparseSys(double sLambda, int iter, int sparseType)
853  {
854  // set matrix sizes and clear
855  // assumes scales vars are all free
856  int nFree = nodes.size() - nFixed;
857 
858  // long long t0, t1, t2, t3;
859  // t0 = utime();
860 
861  if (iter == 0)
862  csp.setupBlockStructure(nFree); // initialize CSparse structures
863  else
864  csp.setupBlockStructure(0); // zero out CSparse structures
865 
866  // t1 = utime();
867 
868  VectorXi dcnt(nFree);
869  dcnt.setZero(nFree);
870 
871  // lambda augmentation
872  double lam = 1.0 + sLambda;
873 
874  // loop over P2 constraints
875  for(size_t pi=0; pi<p2cons.size(); pi++)
876  {
877  ConP2 &con = p2cons[pi];
878  con.setJacobians(nodes);
879 
880  // add in 4 blocks of A; actually just need upper triangular
881  int i0 = con.ndr-nFixed; // will be negative if fixed
882  int i1 = con.nd1-nFixed; // will be negative if fixed
883 
884  if (i0>=0)
885  {
886  Matrix<double,6,6> m = con.J0t*con.prec*con.J0;
887  csp.addDiagBlock(m,i0);
888  dcnt(con.ndr - nFixed)++;
889  }
890  if (i1>=0)
891  {
892  dcnt(con.nd1 - nFixed)++;
893  Matrix<double,6,6> tp = con.prec * con.J1;
894  Matrix<double,6,6> m = con.J1t * tp;
895  csp.addDiagBlock(m,i1);
896  if (i0>=0)
897  {
898  Matrix<double,6,6> m2 = con.J0t * tp;
899  if (i1 < i0)
900  {
901  m = m2.transpose();
902  csp.addOffdiagBlock(m,i1,i0);
903  }
904  else
905  csp.addOffdiagBlock(m2,i0,i1);
906  }
907  }
908 
909  // add in 2 blocks of B
910  if (i0>=0)
911  csp.B.block<6,1>(i0*6,0) -= con.J0t * con.prec * con.err;
912  if (i1>=0)
913  csp.B.block<6,1>(i1*6,0) -= con.J1t * con.prec * con.err;
914  } // finish P2 constraints
915 
916  // t2 = utime();
917 
918  // set up sparse matrix structure from blocks
919  if (sparseType == SBA_BLOCK_JACOBIAN_PCG)
920  csp.incDiagBlocks(lam); // increment diagonal block
921  else
922  csp.setupCSstructure(lam,iter==0);
923 
924  // t3 = utime();
925 
926  // printf("\n[SetupSparseSys] Block: %0.1f Cons: %0.1f CS: %0.1f\n",
927  // (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
928 
929  int ndc = 0;
930  for (int i=0; i<nFree; i++)
931  if (dcnt(i) == 0) ndc++;
932 
933  if (ndc > 0)
934  cout << "[SetupSparseSys] " << ndc << " disconnected nodes" << endl;
935  }
936 
937 
946 
947  int SysSPA::doSPA(int niter, double sLambda, int useCSparse, double initTol,
948  int maxCGiters)
949  {
950  Node::initDr();
951  int nFree = nodes.size() - nFixed; // number of free nodes
952 
953  // number of nodes
954  int ncams = nodes.size();
955  // number of scale variables
956  int nscales = scales.size();
957 
958  // save old scales
959  vector<double> oldscales;
960  oldscales.resize(nscales);
961 
962  // initialize vars
963  if (sLambda > 0.0) // do we initialize lambda?
964  lambda = sLambda;
965 
966  // set number of constraints
967  int ncons = p2cons.size();
968 
969  // check for fixed frames
970  for (int i=0; i<ncams; i++)
971  {
972  Node &nd = nodes[i];
973  if (i >= nFixed)
974  nd.isFixed = false;
975  else
976  nd.isFixed = true;
977  nd.setTransform(); // set up world-to-node transform for cost calculation
978  nd.setDr(true); // always use local angles
979  }
980 
981  // initialize vars
982  double laminc = 2.0; // how much to increment lambda if we fail
983  double lamdec = 0.5; // how much to decrement lambda if we succeed
984  int iter = 0; // iterations
985  sqMinDelta = 1e-8 * 1e-8;
986  double cost = calcCost();
987  if (verbose)
988  cout << iter << " Initial squared cost: " << cost << " which is "
989  << sqrt(cost/ncons) << " rms error" << endl;
990 
991  int good_iter = 0;
992  for (; iter<niter; iter++) // loop at most <niter> times
993  {
994  // set up and solve linear system
995  // NOTE: shouldn't need to redo all calcs in setupSys if we
996  // got here from a bad update
997 
998  long long t0, t1, t2, t3;
999  t0 = utime();
1000  if (useCSparse)
1001  setupSparseSys(lambda,iter,useCSparse); // set up sparse linear system
1002  else
1003  setupSys(lambda); // set up linear system
1004 
1005  // use appropriate linear solver
1006  if (useCSparse == SBA_BLOCK_JACOBIAN_PCG)
1007  {
1008  if (csp.B.rows() != 0)
1009  {
1010  int iters = csp.doBPCG(maxCGiters,initTol,iter);
1011  if (verbose)
1012  cout << "[Block PCG] " << iters << " iterations" << endl;
1013  }
1014  }
1015  else if (useCSparse > 0)
1016  {
1017  bool ok = csp.doChol();
1018  if (!ok)
1019  cout << "[DoSPA] Sparse Cholesky failed!" << endl;
1020  }
1021  else
1022  A.ldlt().solveInPlace(B); // Cholesky decomposition and solution
1023 
1024  // get correct result vector
1025  VectorXd &BB = useCSparse ? csp.B : B;
1026 
1027  // check for convergence
1028  // this is a pretty crummy convergence measure...
1029  double sqDiff = BB.squaredNorm();
1030  if (sqDiff < sqMinDelta) // converged, done...
1031  {
1032  break;
1033  }
1034 
1035  // update the frames
1036  int ci = 0;
1037  for(int i=0; i < ncams; i++)
1038  {
1039  Node &nd = nodes[i];
1040  if (nd.isFixed) continue; // not to be updated
1041  nd.oldtrans = nd.trans; // save in case we don't improve the cost
1042  nd.oldqrot = nd.qrot;
1043  nd.trans.head<3>() += BB.segment<3>(ci);
1044 
1045  Quaternion<double> qr;
1046  qr.vec() = BB.segment<3>(ci+3);
1047  qr.w() = sqrt(1.0 - qr.vec().squaredNorm());
1048 
1049  Quaternion<double> qrn,qrx;
1050  qrn = nd.qrot;
1051  qr = qrn*qr; // post-multiply
1052  qr.normalize();
1053  if (qr.w() < 0.0)
1054  nd.qrot.coeffs() = -qr.coeffs();
1055  else
1056  nd.qrot.coeffs() = qr.coeffs();
1057 
1058  nd.setTransform(); // set up projection matrix for cost calculation
1059  nd.setDr(true); // set rotational derivatives
1060  ci += 6; // advance B index
1061  }
1062 
1063  // update the scales
1064  ci = 6*nFree; // head of scale vars
1065  if (nscales > 0) // could be empty
1066  for(int i=0; i < nscales; i++)
1067  {
1068  oldscales[i] = scales[i];
1069  scales[i] += B(ci);
1070  ci++;
1071  }
1072 
1073 
1074  // new cost
1075  double newcost = calcCost();
1076  if (verbose)
1077  cout << iter << " Updated squared cost: " << newcost << " which is "
1078  << sqrt(newcost/ncons) << " rms error" << endl;
1079 
1080  // check if we did good
1081  if (newcost < cost) // && iter != 0) // NOTE: iter==0 case is for checking
1082  {
1083  cost = newcost;
1084  lambda *= lamdec; // decrease lambda
1085  // laminc = 2.0; // reset bad lambda factor; not sure if this is a good idea...
1086  good_iter++;
1087  }
1088  else
1089  {
1090  lambda *= laminc; // increase lambda
1091  laminc *= 2.0; // increase the increment
1092 
1093  // reset nodes
1094  for(int i=0; i<ncams; i++)
1095  {
1096  Node &nd = nodes[i];
1097  if (nd.isFixed) continue; // not to be updated
1098  nd.trans = nd.oldtrans;
1099  nd.qrot = nd.oldqrot;
1100  nd.setTransform(); // set up projection matrix for cost calculation
1101  nd.setDr(true);
1102  }
1103 
1104  // reset scales
1105  if (nscales > 0) // could be empty
1106  for(int i=0; i < nscales; i++)
1107  scales[i] = oldscales[i];
1108 
1109 
1110  cost = calcCost(); // need to reset errors
1111  if (verbose)
1112  cout << iter << " Downdated cost: " << cost << endl;
1113  // NOTE: shouldn't need to redo all calcs in setupSys
1114  }
1115  }
1116 
1117  // return number of iterations performed
1118  return good_iter;
1119 
1120  }
1121 
1122 
1123  // write out the precision matrix for CSparse
1124  void SysSPA::writeSparseA(char *fname, bool useCSparse)
1125  {
1126  ofstream ofs(fname);
1127  if (!ofs)
1128  {
1129  cout << "Can't open file " << fname << endl;
1130  return;
1131  }
1132 
1133  // cameras
1134  if (useCSparse)
1135  {
1136  setupSparseSys(0.0,0,useCSparse);
1137 
1138  int *Ai = csp.A->i;
1139  int *Ap = csp.A->p;
1140  double *Ax = csp.A->x;
1141 
1142  for (int i=0; i<csp.csize; i++)
1143  for (int j=Ap[i]; j<Ap[i+1]; j++)
1144  if (Ai[j] <= i)
1145  ofs << Ai[j] << " " << i << setprecision(16) << " " << Ax[j] << endl;
1146  }
1147  else
1148  {
1149  Eigen::IOFormat pfmt(16);
1150 
1151  int nrows = A.rows();
1152  int ncols = A.cols();
1153 
1154  for (int i=0; i<nrows; i++)
1155  for (int j=i; j<ncols; j++)
1156  {
1157  double a = A(i,j);
1158  if (A(i,j) != 0.0)
1159  ofs << i << " " << j << setprecision(16) << " " << a << endl;
1160  }
1161  }
1162 
1163  ofs.close();
1164  }
1165 
1166 
1167  // Set up spanning tree initialization
1168  void SysSPA::spanningTree(int node)
1169  {
1170  int nnodes = nodes.size();
1171 
1172  // set up an index from nodes to their constraints
1173  vector<vector<int> > cind;
1174  cind.resize(nnodes);
1175 
1176  for(size_t pi=0; pi<p2cons.size(); pi++)
1177  {
1178  ConP2 &con = p2cons[pi];
1179  int i0 = con.ndr;
1180  int i1 = con.nd1;
1181  cind[i0].push_back(i1);
1182  cind[i1].push_back(i0);
1183  }
1184 
1185  // set up breadth-first algorithm
1186  VectorXd dist(nnodes);
1187  dist.setConstant(1e100);
1188  if (node >= nnodes)
1189  node = 0;
1190  dist[node] = 0.0;
1191  std::multimap<double,int> open; // open list, priority queue - can have duplicates
1192  open.emplace(0.0,node);
1193 
1194  // do breadth-first computation
1195  while (!open.empty())
1196  {
1197  // get top node, remove it
1198  int ni = open.begin()->second;
1199  double di = open.begin()->first;
1200  open.erase(open.begin());
1201  if (di > dist[ni]) continue; // already dealt with
1202 
1203  // update neighbors
1204  Node &nd = nodes[ni];
1205  Matrix<double,3,4> n2w;
1206  transformF2W(n2w,nd.trans,nd.qrot); // from node to world coords
1207 
1208  vector<int> &nns = cind[ni];
1209  for (int i=0; i<(int)nns.size(); i++)
1210  {
1211  ConP2 &con = p2cons[nns[i]];
1212  double dd = con.tmean.norm(); // incremental distance
1213  // neighbor node index
1214  int nn = con.nd1;
1215  if (nn == ni)
1216  nn = con.ndr;
1217  Node &nd2 = nodes[nn];
1218  Vector3d tmean = con.tmean;
1219  Quaterniond qpmean = con.qpmean;
1220  if (nn == con.ndr) // wrong way, reverse
1221  {
1222  qpmean = qpmean.inverse();
1223  tmean = nd.qrot.toRotationMatrix().transpose()*nd2.qrot.toRotationMatrix()*tmean;
1224  }
1225 
1226  if (dist[nn] > di + dd) // is neighbor now closer?
1227  {
1228  // set priority queue
1229  dist[nn] = di+dd;
1230  open.emplace(di+dd,nn);
1231  // update initial pose
1232  Vector4d trans;
1233  trans.head(3) = tmean;
1234  trans(3) = 1.0;
1235  nd2.trans.head(3) = n2w*trans;
1236  nd2.qrot = qpmean*nd.qrot;
1237  nd2.normRot();
1238  nd2.setTransform();
1239  nd2.setDr(true);
1240  }
1241  }
1242  }
1243 
1244  }
1245 
1246 
1247 } // namespace sba
sba::ConP2::J1t
Eigen::Matrix< double, 6, 6 > J1t
Definition: sba.h:324
sba::ConScale::sv
int sv
Scale variable index.
Definition: sba.h:361
Eigen
Definition: sba_file_io.h:18
sba::Node::setDr
void setDr(bool local=false)
Set angle derivates.
Definition: node.cpp:56
sba::ConP2::calcErr
double calcErr(const Node &nd0, const Node &nd1)
calculates projection error and stores it in <err>
Definition: spa.cpp:586
utime
static long long utime()
Definition: spa.cpp:66
sba::ConScale::nd1
int nd1
Node index for the second node.
Definition: sba.h:358
sba::SysSPA
SysSPA holds a set of nodes and constraints for sparse pose adjustment.
Definition: sba.h:440
sba::Node::oldqrot
Eigen::Quaternion< double > oldqrot
Previous quaternion rotation, saved for downdating within LM.
Definition: node.h:147
sba::Node::normRot
void normRot()
Normalize quaternion to unit. Problem with global derivatives near w=0, solved by a hack for now.
Definition: node.cpp:37
sba::ConScale::J1
Eigen::Vector3d J1
Definition: sba.h:380
sba::ConScale::calcErr
double calcErr(const Node &nd0, const Node &nd1, double alpha)
calculates projection error and stores it in <err>
Definition: spa.cpp:629
sba::Node::dRdy
Eigen::Matrix< double, 3, 3 > dRdy
Definition: node.h:126
sba::ConP2::J1
Eigen::Matrix< double, 6, 6 > J1
Definition: sba.h:324
sba::writeSparseA
void writeSparseA(const char *fname, SysSBA &sba)
Definition: sba_file_io.cpp:418
sba::ConP2::qpmean
Eigen::Quaternion< double > qpmean
Definition: sba.h:310
ok
ROSCPP_DECL bool ok()
sba::Node::isFixed
bool isFixed
For SBA, is this camera fixed or free?
Definition: node.h:141
sba::readP2File
bool readP2File(char *fname, SysSPA spa)
constraint files
Definition: spa.cpp:75
NORMALIZE_Q
#define NORMALIZE_Q
Definition: spa.cpp:216
sba
Definition: bpcg.h:60
sba::ConP2::J0
Eigen::Matrix< double, 6, 6 > J0
jacobian with respect to frames; uses dR'/dq from Node calculation
Definition: sba.h:324
sba::Node::w2n
Eigen::Matrix< double, 3, 4 > w2n
Resultant transform from world to node coordinates.
Definition: node.h:80
sba::Node::qrot
Eigen::Quaternion< double > qrot
Rotation of the node expressed as a Quaternion.
Definition: node.h:70
sba::ConScale::setJacobians
void setJacobians(std::vector< Node, Eigen::aligned_allocator< Node > > &nodes)
jacobians are computed from (ti - tj)^2 - a*kij = 0
Definition: spa.cpp:380
SBA_BLOCK_JACOBIAN_PCG
#define SBA_BLOCK_JACOBIAN_PCG
Definition: sba.h:69
sba::Node::oldtrans
Eigen::Matrix< double, 4, 1 > oldtrans
Previous translation, saved for downdating within LM.
Definition: node.h:144
sba::ConP2::nd1
int nd1
Node index for the second node.
Definition: sba.h:306
sba::ConScale::J0
Eigen::Vector3d J0
Definition: sba.h:376
sba::ConScale
Definition: sba.h:349
sba::SysSPA::p2cons
std::vector< ConP2, Eigen::aligned_allocator< ConP2 > > p2cons
Set of P2 constraints.
Definition: sba.h:483
sba::Node::setTransform
void setTransform()
Sets the transform matrices of the node.
Definition: node.cpp:8
sba::ConP2::setJacobians
void setJacobians(std::vector< Node, Eigen::aligned_allocator< Node > > &nodes)
Definition: spa.cpp:221
sba::ConP2::ndr
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int ndr
Reference pose index.
Definition: sba.h:303
std
sba::ConP2::prec
Eigen::Matrix< double, 6, 6 > prec
Definition: sba.h:311
sba::ConP2
Definition: sba.h:297
sba::ConP2::calcErrDist
double calcErrDist(const Node &nd0, const Node &nd1)
calculate error in distance only, no weighting
Definition: spa.cpp:616
sba::Node::trans
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 4, 1 > trans
Translation in homogeneous coordinates, last element is 1.0.
Definition: node.h:69
sba::ConP2::tmean
Eigen::Vector3d tmean
Mean vector, quaternion (inverse) and precision matrix for this constraint.
Definition: sba.h:309
sba::transformF2W
void transformF2W(Eigen::Matrix< double, 3, 4 > &m, const Eigen::Matrix< double, 4, 1 > &trans, const Eigen::Quaternion< double > &qrot)
Definition: node.cpp:166
sba::ConP2::J0t
Eigen::Matrix< double, 6, 6 > J0t
Definition: sba.h:324
sba::ConScale::nd0
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int nd0
Reference pose index.
Definition: sba.h:355
sba::Node::dRdz
Eigen::Matrix< double, 3, 3 > dRdz
Definition: node.h:126
sba.h
sba::Node
NODE holds graph nodes corresponding to frames, for use in sparse bundle adjustment....
Definition: node.h:63
sba::Node::dRdx
Eigen::Matrix< double, 3, 3 > dRdx
Derivatives of the rotation matrix transpose wrt quaternion xyz, used for calculating Jacobian wrt po...
Definition: node.h:126
sba::ConP2::err
Eigen::Matrix< double, 6, 1 > err
error
Definition: sba.h:315
sba::SysSPA::scons
std::vector< ConScale, Eigen::aligned_allocator< ConScale > > scons
Set of scale constraints.
Definition: sba.h:486
sba::ConScale::err
double err
error
Definition: sba.h:370
sba::ConScale::w
double w
Weight for this constraint.
Definition: sba.h:367
sba::SysSPA::nodes
std::vector< Node, Eigen::aligned_allocator< Node > > nodes
set of nodes (camera frames) for SPA system, indexed by position;
Definition: sba.h:474
sba::ConScale::ks
double ks
Scale factor for this constraint.
Definition: sba.h:364


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