[Jderobot-admin] jderobot-r999 - trunk/src/stable/libs/geometry/progeo

rocapal en jderobot.org rocapal en jderobot.org
Dom Oct 6 11:33:34 CEST 2013


Author: rocapal
Date: 2013-10-06 11:32:33 +0200 (Sun, 06 Oct 2013)
New Revision: 999

Modified:
   trunk/src/stable/libs/geometry/progeo/Progeo.cpp
   trunk/src/stable/libs/geometry/progeo/Progeo.h
Log:
#61 added getPosition and getFoa methods


Modified: trunk/src/stable/libs/geometry/progeo/Progeo.cpp
===================================================================
--- trunk/src/stable/libs/geometry/progeo/Progeo.cpp	2013-10-05 18:38:52 UTC (rev 998)
+++ trunk/src/stable/libs/geometry/progeo/Progeo.cpp	2013-10-06 09:32:33 UTC (rev 999)
@@ -24,57 +24,60 @@
 #include "Progeo.h"
 namespace Progeo {
 
-  Progeo::Progeo()
-  {
-  }
-  
-  Progeo::Progeo(Eigen::Vector4d posCamera,
-		 Eigen::Matrix3d KMatrix,
-		 Eigen::Matrix4d RTMatrix,
-		 int width, int height)
-  {
+Progeo::Progeo()
+{
+}
 
-    setPosition(posCamera);
-    setKMatrix(KMatrix);
-    setRTMatrix(RTMatrix);
-    setImageSize(width, height);
-    foa(0) = 0.;
-    foa(1) = 0.;
-    foa(2) = 0.;
-    foa(3) = 0.;
-  }
+Progeo::Progeo(Eigen::Vector4d posCamera,
+		Eigen::Matrix3d KMatrix,
+		Eigen::Matrix4d RTMatrix,
+		int width, int height)
+{
 
-  Progeo::Progeo(std::string filename)
-  {
-    xmlDocPtr doc;
-    xmlNodePtr cur, curAux, curAux_child;
-    doc = xmlParseFile(filename.c_str());
-    if (doc == NULL ) {
-      fprintf(stderr,"Document not parsed successfully. \n");
-      return;
-    }
-    
-    cur = xmlDocGetRootElement(doc);
-    if (cur == NULL) {
-        fprintf(stderr,"empty document\n");
-        xmlFreeDoc(doc);
-        return;
-    }
+	setPosition(posCamera);
+	setKMatrix(KMatrix);
+	setRTMatrix(RTMatrix);
+	setImageSize(width, height);
+	foa(0) = 0.;
+	foa(1) = 1.;
+	foa(2) = 0.;
+	foa(3) = 1.;
 
-    if (xmlStrcmp(cur->name, (const xmlChar *) "calibration_camera")) {
-        fprintf(stderr,"document of the wrong type, root node != calibration_camera");
-        xmlFreeDoc(doc);
-        return;
-    }
+	roll = 0.;
 
-    while (cur != NULL) {
-        if ((!xmlStrcmp(cur->name, (const xmlChar *)"calibration_camera"))) {
+}
 
-            xmlChar *key;
-            curAux = cur->xmlChildrenNode;
+Progeo::Progeo(std::string filename)
+{
+	xmlDocPtr doc;
+	xmlNodePtr cur, curAux, curAux_child;
+	doc = xmlParseFile(filename.c_str());
+	if (doc == NULL ) {
+		fprintf(stderr,"Document not parsed successfully. \n");
+		return;
+	}
 
-            while (curAux != NULL) {
-                /*
+	cur = xmlDocGetRootElement(doc);
+	if (cur == NULL) {
+		fprintf(stderr,"empty document\n");
+		xmlFreeDoc(doc);
+		return;
+	}
+
+	if (xmlStrcmp(cur->name, (const xmlChar *) "calibration_camera")) {
+		fprintf(stderr,"document of the wrong type, root node != calibration_camera");
+		xmlFreeDoc(doc);
+		return;
+	}
+
+	while (cur != NULL) {
+		if ((!xmlStrcmp(cur->name, (const xmlChar *)"calibration_camera"))) {
+
+			xmlChar *key;
+			curAux = cur->xmlChildrenNode;
+
+			while (curAux != NULL) {
+				/*
                 if ((!xmlStrcmp(curAux->name, (const xmlChar *)"name"))) {
                     key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
 
@@ -85,303 +88,292 @@
                     this->name = a;
                     xmlFree(key);
                 }else */if ((!xmlStrcmp(curAux->name, (const xmlChar *)"roll"))) {
-                    key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
-                    this->roll = atof((char*)(char*)key);
-                    xmlFree(key);
+                	key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+                	this->roll = atof((char*)(char*)key);
+                	xmlFree(key);
                 } else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"fdistx"))) {
-                    key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
-                    this->fdistx = atof((char*)key);
-                    xmlFree(key);
+                	key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+                	this->fdistx = atof((char*)key);
+                	xmlFree(key);
                 } else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"fdisty"))) {
-                    key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
-                    this->fdisty = atof((char*)key);
-                    xmlFree(key);
+                	key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+                	this->fdisty = atof((char*)key);
+                	xmlFree(key);
                 } else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"u0"))) {
-                    key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
-                    this->u0 = atof((char*)key);
-                    xmlFree(key);
+                	key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+                	this->u0 = atof((char*)key);
+                	xmlFree(key);
                 } else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"v0"))) {
-                    key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
-                    this->v0 = atof((char*)key);
-                    xmlFree(key);
+                	key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+                	this->v0 = atof((char*)key);
+                	xmlFree(key);
                 } else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"skew"))) {
-                    key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
-                    this->skew = atof((char*)key);
-                    xmlFree(key);
+                	key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+                	this->skew = atof((char*)key);
+                	xmlFree(key);
                 } else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"rows"))) {
-                    key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
-                    this->rows = atof((char*)key);
-                    xmlFree(key);
+                	key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+                	this->rows = atof((char*)key);
+                	xmlFree(key);
                 } else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"columns"))) {
-                    key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
-                    this->columns = atof((char*)key);
-                    xmlFree(key);
+                	key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+                	this->columns = atof((char*)key);
+                	xmlFree(key);
                 } else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"position"))) {
-                    curAux_child = curAux->xmlChildrenNode;
+                	curAux_child = curAux->xmlChildrenNode;
 
-                    while (curAux_child != NULL) {
-                        if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"x"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->position(0) = atof((char*)key);
-                            xmlFree(key);
+                	while (curAux_child != NULL) {
+                		if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"x"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->position(0) = atof((char*)key);
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"y"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->position(1) = atof((char*)key);
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"y"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->position(1) = atof((char*)key);
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"z"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->position(2) = atof((char*)key);
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"z"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->position(2) = atof((char*)key);
+                			xmlFree(key);
 
-                        }
-                        curAux_child = curAux_child->next;
-                    }
+                		}
+                		curAux_child = curAux_child->next;
+                	}
                 } else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"foa"))) {
-                    curAux_child = curAux->xmlChildrenNode;
+                	curAux_child = curAux->xmlChildrenNode;
 
-                    while (curAux_child != NULL) {
-                        if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"x"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->foa(0) = atof((char*)key);
-                            xmlFree(key);
+                	while (curAux_child != NULL) {
+                		if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"x"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->foa(0) = atof((char*)key);
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"y"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->foa(1) = atof((char*)key);
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"y"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->foa(1) = atof((char*)key);
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"z"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->foa(2) = atof((char*)key);
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"z"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->foa(2) = atof((char*)key);
+                			xmlFree(key);
 
-                        }
-                        curAux_child = curAux_child->next;
-                    }
+                		}
+                		curAux_child = curAux_child->next;
+                	}
                 } else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"k_matrix"))) {
-                    curAux_child = curAux->xmlChildrenNode;
+                	curAux_child = curAux->xmlChildrenNode;
 
-                    while (curAux_child != NULL) {
-                        if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k11"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->k11 = atof((char*)key);
-                            K(0,0) = this->k11;
-                            xmlFree(key);
+                	while (curAux_child != NULL) {
+                		if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k11"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->k11 = atof((char*)key);
+                			K(0,0) = this->k11;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k12"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->k12 = atof((char*)key);
-                            K(0,1) = this->k12;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k12"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->k12 = atof((char*)key);
+                			K(0,1) = this->k12;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k13"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->k13 = atof((char*)key);
-                            K(0,2) = this->k13;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k13"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->k13 = atof((char*)key);
+                			K(0,2) = this->k13;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k14"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->k14 = atof((char*)key);
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k14"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->k14 = atof((char*)key);
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k21"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->k21 = atof((char*)key);
-                            K(1,0) = this->k21;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k21"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->k21 = atof((char*)key);
+                			K(1,0) = this->k21;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k22"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->k22 = atof((char*)key);
-                            K(1,1) = this->k22;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k22"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->k22 = atof((char*)key);
+                			K(1,1) = this->k22;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k23"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->k23 = atof((char*)key);
-                            K(1,2) = this->k23;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k23"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->k23 = atof((char*)key);
+                			K(1,2) = this->k23;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k24"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->k24 = atof((char*)key);
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k24"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->k24 = atof((char*)key);
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k31"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->k31 = atof((char*)key);
-                            K(2,0) = this->k31;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k31"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->k31 = atof((char*)key);
+                			K(2,0) = this->k31;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k32"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->k32 = atof((char*)key);
-                            K(2,1) = this->k32;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k32"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->k32 = atof((char*)key);
+                			K(2,1) = this->k32;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k33"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->k33 = atof((char*)key);
-                            K(2,2) = this->k33;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k33"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->k33 = atof((char*)key);
+                			K(2,2) = this->k33;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k34"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->k34 = atof((char*)key);
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k34"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->k34 = atof((char*)key);
+                			xmlFree(key);
 
-                        }
-                        curAux_child = curAux_child->next;
-                    }
+                		}
+                		curAux_child = curAux_child->next;
+                	}
                 } else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"rt_matrix"))) {
-                    curAux_child = curAux->xmlChildrenNode;
+                	curAux_child = curAux->xmlChildrenNode;
 
-                    while (curAux_child != NULL) {
-                        if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt11"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt11 = atof((char*)key);
-                            RT(0,0) = this->rt11;
-                            xmlFree(key);
+                	while (curAux_child != NULL) {
+                		if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt11"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt11 = atof((char*)key);
+                			RT(0,0) = this->rt11;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt12"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt12 = atof((char*)key);
-                            RT(0,1) = this->rt12;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt12"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt12 = atof((char*)key);
+                			RT(0,1) = this->rt12;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt13"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt13 = atof((char*)key);
-                            RT(0,2) = this->rt13;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt13"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt13 = atof((char*)key);
+                			RT(0,2) = this->rt13;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt14"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt14 = atof((char*)key);
-                            RT(0,3) = this->rt14;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt14"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt14 = atof((char*)key);
+                			RT(0,3) = this->rt14;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt21"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt21 = atof((char*)key);
-                            RT(1,0) = this->rt21;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt21"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt21 = atof((char*)key);
+                			RT(1,0) = this->rt21;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt22"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt22 = atof((char*)key);
-                            RT(1,1) = this->rt22;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt22"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt22 = atof((char*)key);
+                			RT(1,1) = this->rt22;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt23"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt23 = atof((char*)key);
-                            RT(1,2) = this->rt23;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt23"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt23 = atof((char*)key);
+                			RT(1,2) = this->rt23;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt24"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt24 = atof((char*)key);
-                            RT(1,3) = this->rt24;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt24"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt24 = atof((char*)key);
+                			RT(1,3) = this->rt24;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt31"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt31 = atof((char*)key);
-                            RT(2,0) = this->rt31;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt31"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt31 = atof((char*)key);
+                			RT(2,0) = this->rt31;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt32"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt32 = atof((char*)key);
-                            RT(2,1) = this->rt32;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt32"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt32 = atof((char*)key);
+                			RT(2,1) = this->rt32;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt33"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt33 = atof((char*)key);
-                            RT(2,2) = this->rt33;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt33"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt33 = atof((char*)key);
+                			RT(2,2) = this->rt33;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt34"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt34 = atof((char*)key);
-                            RT(2,3) = this->rt34;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt34"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt34 = atof((char*)key);
+                			RT(2,3) = this->rt34;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt41"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt41 = atof((char*)key);
-                            RT(3,0) = this->rt41;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt41"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt41 = atof((char*)key);
+                			RT(3,0) = this->rt41;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt42"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt42 = atof((char*)key);
-                            RT(3,1) = this->rt42;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt42"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt42 = atof((char*)key);
+                			RT(3,1) = this->rt42;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt43"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt43 = atof((char*)key);
-                            RT(3,2) = this->rt43;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt43"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt43 = atof((char*)key);
+                			RT(3,2) = this->rt43;
+                			xmlFree(key);
 
-                        } else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt44"))) {
-                            key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
-                            this->rt44 = atof((char*)key);
-                            RT(3,3) = this->rt44;
-                            xmlFree(key);
+                		} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt44"))) {
+                			key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+                			this->rt44 = atof((char*)key);
+                			RT(3,3) = this->rt44;
+                			xmlFree(key);
 
-                        }
-                        curAux_child = curAux_child->next;
-                    }
+                		}
+                		curAux_child = curAux_child->next;
+                	}
                 }
                 curAux = curAux->next;
-            }
-        }
-        cur = cur->next;
-    }
+			}
+		}
+		cur = cur->next;
+	}
 
-    xmlFreeDoc(doc);
-    return;
+	xmlFreeDoc(doc);
+	return;
 }
 
 void Progeo::display_camerainfo() {
-    printf("------------------------------------------------------\n");
-    printf("Camera %s\n\n", this->name.c_str());
-    printf("     Position: (X,Y,Z,H)=(%.1f,%.1f,%.1f,%.1f)\n",   position(0),
-           position(1),
-           position(2),
-           position(3));
-    printf("     Focus of Attention: (x,y,z,h)=(%.1f,%.1f,%.1f,%.1f)\n\n",  foa(0),
-           foa(1),
-           foa(2),
-           foa(3));
-    printf("     Focus DistanceX(vertical): %.1f mm\n",fdistx);
-    printf("     Focus DistanceY(horizontal): %.1f mm\n",fdisty);
+	printf("------------------------------------------------------\n");
+	printf("Camera %s\n\n", this->name.c_str());
+	printf("     Position: (X,Y,Z,H)=(%.1f,%.1f,%.1f,%.1f)\n",   position(0),
+			position(1),
+			position(2),
+			position(3));
+	printf("     Focus of Attention: (x,y,z,h)=(%.1f,%.1f,%.1f,%.1f)\n\n",  foa(0),
+			foa(1),
+			foa(2),
+			foa(3));
+	printf("     Focus DistanceX(vertical): %.1f mm\n",fdistx);
+	printf("     Focus DistanceY(horizontal): %.1f mm\n",fdisty);
 
-    printf("     Skew: %.5f \n",skew);
-    printf("     Optical Center: (x,y)=(%.1f,%.1f)\n\n",u0,v0);
+	printf("     Skew: %.5f \n",skew);
+	printf("     Optical Center: (x,y)=(%.1f,%.1f)\n\n",u0,v0);
 
-    //  std::cout << "\t\tK Matrix:\t" << k11 << " " << k12 << " " << k13 << " " << k14 << std::endl;
-    //  std::cout << "\t\t\t" << k21 << " " << k22 << " " << k23 << " " << k24 << std::endl;
-    //  std::cout << "\t\t\t" << k31 << " " << k32 << " " << k33 << " " << k34 << std::endl;
-    //std::cout << "\t\t\t\t" << k41 << " " << k42 << " " << k43 << " " << k44 << std::endl;
+	std::cout << "K Matrix: \n" << K << std::endl;
+	std::cout <<" R&T Matrix:\n"<<  RT << std::endl;
 
-    std::cout << "K Matrix: \n" << K << std::endl;
-
-    //  std::cout << "\t\tR&T Matrix:\t" << rt11 << " " << rt12 << " " << rt13 << " " << rt14 << std::endl;
-    //  std::cout << "\t\t\t" << rt21 << " " << rt22 << " " << rt23 << " " << rt24 << std::endl;
-    //  std::cout << "\t\t\t" << rt31 << " " << rt32 << " " << rt33 << " " << rt34 << std::endl;
-    //  std::cout << "\t\t\t" << rt41 << " " << rt42 << " " << rt43 << " " << rt44 << std::endl;
-
-    std::cout <<" R&T Matrix:\n"<<  RT << std::endl;
-
-    printf("------------------------------------------------------\n");
+	printf("------------------------------------------------------\n");
 }
 
 int Progeo::project(Eigen::Vector4d in, Eigen::Vector3d &out)
@@ -389,114 +381,114 @@
    returns 1 if "in" 3Dpoint projects into a 2D finite point,
    0 otherwise */
 {
-    Eigen::Vector4d a;
-    a = RT*in;
+	Eigen::Vector4d a;
+	a = RT*in;
 
-    out = K*a.head(3);
+	out = K*a.head(3);
 
-    // optical 2 graphics
-    out(0) = out(0)/out(2);
-    out(1) = out(1)/out(2);
-    out(2) = 1.0;
+	// optical 2 graphics
+	out(0) = out(0)/out(2);
+	out(1) = out(1)/out(2);
+	out(2) = 1.0;
 
-    double aux = out(0);
-    out(0) = out(1);
-    out(1) = this->rows-1-aux;
+	double aux = out(0);
+	out(0) = out(1);
+	out(1) = this->rows-1-aux;
 
-    if (out(2)!=0.) {
-        return 1;
-    } else {
-        return 0;
-    }
+	if (out(2)!=0.) {
+		return 1;
+	} else {
+		return 0;
+	}
 }
 
 void Progeo::backproject(Eigen::Vector3d point, Eigen::Vector4d& pro)
 {
-   //GRAPHIC_TO_OPTICAL
-    int opX = this->rows -1 -point(1);
-    int opY = point(0);
+	//GRAPHIC_TO_OPTICAL
+	int opX = this->rows -1 -point(1);
+	int opY = point(0);
 
-    Eigen::Matrix3d ik;
-    ik = K;
-    ik = ik.inverse().eval();
+	Eigen::Matrix3d ik;
+	ik = K;
+	ik = ik.inverse().eval();
 
-    Eigen::Vector3d Pi(opX*K(0,0)/point(2), opY*K(0,0)/point(2),K(0,0));
+	Eigen::Vector3d Pi(opX*K(0,0)/point(2), opY*K(0,0)/point(2),K(0,0));
 
-    Eigen::Vector3d a;
-    a = ik*Pi;
+	Eigen::Vector3d a;
+	a = ik*Pi;
 
-    Eigen::Vector4d aH;
-    aH(0) = a(0);
-    aH(1) = a(1);
-    aH(2) = a(2);
-    aH(3) = 1.0;
+	Eigen::Vector4d aH;
+	aH(0) = a(0);
+	aH(1) = a(1);
+	aH(2) = a(2);
+	aH(3) = 1.0;
 
-    Eigen::Matrix4d RT2;
-    RT2 = RT;
+	Eigen::Matrix4d RT2;
+	RT2 = RT;
 
-    RT2(0, 3) = .0;
-    RT2(1, 3) = .0;
-    RT2(2, 3) = .0;
-    RT2(3, 3) = 1.0;
+	RT2(0, 3) = .0;
+	RT2(1, 3) = .0;
+	RT2(2, 3) = .0;
+	RT2(3, 3) = 1.0;
 
 
-    Eigen::Vector4d b;
+	Eigen::Vector4d b;
 
-    b = RT2*aH;
+	b = RT2*aH;
 
-    Eigen::Matrix4d Translate;
-    Translate.setIdentity();
-    Translate(0, 3) = position(0);
-    Translate(1, 3) = position(1);
-    Translate(2, 3) = position(2);
+	Eigen::Matrix4d Translate;
+	Translate.setIdentity();
+	Translate(0, 3) = position(0);
+	Translate(1, 3) = position(1);
+	Translate(2, 3) = position(2);
 
-    b = Translate*b;
+	b = Translate*b;
 
-    pro(0) = b(0)/b(3);
-    pro(1) = b(1)/b(3);
-    pro(2) = b(2)/b(3);
-    pro(3) = b(3);
+	pro(0) = b(0)/b(3);
+	pro(1) = b(1)/b(3);
+	pro(2) = b(2)/b(3);
+	pro(3) = b(3);
 }
 
-  void Progeo::setPosition (Eigen::Vector4d pos)
-  {
-    position = pos;
-  }
+void Progeo::setPosition (Eigen::Vector4d pos)
+{
+	position = pos;
+}
 
-  void Progeo::setKMatrix (Eigen::Matrix3d KMatrix)
-  {
-    K = KMatrix;
-    fdistx = K(0,0);
-    fdisty = K(1,1);
-    u0 = K(0,2);
-    v0 = K(1,2);
-  }
+void Progeo::setKMatrix (Eigen::Matrix3d KMatrix)
+{
+	K = KMatrix;
+	fdistx = K(0,0);
+	fdisty = K(1,1);
+	u0 = K(0,2);
+	v0 = K(1,2);
+}
 
-  void Progeo::setRTMatrix (Eigen::Matrix4d RTMatrix)
-  {
-    RT = RTMatrix;
-  }
-  
-  void Progeo::setImageSize (int width, int height)
-  {
-    rows = height;
-    columns = width;
-  }
+void Progeo::setRTMatrix (Eigen::Matrix4d RTMatrix)
+{
+	RT = RTMatrix;
+}
 
-  void Progeo::update_camera_matrix()
-  {}
+void Progeo::setImageSize (int width, int height)
+{
+	rows = height;
+	columns = width;
+}
 
-  void Progeo::pixel2optical (Eigen::Vector3d &point)
-  {
-    double aux = point(0);
-    point(0) = rows-1-point(1);
-    point(1) = aux;
-  }
+void Progeo::update_camera_matrix()
+{}
 
-  void Progeo::optical2pixel(Eigen::Vector3d &point)
-  {
-    double aux = point(1);
-    point(1) = rows-1-point(0);
-    point(0) = aux;
-  }
+void Progeo::pixel2optical (Eigen::Vector3d &point)
+{
+	double aux = point(0);
+	point(0) = rows-1-point(1);
+	point(1) = aux;
 }
+
+void Progeo::optical2pixel(Eigen::Vector3d &point)
+{
+	double aux = point(1);
+	point(1) = rows-1-point(0);
+	point(0) = aux;
+}
+}

Modified: trunk/src/stable/libs/geometry/progeo/Progeo.h
===================================================================
--- trunk/src/stable/libs/geometry/progeo/Progeo.h	2013-10-05 18:38:52 UTC (rev 998)
+++ trunk/src/stable/libs/geometry/progeo/Progeo.h	2013-10-06 09:32:33 UTC (rev 999)
@@ -21,6 +21,8 @@
  *
  */
 
+
+
 #ifndef PROGEOMM_H
 #define PROGEOMM_H
 
@@ -32,7 +34,6 @@
 #include <stdlib.h>
 #include <errno.h>
 
-/*xml*/
 #include <libxml/parser.h>
 #include <libxml/xmlreader.h>
 #include <libxml/xpath.h>
@@ -40,76 +41,88 @@
 
 namespace Progeo {
 
+/**
+	\class Progeo
+	\brief This class implements a pinhole camera model
+	\autor Roberto Calvo <rocapal en gsyc.urjc.es>
+	\date  06/10/2013
+ **/
+
+
+
 class Progeo
 {
 
 public:
 
-    Progeo();
-    Progeo(std::string filename);
+	Progeo();
+	Progeo(std::string filename);
+	Progeo(Eigen::Vector4d posCamera,
+			Eigen::Matrix3d KMatrix,
+			Eigen::Matrix4d RTMatrix,
+			int width, int height);
 
-    Progeo(Eigen::Vector4d posCamera,
-           Eigen::Matrix3d KMatrix,
-           Eigen::Matrix4d RTMatrix,
-           int width, int height);
+	void setPosition (Eigen::Vector4d pos);
+	void setKMatrix (Eigen::Matrix3d KMatrix);
+	void setRTMatrix (Eigen::Matrix4d RTMatrix);
+	void setImageSize (int width, int height);
 
-    void setPosition (Eigen::Vector4d pos);
-    void setKMatrix (Eigen::Matrix3d KMatrix);
-    void setRTMatrix (Eigen::Matrix4d RTMatrix);
-    void setImageSize (int width, int height);
-    
-    void display_camerainfo();
-    void backproject(Eigen::Vector3d point, Eigen::Vector4d& pro);
-    int project(Eigen::Vector4d in, Eigen::Vector3d &out);
-    void update_camera_matrix();
+	Eigen::Vector4d getPosition() {return position; };
+	Eigen::Vector4d getFoa() {return foa; };
 
-    void pixel2optical (Eigen::Vector3d &point);
-    void optical2pixel (Eigen::Vector3d &point);
+	void display_camerainfo();
+	void backproject(Eigen::Vector3d point, Eigen::Vector4d& pro);
+	int project(Eigen::Vector4d in, Eigen::Vector3d &out);
+	void update_camera_matrix();
 
+	void pixel2optical (Eigen::Vector3d &point);
+	void optical2pixel (Eigen::Vector3d &point);
+
+
 private:
 
-    /* camera 3d position in mm */
-    Eigen::Vector4d position;
+	/* camera 3d position in mm */
+	Eigen::Vector4d position;
 
-    /* camera 3d focus of attention in mm */
-    Eigen::Vector4d foa;
+	/* camera 3d focus of attention in mm */
+	Eigen::Vector4d foa;
 
-    /* top right and bottom left points */
-    Eigen::Vector3d tr, bl;
+	/* top right and bottom left points */
+	Eigen::Vector3d tr, bl;
 
-    /* camera roll position angle in rads */
-    float roll;
+	/* camera roll position angle in rads */
+	float roll;
 
-    /* focus x distance in mm*/
-    float fdistx;
-    float fdisty;
+	/* focus x distance in mm*/
+	float fdistx;
+	float fdisty;
 
-    /* pixels */
-    float u0,v0;
+	/* pixels */
+	float u0,v0;
 
-    /*angle between the x and y pixel axes in rads*/
-    float skew
+	/*angle between the x and y pixel axes in rads*/
+	float skew
 
-    /* image height in pixels */;
-    int rows;
+	/* image height in pixels */;
+	int rows;
 
-    /* image width in pixels */
-    int columns;
+	/* image width in pixels */
+	int columns;
 
-    /* camera K matrix */
-    float k11, k12, k13, k14, k21, k22, k23, k24, k31, k32, k33, k34;
-    Eigen::Matrix3d K;
+	/* camera K matrix */
+	float k11, k12, k13, k14, k21, k22, k23, k24, k31, k32, k33, k34;
+	Eigen::Matrix3d K;
 
-    /* camera rotation + translation matrix */
-    float rt11, rt12, rt13, rt14, rt21, rt22, rt23, rt24, rt31, rt32, rt33, rt34, rt41, rt42, rt43, rt44;
-    Eigen::Matrix4d RT;
+	/* camera rotation + translation matrix */
+	float rt11, rt12, rt13, rt14, rt21, rt22, rt23, rt24, rt31, rt32, rt33, rt34, rt41, rt42, rt43, rt44;
+	Eigen::Matrix4d RT;
 
-    /* distortion parameters */
-    float d1,d2,d3,d4,d5,d6;
-    float dx,dy;
+	/* distortion parameters */
+	float d1,d2,d3,d4,d5,d6;
+	float dx,dy;
 
-    /* name */
-    std::string name;
+	/* name */
+	std::string name;
 };
 }
 



More information about the Jderobot-admin mailing list