robotPoseAnalysis.hpp

/* $Id$ */

#ifndef SIMU_ROBOT_POSE_ANALYSIS_HPP
#define SIMU_ROBOT_POSE_ANALYSIS_HPP

#include "kernel/jafarException.hpp"
#include "kernel/dataLog.hpp"

#include "jmath/jblas.hpp"

#include "slam/slamEkf.hpp"

#include "simu/robot.hpp"

namespace jafar {
  namespace simu {

    class RobotPoseAnalysis  : public jafar::kernel::DataLoggable {

    public:

      enum Type {XY, XYT, XYZ, XYZYPR};

    private:

      Type p_type;

      std::size_t size() const {
  switch(p_type) {
  case XY:
    return 2;
  case XYT:
  case XYZ:
    return 3;
  case XYZYPR:
    return 6;
  }
      }

      slam::SlamEkf const& p_slam;
      simu::Robot const& p_robot;

    public:

      RobotPoseAnalysis(jafar::slam::SlamEkf& slam,
      Robot const& robot,
      Type type = XYZYPR):
  p_type(type), p_slam(slam), p_robot(robot) {}

      double computeNEES() const {
  using namespace ublas;
  try {
    
      jblas::vec xf = p_robot.pose.getX() - p_slam.refPose(p_robot.robotId);
    if (xf(3) > M_PI)
      xf(3) -= 2*M_PI;
    if (xf(3) < -M_PI)
      xf(3) += 2*M_PI;

      jblas::sym_mat xfCov = p_slam.refPoseCov(p_robot.robotId);
      
      jblas::vec x(size());
    jblas::sym_mat xCov(size(), size());

    switch(p_type) {

    case XY:
    case XYZ:
      {
        range r(0,size());
        x.assign(project(xf,r));
        xCov.assign(project(xfCov,r,r));
        break;
      }
    case XYT:
      {
        x(0) = xf(0);
        x(1) = xf(1);
        x(2) = xf(3);
        range r(0,2);
        project(xCov,r,r).assign(project(xfCov,r,r));
        xCov(0,2) = xfCov(0,3);
        xCov(1,2) = xfCov(1,3);
        xCov(2,2) = xfCov(3,3);
        break;
      }
    case XYZYPR:
      {
        x.assign(xf);
        xCov.assign(xfCov);
        break;
      }
        
    }
    
    jblas::mat xCovInv(xCov);
    JFR_TRACE_BEGIN;
    jmath::ublasExtra::lu_inv(xCov, xCovInv);
    JFR_TRACE_END("RobotPoseAnalysis::computeNEES");
    return ublas::inner_prod(x, ublas::prod(xCovInv, x));
  }
  catch(std::exception const& e) {
    JFR_WARNING("cannot compute NEES, returning 0");
    return 0.0;
  }
      }

      jblas::vec computeErrors() const {
  using namespace jblas;
  using namespace ublas;
  vec err(size());

  switch(p_type) {
  case XYT:
    err(0) = p_robot.pose.getX()(0) - p_slam.refPose(p_robot.robotId)(0);
    err(1) = p_robot.pose.getX()(1) - p_slam.refPose(p_robot.robotId)(1);
    err(2) = p_robot.pose.getX()(3) - p_slam.refPose(p_robot.robotId)(3);
    break;
  case XYZYPR:
    err.assign(p_robot.pose.getX() - p_slam.refPose(p_robot.robotId));
    break;
  default:
    JFR_RUN_TIME("not implemented yet");
  }

  for (std::size_t i = 0 ; i < err.size() ; ++i) {
    err(i) = fabs(err(i));
  }
  return err;
      }
      
    protected:

      void writeLogHeader(kernel::DataLogger& log) const {
  log.writeComment("simu: RobotPoseAnalysis");
  log.writeLegend("robot pose NEES");
  switch(p_type) {
  case XYT:
    log.writeLegendTokens("err_x err_y err_yaw");
    break;
  case XYZYPR:
    log.writeLegendTokens("err_x err_y err_z err_yaw err_pitch err_roll");
    break;
  default:
    JFR_RUN_TIME("not implemented yet");
  }
      }

      void writeLogData(kernel::DataLogger& log) const {
  log.writeData(computeNEES());
  log.writeDataVector(computeErrors());
      }

    }; // class PoseNEES

  }
}

#endif // SIMU_ROBOT_POSE_ANALYSIS_HPP