IVIGIT: added visual point projecter entity.

include/CMakeLists.txt

@@ -58,6 +58,7 @@ SET(NEWHEADERS
   sot/core/feature-1d.hh
   sot/core/feature-point6d-relative.hh
   sot/core/feature-visual-point.hh
+  sot/core/visual-point-projecter.hh
   sot/core/feature-posture.hh
   sot/core/feature-task.hh
   sot/core/feature-line-distance.hh

include/sot/core/visual-point-projecter.hh

+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-core.
+ * sot-core is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-core 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 Lesser General Public License for more details.  You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-core.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_core_VisualPointProjecter_H__
+#define __sot_core_VisualPointProjecter_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+#  if defined (visual_point_projecter_EXPORTS)
+#    define SOTVISUALPOINTPROJECTER_EXPORT __declspec(dllexport)
+#  else
+#    define SOTVISUALPOINTPROJECTER_EXPORT __declspec(dllimport)
+#  endif
+#else
+#  define SOTVISUALPOINTPROJECTER_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* Matrix */
+#include <jrl/mal/boost.hh>
+namespace ml = maal::boost;
+#include <sot/core/matrix-homogeneous.hh>
+
+/* SOT */
+#include <dynamic-graph/signal-helper.h>
+#include <dynamic-graph/entity-helper.h>
+
+namespace dynamicgraph {
+  namespace sot {
+
+    /* --------------------------------------------------------------------- */
+    /* --- CLASS ----------------------------------------------------------- */
+    /* --------------------------------------------------------------------- */
+
+
+    class SOTVISUALPOINTPROJECTER_EXPORT VisualPointProjecter
+      :public ::dynamicgraph::Entity
+      ,public ::dynamicgraph::EntityHelper<VisualPointProjecter>
+      {
+
+      public: /* --- CONSTRUCTOR ---- */
+
+	VisualPointProjecter( const std::string & name );
+
+      public: /* --- ENTITY INHERITANCE --- */
+
+	static const std::string CLASS_NAME;
+	virtual void display( std::ostream& os ) const;
+	virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+
+      public:  /* --- SIGNALS --- */
+
+	DECLARE_SIGNAL_IN(point3D,ml::Vector);
+	DECLARE_SIGNAL_IN(transfo,MatrixHomogeneous);
+
+	DECLARE_SIGNAL_OUT(point3Dgaze,ml::Vector);
+	DECLARE_SIGNAL_OUT(depth,double);
+	DECLARE_SIGNAL_OUT(point2D,ml::Vector);
+
+      }; // class VisualPointProjecter
+
+  } // namespace sot
+} // namespace dynamicgraph
+
+#endif // #ifndef __sot_core_VisualPointProjecter_H__

src/CMakeLists.txt

@@ -74,6 +74,7 @@ SET(plugins
 	feature/feature-task
 	feature/feature-line-distance
 	feature/feature-posture
+	feature/visual-point-projecter
 
 	traces/reader
 

src/feature/visual-point-projecter.cpp

+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-core.
+ * sot-core is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-core 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 Lesser General Public License for more details.  You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-core.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <sot/core/visual-point-projecter.hh>
+#include <sot/core/debug.hh>
+#include <dynamic-graph/factory.h>
+
+namespace dynamicgraph
+{
+  namespace sot
+  {
+
+      namespace dg = ::dynamicgraph;
+      using namespace dg;
+
+      /* --- DG FACTORY ------------------------------------------------------- */
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(VisualPointProjecter,"VisualPointProjecter");
+
+      /* --- CONSTRUCTION ----------------------------------------------------- */
+      /* --- CONSTRUCTION ----------------------------------------------------- */
+      /* --- CONSTRUCTION ----------------------------------------------------- */
+    VisualPointProjecter::
+      VisualPointProjecter( const std::string & name )
+	: Entity(name)
+
+	,CONSTRUCT_SIGNAL_IN(point3D,ml::Vector)
+	,CONSTRUCT_SIGNAL_IN(transfo,MatrixHomogeneous)
+
+	,CONSTRUCT_SIGNAL_OUT(point3Dgaze,ml::Vector,point3DSIN<<transfoSIN )
+	,CONSTRUCT_SIGNAL_OUT(depth,double,point3DgazeSOUT )
+	,CONSTRUCT_SIGNAL_OUT(point2D,ml::Vector,point3DgazeSOUT<<depthSOUT )
+      {
+	Entity::signalRegistration( point3DSIN );
+	Entity::signalRegistration( transfoSIN );
+	Entity::signalRegistration( point3DgazeSOUT );
+	Entity::signalRegistration( point2DSOUT );
+	Entity::signalRegistration( depthSOUT );
+      }
+
+
+      /* --- SIGNALS ---------------------------------------------------------- */
+      /* --- SIGNALS ---------------------------------------------------------- */
+      /* --- SIGNALS ---------------------------------------------------------- */
+
+      ml::Vector& VisualPointProjecter::
+      point3DgazeSOUT_function( ml::Vector &p3g, int iter )
+      {
+	const ml::Vector & p3 = point3DSIN(iter);
+	const MatrixHomogeneous & M = transfoSIN(iter);
+	MatrixHomogeneous Mi; M.inverse(Mi);
+	Mi.multiply(p3,p3g);
+	return p3g;
+      }
+
+      ml::Vector& VisualPointProjecter::
+      point2DSOUT_function( ml::Vector &p2, int iter )
+      {
+	sotDEBUGIN(15);
+
+	const ml::Vector & p3 = point3DgazeSOUT(iter);
+	const double &z =depthSOUT(iter);
+	assert(z>0);
+
+	p2.resize(2);
+	p2(0) = p3(0) / z;
+	p2(1) = p3(1) / z; 
+
+	sotDEBUGOUT(15);
+	return p2;
+      }
+
+      double& VisualPointProjecter::
+      depthSOUT_function( double & z, int iter )
+      {
+	const ml::Vector & p3 = point3DgazeSOUT(iter);
+	assert(p3.size() == 3);
+        z=p3(2);
+	return z;
+      }
+
+      /* --- ENTITY ----------------------------------------------------------- */
+      /* --- ENTITY ----------------------------------------------------------- */
+      /* --- ENTITY ----------------------------------------------------------- */
+
+      void VisualPointProjecter::
+      display( std::ostream& os ) const
+      {
+	os << "VisualPointProjecter "<<getName();
+      }
+
+  } // namespace sot
+} // namespace dynamicgraph
+