1 #include "EParametrization.hh"
3 #include <Base/Debug/Exception.hh>
4 #include <Base/Common/CompareFloatingPoint.hh>
5 #include <Base/Geometry/HomgPoint2D.hh>
6 #include <Base/Geometry/HomgPoint2DCov.hh>
24 Set(EQvec, covariance);
33 Set(epipole, orientation, covariance);
42 EQ2E_(epipole, orientation, EQvec);
43 Set(EQvec, covariance);
51 EQ2E_(epipole, orientation, EQvec);
60 if (EQvec.
size() != 7){
61 BEXCEPTION(
"Invalid vector size (must be 7, " << EQvec.
size() <<
" given)!");
72 BEXCEPTION(
"Dimension of given covariance matrix is invalid "
73 "(7x7 matrix needed, " << Cov.
num_rows() <<
"x"
74 << Cov.
num_cols() <<
" matrix given)!\n");
162 std::ifstream ifs(file.c_str());
176 Save(
const std::string& file)
const
178 std::ofstream ofs(file.c_str());
195 for (
int r=0; r<4; r++){
196 for (
int c=0; c<4; c++){
197 res[r][c] =
Cov_[r+3][c+3];
207 for (
int r=0; r<3; r++){
208 for (
int c=0; c<3; c++){
209 cov[r][c] =
Cov_[r][c];
250 BIASASSERT(
E_.
Size()==7u);
253 double quat_norm = 0.0;
254 for (
int i=3; i<7; i++){
255 quat_norm +=
E_[i] *
E_[i];
258 quat_norm = sqrt(quat_norm);
259 BIASASSERT(!
Equal(quat_norm, 0.0));
260 for (
int i=3; i<7; i++){
265 double ep_norm = 0.0;
266 for (
int i=0; i<3; i++){
267 ep_norm +=
E_[i] *
E_[i];
270 ep_norm = sqrt(ep_norm);
271 BIASASSERT(!
Equal(ep_norm, 0.0));
272 for (
int i=0; i<3; i++){
280 if (update_covariance)
293 tmp_q /= q.ScalarProduct(q);
299 for (
int i=0; i<3; i++) {
300 for (
int j=0; j<3; j++) {
304 for (
int i=0; i<4; i++) {
305 for (
int j=0; j<4; j++) {
306 J[i+3][j+3] = Jq[i][j];
335 is>>temp>>epipole>>temp>>orientation>>temp>>cov;
336 p.
Set(epipole, orientation, cov);
void E2EQ_(const Vector< EP_TYPE > &E, Vector3< EP_TYPE > &epipole, Quaternion< EP_TYPE > &orientation) const
void SetEQ(const Vector< EP_TYPE > &EQvec)
Set this from E parametrization.
Subscript num_cols() const
void Invert()
inverts this, by changing the rotation axis by *=-1
Matrix< T > Transpose() const
transpose function, storing data destination matrix
Vector3< EP_TYPE > GetEpipole3() const
returns the epipole (first 3 entries) part of the essential matrix parametrization vector ...
void ScalarProduct(const Vector3< T > &argvec, T &result) const
scalar product (=inner product) of two vectors, storing the result in result
void OuterProduct(const Vector3< T > &v, Matrix3x3< T > &res) const
outer product, constructs a matrix.
Matrix4x4< EP_TYPE > GetOrientationCov() const
covariance to GetOrientation()
int SetFromQuaternion(const Quaternion< ROTATION_MATRIX_TYPE > &q)
Set rotation matrix from a quaternion.
unsigned int Size() const
length of the vector
Matrix3x3< EP_TYPE > GetEssentialMatrix() const
returns the essential matrix computed from parametrization
int MultVec(const Vector3< QUAT_TYPE > &vec, Vector3< QUAT_TYPE > &res) const
rotates the given Vector qith the quaternion ( q v q* ) the resulting vector is given in res ...
Slim class bundling essential matrix parameterization and associated covariance matrix.
class Vector4 contains a Vector of dim.
void SetZero()
Sets all values to zero.
void EQ2E_(const Vector3< EP_TYPE > &epipole, const Quaternion< EP_TYPE > &orientation, Vector< EP_TYPE > &E) const
Vector< EP_TYPE > E_
first 3 entries relate to the epipole, the last four to the quaternion orientation ...
void Set(const Vector3< EP_TYPE > &epipole, const Quaternion< EP_TYPE > &orientation, const Matrix< EP_TYPE > &covariance)
Set this from epipole, orientation and covariance.
Matrix< EP_TYPE > Cov_
first 3 columns relate to epipole, the last four columns relate to the orientation ...
void SetOrientation(const Quaternion< EP_TYPE > &q)
set orientation part of vector and zero covariance matrix
bool Save(const std::string &file) const
Quaternion< EP_TYPE > GetOrientation() const
returns the quaternion (last 4 entries) part of the essential matrix parametrization vector ...
void Invert()
inverts the direction,
Matrix< EP_TYPE > GetCovarianceMatrix() const
return whole (with orientation and epipole) covariance matrix
is a 'fixed size' quadratic matrix of dim.
std::ostream & operator<<(std::ostream &os, const Array2D< T > &arg)
class Vector3 contains a Vector of fixed dim.
bool Equal(const T left, const T right, const T eps)
comparison function for floating point values See http://www.boost.org/libs/test/doc/components/test_...
bool Load(const std::string &file)
void SetEpipole(const Vector3< EP_TYPE > &epipole)
set epipole part of vector and zero covariance matrix
Implements a 3D rotation matrix.
Subscript num_rows() const
is a 'fixed size' quadratic matrix of dim.
void SetAsCrossProductMatrix(const Vector3< T > &vec)
Sets matrix from vector as cross product matrix of this vector.
class for rotation with axis and angle
void GetEpipole(Vector3< EP_TYPE > &epipole) const
returns the epipole (first 3 entries) part of the essential matrix parametrization vector ...
void Normalize(const bool update_covariance=true)
void SetCovarianceMatrix(const Matrix< EP_TYPE > &Cov)
Sets covariance matrix from Cov.
BIASCommon_EXPORT std::istream & operator>>(std::istream &is, BIAS::TimeStamp &ts)
Standard input operator for TimeStamps.
void GetEpipoleCov(Matrix3x3< EP_TYPE > &cov) const
covariance to GetEpipole(),