31 #include <Base/Debug/LogFacility.hh>
32 #include "GenSynthMatchesRig.hh"
33 #include <Base/ImageUtils/ImageDraw.hh>
34 #include <Geometry/PMatrix.hh>
44 std::vector<BIAS::GenSynthMatches> cams = m.
GetCams();
45 std::vector<BIAS::GenSynthMatches>::iterator it;
47 for (it = cams.begin(), i=0;
50 os <<
"Camera " << i <<
": " << std::endl
108 for (
int slavecam = 0; slavecam < *
NumCameras_-1; slavecam++) {
110 std::vector<std::map<std::string, double> > tmpVec;
112 for (
int timestep = 0; timestep < numImages; timestep++) {
195 std::map<std::string, double> tmpMap;
198 tmpMap[
"GlobalPosX"] = TGlobal[0][3];
199 tmpMap[
"GlobalPosY"] = TGlobal[1][3];
200 tmpMap[
"GlobalPosZ"] = TGlobal[2][3];
204 RGlobal[0][0] = TGlobal[0][0];
205 RGlobal[0][1] = TGlobal[0][1];
206 RGlobal[0][2] = TGlobal[0][2];
208 RGlobal[1][0] = TGlobal[1][0];
209 RGlobal[1][1] = TGlobal[1][1];
210 RGlobal[1][2] = TGlobal[1][2];
212 RGlobal[2][0] = TGlobal[2][0];
213 RGlobal[2][1] = TGlobal[2][1];
214 RGlobal[2][2] = TGlobal[2][2];
221 tmpMap[
"GlobalAnglesX"] = aX;
222 tmpMap[
"GlobalAnglesY"] = aY;
223 tmpMap[
"GlobalAnglesZ"] = aZ;
234 tmpVec.push_back(std::map<std::string, double>(tmpMap));
271 #ifdef BIASASSERT_ISACTIVE
276 if (*
NumCameras_ > GENSYNTHMATCHESRIG_MAX_NR_OF_CAMERAS) {
277 BIASERR(
"Only " << GENSYNTHMATCHESRIG_MAX_NR_OF_CAMERAS <<
" allowed...");
286 BIASASSERT(MasterCenterY_.size() >= numImages);
288 BIASASSERT(MasterCenterZ_.size() >= numImages);
290 BIASASSERT(MasterAnglesX_.size() >= numImages);
292 BIASASSERT(MasterAnglesY_.size() >= numImages);
294 BIASASSERT(MasterAnglesZ_.size() >= numImages);
296 std::map<std::string, double> myMap;
299 for (
int i = 1; i <= GENSYNTHMATCHESRIG_MAX_NR_OF_CAMERAS; i++) {
352 while (res != 0 && counter < 1000) {
356 GSM_RETURN_ERROR_HANDLING_STATIC_POINT) {
357 BIASERR(
"Could not set up master camera.");
362 std::vector<BIAS::HomgPoint3D> staticPoints;
366 std::vector<BIAS::HomgPoint3D> movingPoints;
373 for (
int slaveCamIndex = 1; slaveCamIndex < *
NumCameras_; slaveCamIndex++){
390 if (res == GSM_RETURN_ERROR_HANDLING_STATIC_POINT) {
391 BLD(
"bad index was " << index <<
" static object not seen "
394 <<
" - trying again... (" << counter+1 <<
"/1000)" << endl);
400 }
else if (res == GSM_RETURN_ERROR_HANDLING_MOVING_POINT) {
401 BLD(
"bad index was " << index <<
" moving object not seen "
404 <<
" - trying again... (" << counter+1 <<
"/1000)" << endl);
410 }
else if (res != GSM_RETURN_OK) {
411 BIASERR(
"Unexpected result ( " << res <<
")");
416 if (counter >= 1000) {
417 BIASERR(
"Could not find a solution for all cameras (1000 trials). Maybe no"
418 <<
" common viewing volume.");
436 BIASASSERT(camID > 0);
449 std::vector<std::map<std::string, double> > currentSlave =
452 for (
int i = 0; i < numImages; i++) {
454 centerX[i] = currentSlave[i][
"GlobalPosX"];
455 centerY[i] = currentSlave[i][
"GlobalPosY"];
456 centerZ[i] = currentSlave[i][
"GlobalPosZ"];
457 anglesX[i] = currentSlave[i][
"GlobalAnglesX"];
458 anglesY[i] = currentSlave[i][
"GlobalAnglesY"];
459 anglesZ[i] = currentSlave[i][
"GlobalAnglesZ"];
490 &imgs,
int minindex,
int maxindex)
const
508 int maxindex,
int camIndex)
const
510 if (camIndex < (
int)
CamMatches_.size() && camIndex >= 0) {
511 CamMatches_[camIndex].DrawTrue(img, minindex, maxindex);
513 BIASERR(
"Could not draw image for camera at index " << camIndex <<
". "
514 <<
"only " <<
CamMatches_.size() <<
" cameras available.");
526 int minindex,
int maxindex)
const
544 int maxindex,
int camIndex)
const
546 if (camIndex < (
int)
CamMatches_.size() && camIndex >= 0) {
547 CamMatches_[camIndex].DrawNoisy(img, minindex, maxindex);
549 BIASERR(
"Could not draw image for camera at index " << camIndex <<
". "
550 <<
"only " <<
CamMatches_.size() <<
" cameras available.");
568 #ifdef BIAS_HAVE_XML2
570 stringstream camFile;
572 xmlNodePtr rootNode = xmlObject.
create(
"GenSynthMatchesRigData");
574 for (
unsigned int i=0; i<
CamMatches_.size(); i++) {
575 camFile << file.c_str() <<
".cam" << i;
576 xmlNodePtr camNode = xmlObject.
addChildNode(rootNode,
"Camera");
577 xmlObject.
addAttribute(camNode,
"FileName", camFile.str());
582 return xmlObject.
write(file);
585 BIASERR(
"BIAS_HAVE_XML2 must be defined to write to a file.");
614 #ifdef BIAS_HAVE_XML2
619 xmlNodePtr rootNode = xmlObject.
read(file);
621 BIASERR(
"GenSynthMatchesRig: Error reading file " << file);
624 std::vector<xmlNodePtr> cams;
627 if (cams.size() == 0) {
628 BIASERR(
"Child Node of root node (named \"Camera\") expected but does "
635 for (i = 0; i < (int)cams.size(); i++) {
637 if (! fileNameAttr) {
638 BIASERR(
"GenSynthMatchesRig: All Camera nodes must have an \"FileName\""
659 BIASERR(
"BIAS_HAVE_XML2 must be defined to read from a file.");
670 cout <<
"master Cam:" << endl
675 std::vector<std::map<std::string, double> >::iterator it;
678 cout <<
"slave Cam " << i <<
":" << endl
679 <<
"[ " << (*it)[
"relPosX"] <<
", " << (*it)[
"relPosY"] <<
", "
680 << (*it)[
"relPosZ"] <<
" ]" <<
" " <<
"[ " << (*it)[
"relRotX"]
681 <<
", " << (*it)[
"relRotY"] <<
", " << (*it)[
"relRotZ"] <<
" ]"
709 2, 0, INT_MAX, 0, grp);
716 std::map<std::string, double> myMap;
719 for (
int i = 1; i <= GENSYNTHMATCHESRIG_MAX_NR_OF_CAMERAS; i++) {
723 if ( para.
CheckParam(
"Cam" + oss.str() +
"_RelPosX") ){
729 "relative tranlation (X component)"
730 "of camera " + oss.str(),
731 2.0, 0.0, DBL_MAX, 0, grp));
735 if ( para.
CheckParam(
"Cam" + oss.str() +
"_RelPosY") ){
741 "relative tranlation (Y component)"
742 "of camera " + oss.str(),
743 0.0, 0.0, DBL_MAX, 0, grp));
747 if ( para.
CheckParam(
"Cam" + oss.str() +
"_RelPosZ") ){
753 "relative tranlation (Z component)"
754 "of camera " + oss.str(),
755 0.0, 0.0, DBL_MAX, 0, grp));
759 if ( para.
CheckParam(
"Cam" + oss.str() +
"_RelRotX") ){
765 "relative rotation (rad) (X component)"
766 "of camera " + oss.str(),
767 0.0, 0.0, DBL_MAX, 0, grp));
771 if ( para.
CheckParam(
"Cam" + oss.str() +
"_RelRotY") ){
777 "relative rotation (rad) (Y component)"
778 "of camera " + oss.str(),
779 0.0, 0.0, DBL_MAX, 0, grp));
783 if ( para.
CheckParam(
"Cam" + oss.str() +
"_RelRotZ") ){
789 "relative rotation (rad) (Z component)"
790 "of camera " + oss.str(),
791 0.0, 0.0, DBL_MAX, 0, grp));
void addAttribute(const xmlNodePtr Node, const std::string &AttributeName, bool AttributeValue)
Add an attribute to a node.
double * GetParamDouble(const std::string &name) const
int write(const std::string &Filename, bool AutoAddCompressionSuffix=true) const
Write the whole tree that was constructed in memory to disk.
void SetXYZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order XYZ.
void TransposeIP()
tranpose this matrix "in place" example: 0 1 2 –> 0 3 6 3 4 5 –> 1 4 7 6 7 8 –> 2 5 8 ...
xmlNodePtr read(const std::string &Filename)
Read and parse an XML file from disk, DtD validation is not yet implemented.
int GetMoving3DPoints(std::vector< BIAS::HomgPoint3D > &points)
read the moving 3D points used for computaion
void CreateCamMovement(const bool fixed_error=false)
see _CreateCamMovement for docu
std::vector< VecOMapOStrDbl > SlaveCamsGlobal_
BIAS::Vector< double > MasterAnglesY_
void PrintCamPoses_()
Print the poses of the cameras. For debugging only.
int Read(std::string file)
read in xml/ascii file format
void DrawNoisy(std::vector< Image< unsigned char > > &im, int minindex=0, int maxindex=1) const
Creates images showing the 2D matches for all cameras.
xmlNodePtr create(const std::string &RootNodeName)
Create the base of a new XML-Tree in memory, already with a one and only root node.
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)
int CreateMatches()
Creates 3D points and generates 2D matches for them.
BIAS::Vector< double > MasterCenterZ_
virtual ~GenSynthMatchesRig()
Default destructor...
int SetStatic3DPoints(const std::vector< BIAS::HomgPoint3D > &points)
set the static 3D points used for computation
xmlAttrPtr getFirstAttribute(const xmlNodePtr Node)
Get the first attribute of a given parent, or NULL for no attributes.
class for generating synthetic matches
int Read(const std::string &fname)
binary read
BIAS::Vector< double > MasterCenterX_
motion vectors read from the param file
std::string getAttributeValueString(const xmlAttrPtr Attribute) const
int SetMoving3DPoints(const std::vector< BIAS::HomgPoint3D > &points)
set the moving 3D points used for computation
int CreateMoving3DPoint(int index)
Replace the moving 3D point at index by a newly generated one.
Param * allParams_
Pointer to the parameter object.
BIAS::Vector< double > MasterCenterY_
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
Wrapper class for reading and writing XML files based on the XML library libxml2. ...
bool CheckParam(const std::string &name)
Check if parameter has already been added.
xmlNodePtr addChildNode(const xmlNodePtr ParentNode, const std::string &NewNodeName)
Add a child node to an incoming node with the given name.
void CreateMovingTransforms()
fills _MovingTransform
BIAS::Vector< double > * GetParamVecDbl(const std::string &name) const
int GetStatic3DPoints(std::vector< BIAS::HomgPoint3D > &points)
read the static 3D points used for computation
std::vector< GenSynthMatches > CamMatches_
Vector containing the cameras.
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...
void GetChildren(const xmlNodePtr ParentNode, const std::string &ChildName, std::vector< xmlNodePtr > &childrenWithSameName)
Convenience wrapper: returns a vector of pointors to every (direct) child with the specified name...
int * GetParamInt(const std::string &name) const
int GetFreeGroupID()
returns unused group id
BIAS::Vector< double > MasterAnglesZ_
Generates synthetic matches for cameras in a rig.
std::ostream & operator<<(std::ostream &os, const Array2D< T > &arg)
BIAS::Vector< double > MasterAnglesX_
void AddParameters_(Param ¶)
Add parameters to the param object.
void UpdateParams_(int camID)
Update the param object for the given slave camera.
int Write(std::string file) const
write in xml/ascii file format
void DrawTrue(std::vector< Image< unsigned char > > &im, int minindex=0, int maxindex=1) const
Create images for all cameras showing the matches.
This class Param provides generic support for parameters.
void GetParameters_()
read the parameters from the param object.
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
int GetRotationAnglesXYZ(double &PhiX, double &PhiY, double &PhiZ) const
Get Euler angles for this rotation matrix in order XYZ.
void Setup_()
Setup the global poses of the cameras and read the parameters.
std::vector< std::map< std::string, double > > SlaveCamsRelative_
int Create3DPoints(bool fixed_errors=false)
Create static and moving 3D points see _CreateCamMovement for explanation of parameter.
GenSynthMatchesRig(Param ¶)
Constructor - register parameters.
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:
int * NumCameras_
The number of cameras used. Read from the param object.
std::vector< BIAS::GenSynthMatches > GetCams() const
returns the vector of the used GenSynthMatches object
int GetGroupID(const std::string &name)
returns group id of parameter with name