incrementalBundler.hpp

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

#ifndef _INCREMENTAL_BUNDLER_HPP
#define _INCREMENTAL_BUNDLER_HPP

#include "geom/t3d.hpp"
#include "geom/t3dPointTools.hpp"
#include "geom/Declarations.hpp"
#include "camera/cameraContainer.hpp"
#include "bundler/view.hpp"
#include "jmath/jblas.hpp"
#include "jmath/vlls.hpp"
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_io.hpp>

namespace jafar {
  namespace bundler {
    
    class IncrementalBundler {
      private:
      std::map<int, jblas::mat34> m_projections;
      std::map<int, jblas::vec2> m_pixels;
      std::map<int, double> m_errors;
      jblas::vec3 m_solution;
      double error;
      jmath::VariableSizeLinearLeastSquares2 vlls;

      public:
      IncrementalBundler() : vlls(4) {};
      ~IncrementalBundler();
      inline jblas::vec solve() { 
  vlls.solve(); 
  JFR_DEBUG("vlls solution "<<vlls.x())
  return vlls.x();
      };
      inline jblas::vec3 solution() {
  jblas::vec homogenious = vlls.x();
  if(homogenious(3) != 1) {
    m_solution(0) = homogenious(0) / homogenious(3);
    m_solution(1) = homogenious(1) / homogenious(3);
    m_solution(2) = homogenious(2) / homogenious(3);
  } else {
    m_solution(0) = homogenious(0);
    m_solution(1) = homogenious(1);
    m_solution(2) = homogenious(2);
  }
  JFR_DEBUG("normalized solution "<<m_solution)
  return m_solution;
      };
      double reprojectionError();
      void addEntry(const jblas::vec2& pixel, const jblas::mat34& projection);
      void addEntry(int index, const jblas::vec2& pixel, const jblas::mat34& projection);
      void addProjection(const jblas::mat34& projection, const int& id);
      void addObservation(const jblas::vec2& pixel, const int& view);
      void fill();
      std::map<int, double>& errors() {
  return m_errors;
      }; 
      bool refine(const double& coef = 1.5, size_t minentries =3);
      double tune(const double& wish = 0.2,
      const double& coef = 1.5, 
      size_t minentries = 3, 
      int maxiter = 10);
      /*@return number of entries in the system in term of observations ie number
       * of entries in the linear least squares system / 2
       */
      inline int entries() const {
        return vlls.countMeasures()/2;
      }

      inline std::pair<jblas::mat34, jblas::vec2> data(int viewId){
        JFR_PRECOND(m_projections.size()!=0 && m_pixels.size()!=0,
        "IncrementalBundler::data no data is saved!")

        std::map<int, jblas::mat34>::const_iterator projection = m_projections.find(viewId);
        JFR_PRECOND(projection != m_projections.end(),
                    "IncrementalBundler::data no projection data for this entry")
        std::map<int, jblas::vec2>::const_iterator pixel = m_pixels.find(viewId);
        JFR_PRECOND(pixel != m_pixels.end(),
                    "IncrementalBundler::data no pixel data for this entry")
        std::pair<jblas::mat34, jblas::vec2> datum(projection->second, pixel->second);
        return datum;
      }
    };

  }//namespace bundler

}//namespace jafar

#endif