00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00024
00025 #ifndef QVEUCLIDEANMAPPING3_H
00026 #define QVEUCLIDEANMAPPING3_H
00027
00028 #include <QVMatrix>
00029 #include <QV3DPointF>
00030 #include <qvmath.h>
00031 #include <qvprojective.h>
00032
00045 class QVEuclideanMapping3
00046 {
00047 private:
00048 QVQuaternion q;
00049 QV3DPointF t;
00050
00051 public:
00053 QVEuclideanMapping3(): q(), t() { }
00054
00056 QVEuclideanMapping3(const QVEuclideanMapping3 &other): q(other.q), t(other.t) { }
00057
00061 QVEuclideanMapping3(const QVQuaternion &q, const QV3DPointF &t): q(q), t(t) { }
00062
00072 QVEuclideanMapping3(const double quatx, const double quaty, const double quatz, const double quatw,
00073 const double tx, const double ty, const double tz):
00074 q(quatx, quaty, quatz, quatw), t(tx, ty, tz)
00075 { }
00076
00082 QVEuclideanMapping3(const QVVector &v): q(v[0], v[1], v[2], v[3]), t(v[4], v[5], v[6])
00083 { }
00084
00089 QVEuclideanMapping3(const QVMatrix &Rt)
00090 {
00091 const QVMatrix refinedRt = refineExtrinsicCameraMatrixWithPolarDecomposition(Rt);
00092 q = refinedRt.getSubmatrix(0,2,0,2);
00093 t = QV3DPointF(refinedRt(0,3), refinedRt(1,3), refinedRt(2,3));
00094 }
00095
00099 QV3DPointF apply(const QV3DPointF &X) const
00100 {
00101 return q.rotate(X) + t;
00102 }
00103
00109 operator QVVector() const
00110 {
00111 QVVector result(7);
00112
00113 result[0] = q[0];
00114 result[1] = q[1];
00115 result[2] = q[2];
00116 result[3] = q[3];
00117
00118 result[4] = t[0];
00119 result[5] = t[1];
00120 result[6] = t[2];
00121
00122 return result;
00123 }
00124
00127 bool operator ==(const QVEuclideanMapping3 &other) const
00128 {
00129 return (q == other.q and t == other.t);
00130 }
00131
00134 bool operator !=(const QVEuclideanMapping3 &other) const
00135 {
00136 return (q != other.q or t != other.t);
00137 }
00138
00143 QVMatrix toRotationTranslationMatrix() const
00144 {
00145 QVMatrix Rt = QVMatrix::identity(4);
00146 const QVMatrix R = q.toRotationMatrix();
00147
00148 for (int i = 0; i < 3; i++)
00149 {
00150 for (int j = 0; j < 3; j++)
00151 Rt(i,j) = R(i,j);
00152 Rt(i,3) = t[i];
00153 }
00154
00155 return Rt;
00156 }
00157 };
00158 #endif