26 #include "Geometry/ProjectionParametersGreatCircles.hh"
27 #include <Base/Common/CompareFloatingPoint.hh>
33 #define M_PI_TOL M_PI*1.1
34 #define M_PI_2_TOL M_PI_2*1.1
40 return focallengthTheta_;
46 BIASERR(
"unfinished");
54 BIASERR(
"unfinished");
66 const double x = euc[0];
68 const double y = euc[1];
70 const double z = euc[2];
73 if((l<1e-12) || (fabs(z)<1e-12 && fabs(y)<1e-12)) {
92 if(phi<-M_PI || phi>M_PI || theta<0 || theta>M_PI)
103 ray[0] = -cos(theta);
104 ray[1] = sin(theta)*sin(phi);
105 ray[2] = sin(theta)*cos(phi);
112 imgPoint[0] = theta*focallengthTheta_ + principalX_;
113 imgPoint[1] = phi*focallengthTheta_*aspectratio_ + principalY_;
119 double& phi,
double& theta)
const
121 theta = (imgPoint[0]-principalX_)/focallengthTheta_;
122 phi = (imgPoint[1]-principalY_)/(focallengthTheta_*aspectratio_);
127 const double minTheta,
const double maxTheta,
128 const unsigned int imgWidth,
const unsigned int imgHeight)
130 BIASASSERT(minPhi>=-M_PI && minPhi<=M_PI);
131 BIASASSERT(minTheta>=0 && maxTheta<=M_PI);
132 BIASASSERT(maxTheta>minTheta);
133 BIASASSERT(maxTheta<=M_PI);
135 BIASASSERT(maxPhi>minPhi);
136 BIASASSERT(maxPhi<=M_PI);
144 focallengthTheta_ = width_/(maxTheta-minTheta);
145 principalX_ = -focallengthTheta_*minTheta-0.5;
147 double focallengthPhi = height_/(maxPhi-minPhi);
149 aspectratio_ = focallengthPhi/focallengthTheta_;
153 principalY_ = -focallengthTheta_*aspectratio_*minPhi-0.5;
170 double deltaTheta = width_/focallengthTheta_;
171 double deltaPhi = height_/(focallengthTheta_*aspectratio_);
173 principalX_/=focallengthTheta_;
175 principalY_/=(focallengthTheta_*aspectratio_);
177 double widthTemp = (double)width_/ratio;
178 double heightTemp = (double)height_/ratio;
179 width_ = (int)rint(widthTemp);
180 height_ = (int)rint(heightTemp);
181 focallengthTheta_ = width_/deltaTheta;
182 double focallengthPhi = height_/deltaPhi;
183 aspectratio_ = focallengthPhi/focallengthTheta_;
185 principalX_*=focallengthTheta_;
186 principalY_*=(focallengthTheta_*aspectratio_);
193 Rescale(
unsigned int width,
unsigned int height)
197 double deltaTheta = width_/focallengthTheta_;
198 double deltaPhi = height_/(focallengthTheta_*aspectratio_);
200 principalX_/=focallengthTheta_;
202 principalY_/=(focallengthTheta_*aspectratio_);
204 double widthTemp = (double)width_;
205 double heightTemp = (double)height_;
206 width_ = (int)rint(widthTemp);
207 height_ = (int)rint(heightTemp);
208 focallengthTheta_ = width_/deltaTheta;
209 double focallengthPhi = height_/deltaPhi;
210 aspectratio_ = focallengthPhi/focallengthTheta_;
212 principalX_*=focallengthTheta_;
213 principalY_*=(focallengthTheta_*aspectratio_);
221 const double halfAngleRange,
222 double& thetaMin,
double& thetaMax,
223 double& phiMin,
double& phiMax)
const
227 Pose_.GlobalToLocal(worldPoint).GetEuclidean(localEucPoint);
229 if(!LocalEuclideanCamCoordinates2SphericalAngles(localEucPoint,
231 BIASERR(
"unable to transform given point into angle representation!");
235 double thetaExtendedMin = theta - halfAngleRange;
236 double thetaExtendedMax = theta + halfAngleRange;
239 thetaMin = (thetaExtendedMin<=0) ? 0 : thetaExtendedMin;
240 thetaMax = (thetaExtendedMax>=M_PI) ? M_PI : thetaExtendedMax;
243 if((thetaExtendedMin<=0) || (thetaExtendedMax>=M_PI)) {
249 double phiExtendedMin = phi - halfAngleRange;
250 if(phiExtendedMin < -M_PI) {
256 double phiExtendedMax = phi + halfAngleRange;
257 if(phiExtendedMax > M_PI) {
263 phiMin = phiExtendedMin;
264 phiMax = phiExtendedMax;
271 const double& halfAngleRange)
274 double thetaMin, thetaMax, phiMin, phiMax;
275 result = GetAngleInterval(worldPoint,
277 thetaMin, thetaMax, phiMin, phiMax);
279 SetInternals(phiMin, phiMax,
311 bool IgnoreDistortion)
const
315 if(localX[2] > 0.0) {
316 x = ProjectLocal(localX, IgnoreDistortion);
319 return (x[0]>=0.0 && x[1]>=0.0 &&
320 x[0]<=(
double)(width_-1) && x[1]<=(
double)(height_-1) );
331 if(LocalEuclideanCamCoordinates2SphericalAngles(point, phi, theta)) {
333 Angle2Image(phi, theta, res);
347 bool IgnoreDistortion)
const
349 BIASERR(
"unfinished\n");
359 bool ignoreDistortion)
const
362 UnProjectLocal(pos, p, d, ignoreDistortion);
364 if (
Equal(norm, 0.0))
368 return Pose_.LocalToGlobal(local);
377 Image2Angle(cPos, phi, theta);
378 SphericalAngles2LocalEuclideanRay(phi, theta, direction);
382 #ifdef BIAS_HAVE_XML2
385 double& Version)
const {
386 TopLevelTag =
"ProjectionParametersGreatCircles";
392 XMLIO& XMLObject)
const {
401 XMLObject.
addAttribute(Node,
"focallengthTheta", focallengthTheta_);
413 xmlNodePtr childNode;
414 if ((childNode = XMLObject.
getChild(Node, TopLevelName))==NULL) {
415 BIASERR(
"Error in xml, no tag" << TopLevelName);
424 #endif // BIAS_HAVE_XML2
void addAttribute(const xmlNodePtr Node, const std::string &AttributeName, bool AttributeValue)
Add an attribute to a node.
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 SetInternals(const double minPhi, const double maxPhi, const double minTheta, const double maxTheta, const unsigned int imgWidth, const unsigned int imgHeight)
Calculates and sets internal camera parameters.
virtual HomgPoint2D ProjectLocal(const Vector3< double > &point, bool IgnoreDistortion=false) const
calculates the projection of a point in the camera coordinate system to a pixel in the image plane of...
virtual int XMLOut(const xmlNodePtr Node, XMLIO &XMLObject) const
specialization of XML write function
virtual void UnProjectLocal(const HomgPoint2D &pos, Vector3< double > &pointOnRay, Vector3< double > &direction, bool ignoreDistortion=false) const
calculates the viewing ray from the camera center (in the camera coordinate system) which belongs to ...
virtual int XMLOut(const xmlNodePtr Node, XMLIO &XMLObject) const
Specialization of XML write function.
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...
HomgPoint3D UnProjectToImagePlane(const HomgPoint2D &pos, const double &depth=1.0, bool IgnoreDistortion=false) const
map points from image onto unit diameter image plane in 3D.
virtual const bool DoIntrinsicsDiffer(const ProjectionParametersBase *p) const
double GetFocallengthTheta()
ProjectionParametersGreatCircles & operator=(const ProjectionParametersGreatCircles &P)
virtual bool Undistort(BIAS::HomgPoint2D &point2d) const
Interface defintion for lens undistortion function, implemented by derived classes.
virtual int XMLIn(const xmlNodePtr Node, XMLIO &XMLObject)
Specialization of XML read function.
spherical camera that samples along big circles containig the H vector.
virtual const bool DoIntrinsicsDiffer(const ProjectionParametersBase *p) const
double focallengthTheta_
scale from angles to image
static bool SphericalAngles2LocalEuclideanRay(const double phi, const double theta, BIAS::Vector3< double > &ray)
Calculate euclidean ray direction in local camera frame from angles.
void Image2Angle(const BIAS::HomgPoint2D &imgPoint, double &phi, double &theta) const
Calculates local angle vectors from image coordinate.
virtual bool Distort(BIAS::HomgPoint2D &point2d) const
Interface defintion for lens distortion function, implemented by derived classes. ...
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.
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
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_...
virtual void Rescale(double ratio, const double offset=0.0)
double getAttributeValueDouble(const xmlAttrPtr Attribute) const
virtual int XMLGetClassName(std::string &TopLevelTag, double &Version) const
specialization of XML block name function
bool SetAngleInterval(const HomgPoint3D &worldPoint, const double &halfAngleRange)
Sets viewing volume of camera using GetAngleInterval().
virtual int XMLGetClassName(std::string &TopLevelTag, double &Version) const
Specialization of XML block name function.
bool GetAngleInterval(const HomgPoint3D &worldPoint, const double halfAngleRange, double &thetaMin, double &thetaMax, double &phiMin, double &phiMax) const
Determines angle intervalls for the camera from viewing cone.
Camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
virtual int XMLIn(const xmlNodePtr Node, XMLIO &XMLObject)
specialization of XML read function
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
bool DoesPointProjectIntoImageLocal(const Vector3< double > &localX, HomgPoint2D &x, bool IgnoreDistortion=false) const
Checks if 3D point projects into specified image and returns belonging 2D image point.
static bool LocalEuclideanCamCoordinates2SphericalAngles(const BIAS::Vector3< double > &euc, double &phi, double &theta)
Calculate spherical angles from local 3D point.
void Angle2Image(double phi, double theta, BIAS::HomgPoint2D &imgPoint) const
Calculates image coordinates from local angle vectors.
virtual ProjectionParametersBase & operator=(const ProjectionParametersBase &p)
class BIASGeometryBase_EXPORT HomgPoint3D