[Jderobot-admin] jderobot-r960 - in trunk/src/libs/geometry: . math progeo

eperdices en jderobot.org eperdices en jderobot.org
Vie Ago 2 14:54:30 CEST 2013


Author: eperdices
Date: 2013-08-02 14:53:30 +0200 (Fri, 02 Aug 2013)
New Revision: 960

Added:
   trunk/src/libs/geometry/math/Point2D.cpp
   trunk/src/libs/geometry/math/Point2D.h
   trunk/src/libs/geometry/math/Point3D.cpp
   trunk/src/libs/geometry/math/Point3D.h
   trunk/src/libs/geometry/math/geoconst.h
   trunk/src/libs/geometry/math/recta.cpp
   trunk/src/libs/geometry/math/recta.h
Removed:
   trunk/src/libs/geometry/math/recta.cpp
   trunk/src/libs/geometry/math/recta.h
Modified:
   trunk/src/libs/geometry/CMakeLists.txt
   trunk/src/libs/geometry/math/Line2D.cpp
   trunk/src/libs/geometry/math/Line2D.h
   trunk/src/libs/geometry/math/matriz4x4.h
   trunk/src/libs/geometry/progeo/progeo.cpp
   trunk/src/libs/geometry/progeo/progeo2.cpp
Log:
Corregido bug en Progeo y a?\195?\177adidas clases Point2D y Point3D


Modified: trunk/src/libs/geometry/CMakeLists.txt
===================================================================
--- trunk/src/libs/geometry/CMakeLists.txt	2013-08-02 08:41:48 UTC (rev 959)
+++ trunk/src/libs/geometry/CMakeLists.txt	2013-08-02 12:53:30 UTC (rev 960)
@@ -3,20 +3,19 @@
 PROJECT(libgeometry)
  
 INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR} /usr/include/libxml2/)
+
+set(SRC_FILES math/matriz3x3.cpp  math/matriz4x4.cpp  math/plano.cpp  math/utils.cpp  math/vector2d.cpp  math/vector3.cpp  math/vector2H.cpp math/vector3H.cpp math/Line2D.cpp math/Point2D.cpp math/Point3D.cpp collada/colladaparser.cpp collada/color.cpp collada/malla.cpp collada/material.cpp collada/submalla.cpp progeo/progeo2.cpp math/segmento.cpp math/recta.cpp) 
  
-ADD_LIBRARY(geometry_shared SHARED math/matriz3x3.cpp  math/matriz4x4.cpp  math/plano.cpp  math/recta.cpp  
-				   math/segmento.cpp  math/utils.cpp  math/vector2d.cpp  math/vector2H.cpp  math/vector3.cpp  math/vector3H.cpp
-				   math/Line2D.cpp 
-                                   collada/colladaparser.cpp collada/color.cpp collada/malla.cpp collada/material.cpp collada/submalla.cpp progeo/progeo2.cpp)
+ADD_LIBRARY(geometry_shared SHARED ${SRC_FILES})
 
-target_link_libraries(geometry_shared tinyxml GLU )
+include(FindPkgConfig)
+find_package(Boost COMPONENTS system filesystem REQUIRED)
 
+target_link_libraries(geometry_shared tinyxml GLU opencv_core opencv_highgui ${Boost_LIBRARIES})
+
 SET_PROPERTY(TARGET geometry_shared PROPERTY SOVERSION 1.0)
  
-ADD_LIBRARY(geometry_static STATIC math/matriz3x3.cpp  math/matriz4x4.cpp  math/plano.cpp  math/recta.cpp  
-                                   math/segmento.cpp  math/utils.cpp  math/vector2d.cpp  math/vector2H.cpp  math/vector3.cpp  math/vector3H.cpp 
-                                   math/Line2D.cpp
-                                   collada/colladaparser.cpp collada/color.cpp collada/malla.cpp collada/material.cpp collada/submalla.cpp progeo/progeo2.cpp)
+#ADD_LIBRARY(geometry_static STATIC ${SRC_FILES})
 
-target_link_libraries(geometry_shared tinyxml GLU ) 
-SET_TARGET_PROPERTIES(geometry_shared geometry_static PROPERTIES OUTPUT_NAME geometry)
+#target_link_libraries(geometry_shared tinyxml GLU ) 
+#SET_TARGET_PROPERTIES(geometry_shared geometry_static PROPERTIES OUTPUT_NAME geometry)

Modified: trunk/src/libs/geometry/math/Line2D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Line2D.cpp	2013-08-02 08:41:48 UTC (rev 959)
+++ trunk/src/libs/geometry/math/Line2D.cpp	2013-08-02 12:53:30 UTC (rev 960)
@@ -35,6 +35,10 @@
   this->v = this->getLine(p1, p2);
 }
 
+Line2D::Line2D(Point2D p1, Point2D p2) {
+  this->v = this->getLine(p1, p2);
+}
+
 Line2D::Line2D(double va, double vb, double vc) {
   this->v << va, vb, vc;
 }
@@ -72,6 +76,11 @@
   return v;
 }
 
+Eigen::Vector3d
+Line2D::getLine(Point2D p1, Point2D p2) {
+  return this->getLine(p1.getPoint()(0), p1.getPoint()(1), p2.getPoint()(0), p2.getPoint()(1));
+}
+
 Line2D
 Line2D::getNormalLine(double px, double py) {
   Eigen::Vector3d vn;
@@ -85,3 +94,67 @@
   return Line2D(vn);
 }
 
+Line2D
+Line2D::getNormalLine(Point2D p) {
+  return this->getNormalLine(p.getPoint()(0), p.getPoint()(1));
+}
+
+
+
+
+
+/*
+Recta Recta::Perpendicular (float PuntoX, float PuntoY)
+{
+	Recta Recta_Perp;
+  
+    if ( fabs(this->m) < 0.001 ){
+        Recta_Perp.m = infinito; //( 1 / ( Recta.m * ( -1 ) ) );
+
+		Recta_Perp.c =  PuntoX;  
+	}else{
+        Recta_Perp.m = -( 1 / ( this->m ) );
+
+        Recta_Perp.c = ( ( -Recta_Perp.m *  PuntoX  ) + PuntoY );
+	}
+	return Recta_Perp;
+  
+}
+
+Recta Recta::Paralela_Der_Dist (float  distancia, float x)
+{
+  Recta recta_salida;
+
+   if( fabs(this->m) < 1000 ){
+      recta_salida.m = this->m;
+      recta_salida.c = this->c;
+
+      recta_salida.c = recta_salida.c - distancia;
+
+   }else{
+       recta_salida.m = infinito;
+       recta_salida.c = x - distancia;
+   }
+  return recta_salida;
+
+}
+Recta Recta::Paralela_Izq_Dist ( float  distancia, float x)
+{
+  Recta recta_salida;
+
+  if( fabs(this->m) < 1000 ){
+      recta_salida.m = this->m;
+      recta_salida.c = this->c;
+
+      recta_salida.c = recta_salida.c + distancia;
+
+  }else{
+      recta_salida.m = infinito;
+      recta_salida.c = x + distancia;
+  }
+
+
+
+  return recta_salida;
+
+}*/

Modified: trunk/src/libs/geometry/math/Line2D.h
===================================================================
--- trunk/src/libs/geometry/math/Line2D.h	2013-08-02 08:41:48 UTC (rev 959)
+++ trunk/src/libs/geometry/math/Line2D.h	2013-08-02 12:53:30 UTC (rev 960)
@@ -30,13 +30,14 @@
 
 #include <math.h>
 #include <eigen3/Eigen/Dense>
+#include "Point2D.h"
 
-class Line2D{
+class Line2D {
 public:
   Line2D();
   Line2D(double p1x, double p1y, double p2x, double p2y);
   Line2D(Eigen::Vector2d p1, Eigen::Vector2d p2);
-  //Line2D(Point2D p1, Point2D p2);
+  Line2D(Point2D p1, Point2D p2);
   Line2D(double va, double vb, double vc);
   Line2D(Eigen::Vector3d v);
 
@@ -45,9 +46,11 @@
   /*Calculate line from 2 2D points*/
   Eigen::Vector3d getLine(double p1x, double p1y, double p2x, double p2y);
   Eigen::Vector3d getLine(Eigen::Vector2d p1, Eigen::Vector2d p2);
+  Eigen::Vector3d getLine(Point2D p1, Point2D p2);
 
   /*Calculate a 2D normal line from current 2D line and a 2D point*/
   Line2D getNormalLine(double px, double py);
+  Line2D getNormalLine(Point2D p);
   
 private:
 

Added: trunk/src/libs/geometry/math/Point2D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Point2D.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/math/Point2D.cpp	2013-08-02 12:53:30 UTC (rev 960)
@@ -0,0 +1,53 @@
+/*
+ *
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/. 
+ *
+ *  Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
+ *            Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *            Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ *
+ */
+
+#include "Point2D.h"
+
+Point2D::Point2D() {
+  this->point.setZero();
+}
+
+Point2D::Point2D(double x, double y, double h) {
+  this->point << x, y, h;
+}
+
+Point2D::Point2D(Eigen::Vector2d p, double h) {
+  this->point << p(0), p(1), h;
+}
+
+Point2D::Point2D(Eigen::Vector3d p) {
+  this->point = p;
+}
+
+Eigen::Vector3d
+Point2D::getPoint() {
+  return this->point;
+}
+
+double
+Point2D::distanceTo(Point2D p) {
+  return sqrt(G_SQUARE(this->point(0)-p.point(0)) + G_SQUARE(this->point(1)-p.point(1)));
+}
+
+
+

Added: trunk/src/libs/geometry/math/Point2D.h
===================================================================
--- trunk/src/libs/geometry/math/Point2D.h	                        (rev 0)
+++ trunk/src/libs/geometry/math/Point2D.h	2013-08-02 12:53:30 UTC (rev 960)
@@ -0,0 +1,52 @@
+/*
+ *
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/. 
+ *
+ *  Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
+ *            Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *            Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ *
+ *
+ *
+ *  Point2D: Represents a 2D Point in homogeneous coordinates
+ */
+
+#ifndef POINT2D_H
+#define POINT2D_H
+#define EIGEN_DONT_ALIGN_STATICALLY True
+
+#include <math.h>
+#include <eigen3/Eigen/Dense>
+#include "geoconst.h"
+
+class Point2D {
+public:
+  Point2D();
+  Point2D(double x, double y, double h=1.0);
+  Point2D(Eigen::Vector2d p, double h=1.0);
+  Point2D(Eigen::Vector3d p);
+
+  Eigen::Vector3d getPoint();
+
+  double distanceTo(Point2D p);
+  
+private:
+
+  Eigen::Vector3d point;
+    
+};
+
+#endif

Added: trunk/src/libs/geometry/math/Point3D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Point3D.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/math/Point3D.cpp	2013-08-02 12:53:30 UTC (rev 960)
@@ -0,0 +1,53 @@
+/*
+ *
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/. 
+ *
+ *  Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
+ *            Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *            Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ *
+ */
+
+#include "Point3D.h"
+
+Point3D::Point3D() {
+  this->point.setZero();
+}
+
+Point3D::Point3D(double x, double y, double z, double h) {
+  this->point << x, y, z, h;
+}
+
+Point3D::Point3D(Eigen::Vector3d p, double h) {
+  this->point << p(0), p(1), p(2), h;
+}
+
+Point3D::Point3D(Eigen::Vector4d p) {
+  this->point = p;
+}
+
+Eigen::Vector4d
+Point3D::getPoint() {
+  return this->point;
+}
+
+double
+Point3D::distanceTo(Point3D p) {
+  return sqrt(G_SQUARE(this->point(0)-p.point(0)) + G_SQUARE(this->point(1)-p.point(1)) + + G_SQUARE(this->point(2)-p.point(2)));
+}
+
+
+

Added: trunk/src/libs/geometry/math/Point3D.h
===================================================================
--- trunk/src/libs/geometry/math/Point3D.h	                        (rev 0)
+++ trunk/src/libs/geometry/math/Point3D.h	2013-08-02 12:53:30 UTC (rev 960)
@@ -0,0 +1,52 @@
+/*
+ *
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/. 
+ *
+ *  Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
+ *            Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *            Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ *
+ *
+ *
+ *  Point3D: Represents a 3D Point in homogeneous coordinates
+ */
+
+#ifndef POINT3D_H
+#define POINT3D_H
+#define EIGEN_DONT_ALIGN_STATICALLY True
+
+#include <math.h>
+#include <eigen3/Eigen/Dense>
+#include "geoconst.h"
+
+class Point3D {
+public:
+  Point3D();
+  Point3D(double x, double y, double z, double h=1.0);
+  Point3D(Eigen::Vector3d p, double h=1.0);
+  Point3D(Eigen::Vector4d p);
+
+  Eigen::Vector4d getPoint();
+
+  double distanceTo(Point3D p);
+  
+private:
+
+  Eigen::Vector4d point;
+    
+};
+
+#endif

Added: trunk/src/libs/geometry/math/geoconst.h
===================================================================
--- trunk/src/libs/geometry/math/geoconst.h	                        (rev 0)
+++ trunk/src/libs/geometry/math/geoconst.h	2013-08-02 12:53:30 UTC (rev 960)
@@ -0,0 +1,12 @@
+#ifndef G_SQUARE
+#define G_SQUARE(a) ( (a) * (a) )
+#endif
+
+#ifndef G_PI
+#define G_PI 3.1415926535897932384626433832795
+#endif
+
+#ifndef G_INFINITE
+#define G_INFINITE 9.9e9
+#endif
+

Modified: trunk/src/libs/geometry/math/matriz4x4.h
===================================================================
--- trunk/src/libs/geometry/math/matriz4x4.h	2013-08-02 08:41:48 UTC (rev 959)
+++ trunk/src/libs/geometry/math/matriz4x4.h	2013-08-02 12:53:30 UTC (rev 960)
@@ -34,7 +34,7 @@
         public: Matriz4x4 operator*(Matriz3x3 _mat) const;
         public: Matriz4x4 operator*( Matriz4x4 &_mat) const;
         public: Vector3 operator*(Vector3 &_vec) const;
-    private:
+    //private:
         Eigen::Matrix4f matriz;
     };
 

Deleted: trunk/src/libs/geometry/math/recta.cpp
===================================================================
--- trunk/src/libs/geometry/math/recta.cpp	2013-08-02 08:41:48 UTC (rev 959)
+++ trunk/src/libs/geometry/math/recta.cpp	2013-08-02 12:53:30 UTC (rev 960)
@@ -1,73 +0,0 @@
-#include "recta.h"
-
-Recta::Recta()
-{
-	this->m = 0;
-	this->c = 0;
-}
-
-Recta::Recta(float m, float c)
-{
-	this->m = m;
-	this->c = c;
-}
-
-
-Recta::~Recta()
-{
-}
-
-Recta Recta::Perpendicular (float PuntoX, float PuntoY)
-{
-	Recta Recta_Perp;
-  
-    if ( fabs(this->m) < 0.001 ){
-        Recta_Perp.m = infinito /*( 1 / ( Recta.m * ( -1 ) ) );*/ ;
-
-		Recta_Perp.c =  PuntoX;  
-	}else{
-        Recta_Perp.m = -( 1 / ( this->m ) );
-
-        Recta_Perp.c = ( ( -Recta_Perp.m *  PuntoX  ) + PuntoY );
-	}
-	return Recta_Perp;
-  
-}
-
-Recta Recta::Paralela_Der_Dist (float  distancia, float x)
-{
-  Recta recta_salida;
-
-   if( fabs(this->m) < 1000 ){
-      recta_salida.m = this->m;
-      recta_salida.c = this->c;
-
-      recta_salida.c = recta_salida.c - distancia;
-
-   }else{
-       recta_salida.m = infinito;
-       recta_salida.c = x - distancia;
-   }
-  return recta_salida;
-
-}
-Recta Recta::Paralela_Izq_Dist ( float  distancia, float x)
-{
-  Recta recta_salida;
-
-  if( fabs(this->m) < 1000 ){
-      recta_salida.m = this->m;
-      recta_salida.c = this->c;
-
-      recta_salida.c = recta_salida.c + distancia;
-
-  }else{
-      recta_salida.m = infinito;
-      recta_salida.c = x + distancia;
-  }
-
-
-
-  return recta_salida;
-
-}

Added: trunk/src/libs/geometry/math/recta.cpp
===================================================================
--- trunk/src/libs/geometry/math/recta.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/math/recta.cpp	2013-08-02 12:53:30 UTC (rev 960)
@@ -0,0 +1,73 @@
+#include "recta.h"
+
+Recta::Recta()
+{
+	this->m = 0;
+	this->c = 0;
+}
+
+Recta::Recta(float m, float c)
+{
+	this->m = m;
+	this->c = c;
+}
+
+
+Recta::~Recta()
+{
+}
+
+Recta Recta::Perpendicular (float PuntoX, float PuntoY)
+{
+	Recta Recta_Perp;
+  
+    if ( fabs(this->m) < 0.001 ){
+        Recta_Perp.m = infinito /*( 1 / ( Recta.m * ( -1 ) ) );*/ ;
+
+		Recta_Perp.c =  PuntoX;  
+	}else{
+        Recta_Perp.m = -( 1 / ( this->m ) );
+
+        Recta_Perp.c = ( ( -Recta_Perp.m *  PuntoX  ) + PuntoY );
+	}
+	return Recta_Perp;
+  
+}
+
+Recta Recta::Paralela_Der_Dist (float  distancia, float x)
+{
+  Recta recta_salida;
+
+   if( fabs(this->m) < 1000 ){
+      recta_salida.m = this->m;
+      recta_salida.c = this->c;
+
+      recta_salida.c = recta_salida.c - distancia;
+
+   }else{
+       recta_salida.m = infinito;
+       recta_salida.c = x - distancia;
+   }
+  return recta_salida;
+
+}
+Recta Recta::Paralela_Izq_Dist ( float  distancia, float x)
+{
+  Recta recta_salida;
+
+  if( fabs(this->m) < 1000 ){
+      recta_salida.m = this->m;
+      recta_salida.c = this->c;
+
+      recta_salida.c = recta_salida.c + distancia;
+
+  }else{
+      recta_salida.m = infinito;
+      recta_salida.c = x + distancia;
+  }
+
+
+
+  return recta_salida;
+
+}

Deleted: trunk/src/libs/geometry/math/recta.h
===================================================================
--- trunk/src/libs/geometry/math/recta.h	2013-08-02 08:41:48 UTC (rev 959)
+++ trunk/src/libs/geometry/math/recta.h	2013-08-02 12:53:30 UTC (rev 960)
@@ -1,24 +0,0 @@
-#ifndef Recta_H
-#define Recta_H
-
-#include <math.h>	
-
-#define infinito 9.9e9
-
-class Recta{
-	public:
-		Recta();
-		Recta(float m, float c);
-		~Recta();
-		
-		Recta Perpendicular (float PuntoX, float PuntoY);
-		
-        Recta Paralela_Der_Dist (float distancia, float x);
-        Recta Paralela_Izq_Dist (float distancia, float x);
-		
-		float m;
-		float c;
-	private:
-};
-
-#endif

Added: trunk/src/libs/geometry/math/recta.h
===================================================================
--- trunk/src/libs/geometry/math/recta.h	                        (rev 0)
+++ trunk/src/libs/geometry/math/recta.h	2013-08-02 12:53:30 UTC (rev 960)
@@ -0,0 +1,24 @@
+#ifndef Recta_H
+#define Recta_H
+
+#include <math.h>	
+
+#define infinito 9.9e9
+
+class Recta{
+	public:
+		Recta();
+		Recta(float m, float c);
+		~Recta();
+		
+		Recta Perpendicular (float PuntoX, float PuntoY);
+		
+        Recta Paralela_Der_Dist (float distancia, float x);
+        Recta Paralela_Izq_Dist (float distancia, float x);
+		
+		float m;
+		float c;
+	private:
+};
+
+#endif

Modified: trunk/src/libs/geometry/progeo/progeo.cpp
===================================================================
--- trunk/src/libs/geometry/progeo/progeo.cpp	2013-08-02 08:41:48 UTC (rev 959)
+++ trunk/src/libs/geometry/progeo/progeo.cpp	2013-08-02 12:53:30 UTC (rev 960)
@@ -416,7 +416,7 @@
     ik.getMatriz()= K.getCopyMatriz();
     ik.getMatriz() = ik.getCopyMatriz().inverse();
 
-    math::Vector2H Pi(opX*this->k11/point.getH(), opY*this->k11/point.getH(),this->k11);
+    math::Vector2H Pi(opX*K.getMatriz()(0,0)/point.getH(), opY*K.getMatriz()(0,0)/point.getH(),K.getMatriz()(0,0));
 
     math::Vector2H a;
     a.vector = ik.getCopyMatriz()*Pi.vector;

Modified: trunk/src/libs/geometry/progeo/progeo2.cpp
===================================================================
--- trunk/src/libs/geometry/progeo/progeo2.cpp	2013-08-02 08:41:48 UTC (rev 959)
+++ trunk/src/libs/geometry/progeo/progeo2.cpp	2013-08-02 12:53:30 UTC (rev 960)
@@ -391,15 +391,15 @@
     out = K*a.head(3);
 
     // optical 2 graphics
-    out(0) = out(0)/out(3);
-    out(1) = out(1)/out(3);
+    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;
 
-    if (out(3)!=0.) {
+    if (out(2)!=0.) {
         return 1;
     } else {
         return 0;
@@ -408,16 +408,15 @@
 
 void Progeo::backproject(Eigen::Vector3d point, Eigen::Vector4d& pro)
 {
-
-    //GRAPHIC_TO_OPTICAL
+   //GRAPHIC_TO_OPTICAL
     int opX = this->rows -1 -point(1);
     int opY = point(0);
 
     Eigen::Matrix3d ik;
     ik = K;
-    ik = ik.inverse();
+    ik = ik.inverse().eval();
 
-    Eigen::Vector3d Pi(opX*this->k11/point(3), opY*this->k11/point(3),this->k11);
+    Eigen::Vector3d Pi(opX*K(0,0)/point(2), opY*K(0,0)/point(2),K(0,0));
 
     Eigen::Vector3d a;
     a = ik*Pi;
@@ -425,7 +424,7 @@
     Eigen::Vector4d aH;
     aH(0) = a(0);
     aH(1) = a(1);
-    aH(2) = a(3);
+    aH(2) = a(2);
     aH(3) = 1.0;
 
     Eigen::Matrix4d RT2;



More information about the Jderobot-admin mailing list