25 #include "GenSynthMatches.hh"
26 #include <Base/ImageUtils/ImageDraw.hh>
27 #include <Geometry/PMatrix.hh>
28 #include <Utils/ThreeDOut.hh>
53 os << setw(3) << i <<
" : " << p3d <<
" : ";
62 os << setw(3)<<i<<
" : "<<
80 os << k<<
":"<<m.
_Points[k][i]<<
" ";
85 os << setw(3) << i <<
" : " << p3d <<
" : ";
87 os << k<<
":"<<m.
_Points[k][i]<<
" ";
94 os << setw(3)<<i<<
" : "<<
97 os << k<<
":"<<m.
_Points[k][i]<<
" ";
105 os << "normalized true matches (0-"<<*m._NumStaticPoints-1<<"), outliers ("
106 <<*m._NumStaticPoints<<"-"<<*m._NumStaticPoints+*m._NumOutliers-1
107 <<") and moving objs ("<<*m._NumStaticPoints+*m._NumOutliers<<"-"
108 <<*m._NumStaticPoints+*m._NumOutliers+
109 *m._NumMovingPoints * *m._NumMovingObjects-1<<"): \n";
110 for (int i=0; i<*m._NumStaticPoints; i++){
111 os << setw(3)<<i<<" : "<< m._StaticPoints[i] << " : ";
112 for (int k=0; k<*m._NumImages; k++){
113 os << k<<":"<<m._GT_NormalizedPoints[k][i]<<" ";
117 for (int i=*m._NumStaticPoints; i<*m._NumStaticPoints+*m._NumOutliers; i++){
118 os << setw(3) << i << " : " << p3d << " : ";
119 for (int k=0; k<*m._NumImages; k++){
120 os << k<<":"<<m._GT_NormalizedPoints[k][i]<<" ";
124 for (int i=*m._NumStaticPoints+*m._NumOutliers;
125 i<*m._NumStaticPoints+*m._NumOutliers+
126 *m._NumMovingPoints * *m._NumMovingObjects; i++){
127 os << setw(3)<<i<<" : "<<
128 m._MovingPoints[i-*m._NumStaticPoints-*m._NumOutliers] << " : ";
129 for (int k=0; k<*m._NumImages; k++){
130 os << k<<":"<<m._GT_NormalizedPoints[k][i]<<" ";
137 os << "normalized noisy matches (0-"<<*m._NumStaticPoints-1<<"), outliers ("
138 <<*m._NumStaticPoints<<"-"<<*m._NumStaticPoints+*m._NumOutliers-1
139 <<") and moving objs ("<<*m._NumStaticPoints+*m._NumOutliers<<"-"
140 <<*m._NumStaticPoints+*m._NumOutliers+
141 *m._NumMovingPoints * *m._NumMovingObjects-1<<"): \n";
142 for (int i=0; i<*m._NumStaticPoints; i++){
143 os << setw(3)<<i<<" : "<< m._StaticPoints[i] << " : ";
144 for (int k=0; k<*m._NumImages; k++){
145 os << k<<":"<<m._NormalizedPoints[k][i]<<" ";
149 for (int i=*m._NumStaticPoints; i<*m._NumStaticPoints+*m._NumOutliers; i++){
150 os << setw(3) << i << " : " << p3d << " : ";
151 for (int k=0; k<*m._NumImages; k++){
152 os << k<<":"<<m._NormalizedPoints[k][i]<<" ";
156 for (int i=*m._NumStaticPoints+*m._NumOutliers;
157 i<*m._NumStaticPoints+*m._NumOutliers+*m._NumMovingPoints *
158 *m._NumMovingObjects; i++){
159 os << setw(3)<<i<<" : "<<
160 m._MovingPoints[i-*m._NumStaticPoints-*m._NumOutliers] << " : ";
161 for (int k=0; k<*m._NumImages; k++){
162 os << k<<":"<<m._NormalizedPoints[k][i]<<" ";
204 if (!points3D_set_by_user) {
231 BIASERR(
"user error: no 3D points set. Use SetStatic3DPoints() "
232 "or Create3DPoints() before creating matches.");
233 return GSM_RETURN_NO_STATIC_3D_POINTS;
237 BIASERR(
"user error: no moving 3D points set. Use Set3DPoints() "
238 "or Create3DPoints() before creating matches.");
239 return GSM_RETURN_NO_MOVING_3D_POINTS;
246 return GSM_RETURN_ERROR_HANDLING_STATIC_POINT;
254 return GSM_RETURN_ERROR_HANDLING_MOVING_POINT;
257 return GSM_RETURN_OK;
269 BIASERR(
"user error: maybe no common viewing volume");
270 return GSM_RETURN_STATIC_OBJECT_NOT_VISIBLE;
277 BIASERR(
"user error: moving object not visible");
278 return GSM_RETURN_MOVING_OBJECT_NOT_VISIBLE;
281 return GSM_RETURN_OK;
295 const std::vector<std::vector<HomgPoint2D> >* p)
const
299 int imag=(int)rint(mag);
302 unsigned char background = 255;
306 if (minindex==maxindex){
312 BIASERR(
"invalid image to draw: "<<minindex<<
" "<<maxindex<<
" "
317 unsigned char red[3]={255, 0, 0};
319 #ifndef BIAS_HAVE_OPENCV
320 unsigned char hred[3]={200, 0, 0};
322 unsigned char green[3]={0, 255, 0};
324 #ifndef BIAS_HAVE_OPENCV
325 unsigned char hgreen[3]={0, 200, 0};
327 unsigned char blue[3]={0, 0, 255};
328 #ifndef BIAS_HAVE_OPENCV
329 unsigned char hblue[3]={0, 0, 128};
331 unsigned int start[2], end[2];
332 for (
int k=minindex+1; k<maxindex+1; k++){
335 for (
int l=-line_width;l<=line_width; l++){
336 start[0]=(
unsigned int)rint((*p)[k-1][i][0]*mag);
337 start[1]=(
unsigned int)rint((*p)[k-1][i][1]*mag+(double)l);
338 end[0]=(
unsigned int)rint((*p)[k][i][0]*mag);
339 end[1]=(
unsigned int)rint((*p)[k][i][1]*mag+(double)l);
340 #ifndef BIAS_HAVE_OPENCV
345 Arrow(im, start[0], start[1], end[0], end[1],
351 for (
int i=*_NumStaticPoints; i<*_NumStaticPoints+*
_NumOutliers; i++){
352 for (
int l=-line_width;l<=line_width; l++){
353 start[0]=(
unsigned int)rint((*p)[k-1][i][0]*mag);
354 start[1]=(
unsigned int)rint((*p)[k-1][i][1]*mag+(double)l);
355 end[0]=(
unsigned int)rint((*p)[k][i][0]*mag);
356 end[1]=(
unsigned int)rint((*p)[k][i][1]*mag+(double)l);
357 #ifndef BIAS_HAVE_OPENCV
362 Arrow(im, start[0], start[1], end[0], end[1],
368 for (
int i=*_NumStaticPoints+*_NumOutliers;
369 i<*_NumStaticPoints+*_NumOutliers+
371 for (
int l=-line_width;l<=line_width; l++){
372 start[0]=(
unsigned int)rint((*p)[k-1][i][0]*mag);
373 start[1]=(
unsigned int)rint((*p)[k-1][i][1]*mag+(double)l);
374 end[0]=(
unsigned int)rint((*p)[k][i][0]*mag);
375 end[1]=(
unsigned int)rint((*p)[k][i][1]*mag+(double)l);
376 #ifndef BIAS_HAVE_OPENCV
381 Arrow(im, start[0], start[1], end[0], end[1],
392 const std::vector<std::vector<HomgPoint2D> >* p)
const
395 BIASERR(
"invalid image to draw: "<<index<<
" "<<*
_NumImages);
398 unsigned char red[3]={255, 0, 0};
399 unsigned char green[3]={0, 255, 0};
400 unsigned char blue[3]={0, 0, 255};
402 int k = index, imag=(int)rint(mag);
403 unsigned int radius=1, end[2];
406 end[0]=(
unsigned int)rint((*p)[k][i][0]*mag);
407 end[1]=(
unsigned int)rint((*p)[k][i][1]*mag);
408 if (end[0]>=radius && end[1]>=radius &&
415 for (
int i=*_NumStaticPoints; i<*_NumStaticPoints+*
_NumOutliers; i++){
416 end[0]=(
unsigned int)rint((*p)[k][i][0]*mag);
417 end[1]=(
unsigned int)rint((*p)[k][i][1]*mag);
418 if (end[0]>=radius && end[1]>=radius &&
424 for (
int i=*_NumStaticPoints+*_NumOutliers;
426 end[0]=(
unsigned int)rint((*p)[k][i][0]*mag);
427 end[1]=(
unsigned int)rint((*p)[k][i][1]*mag);
428 if (end[0]>=radius && end[1]>=radius &&
439 ofstream of(file.c_str());
441 BIASERR(
"error opening "<<file);
446 BIASERR(
"error writing to "<<file);
455 # pragma warning(push)
456 # pragma warning(disable : 4702)
460 ifstream of(file.c_str());
462 BIASERR(
"error opening "<<file);
465 BIASERR(
"unfinished");
478 # pragma warning(pop)
517 GetGTR(std::vector<RMatrixBase>& R)
const
524 GetGTC(std::vector<HomgPoint3D>& C)
const
531 GetGTP(std::vector<PMatrixBase>& P)
const
557 BIASERR(
"invalid frame number");
563 for (
unsigned i=0; i<p.size(); i++)
572 for (
int i=*_NumStaticPoints; i<*
_NumPoints; i++) inl[i]=
false;
619 _GT_C[i-1].GetEuclidean(C1);
620 _GT_C[i].GetEuclidean(C2);
621 BIASCDOUT(GSM_FMATRIX,
"\n------------------------------------------\n\n");
622 BIASCDOUT(GSM_FMATRIX,
"C1: "<<C1<<
"R1: "<<
_GT_R[i-1]<<endl);
623 BIASCDOUT(GSM_FMATRIX,
"C2: "<<C2<<
"R2: "<<
_GT_R[i]<<endl);
624 F[i-1][0].Compose(
_GT_R[i-1], C1,
_GT_R[i], C2);
625 BIASCDOUT(GSM_FMATRIX,
"F from image "<<i-1<<
" to "<<i
626 <<
" for static objects is "<<F[i-1][0]<<endl);
630 BIASCDOUT(GSM_FMATRIX, setw(5)<<k<<
" p2^T * F * p1 = "
640 _GT_C[i-1].GetEuclidean(C1);
642 BIASCDOUT(GSM_FMATRIX,
"\n------\n\nC1: "<<C1<<
"R1: "<<R1<<endl);
644 idx=l * *_NumMovingObjects + i-1;
650 idx=l * *_NumMovingObjects + i;
653 _GT_C[i].GetEuclidean(C2);
657 R.
Mult((C2-cc2), C2);
663 BIASCDOUT(GSM_FMATRIX,
"C2: "<<C2<<
"R2: "<<R2<<endl);
665 F[i-1][l+1].Compose(R1, C1, R2, C2);
666 BIASCDOUT(GSM_FMATRIX,
"F from image "<<i-1<<
" to "<<i<<
" for "<<l
667 <<
"th objects is "<<F[i-1][l+1]<<endl);
673 BIASCDOUT(GSM_FMATRIX, setw(5)<<k<<
" p2^T * F * p1 = "
692 BIASCDOUT(D_MD_IO,
"using GenSynthMatches::Write(ostream)\n");
697 os.write((
char*)
_ImWidth,
sizeof(
int));
725 os.write((
char*)
_StaticPoints[i].GetData(),
sizeof(HOMGPOINT3D_TYPE)*4);
728 os.write((
char*)
_MovingPoints[i].GetData(),
sizeof(HOMGPOINT3D_TYPE)*4);
732 os.write((
char*)&(*
_MotionVecX)[i],
sizeof(
double));
733 os.write((
char*)&(*
_MotionVecY)[i],
sizeof(
double));
734 os.write((
char*)&(*
_MotionVecZ)[i],
sizeof(
double));
735 os.write((
char*)&(*
_RotVecX)[i],
sizeof(
double));
736 os.write((
char*)&(*
_RotVecY)[i],
sizeof(
double));
737 os.write((
char*)&(*
_RotVecZ)[i],
sizeof(
double));
739 int idx=k * *_NumImages + i;
747 os.write((
char*)
_GT_R[i].GetData(),
sizeof(ROTATION_MATRIX_TYPE)*9);
748 os.write((
char*)
_GT_C[i].GetData(),
sizeof(HOMGPOINT3D_TYPE)*4);
750 int idx=k * *_NumImages + i;
757 for (
int k=*_NumStaticPoints; k<*_NumStaticPoints+*
_NumOutliers; k++){
758 os.write((
char*)
_GT_Points[i][k].GetData(),
sizeof(HOMGPOINT2D_TYPE)*3);
774 BIASCDOUT(D_MD_IO,
"using GenSynthMatches::Read(ostream)\n");
779 is.read((
char*)
_ImWidth,
sizeof(
int));
810 is.read((
char*)
_StaticPoints[i].GetData(),
sizeof(HOMGPOINT3D_TYPE)*4);
814 is.read((
char*)
_MovingPoints[i].GetData(),
sizeof(HOMGPOINT3D_TYPE)*4);
837 is.read((
char*)&(*
_MotionVecX)[i],
sizeof(
double));
838 is.read((
char*)&(*
_MotionVecY)[i],
sizeof(
double));
839 is.read((
char*)&(*
_MotionVecZ)[i],
sizeof(
double));
840 is.read((
char*)&(*
_RotVecX)[i],
sizeof(
double));
841 is.read((
char*)&(*
_RotVecY)[i],
sizeof(
double));
842 is.read((
char*)&(*
_RotVecZ)[i],
sizeof(
double));
844 int idx=k * *_NumImages + i;
853 is.read((
char*)
_GT_R[i].GetData(),
sizeof(ROTATION_MATRIX_TYPE)*9);
854 is.read((
char*)
_GT_C[i].GetData(),
sizeof(HOMGPOINT3D_TYPE)*4);
857 int idx=k * *_NumImages + i;
866 for (
int k=*_NumStaticPoints; k<*_NumStaticPoints+*
_NumOutliers; k++){
867 is.read((
char*)
_GT_Points[i][k].GetData(),
sizeof(HOMGPOINT2D_TYPE)*3);
923 ep =
_GT_P[i-1] * C4;
925 if (fabs(tmp.ScalarProduct(ep))>0.8){
926 tmp.Set(1.0, 0.0, 0.0);
936 double scale = tan(ang) * (
_GT_C[i-1] - C).NormL2();
938 nC.
Set(C[0]+dC[0], C[1]+dC[1], C[2]+dC[2], 1.0);
953 BIASERR(
"unfinished");
964 BIASCDOUT(GSM_CAM_MOTION,
"_K: "<<
_K<<endl);
965 BIASCDOUT(GSM_CAM_MOTION,
"_Ki: "<<
_Ki<<endl);
968 _GT_R[i].GetRotationAnglesXYZ(r[0], r[1], r[2]);
969 _R[i].GetRotationAnglesXYZ(nr[0], nr[1], nr[2]);
970 BIASCDOUT(GSM_CAM_MOTION,
"_GT_R["<<i<<
"] = "<<r<<endl);
971 BIASCDOUT(GSM_CAM_MOTION,
"_R["<<i<<
"] = "<<nr<<endl);
972 BIASCDOUT(GSM_CAM_MOTION,
"_GT_C["<<i<<
"] = "<<
_GT_C[i]<<endl);
973 BIASCDOUT(GSM_CAM_MOTION,
"_C["<<i<<
"] = "<<
_C[i]<<endl);
974 BIASCDOUT(GSM_CAM_MOTION,
"_GT_P["<<i<<
"] = "<<
_GT_P[i]<<endl);
975 BIASCDOUT(GSM_CAM_MOTION,
"_P["<<i<<
"] = "<<
_P[i]<<endl);
983 BIASASSERT((*_MovingObjMotionVecX).size() >= *
_NumImages
985 BIASASSERT((*_MovingObjMotionVecY).size() >= *
_NumImages
987 BIASASSERT((*_MovingObjMotionVecZ).size() >= *
_NumImages
989 BIASASSERT((*_MovingObjRotVecX).size() >= *
_NumImages
991 BIASASSERT((*_MovingObjRotVecY).size() >= *
_NumImages
993 BIASASSERT((*_MovingObjRotVecZ).size() >= *
_NumImages
1002 BIASCDOUT(GSM_MOVIN_TRANSF, n<<
"th moving object\n");
1004 for (
int k=0; k<ni; k++){
1006 t[0]=(*_MovingObjMotionVecX)[idx];
1007 t[1]=(*_MovingObjMotionVecY)[idx];
1008 t[2]=(*_MovingObjMotionVecZ)[idx];
1009 r[0]=(*_MovingObjRotVecX)[idx];
1010 r[1]=(*_MovingObjRotVecY)[idx];
1011 r[2]=(*_MovingObjRotVecZ)[idx];
1013 BIASCDOUT(GSM_MOVIN_TRANSF, idx<<
"th moving transf: "
1021 BIASASSERT(index >= 0);
1023 double minx, maxx, miny, maxy, minz, maxz;
1066 minx=miny=minz=DBL_MAX;
1067 maxx=maxy=maxz=-DBL_MAX;
1068 for (
int j=0; j<16; j++){
1069 BIASCDOUT(GSM_VIEWINGVOL,
"p3d["<<j<<
"] = "<<p3d[j]<<endl);
1070 if (p3d[j][0]<minx) minx=p3d[j][0];
1071 if (p3d[j][0]>maxx) maxx=p3d[j][0];
1072 if (p3d[j][1]<miny) miny=p3d[j][1];
1073 if (p3d[j][1]>maxy) maxy=p3d[j][1];
1074 if (p3d[j][2]<minz) minz=p3d[j][2];
1075 if (p3d[j][2]>maxz) maxz=p3d[j][2];
1077 BIASCDOUT(GSM_VIEWINGVOL,
"Generating 3D point between ["<<minx<<
", "
1078 <<miny<<
", "<<minz<<
"] and ["<<maxx<<
", "<<maxy<<
", "<<maxz
1085 bool is_seen=
true, seen_in_k=
false;
1099 is_seen=is_seen && seen_in_k;
1101 BIASCDOUT(GSM_3D_POINTS, p<<
" not seen in P["<<k<<
"] : "<<p2d
1108 cerr <<
"_P["<<k<<
"]: "<<
_P[k]<<endl;
1109 cerr <<
"_GT_P["<<k<<
"]: "<<
_GT_P[k]<<endl;
1111 cerr <<
"tried to generate 3D points between ["<<minx<<
", "<<miny
1112 <<
", "<<minz<<
"] and ["<<maxx<<
", "<<maxy<<
", "<<maxz<<
"]\n";
1113 BIASERR(
"more than 1000 tries for one 3D point, maybe no common "
1114 <<
"viewing volume present");
1115 return GSM_RETURN_STATIC_OBJECT_NOT_VISIBLE;
1120 BIASCDOUT(GSM_3D_POINTS, index<<
"th true 3D point: "
1123 return GSM_RETURN_OK;
1129 double minx, maxx, miny, maxy, minz, maxz;
1172 minx=miny=minz=DBL_MAX;
1173 maxx=maxy=maxz=-DBL_MAX;
1174 for (
int j=0; j<16; j++){
1175 BIASCDOUT(GSM_VIEWINGVOL,
"p3d["<<j<<
"] = "<<p3d[j]<<endl);
1176 if (p3d[j][0]<minx) minx=p3d[j][0];
1177 if (p3d[j][0]>maxx) maxx=p3d[j][0];
1178 if (p3d[j][1]<miny) miny=p3d[j][1];
1179 if (p3d[j][1]>maxy) maxy=p3d[j][1];
1180 if (p3d[j][2]<minz) minz=p3d[j][2];
1181 if (p3d[j][2]>maxz) maxz=p3d[j][2];
1183 BIASCDOUT(GSM_VIEWINGVOL,
"Generating 3D point between ["<<minx<<
", "
1184 <<miny<<
", "<<minz<<
"] and ["<<maxx<<
", "<<maxy<<
", "<<maxz
1191 bool is_seen=
true, seen_in_k=
false;
1207 is_seen=is_seen && seen_in_k;
1209 BIASCDOUT(GSM_3D_POINTS, p<<
" not seen in P["<<k<<
"] : "<<p2d
1216 cerr <<
"_P["<<k<<
"]: "<<
_P[k]<<endl;
1217 cerr <<
"_GT_P["<<k<<
"]: "<<
_GT_P[k]<<endl;
1219 cerr <<
"tried to generate 3D points between ["<<minx<<
", "<<miny
1220 <<
", "<<minz<<
"] and ["<<maxx<<
", "<<maxy<<
", "<<maxz<<
"]\n";
1221 BIASERR(
"more than 1000 tries for one 3D point, maybe no common "
1222 <<
"viewing volume present");
1228 BIASCDOUT(GSM_3D_POINTS, i<<
"th true 3D point: "<<
_StaticPoints[i]<<endl);
1239 bool seen_in_k=
true, any_seen_in_k=
false;
1243 corner[0][3]=corner[1][3]=corner[2][3]=corner[3][3]=1.0;
1244 corner[4][3]=corner[5][3]=corner[6][3]=corner[7][3]=1.0;
1246 corner[0][2]=corner[1][2]=corner[2][2]=corner[3][2]=
1247 (*_SizeMovingObjZ)[i]/2.0;
1248 corner[4][2]=corner[5][2]=corner[6][2]=corner[7][2]=
1249 -(*_SizeMovingObjZ)[i]/2.0;
1251 corner[0][1]=corner[1][1]=corner[4][1]=corner[5][1]=
1252 (*_SizeMovingObjY)[i]/2.0;
1253 corner[2][1]=corner[3][1]=corner[6][1]=corner[7][1]=
1254 -(*_SizeMovingObjY)[i]/2.0;
1256 corner[0][0]=corner[3][0]=corner[4][0]=corner[7][0]=
1257 (*_SizeMovingObjX)[i]/2.0;
1258 corner[1][0]=corner[2][0]=corner[5][0]=corner[6][0]=
1259 -(*_SizeMovingObjX)[i]/2.0;
1262 int idx = i * *_NumImages + k;
1264 for (
int l=0; l<8; l++){
1270 any_seen_in_k = any_seen_in_k || seen_in_k;
1272 if (!any_seen_in_k){
1275 cerr <<
"_P["<<k<<
"]: "<<
_P[k]<<endl;
1276 cerr <<
"_GT_P["<<k<<
"]: "<<
_GT_P[k]<<endl;
1277 cerr <<
"_R["<<k<<
"]: "<<
_R[k]<<endl;
1278 _R[k].GetRotationAnglesXYZ(r1, r2, r3);
1279 cerr <<
"rotation angles of _R["<<k<<
"]: "<<r1<<
"\t"<< r2<<
"\t"
1281 cerr <<
"_GT_R["<<k<<
"]: "<<
_GT_R[k]<<endl;
1282 cerr <<
"_C["<<k<<
"]: "<<
_C[k]<<endl;
1283 cerr <<
"_GT_C["<<k<<
"]: "<<
_GT_C[k]<<endl;
1284 cerr <<
"corners: "<<endl;
1285 for (
int l=0; l<8; l++){
1289 cerr <<setw(3)<<l<<
" corn: "<<corner[l]<<
"\tp3d: "<<p3d<<
"\tp2d: "
1290 <<p2d<<
"\tnp2d: "<<np2d<<endl;
1292 BIASERR(
"no corner of moving object not seen in image "<<k);
1296 p3d.
Set(0.0, 0.0, 0.0);
1303 cerr <<
"_P["<<k<<
"]: "<<
_P[k]<<endl;
1304 cerr <<
"_GT_P["<<k<<
"]: "<<
_GT_P[k]<<endl;
1305 cerr <<
"_R["<<k<<
"]: "<<
_R[k]<<endl;
1306 _R[k].GetRotationAnglesXYZ(r1, r2, r3);
1307 cerr <<
"rotation angles of _R["<<k<<
"]: "<<r1<<
"\t"<< r2<<
"\t"
1309 cerr <<
"_GT_R["<<k<<
"]: "<<
_GT_R[k]<<endl;
1310 cerr <<
"_C["<<k<<
"]: "<<
_C[k]<<endl;
1311 cerr <<
"_GT_C["<<k<<
"]: "<<
_GT_C[k]<<endl;
1312 BIASERR(
"center of moving object not seen in image "<<k<<
" center "
1313 <<p3d<<
" p2d: "<<p2d<<
" np2d: "<<np2d);
1328 int movingPointIndex = index - offset;
1333 BIASASSERT(movingPointIndex >= 0);
1341 bool seen_in_all=
true, seen_in_k=
true;
1343 x2=(*_SizeMovingObjX)[l]/2.0;
1344 y2=(*_SizeMovingObjY)[l]/2.0;
1345 z2=(*_SizeMovingObjZ)[l]/2.0;
1360 BIASCDOUT(GSM_3D_POINTS, p3d<<
" moving point not seen in k "
1361 <<p2d<<
"/"<<np2d<<endl);
1365 seen_in_all = seen_in_k && seen_in_all;
1367 }
while (!seen_in_all && mytry < 1000);
1368 BIASCDOUT(GSM_3D_POINTS, movingPointIndex<<
"th moving 3D point : "
1371 return GSM_RETURN_OK;
1382 bool seen_in_all=
true, seen_in_k=
true;
1388 x2=(*_SizeMovingObjX)[l]/2.0;
1389 y2=(*_SizeMovingObjY)[l]/2.0;
1390 z2=(*_SizeMovingObjZ)[l]/2.0;
1404 BIASCDOUT(GSM_3D_POINTS, p3d<<
" moving point not seen in k "
1405 <<p2d<<
"/"<<np2d<<endl);
1409 seen_in_all = seen_in_k && seen_in_all;
1410 }
while (!seen_in_all);
1411 BIASCDOUT(GSM_3D_POINTS, mpi<<
"th moving 3D point : "
1435 if (res != GSM_RETURN_OK) {
1454 BIASCDOUT(GSM_NOISE,
"s2d: "<<noise << endl);
1458 BIASERR(
"static point "<<i<<
"<< not seen in image "<<k<<
" : "
1461 return GSM_RETURN_STATIC_OBJECT_NOT_VISIBLE;
1468 BIASCDOUT(GSM_2D_MATCHES,
"match "<<i<<
" : ");
1470 BIASCDOUT(GSM_2D_MATCHES, k<<
":"<<
_GT_Points[k][i]<<
"/"
1474 BIASCDOUT(GSM_2D_MATCHES, endl);
1478 return GSM_RETURN_OK;
1494 if (!
_IsSeen(_GT_Points[k][i])) {
1495 BIASERR(
"static GT point "<<i<<
"<< not seen in image "<<k<<
" : "
1496 <<_GT_Points[k][i]);
1498 return GSM_RETURN_STATIC_OBJECT_NOT_VISIBLE;
1502 return GSM_RETURN_OK;
1509 bool seen_from_all=
true, seen_in_k=
true;
1518 BIASCDOUT(GSM_NOISE,
"outl: "<<noise << endl);
1523 seen_from_all = seen_from_all && seen_in_k;
1529 _GT_Points[1][i]=_GT_Points[0][i]+outldisp;
1532 BIASCDOUT(GSM_NOISE,
"outl: "<<noise << endl);
1533 _Points[1][i]=_GT_Points[1][i]+noise;
1537 seen_from_all = seen_from_all && seen_in_k;
1541 outldisp=R*outldisp;
1543 _GT_Points[k][i]=_GT_Points[k-1][i]+outldisp;
1546 BIASCDOUT(GSM_NOISE,
"outl: "<<noise << endl);
1547 _Points[k][i]=_GT_Points[k][i]+noise;
1551 seen_from_all = seen_from_all && seen_in_k;
1552 if (!seen_from_all)
break;
1554 }
while (!seen_from_all);
1560 BIASCDOUT(GSM_2D_MATCHES,
"match "<<i<<
" : ");
1562 BIASCDOUT(GSM_2D_MATCHES, k<<
":"<<
_GT_Points[k][i]<<
"/"
1566 BIASCDOUT(GSM_2D_MATCHES, endl);
1579 BIASERR(
"internal data structs not allocated call "
1580 <<
"_CreateStatic2DPoints() first ");
1586 if (res != GSM_RETURN_OK) {
1597 BIASCDOUT(GSM_MOVIN_3D_POINTS,
"object "<<l<<endl);
1599 int idx=l* *_NumImages + k;
1602 BIASCDOUT(GSM_MOVIN_3D_POINTS,
"using moving transf "<<idx
1603 <<
" and _MovingPoints["<<i-offset+l * *_NumMovingPoints
1604 <<
"] to create point in image "<<k<<
"\n");
1609 BIASCDOUT(GSM_NOISE,
"m2d: "<<noise << endl);
1611 BIASCDOUT(GSM_MOVIN_3D_POINTS,
"moving 3d point "<<i<<
" "<<p3d
1612 <<
"\t "<<
_Points[k][i]<<endl);
1615 BIASERR(
"moving point "<<i<<
"<< not seen in image "<<k<<
" : "
1618 return GSM_RETURN_ERROR_HANDLING_MOVING_POINT;
1623 return GSM_RETURN_OK;
1632 BIASERR(
"internal data structs not allocated call "
1633 <<
"_CreateStatic2DPoints() first ");
1640 BIASCDOUT(GSM_MOVIN_3D_POINTS,
"object "<<l<<endl);
1642 int idx=l * *_NumImages + k;
1645 BIASCDOUT(GSM_MOVIN_3D_POINTS,
"using moving transf "<< idx <<
_MovingTransform[idx]
1646 <<
" and _MovingPoints["<<i-offset+l * *_NumMovingPoints
1647 <<
"] to create point in image "<<k<<
"\n");
1651 BIASCDOUT(GSM_MOVIN_3D_POINTS,
"moving 3d point "<<i<<
" "<<p3d
1655 if (!
_IsSeen(_GT_Points[k][i])) {
1656 BIASERR(
"moving point "<<i<<
"<< not seen in image "<<k<<
" : "
1657 <<_GT_Points[k][i]);
1659 return GSM_RETURN_ERROR_HANDLING_MOVING_POINT;
1664 BIASCDOUT(GSM_MOVIN_3D_POINTS, endl);
1665 return GSM_RETURN_OK;
1751 _UserGivenP=para.
AddParamBool(
"UserGivenP",
"if true use MotionVec? and RotVec?, if false use Overall? ",
true, 0, grp);
1756 dflt[0]=0.0; dflt[1]=-0.1;
1763 dflt[0]=0.0; dflt[1]=0.0;
1770 dflt[0]=0.0; dflt[1]=1.0;
1777 dflt[0]=0.0; dflt[1]=0.0;
1784 dflt[0]=0.0; dflt[1]=-0.1;
1791 dflt[0]=0.0; dflt[1]=0.0;
1841 _SigmaCamRotX=para.
AddParamDouble(
"SigmaCamRotX",
"sigma of normal distributed noise of cam rotation", 0.01, 0.0, DBL_MAX, 0, grp);
1847 _SigmaCamRotY=para.
AddParamDouble(
"SigmaCamRotY",
"sigma of normal distributed noise of cam rotation", 0.01, 0.0, DBL_MAX, 0, grp);
1853 _SigmaCamRotZ=para.
AddParamDouble(
"SigmaCamRotZ",
"sigma of normal distributed noise of cam rotation", 0.01, 0.0, DBL_MAX, 0, grp);
1871 _OutlDirChange=para.
AddParamDouble(
"OutlDirChange",
"maximal change of direction for outlying matches (RAD)", 0.05, -M_PI, M_PI, 0, grp);
1891 sizedflt[0]=5.0; sizedflt[1]=5.0;
1897 sizedflt[0]=1.5; sizedflt[1]=1.5;
1903 sizedflt[0]=2.0; sizedflt[1]=2.0;
1911 dflt[0]=5.0; dflt[1]=5.5; dflt[2]=0.0; dflt[3]=-1.0;
1918 dflt[0]=0.0; dflt[1]=0.0; dflt[2]=1.5; dflt[3]=1.5;
1925 dflt[0]=30.0; dflt[1]=30.0; dflt[2]=18.0; dflt[3]=18.0;
1932 dflt[0]=0.0; dflt[1]=0.0; dflt[2]=0.0; dflt[3]=0.0;
1939 dflt[0]=0.0; dflt[1]=-0.1; dflt[2]=0.0; dflt[3]=0.1;
1946 dflt[0]=0.0; dflt[1]=0.0; dflt[2]=0.0; dflt[3]=0.0;
1962 BIASERR(
"inconsistent parameter (number of points), correcting");
1971 BIASERR(
"inconsistent parameter (NumMovingObjects and SizeMovingObj?)"
1984 BIASERR(
"inconsistent parameter (NumMovingObjects, NumImages and MovingObj*Vec?)");
1994 BIASERR(
"MotionVec? and RotVec? must be of size NumImages");
2049 vector<HomgPoint3D> C;
2050 vector<RMatrixBase> R;
2054 vector<PMatrix> P(R.size());
2056 for (
unsigned i=0; i< P.size(); i++){
2057 P[i].Compose(K, R[i], C[i]);
2059 vector<HomgPoint3D> p, mp;
2062 for (
unsigned i=0; i<mp.size(); i++) p.push_back(mp[i]);
2069 const vector<HomgPoint3D>& p)
const
2073 RGBAuc green(0,255,0,255), red(255,0,0,255), blue(0,0,255,255);
2075 const double fac = (P.size()>1)?((
double)P.size()-1.0):(1.0);
2077 for (
unsigned i=0; i<P.size(); i++){
2078 double f = (double)i/fac;
2079 color =
RGBAuc((
unsigned char)rint(f*255),
2080 (
unsigned char)rint((1.0-f)*255),0,255);
2086 int max_inlier, max_outlier, max_mov_obj, num_mov, num_pt;
2088 BIASASSERT(max_outlier>=0 && max_outlier<max_mov_obj);
2089 num_mov = max_mov_obj - max_outlier;
2090 num_pt = num_mov + max_inlier;
2091 cout << p.size() <<
" "<<num_pt<<endl;
2092 BIASASSERT((
int)p.size()==num_pt);
2094 for (
int i=0; i<max_inlier; i++){
2097 for (
int i=max_inlier; i<num_pt; i++){
2109 if (tdo.
VRMLOut(file_name)!=0){
2110 BIASERR(
"error creating vrml file \""<<file_name<<
"\"\n");
2113 cout <<
"wrote \""<<file_name<<
"\"\n";
void Release()
reimplemented from ImageBase
std::vector< std::vector< HomgPoint2D > > _NormalizedPoints
unsigned int AddPose(const PoseParametrization &p, const RGBAuc &colorSelection=RGBAuc_WHITE_OPAQUE, const double radius=0.1, const double scale=1.0, const int camera_type=-1)
std::vector< BIAS::HomgPoint3D > _MovingPoints
int Read(const std::string &fname)
binary read
double * GetParamDouble(const std::string &name) const
static int Arrow(Image< StorageType > &im, const unsigned start[2], const unsigned end[2], const unsigned length, const unsigned width, const StorageType value[])
draws an arrow from start to end, the tips of the head are length pixel back on the line and width pi...
BIAS::Vector< double > * _MovingObjRotVecZ
void Set(const T *pv)
copy the array of vectorsize beginning at *T to this->data_
BIAS::Vector< double > * _RotVecX
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.
int _AreMovingObjectsVisible()
returns -1 if no corner of moving object boundary is seen in images
int VRMLOut(const std::string &sFilename)
flush all 3d objects to a vrml file with name sFilename, this is the function most users would call ...
void GetInliers(std::vector< bool > &inl) const
inl[i] indicates if Get(GT)Points()[i] is outlier
double * _SigmaCamCenterY
int WriteData(std::string file) const
write in ascii file format
bool IsEmpty() const
check if ImageData_ points to allocated image buffer or not
bool _IsSeen(HomgPoint2D &p, double border=0.0)
void SetParamsCameraStyle(CameraDrawingStyle cameraStyle)
std::vector< PMatrixBase > _P
int _CreateStatic2DPoints(int &index)
fills *_NumStaticPoints first entries of _GT_Points, _Points, _GT_NormalizedPoints, _NormalizedPoints also resizes the above vectors to [*_NumImages][*_NumStaticPoints+*_NumMovingObjects+*_NumOutliers]
BIAS::Vector< double > * _SizeMovingObjZ
void SetParamsPointDrawingStyle(const PointDrawingStyle pointStyle)
int _CreateMoving3DPoints()
fills _MovingPoints returns -1 if no corner of moving object boundary is seen in images or the center...
void Set(const HOMGPOINT3D_TYPE &x, const HOMGPOINT3D_TYPE &y, const HOMGPOINT3D_TYPE &z)
set elementwise with given 3 euclidean scalar values.
Unified output of 3D entities via OpenGL or VRML.
void GetK(KMatrix &K) const
MatchDataBase(Param ¶)
double * _SigmaCamCenterX
bool * AddParamBool(const std::string &name, const std::string &help, bool deflt=false, char cmdshort=0, int Group=GRP_NOSHOW)
BIAS::Vector< double > * _MovingObjRotVecY
double * AddParamDouble(const std::string &name, const std::string &help, double deflt=0.0, double min=-DBL_MAX, double max=DBL_MAX, char cmdshort=0, int Group=GRP_NOSHOW)
GenSynthMatches & operator=(const MatchDataBase &mdb)
static int CircleCenterFilled(Image< StorageType > &im, unsigned int CenterX, unsigned int CenterY, unsigned int Radius, const StorageType Value[])
draws a filled circle using Value
void DrawTrue(Image< unsigned char > &im, int minindex=0, int maxindex=1) const
std::vector< BIAS::RMatrixBase > _GT_R
Slim class bundeling pose parametrization and associated covariance matrix.
unsigned int Size() const
length of the vector
int CreateGTVRML(const std::string &file_name) const
std::vector< std::vector< HomgPoint2D > > _GT_Points
double GetUniformDistributed(const double min, const double max)
on succesive calls return uniform distributed random variable between min and max ...
bool _CheckParams() const
void Get3DPoints(std::vector< HomgPoint3D > &p) const
p = _StaticPoints;
BIAS::Vector< double > * _MovingObjRotVecX
int _CreateStaticGT2DPoints(int &index)
fills *_NumStaticPoints first entries of _GT_Points and _GT_NormalizedPoints also resizes the above v...
std::vector< BIAS::HomgPoint3D > _StaticPoints
class for generating synthetic matches
int Read(const std::string &fname)
binary read
double * _MaxDispOutliers
bool * GetParamBool(const std::string &name) const
void GetPointSourceBorders(int &max_inlier, int &max_outlier, int &max_mov_obj) const
when getting point list for an image, inidices 0 - max_inlier-1 are true matches generated by static ...
double * _SigmaImagePoints
std::vector< RMatrixBase > _R
void _Draw(Image< unsigned char > &im, int minindex, int maxindex, const std::vector< std::vector< HomgPoint2D > > *p) const
helper function used for visualizing the matches
bool DebugLevelIsSet(const long int lv) const
int CreateMoving3DPoint(int index)
Replace the moving 3D point at index by a newly generated one.
class Vector4 contains a Vector of dim.
Vector< T > & newsize(Subscript N)
int CreateStatic3DPoint(int index)
Replace the static 3D point at index by a newly generated one.
bool IsUsedGroupID(const int group_id)
returns if the group id is used
double * _PrinciplePointY
int Write(const std::string &fname) const
binary write
virtual ~GenSynthMatches()
void CrossProduct(const Vector3< T > &argvec, Vector3< T > &destvec) const
cross product of two vectors destvec = this x argvec
virtual void _AddParameter(Param ¶)
fills all protected pointer values
static int Line(Image< StorageType > &im, const unsigned int start[2], const unsigned int end[2], const StorageType value[])
lines
void Mult(const Vector3< T > &argvec, Vector3< T > &destvec) const
matrix - vector multiplicate this matrix with Vector3, storing the result in destvec calculates: dest...
bool CheckParam(const std::string &name)
Check if parameter has already been added.
int ReadData(std::string file)
read in ascii file format
BIAS::Vector< double > * GetParamVecDbl(const std::string &name) const
int CreateMatches(bool points3D_set_by_user=false, bool fixed_errors=false)
returns negative value, if error occurs, 0 on success see _CreateCamMovement for explanation of param...
class for representing matches, used in GenSynthMatches and biasshowsm
BIAS::Vector< double > * _MovingObjMotionVecX
double * _PrinciplePointX
int * GetParamInt(const std::string &name) const
void GetGTR(int imNo, RMatrixBase &R) const
R = _GT_R[imNo];.
int GetFreeGroupID()
returns unused group id
void SetParamsPointSize(const double pointSize)
void FillImageWithConstValue(StorageType Value)
fill grey images
BIAS::Vector< double > * _MotionVecY
BIAS::Vector4< unsigned char > RGBAuc
void GetGTNormalizedPoints(int imNo, std::vector< HomgPoint2D > &p) const
p = _GT_NormalizedPoints[imNo];
BIAS::Vector< double > * _MovingObjMotionVecZ
BIAS::Vector< double > * _RotVecY
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
describes a projective 3D -> 2D mapping in homogenous coordinates
std::ostream & operator<<(std::ostream &os, const Array2D< T > &arg)
BIAS::Vector< double > * AddParamVecDbl(const std::string &name, const std::string &help, const BIAS::Vector< double > &deflt, char cmdshort=0, int Group=GRP_NOSHOW)
Add a parameter that expects a string on command line like "<value0> <value1> <value2> ...
void _AddUniformDistributedOutliers()
fills _GT_Points, _Points, _GT_NormalizedPoints, _NormalizedPoints from entries [*_NumStaticPoints] t...
void Init(unsigned int Width, unsigned int Height, unsigned int channels=1, enum EStorageType storageType=ST_unsignedchar, const bool interleaved=true)
calls Init from ImageBase storageType is ignored, just dummy argument
int _CreateMovingGT2DPoints(int &index)
fills _GT_Points, _GT_NormalizedPoints from entries [*_NumStaticPoints+_*NumOutliers] to [*_NumStatic...
int _CreateMoving2DPoints(int &index)
fills _GT_Points, _Points, _GT_NormalizedPoints, _NormalizedPoints from entries [*_NumStaticPoints+_*...
BIAS::Vector< double > * _MovingObjMotionVecY
BIAS::Vector< double > * _SizeMovingObjY
BIAS::Vector3< T > CoordSphereToEuclidean() const
coordinate transfrom.
This class Param provides generic support for parameters.
int SetGroupName(const int group_id, const std::string &name)
sets the name for a group
Matrix3x3< T > Transpose() const
returns transposed matrix tested 12.06.2002
std::vector< BIAS::EuclideanTransf3D > _MovingTransform
int CreateVRML_(const std::string &file_name, const std::vector< PMatrix > &P, const std::vector< HomgPoint3D > &p) const
void Reset()
calls srand() with a seed generated from system call time, also initializes some other variables ...
double * _SigmaCamCenterZ
void GetGTPoints(int imNo, std::vector< HomgPoint2D > &p) const
p = _GT_Points[imNo];
std::vector< HomgPoint3D > _C
void GetGTP(int imNo, PMatrixBase &P) const
P = _GT_P[imNo];.
std::vector< std::vector< HomgPoint2D > > _Points
int _CreateStatic3DPoints()
fills _StaticPoints returns -1 if no common viewing volume of all cameras is present ...
void _CreateCamMovement(const bool fixed_error)
fills _k, _Ki, _GT_R, _R, GT_C, _C, _GT_P and _P
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
int Create3DPoints(bool fixed_errors=false)
Create static and moving 3D points see _CreateCamMovement for explanation of parameter.
BIAS::Vector< double > * _SizeMovingObjX
int * AddParamInt(const std::string &name, const std::string &help, int deflt=0, int min=std::numeric_limits< int >::min(), int max=std::numeric_limits< int >::max(), char cmdshort=0, int Group=GRP_NOSHOW)
For all adding routines:
void BackprojectWorldCoo(const HomgPoint2D &point, double depth, HomgPoint3D &res)
returns a 3D point res which is located depth away from center of projection in direction given by po...
Implements a 3D rotation matrix.
unsigned int AddPMatrix(BIAS::PMatrix &P, const unsigned int &width, const unsigned int &height, const BIAS::RGBAuc &Color=RGBAuc_WHITE_OPAQUE, const double &dScale=DEF_P_SCALE, const std::string &name="")
decompose P and add to internal data structures.
int Write(const std::string &fname) const
binary write
BIAS::Vector< double > * _RotVecZ
double GetNormalDistributed(const double mean, const double sigma)
on succesive calls return normal distributed random variable with mean and standard deviation sigma ...
std::vector< BIAS::PMatrixBase > _GT_P
describes a projective 3D -> 2D mapping in homogenous coordinates
void _DrawPoints(Image< unsigned char > &im, int index, const std::vector< std::vector< HomgPoint2D > > *p) const
helper function used for visualizing the matches
unsigned int AddPoint(const BIAS::Vector3< double > &v, const BIAS::RGBAuc &Color=RGBAuc_WHITE_OPAQUE)
GenSynthMatches(Param ¶)
void DrawNoisy(Image< unsigned char > &im, int minindex=0, int maxindex=1) const
void _CreateMovingTransforms()
fills _MovinTransforms
void GetGTNormalizedF(std::vector< std::vector< FMatrixBase > > &F) const
A moving 3D point X_i is generated for image i from 3D Point X using the euclidean transform E_i=[R_i...
KMatrix Invert() const
returns analyticaly inverted matrix
BIAS::Vector< double > * _MotionVecX
void Set(const HOMGPOINT2D_TYPE &x, const HOMGPOINT2D_TYPE &y)
set elementwise with given 2 euclidean scalar values.
std::vector< BIAS::HomgPoint3D > _GT_C
void GetGTC(int imNo, HomgPoint3D &C) const
C = _GT_C[imNo];.
Euclidean transformation for 3D points.
double * _OutlLengthChange
int GetGroupID(const std::string &name)
returns group id of parameter with name
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
void Get3DMovingPoints(std::vector< HomgPoint3D > &p, const int frame=0) const
p = _MovingTransform[frame] * _MovingPoints;
BIAS::Vector< double > * _MotionVecZ
std::vector< std::vector< HomgPoint2D > > _GT_NormalizedPoints