QVision: Qt's Image, Video and Computer Vision Library

00001 /*
00002  *      Copyright (C) 2011, 2012. PARP Research Group.
00003  *      <http://perception.inf.um.es>
00004  *      University of Murcia, Spain.
00005  *
00006  *      This file is part of the QVision library.
00007  *
00008  *      QVision is free software: you can redistribute it and/or modify
00009  *      it under the terms of the GNU Lesser General Public License as
00010  *      published by the Free Software Foundation, version 3 of the License.
00011  *
00012  *      QVision is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *      GNU Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with QVision. If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00024 
00025 #ifndef SO3ESSENTIALEVALUATION_H
00026 #define SO3ESSENTIALEVALUATION_H
00027 #ifndef DOXYGEN_IGNORE_THIS
00028 
00029 
00030 #include<qvmath.h>
00031 #include<QVFunction>
00032 #include<QVVector>
00033 #include<QVMatrix>
00034 #include<qvnumericalanalysis.h>
00035 
00036 void so3EssentialEvaluation(const double *pose1, const double *pose2, double *cg0, const double Epsi = 1e-32);
00037 QVMatrix getRotation(const double r1, const double r2, const double r3, const double Epsi);
00038 
00039 class QuadraticFunction2: public QVFunction<QVVector, QVVector>
00040         {
00041         private:
00042                 QVVector evaluate(const QVVector &x)
00043                         {
00044                         const QVVector  v1 = x.mid(0,3),
00045                                         c1 = x.mid(3,3),
00046                                         v2 = x.mid(6,3),
00047                                         c2 = x.mid(9,3);
00048 
00049                         const QVMatrix  R1 = expSO3(v1), R2 = expSO3(v2);
00050                         const QVMatrix  E = R2 * ( (c2-c1) / (c2-c1).norm2() ).crossProductMatrix() * R1.transpose();
00051 
00052                         return QVVector(9, E.getReadData());
00053                         }
00054 
00055         public:
00056                 QuadraticFunction2(): QVFunction<QVVector, QVVector>()
00057                         { }
00058         };
00059 
00060 QVMatrix so3EssentialEvaluation_bis(const QVVector pose1, const QVVector pose2);
00061 void test_so3EssentialEvaluation();
00062 
00063 #endif // DOXYGEN_IGNORE_THIS
00064 #endif // SO3ESSENTIALEVALUATION_H
00065 
00066
src/qvsfm/qvgea/so3EssentialEvaluation.h
