25 #include <Base/Common/BIASpragma.hh>
29 #include <Base/Common/CompareFloatingPoint.hh>
30 #include <Base/Geometry/AffineTransf.hh>
31 #include <MathAlgo/Lapack.hh>
34 #include <MathAlgo/SVD.hh>
41 #define STREAM_PRECISION 12
46 if (Svd_ != NULL)
delete Svd_;
63 BIASERR(
"The size of a P-Matrix has to be 3x4. newsize can't create a "
64 <<rows<<
" x "<< cols <<
" P-matrix. aborting.");
77 BIASERR(
" wrong size in constructor! Amat is a "<<Amat.
num_rows()
78 <<
" x "<<Amat.
num_cols()<<
" matrix but must be 3x4."
127 for (
int i=0;i<U.num_rows();i++)
128 for (
int j=0;j<U.num_cols();j++) {
129 SinvUT[i][j] = 1.0/S[i] * U[j][i];
140 if (fabs(Nullvec[3]) > DBL_EPSILON) {
142 C.
Set(Nullvec[0]/Nullvec[3], Nullvec[1]/Nullvec[3], Nullvec[2]/Nullvec[3],
146 C.
Set(Nullvec[0],Nullvec[1],Nullvec[2],Nullvec[3]);
258 if (0!=
GetH(h)) BIASERR(
"could not access H.");
283 if (0!=
GetV(v)) BIASERR(
"could not access V.");
306 if (0!=
GetA(a)) BIASERR(
"could not access A.");
319 int decomposeresult = 0;
328 for (
int i=0; i<3; i++)
329 rho += (*
this)[2][i] * (*this)[2][i];
333 BIASERR(
"degenerate case detected in decomposition. P="<<(*
this)<<endl);
342 for (
int i=0; i<3; i++) {
343 for (
int j=0; j<3; j++)
344 matA[i][j] = Ptmp[i][j];
346 vecB[i] = -Ptmp[i][3];
355 BIASERR(
"error unable to compute Hinf_.");
357 decomposeresult = -1;
377 for (
int col=0; col<3; col++) {
378 z1[col] = Ptmp[0][col];
379 z2[col] = Ptmp[1][col];
381 R_[col][2] = Ptmp[2][col];
384 A_[col] = Ptmp[2][col];
415 for (
int i=0; i<3; i++) {
423 for (
int i=0; i<3; i++){
424 sum[i] +=
R_[i][0] *
R_[i][0] +
R_[i][1] *
R_[i][1] +
R_[i][2] *
R_[i][2];
432 BIASERR(
"warning, numerical instability in PMatrix::Decompose_(): "
433 <<
"R_ is nor orthogonal (not a rotation matrix), scalar "
437 if ( fabs(sum[0]-1.0)>1e-10 || fabs(sum[1]-1.0)>1e-10 ||
438 fabs(sum[2]-1.0)>1e-10 ){
439 BIASERR(
"warning, numerical instability in PMatrix::Decompose_(): "
440 <<
"R_ is not a orthonormal (not a rotation matrix) "
441 <<
"Vector lengthes are"<<sum[0] <<
"," << sum[1] <<
","
450 return decomposeresult;
456 const double& z_depth,
465 res.
Set(p[0]+C[0], p[1]+C[1], p[2]+C[2], C[3]);
474 BIASERR(
"error getting C");
479 hray.
Set(depth*ray[0], depth*ray[1], depth*ray[2], 0.0);
493 viewray[0] = newPoint[0] - center[0];
494 viewray[1] = newPoint[1] - center[1];
495 viewray[2] = newPoint[2] - center[2];
497 double dS = viewray[0] * (*this)[2][0] + viewray[1] * (*this)[2][1] +
498 viewray[2] * (*this)[2][2];
501 newPoint[0] = center[0] - viewray[0];
502 newPoint[1] = center[1] - viewray[1];
503 newPoint[2] = center[2] - viewray[2];
528 R_.
Mult(PlaneVec, WorldPlaneVec);
530 plane[3] = WorldPlaneVec.
NormL2();
564 at.
Mult(Plane, TransformedPlane);
569 if ((dot < PMATRIX_EPSILON) && (dot > -PMATRIX_EPSILON)) {
572 intersection[0] = TransformedPlane[0];
573 intersection[1] = TransformedPlane[1];
574 intersection[2] = TransformedPlane[2] + TransformedPlane[3];
589 for (
register int i=0; i<3; i++){
595 for (
unsigned int j=0; j<3; j++) {
596 for (
unsigned int i=0; i<3; i++) {
597 Hinf_[i][j] = (*this)[i][j];
602 BIASERR(
"error computing Hinf_.");
603 cout <<
"K: "<<K<<endl<<
"R: "<<R<<endl<<
"C: "<<C<<endl;BIASABORT; }
623 cnew.
Set(C[0]/C[3], C[1]/C[3], C[2]/C[3]);
625 for (
unsigned int j=0; j<3; j++) {
626 for (
unsigned int i=0; i<3; i++) {
627 Hinf_[i][j] = (*this)[i][j];
631 if (hres!=0) BIASERR(
"error: unable to compute Hinf_.");
638 BIASERR(
"cannot compose camera at infinity");
648 C.
Set(parametrization[0], parametrization[1], parametrization[2]);
660 return result / result.
NormL2();
686 Kinv.
Mult(point, result);
696 if (cols<0 || cols>10000 || rows<0 || rows>10000 ) {
698 BIASERR(
" image cols x rows = "<<cols<<
" x "<< rows
699 <<
" is to small or to big!");
713 A0[0] = (*this)[2][0];
714 A0[1] = (*this)[2][1];
715 A0[2] = (*this)[2][2];
719 H0[0] = (*this)[0][0];
720 H0[1] = (*this)[0][1];
721 H0[2] = (*this)[0][2];
724 V0[0] = (*this)[1][0];
725 V0[1] = (*this)[1][1];
726 V0[2] = (*this)[1][2];
731 TTh = A0 * ((cols-1)/2.0);
732 TTv = A0 * ((rows-1)/2.0);
738 double imgCenterX = cols/2.0;
740 double imgCenterY = rows/2.0;
744 double opticalCenterX = K[0][2];
745 double opticalCenterY = K[1][2];
746 double bxh = opticalCenterX - imgCenterX;
748 double byh = opticalCenterY - imgCenterY;
751 double skew = K[0][1];
753 double sy = (fx/fy)*sx;
759 os<<setprecision(12);
760 os <<
";CAHV 1.0" << endl;
761 os <<
";Generated by PMatrix::WriteCAHV_10 (JW 2002)" << endl;
762 os <<
"; Cx Cy Cz"<<endl;
763 os <<C[0]<<
" "<<C[1]<<
" "<<C[2]<<endl;
764 os <<
"; Ax Ay Az"<<endl;
765 os <<A0[0]<<
" "<<A0[1]<<
" "<<A0[2]<<endl;
766 os <<
"; Hx Hy Hz"<<endl;
767 os <<H[0]<<
" "<<H[1]<<
" "<<H[2]<<endl;
768 os <<
"; Vx Vy Vz"<<endl;
769 os <<V[0]<<
" "<<V[1]<<
" "<<V[2]<<endl;
771 os <<
"; Radialdistortion rd "<<endl;
773 os <<
"; Pixelsize sx (fixed) sy (ratio)"<<endl;
774 os <<sx<<
" "<<sy<<endl;
775 os <<
"; Image-size: colums rows"<<endl;
776 os <<cols<<
" "<<rows<<endl;
777 os <<
"; Internal parameters:"<<endl;
778 os <<
"; fx = "<< fx <<
" fy = "<< fy <<
" skew = "<< skew << endl;
779 os <<
"; hx = "<< bxh<<
" hy = "<< byh << endl;
790 const unsigned int dimY,
801 }
else if (dimX<dimY) {
814 float alpha = acos( cosAlpha );
829 const unsigned int dimY,
830 const bool & compY) {
836 south2d =
HomgPoint2D(0.5*(
double)dimX, (
double)dimY, 1.0);
840 south2d =
HomgPoint2D((
double)dimX, 0.5*(
double)dimY, 1.0);
849 float alpha = acos( cosAlpha );
856 for(
int i=0; i<3; i++)
857 for(
int j=0; j<4; j++)
859 a[i] = (*this)[i][j];
861 A[i][j] = (*this)[i][j];
885 ProjH[0][0] = PseudoInv[0][0];
886 ProjH[0][1] = PseudoInv[0][1];
887 ProjH[0][2] = PseudoInv[0][2];
888 ProjH[1][0] = PseudoInv[1][0];
889 ProjH[1][1] = PseudoInv[1][1];
890 ProjH[1][2] = PseudoInv[1][2];
891 ProjH[2][0] = PseudoInv[2][0];
892 ProjH[2][1] = PseudoInv[2][1];
893 ProjH[2][2] = PseudoInv[2][2];
906 const double& addppy) {
907 ifstream ifs(filename.c_str());
909 perror(filename.c_str());
912 int thereturn =
BBCIn(ifs, addppx, addppy);
918 const double& addppy) {
920 unsigned int BBCWidth=0, BBCHeight=0;
922 double BBCPxSizeX=0, BBCPxSizeY=0, BBCCxShift=0,
923 BBCCyShift=0,BBCFocalLength=0;
927 while (ifs.getline(linebuf,200)) {
932 if (key ==
"CVec") iss >> BBCC[0]>> BBCC[1]>> BBCC[2];
933 if (key ==
"AVec") iss >> BBCA[0]>> BBCA[1]>> BBCA[2];
934 if (key ==
"UpVec") iss >> BBCUp[0]>> BBCUp[1]>> BBCUp[2];
935 if (key ==
"Width") iss >> BBCWidth;
936 if (key ==
"Height") iss >> BBCHeight;
937 if (key ==
"PixelSizeX") iss >> BBCPxSizeX;
938 if (key ==
"PixelSizeY") iss >> BBCPxSizeY;
939 if (key ==
"CenterPointShiftX") iss >> BBCCxShift;
940 if (key ==
"CenterPointShiftY") iss >> BBCCyShift;
941 if (key ==
"FocalLength") iss >> BBCFocalLength;
942 if (key ==
"}")
break;
959 K[0][0] = BBCFocalLength / BBCPxSizeX;
960 K[1][1] = BBCFocalLength / BBCPxSizeY;
964 K[0][2] = double(BBCWidth-1)/2.0 + (BBCCxShift+addppx) / BBCPxSizeX;
965 K[1][2] = double(BBCHeight-1)/2.0 + (BBCCyShift+addppy) / BBCPxSizeY;
981 for (
unsigned int i=0;i<3;i++){
997 #ifdef BIAS_HAVE_XML2
1000 double& Version)
const {
1001 TopLevelTag =
"PMatrix";
1010 BIASERR(
"Undecomposed PMatrix cannot be metric !!!");
1014 xmlNodePtr childNode;
1019 childNode = XMLObject.
addChildNode(Node,
"Calibration");
1020 stringstream streamK, streamR;
1021 streamK << std::setprecision(STREAM_PRECISION);
1022 for (
unsigned int i=0; i<9; i++) {
1023 streamK << (double)
K_.
GetData()[i] <<
" ";
1025 XMLObject.
addContent(childNode, streamK.str());
1029 streamR << std::setprecision(STREAM_PRECISION);
1030 for (
unsigned int i=0; i<9; i++) {
1031 streamR << (double)
R_.
GetData()[i] <<
" ";
1033 XMLObject.
addContent(childNode, streamR.str());
1046 stringstream datastream;
1047 datastream << std::setprecision(STREAM_PRECISION);
1048 for (
unsigned int i=0; i<12; i++) {
1049 datastream << (double)
GetData()[i] <<
" ";
1051 XMLObject.
addContent(theNode, datastream.str());
1057 xmlNodePtr childNode;
1062 if ((childNode = XMLObject.
getChild(Node,
"Calibration"))==NULL) {
1063 BIASERR(
"error in xml structure");
1068 unsigned int CurrentData = 0;
1070 while (ssCalibration.good() && CurrentData<9) {
1071 ssCalibration >> (pData[CurrentData++]);
1073 if (CurrentData<9) {
1075 BIASERR(
"Error parsing XML code ...");
1081 if ((childNode = XMLObject.
getChild(Node,
"Rotation"))==NULL) {
1082 BIASERR(
"error in xml structure");
1089 while (ssRotation.good() && CurrentData<9) {
1090 ssRotation >> (pData[CurrentData++]);
1092 if (CurrentData<9) {
1094 BIASERR(
"Error parsing XML code ...");
1100 if ((childNode = XMLObject.
getChild(Node,
"Center"))==NULL) {
1102 BIASERR(
"Error parsing XML code ...");
1116 if ((childNode = XMLObject.
getChild(Node,
"DataFields"))==NULL) {
1117 BIASERR(
"error in xml structure");
1123 unsigned int CurrentData=0;
1126 while (ssData.good() && CurrentData<12) {
1127 ssData >> (pData[CurrentData++]);
1129 if (CurrentData<12) {
1131 BIASERR(
"Error parsing XML code ...");
1142 #endif // BIAS_HAVE_XML2
1148 std::ofstream fs( filename.c_str() );
1153 fs<<
"# Covariance [x y z r1 r2 r3] : "<<endl;
1156 fs<<
"# R: "<<
R_<<endl;
1157 fs<<
"# C: "<<
C_<<endl;
1158 fs<<
"# K: "<<
K_<<endl;
1167 if (points2D.size() != points3D.size()) {
1169 BIASERR(
"Vectors have different sizes " << points2D.size() <<
" "<< points3D.size() << endl);
1172 double projectionError = 0.0;
1174 for(
unsigned int i = 0; i < points2D.size(); i++){
1177 temp2 = points2D[i];
1180 projectionError /= points2D.size();
1182 return projectionError;
Matrix3x4< double > PConsistencyBackup_
backup of P at time of last decomposition to check whether decomposition is still valid ...
Vector< double > GetNullvector(const int last_offset=0)
return one of the nullvectors.
void addAttribute(const xmlNodePtr Node, const std::string &AttributeName, bool AttributeValue)
Add an attribute to a node.
bool IsDecomposed_
tells us whether we have a chached decomposition with C,A,...
Vector3< double > GetRayWorldCoo(const HomgPoint2D &point)
returns vector from C to point, w of point determines ray length analytical inverse of K is used ...
Matrix4x4< PMATRIX_TYPE > GetCanonicalH() const
Get the projective homography matrix so that P * H = [ I | 0 ].
PMatrixBase & operator=(const PMatrixBase &mat)
KMatrix K_
camera calibration matrix (intrinsic parameter)
void GetEuclidean(Vector3< HOMGPOINT3D_TYPE > &dest) const
calculate affine coordinates of this and write them to dest affine coordinates are projective coordin...
void Set(const T *pv)
copy the array of vectorsize beginning at *T to this->data_
void SetAsRotationMatrix(Matrix3x3< double > &R)
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
computes and holds the singular value decomposition of a rectangular (not necessarily quadratic) Matr...
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 ...
Subscript num_cols() const
Vector< double > Solve(const Vector< double > &y) const
virtual ~PMatrix()
destructor untested (04/17/2002)
HomgPoint3D BackprojectPseudoinverse(const HomgPoint2D &x)
Returns a homogenized 3D point X on the viewray of x, which always lies in front of the camera(this i...
int Decompose_()
Computes K,R,C, assuming a metric PMatrix P = K[R^T | -R^T * C] with K[0][1]=0 (zero skew) and K[2][2...
void CheckSVD_()
check if SVD is still valid
xmlNodePtr getChild(const xmlNodePtr ParentNode, const std::string &ChildName)
Get a child of a Parent node by specifying the childs name, NULL is returned if the ParentNode has no...
void ScalarProduct(const Vector3< T > &argvec, T &result) const
scalar product (=inner product) of two vectors, storing the result in result
void Set(const HOMGPOINT3D_TYPE &x, const HOMGPOINT3D_TYPE &y, const HOMGPOINT3D_TYPE &z)
set elementwise with given 3 euclidean scalar values.
HomgPoint2D & Homogenize()
homogenize class data member elements to W==1 by divison by W
int WriteCAHV_10(int cols, int rows, std::ostream &os=std::cout)
write the CAHV decomposition of this P matrix in CAHV 1,0 format to stream os (see RK's/TNT CAHV1...
Vector3< double > V0_
unit vector in wcs assigning the direction of the vertical axis of the image plane ...
void Homogenize()
homogenize class data member elements to W==1 by divison by W
Matrix3x3< double > R_
rotation matrix (intrinsic parameter)
Matrix< double > Covariance_
float GetFieldOfViewY(const unsigned int dimX, const unsigned int dimY, const bool &compY=true)
E_ParametrizationType
description of supported parametrization types
virtual int XMLOut(const xmlNodePtr Node, XMLIO &XMLObject) const
specialization of XML write function
int BBCIn(std::ifstream &ifs, const double &AddCenterPointShiftX=0.0, const double &AddCenterPointShiftY=0.0)
Load next piece of calibration data from an open BBC-Free-D file.
double GetProjectionError(const std::vector< BIAS::HomgPoint2D > &points2D, const std::vector< BIAS::HomgPoint3D > &points3D)
returns average squared projection Error
SVD * Svd_
(for decomposition, docu: JW 11/2002)
BIAS::Vector3< double > GetH()
std::string getAttributeValueString(const xmlAttrPtr Attribute) const
int LoadBBC(const std::string &filename, const double &AddCenterPointShiftX=0.0, const double &AddCenterPointShiftY=0.0)
Load calibration data file from BBC-Free-D system.
void Mult(Vector4< double > &argv, Vector4< double > &destv)
args must be of type Vector4<double> because this should work not only for HomgPoint3D but also for H...
Matrix3x3< double > GetR()
void addContent(const xmlNodePtr Node, const std::string &Content)
Add content to a node.
void SetZero()
Sets all values to zero.
void CrossProduct(const Vector3< T > &argvec, Vector3< T > &destvec) const
cross product of two vectors destvec = this x argvec
Wrapper class for reading and writing XML files based on the XML library libxml2. ...
float GetFieldOfView(const unsigned int dimX, const unsigned int dimY, const bool optmin=true)
HomgPlane3D GetImagePlane()
calls the above
void Mult(const Vector3< T > &argvec, Vector3< T > &destvec) const
matrix - vector multiplicate this matrix with Vector3, storing the result in destvec calculates: dest...
xmlNodePtr addChildNode(const xmlNodePtr ParentNode, const std::string &NewNodeName)
Add a child node to an incoming node with the given name.
BIAS::Vector3< double > GetV()
Matrix< double > GetV() const
return V
int GetInverse(Matrix3x3< T > &inv) const
Matrix inversion: inverts this and stores resulty in argument inv.
const Matrix< double > & GetVT() const
return VT (=transposed(V))
virtual int XMLGetClassName(std::string &TopLevelTag, double &Version) const
specialization of XML block name function
void InvalidateDecomposition()
to re-Decompose_() after filling with data use this.
int GetNullVector(HomgPoint3D &C)
this is another way of getting C, especially interesting for non-metric cameras
RMatrixBase Parametrization2R_(const Vector< double > ¶metrization, const enum E_ParametrizationType param_type) const
const Vector< double > & GetS() const
return S which is a vector of the singular values of A in descending order.
a line l = (a b c)^T is a form of the implicit straight line equation 0 = a*x + b*y + c if homogenize...
PMATRIX_TYPE * GetData()
get the pointer to the data array of the matrix (for faster direct memeory access) ...
bool IsAtInfinity() const
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
describes a projective 3D -> 2D mapping in homogenous coordinates
int GetIntersectionOfImagePlanes(PMatrix &P, HomgLine2D &intersection)
return line resulting of intersection of the two image planes in pixel coordinates of this camera ret...
void ScalarProduct(const Vector4< T > &argvec, T &result) const
scalar product (=inner product) of two vectors, storing the result in result
bool Save(const std::string &filename)
double getAttributeValueDouble(const xmlAttrPtr Attribute) const
const Matrix< double > & GetU() const
return U U is a m x m orthogonal matrix
int GetHinf(BIAS::Matrix3x3< double > &Hinf)
Matrix< double > Invert()
returns pseudoinverse of A = U * S * V^T A^+ = V * S^+ * U^T
Vector3< double > GetNormRayWorldCoo(const HomgPoint2D &point)
returns normed vector from C to point analytical inverse of K is used
void DecomposeAa(BIAS::Matrix3x3< PMATRIX_TYPE > &A, BIAS::Vector3< PMATRIX_TYPE > &a) const
decompose the P-matrix to [ A | a ] where A is a 3x3-matrix and a is a 3-vector.
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
Vector3< double > C_
camera center in wcs (extrinsic parameter)
BIAS::Vector3< double > GetA()
returns the unit vector A which is the normal vector to the image plane in world coordinates.
void BackprojectWorldCoo(const HomgPoint2D &point, double depth, HomgPoint3D &res)
returns a 3D point res which is located depth away from center of projection in direction given by po...
Matrix3x4< double > PConsistencyBackupSVD_
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) ...
Vector3< double > A_
unit vector in wcs assigning the direction of the optical axis
int InvertIP()
In place matrix conversion.
Subscript num_rows() const
BIAS::Vector3< double > GetC()
computes translation vector origin world coo -> origin camera coo (center), uses decomposition, which is cached
bool IsMetric_
tells us whether we have an arbitrary 3x4 matrix or a P which is exactly a composition of P = K * R [...
describes a projective 3D -> 2D mapping in homogenous coordinates
void BackprojectByZDepth(const double &x, const double &y, const double &z_depth, HomgPoint3D &res)
Inverts the PMatrix application to a 3D point whose z-component in cameracoordinates and its imagepoi...
int GetPseudoInverse(BIAS::Matrix< double > &Pinv)
returns 4x3 pseudo inverse
void GetSVD(Matrix< double > &U, Vector< double > &S, Matrix< double > &VT)
Returns SVD of this.
virtual int XMLIn(const xmlNodePtr Node, XMLIO &XMLObject)
specialization of XML read function
KMatrix Invert() const
returns analyticaly inverted matrix
void CheckDecomposition_()
check if Decomposition is still valid
Vector3< T > & Normalize()
normalize this vector to length 1
HOMGPOINT2D_TYPE DistanceSquared(const HomgPoint2D &point) const
std::string getNodeContentString(const xmlNodePtr Node) const
Get the content of a given Node.
void GetImagePlane(HomgPlane3D &plane)
returns image plane in world coordinates
PMatrix & operator=(const PMatrix &mat)
assignment operator
Vector3< double > H0_
unit vector in wcs assigning the direction of the horizontal axis of the image plane ...
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
class BIASGeometryBase_EXPORT HomgPoint2D
void Compose(const Matrix3x3< double > &K, const Matrix3x3< double > &R, const Vector3< double > &C)
composes this from K, R and C using P = [ K R' | -K R' C ] with R' = transpose(R) ...
last line of an affine transformation in 3d space is always 0 0 0 1 and can therefor be droped from t...
void SetIdentity()
set the elements of this matrix to the identity matrix (possibly overriding the inherited method) ...
Matrix3x3< double > Hinf_
Hinfinity.
PMatrix & newsize(int rows, int cols)
setting a new size different from 3x4 is not allowed for the fixed size P-Matrix overloads the genera...
A homogeneous plane (in P^3) All points X on the plane p fulfill p ' * X = 0.