26 #ifndef __BIAS_ProjectionParametersPerspective_hh__
27 #define __BIAS_ProjectionParametersPerspective_hh__
28 #include "bias_config.h"
30 #include <Base/Common/BIASpragmaStart.hh>
31 #include <Geometry/ProjectionParametersBase.hh>
32 #include <Base/Geometry/KMatrix.hh>
33 #include <Geometry/PMatrix.hh>
34 #include <Base/Image/Image.hh>
37 #define UNDISTORT_ITERS 20
62 const unsigned int height = 0)
69 const unsigned int width = 10000,
70 const unsigned int height = 10000)
85 this->invK_ = P.
invK_;
115 int SetIntrinsics(
double minPhi,
double maxPhi,
116 double minTheta,
double maxTheta,
117 double angleStep,
double aspectratio = 1.0);
120 unsigned int width,
unsigned int height);
124 virtual int GetMinimalAngularSamplingStep(
double& minAngleStep);
131 bool IgnoreDistortion =
false)
const;
142 bool IgnoreDistortion =
false)
const;
156 bool IgnoreDistortion =
false)
const;
162 const double& depth = 1.0,
163 bool IgnoreDistortion =
false)
const;
171 virtual void UnProjectLocal(
const HomgPoint2D& pos,
174 bool IgnoreDistortion =
false)
const;
181 bool IgnoreDistortion =
false,
bool Normalize =
false);
186 const double& zDistance,
187 bool IgnoreDistortion =
false)
const;
238 bool GetCutOutParameters(
const Vector2<int>& centerPoint,
239 const unsigned int halfWidth,
240 const unsigned int halfHeight,
260 double& kc3,
double& kc4)
const
269 double& kc3,
double& kc4,
280 return ((kc1_!=0.0) || (kc2_!=0.0) || (kc3_!=0.0) || (kc4_!=0.0));
288 const unsigned int width = 512,
289 const unsigned int height = 512) {
290 SetImageSize(width, height);
291 focallength_ = double(width)/(2.0*tan(FoV/2.0));
293 principalX_ = 0.5*double(width-1);
294 principalY_ = 0.5*double(height-1);
316 double AspectRatio) {
374 double kc3,
double kc4)
387 double kc3,
double kc4,
399 double kc3,
double kc4){
426 double& kc3,
double& kc4)
const {
429 BIASWARN(
"Distortion type is not inverse, parameters might be invalid");
442 double& kc3,
double& kc4,
447 BIASWARN(
"Distortion type is not DISTYPE_BROWN. the parameters might not be valid.")
460 BIASWARN(
"Distortion type is not DistypeRAD3, the parameters might be invalid." << distType_);
471 BIASWARN(
"Distortion type is not DistypeINVRAD3, the parameters might be invalid.");
483 return PMatrix(K_, GetR(), GetC());
501 BIASASSERT(K[0][0]!=0);
502 focallength_ = K[0][0];
505 double aspectratio = K[1][1]/K[0][0];
507 K_ = K; invK_ = K.
Invert();
526 virtual void Rescale(
double ratio,
const double offset = 0.0) {
528 focallength_ /= ratio;
539 virtual void Rescale(
unsigned int width,
unsigned int height){
540 unsigned int oldwidth = width_;
541 unsigned int oldheight = height_;
543 double ratio = (double)oldwidth/(
double)width_;
544 focallength_ /= ratio;
546 aspectratio_ /= ((double)oldheight/(
double)height_) / ((
double)oldwidth/(double)width_);
556 void GetIdealK(
KMatrix& K)
const;
557 void SetIdealImageSize(
unsigned int width,
unsigned int height);
558 void GetIdealImageSize(
unsigned int& width,
unsigned int& height)
const;
569 float TransformPolarToCartesianCoordinates(
const HomgPoint2D& pos,
const float depthPolar);
579 float TransformCartesianToPolarCoordinates(
const HomgPoint2D& pos,
const float depthCartesian);
593 void UpdateMinZLocal();
605 const bool transpose=
false);
617 const bool transpose=
false);
634 const unsigned int width,
635 const unsigned int height,
637 const bool transpose=
false);
641 #ifdef BIAS_HAVE_XML2
643 virtual int XMLGetClassName(std::string& TopLevelTag,
644 double& Version)
const;
647 virtual int XMLOut(
const xmlNodePtr Node,
XMLIO& XMLObject)
const;
650 virtual int XMLIn(
const xmlNodePtr Node,
XMLIO& XMLObject);
669 idealImageWidth_ = 512;
670 idealImageHeight_ = 512;
704 int GetMinimalAngleInCorner_(
const HomgPoint2D& corner,
double& minAngle);
707 inline void Distort_(
const double x,
const double y,
708 double& dist_x,
double& dist_y)
const;
710 inline bool Undistort_(
const double dist_x,
const double dist_y,
711 double& x,
double& y)
const;
719 os <<
"ProjectionParametersPerspective:" << std::endl;
720 os <<
"Pose " << p.
GetPose() << std::endl;
723 os<<
"ImageSize: " << cx <<
" " << cy << std::endl;
724 os<<
"Skew: "<<p.
skew_<<std::endl;
725 os<<
"K:"<<p.
GetK()<<std::endl;
726 std::string dist_model;
729 dist_model =
"Browns distortion model";
733 dist_model =
"Default distortion model";
736 dist_model =
"Inverse rad distorion model";
739 dist_model =
"no distortion model";
742 dist_model =
"radial distortion, 3 coeffs";
745 dist_model =
"inverse radial distortion, 3 coeffs";
747 os<<dist_model<<
": ";
748 os<<
" kc1_: " <<p.
kc1_<<
" kc2_: "<<p.
kc2_<<
" kc3_: "
751 os <<
" r0_: "<< p.
r0_;
752 os <<
"\nminZlocal: "<<p.
minZlocal_<<std::endl;
759 double& dist_x,
double& dist_y)
const
761 double twoxy, xx, yy;
783 rc =
kc1_ * r0squared +
kc2_* r0squared*r0squared;
785 raddist = 1 +
kc1_*rsquared +
kc2_*rsquared*rsquared - rc;
789 raddist = 1 +
kc1_*rsquared +
kc2_*rsquared*rsquared;
793 dist_x = raddist * x +
kc3_*twoxy +
kc4_ * (rsquared + 2.0*xx);
794 dist_y = raddist * y +
kc4_*twoxy +
kc3_ * (rsquared + 2.0*yy);
797 dist_x = raddist * x;
798 dist_y = raddist * y;
803 raddist = 1 +
kc1_*rsquared +
kc2_*rsquared*rsquared;
804 dist_x = raddist * x;
805 dist_y = raddist * y;
808 BIASWARNONCE(
"distorting with dist inverse ");
810 double tandistx, tandisty;
813 for (
unsigned int i = 0; i < UNDISTORT_ITERS; i++){
814 twoxy = 2 * dist_x*dist_y;
815 rsquared = dist_x*dist_x + dist_y*dist_y;
818 raddist = 1.0 + (
kc1_ +
kc2_*rsquared)*rsquared;
820 tandistx =
kc3_*twoxy +
kc4_ * (rsquared + 2*dist_x*dist_x);
821 tandisty =
kc4_*twoxy +
kc3_ * (rsquared + 2*dist_y*dist_y);
824 tmpx = raddist * dist_x + tandistx;
825 tmpy = raddist * dist_y + tandisty;
828 dist_x = x - (tmpx-dist_x);
829 dist_y = y - (tmpy-dist_y);
832 if (fabs(dist_x)>1e10 || fabs(dist_y)>1e10) {
841 raddist = 1 +
kc1_*rsquared +
kc2_*rsquared*rsquared +
kc3_*rsquared*rsquared*rsquared;
842 dist_x = raddist * x;
843 dist_y = raddist * y;
848 BIASWARNONCE(
"distorting with dist inverse ");
851 for (
unsigned int i = 0; i < UNDISTORT_ITERS; i++){
852 twoxy = 2 * dist_x*dist_y;
853 rsquared = dist_x*dist_x + dist_y*dist_y;
856 raddist = 1.0 + (
kc1_ +
kc2_*rsquared+
kc3_*rsquared*rsquared)*rsquared;
859 tmpx = raddist * dist_x;
860 tmpy = raddist * dist_y;
863 dist_x = x - (tmpx-dist_x);
864 dist_y = y - (tmpy-dist_y);
867 if (fabs(dist_x)>1e10 || fabs(dist_y)>1e10) {
875 BIASERR(
"unknown distortion type");
884 double& x,
double& y)
const
890 double tandistx, tandisty;
892 double rc, r0squared;
906 rc =
kc1_ * r0squared +
kc2_* r0squared* r0squared;
908 for (
unsigned int i = 0; i < UNDISTORT_ITERS; i++){
910 rsquared = x*x + y*y;
913 raddist = 1.0 + (
kc1_ +
kc2_*rsquared)*rsquared - rc;
915 tandistx =
kc3_*twoxy +
kc4_ * (rsquared + 2*x*x);
916 tandisty =
kc4_*twoxy +
kc3_ * (rsquared + 2*y*y);
919 x = (dist_x - tandistx)/raddist;
920 y = (dist_y - tandisty)/raddist;
923 if (fabs(x)>1e10 || fabs(y)>1e10) {
929 for (
unsigned int i = 0; i < UNDISTORT_ITERS; i++){
931 rsquared = x*x + y*y;
934 raddist = 1.0 + (
kc1_ +
kc2_*rsquared)*rsquared;
936 tandistx =
kc3_*twoxy +
kc4_ * (rsquared + 2*x*x);
937 tandisty =
kc4_*twoxy +
kc3_ * (rsquared + 2*y*y);
940 distx = raddist * x + tandistx;
941 disty = raddist * y + tandisty;
944 x = dist_x - (distx-x);
945 y = dist_y - (disty-y);
948 if (fabs(x)>1e10 || fabs(y)>1e10) {
955 for (
unsigned int i = 0; i < UNDISTORT_ITERS; i++){
956 rsquared = x*x + y*y;
959 raddist = 1.0 + (
kc1_ +
kc2_*rsquared)*rsquared;
966 x = dist_x - (distx-x);
967 y = dist_y - (disty-y);
970 if (fabs(x)>1e10 || fabs(y)>1e10) {
976 BIASWARNONCE(
"undistorting with dist inverse ")
978 rsquared = dist_x*dist_x + dist_y*dist_y;
979 raddist = 1 +
kc1_*rsquared +
kc2_*rsquared*rsquared;
981 twoxy = 2.0*dist_x*dist_y;
983 x = raddist * dist_x +
kc3_*twoxy +
kc4_ * (rsquared + 2.0*dist_x*dist_x);
984 y = raddist * dist_y +
kc4_*twoxy +
kc3_ * (rsquared + 2.0*dist_y*dist_y);
987 x = raddist * dist_x;
988 y = raddist * dist_y;
995 for (
unsigned int i = 0; i < UNDISTORT_ITERS; i++){
997 rsquared = x*x + y*y;
1000 raddist = 1.0 + (
kc1_ +
kc2_*rsquared+
kc3_*rsquared*rsquared)*rsquared;
1007 x = dist_x - (tmpx-x);
1008 y = dist_y - (tmpy-y);
1011 if (fabs(x)>1e10 || fabs(y)>1e10) {
1021 rsquared = dist_x*dist_x + dist_y*dist_y;
1022 raddist = 1 +
kc1_*rsquared +
kc2_*rsquared*rsquared +
kc3_*rsquared*rsquared*rsquared;
1023 x = raddist * dist_x;
1024 y = raddist * dist_y;
1028 BIASERR(
"unknown distortion type");
1038 #include <Base/Common/BIASpragmaEnd.hh>
bool Undistort_(const double dist_x, const double dist_y, double &x, double &y) const
unsigned int idealImageWidth_
ideal image size
unsigned int idealImageHeight_
virtual void SetPrincipal(const double x, const double y)
Set principal point (in pixels relative to top left corner).
double skew_
skew calibration parameter
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
void SetDistortionType(BIAS_ProjParaPersp_DISTORTION_TYPE distype)
Set type of distortion parameters.
void GetUndistortionRad3(double &kc1, double &kc2, double &kc3)
camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
void GetUndistortionBrown(double &kc1, double &kc2, double &kc3, double &kc4, double &r0) const
Get the lens undistortion parameters including the parameter describing root of the polynomial...
virtual void SetPrincipal(const double x, const double y)
Set principal point in pixels relative to top left corner, virtual overload to recalculate K_...
virtual void Rescale(double ratio, const double offset=0.0)
Adapt internal parameters to resampled image.
void Distort_(const double x, const double y, double &dist_x, double &dist_y) const
void SetMinZLocal(const double &minz)
sets minimum z value of unit-ray in local CCS to be accepted as in front of camera (e...
virtual void SetP(const PMatrix &P)
set from P
int GetR(Matrix3x3< double > &R)
void SetUndistortion(double kc1, double kc2)
void GetUndistortion(double &kc1, double &kc2, double &kc3, double &kc4) const
double minZlocal_
rigorously clip unit rays in CCS with z smaller than this values
void InitParams_()
called from constructors to set zeros to most values
void SetSkew(double skew)
Set the current camera skew factor.
KMatrix K_
focal length, aspect ratio, principal point in K_, inverse in invK_
double GetMinZLocal() const
minimum z value of ray in image (if computed already)
Wrapper class for reading and writing XML files based on the XML library libxml2. ...
BIAS_ProjParaPersp_DISTORTION_TYPE distType_
ProjectionParametersPerspective(const ProjectionParametersPerspective &P)
virtual ~ProjectionParametersPerspective()
void SetUndistortionInverseRad(double kc1, double kc2, double kc3, double kc4)
virtual ProjectionParametersPerspective * Clone() const
covariant virtual copy constructor for use in Projection
void SetUndistortionInverseRad3(double kc1, double kc2, double kc3)
virtual BIAS::KMatrix GetKinv() const
void GetFocalLength(double &f) const
Get the current camera focal length.
class BIASGeometry_EXPORT PMatrix
void GetUndistortionInvRad3(double &kc1, double &kc2, double &kc3)
virtual void SetAspectratio(const double AspectRatio)
Set CCD cell-size in meter, virtual overload to recalculate K_.
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
std::ostream & operator<<(std::ostream &os, const Array2D< T > &arg)
virtual BIAS::PMatrix GetP() const
ProjectionParametersPerspective(const BIAS::PMatrix &P, const unsigned int width=10000, const unsigned int height=10000)
virtual int GetImageSize(unsigned int &Width, unsigned int &Height) const
Obtain image dimensions.
virtual BIAS::KMatrix GetK() const
void SetUndistortionRad3(double kc1, double kc2, double kc3)
double focallength_
focal length is stored in pixel for cellSizeX
int GetC(Vector3< double > &C)
computes translation vector origin world coo -> origin camera coo (center), uses decomposition, which is cached
void SetUndistortionBrown(double kc1, double kc2, double kc3, double kc4, double r0)
Set the lens undistortion parameters including the root of the polynomial.
double GetFocalLength() const
Returns the focal length.
void GetUndistortionInverseRad(double &kc1, double &kc2, double &kc3, double &kc4) const
BIAS_ProjParaPersp_DISTORTION_TYPE
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
ProjectionParametersPerspective(const unsigned int width=0, const unsigned int height=0)
virtual void SetAspectratio(const double AspectRatio)
Set CCD aspect ratio (i.e.
ProjectionParametersPerspective & operator=(const ProjectionParametersPerspective &P)
describes a projective 3D -> 2D mapping in homogenous coordinates
virtual void Rescale(unsigned int width, unsigned int height)
Adapt internal parameters to resampled image.
void SetFocalLengthAndAspect(double f, double AspectRatio)
Set the current camera focal length in pixel and the a spect ratio.
void SetSimplePerspective(const double FoV=M_PI/2, const unsigned int width=512, const unsigned int height=512)
Sets the parameters for a simple perspective camera with imagesize 512x512, focal length 512 (fov = 9...
virtual void Rescale(double ratio, const double offset=0.0)
Adapt internal parameters to resampled image.
KMatrix Invert() const
returns analyticaly inverted matrix
Camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
void SetUndistortion(double kc1, double kc2, double kc3, double kc4)
Set the lens undistortion parameters.
virtual const BIAS::Pose & GetPose() const
Return complete pose object.
virtual void SetK(const KMatrix &K)
sets the internal parameters from a given KMatrix and updates the cached K and its inverse ...
void GetUndistortion(double &kc1, double &kc2, double &kc3, double &kc4, double r0) const
double GetSkew() const
Obtain the current camera skew factor.
double kc1_
lens undistortion parameters after Bouquet also used as parameters for distortion after Brown(...
virtual ProjectionParametersBase & operator=(const ProjectionParametersBase &p)
BIAS_ProjParaPersp_DISTORTION_TYPE GetDistortionType() const
Get type of distortion parameters.
int GetK(KMatrix &K)
calibration matrix