25 #include "Normalization.hh"
27 #include <Base/Geometry/KMatrix.hh>
28 #include <Base/Geometry/RMatrixBase.hh>
29 #include <Base/Geometry/HomgPoint2D.hh>
30 #include <Base/Geometry/HomgPoint3D.hh>
31 #include <Base/Math/Matrix4x4.hh>
32 #include <Base/Debug/Debug.hh>
39 Hartley(
const vector<HomgPoint3D>& Points, vector<HomgPoint3D>& NormPoints,
43 const unsigned int uiPointCount(Points.size());
45 if (uiPointCount<2)
return -1;
49 NormPoints.resize(uiPointCount);
52 double meanX(0), meanY(0), meanZ(0);
53 for (
unsigned int i=0; i<uiPointCount; i++) {
54 meanX += Points[i][0];
55 meanY += Points[i][1];
56 meanZ += Points[i][2];
58 if (Points[i][3]!=1.0) {
59 BIASERR(
"Supplied non-homogenized point ("<<i<<
") ! "<<Points[i]);
64 meanX /= (double)uiPointCount;
65 meanY /= (double)uiPointCount;
66 meanZ /= (double)uiPointCount;
70 for (
unsigned int i=0; i<uiPointCount; i++) {
72 meandist += sqrt((CurPoint[0]-meanX) * (CurPoint[0]-meanX) +
73 (CurPoint[1]-meanY) * (CurPoint[1]-meanY) +
74 (CurPoint[2]-meanZ) * (CurPoint[2]-meanZ));
76 meandist /= (double)uiPointCount;
80 normMatrix[0][0] = M_SQRT2/meandist;
81 normMatrix[0][3] = -meanX/meandist;
82 normMatrix[1][1] = M_SQRT2/meandist;
83 normMatrix[1][3] = -meanY/meandist;
85 normMatrix[2][2] = M_SQRT2/meandist;
86 normMatrix[2][3] = -meanZ/meandist;
93 for (
unsigned int i=0; i<uiPointCount; i++) {
94 normMatrix.
Mult(Points[i], p);
102 Hartley(
const vector<HomgPoint2D>& Points, vector<HomgPoint2D>& NormPoints,
106 const unsigned int uiPointCount(Points.size());
108 if (uiPointCount<2)
return -1;
112 NormPoints.reserve(uiPointCount);
114 if (NormPoints.size()>0) {
115 BIASERR(
"non-empty vector supplied !");
120 double meanX(0), meanY(0);
121 for (
unsigned int i=0; i<uiPointCount; i++) {
122 meanX += Points[i][0];
123 meanY += Points[i][1];
125 if (Points[i][2]!=1.0) {
126 BIASERR(
"Supplied non-homogenized point ("<<i<<
") ! "<<Points[i]);
131 meanX /= (double)uiPointCount;
132 meanY /= (double)uiPointCount;
136 for (
unsigned int i=0; i<uiPointCount; i++) {
138 meandist += sqrt((CurPoint[0]-meanX) * (CurPoint[0]-meanX) +
139 (CurPoint[1]-meanY) * (CurPoint[1]-meanY));
141 meandist /= (double)uiPointCount;
145 Normalization[0][0] = M_SQRT2/meandist;
146 Normalization[0][2] = -meanX/meandist;
147 Normalization[1][1] = M_SQRT2/meandist;
148 Normalization[1][2] = -meanY/meandist;
155 for (
unsigned int i=0; i<uiPointCount; i++) {
156 Normalization.
Mult(Points[i], p);
163 Woelk(
const vector<HomgPoint2D>& Points,
164 vector<HomgPoint2D>& NormPoints,
167 const unsigned int uiPointCount(Points.size());
169 if (uiPointCount<2)
return -1;
173 for (
unsigned int i=0; i<uiPointCount; i++) {
174 BIASASSERT(
Equal(Points[i].NormL2(), 1.0));
177 mean /= (double)uiPointCount;
184 double angle = axis.
NormL2();
186 if (angle>=1.0) angle = 1.0;
187 if (angle<=-1.0) angle = -1.0;
188 if (
Equal(angle, 0.0)){
189 axis.
Set(0.0, 0.0, 1.0);
202 double meanang = 0.0;
203 double max_ang = 1.0;
204 for (
unsigned int i=0; i<uiPointCount; i++) {
205 diff = R * Points[i];
208 max_ang = min(ang, max_ang);
210 meanang /= (double)uiPointCount;
211 if (max_ang>=1.0) max_ang = 1.0;
212 if (max_ang<=-1.0) max_ang = -1.0;
213 max_ang = acos(max_ang);
216 if (meanang>=1.0) meanang = 1.0;
217 if (meanang<=-1.0) meanang = -1.0;
218 meanang = acos(meanang);
223 const double ang_thresh = 1.4;
229 const double des_ang = 1.0;
235 if (max_ang<ang_thresh && meanang<des_ang){
236 double des_scale = tan(des_ang) / tan(meanang);
238 double max_scale = tan(ang_thresh) / tan(max_ang);
240 double scale = min(des_scale, max_scale);
242 for (
int i=0; i<2; i++){
247 Normalization = K * R;
250 NormPoints.resize(uiPointCount);
252 for (
unsigned int i=0; i<uiPointCount; i++) {
253 Normalization.
Mult(Points[i], NormPoints[i]);
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 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
void Set(const Vector3< ROTATION_MATRIX_TYPE > &w, const ROTATION_MATRIX_TYPE phi)
Set from rotation axis w and angle phi (in rad)
void CrossProduct(const Vector3< T > &argvec, Vector3< T > &destvec) const
cross product of two vectors destvec = this x argvec
void Mult(const Vector3< T > &argvec, Vector3< T > &destvec) const
matrix - vector multiplicate this matrix with Vector3, storing the result in destvec calculates: dest...
void Mult(const Vector4< T > &argvec, Vector4< T > &destvec) const
matrix - vector multiplicate this matrix with Vector4, storing the result in destvec, calculates: destvec = (this Matrix) * argvec
class Normalization Functions, e.g.
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
int Hartley(const std::vector< HomgPoint2D > &Points, std::vector< HomgPoint2D > &NormPoints, KMatrix &Normalization) const
linearly transform Points with 3x3 matrix, such that they have mean zero and stddev sqrt(2) ...
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_...
void SetIdentity()
set the elements of this matrix to the identity matrix (posisbly overriding the inherited method) ...
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
Implements a 3D rotation matrix.
int Woelk(const std::vector< HomgPoint2D > &Points, std::vector< HomgPoint2D > &NormPoints, Matrix3x3< double > &Normalization) const
normalization for fisheye cameras wher points may have a w component = 0
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
void SetIdentity()
set the elements of this matrix to the identity matrix (possibly overriding the inherited method) ...