historyManager.hpp

Go to the documentation of this file.

#ifndef HISTORYMANAGER_HPP_
#define HISTORYMANAGER_HPP_

#include "rtslam/mapAbstract.hpp"
#include "rtslam/mapObject.hpp"

/*
L'idée est donc ici non pas de faire du full slam, mais de garder quand meme quelques poses.
Le parse idéalement suivra une progression géométrique.
Et pour ne pas trop compliquer la gestion de ces états, on va essayer de ne pas trop ajouter ou retirer brutalement,
mais plutot les faire glisser... Imaginons trois timepoint : 1s, 10s, 100s
Au début on n'a que l'état courant, puis au coup d'après on ajoute un
nouvel état courant par duplication, et on laisse glisser le premier dans le passé... quand celui dans le passé
atteint le premier timepoint 1s, on duplique alors l'état courant à nouveau, pendant que l'autre part vers le
timepoint suivant 10s, puisqu'il n'y en avait pas déjà en "voyage", sinon il n'y en a qu'un sur 10 qui passe.
L'idée est qu'il y en ait toujours un entre 0 et 1s, un entre 1 et 10s, et un entre 10 et 100s.
Dès qu'un état atteint un timepoint, il place un flag d'autorisation de passage au timepoint précédent, et selon
le flag d'autorisation de passage de son timepoint il continue ou est supprimé.

Maintenant comment dupliquer un état ? L'idée est que ces keyframes constituent des ancres de mémoire, les corrélations
croisées avec le reste de l'état suffisent-elles à le définir, ou faut-il ajouter son timestamp dans l'état ? Il n'y a
pas besoin de l'estimer donc normalement pas besoin de l'y mettre. Mais on ne pourra plus y toucher donc c'est vraiment
ses corrélation croisées qui vont le définir.

Du coup pour dupliquer, on met exactement les memes covariances, mais que met-on pour la covariance entre l'état et
l'état dupliqué ? L'identité, puisqu'ils sont identiques ? Mais ça va les obliger à rester identique, ce que l'on ne
veut pas, puisque si un état va continuer à représenter un timestamp fixe, l'autre va évoluer dans le temps... mais
ce n'est pas juste la prédiction qui fait ça ? Après tout avec  les amers c'est pareil, ça veut juste dire que si je
corrige un gros coup mon état, ça va aussi corriger celui juste avant, mais si je le fais évoluer très loin il va perdre
sa corrélation avec le vieil état... donc oui a priori il faut mettre une cross corrélation complète.

L'autre question c'est de quoi est ce qu'on garde un historique, juste la pose, tout l'état, un entre deux ?
La pose c'est le plus important, clairement, c'est ce qui détermine l'observation des amers, et plus on met
de choses plus ça va remplir l'état (une pose c'est un amer, un état entier c'est trois amers, si on garde trois
éléments dans l'historiques ça fait 3 contre 9 déjà...)
Mais d'un autre coté si on veut aider la consistance de l'estimation des biais par ex ça peut etre utile de les
mettre... Justement si on lui dit que ça ne bouge pas, et qu'on ne le fait donc pas bouger avec le prédicition,
est-ce que réellement le fait de l'avoir en double completement corrélé va l'empecher de devenir inconsistent ?

On pourrait faire un hybride aussi, en garder 1 ou 2 avec tout, et 2 ou 3 autres avec juste la pose...


 */
namespace jafar {
  namespace rtslam {


    class HistoryManagerAbstract
    {
      protected:
        map_ptr_t map;
        jblas::ind_array ia_state;

        jblas::ind_array duplicate()
        {
          jblas::ind_array ia_newstate = map->reserveStates(ia_state.size());
          ublas::project(map->x(), ia_newstate) = ublas::project(map->x(), ia_state);
          ublas::project(map->P(), ia_state, ia_newstate) = ublas::project(map->P(), ia_state, ia_state); // full correlation
          ublas::project(map->P(), ia_newstate, map->ia_used_states()) = ublas::project(map->P(), ia_state, map->ia_used_states());
          return ia_newstate;
        }

      public:
        HistoryManagerAbstract(map_ptr_t map, jblas::ind_array ia_state):
          map(map), ia_state(ia_state) {}
        virtual void process(double date) = 0;
        virtual void print() = 0;
    };


    class HistoryManagerSparse: public HistoryManagerAbstract
    {
      protected:
        struct State { double date; jblas::ind_array ia; };

        struct Gate { double delay_end, delay; State *state; };
        typedef std::vector<Gate> GateVector;
        GateVector gates;

        double start_date;
        double end_date;
        bool ended;
        double first_gate_factor;
        double last_date;
        bool processed;

        double horizon_date;

      public:
        HistoryManagerSparse(map_ptr_t map, jblas::ind_array ia_state, double first, double factor, int count):
          HistoryManagerAbstract(map, ia_state)
        {
          double delay = first;
          for(int i = 0; i < count; ++i) { gates.push_back((Gate){delay,0.0,NULL}); delay *= factor; }
          horizon_date = -1;
          ended = false;
          first_gate_factor = gates.back().delay_end / (gates.back().delay_end - gates.front().delay_end);
        }

/*        HistoryManagerSparse(map_ptr_t map, jblas::ind_array ia_state, std::vector<double> delays):
          HistoryManagerAbstract(map, ia_state),
          delays(delays) {}
*/

        /*
         * The idea there is to duplicate the current state when the last state has passed the first gate,
         * and then each gate passes its state to the next gate until one is deleted.
         * This ensures that each gate always has an associated state in the interval with the previous gate.
         */
        void process(double date)
        {
          last_date = date;
          processed = false;

          // if first time just initialize
          if (horizon_date < 0)
          {
            start_date = date;
            end_date = date + gates.back().delay_end;
            for(GateVector::iterator it = gates.begin(); it != gates.end(); ++it)
            {
              it->state = new State();
              it->state->date = date;
              it->state->ia = duplicate();
            }
            horizon_date = date;
            return;
          }

          // first, check whether there is a state between now and the first gate
          // if has, don't do anything, otherwise duplicate and maintain other states
          if (date >= horizon_date)
          {
            processed = true;
            // scale the delays if not ended
            if (!ended)
            {
              double scale = date - start_date;
              if (date > end_date) { scale = gates.back().delay_end; first_gate_factor = 1.0; ended = true; }
              for(GateVector::iterator it = gates.begin(); it != gates.end(); ++it)
                it->delay = it->delay_end * scale/gates.back().delay_end;
            }

            State *new_state = new State();
            new_state->date = date;

            State *state_to_pass = new_state;
            for(GateVector::iterator it = gates.begin(); it != gates.end(); ++it)
            {
              if (it->state->date <= date - it->delay)
              { // switch current state with the one that is passed
                State *tmp = state_to_pass;
                state_to_pass = it->state;
                it->state = tmp;
              } else
                break;
            }

            // delete state_to_pass
            map->liberateStates(state_to_pass->ia);
            delete state_to_pass;
            // duplicate state
            new_state->ia = duplicate();
            // compute horizon_date
            horizon_date = date + gates.front().delay * first_gate_factor;
          }
        }

        void print()
        {
          const int cwidth = 150;
          double dwidth = gates[gates.size()-1].delay_end + gates[gates.size()-2].delay_end;

          printf("%7.3f%c", last_date-start_date, processed ? '*' : ' ');
          //std::cout << std::fixed << std::setw(7) << std::setprecision(3) << last_date-start_date << (processed ? "*" : " ") << std::setprecision(6); std::cout.unsetf(std::ios_base::floatfield);
          int pos = 8;
          for(GateVector::reverse_iterator it = gates.rbegin(); it != gates.rend(); ++it)
          {
            int p = cwidth - (int)(it->delay * cwidth/dwidth + 0.5) - pos;
            for(int i = 0; i < p-1; ++i) std::cout << " ";
            if (p != 0) std::cout << "@";
            pos += p;
          }
          int p = cwidth - pos + 1;
          for(int i = 0; i < p-1; ++i) std::cout << " ";
          std::cout << "|" << std::endl;

          pos = 0;
          for(GateVector::reverse_iterator it = gates.rbegin(); it != gates.rend(); ++it)
          {
            int p = cwidth - (int)((last_date - it->state->date) * cwidth/dwidth + 0.5) - pos;
            for(int i = 0; i < p-1; ++i) std::cout << " ";
            if (p != 0) std::cout << "X";
            pos += p;
          }
          std::cout << std::endl;

/*
          std::cout << "Gates: ";
          for(GateVector::reverse_iterator it = gates.rbegin(); it != gates.rend(); ++it)
            std::cout << it->delay << " ; ";
          std::cout << "States: ";
          for(GateVector::reverse_iterator it = gates.rbegin(); it != gates.rend(); ++it)
            std::cout << last_date - it->state->date << " ; ";
          std::cout << "Horizon: " << horizon_date;
          std::cout << std::endl;
*/
        }

    };





  }
}

#endif // #ifndef HISTORYMANAGER_HPP_