26 #include "Geometry/ProjectionParametersZoom.hh"
39 if (!IgnoreDistortion) Distort(pos);
45 bool IgnoreDistortion)
const
47 BIASERR(
"unfinished\n");
55 bool IgnoreDistortion)
const
58 UnProjectLocal(pos, p, d, IgnoreDistortion);
65 return Pose_.LocalToGlobal(local);
74 if (!IgnoreDistortion) Undistort(mpos);
76 invK_.Mult(mpos, direction);
84 double kc1,
double kc2,
85 double kc3,
double kc4)
87 if (knownparams_vect_.size()>0) {
88 if (knownparams_vect_[knownparams_vect_.size()-1].zoom<zoom) {
89 BIASERR(
"Zoom parameters only in descending order.");
100 knownparams_vect_.push_back(dp);
110 K_[0][0] = focallengthZoom_;
111 K_[1][1] = aspectratio_ * focallengthZoom_;
113 K_[0][2] = principalX_;
114 K_[1][2] = principalY_;
119 #ifdef BIAS_HAVE_XML2
122 double& Version)
const {
123 TopLevelTag =
"ProjectionParametersZoom";
130 XMLIO& XMLObject)
const {
138 xmlNodePtr childNode;
139 for (
unsigned int i=0; i<knownparams_vect_.size(); i++) {
141 XMLObject.
addAttribute(childNode,
"Zoom", knownparams_vect_[i].zoom);
143 knownparams_vect_[i].focallength);
144 XMLObject.
addAttribute(childNode,
"k1", knownparams_vect_[i].kc1);
145 XMLObject.
addAttribute(childNode,
"k2", knownparams_vect_[i].kc2);
146 XMLObject.
addAttribute(childNode,
"p1", knownparams_vect_[i].kc3);
147 XMLObject.
addAttribute(childNode,
"p2", knownparams_vect_[i].kc4);
159 xmlNodePtr childNode;
160 if ((childNode = XMLObject.
getChild(Node, TopLevelName))==NULL) {
161 BIASERR(
"Error in xml, no tag" << TopLevelName);
165 knownparams_vect_.clear();
167 while (childNode!=NULL) {
168 string nodename = XMLObject.
getNodeName(childNode);
169 if (nodename.compare(
"ZoomStep")==0) {
178 knownparams_vect_.push_back(kpv);
194 kc1Zoom_=relnorm*(knownparams_vect_[i].kc1-knownparams_vect_[i-1].kc1)+
195 knownparams_vect_[i-1].kc1;
197 kc2Zoom_=relnorm*(knownparams_vect_[i].kc2-knownparams_vect_[i-1].kc2)+
198 knownparams_vect_[i-1].kc2;
200 kc3Zoom_=relnorm*(knownparams_vect_[i].kc3-knownparams_vect_[i-1].kc3)+
201 knownparams_vect_[i-1].kc3;
203 kc4Zoom_=relnorm*(knownparams_vect_[i].kc4-knownparams_vect_[i-1].kc4)+
204 knownparams_vect_[i-1].kc4;
218 if (knownparams_vect_[0].zoom<zoom_){
220 zoom_=knownparams_vect_[0].zoom;
223 for (i=0;i<knownparams_vect_.size();i++){
224 if (knownparams_vect_[i].zoom<=zoom_){
231 focallengthZoom_=knownparams_vect_[i-1].focallength;
232 return knownparams_vect_[i-1].focallength;
235 if (knownparams_vect_[i].zoom==zoom_){
237 InterpolateDistortionFromIndex_(i+1,0);
238 return knownparams_vect_[i].focallength;
240 relnorm=(zoom_ - knownparams_vect_[i].zoom) /
241 (knownparams_vect_[i-1].zoom - knownparams_vect_[i].zoom);
244 InterpolateDistortionFromIndex_(i,relnorm);
245 absdist=relnorm*(knownparams_vect_[i].focallength-
246 knownparams_vect_[i-1].focallength);
247 return (absdist+knownparams_vect_[i-1].focallength);
255 if (kc1Zoom_==0 && kc2Zoom_==0 && kc3Zoom_==0 && kc4Zoom_==0)
return true;
264 double tandistx, tandisty;
270 x = (point2d[0]-principalX_)/focallengthZoom_;
271 y = (point2d[1]-principalY_)/(GetAspectratio()*focallengthZoom_);
274 rsquared = x*x + y*y;
277 raddist = 1 + kc1Zoom_*rsquared + kc2Zoom_*rsquared*rsquared;
279 tandistx = kc3Zoom_*twoxy + kc4Zoom_ * (rsquared + 2*x*x);
280 tandisty = kc4Zoom_*twoxy + kc3Zoom_ * (rsquared + 2*y*y);
283 distx = raddist * x + tandistx;
284 disty = raddist * y + tandisty;
287 point2d[0] = distx*focallengthZoom_ + principalX_;
288 point2d[1] = disty*GetAspectratio()*focallengthZoom_ + principalY_;
296 if (kc1Zoom_==0 && kc2Zoom_==0 && kc3Zoom_==0 && kc4Zoom_==0)
return true;
305 double tandistx, tandisty;
311 x = point2d[0] = (point2d[0]-principalX_)/focallengthZoom_;
312 y = point2d[1] = (point2d[1]-principalY_)/(GetAspectratio()*
316 for (
unsigned int i=0;i<5;i++){
319 rsquared = x*x + y*y;
322 raddist = 1 + kc1Zoom_*rsquared + kc2Zoom_*rsquared*rsquared;
324 tandistx = kc3Zoom_*twoxy + kc4Zoom_ * (rsquared + 2*x*x);
325 tandisty = kc4Zoom_*twoxy + kc3Zoom_ * (rsquared + 2*y*y);
328 distx = raddist * x + tandistx;
329 disty = raddist * y + tandisty;
332 x = point2d[0] - (distx-x);
333 y = point2d[1] - (disty-y);
338 point2d[0] = x*focallengthZoom_ + principalX_;
339 point2d[1] = y*GetAspectratio()*focallengthZoom_ + principalY_;
353 if (knownparams_vect_[0].focallength<focallengthZoom_){
354 focallengthZoom_=knownparams_vect_[0].focallength;
357 for (i=0;i<knownparams_vect_.size();i++){
358 if (knownparams_vect_[i].focallength<=focallengthZoom_){
367 if (knownparams_vect_[i].focallength==focallengthZoom_){
369 InterpolateDistortionFromIndex_(i+1,0);
370 return knownparams_vect_[i].zoom;
372 relnorm=(focallengthZoom_ - knownparams_vect_[i].focallength) /
373 (knownparams_vect_[i-1].focallength - knownparams_vect_[i].focallength);
376 InterpolateDistortionFromIndex_(i,relnorm);
377 absdist=relnorm*(knownparams_vect_[i].zoom-
378 knownparams_vect_[i-1].zoom);
379 return (absdist+knownparams_vect_[i-1].zoom);
virtual int XMLIn(const xmlNodePtr Node, XMLIO &XMLObject)
specialization of XML read function
void addAttribute(const xmlNodePtr Node, const std::string &AttributeName, bool AttributeValue)
Add an attribute to a node.
double InterpolateFocalLength_()
double InterpolateAbsZoom_()
xmlNodePtr getNextChild()
Get the next child of the parent specified in the last getFirstChild() call, the class remembers the ...
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
virtual int XMLOut(const xmlNodePtr Node, XMLIO &XMLObject) const
Specialization of XML write function.
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.
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...
virtual bool Undistort(BIAS::HomgPoint2D &point2d) const
Using the Matlab camera calibration toolbox parameters for an undistortion of distorted coordinates...
HomgPoint2D & Homogenize()
homogenize class data member elements to W==1 by divison by W
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...
BIAS::Vector3< T > GetNormalized() const
return a normalized vector of this
virtual int XMLGetClassName(std::string &TopLevelTag, double &Version) const
specialization of XML block name function
virtual int XMLIn(const xmlNodePtr Node, XMLIO &XMLObject)
Specialization of XML read function.
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 Mult(const T &scalar, Vector3< T > &dest) const
void InterpolateDistortionFromIndex_(unsigned int i, double relnorm)
xmlNodePtr getFirstChild(const xmlNodePtr ParentNode)
Get the first child of a given parent, or NULL for no childs.
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_...
double getAttributeValueDouble(const xmlAttrPtr Attribute) const
virtual bool Distort(BIAS::HomgPoint2D &point2d) const
Using the Matlab camera calibration toolbox parameters for a distortion of undistorted coordinates...
std::string getNodeName(const xmlNodePtr Node) const
Get the name of a given Node.
int AddZoomStep(double zoom, double focallength, double kc1, double kc2, double kc3, double kc4)
virtual int XMLGetClassName(std::string &TopLevelTag, double &Version) const
Specialization of XML block name function.
virtual int XMLOut(const xmlNodePtr Node, XMLIO &XMLObject) const
specialization of XML write function
void ComputeK_()
computes the cached KMatrix and its inverse call after every parameter change
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
class BIASGeometryBase_EXPORT HomgPoint3D
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 ...