26 #include "GenGroundTruth.hh"
28 #include <Base/Debug/Debug.hh>
29 #include <Base/Math/Random.hh>
30 #include <Geometry/RMatrix.hh>
31 #include <Base/Geometry/KMatrix.hh>
32 #include <Base/Math/Vector3.hh>
33 #include <Base/Geometry/HomgLine2D.hh>
34 #include <Base/Geometry/HomgPoint3D.hh>
35 #include <Base/ImageUtils/ImageDraw.hh>
41 vector<BIAS::HomgPoint2D>& match2,
45 for (
unsigned int i=0; i<match1.size(); i++){
46 match1[i]=Kinv*match1[i];
47 match2[i]=Kinv*match2[i];
52 vector<BIAS::HomgPoint2D>& match2,
55 for (
unsigned int i=0; i<match1.size(); i++){
56 match1[i]=K*match1[i];
57 match2[i]=K*match2[i];
62 vector<BIAS::HomgPoint2D>& match1,
63 vector<BIAS::HomgPoint2D>& match2,
64 unsigned char lcolor,
unsigned char mcolor)
66 unsigned int start[2], end[2];
70 for (
unsigned int i=0; i<match1.size(); i++){
71 start[0]=(
unsigned int)rint(match1[i][0]);
72 start[1]=(
unsigned int)rint(match1[i][1]);
73 end[0]=(
unsigned int)rint(match2[i][0]);
74 end[1]=(
unsigned int)rint(match2[i][1]);
75 line.
Set(match1[i], match2[i]);
78 BIASERR(
"error drawing epipolar line "<<i);
81 for (
unsigned int i=0; i<match1.size(); i++){
82 start[0]=(
unsigned int)rint(match1[i][0]);
83 start[1]=(
unsigned int)rint(match1[i][1]);
84 end[0]=(
unsigned int)rint(match2[i][0]);
85 end[1]=(
unsigned int)rint(match2[i][1]);
87 BIASERR(
"error drawing match "<<i<<
" ("<<start[0]<<
", "<<start[1]
88 <<
") ("<<end[0]<<
", "<<end[1]<<
") "<<match1[i]<<
" <--> "
96 unsigned width,
unsigned height,
97 vector<vector<HomgPoint2D> >& matches,
98 vector<HomgPoint3D>& points,
99 double minx,
double maxx,
double miny,
double maxy,
100 double minz,
double maxz)
106 vector<HomgPoint2D> p2vec;
107 unsigned numim=pvec.size();
112 for (i=0; i<matches.size(); i++)
116 for (i=0; i<num; i++){
123 for (
unsigned j=0; j<numim; j++){
126 if (p2d[0]>=0 && p2d[0]<width-1 && p2d[1]>=0 && p2d[1]<height-1){
127 p2vec.push_back(p2d);
134 points.push_back(p3d);
135 matches.push_back(p2vec);
141 unsigned width,
unsigned height,
double spatial_noise,
142 vector<vector<HomgPoint2D> >& i_matches,
143 vector<vector<HomgPoint2D> >& n_matches,
144 vector<HomgPoint3D>& points,
145 double minx,
double maxx,
double miny,
double maxy,
146 double minz,
double maxz)
152 vector<HomgPoint2D> p2vec;
153 vector<HomgPoint2D> noisy_p2vec;
154 unsigned numim=pvec.size();
158 for (i=0; i<i_matches.size(); i++)
159 i_matches[i].clear();
160 for (i=0; i<n_matches.size(); i++)
161 n_matches[i].clear();
165 for (i=0; i<num; i++){
173 for (
unsigned j=0; j<numim; j++){
178 if (p2d[0]>=0 && p2d[0]<width-1 && p2d[1]>=0 && p2d[1]<height-1 &&
179 n_p2d[0]>=0 && n_p2d[0]<width-1
180 && n_p2d[1]>=0 && n_p2d[1]<height-1){
181 p2vec.push_back(p2d);
182 noisy_p2vec.push_back(n_p2d);
189 points.push_back(p3d);
190 i_matches.push_back(p2vec);
191 n_matches.push_back(noisy_p2vec);
196 unsigned num,
unsigned width,
unsigned height,
197 double spatial_noise,
double r_noise,
double c_noise,
198 vector<vector<HomgPoint2D> >& i_matches,
199 vector<vector<HomgPoint2D> >& n_matches,
200 vector<HomgPoint3D>& points,
201 double minx,
double maxx,
double miny,
double maxy,
202 double minz,
double maxz,
bool print)
208 vector<HomgPoint2D> p2vec;
209 vector<HomgPoint2D> noisy_p2vec;
210 unsigned numim=pvec.size();
219 for (i=0; i<i_matches.size(); i++)
220 i_matches[i].clear();
221 for (i=0; i<n_matches.size(); i++)
222 n_matches[i].clear();
229 for (i=0; i<numim; i++){
230 if (pvec[i].GetK(K) || pvec[i].GetR(R) || pvec[i].GetC(C)){
231 BIASERR(
"error decomposing pvec["<<i<<
"] : "<<pvec[i]<<
"\nK: "<<K
232 <<
"\nR: "<<R<<
"\nC: "<<C);
236 BIASERR(
"error extracting rotation angles from R: "<<R);
239 cerr << setw(4) <<i<<
" : true C : "<<C
240 <<
"\n true R : "<<r<<endl;
245 R.
SetXYZ(r[0], r[1], r[2]);
250 cerr <<
" noisy C : "<<C
251 <<
"\n noisy R : "<<r<<endl;
258 cerr <<
"creating 3D points and matches"<<endl;
259 for (i=0; i<num; i++){
267 for (
unsigned j=0; j<numim; j++){
274 if (p2d[0]>=0 && p2d[0]<width-1 && p2d[1]>=0 && p2d[1]<height-1 &&
275 n_p2d[0]>=0 && n_p2d[0]<width-1 &&
276 n_p2d[1]>=0 && n_p2d[1]<height-1){
277 p2vec.push_back(p2d);
278 noisy_p2vec.push_back(n_p2d);
284 if (done) cout <<
"*";
else cout <<
".";
287 points.push_back(p3d);
288 i_matches.push_back(p2vec);
289 n_matches.push_back(noisy_p2vec);
294 cout <<
"3D points and matches:"<<endl;
295 for (
unsigned i=0; i<num; i++){
296 cout <<setw(3) << i <<
": true "<<points[i]<<
" \t: ";
297 for (
unsigned k=0; k<numim; k++)
298 cout <<k<<i_matches[i][k]<<
" ";
300 cout <<
" noisy "<<points[i]<<
" \t: ";
301 for (
unsigned k=0; k<numim; k++)
302 cout <<k<<n_matches[i][k]<<
" ";
312 vector<BIAS::PMatrix>& n_pvec,
unsigned num,
313 unsigned width,
unsigned height,
314 double spatial_noise,
double r_noise,
315 double c_noise,
double outlier_fraction,
316 vector<vector<BIAS::HomgPoint2D> >& i_matches,
317 vector<vector<BIAS::HomgPoint2D> >& n_matches,
318 vector<BIAS::HomgPoint3D>& points,
319 double minx,
double maxx,
320 double miny,
double maxy,
321 double minz,
double maxz,
325 unsigned inlier, outlier;
326 outlier=(unsigned)rint((
double)num*outlier_fraction);
328 unsigned numim=pvec.size();
329 double offsetx, offsety;
331 CreateMatches(pvec, n_pvec, num, width, height, spatial_noise, r_noise,
332 c_noise, i_matches, n_matches, points, minx, maxx, miny, maxy,
335 for (
unsigned i=inlier; i<num; i++){
336 cerr << i <<
"is outlier" << endl;
339 for (
unsigned j=1; j<numim;j++){
340 n_matches[i][j][0]+=j*offsetx;
341 n_matches[i][j][1]+=j*offsety;
342 i_matches[i][j][0]+=j*offsetx;
343 i_matches[i][j][1]+=j*offsety;
350 cout <<
"3D points and matches:"<<endl;
351 for (
unsigned i=0; i<num; i++){
353 if (i<inlier) cout <<
"inl ";
else cout <<
"outl ";
354 cout << setw(3) << i <<
": true ";
355 for (
unsigned k=0; k<numim; k++){
356 cout <<k<<i_matches[i][k]<<
"-"
357 <<pvec[k].GetK().Invert()*i_matches[i][k]<<
"\t";
362 for (
unsigned k=0; k<numim; k++)
363 cout <<k<<n_matches[i][k]<<
"-"
364 <<n_pvec[k].GetK().Invert()*n_matches[i][k]<<
"\t";
375 double aX,
double aY,
double aZ,
376 double alphaX,
double alphaY,
double alphaZ,
377 double betaX,
double betaY,
double betaZ,
381 GenerateRandomCube(Points, n, aX, aY, aZ,
382 alphaX, alphaY, alphaZ,
385 alphamax, betamax, 0);
390 double aX,
double aY,
double aZ,
391 double alphaX,
double alphaY,
double alphaZ,
392 double betaX,
double betaY,
double betaZ,
393 double gammaX,
double gammaY,
double gammaZ,
414 newpoint = a + f1*alpha + f2*beta + f3*gamma;
415 newHomgPoint=newpoint;
416 Points.push_back(newHomgPoint);
421 vector<HomgPoint2D> &Points1,
422 vector<HomgPoint2D> &Points2)
428 filein.open(Filename, ios::in);
429 if ( !filein.is_open() )
431 cerr <<
"Error opening file '" << Filename <<
"'" << endl;
434 while ( !filein.eof() )
436 filein.read( (
char*) &x,
sizeof(HOMGPOINT2D_TYPE) );
437 filein.read( (
char*) &y,
sizeof(HOMGPOINT2D_TYPE) );
438 filein.read( (
char*) &w,
sizeof(HOMGPOINT2D_TYPE) );
440 filein.read( (
char*) &x,
sizeof(HOMGPOINT2D_TYPE) );
441 filein.read( (
char*) &y,
sizeof(HOMGPOINT2D_TYPE) );
442 filein.read( (
char*) &w,
sizeof(HOMGPOINT2D_TYPE) );
449 assert( Points1.size() == Points2.size() );
450 return Points1.size();
455 vector<HomgPoint2D> &Points1,
456 vector<HomgPoint2D> &Points2)
458 assert( Points1.size() == Points2.size() );
460 fileout.open(Filename, ios::out);
461 if ( !fileout.is_open() )
463 cerr <<
"Error opening file '" << Filename <<
"'" << endl;
466 for(
unsigned int i=0;i<Points1.size();i++)
468 fileout.write( (
char*) &Points1[i][0],
sizeof(HOMGPOINT2D_TYPE) );
469 fileout.write( (
char*) &Points1[i][1],
sizeof(HOMGPOINT2D_TYPE) );
470 fileout.write( (
char*) &Points1[i][2],
sizeof(HOMGPOINT2D_TYPE) );
471 fileout.write( (
char*) &Points2[i][0],
sizeof(HOMGPOINT2D_TYPE) );
472 fileout.write( (
char*) &Points2[i][1],
sizeof(HOMGPOINT2D_TYPE) );
473 fileout.write( (
char*) &Points2[i][2],
sizeof(HOMGPOINT2D_TYPE) );
476 return Points1.size();
481 vector<HomgPoint3D> & Points )
486 filein.open(Filename, ios::in);
487 if ( !filein.is_open() )
489 cerr <<
"Error opening file '" << Filename <<
"'" << endl;
492 while ( !filein.eof() )
494 filein.read( (
char*) &x,
sizeof(HOMGPOINT3D_TYPE) );
495 filein.read( (
char*) &y,
sizeof(HOMGPOINT3D_TYPE) );
496 filein.read( (
char*) &z,
sizeof(HOMGPOINT3D_TYPE) );
497 filein.read( (
char*) &w,
sizeof(HOMGPOINT3D_TYPE) );
503 return Points.size();
508 vector<HomgPoint3D> & Points )
511 fileout.open(Filename, ios::out);
512 if ( !fileout.is_open() )
514 cerr <<
"Error opening file '" << Filename <<
"'" << endl;
517 for(
unsigned int i=0;i<Points.size();i++)
519 fileout.write( (
char*) &Points[i][0],
sizeof(HOMGPOINT3D_TYPE) );
520 fileout.write( (
char*) &Points[i][1],
sizeof(HOMGPOINT3D_TYPE) );
521 fileout.write( (
char*) &Points[i][2],
sizeof(HOMGPOINT3D_TYPE) );
522 fileout.write( (
char*) &Points[i][3],
sizeof(HOMGPOINT3D_TYPE) );
525 return Points.size();
static void CreateMatchesOutlier(std::vector< BIAS::PMatrix > &pvec, std::vector< BIAS::PMatrix > &n_pvec, unsigned num, unsigned width, unsigned height, double spatial_noise, double r_noise, double c_noise, double outlier_fraction, std::vector< std::vector< BIAS::HomgPoint2D > > &i_matches, std::vector< std::vector< BIAS::HomgPoint2D > > &n_matches, std::vector< BIAS::HomgPoint3D > &points, double minx=-50.0, double maxx=50.0, double miny=-50.0, double maxy=50.0, double minz=-50.0, double maxz=50.0, bool print=false)
Generates ideal matches in cameras from pvec by randomly creating a 3D point in the cube between min[...
static void DeNormalizeMatches(std::vector< BIAS::HomgPoint2D > &match1, std::vector< BIAS::HomgPoint2D > &match2, BIAS::KMatrix K)
de-normalizes the matches by multiplying with K
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
void SetXYZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order XYZ.
HomgPoint2D & Homogenize()
homogenize class data member elements to W==1 by divison by W
static int ReadWorldPoints(const char *Filename, std::vector< BIAS::HomgPoint3D > &Points)
Read 3D world coordinates from a file in binary format.
void Homogenize()
homogenize class data member elements to W==1 by divison by W
bool GetIntersectionsWithImage(unsigned int width, unsigned int height, unsigned int coo[4])
! assumes line is given in pixel coo ! returns true if line intersects with image of given size retur...
static int ReadCorrespondences(const char *Filename, std::vector< BIAS::HomgPoint2D > &Points1, std::vector< BIAS::HomgPoint2D > &Points2)
double GetUniformDistributed(const double min, const double max)
on succesive calls return uniform distributed random variable between min and max ...
unsigned int GetWidth() const
static void DrawMatches(BIAS::Image< unsigned char > &im, std::vector< BIAS::HomgPoint2D > &match1, std::vector< BIAS::HomgPoint2D > &match2, unsigned char lcolor, unsigned char mcolor)
draws the matches in image im with background 0, match in color mcolor and line defined by match in c...
void Set(const HomgPoint2D &p1, const HomgPoint2D &p2)
constructing a line through two points
static int WriteWorldPoints(const char *Filename, std::vector< BIAS::HomgPoint3D > &Points)
Store 3D world coordinates in a file in binary format, so that you can keep the full accuracy of a do...
static void CreateIdealMatches(std::vector< BIAS::PMatrix > &pvec, unsigned num, unsigned width, unsigned height, std::vector< std::vector< BIAS::HomgPoint2D > > &matches, std::vector< BIAS::HomgPoint3D > &points, double minx=-50.0, double maxx=50.0, double miny=-50.0, double maxy=50.0, double minz=-50.0, double maxz=50.0)
Generates ideal matches in cameras from pvec by randomly creating a 3D point in the cube between min[...
static void NormalizeMatches(std::vector< BIAS::HomgPoint2D > &match1, std::vector< BIAS::HomgPoint2D > &match2, BIAS::KMatrix K)
normalizes the matches by multiplying with K^-1
unsigned int GetHeight() const
static void GenerateRandomCube(std::vector< BIAS::HomgPoint3D > &Points, int n=200, double aX=-50.0, double aY=-50.0, double aZ=-50.0, double alphaX=1.0, double alphaY=0.0, double alphaZ=0.0, double betaX=0.0, double betaY=1.0, double betaZ=0.0, double gammaX=0.0, double gammaY=0.0, double gammaZ=1.0, double alphamax=100.0, double betamax=100.0, double gammamax=100.0)
Generates 'n' points which are all located in a cube specified by space point coords aX...
a line l = (a b c)^T is a form of the implicit straight line equation 0 = a*x + b*y + c if homogenize...
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
static void GenerateRandomPlane(std::vector< BIAS::HomgPoint3D > &Points, int n=200, double aX=-50.0, double aY=-50.0, double aZ=0.0, double alphaX=1.0, double alphaY=0.0, double alphaZ=0.0, double betaX=0.0, double betaY=1.0, double betaZ=0.0, double alphamax=100.0, double betamax=100.0)
Generates 'n' points which are all located on a plane specified by space point coords aX...
int GetRotationAnglesXYZ(double &PhiX, double &PhiY, double &PhiZ) const
Get Euler angles for this rotation matrix in order XYZ.
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
double GetNormalDistributed(const double mean, const double sigma)
on succesive calls return normal distributed random variable with mean and standard deviation sigma ...
describes a projective 3D -> 2D mapping in homogenous coordinates
drawing simple entities into the image like rectangles or lines As all functions are static they have...
static int WriteCorrespondences(const char *Filename, std::vector< BIAS::HomgPoint2D > &Points1, std::vector< BIAS::HomgPoint2D > &Points2)
Store 2D picture correspondences in a file in binary format, so that you can keep the full accuracy o...
KMatrix Invert() const
returns analyticaly inverted matrix
static void CreateMatches(std::vector< BIAS::PMatrix > &pvec, unsigned num, unsigned width, unsigned height, double spatial_noise, std::vector< std::vector< BIAS::HomgPoint2D > > &i_matches, std::vector< std::vector< BIAS::HomgPoint2D > > &n_matches, std::vector< BIAS::HomgPoint3D > &points, double minx=-50.0, double maxx=50.0, double miny=-50.0, double maxy=50.0, double minz=-50.0, double maxz=50.0)
Generates ideal matches in cameras from pvec by randomly creating a 3D point in the cube between min[...
Vector3< T > & Normalize()
normalize this vector to length 1
class for producing random numbers from different distributions
class BIASGeometryBase_EXPORT HomgPoint2D
void Compose(const Matrix3x3< double > &K, const Matrix3x3< double > &R, const Vector3< double > &C)
composes this from K, R and C using P = [ K R' | -K R' C ] with R' = transpose(R) ...
class BIASGeometryBase_EXPORT HomgPoint3D