diff --git a/CMakeLists.txt b/CMakeLists.txt
index c142d50160d2026c9bbe074a173b3acea4624601..f70429ebc5d0f0a9dc2196797ccfe68788b934e9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -59,6 +59,7 @@ SET(libs
solver-kine
task-joint-limits
task-inequality
+ feature-projected-line
)
LIST(APPEND LOGGING_WATCHED_TARGETS ${libs})
@@ -85,7 +86,7 @@ SET(headers
task-joint-limits.h
task-inequality.h
solver-kine.h
-
+ feature-projected-line.h
)
# Add subdirectories.
diff --git a/src/dynamic_graph/sot/dyninv/__init__.py b/src/dynamic_graph/sot/dyninv/__init__.py
index cbee4151073ea108463dc2c465602d50a1597261..d866a4cd29aee039e296df81d931bc078b70f3bc 100755
--- a/src/dynamic_graph/sot/dyninv/__init__.py
+++ b/src/dynamic_graph/sot/dyninv/__init__.py
@@ -30,3 +30,6 @@ TaskJointLimits('')
from task_inequality import TaskInequality
TaskInequality('')
+
+from feature_projected_line import FeatureProjectedLine
+FeatureProjectedLine('')
diff --git a/src/feature-projected-line.cpp b/src/feature-projected-line.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..edb3b8bec6e5a8eb33bf2d92b3dec536b6a1e8f6
--- /dev/null
+++ b/src/feature-projected-line.cpp
@@ -0,0 +1,162 @@
+/*
+ * Copyright 2011,
+ * Nicolas Mansard
+ * LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv 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-dyninv 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-dyninv. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* --- SOT --- */
+//#define VP_DEBUG
+//#define VP_DEBUG_MODE 45
+
+#include <sot-dyninv/feature-projected-line.h>
+
+#include <dynamic-graph/command.h>
+#include <dynamic-graph/command-setter.h>
+#include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/command-bind.h>
+
+#include <sot/core/debug.hh>
+#include <sot/core/exception-feature.hh>
+
+#include <sot/core/matrix-homogeneous.hh>
+#include <sot/core/matrix-rotation.hh>
+#include <sot/core/vector-utheta.hh>
+
+#include <sot/core/factory.hh>
+
+namespace dynamicgraph
+{
+ namespace sot
+ {
+ namespace dyninv
+ {
+
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureProjectedLine,"FeatureProjectedLine");
+
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+
+ FeatureProjectedLine::
+ FeatureProjectedLine( const std::string& pointName )
+ : FeatureAbstract( pointName )
+
+ ,CONSTRUCT_SIGNAL_IN(xa,MatrixHomogeneous)
+ ,CONSTRUCT_SIGNAL_IN(xb,MatrixHomogeneous)
+ ,CONSTRUCT_SIGNAL_IN(Ja,ml::Matrix)
+ ,CONSTRUCT_SIGNAL_IN(Jb,ml::Matrix)
+ ,CONSTRUCT_SIGNAL_IN(xc,ml::Vector)
+ {
+ jacobianSOUT.addDependency( xaSIN );
+ jacobianSOUT.addDependency( xbSIN );
+ jacobianSOUT.addDependency( xcSIN );
+ jacobianSOUT.addDependency( JaSIN );
+ jacobianSOUT.addDependency( JbSIN );
+
+ errorSOUT.addDependency( xaSIN );
+ errorSOUT.addDependency( xbSIN );
+ errorSOUT.addDependency( xcSIN );
+
+ activationSOUT.removeDependency( desiredValueSIN );
+
+ signalRegistration( xaSIN << xbSIN << xcSIN << JaSIN << JbSIN );
+ }
+
+
+ /* --------------------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+ unsigned int& FeatureProjectedLine::
+ getDimension( unsigned int & dim, int )
+ {
+ dim = 2;
+ return dim;
+ }
+
+ /** Compute the interaction matrix from a subset of
+ * the possible features.
+ */
+ ml::Matrix& FeatureProjectedLine::
+ computeJacobian( ml::Matrix& J,int time )
+ {
+ sotDEBUGIN(15);
+
+ const MatrixHomogeneous & A = xaSIN(time), & B = xbSIN(time);
+ const ml::Vector & C = xcSIN(time);
+ const double
+ xa=A(0,3),xb=B(0,3),xc=C(0),
+ ya=A(1,3),yb=B(1,3),yc=C(1);
+
+ const ml::Matrix & JA = JaSIN(time), & JB = JbSIN(time);
+
+ const int nq=JA.nbCols();
+ assert((int)JB.nbCols()==nq);
+ J.resize(1,nq);
+ for( int i=0;i<nq;++i )
+ {
+ const double
+ & dxa=JA(0,i),& dxb=JB(0,i),
+ & dya=JA(1,i),& dyb=JB(1,i);
+ J(0,i) = dxa*(yb-yc) - dxb*(ya-yc) - dya*(xb-xc) + dyb*(xa-xc);
+ }
+
+ sotDEBUGOUT(15);
+ return J;
+ }
+
+ /** Compute the error between two visual features from a subset
+ * a the possible features.
+ */
+ ml::Vector&
+ FeatureProjectedLine::computeError( ml::Vector& error,int time )
+ {
+ sotDEBUGIN(15);
+
+ const MatrixHomogeneous & A = xaSIN(time),& B = xbSIN(time);
+ const ml::Vector & C = xcSIN(time);
+ const double
+ xa=A(0,3),xb=B(0,3),xc=C(0),
+ ya=A(1,3),yb=B(1,3),yc=C(1);
+
+ error.resize(1);
+ error(0) = (xb-xa)*(yc-ya)-(yb-ya)*(xc-xa);
+
+ sotDEBUGOUT(15);
+ return error ;
+ }
+
+ void FeatureProjectedLine::
+ display( std::ostream& os ) const
+ {
+ os <<"ProjectedLine <"<<name<<">" ;
+ }
+
+
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
+
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/src/feature-projected-line.h b/src/feature-projected-line.h
new file mode 100644
index 0000000000000000000000000000000000000000..2ddd9a8adf11183eb30ea131e43a3ca4a2cc7da4
--- /dev/null
+++ b/src/feature-projected-line.h
@@ -0,0 +1,98 @@
+/*
+ * Copyright 2011,
+ * Nicolas Mansard
+ * LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv 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-dyninv 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-dyninv. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_dyninv_FeatureProjectedLine_H__
+#define __sot_dyninv_FeatureProjectedLine_H__
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* SOT */
+#include <sot/core/feature-abstract.hh>
+#include <sot/core/exception-task.hh>
+#include <sot/core/matrix-homogeneous.hh>
+#include <sot/core/exception-feature.hh>
+
+#include <sot-dyninv/signal-helper.h>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (feature_projectedLine_EXPORTS)
+# define SOTFEATUREPROJECTEDLINE_EXPORT __declspec(dllexport)
+# else
+# define SOTFEATUREPROJECTEDLINE_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTFEATUREPROJECTEDLINE_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+namespace dynamicgraph {
+ namespace sot {
+ namespace dyninv {
+
+ /*!
+ \class FeatureProjectedLine
+ */
+ class SOTFEATUREPROJECTEDLINE_EXPORT FeatureProjectedLine
+ : public FeatureAbstract
+ {
+ public:
+ static const std::string CLASS_NAME;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+
+ /* --- SIGNALS ------------------------------------------------------------ */
+ public:
+
+ DECLARE_SIGNAL_IN(xa,MatrixHomogeneous);
+ DECLARE_SIGNAL_IN(xb,MatrixHomogeneous);
+ DECLARE_SIGNAL_IN(Ja,ml::Matrix);
+ DECLARE_SIGNAL_IN(Jb,ml::Matrix);
+ DECLARE_SIGNAL_IN(xc,ml::Vector);
+
+ public:
+ FeatureProjectedLine( const std::string& name );
+ virtual ~FeatureProjectedLine( void ) {}
+
+ virtual unsigned int& getDimension( unsigned int & dim, int time );
+
+ virtual ml::Vector& computeError( ml::Vector& res,int time );
+ virtual ml::Matrix& computeJacobian( ml::Matrix& res,int time );
+ virtual ml::Vector& computeActivation( ml::Vector& res,int) {return res;}
+
+ virtual void display( std::ostream& os ) const;
+
+ } ;
+ } /* namespace dyninv */
+ } /* namespace sot */
+} /* namespace dynamicgraph */
+
+#endif // #ifndef __SOT_FEATURE_PROJECTEDLINE_HH__
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */