[Jderobot-admin] jderobot-r1109 - in trunk/src/stable: components/openniServer libs/geometry/progeo

frivas en jderobot.org frivas en jderobot.org
Mar Nov 19 22:48:16 CET 2013


Author: frivas
Date: 2013-11-19 22:48:15 +0100 (Tue, 19 Nov 2013)
New Revision: 1109

Modified:
   trunk/src/stable/components/openniServer/CMakeLists.txt
   trunk/src/stable/libs/geometry/progeo/Progeo.cpp
   trunk/src/stable/libs/geometry/progeo/Progeo.h
Log:
#92 included first version of displayLine


Modified: trunk/src/stable/components/openniServer/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/openniServer/CMakeLists.txt	2013-11-15 15:02:02 UTC (rev 1108)
+++ trunk/src/stable/components/openniServer/CMakeLists.txt	2013-11-19 21:48:15 UTC (rev 1109)
@@ -29,7 +29,6 @@
 		set (concat "/")
 		file (COPY ${OPENNI2_LIB}/OpenNI2 DESTINATION ${CMAKE_CURRENT_SOURCE_DIR})
 	endif()
-	message ("-----------------------------------" ${LIBS_DIR})
 
 
 	if  ( NITE2_LIB )

Modified: trunk/src/stable/libs/geometry/progeo/Progeo.cpp
===================================================================
--- trunk/src/stable/libs/geometry/progeo/Progeo.cpp	2013-11-15 15:02:02 UTC (rev 1108)
+++ trunk/src/stable/libs/geometry/progeo/Progeo.cpp	2013-11-19 21:48:15 UTC (rev 1109)
@@ -27,6 +27,8 @@
 
 namespace Progeo {
 
+#define BIGNUM 1.0e4
+
 Progeo::Progeo()
 {
 }
@@ -397,9 +399,9 @@
 	out(2) = 1.0;
 
 	// optical 2 graphics
-	double aux = out(0);
+	/*double aux = out(0);
 	out(0) = out(1);
-	out(1) = this->rows-1-aux;
+	out(1) = this->rows-1-aux;*/
 
 	if (out(2)!=0.) {
 		return 1;
@@ -466,125 +468,6 @@
 }
 
 
-void Progeo::backprojectCV(Eigen::Vector3d point, Eigen::Vector4d& pro)
-{
-	//GRAPHIC_TO_OPTICAL
-	//int opX = this->rows -1 -point(1);
-	//int opY = point(0);
-
-	/*Eigen::Matrix3d ik;
-	ik = K;
-	ik = ik.inverse().eval();
-*/
-	//Eigen::Vector3d Pi(point(0)/point(2), point(1)/point(2),1);
-
-
-
-
-//float k;
-	std::string line;
-	float p;
-
-	cv::Point3f foa;
-	cv::Mat myK = cv::Mat(cv::Size(3,3),CV_32FC1);
-	cv::Mat myR = cv::Mat(cv::Size(3,3),CV_32FC1);
-		
-	
-
-	myK.at<float>(0,0)=K(0,0);
-	myK.at<float>(0,1)=K(0,1);
-	myK.at<float>(0,2)=K(0,2);
-	myK.at<float>(1,0)=K(1,0);
-	myK.at<float>(1,1)=K(1,1);
-	myK.at<float>(1,2)=K(1,2);
-	myK.at<float>(2,0)=K(2,0);
-	myK.at<float>(2,1)=K(2,1);
-	myK.at<float>(2,2)=K(2,2);
-
-
-	myR.at<float>(0,0)=RT(0,0);
-	myR.at<float>(0,1)=RT(0,1);
-	myR.at<float>(0,2)=RT(0,2);
-	myR.at<float>(1,0)=RT(1,0);
-	myR.at<float>(1,1)=RT(1,1);
-	myR.at<float>(1,2)=RT(1,2);
-	myR.at<float>(2,0)=RT(2,0);
-	myR.at<float>(2,1)=RT(2,1);
-	myR.at<float>(2,2)=RT(2,2);
-
-
-	cv::Mat ik=cv::Mat(myK.size(), myK.type());
-	cv::invert(myK,ik);
-
-	
-	cv::Mat Pi=cv::Mat(cv::Size(1,3),CV_32FC1);
-	Pi.at<float>(0,0)=(float)point(0)/point(2);
-	Pi.at<float>(0,1)=(float)point(1)/point(2);
-	Pi.at<float>(0,2)=1.;
-	
-	cv::Mat a;
-	a=ik*Pi;
-
-	cv::Mat aH=cv::Mat(cv::Size(1,4), CV_32FC1);
-	aH.at<float>(0,0)=a.at<float>(0,0);
-	aH.at<float>(0,1)=a.at<float>(0,1);
-	aH.at<float>(0,2)=a.at<float>(0,2);
-	aH.at<float>(0,3)=1.;
-
-	std::cout << " -------------------- " << std::endl;
-	std::cout << "a= " << a << std::endl;
-	std::cout << aH << std::endl;
-//	std::cout 
-
-	cv::Mat rT;
-	cv::transpose(myR,rT);
-
-	cv::Mat rTH=cv::Mat(cv::Size(4,4),CV_32FC1, cv::Scalar(0));
-	for (int i=0; i<3; i++)
-		for (int j=0; j<3; j++)
-			rTH.at<float>(i,j)=rT.at<float>(i,j);
-	rTH.at<float>(3,3)=1.;
-	
-	
-	cv::Mat b;
-	b=rTH*aH;
-
-	std::cout << "b= " << a << std::endl;
-	
-	cv::Mat iT=cv::Mat(cv::Size(4,4),CV_32FC1, cv::Scalar(0));
-	iT.at<float>(0,0)=1.;
-	iT.at<float>(1,1)=1;
-	iT.at<float>(2,2)=1;
-	iT.at<float>(3,3)=1;
-	iT.at<float>(0,3)=position(0);
-	iT.at<float>(1,3)=position(1);
-	iT.at<float>(2,3)=position(2);
-	cv::Mat Pw;
-	Pw=iT*b;
-	
-
-	pro(0)=Pw.at<float>(0,0);
-	pro(1)=Pw.at<float>(0,1);
-	pro(2)=Pw.at<float>(0,2);
-	pro(3) = 1;
-
-
-
-	/*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(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;
@@ -780,14 +663,14 @@
 	xmlDocSetRootElement(doc, root_node);
 
 	// Camera position
-	xmlNewChild(node_position, NULL, BAD_CAST "x", BAD_CAST double2char(position(0)));
-	xmlNewChild(node_position, NULL, BAD_CAST "y", BAD_CAST double2char(position(1)));
-	xmlNewChild(node_position, NULL, BAD_CAST "z", BAD_CAST double2char(position(2)));
+	xmlNewChild(node_position, NULL, BAD_CAST "x", BAD_CAST double2char((double)(position(0))));
+	xmlNewChild(node_position, NULL, BAD_CAST "y", BAD_CAST double2char((double)(position(1))));
+	xmlNewChild(node_position, NULL, BAD_CAST "z", BAD_CAST double2char((double)(position(2))));
 
 	// Foa position
-	xmlNewChild(node_foa, NULL, BAD_CAST "x", BAD_CAST double2char(foa(0)));
-	xmlNewChild(node_foa, NULL, BAD_CAST "y", BAD_CAST double2char(foa(1)));
-	xmlNewChild(node_foa, NULL, BAD_CAST "z", BAD_CAST double2char(foa(2)));
+	xmlNewChild(node_foa, NULL, BAD_CAST "x", BAD_CAST double2char((double)foa(0)));
+	xmlNewChild(node_foa, NULL, BAD_CAST "y", BAD_CAST double2char((double)foa(1)));
+	xmlNewChild(node_foa, NULL, BAD_CAST "z", BAD_CAST double2char((double)foa(2)));
 
 
 	// Intrisic parameters
@@ -801,34 +684,34 @@
 	xmlNewChild(root_node, NULL, BAD_CAST "columns", BAD_CAST double2char(columns));
 
 	// K Matrix
-	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k11", BAD_CAST double2char(K(0,0)));
-	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k12", BAD_CAST double2char(K(0,1)));
-	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k13", BAD_CAST double2char(K(0,2)));
-	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k21", BAD_CAST double2char(K(1,0)));
-	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k22", BAD_CAST double2char(K(1,1)));
-	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k23", BAD_CAST double2char(K(1,2)));
-	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k31", BAD_CAST double2char(K(2,0)));
-	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k32", BAD_CAST double2char(K(2,1)));
-	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k33", BAD_CAST double2char(K(2,2)));
+	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k11", BAD_CAST double2char((double)K(0,0)));
+	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k12", BAD_CAST double2char((double)K(0,1)));
+	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k13", BAD_CAST double2char((double)K(0,2)));
+	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k21", BAD_CAST double2char((double)K(1,0)));
+	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k22", BAD_CAST double2char((double)K(1,1)));
+	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k23", BAD_CAST double2char((double)K(1,2)));
+	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k31", BAD_CAST double2char((double)K(2,0)));
+	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k32", BAD_CAST double2char((double)K(2,1)));
+	xmlNewChild(node_k_matrix, NULL, BAD_CAST "k33", BAD_CAST double2char((double)K(2,2)));
 
 
 	// RT Matrix
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt11", BAD_CAST double2char(RT(0,0)));
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt12", BAD_CAST double2char(RT(0,1)));
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt13", BAD_CAST double2char(RT(0,2)));
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt14", BAD_CAST double2char(RT(0,3)));
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt21", BAD_CAST double2char(RT(1,0)));
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt22", BAD_CAST double2char(RT(1,1)));
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt23", BAD_CAST double2char(RT(1,2)));
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt24", BAD_CAST double2char(RT(1,3)));
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt31", BAD_CAST double2char(RT(2,0)));
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt32", BAD_CAST double2char(RT(2,1)));
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt33", BAD_CAST double2char(RT(2,2)));
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt34", BAD_CAST double2char(RT(2,3)));
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt41", BAD_CAST double2char(RT(3,0)));
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt42", BAD_CAST double2char(RT(3,1)));
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt43", BAD_CAST double2char(RT(3,2)));
-	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt44", BAD_CAST double2char(RT(3,3)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt11", BAD_CAST double2char((double)RT(0,0)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt12", BAD_CAST double2char((double)RT(0,1)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt13", BAD_CAST double2char((double)RT(0,2)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt14", BAD_CAST double2char((double)RT(0,3)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt21", BAD_CAST double2char((double)RT(1,0)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt22", BAD_CAST double2char((double)RT(1,1)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt23", BAD_CAST double2char((double)RT(1,2)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt24", BAD_CAST double2char((double)RT(1,3)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt31", BAD_CAST double2char((double)RT(2,0)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt32", BAD_CAST double2char((double)RT(2,1)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt33", BAD_CAST double2char((double)RT(2,2)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt34", BAD_CAST double2char((double)RT(2,3)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt41", BAD_CAST double2char((double)RT(3,0)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt42", BAD_CAST double2char((double)RT(3,1)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt43", BAD_CAST double2char((double)RT(3,2)));
+	xmlNewChild(node_rt_matrix, NULL, BAD_CAST "rt44", BAD_CAST double2char((double)RT(3,3)));
 
 	xmlSaveFormatFileEnc(filename.c_str(), doc, "UTF-8", 1);
 	xmlFreeDoc(doc);
@@ -1089,5 +972,186 @@
 	xmlFreeDoc(doc);
 }
 
+int Progeo::displayline(Eigen::Vector3d p1, Eigen::Vector3d p2, Eigen::Vector3d& a, Eigen::Vector3d& b)
+/* it takes care of important features: before/behind the focal plane, inside/outside the image frame */
+{
 
+	Eigen::Vector3d l,l0,l1,l2,l3; /* in fact lines in homogeneous coordinates */
+	Eigen::Vector3d pa,pb,i0,i1,i2,i3,gooda,goodb;
+  int mycase=0;
+  float Xmax,Xmin,Ymax,Ymin;
+  float papb=0.;
+
+  Xmin=0.;
+  Xmax=this->rows-1.;
+  Ymin=0.;
+  Ymax=this->columns-1.;
+  /* Watchout!: they can't reach camera.rows or camera.columns, their are not valid values for the pixels */
+
+  l0(1)=0.; l0(1)=1.; l0(2)=-Ymin;
+  l1(1)=0.; l1(1)=1.; l1(2)=-Ymax;
+  l2(1)=1.; l2(1)=0.; l2(2)=-Xmax;
+  l3(1)=1.; l3(1)=0.; l3(2)=-Xmin;
+
+  if ((p1(2)<0.)&&(p2(2)<0.)){
+    /* both points behind the focal plane: don't display anything */
+    mycase=10;
+
+  }else{
+    if ((p1(2)>0.)&&(p2(2)<0.)){
+        /* p1 before the focal plane, p2 behind */
+		/*Calculates p2 = p1 + -inf(p2-p1)*/
+		p2(0) = p1(1) + (-BIGNUM)*(p2(1)-p1(1));
+		p2(1) = p1(1) + (-BIGNUM)*(p2(1)-p1(1));
+		p2(2)=-p2(2);/* undo the "project" trick to get the right value */
+    }else if ((p1(2)<0.)&&(p2(2)>0.)){
+        /* p2 before the focal plane, p1 behind */
+		/*Calculates p1 = p2 + -inf(p1-p2)*/
+		p1(1) = p2(1) + (-BIGNUM)*(p1(1)-p2(1));
+		p1(1) = p2(1) + (-BIGNUM)*(p1(1)-p2(1));
+		p1(2)=-p1(2);/* undo the "project" trick to get the right value */
+    }
+
+    /* both points before the focal plane */
+    if ((p1(1)>=Xmin) && (p1(1)<Xmax+1) && (p1(1)>=Ymin) && (p1(1)<Ymax+1) &&
+	(p2(1)>=Xmin) && (p2(1)<Xmax+1) && (p2(1)>=Ymin) && (p2(1)<Ymax+1)){
+      /* both inside the image limits */
+
+      gooda(1)=p1(1); gooda(1)=p1(1); gooda(2)=p1(2);
+      goodb(1)=p2(1); goodb(1)=p2(1); goodb(2)=p2(2);
+      mycase=2;
+
+    }else if ((p1(1)>=Xmin) && (p1(1)<Xmax+1) && (p1(1)>=Ymin) && (p1(1)<Ymax+1) &&
+	      ((p2(1)<Xmin) || (p2(1)>=Xmax+1) || (p2(1)<Ymin) || (p2(1)>=Ymax+1))){
+      /* p1 inside, p2 outside */
+      gooda(1)=p1(1); gooda(1)=p1(1); gooda(2)=p1(2);
+      goodb(1)=p1(1); goodb(1)=p1(1); goodb(2)=p1(2);
+      pa(1)=p1(1); pa(1)=p1(1); pa(2)=p1(2);
+      pb(1)=p2(1); pb(1)=p2(1); pb(2)=p2(2);
+      mycase=3;
+
+    }else if ((p2(1)>=Xmin) && (p2(1)<Xmax+1) && (p2(1)>=Ymin) && (p2(1)<Ymax+1) &&
+	      ((p1(1)<Xmin) || (p1(1)>=Xmax+1) || (p1(1)<Ymin) || (p1(1)>=Ymax+1))){
+      /* p2 inside, p1 outside */
+
+      gooda(1)=p2(1); gooda(1)=p2(1); gooda(2)=p2(2);
+      goodb(1)=p2(1); goodb(1)=p2(1); goodb(2)=p2(2);
+      pa(1)=p2(1); pa(1)=p2(1); pa(2)=p2(2);
+      pb(1)=p1(1); pb(1)=p1(1); pb(2)=p1(2);
+      mycase=4;
+
+    }else{
+      /* both outside */
+      pa(1)=p2(1); pa(1)=p2(1); pa(2)=p2(2);
+      pb(1)=p1(1); pb(1)=p1(1); pb(2)=p1(2);
+      mycase=5;
+    }
+    l(1)=pa(1)*pb(2)-pb(1)*pa(2); l(1)=pb(1)*pa(2)-pa(1)*pb(2); l(2)=pa(1)*pb(1)-pb(1)*pa(1);
+    i0(1)=l(1)*l0(2)-l(2)*l0(1); i0(1)=l(2)*l0(1)-l(1)*l0(2); i0(2)=l(1)*l0(1)-l(1)*l0(1);
+    i1(1)=l(1)*l1(2)-l(2)*l1(1); i1(1)=l(2)*l1(1)-l(1)*l1(2); i1(2)=l(1)*l1(1)-l(1)*l1(1);
+    i2(1)=l(1)*l2(2)-l(2)*l2(1); i2(1)=l(2)*l2(1)-l(1)*l2(2); i2(2)=l(1)*l2(1)-l(1)*l2(1);
+    i3(1)=l(1)*l3(2)-l(2)*l3(1); i3(1)=l(2)*l3(1)-l(1)*l3(2); i3(2)=l(1)*l3(1)-l(1)*l3(1);
+    if (i0(2)!=0.) i0(1)=i0(1)/i0(2); i0(1)=i0(1)/i0(2); i0(2)=1.;
+    if (i1(2)!=0.) i1(1)=i1(1)/i1(2); i1(1)=i1(1)/i1(2); i1(2)=1.;
+    if (i2(2)!=0.) i2(1)=i2(1)/i2(2); i2(1)=i2(1)/i2(2); i2(2)=1.;
+    if (i3(2)!=0.) i3(1)=i3(1)/i3(2); i3(1)=i3(1)/i3(2); i3(2)=1.;
+
+    papb=(pb(1)-pa(1))*(pb(1)-pa(1))+(pb(1)-pa(1))*(pb(1)-pa(1));
+
+	float maxdot = -1;
+
+    if (i0(2)!=0.){
+      if ((i0(1)>=Xmin) && (i0(1)<Xmax+1) && (i0(1)>=Ymin) && (i0(1)<Ymax+1)){
+	if ((((pb(1)-pa(1))*(i0(1)-pa(1))+(pb(1)-pa(1))*(i0(1)-pa(1)))>=0.) &&
+	    (((pb(1)-pa(1))*(i0(1)-pa(1))+(pb(1)-pa(1))*(i0(1)-pa(1)))<papb) &&
+		(((pb(1)-pa(1))*(i0(1)-pa(1))+(pb(1)-pa(1))*(i0(1)-pa(1)))>maxdot)){
+	  if ((mycase==3)||(mycase==4)||(mycase==6)){
+	    goodb(1)=i0(1); goodb(1)=i0(1); goodb(2)=i0(2);
+		maxdot = (pb(1)-pa(1))*(i0(1)-pa(1))+(pb(1)-pa(1))*(i0(1)-pa(1));
+
+	  }else if (mycase==5){
+	    gooda(1)=i0(1); gooda(1)=i0(1); gooda(2)=i0(2);
+	    goodb(1)=i0(1); goodb(1)=i0(1); goodb(2)=i0(2);
+	    mycase=6;
+	  }
+	}
+      }
+    }else; /* i0 at infinite, parallel lines */
+
+    if (i1(2)!=0.){
+      if ((i1(1)>=Xmin) && (i1(1)<Xmax+1) && (i1(1)>=Ymin) && (i1(1)<Ymax+1)){
+	if ((((pb(1)-pa(1))*(i1(1)-pa(1))+(pb(1)-pa(1))*(i1(1)-pa(1)))>=0.)&&
+	    (((pb(1)-pa(1))*(i1(1)-pa(1))+(pb(1)-pa(1))*(i1(1)-pa(1)))<papb) &&
+		(((pb(1)-pa(1))*(i1(1)-pa(1))+(pb(1)-pa(1))*(i1(1)-pa(1)))>maxdot)){
+	  if ((mycase==3)||(mycase==4)||(mycase==6)){
+	    goodb(1)=i1(1); goodb(1)=i1(1); goodb(2)=i1(2);
+		maxdot = (pb(1)-pa(1))*(i1(1)-pa(1))+(pb(1)-pa(1))*(i1(1)-pa(1));
+
+	  }else if (mycase==5){
+	    gooda(1)=i1(1); gooda(1)=i1(1); gooda(2)=i1(2);
+	    goodb(1)=i1(1); goodb(1)=i1(1); goodb(2)=i1(2);
+	    mycase=6;
+	  }
+	}
+      }
+    }else; /* i1 at infinite, parallel lines */
+
+    if (i2(2)!=0.){
+      if ((i2(1)>=Xmin) && (i2(1)<Xmax+1) && (i2(1)>=Ymin) && (i2(1)<Ymax+1)){
+	if ((((pb(1)-pa(1))*(i2(1)-pa(1))+(pb(1)-pa(1))*(i2(1)-pa(1)))>=0.)&&
+	    (((pb(1)-pa(1))*(i2(1)-pa(1))+(pb(1)-pa(1))*(i2(1)-pa(1)))<papb) &&
+	    (((pb(1)-pa(1))*(i2(1)-pa(1))+(pb(1)-pa(1))*(i2(1)-pa(1)))>maxdot)){
+	  if ((mycase==3)||(mycase==4)||(mycase==6)){
+	    goodb(1)=i2(1); goodb(1)=i2(1); goodb(2)=i2(2);
+		maxdot = (pb(1)-pa(1))*(i2(1)-pa(1))+(pb(1)-pa(1))*(i2(1)-pa(1));
+
+	  }else if (mycase==5){
+	    gooda(1)=i2(1); gooda(1)=i2(1); gooda(2)=i2(2);
+	    goodb(1)=i2(1); goodb(1)=i2(1); goodb(2)=i2(2);
+	    mycase=6;
+	  }
+	}
+      }
+    }else; /* i2 at infinite, parallel lines */
+
+    if (i3(2)!=0.){
+      if  ((i3(1)>=Xmin) && (i3(1)<Xmax+1) && (i3(1)>=Ymin) && (i3(1)<Ymax+1)){
+	if ((((pb(1)-pa(1))*(i3(1)-pa(1))+(pb(1)-pa(1))*(i3(1)-pa(1)))>=0.) &&
+	    (((pb(1)-pa(1))*(i3(1)-pa(1))+(pb(1)-pa(1))*(i3(1)-pa(1)))<papb) &&
+	    (((pb(1)-pa(1))*(i3(1)-pa(1))+(pb(1)-pa(1))*(i3(1)-pa(1)))>maxdot)){
+	  if ((mycase==3)||(mycase==4)||(mycase==6)){
+	    goodb(1)=i3(1); goodb(1)=i3(1); goodb(2)=i3(2);
+		maxdot = (pb(1)-pa(1))*(i3(1)-pa(1))+(pb(1)-pa(1))*(i3(1)-pa(1));
+
+	  }else if (mycase==5){
+	    gooda(1)=i3(1); gooda(1)=i3(1); gooda(2)=i3(2);
+	    goodb(1)=i3(1); goodb(1)=i3(1); goodb(2)=i3(2);
+	    mycase=6;
+	  }
+	}
+      }
+    }else; /* i3 at infinite, parallel lines */
+
+  }
+
+  /*if (debug==1){
+    printf("p3: x=%.f y=%.f h=%.f\np2: x=%.f, y=%.f h=%.f\n",p1(1),p1(1),p1(2),p2(1),p2(1),p2(2));
+    printf("case: %d\n i0: x=%.1f y=%.1f z=%.f dot=%.2f\n i1: x=%.1f y=%.1f z=%.f dot=%.2f\n i2: x=%.1f y=%.1f z=%.f dot=%.2f\n i3: x=%.1f y=%.1f z=%.f dot=%.2f\n",mycase,
+	   i0(1),i0(1),i0(2),((pb(1)-pa(1))*(i0(1)-pa(1))+(pb(1)-pa(1))*(i0(1)-pa(1))),
+	   i1(1),i1(1),i1(2),((pb(1)-pa(1))*(i1(1)-pa(1))+(pb(1)-pa(1))*(i1(1)-pa(1))),
+	   i2(1),i2(1),i2(2),((pb(1)-pa(1))*(i2(1)-pa(1))+(pb(1)-pa(1))*(i2(1)-pa(1))),
+	   i3(1),i3(1),i3(2),((pb(1)-pa(1))*(i3(1)-pa(1))+(pb(1)-pa(1))*(i3(1)-pa(1))));
+    printf("gooda:  x=%.f y=%.f z=%.f\n",gooda(1),gooda(1),gooda(2));
+    printf("goodb:  x=%.f y=%.f z=%.f\n",goodb(1),goodb(1),goodb(2));
+  }*/
+
+  a(0)=gooda(1); b(0)=goodb(1);
+  a(1)=gooda(1); b(1)=goodb(1);
+  a(2)=gooda(2); b(2)=goodb(2);
+
+  if((mycase!=2)&&(mycase!=3)&&(mycase!=4)&&(mycase!=6)) return 0;
+  else return 1;
 }
+
+
+}

Modified: trunk/src/stable/libs/geometry/progeo/Progeo.h
===================================================================
--- trunk/src/stable/libs/geometry/progeo/Progeo.h	2013-11-15 15:02:02 UTC (rev 1108)
+++ trunk/src/stable/libs/geometry/progeo/Progeo.h	2013-11-19 21:48:15 UTC (rev 1109)
@@ -91,9 +91,10 @@
 
 	void saveToFile (std::string filename, bool updateMatrix=true);
 	void readFromFile(std::string filename);
-	void backprojectCV(Eigen::Vector3d point, Eigen::Vector4d& pro);
+	int displayline(Eigen::Vector3d p1, Eigen::Vector3d p2, Eigen::Vector3d& a, Eigen::Vector3d& b);
 
 
+
 private:
 
 	/* camera 3d position in mm */



More information about the Jderobot-admin mailing list