26 #include "Geometry/ProjectionParametersSphericalFast.hh"
27 #include <Base/Common/CompareFloatingPoint.hh>
28 #include <MathAlgo/PolynomialSolve.hh>
29 #include <MathAlgo/Minpack.hh>
36 BIASERR(
"unfinished");
44 BIASERR(
"unfinished");
57 cout <<
"casted to null pointer " << endl;
62 cout <<
"radii do not match " << endl;
67 cout <<
"cam angles do not match " << endl;
78 bool IgnoreDistortion)
const {
80 ProjectLocal(localX, x, IgnoreDistortion);
82 sqrt( (x[0]-principalX_) * (x[0]-principalX_) +
83 (x[1]-principalY_) * (x[1]-principalY_) );
85 return (x[0]>=0.0 && x[1]>=0.0 &&
86 x[0]<=(
double)(width_-1) && x[1]<=(
double)(height_-1) &&
93 bool IgnoreDistortion)
const {
96 if (
Equal(rPhiTheta[0], 0.0)) {
101 TransfCoordSphere2Image_(rPhiTheta[2], rPhiTheta[1], p2d);
108 bool IgnoreDistortion)
const {
110 UnProjectLocal(pos, p, d, IgnoreDistortion);
111 double radius = sqrt(d[2]*d[2] + d[1]*d[1] + d[0]*d[0]);
112 if(
Equal(radius, 0.0))
115 if(
Equal(d.NormL2(), 0.0))
119 return Pose_.LocalToGlobal(local);
129 TransfCoordImage2Sphere_(pos, rPhiTheta[2], rPhiTheta[1]);
140 bool IgnoreDistortion)
const {
142 UnProjectLocal(pos, p, x, IgnoreDistortion);
151 maxCamAngle_ = maxCamAngle;
158 const std::vector<double>& coefficients,
159 const bool undistCoeffs) {
162 maxCamAngle_ = maxCamAngle;
165 coeffsUndist_ = coefficients;
167 EstimateDistortionPolynomial(coeffsDist_, coeffsUndist_.size());
169 coeffsDist_ = coefficients;
171 EstimateUndistortionPolynomial(coeffsUndist_, coeffsDist_.size());
185 unsigned int counter = 0;
186 for(
double a=0.0; a < maxCamAngle_; a+=0.1){
187 val = EvaluatePolynomial(a, coeffsDist_);
188 valInv = EvaluatePolynomial(val, coeffsUndist_);
189 error = fabs(a-valInv);
194 cout <<
"TestPolynomial Inversion: average error " << error/(double)counter << endl;
201 double &theta,
double &phi)
const {
206 double rSrc = sqrt((sourceH[0]-principalX_)*(sourceH[0]-principalX_)+
207 (sourceH[1]-principalY_)*(sourceH[1]-principalY_));
208 phi = atan2(sourceH[1]-principalY_,sourceH[0]-principalX_);
210 double calMaxCamAngle = EvaluatePolynomial(maxCamAngle_, coeffsDist_);
213 double RadiusCalMax = maxCamAngle_ / calMaxCamAngle * radius_;
215 double calTheta = rSrc /RadiusCalMax * maxCamAngle_;
216 theta = EvaluatePolynomial(calTheta, coeffsUndist_);
225 EnforceNormalRange_(theta, phi);
227 if(theta > maxCamAngle_){
235 double calMaxCamAngle = EvaluatePolynomial(maxCamAngle_, coeffsDist_);
236 double calTheta = EvaluatePolynomial(theta, coeffsDist_);
239 rSrc = radius_ * calTheta / calMaxCamAngle;
241 Source[0] = rSrc * cos(phi) + principalX_;
242 Source[1] = rSrc * sin(phi) + principalY_;
257 BIASASSERT(fabs(theta)<20.0);
258 BIASASSERT(fabs(phi)<20.0);
261 while (theta >= M_PI*2.0) theta -= M_PI*2.0;
262 while (theta < 0.0) theta += M_PI*2.0;
265 while (phi >= M_PI) phi -= M_PI*2.0;
266 while (phi < -M_PI) phi += M_PI*2.0;
276 BIASERR(
"View difference for different projection models requested !");
286 A[0] = R[0][2]; A[1] = R[1][2]; A[2] = R[2][2];
289 A0[0] = R0[0][2]; A0[1] = R0[1][2]; A0[2] = R0[2][2];
293 double coshalffieldofview = cos(maxCamAngle_);
295 double newness = (GetC()-C0).NormL2();
296 newness += 1.0/(1.0-coshalffieldofview)*(1.0-fabs(A.
ScalarProduct(A0)));
303 const double perspHalfViewingAngleDEG,
305 double halffov = perspHalfViewingAngleDEG * M_PI/180.0;
308 double viewTheta, viewPhi;
309 TransfCoordImage2Sphere_(viewCenter, viewTheta, viewPhi);
315 vector<Vector3<double> > vecRPT;
316 vector<HomgPoint2D> vecSpProj;
321 for (
unsigned int i=0; i<vecRPT.size(); i++) {
323 rot.
MultVec(vecRPT[i].CoordSphereToEuclidean(), vecRot);
324 vecSpProj.push_back(ProjectLocal(vecRot));
327 for (
unsigned int i=0; i<vecSpProj.size(); i+=2) {
328 if ((vecSpProj[i]-vecSpProj[i+1]).NormL2()>MaxPixDiff) {
329 MaxPixDiff = (vecSpProj[i]-vecSpProj[i+1]).NormL2();
332 int imgsize = int(MaxPixDiff*tan(halffov)/tan(M_PI/180.0));
333 double focallength = imgsize/tan(halffov);
350 double ang = maxangle;
352 if (ang > GetMaxCamAngle()) ang = GetMaxCamAngle() - 0.01;
357 #ifdef BIAS_HAVE_XML2
360 double& Version)
const {
361 TopLevelTag =
"ProjectionParametersSphericalFast";
368 XMLIO& XMLObject)
const {
377 xmlNodePtr childNode, childNode2;
381 for (
unsigned int i=0; i< coeffsDist_.size(); i++) {
382 childNode2= XMLObject.
addChildNode(childNode,
"coeffsDist");
383 XMLObject.
addAttribute(childNode2,
"coeff", coeffsDist_[i]);
385 childNode = XMLObject.
addChildNode(Node,
"Undistortion");
386 for (
unsigned int i=0; i< coeffsUndist_.size(); i++) {
387 childNode2= XMLObject.
addChildNode(childNode,
"coeffsUndist");
388 XMLObject.
addAttribute(childNode2,
"coeff", coeffsUndist_[i]);
398 xmlNodePtr childNode;
401 if ((childNode = XMLObject.
getChild(Node, TopLevelName))==NULL) {
402 BIASERR(
"Error in xml, no tag" << TopLevelName);
414 childNode = XMLObject.
getChild(Node,
"Distortion");
417 string nodeName2 = (childNode2 ? XMLObject.
getNodeName(childNode2) :
"");
418 if (nodeName2.compare(
"coeffsDist") == 0){
423 if (nodeName2.compare(
"coeffsDist") == 0) {
425 coeffsDist_.push_back(c);
429 InitPolyCoeffs(maxCamAngle_, coeffsDist_,
false);
439 int InvertPolynomialErrorFunction(
void *p,
int m,
int n,
const double *x,
440 double *fvec,
int iflag) {
444 vector<double> params;
445 for (
int i=0; i<n; i++){
446 params.push_back(x[i]);
450 for (
int j=0; j<m; j++){
463 std::vector<double>& a,
464 std::vector<double>& b,
465 unsigned int degree){
469 coefficients.clear();
470 coefficients.resize(degree, 0.0);
473 for (
unsigned int row=0; row < a.size(); row++) {
475 for (
unsigned int col=1; col < degree; col++) {
476 A[row][col] = A[row][col-1]*a[row]*a[row];
483 int result = mySVD.
Solve(A, B, X);
485 BIASERR(
"linear solution failed");
494 myvaluesPolynomial = b;
495 mylocationsPolynomial = a;
497 if ((result =
LevenbergMarquardt(&InvertPolynomialErrorFunction, (
void*)(
this), a.size(), degree,
499 MINPACK_DEF_TOLERANCE))!=0) {
500 BIASERR(
"Error in Levenberg-Marquardt-Optimization of Polynomial: "<< result);
502 for (
unsigned int col=0; col<degree; col++) coefficients[col] = X[col];
505 for (
unsigned int col=0; col<degree; col++) {
506 coefficients[col] = resultcoeff[col];
514 if (result!=0)
return 1;
523 const unsigned int degree) {
527 double stepSize = 1.0/180*M_PI;
528 unsigned int numSamples = (
unsigned int)(maxCamAngle_/stepSize+1);
529 vector<double> a(numSamples), b(numSamples);
533 unsigned int counter=0;
534 for(
double theta=0.0; theta < maxCamAngle_ && counter < numSamples; theta+=stepSize, counter++){
535 double calTheta = EvaluatePolynomial(theta, coeffsDist_);
536 a[counter] = calTheta;
542 return FitPolynomial(coefficients, a, b, degree);
548 const unsigned int degree) {
553 double stepSize = 1.0/180*M_PI;
557 unsigned int counter = 0;
558 unsigned int highBoundary = (
unsigned int)(maxCamAngle_/stepSize*2);
561 a.reserve(highBoundary);
562 b.reserve(highBoundary);
564 while(calTheta <= maxCamAngle_ && counter < highBoundary){
565 calTheta = EvaluatePolynomial(theta, coeffsUndist_);
567 b.push_back(calTheta);
574 return FitPolynomial(coefficients, a, b, degree);
583 if(posX>static_cast<int>(width_-1)) posX=width_-1;
584 if(posY>static_cast<int>(height_-1)) posY=height_-1;
589 int rad = (int)rint(floor(radius_));
590 int center[2] = {(int)rint(principalX_), (int)rint(principalY_)};
591 fovCircle_.Init(center,rad);
593 fovCircle_.GetNext(next);
594 ProjectOutsidePositions_(next[0], next[1]);
602 bool res = fovCircle_.GetNext(next);
603 ProjectOutsidePositions_(next[0], next[1]);
virtual BIAS::KMatrix GetFakeKMatrix(double &imgsize, int resolution=0, const double &maxangle=1.4) const
Returns a fake KMatrix for the camera.
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.
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.
computes and holds the singular value decomposition of a rectangular (not necessarily quadratic) Matr...
BIAS::Vector3< T > CoordEuclideanToSphere() const
coordinate transform.
Vector< double > Solve(const Vector< double > &y) const
virtual int XMLOut(const xmlNodePtr Node, XMLIO &XMLObject) const
Specialization of XML write function.
camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
virtual void SetPrincipal(const double x, const double y)
Set principal point in pixels relative to top left corner, virtual overload to recalculate K_...
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
virtual void SetImageSize(const unsigned int w, const unsigned int h)
Set image dimensions (in pixels).
HomgPoint2D & Homogenize()
homogenize class data member elements to W==1 by divison by W
virtual const bool DoIntrinsicsDiffer(const ProjectionParametersBase *p) const
void SetZero()
set all values to 0
void SetUndistortion(double kc1, double kc2)
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 ...
virtual int XMLIn(const xmlNodePtr Node, XMLIO &XMLObject)
Specialization of XML read function.
virtual void SetPose(const BIAS::Pose pose)
Set pose from pose object.
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.
virtual BIAS::RMatrix GetR() const
Get orientation as rotation matrix R.
long int LevenbergMarquardt(minpack_funcder_mn fun, void *p, long int m, long int n, Vector< double > &InitialGuess, Vector< double > &Result, double Tolerance)
Solves an overdetermined system f(x) = 0 of m non-linear equations with n parameters using the Levenb...
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.
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_...
xmlAttrPtr getAttributeByName(const xmlNodePtr Node, const std::string &attribute_name)
search for a specific attribute
double x
If using BorderPixel methods these are the coordinates of the pixel.
BIAS::Vector3< T > CoordSphereToEuclidean() const
coordinate transfrom.
double getAttributeValueDouble(const xmlAttrPtr Attribute) const
std::string getNodeName(const xmlNodePtr Node) const
Get the name of a given Node.
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
void SetFocalLengthAndAspect(double f, double AspectRatio)
Set the current camera focal length in pixel and the a spect ratio.
virtual int XMLGetClassName(std::string &TopLevelTag, double &Version) const
Specialization of XML block name function.
Camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
Vector3< T > & Normalize()
normalize this vector to length 1
class BIASGeometryBase_EXPORT HomgPoint3D
void SetValueAsAxisRad(const Vector3< QUAT_TYPE > &axis, QUAT_TYPE angle)
sets the quaternion with given rotation axis and angle (in rad)