1 #include <Geometry/Covariance3Dto2DHomg.hh>
3 #include <Base/Geometry/PMatrixBase.hh>
4 #include <Base/Geometry/PoseParametrization.hh>
5 #include <Base/Geometry/HomgPoint2D.hh>
6 #include <Base/Geometry/HomgPoint2DCov.hh>
7 #include <Base/Geometry/HomgPoint3D.hh>
8 #include <Base/Geometry/HomgPoint3DCov.hh>
9 #include <Base/Debug/Exception.hh>
18 K_(K), Parametrization_(par)
37 const int pose_dim = Pose.
Size();
38 const int dim = pose_dim + 3;
41 BEXCEPTION(
"only for homogenized entities (unfinished)");
47 for (
int c=0; c<3; c++){
49 for (
int r=0; r<3; r++){
50 src_cov[r][c] = Cov_X[r][c];
54 for (
int c=0; c<pose_dim; c++){
56 for (
int r=0; r<pose_dim; r++){
57 src_cov[r+3][c+3] = Cov_Pose[r][c];
63 const int num_cols = src_cov.
num_cols();
64 for (
int i=0; i<num_cols; i++) {
65 if (src_cov[i][i] <= 0.0) src_cov[i][i] = 0.0;
69 BIASCDOUT(D_C3DT2D_COV,
"src: "<<src<<
"\nsrc_cov: "<<src_cov<<endl);
70 if (
Transform(src, src_cov, dst, dst_cov)!=0){
73 BIASCDOUT(D_C3DT2D_COV,
"dst: "<<dst<<
"\ndst_cov: "<<dst_cov<<endl);
85 const int pose_size = src.
Size() - 3;
87 for (
int i=0; i<pose_size; i++){
90 BIASCDOUT(D_C3DT2D_COV,
"K: "<<
K_<<
"\tparam: "<<param<<endl);
93 X.
Set(src[0], src[1], src[2], 1.0);
97 if (fabs(dst[2]) < 1e-7)
return -1;
100 const double dst_norm = dst.
NormL2();
101 if (dst_norm < 1e-7)
return -1;
104 BIASCDOUT(D_C3DT2D_COV,
"Pc32: "<<P<<
"\tXc32: "<<X<<
"\txx32: "<<dst<<endl);
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
class representing the covariance matrix of a homogenous point 2D
Subscript num_cols() const
class BIASGeometryBase_EXPORT HomgPoint2DCov
virtual int Transform_(const BIAS::Vector< double > &src, BIAS::Vector< double > &dst) const
src is a vector consisting of [ 3d point, camera center, orientation ] The orientation must be repres...
bool useHomogenizedPoints_
normalize or homogenize ???
int Project(const BIAS::PoseParametrization &Pose, const BIAS::HomgPoint3D &X, const BIAS::HomgPoint3DCov &Cov_X, BIAS::HomgPoint2D &x, BIAS::HomgPoint2DCov &Cov_x) const
compute
double NormL2() const
Return the L2 norm: sqrt(a^1 + a^2 + ...)
void Set(const HOMGPOINT3D_TYPE &x, const HOMGPOINT3D_TYPE &y, const HOMGPOINT3D_TYPE &z)
set elementwise with given 3 euclidean scalar values.
Matrix< T > & newsize(Subscript M, Subscript N)
Slim class bundeling pose parametrization and associated covariance matrix.
unsigned int Size() const
length of the vector
E_ParametrizationType
description of supported parametrization types
Vector< T > & newsize(Subscript N)
Represents 3d pose transformations, parametrized as Euclidean translation and unit quaternion orienta...
enum BIAS::PMatrixBase::E_ParametrizationType Parametrization_
void SetZero()
Sets all values to zero.
const Matrix< PP_TYPE > & GetCovarianceMatrix() const
returns the covariance matrix associated with the CQ vector
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
describes a projective 3D -> 2D mapping in homogenous coordinates
class representing the covariance matrix of a homogenous point 3D
const Vector< PP_TYPE > & GetCQ() const
returns a vector of dimension 7 where first 3 entries are the position and the last 4 entries are the...
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
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) ...
bool IsHomogenized() const
void MakeSymmetric()
componentwise: this = 0.5(this + this^T) yields symmetric matrix only allowed for square shaped matri...
bool IsHomogenized() const
Covariance3Dto2DHomg(const BIAS::KMatrix &K, const enum BIAS::PMatrixBase::E_ParametrizationType par)