25 #include <Base/Common/BIASpragma.hh>
27 #define _CRT_SECURE_NO_DEPRECATE
30 #include <Base/Image/Image.hh>
32 #define MAXHEADERLINES 1024
37 template <
class StorageType>
41 Orientation_[0] = 0.0;
42 Orientation_[1] = 0.0;
43 Orientation_[2] = 0.0;
45 _C.Set(0.0, 0.0, 0.0);
49 _Point.Set(0.0,0.0,0.0);
53 OrientationValid_ =
false;
63 inertialValid_ =
false;
69 template <
class StorageType>
75 template <
class StorageType>
79 template <
class StorageType>
81 :
Image<StorageType>(img)
86 template <
class StorageType>
89 if (ASCIIValid_) os <<
"ASCII : "<<ASCIIString_<<endl;
90 if (IsTimeValid()) os <<
"Timestamp : "<<TimeSec_<<
":"<<TimeUSec_<<endl;
91 if (IsOrientationValid())
92 os <<
"Orientation : "<<Orientation_[0]<<
" "<<Orientation_[1]<<
" "
93 <<Orientation_[2]<<endl;
94 if (IsPValid()) os <<
"P-Matrix: " << endl << P_;
96 bool isPDecomposable = IsPValid() && (P_.NormFrobenius() > 1E-50);
99 bool hasRMatrix =
false;
101 ROTATION_MATRIX_TYPE RAngle=0;
105 os <<
"R-Matrix: " << endl << _R;
106 _R.GetRotationAxisAngle(RAxis, RAngle);
110 if (isPDecomposable) {
111 os <<
"R-Matrix is empty - decomposed from P: " << endl << P_.GetR();
112 RMatrix(P_.GetR()).GetRotationAxisAngle(RAxis, RAngle);
113 RMatrix(P_.GetR()).GetQuaternion(Q);
118 RAngle = (RAngle / M_PI) * 180.0;
119 os <<
"[Axis, Angle] = [("
123 << RAngle <<
" deg]" << endl;
124 os <<
"Quaternion: ["<<Q[0]<<
", "
130 os <<
"K-Matrix: " << endl << _K;
133 os <<
"K-Matrix is empty - decomposed from P: " << endl << P_.GetK();
136 os <<
"C-Vector: " << _C << endl;
139 os <<
"C-Vector is empty - decomposed from P: " << P_.GetC() << endl;
141 if (IsPointValid()) os <<
"HomgPoint2D: " << _Point << endl;
142 if (IsfValid()) os <<
"Focal length: "<< _f << endl;
143 if (IsProjValid()) os<<
"Projection: "<<Proj_<<endl;
144 os<<
"RefUUID: "<<RefUUID_<<endl;
146 if (IsptuValid()) os << ptu << endl;
147 if (IsinertialValid()) os << inertial << endl;
148 if (IsgpsValid()) os << gps << endl;
153 template <
class StorageType>
159 setlocale(LC_ALL,
"C");
163 BIASDOUT(D_CAMERA_METADATA,
"_MetaData has " << this->_MetaData.size()
167 BIASDOUT(D_CAMERA_METADATA,
"found ASCII String");
169 ASCIIString_ = ad.
sdata;
173 for(
int i=0; i<ad.
length; i++){
174 ASCIIString_+=ad.
data[i];
181 BIASDOUT(D_CAMERA_METADATA,
"found Projection");
186 char *tmp =
new char[ad.
length+1];
194 #ifdef BIAS_HAVE_XML2
195 int res = Proj_.XMLReadFromString(flatproj);
197 BIASERR(
"Failed to parse string with length: "<<ad.
length<<
" string: "
201 if(!Proj_.GetParameters()->PoseValid()) {
202 BIASERR(
"returning invalid pose!");
205 #endif //BIAS_HAVE_XML2
210 BIASDOUT(D_CAMERA_METADATA,
"found PMatrix");
212 sscanf(ad.
sdata.c_str(),
213 "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
214 &P[0][0], &P[0][1], &P[0][2], &P[0][3],
215 &P[1][0], &P[1][1], &P[1][2], &P[1][3],
216 &P[2][0], &P[2][1], &P[2][2], &P[2][3]);
234 BIASWARN(
"ignored zero PMatrix in meta data.");
239 BIASDOUT(D_CAMERA_METADATA,
"found TimeStamp");
242 sscanf(ad.
sdata.c_str(),
"%llu %llu " , &TimeSec_, &TimeUSec_);
246 TimeSec_ = ((
unsigned long long int*)ad.
data)[0];
247 TimeUSec_ = ((
unsigned long long int*)ad.
data)[1];
251 int tSec = 0, tUSec = 0;
252 sscanf(ad.
sdata.c_str(),
"%d %d " , &tSec, &tUSec);
253 TimeSec_ = (
unsigned long long int) tSec;
254 TimeUSec_ = (
unsigned long long int) tUSec;
256 TimeSec_ = (
unsigned long long int) ((
int*)ad.
data)[0];
257 TimeUSec_ = (
unsigned long long int) ((
int*)ad.
data)[1];
263 BIASDOUT(D_CAMERA_METADATA,
"found Orientation");
265 sscanf(ad.
sdata.c_str(),
"%f %f %f" ,&Orientation_[0], &Orientation_[1],
270 OrientationValid_=
true;
273 BIASDOUT(D_CAMERA_METADATA,
"found CVector");
275 sscanf(ad.
sdata.c_str(),
"%lf %lf %lf ", &_C[0], &_C[1], &_C[2]);
282 BIASDOUT(D_CAMERA_METADATA,
"found RMatrix");
284 sscanf(ad.
sdata.c_str(),
"%lf %lf %lf %lf %lf %lf %lf %lf %lf",
285 &_R[0][0], &_R[0][1], &_R[0][2],
286 &_R[1][0], &_R[1][1], &_R[1][2],
287 &_R[2][0], &_R[2][1], &_R[2][2]);
294 BIASDOUT(D_CAMERA_METADATA,
"found KMatrix");
296 sscanf(ad.
sdata.c_str(),
"%lf %lf %lf %lf %lf %lf %lf %lf %lf",
297 &_K[0][0], &_K[0][1], &_K[0][2],
298 &_K[1][0], &_K[1][1], &_K[1][2],
299 &_K[2][0], &_K[2][1], &_K[2][2]);
306 BIASDOUT(D_CAMERA_METADATA,
"found HomgPoint2D");
308 sscanf(ad.
sdata.c_str(),
"[ %lf %lf %lf ]",
309 &_Point[0], &_Point[1], &_Point[2]);
316 BIASDOUT(D_CAMERA_METADATA,
"found Focal Length");
318 sscanf(ad.
sdata.c_str(),
"%lf", &_f);
326 RefUUID_.SetFromString(ad.
sdata);
329 char * tmpstring =
new char[ad.
length+1];
332 RefUUID_.SetFromString(tmpstring);
335 BIASDOUT(D_CAMERA_METADATA,
"found Reference UUID: "<<RefUUID_);
340 BIASDOUT(D_CAMERA_METADATA,
"found PTU_Data");
347 ptu.ReadBinary(ad.
data);
349 BIASDOUT(D_CAMERA_METADATA,
"cam.ptu: "<<ptu<<endl);
354 BIASDOUT(D_CAMERA_METADATA,
"found Inertial_Sensor");
361 inertial.ReadBinary(ad.
data);
363 BIASDOUT(D_CAMERA_METADATA,
"cam.inertiual: "<<inertial<<endl);
367 BIASDOUT(D_CAMERA_METADATA,
"found GPS Data");
373 gps.ReadBinary(ad.
data);
375 BIASDOUT(D_CAMERA_METADATA,
"cam.gps: "<<gps<<endl);
393 template <
class StorageType>
396 this->operator=(src);
401 template <
class StorageType>
445 template <
class StorageType>
448 setlocale(LC_ALL,
"C");
452 BIASDOUT(D_CAMERA_METADATA,
"UpdateMetaData()");
456 (
char*)ASCIIString_.c_str());
457 this->_MetaData.Add(
"#[ASCII]", ASCIIString_);
458 BIASDOUT(D_CAMERA_METADATA,
"Added ASCII String");
462 (
char*)P_.GetData());
463 sprintf(tmpline,
"%.30g %.30g %.30g %.30g %.30g %.30g %.30g %.30g %.30g %.30g %.30g %.30g",
464 P_[0][0], P_[0][1], P_[0][2], P_[0][3],
465 P_[1][0], P_[1][1], P_[1][2], P_[1][3],
466 P_[2][0], P_[2][1], P_[2][2], P_[2][3]);
467 this->_MetaData.Add(
"#[PMatrix]", tmpline);
468 BIASDOUT(D_CAMERA_METADATA,
"Added PMatrix");
472 unsigned long long int time[2];
476 sprintf(tmpline,
"%llu %llu" , TimeSec_, TimeUSec_);
477 this->_MetaData.Add(
"#[TimeStamp]", tmpline);
478 BIASDOUT(D_CAMERA_METADATA,
"Added Time");
480 if (OrientationValid_) {
482 (
char*)Orientation_);
483 sprintf(tmpline,
"%.30f %.30f %.30f" , Orientation_[0], Orientation_[1],
485 this->_MetaData.Add(
"#[Orientation]", tmpline);
486 BIASDOUT(D_CAMERA_METADATA,
"Added Orientation");
490 (
char*)_C.GetData());
491 sprintf(tmpline,
"%.30g %.30g %.30g ", _C[0], _C[1], _C[2]);
492 this->_MetaData.Add(
"#[CVector]", tmpline);
493 BIASDOUT(D_CAMERA_METADATA,
"Added CVector");
497 (
char*)_R.GetData());
498 sprintf(tmpline,
"%.30g %.30g %.30g %.30g %.30g %.30g %.30g %.30g %.30g",
499 _R[0][0], _R[0][1], _R[0][2],
500 _R[1][0], _R[1][1], _R[1][2],
501 _R[2][0], _R[2][1], _R[2][2]);
502 this->_MetaData.Add(
"#[RMatrix]", tmpline);
503 BIASDOUT(D_CAMERA_METADATA,
"Added RMatrix");
507 (
char*)_K.GetData());
508 sprintf(tmpline,
"%.30g %.30g %.30g %.30g %.30g %.30g %.30g %.30g %.30g",
509 _K[0][0], _K[0][1], _K[0][2],
510 _K[1][0], _K[1][1], _K[1][2],
511 _K[2][0], _K[2][1], _K[2][2]);
512 this->_MetaData.Add(
"#[KMatrix]", tmpline);
513 BIASDOUT(D_CAMERA_METADATA,
"Added KMatrix");
518 sprintf(tmpline,
"%.30g", _f);
519 this->_MetaData.Add(
"#[Focal Length]", tmpline);
520 BIASDOUT(D_CAMERA_METADATA,
"Added Focal Length");
524 #ifdef BIAS_HAVE_XML2
525 Proj_.XMLWriteToString(flatproj);
526 #endif //BIAS_HAVE_XML2
527 for (
unsigned int i=0;i<flatproj.length(); i++) {
528 if (flatproj[i] == 10) flatproj[i] = 32;
529 if (flatproj[i] == 13) flatproj[i] = 32;
532 (
char*)flatproj.c_str());
534 this->_MetaData.Add(
"#[Projection]", flatproj.c_str());
535 BIASDOUT(D_CAMERA_METADATA,
"Added Projection");
539 if (RefUUID_.IsValid()) {
540 string uids=RefUUID_.GetString();
542 this->_MetaData.Add(
"#[Reference UUID]", uids);
543 BIASDOUT(D_CAMERA_METADATA,
"Added Reference UUID: "<<uids);
549 char *tmpdata = NULL;
552 length=ptu.GetBinaryLength();
553 tmpdata=
new char[length+1];
554 BIASCDOUT(D_CAMERA_METADATA,
"allocated memory: "<<(
void *)tmpdata<<endl);
555 ptu.WriteBinary(tmpdata);
556 BIASCDOUT(D_CAMERA_METADATA, *(
double*)tmpdata<<endl);
558 BIASDOUT(D_CAMERA_METADATA,
"Added PTU Data");
561 BIASDOUT(D_CAMERA_METADATA,
"ptu "<<os.str());
562 this->_MetaData.Add(
"#[PTU Data]", os.str());
563 BIASDOUT(D_CAMERA_METADATA,
"Added string PTU Data");
564 BIASCDOUT(D_CAMERA_METADATA,
"attempting to free memory: "
565 <<(
void *)tmpdata<<endl);
566 delete[] tmpdata; tmpdata=NULL;
569 length=inertial.GetBinaryLength();
570 tmpdata=
new char[length+1];
571 inertial.WriteBinary(tmpdata);
575 this->_MetaData.Add(
"#[Inertial Sensor]", os.str());
576 BIASDOUT(D_CAMERA_METADATA,
"Added Inertial Sensor");
577 delete[] tmpdata; tmpdata=NULL;
580 length=gps.GetBinaryLength();
581 tmpdata=
new char[length+1];
582 gps.WriteBinary(tmpdata);
586 this->_MetaData.Add(
"#[GPS Data]", os.str());
587 BIASDOUT(D_CAMERA_METADATA,
"Added GPS Data");
588 delete[] tmpdata; tmpdata=NULL;
592 BIASDOUT(D_CAMERA_METADATA,
"Finished adding meta data");
600 template class BIASImage_EXPORT Camera<type>
608 #ifdef BUILD_IMAGE_INT
611 #ifdef BUILD_IMAGE_CHAR
614 #ifdef BUILD_IMAGE_SHORT
617 #ifdef BUILD_IMAGE_USHORT
618 INST(
unsigned short);
620 #ifdef BUILD_IMAGE_UINT
623 #ifdef BUILD_IMAGE_DOUBLE
bool PointValid_
is correspondig field of data valid or unknown ?
bool TimeValid_
is correspondig field of data valid or unknown ?
bool PValid_
is correspondig field of data valid or unknown ?
extends the Image by MetaData support (e.g.
KMatrix _K
camera calibration with internals
bool RValid_
is correspondig field of data valid or unknown ?
camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
virtual void SetImageSize(const unsigned int w, const unsigned int h)
Set image dimensions (in pixels).
bool gpsValid_
is correspondig field of data valid or unknown ?
Vector3< double > _C
camera center in wcs
HomgPoint2D _Point
a point, it is up to you what you put in
int length
number of bytes used by the data block
virtual void SetP(const PMatrix &P)
set from P
double NormL2() const
Return the L2 norm: a^2 + b^2 + c^2 + ...
std::string sdata
the data as given in ascii meta data
Camera< StorageType > & operator=(const Camera< StorageType > &src)
char * data
pointer to block of data
This class hides the underlying projection model, like projection matrix, spherical camera...
double _f
focal length in m
unsigned long long int TimeUSec_
time stamp in microseconds
PMatrix P_
pmatrix, maybe non-metric
bool fValid_
is correspondig field of data valid or unknown ?
The image template class for specific storage types.
T * GetData()
get the pointer to the data array of the matrix (for faster direct memeory access) ...
bool inertialValid_
is correspondig field of data valid or unknown ?
enum TAppData tag
The tag defines the type of data, e.g.
class Vector3 contains a Vector of fixed dim.
Image< StorageType > & operator=(const ImageBase &Source)
copies ImageBase to an desired Image<>, be careful with this, because u can do something like: Image<...
float Orientation_[3]
orientation data from sensor ?
int UpdateMetaData()
copy P_ and co.
void PrintAppData(std::ostream &os)
see whats in the camera, for debug
unsigned long long int TimeSec_
time stamp in seconds
bool KValid_
is correspondig field of data valid or unknown ?
bool ptuValid_
is correspondig field of data valid or unknown ?
RMatrix _R
camera orientation
describes a projective 3D -> 2D mapping in homogenous coordinates
void Init_()
called by constructors to init all fields
bool CValid_
is correspondig field of data valid or unknown ?
This is the base class for images in BIAS.
int ParseMetaData(bool bUse2x64bitTS=true)
After ImageIO::Load() operated on AppData_, this method fills P_, Timestamp, DC_*, ...
bool OrientationValid_
is correspondig field of data valid or unknown ?
this is a chunk of metadata, also see MetaData