feature-visual-point.cpp

Go to the documentation of this file.

/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */
 
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
/* --- SOT --- */
#include <sot/core/debug.hh>
#include <sot/core/exception-feature.hh>
#include <sot/core/factory.hh>
#include <sot/core/feature-visual-point.hh>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
 
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureVisualPoint, "FeatureVisualPoint");
 
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
FeatureVisualPoint::FeatureVisualPoint(const string &pointName)
    : FeatureAbstract(pointName),
      L(),
      xySIN(NULL, "sotFeatureVisualPoint(" + name + ")::input(vector)::xy"),
      ZSIN(NULL, "sotFeatureVisualPoint(" + name + ")::input(double)::Z"),
      articularJacobianSIN(
          NULL, "sotFeatureVisualPoint(" + name + ")::input(matrix)::Jq") {
  ZSIN = 1.;
 
  jacobianSOUT.addDependency(xySIN);
  jacobianSOUT.addDependency(ZSIN);
  jacobianSOUT.addDependency(articularJacobianSIN);
 
  errorSOUT.addDependency(xySIN);
  errorSOUT.addDependency(ZSIN);
 
  signalRegistration(xySIN << ZSIN << articularJacobianSIN);
}
 
void FeatureVisualPoint::addDependenciesFromReference(void) {
  assert(isReferenceSet());
  errorSOUT.addDependency(getReference()->xySIN);
}
 
void FeatureVisualPoint::removeDependenciesFromReference(void) {
  assert(isReferenceSet());
  errorSOUT.removeDependency(getReference()->xySIN);
}
 
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
size_type &FeatureVisualPoint::getDimension(size_type &dim, sigtime_t time) {
  sotDEBUG(25) << "# In {" << endl;
 
  const Flags &fl = selectionSIN.access(time);
 
  dim = 0;
  if (fl(0)) dim++;
  if (fl(1)) dim++;
 
  sotDEBUG(25) << "# Out }" << endl;
  return dim;
}
 
Matrix &FeatureVisualPoint::computeJacobian(Matrix &J, sigtime_t time) {
  sotDEBUG(15) << "# In {" << endl;
 
  sotDEBUG(15) << "Get selection flags." << endl;
  const Flags &fl = selectionSIN(time);
 
  const size_type dim = dimensionSOUT(time);
  L.resize(dim, 6);
  std::size_t cursorL = 0;
 
  sotDEBUG(5) << std::endl;
 
  const double &Z = ZSIN(time);
  sotDEBUG(5) << xySIN(time) << std::endl;
  const double &x = xySIN(time)(0);
  const double &y = xySIN(time)(1);
 
  if (Z < 0) {
    throw(ExceptionFeature(ExceptionFeature::BAD_INIT,
                           "VisualPoint is behind the camera", " (Z=%.1f).",
                           Z));
  }
 
  if (fabs(Z) < 1e-6) {
    throw(ExceptionFeature(ExceptionFeature::BAD_INIT,
                           "VisualPoint Z coordinates is null", " (Z=%.3f)",
                           Z));
  }
 
  if (fl(0)) {
    L(cursorL, 0) = -1 / Z;
    L(cursorL, 1) = 0;
    L(cursorL, 2) = x / Z;
    L(cursorL, 3) = x * y;
    L(cursorL, 4) = -(1 + x * x);
    L(cursorL, 5) = y;
 
    cursorL++;
  }
 
  if (fl(1)) {
    L(cursorL, 0) = 0;
    L(cursorL, 1) = -1 / Z;
    L(cursorL, 2) = y / Z;
    L(cursorL, 3) = 1 + y * y;
    L(cursorL, 4) = -x * y;
    L(cursorL, 5) = -x;
 
    cursorL++;
  }
  sotDEBUG(15) << "L:" << endl << L << endl;
  sotDEBUG(15) << "Jq:" << endl << articularJacobianSIN(time) << endl;
 
  J = L * articularJacobianSIN(time);
 
  sotDEBUG(15) << "# Out }" << endl;
  return J;
}
 
Vector &FeatureVisualPoint::computeError(Vector &error, sigtime_t time) {
  const Flags &fl = selectionSIN(time);
  sotDEBUGIN(15);
  error.resize(dimensionSOUT(time));
  std::size_t cursorL = 0;
 
  if (!isReferenceSet()) {
    throw(ExceptionFeature(ExceptionFeature::BAD_INIT,
                           "S* is not of adequate type."));
  }
 
  if (fl(0)) {
    error(cursorL++) = xySIN(time)(0) - getReference()->xySIN(time)(0);
  }
  if (fl(1)) {
    error(cursorL++) = xySIN(time)(1) - getReference()->xySIN(time)(1);
  }
 
  sotDEBUGOUT(15);
  return error;
}
 
void FeatureVisualPoint::display(std::ostream &os) const {
  os << "VisualPoint <" << name << ">:";
 
  try {
    const Vector &xy = xySIN.accessCopy();
    const Flags &fl = selectionSIN.accessCopy();
    if (fl(0)) os << " x=" << xy(0);
    if (fl(1)) os << " y=" << xy(1);
  } catch (const ExceptionAbstract &e) {
    os << " XY or select not set.";
  }
 
  try {
    const double &z = ZSIN.accessCopy();
    os << " Z=" << z << " ";
  } catch (const ExceptionAbstract &e) {
    os << " Z not set.";
  }
}
 
/*
 * Local variables:
 * c-basic-offset: 2
 * End:
 */