spa.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 //
00036 // Sparse Pose Adjustment classes and functions
00037 //
00038 
00039 //
00040 // Ok, we're going with 2-pose constraints, and added scale variables.
00041 // Format for files:
00042 // Format for files:
00043 //   #nodes #scales #p2p_constraints #scale_constraints
00044 //   x y z rx ry rz   ; first node, vector part of unit quaternion
00045 //   ...
00046 //   p1 p2 Lambda_12  ; first p2p constraint
00047 //   ...
00048 //   p1 p2 s1 K_12 w  ; first scale constraint, 
00049 //                    ; (p1-p2)^2 = s1*K_12 with weight w
00050 
00051 
00052 #include <stdio.h>
00053 #include "sba/sba.h"
00054 #include <Eigen/Cholesky>
00055 
00056 using namespace Eigen;
00057 using namespace std;
00058 
00059 #include <iostream>
00060 #include <iomanip>
00061 #include <fstream>
00062 #include <sys/time.h>
00063 
00064 // elapsed time in microseconds
00065 static long long utime()
00066 {
00067   timeval tv;
00068   gettimeofday(&tv,NULL);
00069   long long ts = tv.tv_sec;
00070   ts *= 1000000;
00071   ts += tv.tv_usec;
00072   return ts;
00073 }
00074 
00075 namespace sba
00076 {
00077   // reads in a file of pose constraints
00078   bool readP2File(char *fname, SysSPA spa)
00079   {
00080     // Format for files:
00081     //   #nodes #scales #p2p_constraints #scale_constraints
00082     //   x y z rx ry rz   ; first node, vector part of unit quaternion
00083     //   ...
00084     //   p1 p2 Lambda_12  ; first p2p constraint
00085     //   ...
00086     //   p1 p2 s1 K_12 w  ; first scale constraint, 
00087     //                    ; (p1-p2)^2 = s1*K_12 with weight w
00088 
00089     ifstream ifs(fname);
00090     if (ifs == NULL)
00091       {
00092         cout << "Can't open file " << fname << endl;
00093         return false;
00094       }
00095     ifs.precision(10);          // what is this for???
00096 
00097     // read header
00098     string line;
00099     if (!getline(ifs,line) || line != "# P2 Constraint File")
00100     {
00101       cout << "Bad header" << endl;
00102       return false;
00103     }
00104     cout << "Found P2 constraint file" << endl;
00105 
00106     // read number of cameras, scalers, and p2p constraints
00107     int ncams, nss, nscs, np2s;
00108     if (!(ifs >> ncams >> nss >> np2s >> nscs))
00109     {
00110       cout << "Bad entity count" << endl;  
00111       return false;
00112     }
00113     cout << "Number of cameras: " << ncams 
00114          << "  Number of scalers: " << nss
00115          << "  Number of p2p constraints: " << np2s
00116          << "  Number of scale constraints: " << nscs << endl;
00117     
00118     cout << "Reading in camera data..." << flush;
00119     std::vector<Node,Eigen::aligned_allocator<Node> > &nodes = spa.nodes;
00120     Node nd;
00121 
00122     for (int i=0; i<ncams; i++)
00123       {
00124         double v1,v2,v3;
00125         if (!(ifs >> v1 >> v2 >> v3))
00126           {
00127             cout << "Bad node translation params at number " << i << endl;
00128             return false;
00129           }
00130         nd.trans = Vector4d(v1,v2,v3,1.0);
00131 
00132         if (!(ifs >> v1 >> v2 >> v3))
00133           {
00134             cout << "Bad node rotation quaternion at number " << i << endl;
00135             return false;
00136           }
00137         Matrix3d m;
00138         nd.qrot = Quaternion<double>(v1,v2,v2,0.0);
00139         nd.normRot();           // normalize to unit vector and compute w
00140         
00141         nodes.push_back(nd);
00142       }
00143     cout << "done" << endl;
00144 
00145     // read in p2p constraints
00146     cout << "Reading in constraint data..." << flush;
00147     std::vector<ConP2,Eigen::aligned_allocator<ConP2> > &cons = spa.p2cons;
00148     ConP2 con;
00149 
00150     for (int i=0; i<np2s; i++)
00151       {
00152         int p1, p2;
00153         double vz[6];           // mean
00154         double vv[36];          // prec
00155 
00156         if (!(ifs >> p1 >> p2))
00157           {
00158             cout << "Bad node indices at constraint " << i << endl;
00159             return false;
00160           }
00161 
00162         for (int i=0; i<6; i++)
00163           {
00164             if (!(ifs >> vz[i]))
00165               {
00166                 cout << "Bad constraint mean at constraint " << i << endl;
00167                 return false;
00168               }
00169           }
00170 
00171         for (int i=0; i<36; i++)
00172           {
00173             if (!(ifs >> vv[i]))
00174               {
00175                 cout << "Bad constraint precision at constraint " << i << endl;
00176                 return false;
00177               }
00178           }
00179 
00180         con.ndr = p1;
00181         con.nd1 = p2;
00182 
00183         //        con.setMean(Matrix<double,6,1>(vz));
00184         con.prec = Matrix<double,6,6>(vv);
00185 
00186         cons.push_back(con);
00187       }
00188 
00189 
00190     // read in scale constraints
00191     cout << "Reading in scale constraint data..." << flush;
00192     std::vector<ConScale,Eigen::aligned_allocator<ConScale> > &scons = spa.scons;
00193     ConScale scon;
00194 
00195     for (int i=0; i<nscs; i++)
00196       {
00197         int p1, p2, sv;
00198         double ks, w;
00199 
00200         if (!(ifs >> p1 >> p2 >> sv >> ks >> w))
00201           {
00202             cout << "Bad scale constraint at constraint " << i << endl;
00203             return false;
00204           }
00205         
00206         scon.nd0 = p1;
00207         scon.nd1 = p2;
00208         scon.sv  = sv;
00209         scon.ks  = ks;
00210         scon.w   = w;
00211 
00212         scons.push_back(scon);
00213       }
00214 
00215     return true;
00216   }
00217 
00218 // Do we want to normalize quaternion vectors based on the sign of w?
00219 #define NORMALIZE_Q
00220 
00221   // set up Jacobians
00222   // see Konolige RSS 2010 submission for details
00223 
00224   void ConP2::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes)
00225   {
00226     // node references
00227     Node &nr = nodes[ndr];
00228     Matrix<double,4,1> &tr = nr.trans;
00229     Quaternion<double> &qr = nr.qrot;
00230     Node &n1 = nodes[nd1];
00231     Matrix<double,4,1> &t1 = n1.trans;
00232     Quaternion<double> &q1 = n1.qrot;
00233 
00234     // first get the second frame in first frame coords
00235     Eigen::Matrix<double,3,1> pc = nr.w2n * t1;
00236 
00237     // Jacobians wrt first frame parameters
00238 
00239     // translational part of 0p1 wrt translational vars of p0
00240     // this is just -R0'  [from 0t1 = R0'(t1 - t0)]
00241     J0.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);
00242 
00243 
00244     // translational part of 0p1 wrt rotational vars of p0
00245     // dR'/dq * [pw - t]
00246     Eigen::Matrix<double,3,1> pwt;
00247     pwt = (t1-tr).head(3);   // transform translations
00248 
00249     // dx
00250     Eigen::Matrix<double,3,1> dp = nr.dRdx * pwt; // dR'/dq * [pw - t]
00251     J0.block<3,1>(0,3) = dp;
00252     // dy
00253     dp = nr.dRdy * pwt; // dR'/dq * [pw - t]
00254     J0.block<3,1>(0,4) = dp;
00255     // dz
00256     dp = nr.dRdz * pwt; // dR'/dq * [pw - t]
00257     J0.block<3,1>(0,5) = dp;
00258 
00259     // rotational part of 0p1 wrt translation vars of p0 => zero
00260     J0.block<3,3>(3,0).setZero();
00261 
00262     // rotational part of 0p1 wrt rotational vars of p0
00263     // from 0q1 = qpmean * s0' * q0' * q1
00264 
00265     // dqdx
00266     Eigen::Quaternion<double> qr0, qr1, qrn, qrd;
00267     qr1.coeffs() = q1.coeffs();
00268     qrn.coeffs() = Vector4d(-qpmean.w(),-qpmean.z(),qpmean.y(),qpmean.x());  // qpmean * ds0'/dx
00269     qr0.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
00270     qr0 = qr0*qr1;              // rotate to zero mean
00271     qrd = qpmean*qr0;           // for normalization check
00272     qrn = qrn*qr0;
00273 
00274 #ifdef NORMALIZE_Q
00275     if (qrd.w() < 0.0)
00276       qrn.vec() = -qrn.vec();
00277 #endif
00278 
00279     J0.block<3,1>(3,3) = qrn.vec();
00280 
00281     // dqdy
00282     qrn.coeffs() = Vector4d(qpmean.z(),-qpmean.w(),-qpmean.x(),qpmean.y());  // qpmean * ds0'/dy
00283     qrn = qrn*qr0;
00284 
00285 #ifdef NORMALIZE_Q
00286     if (qrd.w() < 0.0)
00287       qrn.vec() = -qrn.vec();
00288 #endif
00289 
00290     J0.block<3,1>(3,4) = qrn.vec();
00291 
00292     // dqdz
00293     qrn.coeffs() = Vector4d(-qpmean.y(),qpmean.x(),-qpmean.w(),qpmean.z());  // qpmean * ds0'/dz
00294     qrn = qrn*qr0;
00295 
00296 #ifdef NORMALIZE_Q
00297     if (qrd.w() < 0.0)
00298       qrn.vec() = -qrn.vec();
00299 #endif
00300 
00301     J0.block<3,1>(3,5) = qrn.vec();
00302 
00303     // transpose
00304     J0t = J0.transpose();
00305 
00306     //  cout << endl << "J0 " << ndr << endl << J0 << endl;
00307 
00308     // Jacobians wrt second frame parameters
00309     // translational part of 0p1 wrt translational vars of p1
00310     // this is just R0'  [from 0t1 = R0'(t1 - t0)]
00311     J1.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);
00312 
00313     // translational part of 0p1 wrt rotational vars of p1: zero
00314     J1.block<3,3>(0,3).setZero();
00315 
00316     // rotational part of 0p1 wrt translation vars of p0 => zero
00317     J1.block<3,3>(3,0).setZero();
00318 
00319 
00320     // rotational part of 0p1 wrt rotational vars of p0
00321     // from 0q1 = q0'*s1*q1
00322 
00323     Eigen::Quaternion<double> qrc;
00324     qrc.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
00325     qrc = qpmean*qrc*qr1;       // mean' * qr0' * qr1
00326     qrc.normalize();
00327 
00328     //    cout << endl << "QRC  : " << qrc.coeffs().transpose() << endl;
00329 
00330     double dq = 1.0e-8;
00331     double wdq = 1.0 - dq*dq;
00332     qr1.coeffs() = Vector4d(dq,0,0,wdq);
00333     //    cout << "QRC+x: " << (qrc*qr1).coeffs().transpose() << endl;    
00334     //    cout << "QRdx:  " << ((qrc*qr1).coeffs().transpose() - qrc.coeffs().transpose())/dq << endl;
00335 
00336     // dqdx
00337     qrn.coeffs() = Vector4d(1,0,0,0);
00338     qrn = qrc*qrn;
00339 
00340     //    cout << "J1dx:  " << qrn.coeffs().transpose() << endl;
00341 
00342 #ifdef NORMALIZE_Q
00343     if (qrc.w() < 0.0)
00344       qrn.vec() = -qrn.vec();
00345 #endif
00346 
00347     J1.block<3,1>(3,3) = qrn.vec();
00348 
00349     // dqdy
00350     qrn.coeffs() = Vector4d(0,1,0,0);
00351     qrn = qrc*qrn;
00352 
00353 #ifdef NORMALIZE_Q
00354     if (qrc.w() < 0.0)
00355       qrn.vec() = -qrn.vec();
00356 #endif
00357 
00358     J1.block<3,1>(3,4) = qrn.vec();
00359 
00360     // dqdz
00361     qrn.coeffs() = Vector4d(0,0,1,0);
00362     qrn = qrc*qrn;
00363 
00364 #ifdef NORMALIZE_Q
00365     if (qrc.w() < 0.0)
00366       qrn.vec() = -qrn.vec();
00367 #endif
00368 
00369     J1.block<3,1>(3,5) = qrn.vec();
00370 
00371     // transpose
00372     J1t = J1.transpose();
00373 
00374     //  cout << endl << "J1 " << nd1 << endl << J1 << endl;
00375 
00376   };
00377 
00378 
00379 
00380   // set up Jacobians
00381   // see Konolige RSS 2010 submission for details
00382 
00383   void ConScale::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes)
00384   {
00385     // node references
00386     Node &n0 = nodes[nd0];
00387     Matrix<double,4,1> &t0 = n0.trans;
00388     Node &n1 = nodes[nd1];
00389     Matrix<double,4,1> &t1 = n1.trans;
00390 
00391     Eigen::Matrix<double,3,1> td = (t1-t0).head(3);
00392 
00393     // Jacobians wrt first frame parameters
00394     //  (ti - tj)^2 - a*kij
00395 
00396     // scale error wrt translational vars t0
00397     J0 = -2.0 * td;
00398 
00399     // scale error wrt translational vars t1
00400     J1 =  2.0 * td;
00401 
00402     // scale error wrt scale variable is just -kij
00403   };
00404 
00405 
00406 
00407   // 
00408   // This hasn't been tested, and is probably wrong.
00409   //
00410 
00411   void ConP3P::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > nodes)
00412   {
00413     // node references
00414     Node nr = nodes[ndr];
00415     Matrix<double,4,1> &tr = nr.trans;
00416     Quaternion<double> &qr = nr.qrot;
00417     Node n1 = nodes[nd1];
00418     Matrix<double,4,1> &t1 = n1.trans;
00419     Quaternion<double> &q1 = n1.qrot;
00420     Node n2 = nodes[nd2];
00421     Matrix<double,4,1> &t2 = n2.trans;
00422     Quaternion<double> &q2 = n2.qrot;
00423 
00424     // calculate J10 -> d(0p1)/d(p0)
00425     // rows are indexed by 0p1 parameters
00426     // cols are indexed by p0 parameters
00427 
00428     // translational part of 0p1 wrt translational vars of p0
00429     // this is just -R0'  [from 0t1 = R0'(t1 - t0)]
00430     J10.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);
00431 
00432     // translational part of 0p1 wrt rotational vars of p0
00433     // dR'/dq * [pw - t]
00434     Matrix<double,3,1> pwt = (t1-tr).head(3);
00435     Matrix<double,3,1> dp = nr.dRdx * pwt; // dR'/dqx * [pw - t]
00436     J10.block<3,1>(0,3) = dp;
00437     dp = nr.dRdy * pwt; // dR'/dqy * [pw - t]
00438     J10.block<3,1>(0,4) = dp;
00439     dp = nr.dRdz * pwt; // dR'/dqz * [pw - t]
00440     J10.block<3,1>(0,5) = dp;
00441 
00442     // rotational part of 0p1 wrt translation vars of p0 => zero
00443     J10.block<3,3>(3,0).setZero();
00444 
00445     // rotational part of 0p1 wrt rotational vars of p0
00446     // from 0q1 = q0'*q1
00447 
00448     // dqdx
00449     double wn = -1.0/qr.w();  // need to switch params for small qr(3)
00450     dp[0] = wn*t1[0]*qr.x() - t1[3];
00451     dp[1] = wn*t1[1]*qr.x() + t1[2];
00452     dp[2] = wn*t1[2]*qr.x() - t1[1];
00453     J10.block<3,1>(3,3) = dp;
00454 
00455     // dqdy
00456     dp[0] = wn*t1[0]*qr.y() - t1[2];
00457     dp[1] = wn*t1[1]*qr.y() - t1[3];
00458     dp[2] = wn*t1[2]*qr.y() + t1[0];
00459     J10.block<3,1>(3,4) = dp;
00460 
00461     // dqdz
00462     dp[0] = wn*t1[0]*qr.z() + t1[1];
00463     dp[1] = wn*t1[1]*qr.z() - t1[0];
00464     dp[2] = wn*t1[2]*qr.z() - t1[3];
00465     J10.block<3,1>(3,5) = dp;
00466 
00467     // whew, J10 done...
00468 
00469     // J20 is similar, just using p2
00470     // translational part of 0p2 wrt translational vars of p0
00471     // this is just -R0'  [from 0t2 = R0'(t2 - t0)]
00472     J20.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);
00473 
00474     // translational part of 0p2 wrt rotational vars of p0
00475     // dR'/dq * [pw - t]
00476     pwt = (t2-tr).head(3);
00477     dp = nr.dRdx * pwt; // dR'/dqx * [pw - t]
00478     J20.block<3,1>(0,3) = dp;
00479     dp = nr.dRdy * pwt; // dR'/dqy * [pw - t]
00480     J20.block<3,1>(0,4) = dp;
00481     dp = nr.dRdz * pwt; // dR'/dqz * [pw - t]
00482     J20.block<3,1>(0,5) = dp;
00483 
00484     // rotational part of 0p1 wrt translation vars of p0 => zero
00485     J20.block<3,3>(3,0).setZero();
00486 
00487     // rotational part of 0p2 wrt rotational vars of p0
00488     // from 0q1 = q0'*q2
00489 
00490     // dqdx
00491     wn = -1.0/qr.w();  // need to switch params for small qr(3)
00492     dp[0] = wn*q2.x()*qr.x() - q2.w();
00493     dp[1] = wn*q2.y()*qr.x() + q2.z();
00494     dp[2] = wn*q2.z()*qr.x() - q2.y();
00495     J20.block<3,1>(3,3) = dp;
00496 
00497     // dqdy
00498     dp[0] = wn*q2.x()*qr.y() - q2.z();
00499     dp[1] = wn*q2.y()*qr.y() - q2.w();
00500     dp[2] = wn*q2.z()*qr.y() + q2.x();
00501     J20.block<3,1>(3,4) = dp;
00502 
00503     // dqdz
00504     dp[0] = wn*q2.x()*qr.z() + q2.y();
00505     dp[1] = wn*q2.y()*qr.z() - q2.x();
00506     dp[2] = wn*q2.z()*qr.z() - q2.w();
00507     J20.block<3,1>(3,5) = dp;
00508 
00509     // calculate J11 -> d(0p1)/d(p1)
00510     // rows are indexed by 0p1 parameters
00511     // cols are indexed by p1 parameters
00512 
00513     // translational part of 0p1 wrt translational vars of p1
00514     // this is just R0'  [from 0t1 = R0'(t1 - t0)]
00515     J11.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);
00516 
00517     // translational part of 0p1 wrt rotational vars of p1 => zero
00518     J11.block<3,3>(0,3).setZero();
00519 
00520     // rotational part of 0p1 wrt translation vars of p1 => zero
00521     J11.block<3,3>(3,0).setZero();
00522 
00523     // rotational part of 0p1 wrt rotational vars of p1
00524     // from 0q1 = q0'*q1
00525 
00526     // dqdx1
00527     wn = 1.0/q1.w();  // need to switch params for small q1(3)
00528     dp[0] = wn*qr.x()*q1.x() + qr.w();
00529     dp[1] = wn*qr.y()*q1.x() - qr.z();
00530     dp[2] = wn*qr.z()*q1.x() + qr.y();
00531     J11.block<3,1>(3,3) = dp;
00532 
00533     // dqdy1
00534     dp[0] = wn*qr.x()*q1.y() + qr.z();
00535     dp[1] = wn*qr.y()*q1.y() + qr.w();
00536     dp[2] = wn*qr.z()*q1.y() - qr.x();
00537     J11.block<3,1>(3,4) = dp;
00538 
00539     // dqdz1
00540     dp[0] = wn*qr.x()*q1.z() - qr.y();
00541     dp[1] = wn*qr.y()*q1.z() + qr.x();
00542     dp[2] = wn*qr.z()*q1.z() + qr.w();
00543     J11.block<3,1>(3,5) = dp;
00544 
00545     // whew, J11 done...    
00546 
00547     // J22 is similar to J11
00548     // rows are indexed by 0p2 parameters
00549     // cols are indexed by p2 parameters
00550 
00551     // translational part of 0p2 wrt translational vars of p2
00552     // this is just R0'  [from 0t2 = R0'(t2 - t0)]
00553     J22.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);
00554 
00555     // translational part of 0p2 wrt rotational vars of p2 => zero
00556     J22.block<3,3>(0,3).setZero();
00557 
00558     // rotational part of 0p2 wrt translation vars of p2 => zero
00559     J22.block<3,3>(3,0).setZero();
00560 
00561     // rotational part of 0p2 wrt rotational vars of p2
00562     // from 0q2 = q0'*q2
00563 
00564     // dqdx2
00565     wn = 1.0/q2.w();  // need to switch params for small q2.w()
00566     dp[0] = wn*qr.x()*q2.x() + qr.w();
00567     dp[1] = wn*qr.y()*q2.x() - qr.z();
00568     dp[2] = wn*qr.z()*q2.x() + qr.y();
00569     J22.block<3,1>(3,3) = dp;
00570 
00571     // dqdy1
00572     dp[0] = wn*qr.x()*q2.y() + qr.z();
00573     dp[1] = wn*qr.y()*q2.y() + qr.w();
00574     dp[2] = wn*qr.z()*q2.y() - qr.x();
00575     J22.block<3,1>(3,4) = dp;
00576 
00577     // dqdz1
00578     dp[0] = wn*qr.x()*q2.z() - qr.y();
00579     dp[1] = wn*qr.y()*q2.z() + qr.x();
00580     dp[2] = wn*qr.z()*q2.z() + qr.w();
00581     J22.block<3,1>(3,5) = dp;
00582 
00583     // whew, J22 done...    
00584 
00585   };
00586 
00587 
00588   // error function
00589   inline double ConP2::calcErr(const Node &nd0, const Node &nd1)
00590     { 
00591       Quaternion<double> q0p,q1;
00592       q0p.vec()   = -nd0.qrot.coeffs().head(3); // invert quaternion
00593       q0p.w()     =  nd0.qrot.w();
00594       q1          =  nd1.qrot;
00595       err.block<3,1>(0,0) = nd0.w2n * nd1.trans - tmean;
00596 
00597       //      cout << endl;
00598       //      cout << q0p.coeffs().transpose() << endl;
00599       //      cout << q1.coeffs().transpose() << endl;
00600       //      cout << qpmean.coeffs().transpose() << endl;
00601 
00602       q1 = qpmean * q0p * q1;
00603 
00604       //      cout << q1.coeffs().transpose() << endl << endl;
00605 
00606 // this seems to mess up convergence...
00607 #ifdef NORMALIZE_Q                              
00608       if (q1.w() < 0.0)
00609         err.block<3,1>(3,0) = -q1.vec(); // normalized
00610       else
00611 #endif
00612         err.block<3,1>(3,0) = q1.vec(); // do we have to normalize w???
00613       //      cout << endl << "Error: " << err.transpose() << endl << endl;
00614       return err.dot(prec * err);
00615     }
00616 
00617 
00618   // error function for distance cost
00619   double ConP2::calcErrDist(const Node &nd0, const Node &nd1)
00620     { 
00621       Vector3d derr;
00622       Quaternion<double> q0p,q1;
00623       q0p.vec()   = -nd0.qrot.vec(); // invert quaternion
00624       q0p.w()     =  nd0.qrot.w();
00625       q1          =  nd1.qrot;
00626       derr = nd0.w2n * nd1.trans - tmean;
00627       return derr.dot(derr);
00628     }
00629 
00630 
00631   // error function
00632   inline double ConScale::calcErr(const Node &nd0, const Node &nd1, double alpha)
00633     { 
00634       err = (nd1.trans - nd0.trans).squaredNorm();
00635       err -= ks*alpha;
00636       //      cout << "Scale err: " << err << endl;
00637       return w*err*err;
00638     }
00639 
00640 
00641   // Adds a node to the system. 
00642   // \return the index of the node added.
00643   int SysSPA::addNode(Eigen::Matrix<double,4,1> &trans, 
00644                       Eigen::Quaternion<double> &qrot,
00645                       bool isFixed)
00646   {
00647     Node nd;
00648     nd.trans = trans;
00649     nd.qrot = qrot;
00650     nd.isFixed = isFixed;
00651     nd.setTransform(); // set up world2node transform
00652     nd.setDr(true); // set rotational derivatives
00653     // Should this be local or global?
00654     nd.normRot();//Local();
00655     nodes.push_back(nd);
00656     return nodes.size()-1;
00657   }
00658 
00659 
00660   // add a constraint
00661   // <nd0>, <nd1> are node id's
00662   // <mean> is x,y,th, with th in radians
00663   // <prec> is a 3x3 precision matrix (inverse covariance
00664   // returns true if nodes are found
00665   bool SysSPA::addConstraint(int nd0, int nd1,
00666                              Eigen::Vector3d &tmean,
00667                              Eigen::Quaterniond &qpmean,
00668                              Eigen::Matrix<double,6,6> &prec)
00669   {
00670     if (nd0 >= (int)nodes.size() || nd1 >= (int)nodes.size()) 
00671       return false;
00672     
00673     ConP2 con;
00674     con.ndr = nd0;
00675     con.nd1 = nd1;
00676 
00677     con.tmean = tmean;
00678     Quaternion<double> qr;
00679     qr = qpmean;
00680     qr.normalize();
00681     con.qpmean = qr.inverse(); // inverse of the rotation measurement
00682     con.prec = prec;            
00683 
00684     p2cons.push_back(con);
00685     return true;
00686   }
00687 
00688 
00689   // error measure, squared
00690   // assumes node transforms have already been calculated
00691   // <tcost> is true if we just want the distance offsets
00692   double SysSPA::calcCost(bool tcost)
00693   {
00694     double cost = 0.0;
00695     
00696     // do distance offset
00697     if (tcost)
00698       {
00699         for(size_t i=0; i<p2cons.size(); i++)
00700           {
00701             ConP2 &con = p2cons[i];
00702             double err = con.calcErrDist(nodes[con.ndr],nodes[con.nd1]);
00703             cost += err;
00704           }
00705       }
00706 
00707     // full cost
00708     else 
00709       {
00710         for(size_t i=0; i<p2cons.size(); i++)
00711           {
00712             ConP2 &con = p2cons[i];
00713             double err = con.calcErr(nodes[con.ndr],nodes[con.nd1]);
00714             cost += err;
00715           }
00716         if (scons.size() > 0)       // can have zero scale constraints
00717           for(size_t i=0; i<scons.size(); i++)
00718             {
00719               ConScale &con = scons[i];
00720               double err = con.calcErr(nodes[con.nd0],nodes[con.nd1],scales[con.sv]);
00721               cost += err;
00722             }
00723       }
00724 
00725     return cost;
00726   }
00727 
00728 
00729   // Set up linear system, from RSS submission (konolige 2010)
00730   // This is a relatively compact version of the algorithm! 
00731   // but not Jacobians
00732 
00733   void SysSPA::setupSys(double sLambda)
00734   {
00735     // set matrix sizes and clear
00736     // assumes scales vars are all free
00737     int nFree = nodes.size() - nFixed;
00738     int nscales = scales.size();
00739     A.setZero(6*nFree+nscales,6*nFree+nscales);
00740     B.setZero(6*nFree+nscales);
00741     VectorXi dcnt(nFree);
00742     dcnt.setZero(nFree);
00743 
00744     // lambda augmentation
00745     double lam = 1.0 + sLambda;
00746 
00747     // loop over P2 constraints
00748     for(size_t pi=0; pi<p2cons.size(); pi++)
00749       {
00750         ConP2 &con = p2cons[pi];
00751         con.setJacobians(nodes);
00752 
00753         // add in 4 blocks of A; actually just need upper triangular
00754         // i0 < i1
00755         int i0 = 6*(con.ndr-nFixed); // will be negative if fixed
00756         int i1 = 6*(con.nd1-nFixed); // will be negative if fixed
00757         
00758         if (i0>=0)
00759           {
00760             A.block<6,6>(i0,i0) += con.J0t * con.prec * con.J0;
00761             dcnt(con.ndr - nFixed)++;
00762           }
00763         if (i1>=0)
00764           {
00765             dcnt(con.nd1 - nFixed)++;
00766             Matrix<double,6,6> tp = con.prec * con.J1;
00767             A.block<6,6>(i1,i1) += con.J1t * tp;
00768             if (i0>=0)
00769               {
00770                 A.block<6,6>(i0,i1) += con.J0t * con.prec * con.J1;
00771                 A.block<6,6>(i1,i0) += con.J1t * con.prec * con.J0;
00772               }
00773           }
00774 
00775         // add in 2 blocks of B
00776         if (i0>=0)
00777           B.block<6,1>(i0,0) -= con.J0t * con.prec * con.err;
00778         if (i1>=0)
00779           B.block<6,1>(i1,0) -= con.J1t * con.prec * con.err;
00780       } // finish P2 constraints
00781 
00782     // loop over Scale constraints
00783     if (scons.size() > 0)       // could be zero
00784       for(size_t pi=0; pi<scons.size(); pi++)
00785         {
00786           ConScale &con = scons[pi];
00787           con.setJacobians(nodes);
00788         // add in 4 blocks of A for t0, t1; actually just need upper triangular
00789         // i0 < i1
00790         int i0 = 6*(con.nd0-nFixed); // will be negative if fixed
00791         int i1 = 6*(con.nd1-nFixed); // will be negative if fixed
00792         
00793         if (i0>=0)
00794           {
00795             A.block<3,3>(i0,i0) += con.w * con.J0 * con.J0.transpose();
00796           }
00797         if (i1>=0)
00798           {
00799             A.block<3,3>(i1,i1) += con.w * con.J1 * con.J1.transpose();
00800             if (i0>=0)
00801               {
00802                 A.block<3,3>(i0,i1) += con.w * con.J0 * con.J1.transpose();
00803                 A.block<3,3>(i1,i0) = A.block<3,3>(i0,i1).transpose();
00804               }
00805           }
00806 
00807         // add in 2 blocks of B
00808         if (i0>=0)
00809           B.block<3,1>(i0,0) -= con.w * con.J0 * con.err;
00810         if (i1>=0)
00811           B.block<3,1>(i1,0) -= con.w * con.J1 * con.err;
00812 
00813         // scale variable, add in 5 blocks; actually just need upper triangular
00814         int is = 6*nFree+con.sv;
00815         A(is,is) += con.w * con.ks * con.ks;
00816         if (i0>=0)
00817           {
00818             A.block<3,1>(i0,is) += con.w * con.J0 * -con.ks;
00819             A.block<1,3>(is,i0) = A.block<3,1>(i0,is).transpose();
00820           }
00821         if (i1>=0)
00822           {
00823             A.block<3,1>(i1,is) += con.w * con.J1 * -con.ks;
00824             A.block<1,3>(is,i1) = A.block<3,1>(i1,is).transpose();
00825           }
00826 
00827         // add in scale block of B
00828         B(is) -= con.w * (-con.ks) * con.err;
00829 
00830       } // finish Scale constraints
00831 
00832 
00833     // augment diagonal
00834     A.diagonal() *= lam;
00835 
00836     // check the matrix and vector
00837     for (int i=0; i<6*nFree; i++)
00838       for (int j=0; j<6*nFree; j++)
00839         if (isnan(A(i,j)) ) { printf("[SetupSys] NaN in A\n"); *(int *)0x0 = 0; }
00840 
00841     for (int j=0; j<6*nFree; j++)
00842       if (isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }
00843 
00844     int ndc = 0;
00845     for (int i=0; i<nFree; i++)
00846       if (dcnt(i) == 0) ndc++;
00847 
00848     if (ndc > 0)
00849       cout << "[SetupSys] " << ndc << " disconnected nodes" << endl;
00850   }
00851 
00852 
00853   // Set up sparse linear system; see setupSys for algorithm.
00854   // Currently doesn't work with scale variables
00855   void SysSPA::setupSparseSys(double sLambda, int iter, int sparseType)
00856   {
00857     // set matrix sizes and clear
00858     // assumes scales vars are all free
00859     int nFree = nodes.size() - nFixed;
00860 
00861     //    long long t0, t1, t2, t3;
00862     //    t0 = utime();
00863 
00864     if (iter == 0)
00865       csp.setupBlockStructure(nFree); // initialize CSparse structures
00866     else
00867       csp.setupBlockStructure(0); // zero out CSparse structures
00868 
00869     //    t1 = utime();
00870 
00871     VectorXi dcnt(nFree);
00872     dcnt.setZero(nFree);
00873 
00874     // lambda augmentation
00875     double lam = 1.0 + sLambda;
00876 
00877     // loop over P2 constraints
00878     for(size_t pi=0; pi<p2cons.size(); pi++)
00879       {
00880         ConP2 &con = p2cons[pi];
00881         con.setJacobians(nodes);
00882 
00883         // add in 4 blocks of A; actually just need upper triangular
00884         int i0 = con.ndr-nFixed; // will be negative if fixed
00885         int i1 = con.nd1-nFixed; // will be negative if fixed
00886         
00887         if (i0>=0)
00888           {
00889            Matrix<double,6,6> m = con.J0t*con.prec*con.J0;
00890             csp.addDiagBlock(m,i0);
00891             dcnt(con.ndr - nFixed)++;
00892           }
00893         if (i1>=0)
00894           {
00895             dcnt(con.nd1 - nFixed)++;
00896             Matrix<double,6,6> tp = con.prec * con.J1;
00897             Matrix<double,6,6> m = con.J1t * tp;
00898             csp.addDiagBlock(m,i1);
00899             if (i0>=0)
00900               {
00901                 Matrix<double,6,6> m2 = con.J0t * tp;
00902                 if (i1 < i0)
00903                   {
00904                     m = m2.transpose();
00905                     csp.addOffdiagBlock(m,i1,i0);
00906                   }
00907                 else
00908                   csp.addOffdiagBlock(m2,i0,i1);
00909               }
00910           }
00911 
00912         // add in 2 blocks of B
00913         if (i0>=0)
00914           csp.B.block<6,1>(i0*6,0) -= con.J0t * con.prec * con.err;
00915         if (i1>=0)
00916           csp.B.block<6,1>(i1*6,0) -= con.J1t * con.prec * con.err;
00917       } // finish P2 constraints
00918 
00919     //    t2 = utime();
00920 
00921     // set up sparse matrix structure from blocks
00922     if (sparseType == SBA_BLOCK_JACOBIAN_PCG)
00923       csp.incDiagBlocks(lam);   // increment diagonal block
00924     else
00925       csp.setupCSstructure(lam,iter==0); 
00926 
00927     //    t3 = utime();
00928 
00929     //    printf("\n[SetupSparseSys] Block: %0.1f   Cons: %0.1f  CS: %0.1f\n",
00930     //           (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
00931 
00932     int ndc = 0;
00933     for (int i=0; i<nFree; i++)
00934       if (dcnt(i) == 0) ndc++;
00935 
00936     if (ndc > 0)
00937       cout << "[SetupSparseSys] " << ndc << " disconnected nodes" << endl;
00938   }
00939   
00940 
00949 
00950   int SysSPA::doSPA(int niter, double sLambda, int useCSparse, double initTol,
00951                       int maxCGiters)
00952   {
00953     Node::initDr();
00954     int nFree = nodes.size() - nFixed; // number of free nodes
00955 
00956     // number of nodes
00957     int ncams = nodes.size();
00958     // number of scale variables
00959     int nscales = scales.size();
00960 
00961     // save old scales
00962     vector<double> oldscales;   
00963     oldscales.resize(nscales);
00964     
00965     // initialize vars
00966     if (sLambda > 0.0)          // do we initialize lambda?
00967       lambda = sLambda;
00968 
00969     // set number of constraints
00970     int ncons = p2cons.size();
00971 
00972     // check for fixed frames
00973     for (int i=0; i<ncams; i++)
00974       {
00975         Node &nd = nodes[i];
00976         if (i >= nFixed)
00977           nd.isFixed = false;
00978         else 
00979           nd.isFixed = true;
00980         nd.setTransform();      // set up world-to-node transform for cost calculation
00981         nd.setDr(true);         // always use local angles
00982       }
00983 
00984     // initialize vars
00985     double laminc = 2.0;        // how much to increment lambda if we fail
00986     double lamdec = 0.5;        // how much to decrement lambda if we succeed
00987     int iter = 0;               // iterations
00988     sqMinDelta = 1e-8 * 1e-8;
00989     double cost = calcCost();
00990     if (verbose)
00991       cout << iter << " Initial squared cost: " << cost << " which is " 
00992            << sqrt(cost/ncons) << " rms error" << endl; 
00993 
00994     int good_iter = 0;
00995     for (; iter<niter; iter++)  // loop at most <niter> times
00996     {
00997         // set up and solve linear system
00998         // NOTE: shouldn't need to redo all calcs in setupSys if we 
00999         //   got here from a bad update
01000 
01001         long long t0, t1, t2, t3;
01002         t0 = utime();
01003         if (useCSparse)
01004           setupSparseSys(lambda,iter,useCSparse); // set up sparse linear system
01005         else
01006           setupSys(lambda);     // set up linear system
01007 
01008         // use appropriate linear solver
01009         if (useCSparse == SBA_BLOCK_JACOBIAN_PCG)
01010           {
01011             if (csp.B.rows() != 0)
01012               {
01013                 int iters = csp.doBPCG(maxCGiters,initTol,iter);
01014                 if (verbose)
01015                   cout << "[Block PCG] " << iters << " iterations" << endl;
01016               }
01017           }
01018         else if (useCSparse > 0)
01019         {
01020             bool ok = csp.doChol();
01021             if (!ok)
01022               cout << "[DoSPA] Sparse Cholesky failed!" << endl;
01023         }
01024         else
01025           A.ldlt().solveInPlace(B); // Cholesky decomposition and solution
01026 
01027         // get correct result vector
01028         VectorXd &BB = useCSparse ? csp.B : B;
01029 
01030         // check for convergence
01031         // this is a pretty crummy convergence measure...
01032         double sqDiff = BB.squaredNorm();
01033         if (sqDiff < sqMinDelta) // converged, done...
01034         {
01035           break;
01036         }
01037 
01038         // update the frames
01039         int ci = 0;
01040         for(int i=0; i < ncams; i++)
01041         {
01042             Node &nd = nodes[i];
01043             if (nd.isFixed) continue; // not to be updated
01044             nd.oldtrans = nd.trans; // save in case we don't improve the cost
01045             nd.oldqrot = nd.qrot;
01046             nd.trans.head<3>() += BB.segment<3>(ci);
01047 
01048             Quaternion<double> qr;
01049             qr.vec() = BB.segment<3>(ci+3); 
01050             qr.w() = sqrt(1.0 - qr.vec().squaredNorm());
01051 
01052             Quaternion<double> qrn,qrx;
01053             qrn = nd.qrot;
01054             qr = qrn*qr;        // post-multiply
01055             qr.normalize();
01056             if (qr.w() < 0.0)
01057               nd.qrot.coeffs() = -qr.coeffs();
01058             else
01059               nd.qrot.coeffs() = qr.coeffs();
01060 
01061             nd.setTransform();  // set up projection matrix for cost calculation
01062             nd.setDr(true);     // set rotational derivatives
01063             ci += 6;            // advance B index
01064         }
01065 
01066         // update the scales
01067         ci = 6*nFree;       // head of scale vars
01068         if (nscales > 0)        // could be empty
01069           for(int i=0; i < nscales; i++)
01070           {
01071               oldscales[i] = scales[i];
01072               scales[i] += B(ci);
01073               ci++;
01074           }
01075 
01076 
01077         // new cost
01078         double newcost = calcCost();
01079         if (verbose)
01080           cout << iter << " Updated squared cost: " << newcost << " which is " 
01081            << sqrt(newcost/ncons) << " rms error" << endl;
01082         
01083         // check if we did good
01084         if (newcost < cost) // && iter != 0) // NOTE: iter==0 case is for checking
01085         {
01086             cost = newcost;
01087             lambda *= lamdec;   // decrease lambda
01088             //      laminc = 2.0;       // reset bad lambda factor; not sure if this is a good idea...
01089             good_iter++;
01090         }
01091         else
01092         {
01093             lambda *= laminc;   // increase lambda
01094             laminc *= 2.0;      // increase the increment
01095 
01096             // reset nodes
01097             for(int i=0; i<ncams; i++)
01098             {
01099                 Node &nd = nodes[i];
01100                 if (nd.isFixed) continue; // not to be updated
01101                 nd.trans = nd.oldtrans;
01102                 nd.qrot = nd.oldqrot;
01103                 nd.setTransform(); // set up projection matrix for cost calculation
01104                 nd.setDr(true);
01105             }
01106 
01107             // reset scales
01108             if (nscales > 0)    // could be empty
01109               for(int i=0; i < nscales; i++)
01110                 scales[i] = oldscales[i];
01111 
01112 
01113             cost = calcCost();  // need to reset errors
01114             if (verbose)
01115               cout << iter << " Downdated cost: " << cost << endl;
01116             // NOTE: shouldn't need to redo all calcs in setupSys
01117         }
01118       }
01119 
01120     // return number of iterations performed
01121     return good_iter;
01122 
01123   }
01124 
01125 
01126   // write out the precision matrix for CSparse
01127   void SysSPA::writeSparseA(char *fname, bool useCSparse)
01128   {
01129     ofstream ofs(fname);
01130     if (ofs == NULL)
01131       {
01132         cout << "Can't open file " << fname << endl;
01133         return;
01134       }
01135 
01136     // cameras
01137     if (useCSparse)
01138       {
01139         setupSparseSys(0.0,0,useCSparse);
01140         
01141         int *Ai = csp.A->i;
01142         int *Ap = csp.A->p;
01143         double *Ax = csp.A->x;
01144 
01145         for (int i=0; i<csp.csize; i++)
01146           for (int j=Ap[i]; j<Ap[i+1]; j++)
01147             if (Ai[j] <= i)
01148               ofs << Ai[j] << " " << i << setprecision(16) << " " << Ax[j] << endl;
01149       }
01150     else
01151       {
01152         Eigen::IOFormat pfmt(16);
01153 
01154         int nrows = A.rows();
01155         int ncols = A.cols();
01156     
01157         for (int i=0; i<nrows; i++)
01158           for (int j=i; j<ncols; j++)
01159             {
01160               double a = A(i,j);
01161               if (A(i,j) != 0.0)
01162                 ofs << i << " " << j << setprecision(16) << " " << a << endl;
01163             }
01164       }
01165 
01166     ofs.close();
01167   }
01168 
01169 
01170   // Set up spanning tree initialization
01171   void SysSPA::spanningTree(int node)
01172   {
01173     int nnodes = nodes.size();
01174 
01175     // set up an index from nodes to their constraints
01176     vector<vector<int> > cind;
01177     cind.resize(nnodes);
01178 
01179     for(size_t pi=0; pi<p2cons.size(); pi++)
01180       {
01181         ConP2 &con = p2cons[pi];
01182         int i0 = con.ndr;
01183         int i1 = con.nd1;
01184         cind[i0].push_back(i1);
01185         cind[i1].push_back(i0);        
01186       }
01187 
01188     // set up breadth-first algorithm
01189     VectorXd dist(nnodes);
01190     dist.setConstant(1e100);
01191     if (node >= nnodes)
01192       node = 0;
01193     dist[node] = 0.0;
01194     multimap<double,int> open;  // open list, priority queue - can have duplicates
01195     open.insert(make_pair<double,int>(0.0,node));
01196 
01197     // do breadth-first computation
01198     while (!open.empty())
01199       {
01200         // get top node, remove it
01201         int ni = open.begin()->second;
01202         double di = open.begin()->first;
01203         open.erase(open.begin());
01204         if (di > dist[ni]) continue; // already dealt with
01205 
01206         // update neighbors
01207         Node &nd = nodes[ni];
01208         Matrix<double,3,4> n2w;
01209         transformF2W(n2w,nd.trans,nd.qrot); // from node to world coords
01210 
01211         vector<int> &nns = cind[ni];
01212         for (int i=0; i<(int)nns.size(); i++)
01213           {
01214             ConP2 &con = p2cons[nns[i]];
01215             double dd = con.tmean.norm(); // incremental distance
01216             // neighbor node index
01217             int nn = con.nd1;
01218             if (nn == ni)
01219               nn = con.ndr;
01220             Node &nd2 = nodes[nn];
01221             Vector3d tmean = con.tmean;
01222             Quaterniond qpmean = con.qpmean;
01223             if (nn == con.ndr)       // wrong way, reverse
01224               {
01225                 qpmean = qpmean.inverse();
01226                 tmean = nd.qrot.toRotationMatrix().transpose()*nd2.qrot.toRotationMatrix()*tmean;
01227               }
01228                 
01229             if (dist[nn] > di + dd) // is neighbor now closer?
01230               {
01231                 // set priority queue
01232                 dist[nn] = di+dd;
01233                 open.insert(make_pair<double,int>(di+dd,nn));
01234                 // update initial pose
01235                 Vector4d trans;
01236                 trans.head(3) = tmean;
01237                 trans(3) = 1.0;
01238                 nd2.trans.head(3) = n2w*trans;
01239                 nd2.qrot = qpmean*nd.qrot;
01240                 nd2.normRot();
01241                 nd2.setTransform();
01242                 nd2.setDr(true);
01243               }
01244           }
01245       }
01246     
01247   }
01248   
01249 
01250 }  // namespace sba


sba
Author(s): Kurt Konolige, Helen Oleynikova
autogenerated on Thu Jan 2 2014 12:12:09