27 #include <MathAlgo/SVD.hh>
28 #include <MathAlgo/Lapack.hh>
29 #include <Geometry/RMatrix.hh>
59 for (
unsigned int i=0; i<2; i++) {
60 for (
unsigned int j=0; j<2; j++) {
61 HTaylor[i][j] = Jac[i][j];
65 if (fabs(y[2])<1e-9) {
68 BIASERR(
"Cannot linearize homography near infinity");
76 for (
unsigned int i=0; i<2; i++) HTaylor[i][2] = y[i];
84 BIASASSERT(scenePlane.
NormL2()>1e-5);
99 double norm = normal.
NormL2();
100 if (norm/scenePlane.
NormL2()<1e-9) {
114 HomgPoint3D(point).ScalarProduct(scenePlane, tester);
115 BIASASSERT(fabs(tester)<1e-3);
129 (*this) = K2*(*this)*K1.
Invert();
136 const unsigned int OrigHeight,
138 double & sizeWidth,
double & sizeHeight)
const {
140 double minX, maxX, minY, maxY;
162 minX=maxX=mapped[0][0];
163 minY=maxY=mapped[0][1];
165 for (
unsigned int i=1; i<4; i++) {
168 minX = (mapped[i][0]<minX) ? mapped[i][0] : minX;
169 maxX = (mapped[i][0]>maxX) ? mapped[i][0] : maxX;
170 minY = (mapped[i][1]<minY) ? mapped[i][1] : minY;
171 maxY = (mapped[i][1]>maxY) ? mapped[i][1] : maxY;
176 sizeHeight=maxY-minY;
187 return (mapped[0]-x2[0])*(mapped[0]-x2[0]) + (mapped[1]-x2[1])*(mapped[1]-x2[1]);
193 BIASERR(
"supplied unhomogenized point !");
197 BIASERR(
"Jacobian has wrong size !");
203 const double factor = ((*this)[2][0]*x[0]+(*this)[2][1]*x[1]+(*this)[2][2]);
205 if (fabs(factor)<1e-10) {
216 BIASWARN(
"Trying to obtain Jacobian of point mapped to infinity");
217 if ((*
this)[0][0] * (*
this)[2][1]*x[1] + (*
this)[0][0]*(*
this)[2][2] - (*
this)[0][1]*x[1]*(*
this)[2][0] - (*
this)[0][2]*(*
this)[2][0]<0.0)
218 Jac[0][0] = -HUGE_VAL;
220 Jac[0][0] = HUGE_VAL;
222 if ((*
this)[1][0]*x[0]*(*
this)[2][1] - (*
this)[1][1]*(*
this)[2][0]*x[0] - (*
this)[1][1]*(*
this)[2][2] + (*
this)[1][2]*(*
this)[2][1] < 0.0)
223 Jac[1][1] = HUGE_VAL;
225 Jac[1][1] = -HUGE_VAL;
230 HMatrix scaled((*
this)/factor);
233 const double enumx = (scaled[0][0]*x[0]+scaled[0][1]*x[1]+scaled[0][2]);
234 const double enumy = (scaled[1][0]*x[0]+scaled[1][1]*x[1]+scaled[1][2]);
237 Jac[0][0] = (scaled[0][0] - scaled[2][0]*enumx);
239 Jac[0][1] = (scaled[0][1] - scaled[2][1]*enumx);
241 Jac[1][0] = (scaled[1][0] - scaled[2][0]*enumy);
243 Jac[1][1] = (scaled[1][1] - scaled[2][1]*enumy);
254 d1 = mysvd.
GetS()[0];
255 d2 = mysvd.
GetS()[1];
257 theta = mysvd.
GetU() * phi;
269 BIASERR(
"Factorization of affine matrix failed ..."<<res<<flush);
272 d1 = mysvd.
GetS()[0];
273 d2 = mysvd.
GetS()[1];
288 A = theta * phi.
Invert() * d * phi;
299 A = phi.
Invert() * d * phi * theta;
304 std::vector<BIAS::HomgPoint2D>& p2) {
308 if(p1.size() != p2.size()) BIASWARN(
"HMatrix::GetReprojectionError HomgPoint2D vectors have different sizes!");
309 int size = (p1.size()<p2.size())? p1.size():p2.size();
314 for(
int i=0; i < size; i++){
319 mapped1 = ((*this) * x1);
341 return (truePoint-approxPoint).
NormL2();
int Invert(Matrix2x2< T > &result) const
analyticaly inverts matrix
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...
a 3x3 Matrix describing projective transformations between planes
Matrix< T > Transpose() const
transpose function, storing data destination matrix
HMatrixBase & operator=(const HMatrixBase &mat)
assignment operators calls corresponding operator from parent
void GetMappedRange(const unsigned int OrigWidth, const unsigned int OrigHeight, BIAS::Vector2< double > &corner, double &sizeWidth, double &sizeHeight) const
maps the rectangle (0,0),(OrigWidth,OrigHeight) with this H and computes mapped range as corner + siz...
void ScalarProduct(const Vector3< T > &argvec, T &result) const
scalar product (=inner product) of two vectors, storing the result in result
HomgPoint2D & Homogenize()
homogenize class data member elements to W==1 by divison by W
static int FactorizeAffineMatrixRRight(const Matrix2x2< double > &A, Matrix2x2< double > &phi, Matrix2x2< double > &theta, double &d1, double &d2)
decompose affine matrix into rotations and nonisotropic scalings
void OuterProduct(const Vector3< T > &v, Matrix3x3< T > &res) const
outer product, constructs a matrix.
void GetJacobian(const HomgPoint2D &x, Matrix< double > &Jac) const
returns jacobian of H: R^2 –> R^2
int GetR(Matrix3x3< double > &R)
int Compute(const Matrix< double > &M, double ZeroThreshold=DEFAULT_DOUBLE_ZERO_THRESHOLD)
set a new matrix and compute its decomposition.
static int FactorizeAffineMatrixRLeft(const Matrix2x2< double > &A, Matrix2x2< double > &phi, Matrix2x2< double > &theta, double &d1, double &d2)
decompose affine matrix into rotations and nonisotropic scalings
static int ComposeAffineMatrixRLeft(Matrix2x2< double > &A, const Matrix2x2< double > &phi, const Matrix2x2< double > &theta, const double &d1, const double &d2)
compose affine matrix from rotations and nonisotropic scalings
double NormL2() const
Return the L2 norm: sqrt(a^2 + b^2 + c^2 + d^2)
void SetZero()
Sets all values to zero.
unsigned int GetRows() const
const Matrix< double > & GetVT() const
return VT (=transposed(V))
void MapAcrossPlane(const PMatrix &P1, const PMatrix &P2, const HomgPlane3D &scenePlane=HomgPlane3D(0, 0, 0, 1))
Set (*this) to map coordinates from camera p1 to p2 using the given scene plane.
HMatrix GetLinearized(const HomgPoint2D &x) const
returns 1st order Taylor expansion of homography at x
double GetReprojectionError(std::vector< BIAS::HomgPoint2D > &p1, std::vector< BIAS::HomgPoint2D > &p2)
compute reprojection error for both vectors of HomgPoint2D
const Vector< double > & GetS() const
return S which is a vector of the singular values of A in descending order.
unsigned int GetCols() const
int GetC(Vector3< double > &C)
computes translation vector origin world coo -> origin camera coo (center), uses decomposition, which is cached
Matrix3x3< T > Transpose() const
returns transposed matrix tested 12.06.2002
const Matrix< double > & GetU() const
return U U is a m x m orthogonal matrix
double GetMappedError(const BIAS::HomgPoint2D &PointPicOne, const BIAS::HomgPoint2D &PointPicTwo) const
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
double ComputeLinearizationError(const HomgPoint2D &x0, const HomgPoint2D &y) const
compute distance between y mapped with homography or first order taylor approximation at development ...
HMatrix & operator=(const HMatrix &mat)
int InvertIP()
In place matrix conversion.
describes a projective 3D -> 2D mapping in homogenous coordinates
static int ComposeAffineMatrixRRight(Matrix2x2< double > &A, const Matrix2x2< double > &phi, const Matrix2x2< double > &theta, const double &d1, const double &d2)
compose affine matrix from rotations and nonisotropic scalings
KMatrix Invert() const
returns analyticaly inverted matrix
bool IsHomogenized() const
HOMGPOINT2D_TYPE DistanceSquared(const HomgPoint2D &point) const
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
class BIASGeometryBase_EXPORT HomgPoint2D
class BIASGeometryBase_EXPORT HomgPoint3D
int GetK(KMatrix &K)
calibration matrix
A homogeneous plane (in P^3) All points X on the plane p fulfill p ' * X = 0.