26 #include "EpipolarLine.hh"
28 #include <Base/Debug/Debug.hh>
29 #include <Base/ImageUtils/ImageDraw.hh>
30 #include <Base/ImageUtils/Bresenham.hh>
34 #define MY_DBL_PRECISION 1e-10
36 #include <Base/Common/BIASpragma.hh>
42 int CompIntersect(
const void *left,
const void *right);
44 ostream&
operator<<(ostream& os,
const enum TEdge edge)
47 case TopEdge: os <<
"top";
break;
48 case RightEdge: os <<
"right";
break;
49 case BottomEdge: os <<
"bottom";
break;
50 case LeftEdge: os <<
"left";
break;
51 case InvalidEdge: os <<
"invalid";
break;
95 (*this).HomgLine2D::operator=(line);
134 dx = dist*fabs(
data_[0]);
135 dy = dist*fabs(
data_[1]);
160 BIASDOUT(D_EL_NEXTLINE,
"dx: "<<dx<<
"\tdy: "<<dy<<
"\tnewpoint: ("
161 <<newpoint[0]<<
", "<<newpoint[1]<<
") far int.: ("
172 return Recalc(0.0, 0.0,
double(width-1),
double(height-1), epipole, epos);
216 double maxx = double(width-1);
218 double maxy = double(height-1);
219 Init(minx, miny, maxx, maxy, epipole, otherpoint, epos);
224 const double& maxx,
const double& maxy,
257 Init(width, height, epipole, pinf,
269 double RMaxImage,
bool forward,
270 unsigned int& DestLength,
unsigned char *Destination)
300 <<
"\tRMinImage:"<<RMinImage<<
"\tRMaxImage:"<<RMaxImage
316 unsigned int& DestLength,
unsigned char *Destination,
319 unsigned int counter= 0;
321 unsigned int DestImageWidth =
322 (
unsigned int)floor(sqrt(pow(Source.
GetWidth(),2.0) +
325 if (Destination == NULL){
326 Destination =
new unsigned char[DestImageWidth];
330 DestLength = ROffset;
333 counter = DestLength;
336 counter = DestImageWidth - DestLength;
345 <<
"\tdest: "<<(
int)Destination[DestLength]);
346 Destination[counter] = (
unsigned char)
352 counter += counterincr;
354 BIASDOUT(D_EL_OFFSET,
"DestLength: " << DestLength);
362 int& y,
double& g,
unsigned char& result)
368 BIASDOUT(D_EL_BRESROUND,x <<
", " << y <<
" : "<< gm <<
"\t"
369 << x <<
", " << y+1 <<
" : "<<g);
370 result = (
unsigned char)((
double)ida[y][x]*gm +
371 (double)ida[y+1][x]*g);
374 BIASDOUT(D_EL_BRESROUND,y <<
", " << y <<
" : "<< gm <<
"\t"
375 << y <<
", " << x+1 <<
" : "<<g);
376 result = (
unsigned char)((
double)ida[x][y]*gm +
377 (double)ida[x+1][y]*g);
382 BIASDOUT(D_EL_BRESROUND,y <<
", " << -x <<
" : "<< gm <<
"\t"
383 << y <<
", " << -x+1 <<
" : "<<g);
384 result = (
unsigned char)((
double)ida[x][-y]*gm +
385 (double)ida[x+1][-y]*g);
390 BIASDOUT(D_EL_BRESROUND,-x <<
", " << y <<
" : "<< gm <<
"\t"
391 << -x <<
", " << y+1 <<
" : "<<g);
392 result = (
unsigned char)((
double)ida[y][-x]*gm +
393 (double)ida[y+1][-x]*g);
398 BIASDOUT(D_EL_BRESROUND,-x <<
", " << -y <<
" : "<< gm <<
" : "
399 <<(
int)ida[-y][-x]<<
"\t" << -x <<
", " << -y-1 <<
" : "<<g
400 <<
" : "<<(
int)ida[-y-1][-x]);
401 result = (
unsigned char)((
double)ida[-y][-x]*gm +
402 (double)ida[-y-1][-x]*g);
403 BIASDOUT(D_EL_BRESROUND,
"result: "<<(
int)result);
407 BIASDOUT(D_EL_BRESROUND,-y <<
", " << -x <<
" : "<< gm <<
"\t"
408 << -y <<
", " << -x+1 <<
" : "<<g);
409 result = (
unsigned char)((
double)ida[-x][-y]*gm +
410 (double)ida[-x-1][-y]*g);
415 BIASDOUT(D_EL_BRESROUND,-y <<
", " << x <<
" : "<< gm <<
"\t"
416 << -y <<
", " << x+1 <<
" : "<<g);
417 result = (
unsigned char)((
double)ida[-x][y]*gm +
418 (double)ida[-x-1][y]*g);
423 BIASDOUT(D_EL_BRESROUND,x <<
", " << -y <<
" : "<< gm <<
"\t"
424 << x <<
", " << -y+1 <<
" : "<<g);
425 result = (
unsigned char)((
double)ida[-y][x]*gm +
426 (double)ida[-y-1][x]*g);
432 BIASERR(
"Invalid LineType, maybe wrong slope in EpipolarLine");
444 dx_ = fabs(Transfslope[0]);
445 dy_ = fabs(Transfslope[1]);
447 BIASDOUT(D_EL_TRANSF,
" dx_ "<<
dx_<<
"\tdy_ "<<
dy_<<
"\ttype: "<<
Type_);
453 BIASDOUT(D_EL_TRANSF,
" dr start: "<<dr<<
"\tstart befor "
455 TransformedStart_[0] +=
dx_ * dr;
456 TransformedStart_[1] +=
dy_ * dr;
461 BIASDOUT(D_EL_TRANSF,
" dr end: "<<dr<<
"\tend befor "
463 TransformedEnd_[0] -=
dx_ * dr;
464 TransformedEnd_[1] -=
dy_ * dr;
470 <<
") transf: ("<<TransformedStart_[0]<<
", "<<TransformedStart_[1]
471 <<
") -> ("<<TransformedEnd_[0]<<
", "<<TransformedEnd_[1]<<
")");
479 int CompIntersect(
const void *left,
const void *right)
490 int intersectioncount = 0;
502 edges[0].
Edge = LeftEdge;
503 edges[1].
Edge = RightEdge;
504 edges[2].
Edge = TopEdge;
505 edges[3].
Edge = BottomEdge;
508 if (!edges[0].
Point.IsAtInfinity()) edges[0].
Point.Homogenize();
509 else edges[0].
Point.
Set(DBL_MAX, DBL_MAX, 0.0);
511 if (!edges[1].
Point.IsAtInfinity()) edges[1].
Point.Homogenize();
512 else edges[1].
Point.
Set(DBL_MAX, DBL_MAX, 0.0);
514 if (!edges[2].
Point.IsAtInfinity()) edges[2].
Point.Homogenize();
515 else edges[2].
Point.
Set(DBL_MAX, DBL_MAX, 0.0);
517 if (!edges[3].
Point.IsAtInfinity()) edges[3].
Point.Homogenize();
518 else edges[3].
Point.
Set(DBL_MAX, DBL_MAX, 0.0);
521 for (
unsigned int i=0; i<4; i++){
544 if ((point[0]==
minx_) ||
545 ((edges[0].
Point[2]!=0.0) &&
552 if ((point[0]==
maxx_) ||
553 ((edges[1].
Point[2]!=0.0) &&
560 if ((point[1]==
miny_) ||
561 ((edges[2].
Point[2]!=0.0) &&
568 if ((point[1]==
maxy_) ||
569 ((edges[3].
Point[2]!=0.0) &&
587 qsort(&edges, 4,
sizeof(
struct TIntersect), CompIntersect);
600 FarEdge_ = edges[0].
Edge;
602 switch(intersectioncount){
626 BIASDOUT(D_EL_INT,
"invalid intersection count "<<intersectioncount
627 <<
" of ep. line: "<<*
this<<
" with epipole: "<<
Epipole_
630 <<
"Image borders are minx, miny, maxx, maxy="
632 <<
" sorted intersections: ");
633 for (
int i=0; i<4; i++){
634 BIASDOUT(D_EL_INT, i <<
": (" << edges[i].
Point[0] <<
", "
635 << edges[i].
Point[1] <<
")\t" << edges[i].EpipoleDistance);
647 template <
class PixelType>
650 unsigned int start[2],
end[2];
652 color[0]=PixelType(255);
653 color[1]=PixelType(255);
654 color[2]=PixelType(255);
685 template <
class PixelType>
702 unsigned int start[2],
end[2];
710 template <
class PixelType>
719 unsigned int width(0), height(0);
724 unsigned int numbercubefaces = 6;
734 double cubefacewidth = double(width+height)/2.0;
736 K[0][0] = K[1][1] = cubefacewidth / 2.1;
737 K[0][2] = K[1][2] = cubefacewidth / 2.0;
747 if (paramsPersp!=NULL) {
750 if (
double(width)>cubefacewidth) cubefacewidth = width;
751 if (
double(height)>cubefacewidth) cubefacewidth = height;
752 K = paramsPersp->
GetK();
754 BIASWARN(
"distortion resolution is heuristic, may get aliased epiline !");
756 K[0][2] += cubefacewidth;
757 K[1][2] += cubefacewidth;
758 cubefacewidth *= 3.0;
761 }
else if (pSphere!=NULL) {
762 K = pSphere->
GetFakeKMatrix(cubefacewidth, 1, 45.1 * M_PI / 180.0);
765 paramsPersp->
SetK(K);
766 paramsPersp->
SetImageSize((
unsigned int)(cubefacewidth),
767 (
unsigned int)(cubefacewidth));
770 for (
unsigned int cubeface = 0; cubeface<numbercubefaces; cubeface++) {
775 case 1:axis[0]=1.0; axis[1]=0.0; axis[2]=0.0; angle=M_PI/2.0;
break;
776 case 2:axis[0]=1.0; axis[1]=0.0; axis[2]=0.0; angle=-M_PI/2.0;
break;
777 case 3:axis[0]=0.0; axis[1]=1.0; axis[2]=0.0; angle=M_PI/2.0;
break;
778 case 4:axis[0]=0.0; axis[1]=1.0; axis[2]=0.0; angle=-M_PI/2.0;
break;
779 case 5:axis[0]=0.0; axis[1]=1.0; axis[2]=0.0; angle=M_PI;
break;
783 paramsPersp->
SetR(paramsOrig->
GetR()*RCube);
784 paramsPersp->
SetC(paramsOrig->
GetC());
805 if (line.NormL2()<1e-10) {
806 BIASERR(
"Epipolar line is strange: "<<line<<
" with epipole ray "
807 <<epipoleRay<<
" ("<<epipole <<
") and infray "<< infRay <<
"("<<pinf<<
")"
808 <<
" at current cube side "
817 int(rint(cubefacewidth)));
819 if (line.Recalc(
int(rint(cubefacewidth)),
int(rint(cubefacewidth)),
821 BIASWARN(
"Epipolar line is not in image");
825 int start[2],
end[2];
827 (int)rint(line.GetCloseIntersectionWithImageBorder()[0]);
829 (int)rint(line.GetCloseIntersectionWithImageBorder()[1]);
831 (int)rint(line.GetFarIntersectionWithImageBorder()[0]);
833 (int)rint(line.GetFarIntersectionWithImageBorder()[1]);
837 int next[2]={start[0],start[1]};
839 bool lastPixelValid =
false;
842 PixelType *color =
new PixelType [image.
GetChannelCount()], *mycolor=NULL;
844 color[c] = PixelType(255);
849 else mycolor = Color;
851 double linefac = sqrt(line[0]* line[0]+ line[1]*line[1]);
852 if (linefac<1e-8) linefac = 1;
853 else linefac = 1.0/linefac;
860 point[0] -= dist * linefac * line[0];
861 point[1] -= dist * linefac * line[1];
871 thisPixel[0] = (int)(rint(point[0]));
872 thisPixel[1] = (int)(rint(point[1]));
876 if (lastPixelValid) {
882 image.
SetPixel(mycolor[c], thisPixel[0], thisPixel[1], c);
886 lastPixel[0] = thisPixel[0];
887 lastPixel[1] = thisPixel[1];
888 lastPixelValid =
true;
891 lastPixelValid =
false;
896 lastPixelValid =
false;
906 template <
class PixelType>
917 PointAtInfinity[3] = 0.0;
918 cout<<
"EpipolarLine::Point at infinity is "<<PointAtInfinity<<endl;
923 infPointOrigImage)) {
927 int(rint(infPointOrigImage[0])),
928 int(rint(infPointOrigImage[1])),
931 cout<<
"EpipolarLine::Point at infinity does not project into image 2:"<<endl;
934 if (infRayOrigImage.
NormL2()<1e-10) {
935 BIASERR(
"invalid point"<<PointAtInfinity);
941 bool onlyHomography = (otherCamCenter-thisP.
GetC()).
NormL2()<1e-5;
945 if (!onlyHomography){
952 Value[0] = PixelType(255);
953 Value[1] = PixelType(255);
954 Value[2] = PixelType(255);
956 int(rint(epipoleOrigImage[0])),
957 int(rint(epipoleOrigImage[1])),
960 int(rint(epipoleOrigImage[0])),
961 int(rint(epipoleOrigImage[1])),
971 if (eline.
NormL2()>1e-10) {
972 cout<<
"Drawing distorted line"<<endl;
974 thisIm, paramsOrig, Color, lineWidth);
976 cout<<
"epipole dir and ray dir coincident !"<<endl;
979 BIASERR(
"pure homography. no epiline.");
984 #define INSTANCE(type) \
985 template void BIASGeometry_EXPORT EpipolarLine::Draw<type>(Image<type>& im);\
986 template void BIASGeometry_EXPORT EpipolarLine::DrawWhole<type>(Image<type>& im); \
987 template void BIASGeometry_EXPORT EpipolarLine::ProjectEpipolarPlane<type>(const Projection& \
989 Image<type> &thisIm, \
990 const Vector3<double>& \
992 const Vector3<double>& \
993 RayDir, type* color, int lineWidth); \
994 template void BIASGeometry_EXPORT EpipolarLine::DrawDistortedLine<type>(const Vector3<double>& epipoleRayOrigImage, const Vector3<double>& infRayOrigImage, Image<type>& image, ProjectionParametersBase* paramsOrig, type* color, int lineWidth);
996 INSTANCE(
unsigned char)
999 #ifdef BUILD_IMAGE_CHAR
1002 #ifdef BUILD_IMAGE_DOUBLE
1005 #ifdef BUILD_IMAGE_UINT
1006 INSTANCE(
unsigned int)
1008 #ifdef BUILD_IMAGE_INT
1011 #ifdef BUILD_IMAGE_SHORT
1014 #ifdef BUILD_IMAGE_USHORT
1015 INSTANCE(
unsigned short)
virtual BIAS::Vector3< double > GetC() const
Get projection center.
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...
double minx_
int Width_; // width of image int Height_; // height of image image borders in xdir, typically 0, width-1
virtual HomgPoint2D & GetFarIntersectionWithImageBorder()
HomgPoint2D FarIntersection_
void Transform(enum TLineType &type, HomgPoint2D &Original, HomgPoint2D &Transformed)
transforms Original according to type in a way that Transformed lies on a line with slope in [0...
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
virtual enum TEdge GetFarEdge() const
HOMGLINE2D_TYPE data_[VECTOR3_SIZE]
enum EPosition DeterminePosition(unsigned int width, unsigned int height)
Used in function DetermineStartEnd_.
enum HomgPoint2D::EPosition EpipolePosition_
virtual void SetR(const BIAS::RMatrix &R)
Set orientation from rotation matrix R.
virtual void Init(const double &minx, const double &miny, const double &maxx, const double &maxy, HomgPoint2D &epipole, HomgPoint2D &otherpoint, enum HomgPoint2D::EPosition epos)
camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
bool IsPositionInImage(const int &x, const int &y) const
check if image contains that pixel position
void ScalarProduct(const Vector3< T > &argvec, T &result) const
scalar product (=inner product) of two vectors, storing the result in result
virtual BIAS::Matrix3x4< double > GetExternals() const
Return cached 3x4 representation of external matrix [R|C].
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
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 bool DoesPointProjectIntoImage(const HomgPoint3D &X, HomgPoint2D &x, bool IgnoreDistortion=false) const
Checks if 3D point projects into specified image and returns belonging 2D image point.
HomgPoint2D CloseIntersection_
unsigned int GetWidth() const
virtual void UnProjectToRay(const HomgPoint2D &pos, Vector3< double > &origin, Vector3< double > &direction, bool ignoreDistortion=false) const
Calculates the view ray, which belongs to the given position on the image plane, in global coordinate...
void GetImageBoundaries(double &minx, double &miny, double &maxx, double &maxy) const
double GetAngle() const
angle to abszissa (-pi, pi]
static int CircleCenter(Image< StorageType > &im, unsigned int CenterX, unsigned int CenterY, unsigned int Radius, const StorageType Value[]=NULL)
draws a circular line, either using Value or a good contrast value
void InverseTransform(enum TLineType &type, HomgPoint2D &Transformed, HomgPoint2D &Original)
executes the inverse transformation to the above
virtual double GetRMax() const
distance epipole - far intersection with image border
void DrawWhole(Image< PixelType > &im)
draws infinite line in image, not stopping at epipole
double miny_
image borders in xdir, typically 0, height-1
void CrossProduct(const Vector3< T > &argvec, Vector3< T > &destvec) const
cross product of two vectors destvec = this x argvec
const ProjectionParametersBase * GetParameters(unsigned int cam=0) const
const parameter access function
static int Line(Image< StorageType > &im, const unsigned int start[2], const unsigned int end[2], const StorageType value[])
lines
This class hides the underlying projection model, like projection matrix, spherical camera...
void Intersection(const HomgLine2D &line, HomgPoint2D &intersect) const
calculates homogenized intersection of two lines
unsigned int GetChannelCount() const
returns the number of Color channels, e.g.
unsigned int GetHeight() const
virtual BIAS::RMatrix GetR() const
Get orientation as rotation matrix R.
bool GetNext(int next[2])
enum TLineType DetermineLineType(double &angle)
EightWaySymmetryHomg Sym_
void Draw(Image< PixelType > &im)
draws (*this) into image (only from Pinf to epipole
virtual enum TEdge GetCloseEdge() const
a line l = (a b c)^T is a form of the implicit straight line equation 0 = a*x + b*y + c if homogenize...
virtual void GetNextLine(bool clockwise, EpipolarLine &next, double dist=1.0)
returns next line with maximal possible distance without pixel loss
void Homogenize()
aequivalent to homogenize for points the gradient triangle is normed to hypothenuse length of 1 ...
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
virtual double ScanLine_(Image< unsigned char > &source, unsigned int ROffset, unsigned int &length, unsigned char *Destination, bool forward)
std::ostream & operator<<(std::ostream &os, const Array2D< T > &arg)
virtual bool DoesPointProjectIntoImage(const BIAS::HomgPoint3D &X, BIAS::HomgPoint2D &x, unsigned int cam=0, bool IgnoreDistortion=false) const
Checks if 3D point projects into specified image and returns belonging 2D image point.
void SetPixel(const StorageType &value, const unsigned int &x, const unsigned int &y, const unsigned short int channel=0)
Set the value of a given pixel (x,y) in channel to value.
static int RectangleCenter(Image< StorageType > &im, const int x, const int y, const int size, const StorageType value[])
Draws the rectangle around X, Y with Size and Value[i] in channel i.
HomgPoint2D TransformedStart_
virtual int GetImageSize(unsigned int &Width, unsigned int &Height) const
Obtain image dimensions.
virtual HomgPoint2D & GetEpipole()
virtual BIAS::KMatrix GetK() const
Matrix3x3< T > Transpose() const
returns transposed matrix tested 12.06.2002
static void DrawDistortedLine(const Vector3< double > &epipoleRayOrigImage, const Vector3< double > &infRayOrigImage, Image< PixelType > &image, ProjectionParametersBase *paramsOrig, PixelType *Color=NULL, int lineWidth=1)
project the plane given by two local rays and the camera center into the image (ANY projection) using...
virtual double GetRMin() const
distance epipole - close intersection with image border
bool DetermineIntersectionsAndEpipoleDistances_(HomgPoint2D &point)
virtual HomgPoint2D GetCopyOfEpipole() const
static void ProjectEpipolarPlane(const Projection &thisP, Image< PixelType > &thisIm, const Vector3< double > &otherCamCenter, const Vector3< double > &RayDir, PixelType *Color=NULL, int lineWidth=1)
visualize the epipolar plane/line as a curve in the image for any Projection
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
Scans a line using Bresenhams integer arithmetic algorithm.
enum HomgPoint2D::EPosition PInfinityPosition_
virtual double ScanLine(Image< unsigned char > &source, HomgPoint2D &epipole, enum HomgPoint2D::EPosition epos, double RMin, double RMax, bool forward, unsigned int &length, unsigned char *Destination)
scans epipolar line using bresenham, midpint alg.
virtual HomgPoint2D GetCopyOfFarIntersectionWithImageBorder() const
virtual enum HomgPoint2D::EPosition GetEpipolePosition() const
virtual void CalcTransformed_()
virtual EpipolarLine & operator=(const EpipolarLine &line)
Camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
void Set(const HOMGPOINT2D_TYPE &x, const HOMGPOINT2D_TYPE &y)
set elementwise with given 2 euclidean scalar values.
int Recalc()
assumes Width, Height_, data_ and Epipole_ are set correct before
enum TEdge FarEdge_ CloseEdge_
HOMGLINE2D_TYPEconst * end() const
Iterator pointing to one element after the last vector element.
Vector3< double > GetC(unsigned int cam=0) const
return Center of camera with index cam.
virtual void SetK(const KMatrix &K)
sets the internal parameters from a given KMatrix and updates the cached K and its inverse ...
virtual void SetC(const BIAS::Vector3< double > &C)
Set projection center.
Vector3< T > & Normalize()
normalize this vector to length 1
virtual HomgPoint2D & GetCloseIntersectionWithImageBorder()
virtual void Interpolate_(enum TLineType &type, unsigned char **ida, int &x, int &y, double &g, unsigned char &result)
HomgPoint2D TransformedCurrent_
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
TLineType
direction of line if start is in coordinate origin as given by compass
HomgPoint2D TransformedEnd_
class BIASGeometryBase_EXPORT HomgPoint3D
enum TLineType GetLineType() const
return type, E_NE if 0 < slope < 45 degree NE_N if 45 < slop < 90 ...
virtual HomgPoint2D GetCopyOfCloseIntersectionWithImageBorder() const
A class for representing epipolar lines with useful functions like ScanLine().
double BilinearInterpolationGrey(const double x, const double y) const
Bilinear interpolation for grey-value images.