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 QVCAMERAPOSE_H
00026 #define QVCAMERAPOSE_H
00027 
00028 #include<QV3DPointF>
00029 #include<QVEuclideanMapping3>
00030 
00042 class QVCameraPose
00043         {
00044         private:
00045                 QVQuaternion orientation;
00046                 QV3DPointF center;
00047 
00048         public:
00055                 QVCameraPose(const QVQuaternion q = QVQuaternion(), const QV3DPointF &centre = QV3DPointF()):   orientation(q), center(centre)
00056                         {
00057                         Q_ASSERT(orientation.norm2() > 0);
00058                         Q_WARNING(not orientation.containsNaN());
00059                         Q_WARNING(not center.containsNaN());
00060                         }
00061 
00068                 QVCameraPose(const QVVector &v):        orientation(v.mid(0,4)), center(v.mid(4,3))
00069                         {
00070                         Q_ASSERT(orientation.norm2() > 0);
00071                         Q_WARNING(not orientation.containsNaN());
00072                         Q_WARNING(not center.containsNaN());
00073                         }
00074 
00082                 QVCameraPose(const QVEuclideanMapping3 e3):
00083                                 orientation(e3.getRotation()),
00084                                 center(e3.getRotation().inverse().rotate( e3.getTranslation() ) * (-1.0))
00085                         {
00086                         Q_ASSERT(orientation.norm2() > 0);
00087                         Q_WARNING(not orientation.containsNaN());
00088                         Q_WARNING(not center.containsNaN());
00089                         }
00090 
00095                 operator QVEuclideanMapping3 () const
00096                         {
00097                         Q_ASSERT(orientation.norm2() > 0);
00098                         Q_WARNING(not orientation.containsNaN());
00099                         Q_WARNING(not center.containsNaN());
00100 
00101                         return QVEuclideanMapping3(orientation, orientation.rotate(center) * (-1.0));           
00102                         }
00103 
00109                 operator QVVector () const
00110                         {
00111                         Q_ASSERT(orientation.norm2() > 0);
00112                         Q_WARNING(not orientation.containsNaN());
00113                         Q_WARNING(not center.containsNaN());
00114 
00115                         QVVector result(7);
00116 
00117                         for(int j = 0; j < 4; j++)
00118                                 result[j] = orientation[j];
00119 
00120                         for(int j = 0; j < 3; j++)
00121                                 result[4+j] = center[j];
00122 
00123                         return result;
00124                         }
00125 
00129                 bool containsNaN()      const   { return orientation.containsNaN() or center.containsNaN(); }
00130 
00134                 bool operator==(const QVCameraPose &other) const
00135                         {
00136                         Q_ASSERT(orientation.norm2() > 0);
00137                         Q_WARNING(not orientation.containsNaN());
00138                         Q_WARNING(not center.containsNaN());
00139 
00140                         return (orientation == other.orientation) and (center == other.center);
00141                         };
00142 
00146                 bool operator!=(const QVCameraPose &other) const
00147                         {
00148                         Q_ASSERT(orientation.norm2() > 0);
00149                         Q_WARNING(not orientation.containsNaN());
00150                         Q_WARNING(not center.containsNaN());
00151 
00152                         return (orientation != other.orientation) or (center != other.center);
00153                         };
00154 
00158                 QVQuaternion getOrientation()   const   { return orientation; }
00159 
00163                 QV3DPointF getCenter()          const   { return center; }
00164 
00169                 QPointF project(const QV3DPointF &p3D)          const
00170                         {
00171                         Q_ASSERT(orientation.norm2() > 0);
00172                         Q_WARNING(not orientation.containsNaN());
00173                         Q_WARNING(not center.containsNaN());
00174 
00175                         return orientation.rotate(p3D - center);
00176                         }
00177 
00182                 QV3DPointF toCameraReferenceCoordinates(const QV3DPointF &p3D)          const
00183                         {
00184                         Q_ASSERT(orientation.norm2() > 0);
00185                         Q_WARNING(not orientation.containsNaN());
00186                         Q_WARNING(not center.containsNaN());
00187 
00188                         return orientation.rotate(p3D - center);
00189                         }
00190 
00195                 QVMatrix toProjectionMatrix() const
00196                         {
00197                         Q_ASSERT(orientation.norm2() > 0);
00198                         Q_WARNING(not orientation.containsNaN());
00199                         Q_WARNING(not center.containsNaN());
00200 
00201                         QVMatrix Rt = QVMatrix::identity(4);
00202                         const QVMatrix R = orientation.toRotationMatrix();
00203                         const QV3DPointF t = orientation.rotate(-center);
00204                         
00205                         for (int i = 0; i < 3; i++)
00206                                 {
00207                                 for (int j = 0; j < 3; j++)
00208                                         Rt(i,j) = R(i,j);
00209                                 Rt(i,3) = t[i];
00210                                 }
00211 
00212                         return Rt;
00213                         }
00214 
00219                 QVCameraPose operator*(const QVCameraPose &other) const         { return compose(other);        };
00220 
00223                 QVCameraPose compose(const QVCameraPose &other) const
00224                         {
00225                         Q_ASSERT(other.orientation.norm2() > 0);
00226                         Q_ASSERT(orientation.norm2() > 0);
00227 
00228                         Q_WARNING(not orientation.containsNaN());
00229                         Q_WARNING(not center.containsNaN());
00230                         Q_WARNING(not other.orientation.containsNaN());
00231                         Q_WARNING(not other.center.containsNaN());
00232 
00233                         /*std::cout << "x1" << std::endl;
00234                         Q_ASSERT((orientation * other.orientation).norm2() > 0);
00235 
00236                         std::cout << "x2" << std::endl;
00237 
00238                         const QVQuaternion q = other.orientation.inverse();
00239 
00240                         std::cout << "x3" << std::endl;*/
00241 
00242                         const QVCameraPose result(orientation * other.orientation, other.center + other.orientation.inverse().rotate(center));
00243 
00244                         #ifdef DEBUG
00245                         if (not (orientation.containsNaN() or center.containsNaN() or other.orientation.containsNaN() or other.center.containsNaN() ) )
00246                                 Q_ASSERT( relativeEuclideanDistance(toProjectionMatrix() * other.toProjectionMatrix(), result.toProjectionMatrix()) < 1e-3);
00247                         #endif // DEBUG
00248 
00249                         //std::cout << "x4" << std::endl;
00250                         return result;
00251                         }
00252 
00255                 #ifdef QVMATRIXALGEBRA_AVAILABLE
00256                 QVCameraPose inverse() const
00257                         {
00258                         Q_ASSERT(orientation.norm2() > 0);
00259                         Q_WARNING(not orientation.containsNaN());
00260                         Q_WARNING(not center.containsNaN());
00261 
00262                         const QVCameraPose result(orientation.inverse(), -orientation.rotate(center));
00263 
00264                         #ifdef DEBUG
00265                         if (not (orientation.containsNaN() or center.containsNaN() ) )
00266                                 Q_ASSERT( relativeEuclideanDistance(toProjectionMatrix().inverse(), result.toProjectionMatrix()) < 1e-6);
00267                         #endif // DEBUG
00268 
00269                         return result;
00270                         }
00271                 #endif // QVMATRIXALGEBRA_AVAILABLE
00272         };
00273 
00274 #include <QMetaType>
00275 Q_DECLARE_METATYPE(QVCameraPose);
00276 
00277 #endif
src/qvmath/qvcamerapose.h
