25 #include "PMatrixBase.hh"
26 #include "RMatrixBase.hh"
27 #include <Base/Debug/Exception.hh>
28 #include <Base/Geometry/Quaternion.hh>
29 #include <Base/Math/BogIO.hh>
49 for (
int row=0; row<3; row++) {
50 (*this)[row][0] = KRt[row][0];
51 (*this)[row][1] = KRt[row][1];
52 (*this)[row][2] = KRt[row][2];
53 (*this)[row][3] = LastCol[row];
63 cnew.
Set(C[0]/C[3], C[1]/C[3], C[2]/C[3]);
66 BIASERR(
"cannot compose camera at infinity");
90 RMatrixBase R = Parametrization2R_(src, param_type);
102 case AXIS_TIMES_ANGLE:
105 BEXCEPTION(
"invalid src vector size "<<src.
Size());
108 axis_angle.
Set(src[3], src[4], src[5]);
115 BEXCEPTION(
"invalid src vector size "<<src.
Size());
118 axis.
Set(src[3], src[4], src[5]);
124 BEXCEPTION(
"invalid src vector size "<<src.
Size());
126 R.
SetXYZ(src[3], src[4], src[5]);
131 BEXCEPTION(
"invalid src vector size "<<src.
Size());
138 BIASERR(
"unknown enum type "<<param_type);
139 BEXCEPTION(
"unkown enum type "<<param_type);
148 BIASERR(
"The size of a P-Matrix has to be 3x4. newsize can't create a "
149 << rows<<
" x "<< cols <<
" PMatrixBase. aborting.");
158 PMATRIX_TYPE *data = vals.
GetData();
159 for (
unsigned int i = 0 ; i < P_MATRIX_ROWS; i++)
160 for (
unsigned int j = 0 ; j < P_MATRIX_COLS; j++)
161 (*
this)[i][j]=*(data++);
171 for (
int i=0;i<3;i++){
173 for (
int j=0;j<3;j++)
174 M[i][j]=(*
this)[i][j];
179 BIASERR(
"error: unable to invert M "<<M<<endl)
199 if (0!=
LoadBogK(filename, K)) result += -1;
200 if (0!=
LoadBogRC(filename, R)) result += -2;
201 if (0!=C.
LoadBogTC(filename)) result += -4;
int LoadBogTC(const std::string &filename)
reads the TC part of a BOG file which is used by Daimler Chrysler for storing Camera center position...
void Set(const T *pv)
copy the array of vectorsize beginning at *T to this->data_
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
void SetXYZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order XYZ.
void TransposeIP()
tranpose this matrix "in place" example: 0 1 2 –> 0 3 6 3 4 5 –> 1 4 7 6 7 8 –> 2 5 8 ...
int LoadBogK(const std::string &filename, Matrix3x3< T > &mat)
int SetFromQuaternion(const Quaternion< ROTATION_MATRIX_TYPE > &q)
Set rotation matrix from a quaternion.
unsigned int Size() const
length of the vector
E_ParametrizationType
description of supported parametrization types
void Set(const Vector3< ROTATION_MATRIX_TYPE > &w, const ROTATION_MATRIX_TYPE phi)
Set from rotation axis w and angle phi (in rad)
void Mult(const Vector3< T > &argvec, Vector3< T > &destvec) const
matrix - vector multiplicate this matrix with Vector3, storing the result in destvec calculates: dest...
RMatrixBase Parametrization2R_(const Vector< double > ¶metrization, const enum E_ParametrizationType param_type) const
void SetFromVector(const Vector< PMATRIX_TYPE > &vals)
Sets P-Matrix from a vector which contains the elements of P row wise.
bool IsAtInfinity() const
is a 'fixed size' quadratic matrix of dim.
int LoadBOG(const std::string &filename)
Load a Daimler .bog fiel and compose a P matrix from it.
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
describes a projective 3D -> 2D mapping in homogenous coordinates
class Vector3 contains a Vector of fixed dim.
void SetFromAxisAngle(Vector3< ROTATION_MATRIX_TYPE > w)
Set from rotation axis * angle (modified Rodrigues vector)
T * GetData() const
get the pointer to the data array of the vector (for faster direct memory access) ...
HomgPoint3D Backproject(const HomgPoint2D &x, double mu=1)
Back projects image point x to 3D space and returns ray on which the 3D space point lies...
void MultIP(const T &scalar)
Implements a 3D rotation matrix.
void Compose(const Matrix3x3< PMATRIX_TYPE > &K, const Matrix3x3< PMATRIX_TYPE > &R, const Vector3< PMATRIX_TYPE > &C)
composes this from K, R and C using P = [ K R' | -K R' C ] with R' = transpose(R) ...
int InvertIP()
In place matrix conversion.
int LoadBogRC(const std::string &filename, Matrix3x3< T > &mat)
PMatrixBase & newsize(int rows, int cols)
setting a new size different from 3x4 is not allowed for the fixed size P-Matrix overloads the genera...
class BIASGeometryBase_EXPORT HomgPoint3D
void SetIdentity()
set the elements of this matrix to the identity matrix (possibly overriding the inherited method) ...