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


libviso2
Author(s): Andreas Geiger, Stephan Wirth
autogenerated on Fri Aug 28 2015 13:37:45