adapter.hpp

/* $Id$ */

#ifndef _BUNDLER_ADAPTER_HPP
#define _BUNDLER_ADAPTER_HPP

#include "kernel/jafarException.hpp"
#include "bundler/viewsManager.hpp"
#include "bundler/bundlerException.hpp"
#include "geom/Declarations.hpp"
#include "geom/Point.hpp"
#include "premodeler/ifManager.hpp"
#include "camera/cameraContainer.hpp"
#include "geom/t3d.hpp"
#include <set>
namespace jafar {

  namespace bundler {
    using namespace std;
    using namespace jafar;
    using namespace bundler;
    using namespace camera;
    using namespace geom;

    template<class TRACKERS>
      class Adapter {

      protected :
      ViewsManager* vManager;
      TRACKERS m_trackers;
      struct Point3DCompare{
        bool operator() (const geom::Point3D& lhs, const geom::Point3D& rhs){
          return lhs.id() < rhs.id();
        }
      };
      std::set<geom::Point3D, Point3DCompare> objects;
      std::map<int, jblas::vec3> points;
      std::map<int, double> errors;
      double mean_error;

      public :
      Adapter()
      {
        vManager = new ViewsManager();
      };
      Adapter(const camera::CameraType& camType,
              const std::string& camCalibFile)
      {
        vManager = new ViewsManager(camCalibFile,
                                    camType);
      };      
      Adapter(const camera::CameraType& camType,
              const std::string& camCalibFile,
              const TRACKERS& trackers) : m_trackers(trackers)
      {
        vManager = new ViewsManager(camCalibFile,
                                    camType);
      };
      Adapter(const jafar::camera::CameraType& camType,
              const std::string& camCalibFile,
              const std::map<int, geom::T3D*>& toOriginFrames,
              const TRACKERS& trackers) : m_trackers(trackers)
      {
        vManager = new ViewsManager(camCalibFile,
                                    camType,
                                    toOriginFrames);
      }
      virtual ~Adapter()
      {
        delete vManager;
      }
      void setCamera(const camera::CameraType& camType,
                     const std::string& camCalibFile) {
        vManager->setCamera(camType, camCalibFile);
      }
      void setTrackers(const TRACKERS& trackers) {
        m_trackers = trackers;
      }
      inline void setReferences(const std::map<int, geom::T3D*>& toOriginFrames) {
        vManager->setToOrigineReferences(toOriginFrames);
      }
      inline void insertReference(const int& index, 
                                  T3D* sensorToOrigin)
      {
        vManager->insertToOriginReference(index, sensorToOrigin);
      }
      void computeMeanReprojectionError()
      {
        cout << ">>> compute mean reprojection errors"<<endl;
        mean_error=0;
        std::map<int, double>::iterator it;
        for (it = errors.begin();
             it != errors.end();
             ++it) {
          mean_error+=it->second;
        }
        mean_error/=errors.size();
        cout << "<<< compute mean reprojection errors "<<mean_error<<endl;
      }
      inline double meanReprojectionError()
      {
        return mean_error;
      }
      void removeOutliers(const double& k = 1.5)
      {
        cout << ">>> remove outliers"<<endl;
        JFR_PRED_ERROR(k>=1.0,
                       BundlerException,
                       BundlerException::ADAPTER_INVALID_CONSTRAINT,
                       "outliers control constrant must be >=1.0");
        computeMeanReprojectionError();
        int counter=0;
        std::map<int, Point3D>::iterator point;
        std::map<int, double>::iterator error;
        for (point = objects.begin();
             point != objects.end();
             ++point) {
          error = errors.find(point->first);
          if (error->second > k * mean_error){
            objects.erase(point);
            ++counter;
          }
        }
        cout << "<<< remove outliers "<<counter<<" points removed"<<endl;
      }
      inline std::set<geom::Point3D, Point3DCompare> trackedObjects()
      {
        return objects;
      }
      inline std::map<int, jblas::vec3> trackedPoints()
      {
        return points;
      }
      void writeToFile(const string& fullFileName)
      {
        ofstream file;
        file.open(fullFileName.c_str(), ios::out);
        if(!file.bad()){
          file << trackedObjects.size() <<endl;
          std::map<int, Point3D>::iterator it;
          for (it = objects.begin();
               it != objects.end();
               it++) {
            file << (it->second).toString() <<endl;
          }
        } else {
    
          //    JFR_RUN_TIME("can not open " + fullFileName + " in write mode!");
          JFR_ERROR(BundlerException,
                    BundlerException::FILESYSTEM_ERROR,
                    "can not open " + fullFileName + " in write mode!");
        }
        file.close();
      }
      inline ViewsManager* viewsManager() const
      {
        return vManager;
      }
      inline std::vector<jafar::bundler::View*> viewsList() const
      {
        return vManager->viewsList();
      }

      protected:
      virtual void process() {};
      virtual void processTracker() {};
      virtual void loadTrackerViews() {};
      void addTrackedObject(const geom::Point3D& p)
      {
        objects.insert(p);
      }
      void addTrackedObject(const geom::Point3D& p,
                            const double& e)
      {
        objects.insert(p);
        errors[p.id()] = e;
      }
      void addTrackedPoint(const int& id,
                           const jblas::vec3& p)
      {
        points[id] = p;
      }
      void addTrackedPoint(const int& id,
                           const jblas::vec3& p,
                           const double& e)
      {
        points[id] = p;
        errors[id] = e;
      }

    }; //class Adapter
  } //namespace bundler
} //namespace jafar
#endif