libviso2: viso_stereo.cpp Source File

Go to the documentation of this file.
00001 /*
00002 Copyright 2011. All rights reserved.
00003 Institute of Measurement and Control Systems
00004 Karlsruhe Institute of Technology, Germany
00005 
00006 This file is part of libviso2.
00007 Authors: Andreas Geiger
00008 
00009 libviso2 is free software; you can redistribute it and/or modify it under the
00010 terms of the GNU General Public License as published by the Free Software
00011 Foundation; either version 2 of the License, or any later version.
00012 
00013 libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY
00014 WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
00015 PARTICULAR PURPOSE. See the GNU General Public License for more details.
00016 
00017 You should have received a copy of the GNU General Public License along with
00018 libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin
00019 Street, Fifth Floor, Boston, MA 02110-1301, USA 
00020 */
00021 
00022 #include "viso_stereo.h"
00023 
00024 using namespace std;
00025 
00026 VisualOdometryStereo::VisualOdometryStereo (parameters param) : param(param), VisualOdometry(param) {
00027   matcher->setIntrinsics(param.calib.f,param.calib.cu,param.calib.cv,param.base);
00028 }
00029 
00030 VisualOdometryStereo::~VisualOdometryStereo() {
00031 }
00032 
00033 bool VisualOdometryStereo::process (uint8_t *I1,uint8_t *I2,int32_t* dims,bool replace) {
00034   matcher->pushBack(I1,I2,dims,replace);
00035   if (Tr_valid) matcher->matchFeatures(2,&Tr_delta);
00036   else          matcher->matchFeatures(2);
00037   matcher->bucketFeatures(param.bucket.max_features,param.bucket.bucket_width,param.bucket.bucket_height);                          
00038   p_matched = matcher->getMatches();
00039   return updateMotion();
00040 }
00041 
00042 vector<double> VisualOdometryStereo::estimateMotion (vector<Matcher::p_match> p_matched) {
00043   
00044   // return value
00045   bool success = true;
00046   
00047   // compute minimum distance for RANSAC samples
00048   double width=0,height=0;
00049   for (vector<Matcher::p_match>::iterator it=p_matched.begin(); it!=p_matched.end(); it++) {
00050     if (it->u1c>width)  width  = it->u1c;
00051     if (it->v1c>height) height = it->v1c;
00052   }
00053   double min_dist = min(width,height)/3.0;
00054   
00055   // get number of matches
00056   int32_t N  = p_matched.size();
00057   if (N<6)
00058     return vector<double>();
00059 
00060   // allocate dynamic memory
00061   X          = new double[N];
00062   Y          = new double[N];
00063   Z          = new double[N];
00064   J          = new double[4*N*6];
00065   p_predict  = new double[4*N];
00066   p_observe  = new double[4*N];
00067   p_residual = new double[4*N];
00068 
00069   // project matches of previous image into 3d
00070   for (int32_t i=0; i<N; i++) {
00071     double d = max(p_matched[i].u1p - p_matched[i].u2p,0.0001f);
00072     X[i] = (p_matched[i].u1p-param.calib.cu)*param.base/d;
00073     Y[i] = (p_matched[i].v1p-param.calib.cv)*param.base/d;
00074     Z[i] = param.calib.f*param.base/d;
00075   }
00076 
00077   // loop variables
00078   vector<double> tr_delta;
00079   vector<double> tr_delta_curr;
00080   tr_delta_curr.resize(6);
00081   
00082   // clear parameter vector
00083   inliers.clear();
00084 
00085   // initial RANSAC estimate
00086   for (int32_t k=0;k<param.ransac_iters;k++) {
00087 
00088     // draw random sample set
00089     vector<int32_t> active = getRandomSample(N,3);
00090 
00091     // clear parameter vector
00092     for (int32_t i=0; i<6; i++)
00093       tr_delta_curr[i] = 0;
00094 
00095     // minimize reprojection errors
00096     VisualOdometryStereo::result result = UPDATED;
00097     int32_t iter=0;
00098     while (result==UPDATED) {
00099       result = updateParameters(p_matched,active,tr_delta_curr,1,1e-6);
00100       if (iter++ > 20 || result==CONVERGED)
00101         break;
00102     }
00103 
00104     // overwrite best parameters if we have more inliers
00105     if (result!=FAILED) {
00106       vector<int32_t> inliers_curr = getInlier(p_matched,tr_delta_curr);
00107       if (inliers_curr.size()>inliers.size()) {
00108         inliers = inliers_curr;
00109         tr_delta = tr_delta_curr;
00110       }
00111     }
00112   }
00113   
00114   // final optimization (refinement)
00115   if (inliers.size()>=6) {
00116     int32_t iter=0;
00117     VisualOdometryStereo::result result = UPDATED;
00118     while (result==UPDATED) {     
00119       result = updateParameters(p_matched,inliers,tr_delta,1,1e-8);
00120       if (iter++ > 100 || result==CONVERGED)
00121         break;
00122     }
00123 
00124     // not converged
00125     if (result!=CONVERGED)
00126       success = false;
00127 
00128   // not enough inliers
00129   } else {
00130     success = false;
00131   }
00132 
00133   // release dynamic memory
00134   delete[] X;
00135   delete[] Y;
00136   delete[] Z;
00137   delete[] J;
00138   delete[] p_predict;
00139   delete[] p_observe;
00140   delete[] p_residual;
00141   
00142   // parameter estimate succeeded?
00143   if (success) return tr_delta;
00144   else         return vector<double>();
00145 }
00146 
00147 vector<int32_t> VisualOdometryStereo::getInlier(vector<Matcher::p_match> &p_matched,vector<double> &tr) {
00148 
00149   // mark all observations active
00150   vector<int32_t> active;
00151   for (int32_t i=0; i<(int32_t)p_matched.size(); i++)
00152     active.push_back(i);
00153 
00154   // extract observations and compute predictions
00155   computeObservations(p_matched,active);
00156   computeResidualsAndJacobian(tr,active);
00157 
00158   // compute inliers
00159   vector<int32_t> inliers;
00160   for (int32_t i=0; i<(int32_t)p_matched.size(); i++)
00161     if (pow(p_observe[4*i+0]-p_predict[4*i+0],2)+pow(p_observe[4*i+1]-p_predict[4*i+1],2) +
00162         pow(p_observe[4*i+2]-p_predict[4*i+2],2)+pow(p_observe[4*i+3]-p_predict[4*i+3],2) < param.inlier_threshold*param.inlier_threshold)
00163       inliers.push_back(i);
00164   return inliers;
00165 }
00166 
00167 VisualOdometryStereo::result VisualOdometryStereo::updateParameters(vector<Matcher::p_match> &p_matched,vector<int32_t> &active,vector<double> &tr,double step_size,double eps) {
00168   
00169   // we need at least 3 observations
00170   if (active.size()<3)
00171     return FAILED;
00172   
00173   // extract observations and compute predictions
00174   computeObservations(p_matched,active);
00175   computeResidualsAndJacobian(tr,active);
00176 
00177   // init
00178   Matrix A(6,6);
00179   Matrix B(6,1);
00180 
00181   // fill matrices A and B
00182   for (int32_t m=0; m<6; m++) {
00183     for (int32_t n=0; n<6; n++) {
00184       double a = 0;
00185       for (int32_t i=0; i<4*(int32_t)active.size(); i++) {
00186         a += J[i*6+m]*J[i*6+n];
00187       }
00188       A.val[m][n] = a;
00189     }
00190     double b = 0;
00191     for (int32_t i=0; i<4*(int32_t)active.size(); i++) {
00192       b += J[i*6+m]*(p_residual[i]);
00193     }
00194     B.val[m][0] = b;
00195   }
00196 
00197   // perform elimination
00198   if (B.solve(A)) {
00199     bool converged = true;
00200     for (int32_t m=0; m<6; m++) {
00201       tr[m] += step_size*B.val[m][0];
00202       if (fabs(B.val[m][0])>eps)
00203         converged = false;
00204     }
00205     if (converged)
00206       return CONVERGED;
00207     else
00208       return UPDATED;
00209   } else {
00210     return FAILED;
00211   }
00212 }
00213 
00214 void VisualOdometryStereo::computeObservations(vector<Matcher::p_match> &p_matched,vector<int32_t> &active) {
00215 
00216   // set all observations
00217   for (int32_t i=0; i<(int32_t)active.size(); i++) {
00218     p_observe[4*i+0] = p_matched[active[i]].u1c; // u1
00219     p_observe[4*i+1] = p_matched[active[i]].v1c; // v1
00220     p_observe[4*i+2] = p_matched[active[i]].u2c; // u2
00221     p_observe[4*i+3] = p_matched[active[i]].v2c; // v2
00222   }
00223 }
00224 
00225 void VisualOdometryStereo::computeResidualsAndJacobian(vector<double> &tr,vector<int32_t> &active) {
00226 
00227   // extract motion parameters
00228   double rx = tr[0]; double ry = tr[1]; double rz = tr[2];
00229   double tx = tr[3]; double ty = tr[4]; double tz = tr[5];
00230 
00231   // precompute sine/cosine
00232   double sx = sin(rx); double cx = cos(rx); double sy = sin(ry);
00233   double cy = cos(ry); double sz = sin(rz); double cz = cos(rz);
00234 
00235   // compute rotation matrix and derivatives
00236   double r00    = +cy*cz;          double r01    = -cy*sz;          double r02    = +sy;
00237   double r10    = +sx*sy*cz+cx*sz; double r11    = -sx*sy*sz+cx*cz; double r12    = -sx*cy;
00238   double r20    = -cx*sy*cz+sx*sz; double r21    = +cx*sy*sz+sx*cz; double r22    = +cx*cy;
00239   double rdrx10 = +cx*sy*cz-sx*sz; double rdrx11 = -cx*sy*sz-sx*cz; double rdrx12 = -cx*cy;
00240   double rdrx20 = +sx*sy*cz+cx*sz; double rdrx21 = -sx*sy*sz+cx*cz; double rdrx22 = -sx*cy;
00241   double rdry00 = -sy*cz;          double rdry01 = +sy*sz;          double rdry02 = +cy;
00242   double rdry10 = +sx*cy*cz;       double rdry11 = -sx*cy*sz;       double rdry12 = +sx*sy;
00243   double rdry20 = -cx*cy*cz;       double rdry21 = +cx*cy*sz;       double rdry22 = -cx*sy;
00244   double rdrz00 = -cy*sz;          double rdrz01 = -cy*cz;
00245   double rdrz10 = -sx*sy*sz+cx*cz; double rdrz11 = -sx*sy*cz-cx*sz;
00246   double rdrz20 = +cx*sy*sz+sx*cz; double rdrz21 = +cx*sy*cz-sx*sz;
00247 
00248   // loop variables
00249   double X1p,Y1p,Z1p;
00250   double X1c,Y1c,Z1c,X2c;
00251   double X1cd,Y1cd,Z1cd;
00252 
00253   // for all observations do
00254   for (int32_t i=0; i<(int32_t)active.size(); i++) {
00255 
00256     // get 3d point in previous coordinate system
00257     X1p = X[active[i]];
00258     Y1p = Y[active[i]];
00259     Z1p = Z[active[i]];
00260 
00261     // compute 3d point in current left coordinate system
00262     X1c = r00*X1p+r01*Y1p+r02*Z1p+tx;
00263     Y1c = r10*X1p+r11*Y1p+r12*Z1p+ty;
00264     Z1c = r20*X1p+r21*Y1p+r22*Z1p+tz;
00265     
00266     // weighting
00267     double weight = 1.0;
00268     if (param.reweighting)
00269       weight = 1.0/(fabs(p_observe[4*i+0]-param.calib.cu)/fabs(param.calib.cu) + 0.05);
00270     
00271     // compute 3d point in current right coordinate system
00272     X2c = X1c-param.base;
00273 
00274     // for all paramters do
00275     for (int32_t j=0; j<6; j++) {
00276 
00277       // derivatives of 3d pt. in curr. left coordinates wrt. param j
00278       switch (j) {
00279         case 0: X1cd = 0;
00280                 Y1cd = rdrx10*X1p+rdrx11*Y1p+rdrx12*Z1p;
00281                 Z1cd = rdrx20*X1p+rdrx21*Y1p+rdrx22*Z1p;
00282                 break;
00283         case 1: X1cd = rdry00*X1p+rdry01*Y1p+rdry02*Z1p;
00284                 Y1cd = rdry10*X1p+rdry11*Y1p+rdry12*Z1p;
00285                 Z1cd = rdry20*X1p+rdry21*Y1p+rdry22*Z1p;
00286                 break;
00287         case 2: X1cd = rdrz00*X1p+rdrz01*Y1p;
00288                 Y1cd = rdrz10*X1p+rdrz11*Y1p;
00289                 Z1cd = rdrz20*X1p+rdrz21*Y1p;
00290                 break;
00291         case 3: X1cd = 1; Y1cd = 0; Z1cd = 0; break;
00292         case 4: X1cd = 0; Y1cd = 1; Z1cd = 0; break;
00293         case 5: X1cd = 0; Y1cd = 0; Z1cd = 1; break;
00294       }
00295 
00296       // set jacobian entries (project via K)
00297       J[(4*i+0)*6+j] = weight*param.calib.f*(X1cd*Z1c-X1c*Z1cd)/(Z1c*Z1c); // left u'
00298       J[(4*i+1)*6+j] = weight*param.calib.f*(Y1cd*Z1c-Y1c*Z1cd)/(Z1c*Z1c); // left v'
00299       J[(4*i+2)*6+j] = weight*param.calib.f*(X1cd*Z1c-X2c*Z1cd)/(Z1c*Z1c); // right u'
00300       J[(4*i+3)*6+j] = weight*param.calib.f*(Y1cd*Z1c-Y1c*Z1cd)/(Z1c*Z1c); // right v'
00301     }
00302 
00303     // set prediction (project via K)
00304     p_predict[4*i+0] = param.calib.f*X1c/Z1c+param.calib.cu; // left u
00305     p_predict[4*i+1] = param.calib.f*Y1c/Z1c+param.calib.cv; // left v
00306     p_predict[4*i+2] = param.calib.f*X2c/Z1c+param.calib.cu; // right u
00307     p_predict[4*i+3] = param.calib.f*Y1c/Z1c+param.calib.cv; // right v
00308     
00309     // set residuals
00310     p_residual[4*i+0] = weight*(p_observe[4*i+0]-p_predict[4*i+0]);
00311     p_residual[4*i+1] = weight*(p_observe[4*i+1]-p_predict[4*i+1]);
00312     p_residual[4*i+2] = weight*(p_observe[4*i+2]-p_predict[4*i+2]);
00313     p_residual[4*i+3] = weight*(p_observe[4*i+3]-p_predict[4*i+3]);
00314   }
00315 }
00316