[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