viewsManager2.hpp

/* $Id: viewsManager.hpp 4232 2009-10-20 14:25:46Z nksallem $ */

#ifndef _VIEWS_MANAGER2_HPP
#define _VIEWS_MANAGER2_HPP

#include "geom/t3d.hpp"
#include "geom/t3dPointTools.hpp"
#include "camera/cameraContainer.hpp"
#include "jmath/jblas.hpp"
#include "jmath/vlls.hpp"
#include "jmath/ublasExtra.hpp"
#include "kernel/jafarException.hpp"
#include "bundler/bundlerException.hpp"

namespace jafar {

  namespace bundler {
    
    class ViewsManager2 {
      private:
      jafar::camera::CameraContainer container;
      jmath::VariableSizeLinearLeastSquares2 vlls;
      jblas::vec3 center;
      double error;
      std::map<size_t, geom::T3D*> references;
      std::map<size_t, jblas::vec2> pixels;
      size_t counter;     
      bool saveData;
      geom::T3D* lastKnownLocation;
      bool one_camera;
      void computeReprojectionError();
      inline geom::T3D* refByIndex(size_t _index) const {
        std::map<size_t, geom::T3D*>::const_iterator it = references.find(_index);
        JFR_PRED_ERROR(it != references.end(), 
                       BundlerException,
                       BundlerException::VIEWSMANAGER_POS_NOT_FOUND,
                       "refByIndex: "<<_index<<" not found!")
        return it->second;
      }

      public:
      ViewsManager2(bool save = false) : 
        vlls(3), counter(0), saveData(save), lastKnownLocation(NULL), one_camera(false) {};
      ViewsManager2(const std::string& camCalibFile,
                    jafar::camera::CameraType camType = jafar::camera::MONO, 
                    bool save = false) :
        vlls(3), counter(0), saveData(save), lastKnownLocation(NULL), one_camera(true)
      {
        setCamera(camType, camCalibFile);
      }
      ViewsManager2(const jblas::vec& camParams, 
                    camera::CameraType camType = jafar::camera::MONO, 
                    bool save = false) :
        vlls(3), counter(0), saveData(save), lastKnownLocation(NULL), one_camera(true)
      {
        setCamera(camType, camParams);
      }
      ~ViewsManager2();
      void setCamera(camera::CameraType camType, const std::string& camCalibFile);
      void setCamera(camera::CameraType camType, const jblas::vec& camParams);
      std::string camCalibFile() const;
      void insert(jblas::vec2 pixel, geom::T3D* ref, size_t index = -1, 
                  const camera::CameraContainer &thisContainer = camera::CameraContainer());
      void insertPixel(size_t index, const jblas::vec2& pixel, 
                       const camera::CameraContainer &container = camera::CameraContainer()) {
        JFR_PRED_ERROR(lastKnownLocation, 
                       BundlerException,
                       BundlerException::VIEWSMANAGER_POS_NOT_FOUND,
                       "last known frame is NULL")
        if(saveData)
          pixels[index] = pixel;

        insert(pixel, lastKnownLocation, index, container);
        lastKnownLocation = NULL;
      }
      void insertReference(size_t index, geom::T3D* ref) {
        if(saveData)
          references[index] = ref;
        lastKnownLocation = ref;
      }
      void solve() 
      { 
        vlls.solve();
        center = vlls.x();
      };
      jblas::vec3 solution() const { return center; };
      double reprojectionError(){
        computeReprojectionError();
        return error;
      }
    };

  }//namespace bundler

}//namespace jafar

#endif