27 #include "Geometry/ProjectionParametersBase.hh"
28 #include <Geometry/SphericalCoordinates.hh>
34 #define PROJECTION_STREAM_PRECISION 12
36 ProjectionParametersBase::
37 ProjectionParametersBase(
const unsigned int width,
38 const unsigned int height)
45 if (width > 0) principalX_ = 0.5*double(width-1);
46 if (height > 0) principalY_ = 0.5*double(height-1);
49 ustTransformIntoImage_ =
false;
50 ustIgnoreDistortion_ =
false;
51 ustNormalize_ =
false;
52 QValid_ = CValid_ =
false;
54 videoSourceType_ =
"";
66 void ProjectionParametersBase::
73 bool ProjectionParametersBase::
86 }
else if (it.
x<width_-1) {
106 void ProjectionParametersBase::
113 bool ProjectionParametersBase::
126 }
else if (it.
x<width_-0.5) {
136 if (it.
y<height_-0.5) {
147 int ProjectionParametersBase::
149 double& minPhi,
double& maxPhi,
double& centerPhi,
150 double& minTheta,
double& maxTheta )
157 if(localSphericalReferenceFrame.
GetC().
NormL2() > 1e-12) {
158 BIASERR(
"Spherical frame position and camera position are different!"
159 << endl << localSphericalReferenceFrame.
GetC().
NormL2());
171 double phi, theta, rho;
177 GetFirstEdgePosition(it);
183 UnProjectLocal(imgPoint, p, d);
186 if(phi<minPhi) minPhi = phi;
187 if(phi>maxPhi) maxPhi = phi;
188 if(theta<minTheta) minTheta = theta;
189 if(theta>maxTheta) maxTheta = theta;
192 while(GetNextEdgePosition(it));
194 imgPoint[0] = (width_-1)/2;
195 imgPoint[1] = (height_-1)/2;
196 UnProjectLocal(imgPoint, p, d);
208 bool minimumIsIn = DoesPointProjectIntoImageLocal(minRay.
GetEuclidean(),
210 bool maximumIsIn = DoesPointProjectIntoImageLocal(maxRay.
GetEuclidean(),
222 bool ProjectionParametersBase::
225 bool IgnoreDistortion)
const
230 return DoesPointProjectIntoImageLocal(localX, x, IgnoreDistortion);
233 const bool ProjectionParametersBase::
256 const bool ProjectionParametersBase::
281 RMatrix ProjectionParametersBase::
293 #ifdef BIAS_HAVE_XML2
295 int ProjectionParametersBase::
296 XMLGetClassName(std::string& TopLevelTag,
297 double& Version)
const {
298 TopLevelTag =
"ProjectionParametersBase";
303 int ProjectionParametersBase::
304 XMLOut(
const xmlNodePtr Node,
XMLIO& XMLObject)
const {
306 xmlNodePtr childNode;
309 XMLIO::IsoLatin1ToUtf8( GetIdentifier(),identutf8);
312 childNode = XMLObject.
addChildNode(Node,
"VideoSourceType");
313 XMLObject.
addAttribute(childNode,
"val", GetVideoSourceType());
316 XMLObject.
addAttribute(childNode,
"width", (
int)width_);
317 XMLObject.
addAttribute(childNode,
"height", (
int)height_);
319 childNode = XMLObject.
addChildNode(Node,
"PrincipalPoint");
323 childNode = XMLObject.
addChildNode(Node,
"AspectRatio");
333 stringstream streamR;
335 streamR << std::setprecision(PROJECTION_STREAM_PRECISION);
336 for (
unsigned int i = 0; i < 4; i++) {
337 streamR << (double) Q.
GetData()[i] <<
" ";
339 XMLObject.
addContent(childNode, streamR.str());
357 int ProjectionParametersBase::
358 XMLIn(
const xmlNodePtr Node,
XMLIO& XMLObject) {
360 xmlNodePtr childNode;
361 if ((childNode = XMLObject.
getChild(Node,
"Identifier"))!=NULL) {
364 XMLIO::Utf8ToIsoLatin1(identutf8,identifier_);
367 if ((childNode = XMLObject.
getChild(Node,
"VideoSourceType"))!=NULL) {
371 if ((childNode = XMLObject.
getChild(Node,
"ImageSize"))!=NULL) {
375 if ((childNode = XMLObject.
getChild(Node,
"PrincipalPoint"))!=NULL) {
379 if ((childNode = XMLObject.
getChild(Node,
"CellSize"))!=NULL) {
380 double cellSizeX, cellSizeY;
383 aspectratio_ = cellSizeX / cellSizeY;
385 if ((childNode = XMLObject.
getChild(Node,
"AspectRatio"))!=NULL) {
391 if ((childNode = XMLObject.
getChild(Node,
"Rotation"))!=NULL) {
394 unsigned int CurrentData = 0;
395 while (ssRotation.good() && CurrentData<4) {
396 ssRotation >> (Q.
GetData()[CurrentData++]);
400 BIASERR(
"error in xml structure, rotation matrix incomplete.");
410 if ((childNode = XMLObject.
getChild(Node,
"Center"))!=NULL) {
422 double ProjectionParametersBase::
425 BIASERR(
"ViewDifference not implemented in derived class");
433 const double& depth,
bool IgnoreDistortion)
const {
435 UnProjectLocal(pos, p, x, IgnoreDistortion);
438 if (fabs(x.
NormL2()-depth)>1e-10) {
439 BIASERR(
"Unprojection failed:" << x <<
" Position:"<< pos
440 <<
" Norm:" << setw(10) <<
double(x.
NormL2()));
450 bool IgnoreDistortion)
const
454 GetR().
Mult(UnProjectToPointLocal(pos, depth, IgnoreDistortion), point);
463 bool IgnoreDistortion)
const
471 int ProjectionParametersBase::
473 BIASASSERT(src.
size()==3);BIASASSERT(dst.
size()==3);
475 if (ustTransformIntoImage_) {
476 xdst = ProjectLocal(xsrc, ustIgnoreDistortion_);
478 UnProjectLocal(xsrc, p, xdst, ustIgnoreDistortion_);
480 if (xdst.NormL2()<1e-10) {
482 dst[0] = dst[1] = dst[2] = 0;
503 bool IgnoreDistortion,
bool Normalize)
506 ustIgnoreDistortion_ = IgnoreDistortion;
507 ustTransformIntoImage_ =
false;
508 ustNormalize_ = Normalize;
513 srcPoint[0] = pos[0];
514 srcPoint[1] = pos[1];
515 srcPoint[2] = pos[2];
524 ProjectionParametersBase::~ProjectionParametersBase(){
529 principalX_ = 999999;
530 principalY_ = 999999;
531 aspectratio_ = 999999;
536 KMatrix ProjectionParametersBase::
537 GetFakeKMatrix(
int resolution,
const double& maxangle)
540 return GetFakeKMatrix(dum, resolution);
543 KMatrix ProjectionParametersBase::
544 GetFakeKMatrix(
double& imgsize,
int resolution,
const double& ang)
const {
550 switch (resolution) {
553 ray1.
Set(sin(ang-M_PI/180.0),0.0,cos(ang-M_PI/180.0));
554 ray2.
Set(sin(ang),0.0,cos(ang));
555 imgCoord1 = ProjectLocal(ray1);
556 imgCoord2 = ProjectLocal(ray2);
557 imgsize=(imgCoord2[0]-imgCoord1[0])*
558 tan(ang)/tan(M_PI/180.0);
562 ray1.
Set(sin(ang-M_PI/180.0),0.0,cos(ang-M_PI/180.0));
563 ray2.
Set(sin(ang),0.0,cos(ang));
564 imgCoord1 = ProjectLocal(ray1);
565 imgCoord2 = ProjectLocal(ray2);
566 imgsize=(imgCoord2[0]-imgCoord1[0])*
567 tan(ang)/(tan(ang)-tan(ang-M_PI/180.0));
570 ray1.
Set(sin(ang),0.0,cos(ang));
571 imgCoord1 = ProjectLocal(ray1);
572 imgsize = imgCoord1[0]-principalX_;
576 K[0][0] = imgsize/tan(ang);
577 K[1][1] = imgsize/tan(ang);
585 void ProjectionParametersBase::
586 Rescale(
double ratio,
const double offset)
588 double widthTemp = (double)width_/ratio;
589 double heightTemp = (double)height_/ratio;
596 width_ = (int)rint(widthTemp);
599 height_ = (int)rint(heightTemp);
607 principalX_ = (principalX_ - offset)/ratio;
608 principalY_ = (principalY_ - offset)/ratio;
614 void ProjectionParametersBase::
615 Rescale(
unsigned int width,
unsigned int height)
618 unsigned int oldWidth = width_;
619 unsigned int oldHeight = height_;
624 principalX_ = principalX_ / ((double)oldWidth/(
double)width_);
625 principalY_ = principalY_ / ((double)oldHeight/(
double)height_);
629 #define JACOBIAN_EPSILON 0.5
631 int ProjectionParametersBase::
634 const bool homogenized)
const {
636 BIASASSERT(homogenized);
642 if (L.
NormL2()<0.5)
return -1;
646 if (R.
NormL2()<0.5)
return -1;
650 x[1]-JACOBIAN_EPSILON),
652 if (T.
NormL2()<0.5)
return -1;
654 x[1]+JACOBIAN_EPSILON),
657 if (B.
NormL2()<0.5)
return -1;
668 if (C.
NormL2()<0.5)
return -1;
675 if (axis.NormL2()>1e-7) {
677 Rot.
Set(axis, acos(C[2]));
701 Jac[0][0] = (R[0]-L[0]);
702 Jac[1][0] = (R[1]-L[1]);
704 Jac[0][1] = (B[0]-T[0]);
705 Jac[1][1] = (B[1]-T[1]);
710 bool ProjectionParametersBase::
718 return transformedBaseline[0] >= 0.0;
721 void ProjectionParametersBase::
722 SetVideoSourceType(
const std::string &name)
724 videoSourceType_ = name;
virtual BIAS::Vector3< double > GetC() const
Get projection center.
void addAttribute(const xmlNodePtr Node, const std::string &AttributeName, bool AttributeValue)
Add an attribute to a node.
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_
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
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 Set(const HOMGPOINT3D_TYPE &x, const HOMGPOINT3D_TYPE &y, const HOMGPOINT3D_TYPE &z)
set elementwise with given 3 euclidean scalar values.
int SetFromQuaternion(const Quaternion< ROTATION_MATRIX_TYPE > &q)
Set rotation matrix from a quaternion.
void GetCartesianRayFromFullPhi(const double &phi, const double &theta, HomgPoint3D &ray) const
Calculates the Euclidean ray belonging to the passed angles in the world coordinate frame...
void SetAffineBase(const CoordinateTransform3D &newAffineBase)
Sets the "local" coordinate frame of the spherical coordinates.
std::string getAttributeValueString(const xmlAttrPtr Attribute) const
int getAttributeValueInt(const xmlAttrPtr Attribute) const
void addContent(const xmlNodePtr Node, const std::string &Content)
Add content to a node.
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
Wrapper class for reading and writing XML files based on the XML library libxml2. ...
xmlNodePtr addChildNode(const xmlNodePtr ParentNode, const std::string &NewNodeName)
Add a child node to an incoming node with the given name.
void AddIP(const T &scalar)
Addition (in place) of an scalar.
int GetInverse(Matrix3x3< T > &inv) const
Matrix inversion: inverts this and stores resulty in argument inv.
void Mult(const T &scalar, Vector3< T > &dest) const
unsigned int height_
height of image in pixels
virtual BIAS::RMatrix GetR() const
Get orientation as rotation matrix R.
virtual Vector3< double > UnProjectToPointLocal(const HomgPoint2D &pos, const double &depth, bool IgnoreDistortion=false) const
calculates a 3D point in the local camera coordinate system, which belongs to the image position pos ...
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
Can be used to run along the image border.
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_...
unsigned int width_
width of image in pixels
double x
If using BorderPixel methods these are the coordinates of the pixel.
const T * GetData() const
get the data pointer the member function itself is const (before {..}) because it doesn't change the...
double getAttributeValueDouble(const xmlAttrPtr Attribute) const
void GetSphericalCoordinatesFullPhi(const HomgPoint3D &point, double &rho, double &phi, double &theta) const
Method calculates spherical coordinates, hereby phi will lie in the range (-M_PI, M_PI] while theta l...
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
double principalX_
principal point in pixel coordinates (one for all zoom settings)
double aspectratio_
aspect ratio of the camera CCD
Camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
Transformation between affine and spherical coordinates.
bool IsHomogenized() const
Vector3< T > & Normalize()
normalize this vector to length 1
std::string getNodeContentString(const xmlNodePtr Node) const
Get the content of a given Node.
bool QValid_
validity flag for orientation and position
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) ...