viso_mono.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_mono.h"
00023 
00024 using namespace std;
00025 
00026 VisualOdometryMono::VisualOdometryMono (parameters param) : param(param), VisualOdometry((VisualOdometry::parameters)param) {
00027 }
00028 
00029 VisualOdometryMono::~VisualOdometryMono () {
00030 }
00031 
00032 bool VisualOdometryMono::process (uint8_t *I,int32_t* dims,bool replace) {
00033   matcher->pushBack(I,dims,replace);
00034   matcher->matchFeatures(0);
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> VisualOdometryMono::estimateMotion (vector<Matcher::p_match> p_matched) {
00041 
00042   // get number of matches
00043   int32_t N = p_matched.size();
00044   if (N<10)
00045     return vector<double>();
00046    
00047   // create calibration matrix
00048   double K_data[9] = {param.calib.f,0,param.calib.cu,0,param.calib.f,param.calib.cv,0,0,1};
00049   Matrix K(3,3,K_data);
00050     
00051   // normalize feature points and return on errors
00052   Matrix Tp,Tc;
00053   vector<Matcher::p_match> p_matched_normalized = p_matched;
00054   if (!normalizeFeaturePoints(p_matched_normalized,Tp,Tc))
00055     return vector<double>();
00056 
00057   // initial RANSAC estimate of F
00058   Matrix E,F;
00059   inliers.clear();
00060   for (int32_t k=0;k<param.ransac_iters;k++) {
00061 
00062     // draw random sample set
00063     vector<int32_t> active = getRandomSample(N,8);
00064 
00065     // estimate fundamental matrix and get inliers
00066     fundamentalMatrix(p_matched_normalized,active,F);
00067     vector<int32_t> inliers_curr = getInlier(p_matched_normalized,F);
00068 
00069     // update model if we are better
00070     if (inliers_curr.size()>inliers.size())
00071       inliers = inliers_curr;
00072   }
00073   
00074   // are there enough inliers?
00075   if (inliers.size()<10)
00076     return vector<double>();
00077   
00078   // refine F using all inliers
00079   fundamentalMatrix(p_matched_normalized,inliers,F); 
00080   
00081   // denormalise and extract essential matrix
00082   F = ~Tc*F*Tp;
00083   E = ~K*F*K;
00084   
00085   // re-enforce rank 2 constraint on essential matrix
00086   Matrix U,W,V;
00087   E.svd(U,W,V);
00088   W.val[2][0] = 0;
00089   E = U*Matrix::diag(W)*~V;
00090   
00091   // compute 3d points X and R|t up to scale
00092   Matrix X,R,t;
00093   EtoRt(E,K,p_matched,X,R,t);
00094   
00095   // normalize 3d points and remove points behind image plane
00096   X = X/X.getMat(3,0,3,-1);
00097   vector<int32_t> pos_idx;
00098   for (int32_t i=0; i<X.n; i++)
00099     if (X.val[2][i]>0)
00100       pos_idx.push_back(i);
00101   Matrix X_plane = X.extractCols(pos_idx);
00102   
00103   // we need at least 10 points to proceed
00104   if (X_plane.n<10)
00105     return vector<double>();
00106   
00107   // get elements closer than median
00108   double median;
00109   smallerThanMedian(X_plane,median);
00110   
00111   // return error on large median (litte motion)
00112   if (median>param.motion_threshold)
00113     return vector<double>();
00114   
00115   // project features to 2d
00116   Matrix x_plane(2,X_plane.n);
00117   x_plane.setMat(X_plane.getMat(1,0,2,-1),0,0);
00118   
00119   Matrix n(2,1);
00120   n.val[0][0]       = cos(-param.pitch);
00121   n.val[1][0]       = sin(-param.pitch);
00122   Matrix   d        = ~n*x_plane;
00123   double   sigma    = median/50.0;
00124   double   weight   = 1.0/(2.0*sigma*sigma);
00125   double   best_sum = 0;
00126   int32_t  best_idx = 0;
00127 
00128   // find best plane
00129   for (int32_t i=0; i<x_plane.n; i++) {
00130     if (d.val[0][i]>median/param.motion_threshold) {
00131       double sum = 0;
00132       for (int32_t j=0; j<x_plane.n; j++) {
00133         double dist = d.val[0][j]-d.val[0][i];
00134         sum += exp(-dist*dist*weight);
00135       }
00136       if (sum>best_sum) {
00137         best_sum = sum;
00138         best_idx = i;
00139       }
00140     }
00141   }
00142   t = t*param.height/d.val[0][best_idx];
00143   
00144   // compute rotation angles
00145   double ry = asin(R.val[0][2]);
00146   double rx = asin(-R.val[1][2]/cos(ry));
00147   double rz = asin(-R.val[0][1]/cos(ry));
00148   
00149   // return parameter vector
00150   vector<double> tr_delta;
00151   tr_delta.resize(6);
00152   tr_delta[0] = rx;
00153   tr_delta[1] = ry;
00154   tr_delta[2] = rz;
00155   tr_delta[3] = t.val[0][0];
00156   tr_delta[4] = t.val[1][0];
00157   tr_delta[5] = t.val[2][0];
00158   return tr_delta;
00159 }
00160 
00161 Matrix VisualOdometryMono::smallerThanMedian (Matrix &X,double &median) {
00162   
00163   // set distance and index vector
00164   vector<double> dist;
00165   vector<int32_t> idx;
00166   for (int32_t i=0; i<X.n; i++) {
00167     dist.push_back(fabs(X.val[0][i])+fabs(X.val[1][i])+fabs(X.val[2][i]));
00168     idx.push_back(i);
00169   }
00170   
00171   // sort elements
00172   sort(idx.begin(),idx.end(),idx_cmp<vector<double>&>(dist));
00173   
00174   // get median
00175   int32_t num_elem_half = idx.size()/2;
00176   median = dist[idx[num_elem_half]];
00177   
00178   // create matrix containing elements closer than median
00179   Matrix X_small(4,num_elem_half+1);
00180   for (int32_t j=0; j<=num_elem_half; j++)
00181     for (int32_t i=0; i<4; i++)
00182       X_small.val[i][j] = X.val[i][idx[j]];
00183         return X_small;
00184 }
00185 
00186 bool VisualOdometryMono::normalizeFeaturePoints(vector<Matcher::p_match> &p_matched,Matrix &Tp,Matrix &Tc) {
00187   
00188   // shift origins to centroids
00189   double cpu=0,cpv=0,ccu=0,ccv=0;
00190   for (vector<Matcher::p_match>::iterator it = p_matched.begin(); it!=p_matched.end(); it++) {
00191     cpu += it->u1p;
00192     cpv += it->v1p;
00193     ccu += it->u1c;
00194     ccv += it->v1c;
00195   }
00196   cpu /= (double)p_matched.size();
00197   cpv /= (double)p_matched.size();
00198   ccu /= (double)p_matched.size();
00199   ccv /= (double)p_matched.size();
00200   for (vector<Matcher::p_match>::iterator it = p_matched.begin(); it!=p_matched.end(); it++) {
00201     it->u1p -= cpu;
00202     it->v1p -= cpv;
00203     it->u1c -= ccu;
00204     it->v1c -= ccv;
00205   }
00206   
00207   // scale features such that mean distance from origin is sqrt(2)
00208   double sp=0,sc=0;
00209   for (vector<Matcher::p_match>::iterator it = p_matched.begin(); it!=p_matched.end(); it++) {
00210     sp += sqrt(it->u1p*it->u1p+it->v1p*it->v1p);
00211     sc += sqrt(it->u1c*it->u1c+it->v1c*it->v1c);
00212   }
00213   if (fabs(sp)<1e-10 || fabs(sc)<1e-10)
00214     return false;
00215   sp = sqrt(2.0)*(double)p_matched.size()/sp;
00216   sc = sqrt(2.0)*(double)p_matched.size()/sc;
00217   for (vector<Matcher::p_match>::iterator it = p_matched.begin(); it!=p_matched.end(); it++) {
00218     it->u1p *= sp;
00219     it->v1p *= sp;
00220     it->u1c *= sc;
00221     it->v1c *= sc;
00222   }
00223   
00224   // compute corresponding transformation matrices
00225   double Tp_data[9] = {sp,0,-sp*cpu,0,sp,-sp*cpv,0,0,1};
00226   double Tc_data[9] = {sc,0,-sc*ccu,0,sc,-sc*ccv,0,0,1};
00227   Tp = Matrix(3,3,Tp_data);
00228   Tc = Matrix(3,3,Tc_data);
00229   
00230   // return true on success
00231   return true;
00232 }
00233 
00234 void VisualOdometryMono::fundamentalMatrix (const vector<Matcher::p_match> &p_matched,const vector<int32_t> &active,Matrix &F) {
00235   
00236   // number of active p_matched
00237   int32_t N = active.size();
00238   
00239   // create constraint matrix A
00240   Matrix A(N,9);
00241   for (int32_t i=0; i<N; i++) {
00242     Matcher::p_match m = p_matched[active[i]];
00243     A.val[i][0] = m.u1c*m.u1p;
00244     A.val[i][1] = m.u1c*m.v1p;
00245     A.val[i][2] = m.u1c;
00246     A.val[i][3] = m.v1c*m.u1p;
00247     A.val[i][4] = m.v1c*m.v1p;
00248     A.val[i][5] = m.v1c;
00249     A.val[i][6] = m.u1p;
00250     A.val[i][7] = m.v1p;
00251     A.val[i][8] = 1;
00252   }
00253    
00254   // compute singular value decomposition of A
00255   Matrix U,W,V;
00256   A.svd(U,W,V);
00257    
00258   // extract fundamental matrix from the column of V corresponding to the smallest singular value
00259   F = Matrix::reshape(V.getMat(0,8,8,8),3,3);
00260   
00261   // enforce rank 2
00262   F.svd(U,W,V);
00263   W.val[2][0] = 0;
00264   F = U*Matrix::diag(W)*~V;
00265 }
00266 
00267 vector<int32_t> VisualOdometryMono::getInlier (vector<Matcher::p_match> &p_matched,Matrix &F) {
00268 
00269   // extract fundamental matrix
00270   double f00 = F.val[0][0]; double f01 = F.val[0][1]; double f02 = F.val[0][2];
00271   double f10 = F.val[1][0]; double f11 = F.val[1][1]; double f12 = F.val[1][2];
00272   double f20 = F.val[2][0]; double f21 = F.val[2][1]; double f22 = F.val[2][2];
00273   
00274   // loop variables
00275   double u1,v1,u2,v2;
00276   double x2tFx1;
00277   double Fx1u,Fx1v,Fx1w;
00278   double Ftx2u,Ftx2v;
00279   
00280   // vector with inliers
00281   vector<int32_t> inliers;
00282   
00283   // for all matches do
00284   for (int32_t i=0; i<(int32_t)p_matched.size(); i++) {
00285 
00286     // extract matches
00287     u1 = p_matched[i].u1p;
00288     v1 = p_matched[i].v1p;
00289     u2 = p_matched[i].u1c;
00290     v2 = p_matched[i].v1c;
00291     
00292     // F*x1
00293     Fx1u = f00*u1+f01*v1+f02;
00294     Fx1v = f10*u1+f11*v1+f12;
00295     Fx1w = f20*u1+f21*v1+f22;
00296     
00297     // F'*x2
00298     Ftx2u = f00*u2+f10*v2+f20;
00299     Ftx2v = f01*u2+f11*v2+f21;
00300     
00301     // x2'*F*x1
00302     x2tFx1 = u2*Fx1u+v2*Fx1v+Fx1w;
00303     
00304     // sampson distance
00305     double d = x2tFx1*x2tFx1 / (Fx1u*Fx1u+Fx1v*Fx1v+Ftx2u*Ftx2u+Ftx2v*Ftx2v);
00306     
00307     // check threshold
00308     if (fabs(d)<param.inlier_threshold)
00309       inliers.push_back(i);
00310   }
00311 
00312   // return set of all inliers
00313   return inliers;
00314 }
00315 
00316 void VisualOdometryMono::EtoRt(Matrix &E,Matrix &K,vector<Matcher::p_match> &p_matched,Matrix &X,Matrix &R,Matrix &t) {
00317 
00318   // hartley matrices
00319   double W_data[9] = {0,-1,0,+1,0,0,0,0,1};
00320   double Z_data[9] = {0,+1,0,-1,0,0,0,0,0};
00321   Matrix W(3,3,W_data);
00322   Matrix Z(3,3,Z_data); 
00323   
00324   // extract T,R1,R2 (8 solutions)
00325   Matrix U,S,V;
00326   E.svd(U,S,V);
00327   Matrix T  = U*Z*~U;
00328   Matrix Ra = U*W*(~V);
00329   Matrix Rb = U*(~W)*(~V);
00330   
00331   // convert T to t
00332   t = Matrix(3,1);
00333   t.val[0][0] = T.val[2][1];
00334   t.val[1][0] = T.val[0][2];
00335   t.val[2][0] = T.val[1][0];
00336   
00337   // assure determinant to be positive
00338   if (Ra.det()<0) Ra = -Ra;
00339   if (Rb.det()<0) Rb = -Rb;
00340   
00341   // create vector containing all 4 solutions
00342   vector<Matrix> R_vec;
00343   vector<Matrix> t_vec;
00344   R_vec.push_back(Ra); t_vec.push_back( t);
00345   R_vec.push_back(Ra); t_vec.push_back(-t);
00346   R_vec.push_back(Rb); t_vec.push_back( t);
00347   R_vec.push_back(Rb); t_vec.push_back(-t);
00348   
00349   // try all 4 solutions
00350   Matrix X_curr;
00351   int32_t max_inliers = 0;
00352   for (int32_t i=0; i<4; i++) {
00353     int32_t num_inliers = triangulateChieral(p_matched,K,R_vec[i],t_vec[i],X_curr);
00354     if (num_inliers>max_inliers) {
00355       max_inliers = num_inliers;
00356       X = X_curr;
00357       R = R_vec[i];
00358       t = t_vec[i];
00359     }
00360   }
00361 }
00362 
00363 int32_t VisualOdometryMono::triangulateChieral (vector<Matcher::p_match> &p_matched,Matrix &K,Matrix &R,Matrix &t,Matrix &X) {
00364   
00365   // init 3d point matrix
00366   X = Matrix(4,p_matched.size());
00367   
00368   // projection matrices
00369   Matrix P1(3,4);
00370   Matrix P2(3,4);
00371   P1.setMat(K,0,0);
00372   P2.setMat(R,0,0);
00373   P2.setMat(t,0,3);
00374   P2 = K*P2;
00375   
00376   // triangulation via orthogonal regression
00377   Matrix J(4,4);
00378   Matrix U,S,V;
00379   for (int32_t i=0; i<(int)p_matched.size(); i++) {
00380     for (int32_t j=0; j<4; j++) {
00381       J.val[0][j] = P1.val[2][j]*p_matched[i].u1p - P1.val[0][j];
00382       J.val[1][j] = P1.val[2][j]*p_matched[i].v1p - P1.val[1][j];
00383       J.val[2][j] = P2.val[2][j]*p_matched[i].u1c - P2.val[0][j];
00384       J.val[3][j] = P2.val[2][j]*p_matched[i].v1c - P2.val[1][j];
00385     }
00386     J.svd(U,S,V);
00387     X.setMat(V.getMat(0,3,3,3),0,i);
00388   }
00389   
00390   // compute inliers
00391   Matrix  AX1 = P1*X;
00392   Matrix  BX1 = P2*X;
00393   int32_t num = 0;
00394   for (int32_t i=0; i<X.n; i++)
00395     if (AX1.val[2][i]*X.val[3][i]>0 && BX1.val[2][i]*X.val[3][i]>0)
00396       num++;
00397   
00398   // return number of inliers
00399   return num;
00400 }


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