25 #include <Image/CylindricalRectification.hh>
26 #include <Geometry/ProjectionParametersSpherical.hh>
27 #include <Geometry/ProjectionParametersPerspective.hh>
28 #include <Geometry/ProjectionParametersCylindric.hh>
29 #include <Base/Geometry/HomgPoint3D.hh>
30 #include <Base/Common/CompareFloatingPoint.hh>
31 #include <Base/ImageUtils/BresenhamCircle.hh>
36 #define SUPERPARENT RectificationBase<InputStorageType, OutputStorageType>
37 #define PARENT RectificationViaProjectionMappingBase<InputStorageType, OutputStorageType>
40 template <
class InputStorageType,
class OutputStorageType>
44 maxHalfAngleInX_(maxAngle)
47 template <
class InputStorageType,
class OutputStorageType>
52 template <
class InputStorageType,
class OutputStorageType>
64 if (ppp == NULL && pps == NULL && ppc == NULL) {
72 template <
class InputStorageType,
class OutputStorageType>
77 Pose rectPoseA, rectPoseB;
79 if (SUPERPARENT::CalculateRectifiedBases(SUPERPARENT::ppBA_->GetPose(),
80 SUPERPARENT::ppBB_->GetPose(),
81 rectPoseA, rectPoseB) != 0) {
82 BIASERR(
"Failed calculating rectified base");
96 double minPhi, maxPhi;
99 double phiMinA, phiMinB;
100 double phiMaxA, phiMaxB;
102 double relativeXResA, relativeXResB;
104 double relativePhiResA, relativePhiResB;
106 if (DetermineCylindricCameraBoundries_(SUPERPARENT::ppBA_,
108 xMinA, xMaxA, phiMinA, phiMaxA,
109 relativeXResA, relativePhiResA) != 0) {
110 BIASERR(
"Error calculating rectification boundaries.\n");
114 if (DetermineCylindricCameraBoundries_(SUPERPARENT::ppBB_,
116 xMinB, xMaxB, phiMinB, phiMaxB,
117 relativeXResB, relativePhiResB) != 0) {
118 BIASERR(
"Error calculating rectification boundaries.\n");
125 if(xMinB > xMaxA || xMinA > xMaxB) {
126 BIASERR(
"it seems that cameras do not have a viewing overlap within epipolar planes!");
129 if(phiMinB > phiMaxA || phiMinA > phiMaxB) {
130 BIASERR(
"it seems that cameras do not have shared epipolar lines!");
136 if (xMinA > xMinB) minX = xMinB;
138 if (xMaxA > xMaxB) maxX = xMaxA;
143 if(phiMinA > phiMinB) minPhi = phiMinA;
144 else minPhi = phiMinB;
145 if(phiMaxA < phiMaxB) maxPhi = phiMaxB;
146 else maxPhi = phiMaxA;
148 double relativeXRes, relativePhiRes;
149 if(relativeXResA > relativeXResB) relativeXRes = relativeXResA;
150 else relativeXRes = relativeXResB;
151 if(relativePhiResA > relativePhiResB) relativePhiRes = relativePhiResA;
152 else relativePhiRes = relativePhiResB;
154 unsigned int xRes = (
unsigned int) ceil(relativeXRes*(maxX - minX));
155 unsigned int phiRes = (
unsigned int) ceil(relativePhiRes*(maxPhi - minPhi));
157 delete PARENT::rectPPA_;
158 delete PARENT::rectPPB_;
165 PARENT::rectPPA_->SetPose(rectPoseA);
166 PARENT::rectPPB_->SetPose(rectPoseB);
190 template <
class InputStorageType,
class OutputStorageType>
194 double radius= sqrt(point[1]*point[1] + point[2]*point[2]);
195 if(
Equal(radius, 0.0)) {
199 double scale = 1.0 / sqrt(point[1]*point[1] + point[2]*point[2]);
201 phi = atan2(point[1], point[2]);
210 template <
class InputStorageType,
class OutputStorageType>
213 double& minX,
double& maxX,
214 double& minPhi,
double& maxPhi)
217 if(ProjectOntoUnitCylinder_(rectPoint, phi)==0) {
218 if(rectPoint[0]>maxX) maxX = rectPoint[0];
219 if(rectPoint[0]<minX) minX = rectPoint[0];
220 if(phi>maxPhi) maxPhi = phi;
221 if(phi<minPhi) minPhi = phi;
225 template <
class InputStorageType,
class OutputStorageType>
229 const Pose& rectPose,
230 double& minX,
double& maxX,
231 double& minPhi,
double& maxPhi,
232 double& relativeXRes,
233 double& relativePhiRes)
const
236 Pose relativeTransform;
239 unsigned int width, height;
252 for(
unsigned int x=0; x<width; x++) {
254 if(testedPoint.
NormL2() < 1e-10) BIASABORT;
256 SetMinMaxFrom_(rectPoint, minX, maxX, minPhi, maxPhi);
259 if(testedPoint.
NormL2() < 1e-10) BIASABORT;
261 SetMinMaxFrom_(rectPoint, minX, maxX, minPhi, maxPhi);
264 for(
unsigned int y=0; y<height; y++) {
266 if(testedPoint.
NormL2() < 1e-10) BIASABORT;
268 SetMinMaxFrom_(rectPoint, minX, maxX, minPhi, maxPhi);
271 if(testedPoint.
NormL2() < 1e-10) BIASABORT;
273 SetMinMaxFrom_(rectPoint, minX, maxX, minPhi, maxPhi);
276 double clippingMaxX = tan(maxHalfAngleInX_);
285 if(minX < -clippingMaxX) minX = -clippingMaxX;
286 if(maxX > clippingMaxX) maxX = clippingMaxX;
318 minX = -clippingMaxX;
325 double deltPhi = maxPhi - minPhi;
327 relativePhiRes = (width+height)/deltPhi;
329 double deltaX = maxX - minX;
330 relativeXRes = sqrt( (
double)(width*width+height*height ))/deltaX;
334 template <
class InputStorageType,
class OutputStorageType>
338 const Pose& rectPose,
339 double& minX,
double& maxX,
340 double& minPhi,
double& maxPhi,
341 double& relativeXRes,
342 double& relativePhiRes)
const
348 Pose relativeTransform;
352 unsigned int width, height;
364 int radius = (int)rint(pps->
GetRadius());
367 int pp[2] = {(int)rint(ppx), (int)rint(ppy)};
369 int ciclePoint[] = {0,0};
370 while(circle.
GetNext(ciclePoint)) {
373 SetMinMaxFrom_(rectPoint, minX, maxX, minPhi, maxPhi);
377 double clippingMaxX = tan(maxHalfAngleInX_);
378 if(minX < -clippingMaxX) minX = -clippingMaxX;
379 if(maxX > clippingMaxX) maxX = clippingMaxX;
402 minX = -clippingMaxX;
421 double numEpiLines = 2.0*M_PI*pps->
GetRadius();
423 double deltaPhi = maxPhi - minPhi;
424 relativePhiRes = numEpiLines/deltaPhi;
437 double anglePerSample = 1.0/pps->
GetRadius();
440 double minXStepSize = tan(anglePerSample);
441 relativeXRes = 1.0/minXStepSize;
470 template <
class InputStorageType,
class OutputStorageType>
474 const Pose& rectPose,
475 double& minX,
double& maxX,
476 double& minPhi,
double& maxPhi,
477 double& relativeXRes,
478 double& relativePhiRes)
const
481 Pose relativeTransform;
484 unsigned int width, height;
497 for(
unsigned int x=0; x<width; x++) {
499 if(testedPoint.
NormL2() < 1e-10) BIASABORT;
501 SetMinMaxFrom_(rectPoint, minX, maxX, minPhi, maxPhi);
504 if(testedPoint.
NormL2() < 1e-10) BIASABORT;
506 SetMinMaxFrom_(rectPoint, minX, maxX, minPhi, maxPhi);
509 for(
unsigned int y=0; y<height; y++) {
511 if(testedPoint.
NormL2() < 1e-10) BIASABORT;
513 SetMinMaxFrom_(rectPoint, minX, maxX, minPhi, maxPhi);
516 if(testedPoint.
NormL2() < 1e-10) BIASABORT;
518 SetMinMaxFrom_(rectPoint, minX, maxX, minPhi, maxPhi);
521 double clippingMaxX = tan(maxHalfAngleInX_);
529 if(minX < -clippingMaxX) minX = -clippingMaxX;
530 if(maxX > clippingMaxX) maxX = clippingMaxX;
552 minX = -clippingMaxX;
562 double deltPhi = maxPhi - minPhi;
564 relativePhiRes = double(height)/deltPhi;
566 double deltaX = maxX - minX;
567 relativeXRes = double(width)/deltaX;
571 template <
class InputStorageType,
class OutputStorageType>
574 const Pose& rectPose,
575 double& minX,
double& maxX,
576 double& minPhi,
double& maxPhi,
577 double& relativeXRes,
578 double& relativePhiRes)
const
589 BIASERR(
"unexpected ProjectionParameter type\n");
592 return DetermineCylindricCameraBoundriesCylindric_(camera,
600 return DetermineCylindricCameraBoundriesSpherical_(camera,
608 return DetermineCylindricCameraBoundriesPerspective_(camera,
626 #if defined(BUILD_IMAGE_CHAR)
632 #if defined(BUILD_IMAGE_USHORT)
636 #if defined(BUILD_IMAGE_SHORT)
640 #if defined(BUILD_IMAGE_SHORT)&&defined(BUILD_IMAGE_USHORT)
644 #if defined(BUILD_IMAGE_INT)
648 #if defined(BUILD_IMAGE_USHORT)
652 #if defined(BUILD_IMAGE_USHORT) && defined(BUILD_IMAGE_INT)
656 #if defined(BUILD_IMAGE_DOUBLE)
void GetEuclidean(Vector3< HOMGPOINT3D_TYPE > &dest) const
calculate affine coordinates of this and write them to dest affine coordinates are projective coordin...
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
int DetermineCylindricCameraBoundries_(const ProjectionParametersBase *camera, const Pose &rectPose, double &minX, double &maxX, double &minPhi, double &maxPhi, double &relativeXRes, double &relativePhiRes) const
Base class for rectification implementations that make use of projections to represent rectified stat...
virtual int DetermineRectificationParameters_()
Calculating the projection parameters for specific rectification.
CylindricalRectification(double maxAngleRAD=70 *DEG_TO_RAD)
Parameter determines the extend of the cylinder along the cylinder axis.
Scans a circle using Bresenham's integer arithmetic algorithm.
camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
virtual int GetPrincipal(double &PrincipalX, double &PrincipalY) const
Get principal point (in pixels relative to top left corner).
Class implements rectification by image projection onto a cylinder.
int DetermineCylindricCameraBoundriesCylindric_(const ProjectionParametersBase *camera, const Pose &rectPose, double &minX, double &maxX, double &minPhi, double &maxPhi, double &relativeXRes, double &relativePhiRes) const
int DetermineCylindricCameraBoundriesSpherical_(const ProjectionParametersBase *camera, const Pose &rectPose, double &minX, double &maxX, double &minPhi, double &maxPhi, double &relativeXRes, double &relativePhiRes) const
Represents 3d pose transformations, parametrized as Euclidean translation and unit quaternion orienta...
virtual bool DoesPointProjectIntoImageLocal(const Vector3< double > &localX, HomgPoint2D &x, bool IgnoreDistortion=false) const
Checks if 3D point projects into specified image and returns belonging 2D image point.
bool GetNext(int next[2])
Returns the coordinate of the next point on the circle.
virtual void UnProjectLocal(const HomgPoint2D &pos, Vector3< double > &origin, Vector3< double > &direction, bool ignoreDistortion=false) const =0
Calculates the view ray, which belongs to the given position on the image plane, in local coordinates...
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
virtual int GetImageSize(unsigned int &Width, unsigned int &Height) const
Obtain image dimensions.
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_...
Camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
virtual bool IsInputCameraValid(const BIAS::Image< InputStorageType > &img, const BIAS::ProjectionParametersBase *proj)
only perspective and spherical cameras are allowed
virtual ~CylindricalRectification()
static void SetMinMaxFrom_(BIAS::HomgPoint3D &rectPoint, double &minX, double &maxX, double &minPhi, double &maxPhi)
Helper for DetermineCylindricCameraBoundries_.
int DetermineCylindricCameraBoundriesPerspective_(const ProjectionParametersBase *camera, const Pose &rectPose, double &minX, double &maxX, double &minPhi, double &maxPhi, double &relativeXRes, double &relativePhiRes) const
Camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
static int ProjectOntoUnitCylinder_(BIAS::HomgPoint3D &point, double &phi)
Helper for DetermineCylindricCameraBoundries_.
virtual const BIAS::Pose & GetPose() const
Return complete pose object.
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
class BIASGeometryBase_EXPORT HomgPoint2D
class BIASGeometryBase_EXPORT HomgPoint3D