reconstruction.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 "reconstruction.h"
00023 #include <fstream>
00024 
00025 using namespace std;
00026 
00027 Reconstruction::Reconstruction () {
00028   K = Matrix::eye(3);
00029 }
00030 
00031 Reconstruction::~Reconstruction () {
00032 }
00033 
00034 void Reconstruction::setCalibration (FLOAT f,FLOAT cu,FLOAT cv) {
00035   FLOAT K_data[9]       = {f,0,cu,0,f,cv,0,0,1};
00036   K                     = Matrix(3,3,K_data);
00037   FLOAT cam_pitch       = -0.08;
00038   FLOAT cam_height      = 1.6;
00039   Tr_cam_road           = Matrix(4,4);
00040   Tr_cam_road.val[0][0] = 1;
00041   Tr_cam_road.val[1][1] = +cos(cam_pitch);
00042   Tr_cam_road.val[1][2] = -sin(cam_pitch);
00043   Tr_cam_road.val[2][1] = +sin(cam_pitch);
00044   Tr_cam_road.val[2][2] = +cos(cam_pitch);
00045   Tr_cam_road.val[0][3] = 0;
00046   Tr_cam_road.val[1][3] = -cam_height;
00047   Tr_cam_road.val[2][3] = 0;
00048 }
00049 
00050 void Reconstruction::update (vector<Matcher::p_match> p_matched,Matrix Tr,int32_t point_type,int32_t min_track_length,double max_dist,double min_angle) {
00051   
00052   // update transformation vector
00053   Matrix Tr_total_curr;
00054   if (Tr_total.size()==0) Tr_total_curr = Matrix::inv(Tr);
00055   else                    Tr_total_curr = Tr_total.back()*Matrix::inv(Tr);
00056   Tr_total.push_back(Tr_total_curr);
00057   Tr_inv_total.push_back(Matrix::inv(Tr_total_curr));
00058   
00059   // update projection vector
00060   Matrix P_total_curr = K*Matrix::inv(Tr_total_curr).getMat(0,0,2,3);
00061   P_total.push_back(P_total_curr);
00062   
00063   // current frame
00064   int32_t current_frame = Tr_total.size();
00065   
00066   // create index vector
00067   int32_t track_idx_max = 0;
00068   for (vector<Matcher::p_match>::iterator m=p_matched.begin(); m!=p_matched.end(); m++)
00069     if (m->i1p > track_idx_max)
00070       track_idx_max = m->i1p;
00071   for (vector<track>::iterator t=tracks.begin(); t!=tracks.end(); t++)
00072     if (t->last_idx > track_idx_max)
00073       track_idx_max = t->last_idx;
00074   int32_t *track_idx = new int32_t[track_idx_max+1];
00075   for (int32_t i=0; i<=track_idx_max; i++)
00076     track_idx[i] = -1;
00077   for (int32_t i=0; i<tracks.size(); i++)
00078     track_idx[tracks[i].last_idx] = i;
00079   
00080   // associate matches to tracks
00081   for (vector<Matcher::p_match>::iterator m=p_matched.begin(); m!=p_matched.end(); m++) {
00082     
00083     // track index (-1 = no existing track)
00084     int32_t idx = track_idx[m->i1p];
00085     
00086     // add to existing track
00087     if (idx>=0 && tracks[idx].last_frame==current_frame-1) {
00088       
00089       tracks[idx].pixels.push_back(point2d(m->u1c,m->v1c));
00090       tracks[idx].last_frame = current_frame;
00091       tracks[idx].last_idx   = m->i1c;
00092       
00093     // create new track
00094     } else {
00095       track t;
00096       t.pixels.push_back(point2d(m->u1p,m->v1p));
00097       t.pixels.push_back(point2d(m->u1c,m->v1c));
00098       t.first_frame = current_frame-1;
00099       t.last_frame  = current_frame;
00100       t.last_idx    = m->i1c;
00101       tracks.push_back(t);
00102     }
00103   }
00104   
00105   // copy tracks
00106   vector<track> tracks_copy = tracks;
00107   tracks.clear();
00108   
00109   // devise tracks into active or reconstruct 3d points
00110   for (vector<track>::iterator t=tracks_copy.begin(); t!=tracks_copy.end(); t++) {
00111     
00112     // track has been extended
00113     if (t->last_frame==current_frame) {
00114       
00115       // push back to active tracks
00116       tracks.push_back(*t);
00117       
00118     // track lost
00119     } else {
00120       
00121       // add to 3d reconstruction
00122       if (t->pixels.size()>=min_track_length) {
00123         
00124         // 3d point
00125         point3d p;
00126         
00127         // try to init point from first and last track frame
00128         if (initPoint(*t,p)) {
00129           if (pointType(*t,p)>=point_type)
00130             if (refinePoint(*t,p))
00131               if(pointDistance(*t,p)<max_dist && rayAngle(*t,p)>min_angle)
00132                 points.push_back(p);
00133         }
00134       }
00135     }
00136   }
00137   
00138   //cout << "P: " << points.size() << endl;
00139   //testJacobian();
00140   
00141   delete track_idx;
00142 }
00143 
00144 bool Reconstruction::initPoint(const track &t,point3d &p) {
00145   
00146   // projection matrices
00147   Matrix  P1 = P_total[t.first_frame];
00148   Matrix  P2 = P_total[t.last_frame];
00149   
00150   // observations
00151   point2d p1 = t.pixels.front();
00152   point2d p2 = t.pixels.back();
00153   
00154   // triangulation via orthogonal regression
00155   Matrix J(4,4);
00156   Matrix U,S,V;
00157   for (int32_t j=0; j<4; j++) {
00158     J.val[0][j] = P1.val[2][j]*p1.u - P1.val[0][j];
00159     J.val[1][j] = P1.val[2][j]*p1.v - P1.val[1][j];
00160     J.val[2][j] = P2.val[2][j]*p2.u - P2.val[0][j];
00161     J.val[3][j] = P2.val[2][j]*p2.v - P2.val[1][j];
00162   }
00163   J.svd(U,S,V);
00164   
00165   // return false if this point is at infinity
00166   float w = V.val[3][3];
00167   if (fabs(w)<1e-10)
00168     return false;
00169 
00170   // return 3d point
00171   p = point3d(V.val[0][3]/w,V.val[1][3]/w,V.val[2][3]/w);
00172   return true;
00173 }
00174 
00175 bool Reconstruction::refinePoint(const track &t,point3d &p) {
00176   
00177   int32_t num_frames = t.pixels.size();
00178   J         = new FLOAT[6*num_frames];
00179   p_observe = new FLOAT[2*num_frames];
00180   p_predict = new FLOAT[2*num_frames];
00181  
00182   int32_t iter=0;
00183   Reconstruction::result result = UPDATED;
00184   while (result==UPDATED) {     
00185     result = updatePoint(t,p,1,1e-5);
00186     if (iter++ > 20 || result==CONVERGED)
00187       break;
00188   }
00189   
00190   delete J;
00191   delete p_observe;
00192   delete p_predict;
00193   
00194   if (result==CONVERGED)
00195     return true;
00196   else
00197     return false;
00198 }
00199 
00200 double Reconstruction::pointDistance(const track &t,point3d &p) {
00201   int32_t mid_frame = (t.first_frame+t.last_frame)/2;
00202   double dx = Tr_total[mid_frame].val[0][3]-p.x;
00203   double dy = Tr_total[mid_frame].val[1][3]-p.y;
00204   double dz = Tr_total[mid_frame].val[2][3]-p.z;
00205   return sqrt(dx*dx+dy*dy+dz*dz);
00206 }
00207 
00208 double Reconstruction::rayAngle(const track &t,point3d &p) {
00209   Matrix c1 = Tr_total[t.first_frame].getMat(0,3,2,3);
00210   Matrix c2 = Tr_total[t.last_frame].getMat(0,3,2,3);
00211   Matrix pt(3,1);
00212   pt.val[0][0] = p.x;
00213   pt.val[1][0] = p.y;
00214   pt.val[2][0] = p.z;
00215   Matrix v1 = c1-pt;
00216   Matrix v2 = c2-pt;
00217   FLOAT  n1 = v1.l2norm();
00218   FLOAT  n2 = v2.l2norm();
00219   if (n1<1e-10 || n2<1e-10)
00220     return 1000;
00221   v1 = v1/n1;
00222   v2 = v2/n2;
00223   return acos(fabs((~v1*v2).val[0][0]))*180.0/M_PI;
00224 }
00225 
00226 int32_t Reconstruction::pointType(const track &t,point3d &p) {
00227   
00228   // project point to first and last camera coordinates
00229   Matrix x(4,1);
00230   x.val[0][0] = p.x;
00231   x.val[1][0] = p.y;
00232   x.val[2][0] = p.z;
00233   x.val[3][0] = 1;
00234   Matrix x1c = Tr_inv_total[t.first_frame]*x;
00235   Matrix x2c = Tr_inv_total[t.last_frame]*x;
00236   Matrix x2r = Tr_cam_road*x2c;
00237   
00238   // point not visible
00239   if (x1c.val[2][0]<=1 || x2c.val[2][0]<=1)
00240     return -1;
00241   
00242   // below road
00243   if (x2r.val[1][0]>0.5)
00244     return 0;
00245   
00246   // road
00247   if (x2r.val[1][0]>-1)
00248     return 1;
00249   
00250   // obstacle
00251   return 2;
00252 }
00253 
00254 Reconstruction::result Reconstruction::updatePoint(const track &t,point3d &p,const FLOAT &step_size,const FLOAT &eps) {
00255   
00256   // number of frames
00257   int32_t num_frames = t.pixels.size();
00258   
00259   // extract observations
00260   computeObservations(t.pixels);
00261   
00262   // compute predictions
00263   vector<Matrix>::iterator P_begin = P_total.begin()+t.first_frame;
00264   vector<Matrix>::iterator P_end   = P_begin+num_frames-1;
00265   
00266   if (!computePredictionsAndJacobian(P_begin,P_end,p))
00267     return FAILED;
00268   
00269   // init
00270   Matrix A(3,3);
00271   Matrix B(3,1);
00272 
00273   // fill matrices A and B
00274   for (int32_t m=0; m<3; m++) {
00275     for (int32_t n=0; n<3; n++) {
00276       FLOAT a = 0;
00277       for (int32_t i=0; i<2*num_frames; i++)
00278         a += J[i*3+m]*J[i*3+n];
00279       A.val[m][n] = a;
00280     }
00281     FLOAT b = 0;
00282     for (int32_t i=0; i<2*num_frames; i++)
00283       b += J[i*3+m]*(p_observe[i]-p_predict[i]);
00284     B.val[m][0] = b;
00285   }
00286   
00287   // perform elimination
00288   if (B.solve(A)) {
00289     p.x += step_size*B.val[0][0];
00290     p.y += step_size*B.val[1][0];
00291     p.z += step_size*B.val[2][0];
00292     if (fabs(B.val[0][0])<eps && fabs(B.val[1][0])<eps && fabs(B.val[2][0])<eps)
00293       return CONVERGED;
00294     else
00295       return UPDATED;
00296   }
00297   return FAILED;
00298 }
00299 
00300 void Reconstruction::computeObservations(const vector<point2d> &pixels) {
00301   for (int32_t i=0; i<pixels.size(); i++) {
00302     p_observe[i*2+0] = pixels[i].u;
00303     p_observe[i*2+1] = pixels[i].v;
00304   }
00305 }
00306 
00307 bool Reconstruction::computePredictionsAndJacobian(const vector<Matrix>::iterator &P_begin,const vector<Matrix>::iterator &P_end,point3d &p) {
00308   
00309   // for all frames do
00310   int32_t k=0;
00311   for (vector<Matrix>::iterator P=P_begin; P<=P_end; P++) {
00312     
00313     // precompute coefficients
00314     FLOAT a  = P->val[0][0]*p.x+P->val[0][1]*p.y+P->val[0][2]*p.z+P->val[0][3];
00315     FLOAT b  = P->val[1][0]*p.x+P->val[1][1]*p.y+P->val[1][2]*p.z+P->val[1][3];
00316     FLOAT c  = P->val[2][0]*p.x+P->val[2][1]*p.y+P->val[2][2]*p.z+P->val[2][3];
00317     FLOAT cc = c*c;
00318        
00319     // check singularities
00320     if (cc<1e-10)
00321       return false;
00322     
00323     // set jacobian entries
00324     J[k*6+0] = (P->val[0][0]*c-P->val[2][0]*a)/cc;
00325     J[k*6+1] = (P->val[0][1]*c-P->val[2][1]*a)/cc;
00326     J[k*6+2] = (P->val[0][2]*c-P->val[2][2]*a)/cc;
00327     J[k*6+3] = (P->val[1][0]*c-P->val[2][0]*b)/cc;
00328     J[k*6+4] = (P->val[1][1]*c-P->val[2][1]*b)/cc;
00329     J[k*6+5] = (P->val[1][2]*c-P->val[2][2]*b)/cc;
00330        
00331     // set prediction
00332     p_predict[k*2+0] = a/c; // u
00333     p_predict[k*2+1] = b/c; // v
00334     
00335     k++;
00336   }
00337   
00338   // success
00339   return true;
00340 }
00341 
00342 void Reconstruction::testJacobian() {
00343   cout << "=================================" << endl;
00344   cout << "TESTING JACOBIAN" << endl;
00345   FLOAT delta = 1e-5;
00346   vector<Matrix> P;
00347   Matrix A(3,4);
00348   A.setMat(K,0,0);
00349   P.push_back(A);
00350   A.setMat(Matrix::rotMatX(0.1)*Matrix::rotMatY(0.1)*Matrix::rotMatZ(0.1),0,0);
00351   A.val[1][3] = 1;
00352   A.val[1][3] = 0.1;
00353   A.val[1][3] = -1.5;
00354   P.push_back(K*A);
00355   cout << P[0] << endll;
00356   cout << P[1] << endll;
00357   J         = new FLOAT[6*2];
00358   p_observe = new FLOAT[2*2];
00359   p_predict = new FLOAT[2*2];
00360   
00361   point3d p_ref(0.1,0.2,0.3);
00362   
00363   FLOAT p_predict1[4];
00364   FLOAT p_predict2[4];
00365   point3d p1 = p_ref;
00366    
00367   for (int32_t i=0; i<3; i++) {
00368     point3d p2 = p_ref;
00369     if (i==0)      p2.x += delta;
00370     else if (i==1) p2.y += delta;
00371     else           p2.z += delta;
00372     cout << endl << "Checking parameter " << i << ":" << endl;
00373     cout << "param1: "; cout << p1.x << " " << p1.y << " " << p1.z << endl;
00374     cout << "param2: "; cout << p2.x << " " << p2.y << " " << p2.z << endl;
00375     computePredictionsAndJacobian(P.begin(),P.end(),p1);
00376     memcpy(p_predict1,p_predict,4*sizeof(FLOAT));
00377     computePredictionsAndJacobian(P.begin(),P.end(),p2);
00378     memcpy(p_predict2,p_predict,4*sizeof(FLOAT));
00379     for (int32_t j=0; j<4; j++) {
00380       cout << "num: " << (p_predict2[j]-p_predict1[j])/delta;
00381       cout << ", ana: " << J[j*3+i] << endl;
00382     }
00383   }
00384   
00385   delete J;
00386   delete p_observe;
00387   delete p_predict;
00388   cout << "=================================" << endl;
00389 }


libviso2
Author(s): Andreas Geiger, Stephan Wirth
autogenerated on Mon Oct 6 2014 08:40:54