dataManagerOnePointRansac.impl.hpp

Go to the documentation of this file.

#include "kernel/misc.hpp"

#include "jmath/randomIntTmplt.hpp"
#include "jmath/misc.hpp"


#include "rtslam/dataManagerOnePointRansac.hpp"
#include "rtslam/kalmanTools.hpp"

#include "rtslam/rtSlam.hpp"
#include "rtslam/observationAbstract.hpp"

#include "rtslam/imageTools.hpp"

#include <algorithm>

/*
 * STATUS: working fine, use it
 * Buffered update is faster than iterative update, but it doesn't allow
 * active search, so we use a mixed approach
 */
#define BUFFERED_UPDATE 1

/*
 * STATUS: working fine, use it
 * The goal is to improve performance by not computing jacobians
 * and covariance matrix for observations that are not visible
 * (but we compute twice the mean for those that are visible)
 * It brings approx 2% speedup
 */
#define PROJECT_MEAN_VISIBILITY 1

/*
 * STATUS: testing purpose
 * apply quaternion constraints in the filter after each update
 */
#define APPLY_CONSTRAINTS 0

#if RELEVANCE_TEST && RELEVANCE_TEST_P
  #error "RELEVANCE_TEST and RELEVANCE_TEST_P are incompatible!"
#endif

namespace jafar {
  namespace rtslam {

    bool compare_lmk_uncertainty(landmark_ptr_t const & lmkPtr1, landmark_ptr_t const & lmkPtr2)
    {
      double uncert1 = lmkPtr1->uncertainty();
      double uncert2 = lmkPtr2->uncertainty();
      if (uncert1 < uncert2) return true; else
        if (uncert1 == uncert2) return (lmkPtr1->countObserved() > lmkPtr2->countObserved()); else
        return false;
    }

    bool compare_obs_uncertainty(observation_ptr_t const & obsPtr1, observation_ptr_t const & obsPtr2)
    {
      return compare_lmk_uncertainty(obsPtr1->landmarkPtr(), obsPtr2->landmarkPtr());
    }

    template<class RawSpec,class SensorSpec, class FeatureSpec, class RoiSpec, class FeatureManagerSpec, class DetectorSpec, class MatcherSpec>
    void DataManagerOnePointRansac<RawSpec,SensorSpec,FeatureSpec,RoiSpec,FeatureManagerSpec,DetectorSpec,MatcherSpec>::
    processKnown(raw_ptr_t data, double date_limit)
    {
      const unsigned min_as_update = 2;
      int n_updates_ransac = algorithmParams.n_updates_ransac;
      // adapt number of ransac updates for this frame
      double date_begin = kernel::Clock::getTime();
      double time_available = date_limit - date_begin;
      if (date_limit > 0)
      {
        if (prev_average_as_update_time < prev_average_ransac_update_time) prev_average_ransac_update_time = prev_average_as_update_time; // to prevent long ransac update to stick
        if (prev_average_ransac_update_time > 0 && prev_average_as_update_time > 0)
          n_updates_ransac = (time_available - min_as_update*prev_average_as_update_time) / prev_average_ransac_update_time;
        else if (prev_average_ransac_update_time > 0) // if no estimate for as time, use ransac time
          n_updates_ransac = (time_available / prev_average_ransac_update_time) - min_as_update;
        if (n_updates_ransac <= 0) n_updates_ransac = (time_available / prev_average_ransac_update_time);
        if (n_updates_ransac <= 0) n_updates_ransac = 1; // do at least one update!
        if ((unsigned)n_updates_ransac > algorithmParams.n_updates_ransac) n_updates_ransac = algorithmParams.n_updates_ransac;
      }
      double average_ransac_update_time = kernel::Clock::getTime();

      boost::shared_ptr<RawSpec> rawData = SPTR_CAST<RawSpec>(data);
      //###
      //### Init, collect visible observations
      //### 
      map_ptr_t mapPtr = sensorPtr()->robotPtr()->mapPtr();
      unsigned numObs = 0, numObsRansac = 0;

      projectAndCollectVisibleObs();
      if (n_updates_ransac <= 0) return; // here to build visibleList and clearFlags in case next manage or detect steps are made afterwards

      unsigned n_tries = algorithmParams.n_tries;
      if (obsVisibleList.size() < n_tries) n_tries = obsVisibleList.size();

      
      //###
      //### Create the different Ransac sets
      //### 
      unsigned current_try = 0;
      if (n_tries >= 2)
      while (current_try < n_tries)
      {
        // select random obs and match it
        observation_ptr_t obsBasePtr;
        getOneMatchedBaseObs(obsBasePtr, rawData);
        if (!obsBasePtr) break; // no more available matched obs

        // 1b. base obs is now matched
        ransac_set_ptr_t ransacSetPtr(new RansacSet);
        ransacSetList.push_back(ransacSetPtr);
        ransacSetPtr->obsBasePtr = obsBasePtr;
        ransacSetPtr->add_inlier(obsBasePtr);

        current_try ++;
        vec x_copy = updateMean(obsBasePtr);

        JFR_VVDEBUG("Ransac try " << current_try << " with base obs " << obsBasePtr->id());

        // for each other obs
        for(ObsList::iterator obsIter = obsVisibleList.begin(); obsIter != obsVisibleList.end(); obsIter++)
        {
          observation_ptr_t obsCurrentPtr = *obsIter;
          if (obsCurrentPtr == obsBasePtr) continue; // ignore the tested observation

          // project
          ransac_exp.resize(obsCurrentPtr->expectation.size(), false);
          projectFromMean(ransac_exp, obsCurrentPtr, x_copy);

          bool inlier = obsCurrentPtr->events.matched && 
                        isLowInnovationInlier(obsCurrentPtr, ransac_exp, matcher->params.lowInnov);

          if (!inlier)
          {
            matchWithExpectedInnovation(rawData, obsCurrentPtr, true);

            inlier = obsCurrentPtr->events.matched &&
                     isLowInnovationInlier(obsCurrentPtr, ransac_exp, matcher->params.lowInnov);
          }
          
          if (inlier)
          {
            // declare inlier
            ransacSetPtr->add_inlier(obsCurrentPtr);
            JFR_VVDEBUG("Obs " << obsCurrentPtr->id() << " is inlier (@" << obsCurrentPtr->measurement.x() << " exp " << obsCurrentPtr->expectation.x() << ")");
          }
          else{
            // declare pending
            ransacSetPtr->pendingObs.push_back(obsCurrentPtr);
            JFR_VVDEBUG("Obs " << obsCurrentPtr->id() << " is pending (@" << (obsCurrentPtr->events.matched ? (jblas::vec)obsCurrentPtr->measurement.x() : (jblas::vec)jblas::zero_vec(2) ) << " exp " << obsCurrentPtr->expectation.x() << ")");
          }
        } // for each other obs
      } // for i = 0:n_tries

      // TODO we should also store the measurement when building the sets,
      // and take the right one when using the best set

      //###
      //### Process the best Ransac set
      //### 
      ransac_set_ptr_t best_set;
      bool pending_buffered_update = false;
      if (ransacSetList.size() != 0)
      {
        // 1. select ransacSet.inliers.size() max
        for(RansacSetList::iterator rsIter = ransacSetList.begin(); rsIter != ransacSetList.end(); ++rsIter)
          if (!best_set || (*rsIter)->size() > best_set->size()) best_set = *rsIter;

        // if there are too many updates to do bufferized, randomly move out some of them
        // to pending, they may be processed in active search if really necessary
        while (best_set->size() > 1 && best_set->size() > (unsigned)n_updates_ransac)
        {
          int n = (rtslam::rand() % (best_set->size() - 1)) + 1; // keep the first one which is the base obs
          best_set->pendingObs.push_back(best_set->inlierObs[n]);
          kernel::fastErase(best_set->inlierObs, n);
        }

        if (best_set->size() > 1)
        {
          // 2. for each obs in inliers
          JFR_DEBUG_BEGIN(); JFR_DEBUG_SEND("Updating with Ransac:");
          #if RELEVANCE_TEST || RELEVANCE_TEST_P
          double innovation_relevance = 0.0;
          #endif
          for(ObsList::iterator obsIter = best_set->inlierObs.begin(); obsIter != best_set->inlierObs.end(); ++obsIter)
          {
            observation_ptr_t obsPtr = *obsIter;

            // restore measurements and innovation that were used for ransac
            obsPtr->measurement = best_set->inlierMeasurements.at(obsPtr->id());
            obsPtr->computeInnovation();

            // 2a. add obs to buffer for EKF update
            #if BUFFERED_UPDATE
            mapPtr->filterPtr->stackCorrection(obsPtr->innovation, obsPtr->INN_rsl, obsPtr->ia_rsl);
            #if RELEVANCE_TEST || RELEVANCE_TEST_P
            innovation_relevance += obsPtr->computeRelevance();
            #endif
            #else
            obsPtr->project(multi_hyps_);
            obsPtr->computeInnovation();
            #if RELEVANCE_TEST
            if (obsPtr->computeRelevance() > jmath::sqr(matcher->params.relevanceTh))
            #endif
            {
              #if RELEVANCE_TEST_P
              obsPtr->update(obsPtr->computeRelevance() > jmath::sqr(matcher->params.relevanceTh));
              #else
              obsPtr->update();
              #endif
              obsPtr->events.updated = true;
              #if APPLY_CONSTRAINTS
              sensorPtr()->robotPtr()->applyConstraints();
              #endif
            }
            #endif
          }
          bool do_update = false;
          #if BUFFERED_UPDATE
          // 3. perform buffered update
          #if RELEVANCE_TEST || RELEVANCE_TEST_P
          innovation_relevance /= best_set->inlierObs.size();
          #endif
          #if RELEVANCE_TEST
          if (innovation_relevance > jmath::sqr(matcher->params.relevanceTh))
          #endif
          {
            #if RELEVANCE_TEST_P
            mapPtr->filterPtr->correctAllStacked(mapPtr->ia_used_states(), innovation_relevance > jmath::sqr(matcher->params.relevanceTh));
            #else
            mapPtr->filterPtr->correctAllStacked(mapPtr->ia_used_states());
            #endif
            #if APPLY_CONSTRAINTS
            sensorPtr()->robotPtr()->applyConstraints();
            #endif
            do_update = true;
          }
          #if RELEVANCE_TEST
          else pending_buffered_update = true;
          #endif
          
          #endif
          
          for(ObsList::iterator obsIter = best_set->inlierObs.begin(); obsIter != best_set->inlierObs.end(); ++obsIter)
            if (do_update || (*obsIter)->events.updated)
            {
              // Add to tesselation grid for active search
              //featMan->addObs(obsPtr->expectation.x(), obsPtr);
              numObs++; numObsRansac++;
              (*obsIter)->events.updated = true;
              JFR_DEBUG_SEND(" " << (*obsIter)->id());
              //JFR_DEBUG_VSEND(" @"<<(*obsIter)->measurement.x());
            } else
            {
              // TODO do that better with an insignificant flag
              (*obsIter)->events.measured = false;
              (*obsIter)->events.matched = false;
            }
          JFR_DEBUG_END();
        }
      }

      double date_now = kernel::Clock::getTime(), date_prev, time_update;
      average_ransac_update_time = date_now - average_ransac_update_time;
      if (numObsRansac > 0) prev_average_ransac_update_time = average_ransac_update_time / numObsRansac;
      double average_as_update_time = date_now;
      bool stop_no_time = false;

      //###
      //### Process some other observations with Active Search
      //### 
      ObsList activeSearchList = ((ransacSetList.size() == 0) || (best_set->size() <= 1) ? obsVisibleList : best_set->pendingObs);
      // FIXME don't search again landmarks that failed as base
      
      JFR_DEBUG_BEGIN(); JFR_DEBUG_SEND("Updating with ActiveSearch:");
      for (unsigned i = 0; i < algorithmParams.n_recomp_gains && !stop_no_time; ++i)
      {
        // 4. for each obs in pending: retake algorithm from active search
        for(ObsList::iterator obsIter = activeSearchList.begin(); obsIter != activeSearchList.end(); ++obsIter)
        {
          observation_ptr_t obsPtr = *obsIter;
          // FIXME maybe don't clear events and don't rematch if already did, especially if didn't reestimate
          obsPtr->clearFlags();
          obsPtr->measurement.matchScore = 0;

          // 1a. project
          obsPtr->project(multi_hyps_);

          // 1b. check visibility
          obsPtr->predictVisibility();

          if (obsPtr->isVisible()) {

            // Add to tesselation grid for active search
            //featMan->addObs(obsPtr->expectation.x(), obsPtr);
            
            /*
            quicly check if expectation has some negative variance values,
            to ignore the observation and prevent from crashing
            (it means that the filter is corrupted and that we should stop
            everything anyway)
            */
            bool valid = true;
            for (unsigned i = 0; i < obsPtr->expectation.P().size1(); ++i)
              if (obsPtr->expectation.P()(i,i) < 0.0) { valid = false; break; }

            if (valid)
            {
              // predict information gain
              obsPtr->predictInfoGain();

              // add to sorted list of observations
              obsListSorted[obsPtr->expectation.infoGain] = obsIter;
            }
          } // visible obs
        } // for each obs

        // loop only the N_UPDATES most interesting obs, from largest info gain to smallest
        for (ObservationListSorted::reverse_iterator obsIter = obsListSorted.rbegin();
          obsIter != obsListSorted.rend() && !stop_no_time; ++obsIter)
        {
          if (i != algorithmParams.n_recomp_gains-1 && obsIter != obsListSorted.rbegin()) break;
          date_prev = kernel::Clock::getTime();
          observation_ptr_t obsPtr = *(obsIter->second);

          // 1a. re-project to get up-to-date means and Jacobians
          if (obsIter != obsListSorted.rbegin()) obsPtr->project(multi_hyps_); // otherwise already projected right before

          // 1b. re-check visibility, just in case re-projection caused this obs to be invisible
          obsPtr->predictVisibility();
          if (obsPtr->isVisible())
          {
            if (numObs < algorithmParams.n_updates_total)
            {
              matchWithExpectedInnovation(rawData, obsPtr, false); // FIXME maybe don't rematch if already matched ?
              if (obsPtr->events.matched
              #if RELEVANCE_TEST
                      && obsPtr->computeRelevance() > jmath::sqr(matcher->params.relevanceTh)
              #endif
                 ) {
                #if RELEVANCE_TEST
                if (pending_buffered_update)
                {
                  mapPtr->filterPtr->correctAllStacked(mapPtr->ia_used_states());
                  pending_buffered_update = false;
                  #if APPLY_CONSTRAINTS
                  sensorPtr()->robotPtr()->applyConstraints();
                  #endif
                  // TODO mark as updated
                }
                #endif
                obsPtr->markUpdated();
                numObs++;
                JFR_DEBUG_SEND(" " << obsPtr->id());
                //JFR_DEBUG_VSEND(" @"<<obsPtr->measurement.x());
                //                kernel::Chrono update_chrono;
                #if RELEVANCE_TEST_P
                obsPtr->update(obsPtr->computeRelevance() > jmath::sqr(matcher->params.relevanceTh));
                #else
                obsPtr->update();
                #endif
                #if APPLY_CONSTRAINTS
                sensorPtr()->robotPtr()->applyConstraints();
                #endif
                //                total_update_time += update_chrono.elapsedMicrosecond();
              }
            } // number of observations
          } // obsPtr->isVisible()


          // check if we need to stop because we don't have enough time
          if (obsPtr->events.updated && date_limit > 0)
          {
            date_now = kernel::Clock::getTime();
            time_update = date_now - date_prev;
            if (date_now+time_update > date_limit) { stop_no_time = true; break; }
          }

        } // foreach observation

        if (i+1 != algorithmParams.n_recomp_gains && obsListSorted.rbegin() != obsListSorted.rend())
          kernel::fastErase(activeSearchList, obsListSorted.rbegin()->second);
        obsListSorted.clear(); // clear the list now or it will prevent the observation to be destroyed until next frame, and will still be displayed
      }
      JFR_DEBUG_END();

      if (pending_buffered_update) mapPtr->filterPtr->clearStack();

      average_as_update_time = kernel::Clock::getTime() - average_as_update_time;
      if (numObs > numObsRansac) prev_average_as_update_time = average_as_update_time / (numObs-numObsRansac);

      // check duplicates
      updateVisibleObs();
      for(size_t i = 0; i < featMan->projections.size(); ++i)
      {
        int nkilled = 0;
        for(size_t j1 = 0; j1 < featMan->projections[i].size(); ++j1)
          if (featMan->projections[i][j1]->events.updated)
            for(size_t j2 = j1+1; j2 < featMan->projections[i].size(); ++j2)
              if (featMan->projections[i][j2]->events.updated)
                if (ublasExtra::prod_xt_P_x(featMan->projections[i][j1]->measurement.P() + featMan->projections[i][j2]->measurement.P(),
                                            featMan->projections[i][j1]->measurement.x() - featMan->projections[i][j2]->measurement.x()) <= 3)
                  {
                    if (featMan->projections[i][j1]->landmarkPtr()->killed != LandmarkAbstract::kNo) break;
                    if (featMan->projections[i][j2]->landmarkPtr()->killed != LandmarkAbstract::kNo) continue; // test now to avoid testing it systematically above
                    nkilled++;
                    if (compare_obs_uncertainty(featMan->projections[i][j2], featMan->projections[i][j1]))
                      { featMan->projections[i][j1]->landmarkPtr()->killed = LandmarkAbstract::kDuplicate; break; } else
                      { featMan->projections[i][j2]->landmarkPtr()->killed = LandmarkAbstract::kDuplicate; continue; }
                  }
/*
        const size_t max_obs_per_cell = 3;
        if (featMan->projections[i].size()-nkilled > max_obs_per_cell)
        {
          std::sort(featMan->projections[i].begin(), featMan->projections[i].end(), compare_obs_uncertainty);
          size_t n_ok = 0;
          for(size_t j = 0; j < featMan->projections[i].size(); ++j)
            if (featMan->projections[i][j]->landmarkPtr()->killed == LandmarkAbstract::kNo)
            {
              n_ok++;
              if (n_ok > max_obs_per_cell) featMan->projections[i][j]->landmarkPtr()->killed = LandmarkAbstract::kCellOverflow;
            }
        }
*/
      }


      //###
      //### Update obs counters and some other stuff
      //### 
      clearCounters();
      for(ObservationList::iterator obsIter = observationList().begin(); obsIter != observationList().end();obsIter++)
      {
        observation_ptr_t obs = *obsIter;
        if (obs->events.visible) JFR_ASSERT(obs->events.predicted, "obs visible without previous steps");
        if (obs->events.measured) JFR_ASSERT(obs->events.visible && obs->events.predicted, "obs measured without previous steps");
        if (obs->events.matched) JFR_ASSERT(obs->events.measured && obs->events.visible && obs->events.predicted, "obs matched without previous steps");
        if (obs->events.updated) JFR_ASSERT(obs->events.matched && obs->events.measured && obs->events.visible && obs->events.predicted, "obs updated without previous steps");
        
        obs->updateDescriptor();
        #if VISIBILITY_MAP
        obs->updateVisibilityMap();
        #endif
        
        if (obs->events.measured) obs->updatable = obs->events.updated;
        if (not (obs->events.measured && !obs->events.matched && !obs->isDescriptorValid()))
        {
          if (obs->events.measured) { obs->counters.nSearch++; counters.measured++; }
          if (obs->events.matched) { obs->counters.nMatch++; counters.matched++; }
          if (obs->events.updated) { obs->counters.nInlier++; counters.updated++; }
        }
        
        if (obs->events.measured) obs->counters.nSearchSinceLastInlier++;
        if (obs->events.visible) { obs->counters.nFrameSinceLastVisible = 0; counters.visible++; }
        if (obs->events.updated) obs->counters.nSearchSinceLastInlier = 0;
      }

      // clear all sets to liberate shared pointers
      ransacSetList.clear();
      obsBaseList.clear();
      obsFailedList.clear();
    }


    template<class RawSpec,class SensorSpec, class FeatureSpec, class RoiSpec, class FeatureManagerSpec, class DetectorSpec, class MatcherSpec>
    void DataManagerOnePointRansac<RawSpec,SensorSpec,FeatureSpec,RoiSpec,FeatureManagerSpec,DetectorSpec,MatcherSpec>::
    detectNew(raw_ptr_t data)
    {
      boost::shared_ptr<RawSpec> rawData = SPTR_CAST<RawSpec>(data);      
      updateVisibleObs();
      obsVisibleList.clear();
      for(unsigned i = 0; i < algorithmParams.n_init; )
      if (mapManagerPtr()->mapSpaceForInit()) {
        //boost::shared_ptr<RawImage> rawDataSpec = SPTR_CAST<RawImage>(rawData);
        RoiSpec roi;
        if (featMan->getRoi(roi)) {
          // FIXME if we already have searched some part of this roi without finding anything, we should not search again
          // this should be done in featMan, as long as no renew has been done, it remembers the getRoi, and detects when it is not followed by a addObs

          boost::shared_ptr<FeatureSpec> featPtr;
          if (detector->detect(rawData, roi, featPtr))
          {
            // 2a. Create the lmk and associated obs object.
            observation_ptr_t obsPtr =
                mapManagerPtr()->createNewLandmark(shared_from_this());

            // 2b. fill data for this obs
            obsPtr->counters.nSearch = 1;
            obsPtr->counters.nMatch = 1;
            obsPtr->counters.nInlier = 1;
            obsPtr->events.visible = true;
            obsPtr->events.predicted = false;
            obsPtr->events.measured = true;
            obsPtr->events.matched = false;
            obsPtr->markUpdated();
            obsPtr->measurement = featPtr->measurement;

            // 2c. compute and fill stochastic data for the landmark
            obsPtr->backProject();

            // 2d. Create lmk descriptor
            detector->fillDataObs(featPtr, obsPtr);

            // FIXME maybe adjust roi to prevent from detecting points too close to edge compared to descriptor size
            // it would be better if we could check that the descriptor cannot be build before adding the landmark to the map...
            if (obsPtr->updateDescriptor())
            {
              #if VISIBILITY_MAP
              obsPtr->updateVisibilityMap();
              #endif
              featMan->addObs(obsPtr->measurement.x(), obsPtr);
              counters.created++;
              
//#ifndef JFR_NDEBUG
#if 0
              // check that point is correlated very close from the source (because of interpolation, and to check bugs)
              obsPtr->project(multi_hyps_);
              if (obsPtr->predictAppearance())
              {
                jblas::sym_mat P = jblas::identity_mat(obsPtr->expectation.size())*jmath::sqr(4.0);
                RoiSpec roi(obsPtr->expectation.x(), P, 1.0);
                obsPtr->searchSize = roi.count();
                matcher->match(rawData, obsPtr->predictedAppearance, roi, obsPtr->measurement, obsPtr->observedAppearance);
                JFR_ASSERT(ublas::norm_2(obsPtr->measurement.x()-obsPtr->expectation.x()) <= 0.01);
              }
#endif
              
              ++i;
            } else
            {
              obsPtr->landmarkPtr()->mapManagerPtr()->unregisterLandmark(obsPtr->landmarkPtr());
              featMan->setFailed(roi);
            }
          } else // create&init
          {
            featMan->setFailed(roi);
          }
        } else break; // getRoi()
      } else break; // if space in map
    } // detect()

#if 0
    template<class RawSpec,class SensorSpec, class FeatureSpec, class RoiSpec, class FeatureManagerSpec, class DetectorSpec, class MatcherSpec>
    void DataManagerOnePointRansac<RawSpec,SensorSpec,FeatureSpec,RoiSpec,FeatureManagerSpec,DetectorSpec,MatcherSpec>::
    process( boost::shared_ptr<RawAbstract> data )
    {
        boost::shared_ptr<RawSpec> dataSpec = SPTR_CAST<RawSpec>(data);
        // 1. Observe known landmarks.
        processKnownObs(dataSpec); // process known landmarks
        // 2. Initialize new landmark.
        detectNewObs(dataSpec);
    }
#endif

    template<class RawSpec,class SensorSpec, class FeatureSpec, class RoiSpec, class FeatureManagerSpec, class DetectorSpec, class MatcherSpec>
    void DataManagerOnePointRansac<RawSpec,SensorSpec,FeatureSpec,RoiSpec,FeatureManagerSpec,DetectorSpec,MatcherSpec>::
    projectAndCollectVisibleObs()
    {
      obsVisibleList.clear();

      for(ObservationList::iterator obsIter = observationList().begin(); obsIter != observationList().end();obsIter++)
      {
        observation_ptr_t obsPtr = *obsIter;

        obsPtr->clearFlags();
        obsPtr->counters.nFrameSinceLastVisible++;
        obsPtr->measurement.matchScore = 0;

        #if PROJECT_MEAN_VISIBILITY
        obsPtr->projectMean();
        #else
        obsPtr->project(multi_hyps_);
        #endif
        
        if (obsPtr->predictVisibility())
        {
          bool add;
          #if VISIBILITY_MAP
          double visibility, viscertainty;
          obsPtr->landmark().visibilityMap.estimateVisibility(obsPtr, visibility, viscertainty);
          add = false;
          if (visibility > 0.75 && viscertainty > 0.75) add = true; else
          if (!obsPtr->landmark().converged)
            { if (visibility < 0.25) visibility = 0.25; }
          else
            { if (visibility < 0.1) visibility = 0.1; } // allow closing the loop! maybe look at neighbors
          if (viscertainty < 0.25) visibility = 0.25;
          if (rtslam::rand()%1024 < visibility*1024) add = true;
          #else
          add = true;
          #endif
          
          if (add)
            obsVisibleList.push_back(obsPtr);
          //else std::cout << __FILE__ << ":" << __LINE__ << " ignore lmk " << obsPtr->id() << std::endl;
        } // visible obs
      } // for each obs
      remainingObsCount = obsVisibleList.size();
    }

    template<class RawSpec,class SensorSpec, class FeatureSpec, class RoiSpec, class FeatureManagerSpec, class DetectorSpec, class MatcherSpec>
    void DataManagerOnePointRansac<RawSpec,SensorSpec,FeatureSpec,RoiSpec,FeatureManagerSpec,DetectorSpec,MatcherSpec>::
    updateVisibleObs()
    {
      featMan->renew();

      for(ObsList::iterator obsIter = obsVisibleList.begin(); obsIter != obsVisibleList.end();obsIter++)
      { // don't recompute visibility to save time
        observation_ptr_t obsPtr = *obsIter;
        bool add = false;
        
        if (obsPtr->events.updated) add = true; else
        if (mapManagerPtr()->isExclusive(obsPtr)) add = true;
        // TODO with visibility stuff
        
        if (add && !obsPtr->events.killed)
          featMan->addObs(obsPtr->expectation.x(), obsPtr); // don't reproject to save time
      } // for each visible obs
    }


    template<class RawSpec,class SensorSpec, class FeatureSpec, class RoiSpec, class FeatureManagerSpec, class DetectorSpec, class MatcherSpec>
    void DataManagerOnePointRansac<RawSpec,SensorSpec,FeatureSpec,RoiSpec,FeatureManagerSpec,DetectorSpec,MatcherSpec>::
    getOneMatchedBaseObs(observation_ptr_t & obsBasePtr, boost::shared_ptr<RawSpec> rawData)
    {
      bool matchedBase = false;
      while (!matchedBase) {
        // select one random obs
//        obsBasePtr = selectOneRandomObs();
        if (remainingObsCount <= 0) { obsBasePtr.reset(); return; }
        int n = rtslam::rand()%remainingObsCount;
        obsBasePtr = obsVisibleList[n];

// JFR_DEBUG("getOneMatchedBaseObs: trying obs " << obsBasePtr->id() << " already matched " << obsBasePtr->events.matched);
        // try to match (if not yet matched)
        if (!obsBasePtr->events.matched)
        {
          obsBasePtr->events.measured = true;

          #if PROJECT_MEAN_VISIBILITY
          obsBasePtr->project(multi_hyps_);
          #endif

          if (!matchWithExpectedInnovation(rawData, obsBasePtr))
            obsFailedList.push_back(obsBasePtr);

// JFR_DEBUG("getOneMatchedBaseObs: obs " << obsBasePtr->id() << " matched " << obsBasePtr->events.matched << " at " << obsBasePtr->measurement.x() << " predicted at " << obsBasePtr->expectation.x() << " with score " << obsBasePtr->getMatchScore());
        }
        if (obsBasePtr->events.matched && obsBasePtr->hypotheses.size() == 0) // multi hyp lmks are inefficient for ransac
        {
          obsBasePtr->computeInnovation();
          obsBaseList.push_back(obsBasePtr);
          matchedBase = true;
        }
        
        --remainingObsCount;
        obsVisibleList[n] = obsVisibleList[remainingObsCount];
        obsVisibleList[remainingObsCount] = obsBasePtr;
      }
    }


    template<class RawSpec,class SensorSpec, class FeatureSpec, class RoiSpec, class FeatureManagerSpec, class DetectorSpec, class MatcherSpec>
    vec DataManagerOnePointRansac<RawSpec,SensorSpec,FeatureSpec,RoiSpec,FeatureManagerSpec,DetectorSpec,MatcherSpec>::
    updateMean(const observation_ptr_t & obsPtr)
    {
      // get map things
      vec x_copy = mapManagerPtr()->mapPtr()->x();
      ind_array ia_x = mapManagerPtr()->mapPtr()->ia_used_states();
      sym_mat &P = mapManagerPtr()->mapPtr()->P();

      // compute Kalman gain
      mat K(ia_x.size(), obsPtr->innovation.size());
      kalman::computeKalmanGain(P, ia_x, obsPtr->innovation, obsPtr->INN_rsl, obsPtr->ia_rsl, K);

      // perform state update to the mean, get temporary copy
      ublas::project(x_copy, ia_x) += ublas::prod(K, obsPtr->innovation.x());

      return x_copy;
    }


    template<class RawSpec,class SensorSpec, class FeatureSpec, class RoiSpec, class FeatureManagerSpec, class DetectorSpec, class MatcherSpec>
    void DataManagerOnePointRansac<RawSpec,SensorSpec,FeatureSpec,RoiSpec,FeatureManagerSpec,DetectorSpec,MatcherSpec>::
    projectFromMean(vec & exp, const observation_ptr_t & obsPtr, const vec & x)
    {
      vec nobs;

      // get global sensor pose
      vec7 robPose = ublas::project(x, obsPtr->sensorPtr()->robotPtr()->pose.ia());
      vec7 senPose = obsPtr->sensorPtr()->pose.x(); // may be in the filter or not, the Gaussian pose points to the right data in both cases
      vec7 senGlobPose = quaternion::composeFrames(robPose, senPose);

      // project landmark
      vec lmk = ublas::project(x, obsPtr->landmarkPtr()->state.ia());
      obsPtr->model->project_func(senGlobPose, lmk, exp, nobs);

      // (we should not modify obsPtr->expectation because it has already been computed with initial prediction and will be used for full/base search)
    }


    template<class RawSpec,class SensorSpec, class FeatureSpec, class RoiSpec, class FeatureManagerSpec, class DetectorSpec, class MatcherSpec>
    bool DataManagerOnePointRansac<RawSpec,SensorSpec,FeatureSpec,RoiSpec,FeatureManagerSpec,DetectorSpec,MatcherSpec>::
    isLowInnovationInlier(const observation_ptr_t & obsPtr, const vec & exp, double lowInnTh)
    {
      vec inn;
      obsPtr->computeInnovationMean(inn, obsPtr->measurement.x(), exp);
      return (ublas::norm_2(inn) < lowInnTh);
    }


    template<class RawSpec,class SensorSpec, class FeatureSpec, class RoiSpec, class FeatureManagerSpec, class DetectorSpec, class MatcherSpec>
    bool DataManagerOnePointRansac<RawSpec,SensorSpec,FeatureSpec,RoiSpec,FeatureManagerSpec,DetectorSpec,MatcherSpec>::
    matchWithExpectedInnovation(boost::shared_ptr<RawSpec> rawData,  observation_ptr_t obsPtr, bool lowInnov)
    {
      if (obsPtr->predictAppearance())
      {
        RoiSpec roi;

        bool global_matched = false;
        double global_score = 0.0;
        bool use_hypotheses = (obsPtr->hypotheses.size() != 0 && !lowInnov);
        int count = (use_hypotheses ? obsPtr->hypotheses.size() : 1);
        for(int i = 0; i < count; ++i)
        {
          // build ROI
          Expectation &exp = (use_hypotheses ? obsPtr->hypotheses[i].expectation : obsPtr->expectation);
          if (exp.x().size() == 2) // basically DsegMatcher handles it's own roi and having a size4 expectation the following roi computation fails, hence the test
          {
            if (lowInnov)
            {
              jblas::sym_mat P = jblas::identity_mat(obsPtr->expectation.size())*jmath::sqr(matcher->params.lowInnov);
              roi = RoiSpec(ransac_exp, P, 2.0);
            } else
            {
              if (ublasExtra::det(exp.P()) > 5*jmath::sqr(matcher->params.maxSearchSize/jmath::sqr(matcher->params.mahalanobisTh)))
                return false; // if bigger than 5 times the max, then don't even try, it will be deleted anyway
              obsPtr->measurement.searchP = matcher->params.measVar*identity_mat(2);
              roi = RoiSpec(exp.x(), exp.P() + obsPtr->measurement.searchP, matcher->params.mahalanobisTh);
            }
            obsPtr->searchSize = roi.count();
            if (obsPtr->searchSize > matcher->params.maxSearchSize) roi.scale(sqrt(matcher->params.maxSearchSize/(double)obsPtr->searchSize));
          }
          else // Segment
          {
            // Rough approximation, this won't be used by Dseg Matcher, only by the simulator
            vec2 p1, p2;
            vec2 var1, var2;
            p1[0] = exp.x()[0]; p1[1] = exp.x()[1];
            p2[0] = exp.x()[2]; p2[1] = exp.x()[3];
            obsPtr->measurement.searchP = matcher->params.measVar*identity_mat(4);
            var1[0] = (exp.P()(0,0) + matcher->params.measVar) * matcher->params.mahalanobisTh;
            var1[1] = (exp.P()(1,1) + matcher->params.measVar) * matcher->params.mahalanobisTh;
            var2[0] = (exp.P()(2,2) + matcher->params.measVar) * matcher->params.mahalanobisTh;
            var2[1] = (exp.P()(3,3) + matcher->params.measVar) * matcher->params.mahalanobisTh;

            int basex = min(p1[0]-var1[0],p2[0]-var2[0]);
            int basey = min(p1[1]-var1[1],p2[1]-var2[1]);
            cv::Rect rect(
              basex, basey,
              max(p1[0]+var1[0],p2[0]+var2[0]) - basex,
              max(p1[1]+var1[1],p2[1]+var2[1]) - basey
            );
            roi = RoiSpec(rect);
          }

          // match
          Measurement measurement(obsPtr->measurement.size());
          matcher->match(rawData, obsPtr->predictedAppearance, roi, measurement, obsPtr->observedAppearance);
          obsPtr->events.measured = true;

          // FIXME maybe replace this condition by multi hyp ? or not because linearity and ambiguity are different criteria
          double threshold = (lowInnov || obsPtr->landmark().converged || sqrt(jmath::sqr(roi.w()) + jmath::sqr(roi.h())) < matcher->params.hi_limit)*count ?
            matcher->params.threshold : matcher->params.hi_threshold;

          bool matched = false;
          if (obsPtr->events.matched && lowInnov)
            { if (measurement.matchScore >= obsPtr->getMatchScore()-1e-6) matched = true; } else // FIXME couldn't we do that even for not lowInnov ? not supposed to happen ?
            { if (measurement.matchScore >= threshold) matched = true; }
          if (use_hypotheses && measurement.matchScore < global_score-1e-6) matched = false;

          if (matched)
          {
            #if PROJECT_MEAN_VISIBILITY
            if (lowInnov && !obsPtr->events.predicted) obsPtr->project(false);
            #endif

            if (use_hypotheses)
              matched = ObservationAbstract::compatibilityTest(measurement, exp, matcher->params.mahalanobisTh);
            else
              matched = ObservationAbstract::compatibilityTest(measurement, obsPtr->expectation, matcher->params.mahalanobisTh);
          }

          if (matched)
          {
            obsPtr->measurement = measurement;
            global_matched = true;
            global_score = measurement.matchScore;
          }
        }
        if (global_matched) obsPtr->events.matched = true;
        return global_matched;
      } else
        return false;
    }


    template<class RawSpec,class SensorSpec, class FeatureSpec, class RoiSpec, class FeatureManagerSpec, class DetectorSpec, class MatcherSpec>
    void DataManagerOnePointRansac<RawSpec,SensorSpec,FeatureSpec,RoiSpec,FeatureManagerSpec,DetectorSpec,MatcherSpec>::
    dump(raw_ptr_t data)
    {
      LoggableMatchings *lm = new LoggableMatchings(data->name() + ".obs");
      for(ObservationList::iterator obsIter = observationList().begin(); obsIter != observationList().end();obsIter++)
      {
        observation_ptr_t obsPtr = *obsIter;
                if (obsPtr->events.updated) lm->addMatching(obsPtr->id(), obsPtr->measurement.x(0), obsPtr->measurement.x(1));
      }
            if (loggerTask) loggerTask->push(lm);
    }


    template<class RawSpec,class SensorSpec, class FeatureSpec, class RoiSpec, class FeatureManagerSpec, class DetectorSpec, class MatcherSpec>
    void DataManagerOnePointRansac<RawSpec,SensorSpec,FeatureSpec,RoiSpec,FeatureManagerSpec,DetectorSpec,MatcherSpec>::
		writeLogHeader(kernel::DataLogger& log) const
    {
      log.writeComment("DataManager");
      log.writeLegendTokens("visible measured matched updated created killed");
    }

    template<class RawSpec,class SensorSpec, class FeatureSpec, class RoiSpec, class FeatureManagerSpec, class DetectorSpec, class MatcherSpec>
    void DataManagerOnePointRansac<RawSpec,SensorSpec,FeatureSpec,RoiSpec,FeatureManagerSpec,DetectorSpec,MatcherSpec>::
		writeLogData(kernel::DataLogger& log) const
    {
      log.writeData(counters.visible);
      log.writeData(counters.measured);
      log.writeData(counters.matched);
      log.writeData(counters.updated);
      log.writeData(counters.created);
      log.writeData(counters.killed);
    }



  } // namespace ::rtslam
} // namespace jafar::