25 #include "Geometry/ProjectionParametersCylindric.hh"
26 #include <Base/Common/CompareFloatingPoint.hh>
35 BIASERR(
"unfinished");
43 BIASERR(
"unfinished");
54 focallengthX_ = double(width_)/(xMax_-xMin_);
56 principalX_ = (-1.0)*xMin_*focallengthX_;
57 focallengthPhi_ = double(height_)/(phiMax_-phiMin_);
59 principalY_ = (-1.0)*phiMin_*focallengthPhi_;
66 xMin_ = (-1.0)*principalX_/focallengthX_;
67 xMax_ = double(width_)/focallengthX_ + xMin_;
68 phiMin_ = (-1.0)*principalY_/focallengthPhi_;
69 phiMax_ = double(height_)/focallengthPhi_ + phiMin_;
75 bool IgnoreDistortion)
const
90 AxisRotationInv_.MultVec(point, p);
96 double a = p[2]*AxisOffsetInv_[2] + p[1]*AxisOffsetInv_[1];
97 double b = AxisOffsetInv_[2]*AxisOffsetInv_[2] +
98 AxisOffsetInv_[1]*AxisOffsetInv_[1];
99 double c = p[2]*p[2] + p[1]*p[1];
100 double scale = a/c + sqrt((a*a / c - b + 1.0) / c);
101 p = scale * p - AxisOffsetInv_;
104 double scale = 1.0/sqrt(p[1]*p[1] + p[2]*p[2]);
108 ProjectToNormalizedCoords_(p, xPhi);
111 res[0] = xPhi[0]*focallengthX_+principalX_;
112 res[1] = xPhi[1]*focallengthPhi_+principalY_;
118 int ProjectionParametersCylindric::
120 bool IgnoreDistortion)
const
122 p2d = ProjectLocal(point, IgnoreDistortion);
130 bool IgnoreDistortion)
const
133 UnProjectLocal(pos, p, d, IgnoreDistortion);
134 double radius = sqrt(d[2]*d[2] + d[1]*d[1]);
135 if (
Equal(radius, 0.0))
138 if (
Equal(d.NormL2(), 0.0))
141 return Pose_.LocalToGlobal(local);
150 direction = UnProjectToCylinderPoint(pos, IgnoreDistortion);
158 bool IgnoreDistortion)
const
161 UnProjectLocal(pos, p, d, IgnoreDistortion);
169 bool IgnoreDistortion)
const
175 p[0] = (pos[0]-principalX_)/focallengthX_;
176 phi = (pos[1]-principalY_)/focallengthPhi_;
181 AxisRotation_.MultVec(p, res);
183 if (UseAxisOffset_) {
193 if (ProjectionParametersBase::DoIntrinsicsDiffer(p)) {
215 p = ProjectLocal(localP, IgnoreDistortion);
217 return (p[0] >= 0.0 && p[1] >= 0.0 &&
218 p[0] <= (
double)(width_-1) && p[1] <= (
double)(height_-1));
228 SetCylinderRotation(rotation, axisType);
236 AxisRotation_ = rotation;
237 if (axisType == AXIS_PARALLEL) {
240 AxisRotation_.Mult(axisRot);
241 }
else if (axisType != AXIS_ORTHOGONAL) {
242 BIASERR(
"ProjectionParametersCylindric::SetCylinderRotation(): "
243 "Invalid axis type given!");
245 AxisRotationInv_ = AxisRotation_.Inverse();
246 AxisRotationInv_.MultVec(AxisOffset_, AxisOffsetInv_);
253 if (axisType == AXIS_ORTHOGONAL) {
255 AxisRotationInv_ = AxisRotation_.Inverse();
256 AxisRotationInv_.MultVec(AxisOffset_, AxisOffsetInv_);
257 }
else if (axisType == AXIS_PARALLEL) {
258 AxisRotation_.SetValueAsAxisRad(
Vector3<double>(0.0, -1.0, 0.0), 0.5*M_PI);
259 AxisRotationInv_ = AxisRotation_.Inverse();
260 AxisRotationInv_.MultVec(AxisOffset_, AxisOffsetInv_);
262 BIASERR(
"ProjectionParametersCylindric::SetCylinderType(): "
263 "Invalid axis type given!");
271 AxisOffset_ = offset;
272 AxisRotationInv_.MultVec(AxisOffset_, AxisOffsetInv_);
273 UseAxisOffset_ = !(
Equal(AxisOffset_.NormL2(), 0.0));
290 BIASERR(
"View difference for different projection models requested !");
302 return (M0-M1).
NormL2()*(1.0 - fabs(sin(phi)));
306 #ifdef BIAS_HAVE_XML2
308 int ProjectionParametersCylindric::
309 XMLGetClassName(std::string &TopLevelTag,
double &Version)
const
311 TopLevelTag =
"ProjectionParametersCylindric";
317 int ProjectionParametersCylindric::
318 XMLOut(
const xmlNodePtr Node,
XMLIO &XMLObject)
const
323 ProjectionParametersBase::XMLGetClassName(TopLevelName, Version);
326 ProjectionParametersBase::XMLOut(theNode, XMLObject);
329 XMLObject.
addAttribute(Node,
"FocallengthX", focallengthX_);
330 XMLObject.
addAttribute(Node,
"FocallengthPhi", focallengthPhi_);
338 AxisRotation_.GetAxisAngle(axis, angle);
341 XMLObject.
addAttribute(Node,
"CylinderAxisRotAngle", angle);
351 int ProjectionParametersCylindric::
352 XMLIn(
const xmlNodePtr Node,
XMLIO &XMLObject)
356 ProjectionParametersBase::XMLGetClassName(TopLevelName, Version);
358 xmlNodePtr childNode = XMLObject.
getChild(Node, TopLevelName);
360 BIASERR(
"Error in XML, tag " << TopLevelName <<
" not found!");
363 if (ProjectionParametersBase::XMLIn(childNode, XMLObject) != 0)
return -1;
373 BIASASSERT(quat.size() == 4);
377 std::vector<double> axis =
379 BIASASSERT(axis.size() == 3);
381 SetCylinderRotation(
Vector3<double>(axis[0], axis[1], axis[2]), angle);
393 std::vector<double> offset =
395 BIASASSERT(offset.size() == 3);
402 #endif // BIAS_HAVE_XML2
virtual BIAS::Vector3< double > GetC() const
Get projection center.
virtual Vector3< double > UnProjectToPointLocal(const HomgPoint2D &pos, const double &depth, bool IgnoreDistortion=false) const
Calculates a 3D point in a local camera coordinate system specified by camSystem, which belongs to th...
void addAttribute(const xmlNodePtr Node, const std::string &AttributeName, bool AttributeValue)
Add an attribute to a node.
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
virtual double ViewDifference(const ProjectionParametersBase *pPPB) const
Calculates difference of cylinder centers weighted with orientation angle up to now.
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
HomgPoint2D & Homogenize()
homogenize class data member elements to W==1 by divison by W
void SetZero()
set all values to 0
bool DoesPointProjectIntoImageLocal(const BIAS::Vector3< double > &localP, HomgPoint2D &p, bool IgnoreDistortion=false) const
Checks if 3D point projects into specified image and returns belonging 2D image point.
double focallengthPhi_
quasi K (principle point already contained in base class)
int MultVec(const Vector3< QUAT_TYPE > &vec, Vector3< QUAT_TYPE > &res) const
rotates the given Vector qith the quaternion ( q v q* ) the resulting vector is given in res ...
std::vector< double > getAttributeValueVecDbl(const xmlAttrPtr Attribute) const
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...
void SetCylinderOffset(const Vector3< double > &offset)
virtual bool Undistort(BIAS::HomgPoint2D &point2d) const
Interface defintion for lens undistortion 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.
Quaternion< double > AxisRotation_
Rotation of cylinder axis.
virtual BIAS::RMatrix GetR() const
Get orientation as rotation matrix R.
virtual const bool DoIntrinsicsDiffer(const ProjectionParametersBase *p) const
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
void UpdateQuasiK_()
Uses normalized coordinate ranges and image dimensions to calculate internals.
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_...
xmlAttrPtr getAttributeByName(const xmlNodePtr Node, const std::string &attribute_name)
search for a specific attribute
Camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
double getAttributeValueDouble(const xmlAttrPtr Attribute) const
virtual Vector3< double > UnProjectToCylinderPoint(const HomgPoint2D &pos, bool IgnoreDistortion=false) const
Calculates the 3D point on the cylinder hull local to the camera which belongs to the image position ...
void UpdateFromQuasiK_()
uses internal params to calculate normalized coordinate ranges.
void SetCylinderType(const ProjectionCylinderAxisEnum axisType)
virtual void UnProjectLocal(const HomgPoint2D &pos, Vector3< double > &pointOnRay, Vector3< double > &direction, bool IgnoreDistortion=false) const
Calculates the view ray, which belongs to the given position on the image plane, in local coordinates...
void SetCylinderRotation(const Vector3< double > &rotAxis, double rotAngle, const ProjectionCylinderAxisEnum axisType=AXIS_ORTHOGONAL)
Camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
ProjectionCylinderAxisEnum
Enumeration of typical directions of the cylinder axis, i.e.
Vector3< T > & Normalize()
normalize this vector to length 1
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
void SetValueAsAxisRad(const Vector3< QUAT_TYPE > &axis, QUAT_TYPE angle)
sets the quaternion with given rotation axis and angle (in rad)
virtual bool Distort(BIAS::HomgPoint2D &point2d) const
Interface defintion for lens distortion function, implemented by derived classes. ...