visual-point-projecter.cpp

Go to the documentation of this file.

/*
 * Copyright 2011, Nicolas Mansard, LAAS-CNRS
 *
 */
 
#include <dynamic-graph/factory.h>
 
#include <sot/core/debug.hh>
#include <sot/core/visual-point-projecter.hh>
 
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, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(transfo, MatrixHomogeneous)
 
      ,
      CONSTRUCT_SIGNAL_OUT(point3Dgaze, dynamicgraph::Vector,
                           m_point3DSIN << m_transfoSIN),
      CONSTRUCT_SIGNAL_OUT(depth, double, m_point3DgazeSOUT),
      CONSTRUCT_SIGNAL_OUT(point2D, dynamicgraph::Vector,
                           m_point3DgazeSOUT << m_depthSOUT) {
  Entity::signalRegistration(m_point3DSIN);
  Entity::signalRegistration(m_transfoSIN);
  Entity::signalRegistration(m_point3DgazeSOUT);
  Entity::signalRegistration(m_point2DSOUT);
  Entity::signalRegistration(m_depthSOUT);
}
 
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
 
dynamicgraph::Vector &VisualPointProjecter::point3DgazeSOUT_function(
    dynamicgraph::Vector &p3g, sigtime_t iter) {
  const dynamicgraph::Vector &p3 = m_point3DSIN(iter);
  const MatrixHomogeneous &M = m_transfoSIN(iter);
  MatrixHomogeneous Mi;
  Mi = M.inverse(Eigen::Affine);
  p3g = Mi.matrix() * p3;
  return p3g;
}
 
dynamicgraph::Vector &VisualPointProjecter::point2DSOUT_function(
    dynamicgraph::Vector &p2, sigtime_t iter) {
  sotDEBUGIN(15);
 
  const dynamicgraph::Vector &p3 = m_point3DgazeSOUT(iter);
  const double &z = m_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, sigtime_t iter) {
  const dynamicgraph::Vector &p3 = m_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