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

frivas en jderobot.org frivas en jderobot.org
Mar Oct 8 17:41:53 CEST 2013


Author: frivas
Date: 2013-10-08 17:40:52 +0200 (Tue, 08 Oct 2013)
New Revision: 1020

Modified:
   trunk/src/stable/libs/geometry/progeo/Progeo.cpp
   trunk/src/stable/libs/geometry/progeo/Progeo.h
Log:
#61 progeo refactor


Modified: trunk/src/stable/libs/geometry/progeo/Progeo.cpp
===================================================================
--- trunk/src/stable/libs/geometry/progeo/Progeo.cpp	2013-10-08 13:56:37 UTC (rev 1019)
+++ trunk/src/stable/libs/geometry/progeo/Progeo.cpp	2013-10-08 15:40:52 UTC (rev 1020)
@@ -417,7 +417,7 @@
 	ik = K;
 	ik = ik.inverse().eval();
 
-	Eigen::Vector3d Pi(pro(0)*K(0,0)/point(2), pro(1)*K(0,0)/point(2),K(0,0));
+	Eigen::Vector3d Pi(point(0)*K(0,0)/point(2), point(1)*K(0,0)/point(2),K(0,0));
 
 	Eigen::Vector3d a;
 	a = ik*Pi;
@@ -449,10 +449,17 @@
 
 	b = Translate*b;
 
-	pro(0) = b(0)/b(3);
+	/*pro(0) = b(0)/b(3);
 	pro(1) = b(1)/b(3);
 	pro(2) = b(2)/b(3);
+	pro(3) = b(3);*/
+
+	/*OJO*/
+	pro(0) = b(2)/b(3);
+	pro(1) = b(0)/b(3);
+	pro(2) = b(1)/b(3);
 	pro(3) = b(3);
+	
 }
 
 void Progeo::setPosition (Eigen::Vector4d pos)
@@ -480,6 +487,12 @@
 	columns = width;
 }
 
+void Progeo::setFoaRoll(Eigen::Vector4d Foa, float Roll)
+{
+	this->foa=Foa;
+	this->roll=Roll;
+}
+
 void Progeo::updateRTMatrix()
 {
 	Eigen::Matrix4d RC;
@@ -692,4 +705,259 @@
 	xmlCleanupParser();
 }
 
+void Progeo::readFromFile(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;
+	}
+	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 *)"roll"))) {
+					key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+					this->roll = atof((const char*)key);
+					xmlFree(key);
+				}else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"fdistx"))) {
+					key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+					this->fdistx = atof((const char*)key);
+					xmlFree(key);
+				}else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"fdisty"))) {
+					key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+					this->fdisty = atof((const char*)key);
+					xmlFree(key);
+				}else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"u0"))) {
+					key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+					this->u0 = atof((const char*)key);
+					xmlFree(key);
+				}else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"v0"))) {
+					key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+					this->v0 = atof((const char*)key);
+					xmlFree(key);
+				}else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"skew"))) {
+					key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+					this->skew = atof((const char*)key);
+					xmlFree(key);
+				}else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"rows"))) {
+					key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+					this->rows = atof((const char*)key);
+					xmlFree(key);
+				}else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"columns"))) {
+					key = xmlNodeListGetString(doc, curAux->xmlChildrenNode, 1);
+					this->columns = atof((const char*)key);
+					xmlFree(key);
+				}else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"position"))) {
+					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((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"y"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->position(1) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"z"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->position(2) = atof((const char*)key);
+							xmlFree(key);
+
+						}
+						curAux_child = curAux_child->next;
+					}
+				}else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"foa"))) {
+					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((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"y"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->foa(1) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"z"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->foa(2) = atof((const char*)key);
+							xmlFree(key);
+
+						}
+						curAux_child = curAux_child->next;
+					}
+				}else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"k_matrix"))) {
+					curAux_child = curAux->xmlChildrenNode;
+					
+					while (curAux_child != NULL) {
+						if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k11"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->K(0,0) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k12"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->K(0,1) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k13"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->K(0,2) = atof((const char*)key);
+							xmlFree(key);
+
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k21"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->K(1,0) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k22"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->K(1,1) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k23"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->K(1,2) = atof((const char*)key);
+							xmlFree(key);
+
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k31"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->K(2,0) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k32"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->K(2,1) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"k33"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->K(2,2) = atof((const char*)key);
+							xmlFree(key);
+
+
+						}
+						curAux_child = curAux_child->next;
+					}
+				} else if ((!xmlStrcmp(curAux->name, (const xmlChar *)"rt_matrix"))) {
+					curAux_child = curAux->xmlChildrenNode;
+					
+					while (curAux_child != NULL) {
+						if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt11"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(0,0) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt12"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(0,1) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt13"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(0,2) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt14"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(0,3) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt21"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(1,0) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt22"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(1,1) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt23"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(1,2) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt24"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(1,3) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt31"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(2,0) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt32"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(2,1) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt33"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(2,2) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt34"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(2,3) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt41"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(3,0) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt42"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(3,1) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt43"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(3,2) = atof((const char*)key);
+							xmlFree(key);
+
+						} else if ((!xmlStrcmp(curAux_child->name, (const xmlChar *)"rt44"))) {
+							key = xmlNodeListGetString(doc, curAux_child->xmlChildrenNode, 1);
+							this->RT(3,3) = atof((const char*)key);
+							xmlFree(key);
+
+						}
+						curAux_child = curAux_child->next;
+					}
+				}
+				curAux = curAux->next;
+			}
+		}
+		cur = cur->next;
+	}
+
+	xmlFreeDoc(doc);
 }
+
+
+}

Modified: trunk/src/stable/libs/geometry/progeo/Progeo.h
===================================================================
--- trunk/src/stable/libs/geometry/progeo/Progeo.h	2013-10-08 13:56:37 UTC (rev 1019)
+++ trunk/src/stable/libs/geometry/progeo/Progeo.h	2013-10-08 15:40:52 UTC (rev 1020)
@@ -66,6 +66,7 @@
 	void setKMatrix (Eigen::Matrix3d KMatrix);
 	void setRTMatrix (Eigen::Matrix4d RTMatrix);
 	void setImageSize (int width, int height);
+	void setFoaRoll(Eigen::Vector4d Foa, float Roll);
 
 	Eigen::Vector4d getPosition() {return position; };
 	Eigen::Vector4d getFoa() {return foa; };
@@ -81,6 +82,8 @@
 	void optical2pixel (Eigen::Vector3d &point);
 
 	void saveToFile (std::string filename);
+	void readFromFile(std::string filename);
+	//void backproject(cv::Point point, cv::Point3f* cam, cv::Point3f* pro);
 
 
 private:



More information about the Jderobot-admin mailing list