reconstruction.h
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 #ifndef RECONSTRUCTION_H
00023 #define RECONSTRUCTION_H
00024 
00025 #include <iostream>
00026 #include <stdio.h>
00027 #include <string.h>
00028 #include <stdlib.h>
00029 #define _USE_MATH_DEFINES
00030 #include <math.h>
00031 
00032 #include "matcher.h"
00033 #include "matrix.h"
00034 
00035 class Reconstruction {
00036 
00037 public:
00038   
00039   // constructor
00040   Reconstruction ();
00041   
00042   // deconstructor
00043   ~Reconstruction ();
00044   
00045   // a generic 3d point
00046   struct point3d {
00047     float x,y,z;
00048     point3d () {}
00049     point3d (float x,float y,float z) : x(x),y(y),z(z) {}
00050   };
00051 
00052   // set calibration parameters (intrinsics), must be called at least once
00053   // input: f ....... focal length (assumes fu=fv)
00054   //        cu,cv ... principal point
00055   void setCalibration (FLOAT f,FLOAT cu,FLOAT cv);
00056   
00057   // takes a set of monocular feature matches (flow method) and the egomotion
00058   // estimate between the 2 frames Tr, tries to associate the features with previous
00059   // frames (tracking) and computes 3d points once tracks gets lost.
00060   // point types: 0 ..... everything
00061   //              1 ..... road and above
00062   //              2 ..... only above road
00063   // min_track_length ... min number of frames a point needs to be tracked for reconstruction
00064   // max_dist ........... maximum point distance from camera
00065   // min_angle .......... avoid angles smaller than this for reconstruction (in degrees)
00066   void update (std::vector<Matcher::p_match> p_matched,Matrix Tr,int32_t point_type=1,int32_t min_track_length=2,double max_dist=30,double min_angle=2);
00067   
00068   // return currently computed 3d points (finished tracks)
00069   std::vector<point3d> getPoints() { return points; }
00070 
00071 private:
00072   
00073   struct point2d {
00074     float u,v;
00075     point2d () {}
00076     point2d (float u,float v) : u(u),v(v) {}
00077   };
00078   
00079   struct track {
00080     std::vector<point2d> pixels;
00081     int32_t first_frame;
00082     int32_t last_frame;
00083     int32_t last_idx;
00084   };
00085   
00086   enum result { UPDATED, FAILED, CONVERGED };
00087   
00088   bool    initPoint(const track &t,point3d &p);
00089   bool    refinePoint(const track &t,point3d &p);
00090   double  pointDistance(const track &t,point3d &p);
00091   double  rayAngle(const track &t,point3d &p);
00092   int32_t pointType(const track &t,point3d &p);
00093   result  updatePoint(const track &t,point3d &p,const FLOAT &step_size,const FLOAT &eps);
00094   void    computeObservations(const std::vector<point2d> &p);
00095   bool    computePredictionsAndJacobian(const std::vector<Matrix>::iterator &P_begin,const std::vector<Matrix>::iterator &P_end,point3d &p);
00096   void    testJacobian();
00097   
00098   // calibration matrices
00099   Matrix K,Tr_cam_road;
00100   
00101   std::vector<track>   tracks;
00102   std::vector<Matrix>  Tr_total;
00103   std::vector<Matrix>  Tr_inv_total;
00104   std::vector<Matrix>  P_total;
00105   std::vector<point3d> points;
00106   
00107   FLOAT *J;                     // jacobian
00108   FLOAT *p_observe,*p_predict;  // observed and predicted 2d points
00109 
00110 };
00111 
00112 #endif // RECONSTRUCTION_H


libviso2
Author(s): Andreas Geiger, maintained by: Stephan Wirth/stephan.wirth@uib.es
autogenerated on Tue Jan 7 2014 11:42:14