demo.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright 2012. 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 /*
00023   Documented C++ sample code of stereo visual odometry (modify to your needs)
00024   To run this demonstration, download the Karlsruhe dataset sequence
00025   '2010_03_09_drive_0019' from: www.cvlibs.net!
00026   Usage: ./viso2 path/to/sequence/2010_03_09_drive_0019
00027 */
00028 
00029 #include <iostream>
00030 #include <string>
00031 #include <vector>
00032 #include <stdint.h>
00033 
00034 #include <viso_stereo.h>
00035 #include <png++/png.hpp>
00036 
00037 using namespace std;
00038 
00039 int main (int argc, char** argv) {
00040 
00041   // we need the path name to 2010_03_09_drive_0019 as input argument
00042   if (argc<2) {
00043     cerr << "Usage: ./viso2 path/to/sequence/2010_03_09_drive_0019" << endl;
00044     return 1;
00045   }
00046 
00047   // sequence directory
00048   string dir = argv[1];
00049   
00050   // set most important visual odometry parameters
00051   // for a full parameter list, look at: viso_stereo.h
00052   VisualOdometryStereo::parameters param;
00053   
00054   // calibration parameters for sequence 2010_03_09_drive_0019 
00055   param.calib.f  = 645.24; // focal length in pixels
00056   param.calib.cu = 661.96; // principal point (u-coordinate) in pixels
00057   param.calib.cv = 194.13; // principal point (v-coordinate) in pixels
00058   param.base     = 0.5707; // baseline in meters
00059   
00060   // init visual odometry
00061   VisualOdometryStereo viso(param);
00062   
00063   // current pose (this matrix transforms a point from the current
00064   // frame's camera coordinates to the first frame's camera coordinates)
00065   Matrix pose = Matrix::eye(4);
00066     
00067   // loop through all frames i=0:372
00068   for (int32_t i=0; i<373; i++) {
00069 
00070     // input file names
00071     char base_name[256]; sprintf(base_name,"%06d.png",i);
00072     string left_img_file_name  = dir + "/I1_" + base_name;
00073     string right_img_file_name = dir + "/I2_" + base_name;
00074     
00075     // catch image read/write errors here
00076     try {
00077 
00078       // load left and right input image
00079       png::image< png::gray_pixel > left_img(left_img_file_name);
00080       png::image< png::gray_pixel > right_img(right_img_file_name);
00081 
00082       // image dimensions
00083       int32_t width  = left_img.get_width();
00084       int32_t height = left_img.get_height();
00085 
00086       // convert input images to uint8_t buffer
00087       uint8_t* left_img_data  = (uint8_t*)malloc(width*height*sizeof(uint8_t));
00088       uint8_t* right_img_data = (uint8_t*)malloc(width*height*sizeof(uint8_t));
00089       int32_t k=0;
00090       for (int32_t v=0; v<height; v++) {
00091         for (int32_t u=0; u<width; u++) {
00092           left_img_data[k]  = left_img.get_pixel(u,v);
00093           right_img_data[k] = right_img.get_pixel(u,v);
00094           k++;
00095         }
00096       }
00097 
00098       // status
00099       cout << "Processing: Frame: " << i;
00100       
00101       // compute visual odometry
00102       int32_t dims[] = {width,height,width};
00103       if (viso.process(left_img_data,right_img_data,dims)) {
00104       
00105         // on success, update current pose
00106         pose = pose * Matrix::inv(viso.getMotion());
00107       
00108         // output some statistics
00109         double num_matches = viso.getNumberOfMatches();
00110         double num_inliers = viso.getNumberOfInliers();
00111         cout << ", Matches: " << num_matches;
00112         cout << ", Inliers: " << 100.0*num_inliers/num_matches << " %" << ", Current pose: " << endl;
00113         cout << pose << endl << endl;
00114 
00115       } else {
00116         cout << " ... failed!" << endl;
00117       }
00118 
00119       // release uint8_t buffers
00120       free(left_img_data);
00121       free(right_img_data);
00122 
00123     // catch image read errors here
00124     } catch (...) {
00125       cerr << "ERROR: Couldn't read input files!" << endl;
00126       return 1;
00127     }
00128   }
00129   
00130   // output
00131   cout << "Demo complete! Exiting ..." << endl;
00132 
00133   // exit
00134   return 0;
00135 }
00136 


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