26 #include "Geometry/ProjectionParametersSpherical.hh"
27 #include <Base/Common/CompareFloatingPoint.hh>
28 #include <MathAlgo/PolynomialSolve.hh>
29 #include <MathAlgo/Minpack.hh>
37 BIASERR(
"unfinished");
46 BIASERR(
"unfinished");
80 if(interpolatorInit_ &&
93 bool IgnoreDistortion)
const
95 ProjectLocal(localX, x, IgnoreDistortion);
97 sqrt( (x[0]-principalX_) * (x[0]-principalX_) +
98 (x[1]-principalY_) * (x[1]-principalY_) );
100 return (x[0]>=0.0 && x[1]>=0.0 &&
101 x[0]<=(
double)(width_-1) && x[1]<=(
double)(height_-1) &&
108 bool IgnoreDistortion)
const
111 if (
Equal(rPhiTheta[0], 0.0)) {
115 if (!IgnoreDistortion) {
116 TransfCoordSphere2Image_(rPhiTheta[2], rPhiTheta[1], p2d);
120 TransfCoordSphere2CalibImage_(rPhiTheta[2], rPhiTheta[1], p2d);
129 bool IgnoreDistortion)
const
132 UnProjectLocal(pos, p, d, IgnoreDistortion);
133 double radius = sqrt(d[2]*d[2] + d[1]*d[1] + d[0]*d[0]);
134 if(
Equal(radius, 0.0))
137 if(
Equal(d.NormL2(), 0.0))
141 return Pose_.LocalToGlobal(local);
153 if (!IgnoreDistortion) {
154 TransfCoordImage2Sphere_(pos, rPhiTheta[2], rPhiTheta[1]);
157 TransfCoordCalibImage2Sphere_(pos, rPhiTheta[2], rPhiTheta[1]);
166 bool IgnoreDistortion)
const
169 UnProjectLocal(pos, p, x, IgnoreDistortion);
177 const std::vector<double>& distAngles,
182 if (undistAngles.size() < 2 || distAngles.size() < 2 ||
183 undistAngles.size() != distAngles.size()) {
184 BIASERR(
"Initialization failed, expected min. 2 and same number of "
185 "undistorted and distorted angles (given " << undistAngles.size()
186 <<
" and " << distAngles.size() <<
" angles)!");
192 interpolatorAngle_.SetKnotPoints(undistAngles);
193 interpolatorAngle_.SetControlPoints(distAngles);
194 interpolatorAngle_.InitSpline();
195 interpolatorAngle_.Spline(dummy, undistAngles[0], 4);
196 MaxCamAngle_ = undistAngles[undistAngles.size()-1];
197 int lutsamples = int(radius_*100);
198 interpolatorAngle_.PrepareLuTSpline(undistAngles[0], MaxCamAngle_,
202 interpolatorAngleBack_.SetKnotPoints(distAngles);
203 interpolatorAngleBack_.SetControlPoints(undistAngles);
204 interpolatorAngleBack_.InitSpline();
205 interpolatorAngleBack_.Spline(dummy, distAngles[0], 4);
206 interpolatorAngleBack_.PrepareLuTSpline(distAngles[0],
207 distAngles[distAngles.size()-1],
209 interpolatorInit_ =
true;
218 if (interpolatorInit_) {
219 BIASWARN(
"Recalculating undistortion polynomial since max. camera "
220 "angles has been reset manually!");
221 RecalculateUndistortion(maxCamAngle);
223 MaxCamAngle_ = maxCamAngle;
231 if (!interpolatorInit_) {
232 BEXCEPTION(
"Undistortion polynomial has not been set previously! "
233 "Call SetUndistortion instead of RecalculateUndistortion!");
235 if (maxCamAngle > MaxCamAngle_) {
236 BIASWARN(
"Extrapolating undistortion polynomial linearly since new "
237 "max. cam angle > old max. cam angle. Might result in strange "
240 std::vector<double> dummy;
241 interpolatorAngle_.GetKnotPoints(dummy);
242 const unsigned int numSamples = dummy.size();
243 std::vector<double> undistAngles(numSamples);
244 std::vector<double> distAngles(numSamples);
245 double distMaxCamAngle = MaxCamAngle_;
246 interpolatorAngle_.Spline(distMaxCamAngle, MaxCamAngle_, 4);
247 const double linearExtrapolationFactor = distMaxCamAngle / MaxCamAngle_;
249 const double thetaStep = maxCamAngle / (numSamples - 1);
250 for (
unsigned int i = 0; i < numSamples; i++, theta += thetaStep) {
251 undistAngles[i] = theta;
253 if (theta < MaxCamAngle_) {
254 interpolatorAngle_.Spline(distAngles[i], theta, 4);
257 distAngles[i] = theta * linearExtrapolationFactor;
261 SetUndistortion(undistAngles, distAngles, radius_);
267 const std::vector<double>& coefficients,
268 const bool mapsRadiusToZ)
270 double pix_def, ang_def, ang_opt;
273 std::vector<double> radiusInPixel, thetaInRad;
274 double maxradius = 0;
275 MaxCamAngle_ = maxCamAngle;
277 acCoeff_ = coefficients;
278 std::vector<double> distAngles;
279 std::vector<double> undistAngles;
280 unsigned int width, height;
281 GetImageSize(width, height);
284 unsigned int rad = radius_ != 0.0 ? (
unsigned int)rint(radius_) :
285 (
unsigned int)rint(
double(width)/2.0);
292 for (
unsigned int currentRadius = 0; currentRadius <= rad; currentRadius++)
295 radiusInPixel.push_back(currentRadius);
300 double currentTheta = atan2(currentRadius, -z);
301 thetaInRad.push_back(currentTheta);
303 if (currentRadius > maxradius && currentTheta < MaxCamAngle_)
304 maxradius = currentRadius;
308 for (
unsigned int sampleIndex = 0; sampleIndex <= rad; sampleIndex++)
314 double currentTheta = MaxCamAngle_ * double(sampleIndex) / double(rad);
318 thetaInRad.push_back(currentTheta);
320 if (radiusInPixel.back() > maxradius)
321 maxradius = radiusInPixel.back();
329 const int steps = 14;
332 double thetaPerPixelCenter = thetaInRad[1] - thetaInRad[0];
339 for (
double sampleTheta = 0; sampleTheta - 1e-10 <= MaxCamAngle_;
340 sampleTheta += MaxCamAngle_/steps)
342 ang_opt = sampleTheta;
344 interpolatorThetaToPixels.
Spline(pix_def, ang_opt);
346 ang_def = pix_def * thetaPerPixelCenter;
347 distAngles.push_back(ang_opt);
348 undistAngles.push_back(ang_def);
351 return SetUndistortion(distAngles, undistAngles, radius_);
357 double &theta,
double &phi)
const
362 double rSrc = sqrt((SourceH[0]-principalX_)*(SourceH[0]-principalX_)+
363 (SourceH[1]-principalY_)*(SourceH[1]-principalY_));
364 phi = atan2(SourceH[1]-principalY_,SourceH[0]-principalX_);
366 if (interpolatorInit_) {
367 double CalMaxCamAngle = 0;
370 (&interpolatorAngle_)->Spline(CalMaxCamAngle, MaxCamAngle_, 4);
374 double RadiusCalMax = MaxCamAngle_ / CalMaxCamAngle * radius_;
376 double CalTheta = rSrc /RadiusCalMax * MaxCamAngle_;
378 (&interpolatorAngleBack_)->Spline(theta, CalTheta, 4);
381 theta = rSrc / radius_ * MaxCamAngle_;
392 double viewTheta, viewPhi;
394 TransfCoordImage2Sphere_(viewCenter, viewTheta, viewPhi);
433 if (theta > MaxCamAngle_)
return -1;
434 if (phi>M_PI) phi-=2*M_PI;
437 if (interpolatorInit_) {
438 double CalMaxCamAngle=0;
441 interpolatorAngle_.SplineFromLuT(CalMaxCamAngle, MaxCamAngle_);
442 interpolatorAngle_.SplineFromLuT(CalTheta, theta);
444 rSrc = radius_ * CalTheta / CalMaxCamAngle;
446 rSrc = radius_ * theta / MaxCamAngle_;
449 Source[0] = rSrc * cos(phi) + principalX_;
450 Source[1] = rSrc * sin(phi) + principalY_;
461 EnforceNormalRange_(theta, phi);
464 if (interpolatorInit_) {
465 double CalMaxCamAngle=0;
468 interpolatorAngle_.SplineFromLuT(CalMaxCamAngle, MaxCamAngle_);
469 interpolatorAngle_.SplineFromLuT(CalTheta, theta);
471 rSrc = radius_ * CalTheta / CalMaxCamAngle;
473 rSrc = radius_ * theta / MaxCamAngle_;
476 Source[0] = rSrc * cos(phi) + principalX_;
477 Source[1] = rSrc * sin(phi) + principalY_;
486 EnforceNormalRange_(theta, phi);
489 double rSrc = radius_ * theta / MaxCamAngle_;
491 Source[0] = rSrc * cos(phi) + principalX_;
492 Source[1] = rSrc * sin(phi) + principalY_;
499 double &theta,
double &phi)
const
504 double rSrc = sqrt((SourceH[0]-principalX_)*(SourceH[0]-principalX_)+
505 (SourceH[1]-principalY_)*(SourceH[1]-principalY_));
506 phi = atan2(SourceH[1]-principalY_,SourceH[0]-principalX_);
507 theta = rSrc / radius_ * MaxCamAngle_;
523 BIASASSERT(fabs(theta)<20.0);
524 BIASASSERT(fabs(phi)<20.0);
527 while (theta >= M_PI*2.0) theta -= M_PI*2.0;
528 while (theta < 0.0) theta += M_PI*2.0;
531 while (phi >= M_PI) phi -= M_PI*2.0;
532 while (phi < -M_PI) phi += M_PI*2.0;
543 BIASERR(
"View difference for different projection models requested !");
553 A[0] = R[0][2]; A[1] = R[1][2]; A[2] = R[2][2];
556 A0[0] = R0[0][2]; A0[1] = R0[1][2]; A0[2] = R0[2][2];
560 double coshalffieldofview = cos(MaxCamAngle_);
562 double newness = (GetC()-C0).NormL2();
563 newness += 1.0/(1.0-coshalffieldofview)*(1.0-fabs(A.
ScalarProduct(A0)));
570 const double perspHalfViewingAngleDEG,
573 double halffov = perspHalfViewingAngleDEG * M_PI/180.0;
576 double viewTheta, viewPhi;
577 TransfCoordImage2Sphere_(viewCenter, viewTheta, viewPhi);
583 vector<Vector3<double> > vecRPT;
584 vector<HomgPoint2D> vecSpProj;
589 for (
unsigned int i=0; i<vecRPT.size(); i++) {
591 rot.
MultVec(vecRPT[i].CoordSphereToEuclidean(), vecRot);
592 vecSpProj.push_back(ProjectLocal(vecRot));
595 for (
unsigned int i=0; i<vecSpProj.size(); i+=2) {
596 if ((vecSpProj[i]-vecSpProj[i+1]).NormL2()>MaxPixDiff) {
597 MaxPixDiff = (vecSpProj[i]-vecSpProj[i+1]).NormL2();
600 int imgsize = int(MaxPixDiff*tan(halffov)/tan(M_PI/180.0));
601 double focallength = imgsize/tan(halffov);
604 pp.
SetQC(GetPose().GetQ()*rot, GetC());
620 double ang = maxangle;
622 if (ang > GetMaxCamAngle()) ang = GetMaxCamAngle() - 0.01;
627 #ifdef BIAS_HAVE_XML2
630 double& Version)
const
632 TopLevelTag =
"ProjectionParametersSpherical";
639 XMLIO& XMLObject)
const
649 xmlNodePtr childNode, childNode2;
652 if (interpolatorInit_) {
654 vector<double> controlpoints, knotpoints;
655 interpolatorAngle_.GetControlPoints(controlpoints);
656 interpolatorAngle_.GetKnotPoints(knotpoints);
657 for (
unsigned int i=0; i<controlpoints.size(); i++) {
658 childNode2= XMLObject.
addChildNode(childNode,
"MeasPoint");
659 XMLObject.
addAttribute(childNode2,
"AngRay", knotpoints[i]);
660 XMLObject.
addAttribute(childNode2,
"AngDeformed", controlpoints[i]);
672 xmlNodePtr childNode;
675 if ((childNode = XMLObject.
getChild(Node, TopLevelName))==NULL) {
676 BIASERR(
"Error in xml, no tag" << TopLevelName);
688 childNode = XMLObject.
getChild(Node,
"Distortion");
692 string nodeName2 = (childNode2 ? XMLObject.
getNodeName(childNode2) :
"");
693 if (nodeName2.compare(
"MeasPoint") == 0)
696 vector<double> AngleCorrX, AngleCorrY;
699 if (nodeName2.compare(
"MeasPoint") == 0) {
702 AngleCorrX.push_back(x);
703 AngleCorrY.push_back(y);
707 SetUndistortion(AngleCorrX, AngleCorrY, radius_);
712 childNode2 = XMLObject.
getChild(childNode,
"AngleCalibPoly");
715 vector<double> coefficients;
721 InitAngleCorrFromPoly(MaxCamAngle_, coefficients);
723 BIASERR(
"No polynomial distortion parameters given (either XML nodes MeasPoint or "
724 "XML node AngleCalibPoly as child of XML node Distortion)!");
729 interpolatorInit_ =
false;
731 BIASERR(
"Max. camera angle is not set (either use XML attribut MaxAngle or set "
732 "distortion parameters)!");
741 int PolynomialWithoutLinearMonomialError(
void *p,
int m,
int n,
const double *x,
742 double *fvec,
int iflag)
744 vector<double> params;
747 params.push_back(x[0]);
749 for (
int i=1; i<n; i++) {
750 params.push_back(x[i]);
755 for (
int j=0; j<m; j++) {
766 const std::vector<double>& x,
767 const std::vector<double>& y,
768 std::vector<double> &coefficients)
772 BIASASSERT(x.size()==y.size());
773 coefficients.resize(degree+1, 0.0);
776 for (
unsigned int row=0; row<x.size(); row++) {
777 const double & xc = x[row];
778 for (
unsigned int col=0; col<degree+1; col++) {
781 for (
unsigned int colpower=0; colpower<col; colpower++)
787 for (
unsigned int row=0; row<x.size(); row++) {
788 A2[row][0] = A[row][0];
789 for (
unsigned int col=2; col<degree+1; col++) {
790 A2[row][col-1] = A[row][col];
795 int result = mySVD.
Solve(A2, B, X);
799 coeff[0] = coefficients[0] = X[0];
801 for (
unsigned int col=2; col<degree+1; col++) {
802 coeff[col-1] = coefficients[col] = X[col-1];
806 myvaluesPolynomialWithoutLinearMonomialError = y;
807 mylocationsPolynomialWithoutLinearMonomialError = x;
813 MINPACK_DEF_TOLERANCE))!=0) {
814 BIASERR(
"Error in Levenberg-Marquardt-Optimization of F: "<<res);
818 coefficients[0] = resultcoeff[0];
820 for (
unsigned int col=2; col<degree+1; col++) {
821 coefficients[col] = resultcoeff[col-1];
829 const unsigned int degree,
830 bool LinearMonomialIsZero)
833 a.reserve((
unsigned int)(radius_+1)); b.reserve((
unsigned int)(radius_+1));
835 for (
unsigned int i=1; i<radius_; i++) {
837 HomgPoint2D pos(principalX_+
double(i),principalY_,1);
839 TransfCoordImage2Sphere_(pos, rPhiTheta[2], rPhiTheta[1]);
847 ret *= -1.0 * double(i) / sqrt((ret[0]*ret[0] + ret[1]*ret[1]));
849 a.push_back(
double(i));
853 if (LinearMonomialIsZero) {
854 return FitPolynomialWithoutLinearMonomial_(degree, a, b, coefficients);
863 const unsigned int degree,
864 bool LinearMonomialIsZero)
867 a.reserve((
unsigned int)(radius_+1));
868 b.reserve((
unsigned int)(radius_+1));
870 double CalMaxCamAngle=0;
871 interpolatorAngle_.SplineFromLuT(CalMaxCamAngle, MaxCamAngle_);
875 for (
unsigned int i=1; i<radius_; i++) {
877 double theta = MaxCamAngle_ * double(i)/double(radius_);
880 interpolatorAngle_.SplineFromLuT(CalTheta, theta);
882 double rSrc = radius_ * CalTheta/CalMaxCamAngle;
888 if (LinearMonomialIsZero) {
889 return FitPolynomialWithoutLinearMonomial_(degree, a, b, coefficients);
901 if(posX>static_cast<int>(width_-1)) posX=width_-1;
902 if(posY>static_cast<int>(height_-1)) posY=height_-1;
907 int rad = (int)rint(floor(radius_));
908 int center[2] = {(int)rint(principalX_), (int)rint(principalY_)};
909 fovCircle_.Init(center,rad);
911 fovCircle_.GetNext(next);
912 ProjectOutsidePositions_(next[0], next[1]);
920 bool res = fovCircle_.GetNext(next);
921 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.
virtual void SetCov(const Matrix< POSE_TYPE > &Cov)
Set pose covariance matrix with respect to C, Q.
void InitSpline()
call this for restart at t= first knot point initiates recalculation of all polynom coefficients at f...
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...
int FitPolynomial(const unsigned int degree, const std::vector< double > &x, const std::vector< double > &y, std::vector< double > &coefficients)
given locations x and measurements y=f(x) approximate f by a polynomial of arbitrary degree and retur...
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 SetKnotPoints(const std::vector< double > &kPnt)
set the additional knot points, if you want nonuniform interpolation.
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 ...
base class for solving polynomial equations
POLYNOMIALSOLVE_TYPE EvaluatePolynomial(const POLYNOMIALSOLVE_TYPE x, const std::vector< POLYNOMIALSOLVE_TYPE > &coeff) const
numerically robust way to evaluate a polynomial at position x, uses Horner scheme (same principle as ...
virtual int XMLIn(const xmlNodePtr Node, XMLIO &XMLObject)
Specialization of XML read function.
this class interpolates a function y=f(t) between given control points (the y-values) ...
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.
int Spline(double &res, double t, unsigned int k=3)
these functions do the Spline interpolation which reaches each control point.
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.
void SetControlPoints(const std::vector< double > &cPnt1)
set the control points, which control the interpolating curve
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).
virtual void SetQC(const BIAS::Quaternion< double > &Q, const BIAS::Vector3< double > &C)
Set pose from unit quaternion Q and projection center C.
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)