sensorAbsloc.hpp

Go to the documentation of this file.

#ifndef SENSORABSLOC_HPP_
#define SENSORABSLOC_HPP_

#include "jmath/jblas.hpp"
#include "jmath/ublasExtra.hpp"
#include "jmath/misc.hpp"

#include "rtslam/rtSlam.hpp"
#include "rtslam/quatTools.hpp"
#include "rtslam/sensorAbstract.hpp"
#include "rtslam/innovation.hpp"

namespace jafar {
  namespace rtslam {

    class SensorAbsloc;
    typedef boost::shared_ptr<SensorAbsloc> absloc_ptr_t;
    
    class SensorAbsloc: public SensorProprioAbstract {
      protected:
        jblas::ind_array ia_rs;
        Innovation *innovation;
        Measurement *measurement;
        Expectation *expectation;
        jblas::mat EXP_rs;
        jblas::mat INN_rs;
        jblas::mat EXP_q;
        double mahaDist2;
        double sigMax;
        int inns; 
        int dats; 
        jblas::vec origin; 

      private:
        size_t indexE_Pos, indexE_OriEuler, indexE_Bundleobs, indexE_AbsVel, indexE_NormVel, indexE_Vel; 
        int indexD_Pos, indexD_OriQuat, indexD_OriEuler, indexD_Bundleobs, indexD_AbsVel, indexD_NormVel, indexD_Vel; 
        ublas::range indexR_Pos, indexR_OriQuat, indexR_AbsVel, indexR_AngVel; 
        ublas::range indexJ_Pos, indexJ_OriQuat, indexJ_AbsVel, indexJ_AngVel; 
        hardware::HardwareSensorProprioAbstract::CovType covType;

      public:

        std::vector<RobotAbstract::Quantity> returnQuants(bool needPose, bool needVel, bool needAngVel)
        {
          std::vector<RobotAbstract::Quantity> res;
          if (needPose) { res.push_back(RobotAbstract::qPos); res.push_back(RobotAbstract::qOriQuat); }
          if (needVel) res.push_back(RobotAbstract::qAbsVel);
          if (needAngVel) res.push_back(RobotAbstract::qAngVel);
          return res;
        }



        SensorAbsloc(const robot_ptr_t & robPtr, const filtered_obj_t inFilter = UNFILTERED, double mahaDist = -1.0, double sigMax = 1e9,
          bool needPose = true, bool needVel = false, bool needAngVel = false):
          SensorProprioAbstract(robPtr, inFilter, 0, returnQuants(needPose, needVel, needAngVel)),
          ia_rs(ia_globalPose), innovation(NULL), measurement(NULL), expectation(NULL),
          mahaDist2(mahaDist*fabs(mahaDist)), sigMax(sigMax), inns(0), dats(0)
        {}

        ~SensorAbsloc() { delete innovation; delete measurement; }
        virtual void setHardwareSensor(hardware::hardware_sensorprop_ptr_t hardwareSensorPtr_)
        {
          hardwareSensorPtr = hardwareSensorPtr_;
          // initialize jacobians and innovation sizes
          inns = hardwareSensorPtr->obsSize();
          dats = hardwareSensorPtr->dataSize();
          innovation = new Innovation(inns);
          measurement = new Measurement(inns);
          expectation = new Expectation(inns);
          EXP_rs.resize(inns, ia_rs.size(), false);
          INN_rs.resize(inns, ia_rs.size(), false);
          EXP_q.resize(3, 4, false);

          // initialize type of data the hardware sensor is providing
          size_t indexE = 0; // current index when building expectation

          // pos for GPS or Motion Capture
          indexD_Pos = hardwareSensorPtr->getQuantity(hardware::HardwareSensorProprioAbstract::qPos);
          if (indexD_Pos >= 0) { indexE_Pos = indexE; indexE += hardwareSensorPtr->QuantityObsSizes[hardware::HardwareSensorProprioAbstract::qPos]; }
          // quaternion orientation (actually converting quaternion observations to euler observations)
          indexD_OriQuat = hardwareSensorPtr->getQuantity(hardware::HardwareSensorProprioAbstract::qOriQuat);
          if (indexD_OriQuat >= 0) { indexE_OriEuler = indexE; indexE += hardwareSensorPtr->QuantityObsSizes[hardware::HardwareSensorProprioAbstract::qOriEuler]; }
          // euler orientation
          indexD_OriEuler = hardwareSensorPtr->getQuantity(hardware::HardwareSensorProprioAbstract::qOriEuler);
          if (indexD_OriEuler >= 0) { indexE_OriEuler = indexE; indexE += hardwareSensorPtr->QuantityObsSizes[hardware::HardwareSensorProprioAbstract::qOriEuler]; }
          // bundle observation for known landmark observation
          indexD_Bundleobs = hardwareSensorPtr->getQuantity(hardware::HardwareSensorProprioAbstract::qBundleobs);
          if (indexD_Bundleobs >= 0) { indexE_Bundleobs = indexE; indexE += hardwareSensorPtr->QuantityObsSizes[hardware::HardwareSensorProprioAbstract::qBundleobs]; }
          // absolute velocity for GPS
          indexD_AbsVel = hardwareSensorPtr->getQuantity(hardware::HardwareSensorProprioAbstract::qAbsVel);
          if (indexD_AbsVel >= 0) { indexE_AbsVel = indexE; indexE += hardwareSensorPtr->QuantityObsSizes[hardware::HardwareSensorProprioAbstract::qAbsVel]; }
          // norm of velocity for Odometry (mode 1)
          indexD_NormVel = hardwareSensorPtr->getQuantity(hardware::HardwareSensorProprioAbstract::qNormVel);
          if (indexD_NormVel >= 0) { indexE_NormVel = indexE; indexE += hardwareSensorPtr->QuantityObsSizes[hardware::HardwareSensorProprioAbstract::qNormVel]; }
          // velocity for Odometry (mode 2)
          indexD_Vel = hardwareSensorPtr->getQuantity(hardware::HardwareSensorProprioAbstract::qVel);
          if (indexD_Vel >= 0) { indexE_Vel = indexE; indexE += hardwareSensorPtr->QuantityObsSizes[hardware::HardwareSensorProprioAbstract::qVel]; }

          indexR_Pos = quantsState.getQuantity(RobotQuantity::qPos);
          indexR_OriQuat = quantsState.getQuantity(RobotQuantity::qOriQuat);
          indexR_AbsVel = quantsState.getQuantity(RobotQuantity::qAbsVel);
          indexR_AngVel = quantsState.getQuantity(RobotQuantity::qAngVel);

          indexJ_Pos = quantsJacob.getQuantity(RobotQuantity::qPos);
          indexJ_OriQuat = quantsJacob.getQuantity(RobotQuantity::qOriQuat);
          indexJ_AbsVel = quantsJacob.getQuantity(RobotQuantity::qAbsVel);
          indexJ_AngVel = quantsJacob.getQuantity(RobotQuantity::qAngVel);

          covType = hardwareSensorPtr->covType();
        }
        
        
        virtual void init(unsigned id)
        {
          RawInfos infos;
          queryAvailableRaws(infos);

          // find minimum variance
          jblas::vec3 min_var; min_var(0) = min_var(1) = min_var(2) = 1e3;
          for(std::vector<RawInfo>::iterator it = infos.available.begin(); it != infos.available.end(); ++it)
          {
            hardwareSensorPtr->observeRaw((*it).id, reading);
            for(int i = 0; i < 3; ++i) if (reading.data(i+1+dats) < min_var(i)) min_var(i) = reading.data(i+1+dats);
            if ((*it).id == id) break;
          }

          // do the weighted average using only readings with variance between min and 2*min
          jblas::vec3 average; average.clear();
          jblas::vec3 sum_coeffs; sum_coeffs.clear();
//          jblas::veci3 sum; sum.clear();
          for(std::vector<RawInfo>::iterator it = infos.available.begin(); it != infos.available.end(); ++it)
          {
            hardwareSensorPtr->observeRaw((*it).id, reading);
            for(int i = 0; i < 3; ++i)
              if (reading.data(i+1+dats) < 2*min_var(i))
                { average(i) += reading.data(i+1)*reading.data(i+1+dats); sum_coeffs(i) += reading.data(i+1+dats); }
//                { average(i) += reading.data(i+1); sum(i) += 1; sum_coeffs(i) += jmath::sqr(reading.data(i+1+dats)); }
            if ((*it).id == id) break;
          }
          for(int i = 0; i < 3; ++i) average(i) /= sum_coeffs(i);
//          for(int i = 0; i < 3; ++i) { average(i) /= sum(i); sum_coeffs(i) = sqrt(sum_coeffs(i))/sum(i); }

          // now initialize the robot state with this variance
          for(int i = 0; i < 3; ++i) { reading.data(i+1) = average(i); reading.data(i+1+dats) = min_var(i); }
//          for(int i = 0; i < 3; ++i) { reading.data(i+1) = average(i); reading.data(i+1+dats) = sum_coeffs(i); }
        }
        

        virtual void process(unsigned id, double date_limit)
        {
          // FIXME should use globalPose() function so that extrinsic estimation works
          if (robotPtr()->is_start_pos_abs_init) use_for_init = false;

          if (use_for_init)
            init(id);
          else
            hardwareSensorPtr->getRaw(id, reading);

          EXP_rs.clear();
          expectation->P() = jblas::zero_mat(inns);
          jblas::vec T = ublas::subrange(pose.x(), 0, 3);
          jblas::vec r = ublas::subrange(pose.x(), 3, 7);
          jblas::vec p = ublas::subrange(robotPtr()->pose.x(), 0, 3);
          jblas::vec q = ublas::subrange(robotPtr()->pose.x(), 3, 7);
          jblas::vec Tr = quaternion::rotate(q,T);

          bool detect_faults = false; // will the data uncertainty be consistent when sensor is faulty (bias): see gating

          // POSITION
          if (indexD_Pos >= 0)
          {
            for(size_t i = 0; i < 3; ++i) if (reading.data(indexD_Pos+i+dats) > sigMax) if (!use_for_init) return;

            // compute expectation->x and EXP_rs
            quaternion::rotate_by_dq(q, T, EXP_q);
            ublas::subrange(EXP_rs, indexE_Pos,indexE_Pos+3, indexJ_Pos.start(), indexJ_Pos.start()+indexJ_Pos.size()) = jblas::identity_mat(3);
            ublas::subrange(EXP_rs, indexE_Pos,indexE_Pos+3, indexJ_OriQuat.start(), indexJ_OriQuat.start()+indexJ_OriQuat.size()) = EXP_q;
            ublas::subrange(expectation->x(), indexE_Pos,indexE_Pos+3) = p + Tr;

            // fill measurement
            ublas::subrange(measurement->x(), indexE_Pos,indexE_Pos+3) = ublas::subrange(reading.data, indexD_Pos,indexD_Pos+3) - robotPtr()->start_pos_abs;
            for(int i = 0; i < 3; ++i) measurement->P()(indexE_Pos+i,indexE_Pos+i) = jmath::sqr(reading.data(indexD_Pos+i+dats));
//std::cout << "sensorAbsLoc pos measurement " << *measurement << std::endl;
          }

          // ORIENTATION QUATERNION AND EULER
          /*
           * We cannot directly observe quaternion because there is an unconstrained degree of liberty
           * and the cov matrix has rank 3 but size 4, so the filter update does not work.
           * So we only observe euler angles, taking care of the modulo for innovation computation.
           * TODO When pitch is close to +-PI/2 we should not use euler but rotation vector for instance
           */
          if (indexD_OriQuat >= 0 || indexD_OriEuler >= 0)
          {
            bool isEuler = (indexD_OriEuler >= 0);
            size_t indexE = indexE_OriEuler;

            size_t indexD = isEuler ? indexD_OriEuler : indexD_OriQuat;
            size_t sizeD = isEuler ? 3 : 4;
            for(size_t i = 0; i < sizeD; ++i) if (reading.data(indexD+i+dats) > sigMax) if (!use_for_init) return;


            // compute expectation->x and EXP_rs
            jblas::mat QR_q(4,4);
            jblas::vec4 qr = quaternion::qProd(q, r);
            quaternion::qProd_by_dq1(r, QR_q);
            jblas::mat ER_qr(3,4);
            jblas::vec3 er; quaternion::q2e(qr, er, ER_qr);
            jblas::mat ER_q = ublas::prod(ER_qr, QR_q);
            ublas::subrange(expectation->x(), indexE,indexE+3) = er;
            ublas::subrange(EXP_rs, indexE,indexE+3, indexJ_OriQuat.start(), indexJ_OriQuat.start()+indexJ_OriQuat.size()) = ER_q;

            // fill measurement
            if (isEuler)
            {
              ublas::subrange(measurement->x(), indexE,indexE+3) = ublas::subrange(reading.data, indexD_OriEuler,indexD_OriEuler+3);
              ublas::subrange(measurement->P(), indexE,indexE+3, indexE,indexE+3) =
                hardwareSensorPtr->extractDataCovBlock(indexD_OriEuler, 3);
            } else
            {
              // convert from quaternion to euler
              jblas::mat E_q(3,4); jblas::vec3 euler;
              quaternion::q2e(ublas::subrange(reading.data, indexD_OriQuat,indexD_OriQuat+4), euler, E_q);
              ublas::subrange(measurement->x(), indexE,indexE+3) = euler;
              jblas::sym_mat quat_P = hardwareSensorPtr->extractDataCovBlock(indexD_OriQuat, 4);
              ublas::subrange(measurement->P(), indexE,indexE+3, indexE,indexE+3) = prod_JPJt(quat_P, E_q);
            }

            // correct measurement to take into account modulo
//std::cout << "observe euler ; before modulo : " << measurement->x() << " ; " << measurement->P() << std::endl;
            for(size_t i = indexE; i < indexE+3; ++i)
            {
              double inn = measurement->x()(i) - expectation->x()(i);
              if (inn >  M_PI) measurement->x()(i) -= 2*M_PI; else
              if (inn < -M_PI) measurement->x()(i) += 2*M_PI;
            }
//std::cout << "after modulo : " << measurement->x() << " ; " << measurement->P() << std::endl;

//std::cout << "sensorAbsLoc ori measurement " << *measurement << std::endl;
          }

          // BUNDLE OBSERVATION
          if (indexD_Bundleobs >= 0)
          {
            // get data
            jblas::vec3 pos = ublas::subrange(reading.data, indexD_Bundleobs+0,indexD_Bundleobs+3);
            jblas::vec3 dir = ublas::subrange(reading.data, indexD_Bundleobs+3,indexD_Bundleobs+6);

            // compute expectation->x and EXP_rs and expectation->P
            jblas::mat33 U_p;
            jblas::vec3 u = pos-p;
            jmath::ublasExtra::normalizeJac(u, U_p);
            jmath::ublasExtra::normalize(u);
            ublas::subrange(expectation->x(), indexE_Bundleobs,indexE_Bundleobs+3) = u;
            ublas::subrange(EXP_rs, indexE_Bundleobs,indexE_Bundleobs+3, indexJ_OriQuat.start(), indexJ_OriQuat.start()+indexJ_OriQuat.size()) = -U_p;

            jblas::sym_mat Pos = hardwareSensorPtr->extractDataCovBlock(indexD_Bundleobs, 3);
            ublas::subrange(expectation->P(), indexE_Bundleobs,indexE_Bundleobs+3, indexE_Bundleobs,indexE_Bundleobs+3) = prod_JPJt(Pos, U_p);

            // fill measurement
            ublas::subrange(measurement->x(), indexE_Bundleobs,indexE_Bundleobs+3) = dir;
            ublas::subrange(measurement->P(), indexE_Bundleobs,indexE_Bundleobs+3, indexE_Bundleobs,indexE_Bundleobs+3) =
              ublasExtra::createSymMat(reading.data, dats+1, dats, 3, 3);

            detect_faults = true;
          }

          // NORM_VEL
          if (indexD_NormVel >= 0)
          {
            if (reading.data(indexD_NormVel+0+dats) > sigMax) if (!use_for_init) return; // don't bother to make useless obs, it means that we want to ignore it

            jblas::vec v = ublas::project(robotPtr()->state.x(), indexR_AbsVel);
            // TODO need to use angular velocity when T != 0
            JFR_ASSERT(ublas::norm_2(ublas::subrange(T,0,2)) < 1e-6, "Currently odometry is only supported with SlamOrigin vertically aligned with RobotOrigin");

            // compute expectation->x and EXP_rs
//            quaternion::rotate_by_dq(q, T, EXP_q);
            jblas::mat NV_v(1, 3); // norm jacobian
            expectation->x(indexE_NormVel) = ublasExtra::norm_2(v, NV_v);
            ublas::subrange(EXP_rs, indexE_NormVel,indexE_NormVel+1, indexJ_AbsVel.start(), indexJ_AbsVel.start()+indexJ_AbsVel.size()) = NV_v;
            // fill measurement
            ublas::subrange(measurement->x(), indexE_NormVel,indexE_NormVel+1) = ublas::subrange(reading.data, indexD_NormVel,indexD_NormVel+1);
            measurement->P()(indexE_NormVel,indexE_NormVel) = jmath::sqr(reading.data(indexD_NormVel+1));

            detect_faults = false;//true; // FIXME should be true but should nevertheless include the noise part
            mahaDist2 = -1; // FIXME TEMP FIX see above
          }


          if (use_for_init)
          {
            // for first reading we force initialization
            // FIXME do it correctly if the robot has already started... not sure it is useful though

            if (indexD_OriQuat >= 0 || indexD_OriEuler >= 0) // need to set ori uncertainty before pos uncertainty because of sensor lever arm
            {
              size_t indexE = indexE_OriEuler;
              jblas::mat Q_exp(4,3);
              quaternion::e2q(ublas::subrange(measurement->x(), indexE,indexE+3), q, Q_exp);

              jblas::vec ri = quaternion::q2qc(r);
              q = quaternion::qProd(q, ri);
              jblas::mat Q_r(4,4);
              quaternion::qProd_by_dq1(ri, Q_r);
              Q_exp = ublas::prod(Q_r, Q_exp);

              ublas::subrange(robotPtr()->pose.x(), 3, 7) = q;
              ublas::subrange(robotPtr()->pose.P(), 3,7, 3,7) = ublasExtra::prod_JPJt(
                ublas::subrange(measurement->P(), indexE,indexE+3, indexE,indexE+3), Q_exp);

              // update tmp variables depending on q
              Tr = quaternion::rotate(q,T);

              std::cout << "AbsLoc sets initial orientation q = " << ublas::subrange(robotPtr()->pose.x(), 3, 7) <<
                " e = " <<  quaternion::q2e(ublas::subrange(robotPtr()->pose.x(), 3, 7)) << std::endl;
            }

            if (indexD_Pos >= 0)
            {
              robotPtr()->start_pos_abs = ublas::subrange(measurement->x(), indexE_Pos,indexE_Pos+3) - Tr - ublas::subrange(robotPtr()->state.x(),0,3);
              ublas::subrange(robotPtr()->pose.x(), 0, 3) = jblas::zero_vec(3);
              ublas::subrange(robotPtr()->pose.P(), 0,3, 0,3) = ublas::subrange(measurement->P(), indexE_Pos,indexE_Pos+3, indexE_Pos,indexE_Pos+3) +
                ublasExtra::prod_JPJt(ublas::subrange(robotPtr()->pose.P(), 3,7, 3,7), EXP_q);

              std::cout << "AbsLoc has origin " << robotPtr()->start_pos_abs <<
                " ; initial position " << ublas::subrange(robotPtr()->pose.x(), 0,3) <<
                " ; initial position var " << ublas::subrange(robotPtr()->pose.P(), 0,3, 0,3) << std::endl;
            }
          } else
          {
            // compute expectation->P and innovation
            expectation->P() += ublasExtra::prod_JPJt(ublas::project(robotPtr()->mapPtr()->filterPtr->P(), ia_rs, ia_rs), EXP_rs);
            innovation->x() = measurement->x() - expectation->x();
            innovation->P() = measurement->P() + expectation->P();
            INN_rs = -EXP_rs;

            // gating
            bool integrate = true;
            if (mahaDist2 > 0.0)
            {
              double maha;
              if (!detect_faults)
                maha = innovation->mahalanobis();
              else
              {
                // if the sensor detects faults and maintains the uncertainty consistent,
                // we should not include the measurement uncertainty in the mahalanobis dist
                // because large meas uncert will make the obs always consistent,
                // but as the fault is a bias it will impact the filter even with large uncert
                jafar::jmath::ublasExtra::lu_inv(expectation->P(), innovation->iP_);
                maha = ublasExtra::prod_xt_iP_x(innovation->iP_, innovation->x());
              }
              if (maha > mahaDist2) integrate = false;
            }

            // correct
            if (integrate)
            {
              map_ptr_t mapPtr = robotPtr()->mapPtr();
              ind_array ia_x = mapPtr->ia_used_states();
              mapPtr->filterPtr->correct(ia_x,*innovation,INN_rs,ia_rs);
            }
          }

          if (use_for_init)
          {
            use_for_init = false;
            robotPtr()->is_start_pos_abs_init = true;
            hardwareSensorPtr->getRaw(id, reading); // just to release data
          }
          
          //hardwareSensorPtr->release();
        }

        
    };
}}

#endif