viso.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.h"
00023 
00024 #include <math.h>
00025 
00026 using namespace std;
00027 
00028 VisualOdometry::VisualOdometry (parameters param) : param(param) {
00029   J         = 0;
00030   p_observe = 0;
00031   p_predict = 0;
00032   matcher   = new Matcher(param.match);
00033   Tr_delta  = Matrix::eye(4);
00034   srand(0);
00035 }
00036 
00037 VisualOdometry::~VisualOdometry () {
00038   delete matcher;
00039 }
00040 
00041 bool VisualOdometry::updateMotion () {
00042   
00043   // estimate motion
00044   vector<double> tr_delta = estimateMotion(p_matched);
00045   
00046   // on failure
00047   if (tr_delta.size()!=6)
00048     return false;
00049   
00050   // set transformation matrix (previous to current frame)
00051   Tr_delta = transformationVectorToMatrix(tr_delta);
00052   
00053   // success
00054   return true;
00055 }
00056 
00057 Matrix VisualOdometry::transformationVectorToMatrix (vector<double> tr) {
00058 
00059   // extract parameters
00060   double rx = tr[0];
00061   double ry = tr[1];
00062   double rz = tr[2];
00063   double tx = tr[3];
00064   double ty = tr[4];
00065   double tz = tr[5];
00066 
00067   // precompute sine/cosine
00068   double sx = sin(rx);
00069   double cx = cos(rx);
00070   double sy = sin(ry);
00071   double cy = cos(ry);
00072   double sz = sin(rz);
00073   double cz = cos(rz);
00074 
00075   // compute transformation
00076   Matrix Tr(4,4);
00077   Tr.val[0][0] = +cy*cz;          Tr.val[0][1] = -cy*sz;          Tr.val[0][2] = +sy;    Tr.val[0][3] = tx;
00078   Tr.val[1][0] = +sx*sy*cz+cx*sz; Tr.val[1][1] = -sx*sy*sz+cx*cz; Tr.val[1][2] = -sx*cy; Tr.val[1][3] = ty;
00079   Tr.val[2][0] = -cx*sy*cz+sx*sz; Tr.val[2][1] = +cx*sy*sz+sx*cz; Tr.val[2][2] = +cx*cy; Tr.val[2][3] = tz;
00080   Tr.val[3][0] = 0;               Tr.val[3][1] = 0;               Tr.val[3][2] = 0;      Tr.val[3][3] = 1;
00081   return Tr;
00082 }
00083 
00084 vector<int32_t> VisualOdometry::getRandomSample(int32_t N,int32_t num) {
00085 
00086   // init sample and totalset
00087   vector<int32_t> sample;
00088   vector<int32_t> totalset;
00089   
00090   // create vector containing all indices
00091   for (int32_t i=0; i<N; i++)
00092     totalset.push_back(i);
00093 
00094   // add num indices to current sample
00095   sample.clear();
00096   for (int32_t i=0; i<num; i++) {
00097     int32_t j = rand()%totalset.size();
00098     sample.push_back(totalset[j]);
00099     totalset.erase(totalset.begin()+j);
00100   }
00101   
00102   // return sample
00103   return sample;
00104 }


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