sba.hpp

/* $Id$ */

#ifndef BUNDLE_SBA_HPP
#define BUNDLE_SBA_HPP

#include <iostream>
#include <string>
#include <cstdlib>

#include "kernel/csvFile.hpp"
#include "bundle/bundleException.hpp"
#include "geom/t3dQuaternion.hpp"
#include "camera/cameraPinhole.hpp"
#include "premodeler/ifManager.hpp"
#include "jmath/jblas.hpp"
#include "jmath/ublasExtra.hpp"


namespace jafar {
  
  namespace bundle {

    class SBACamera;
    std::ostream& operator<<(std::ostream& s, const jafar::bundle::SBACamera& c);

    class SBAObservations;
    
    class SBAObservation {
      private:
      jblas::vec3 point;
      std::map<int, jblas::vec2> projections;
      int nbFramesIn;
      public:
      SBAObservation();
      SBAObservation(const SBAObservation& obs);
      void setPoint(const jblas::vec3& _point);
      jblas::vec3 getPoint() const;
      int nbOfFramesIn() const;
      jblas::vec2 getProjection(const int& frameIndex);
      void insertProjection(const int& frameIndex, const jblas::vec2& _projection);
      typedef std::map<int, jblas::vec2>::const_iterator ProjectionsIterator;
      inline std::map<int, jblas::vec2>::const_iterator projectionsBegin() const {
        return (this->projections).begin();
      };
      inline std::map<int, jblas::vec2>::const_iterator projectionsEnd() const {
        return (this->projections).end();
      };

//       typedef std::map<int, jblas::vec2>::iterator proj_iterator;
//       inline std::map<int, jblas::vec2>::iterator proj_begin() const {
//         return projections.begin();
//       };
//       inline std::map<int, jblas::vec2>::iterator proj_end() const {
//         return projections.end();
//       };

      inline size_t projectionsSize() const {return (this->projections).size();};
      friend class SBAObservations;
    };

    class SBAObservations : public jafar::kernel::CSVFileSaveLoad {
      private:
      static int nbProjections;
      std::map<int, SBAObservation> observations;
      protected:
      void loadCSVFile(kernel::CSVFile& csvFile);
      void saveCSVFile(kernel::CSVFile& csvFile);
      public:
      SBAObservations() : CSVFileSaveLoad() { nbProjections = 0; };
      typedef std::map<int, SBAObservation>::const_iterator ObservationsIterator;
      void insertObservation(const int& obsId, const SBAObservation& obs);
      SBAObservation getObservation(const int& obsId);
      inline std::map<int, SBAObservation>::const_iterator begin(){
        return observations.begin();
      };
      inline std::map<int, SBAObservation>::const_iterator end(){
        return observations.end();
      };
      inline size_t size(){
        return observations.size();
      };
      inline int nbOfProjections() {
        return (int)nbProjections;
      }
      int maxProjectionsNumber();
    };

    class SBACameras;

    class SBACamera {
      private:
      jafar::camera::CameraPinhole camera;
      std::map<int, jafar::geom::T3DQuaternion> cameraMotion;
      public:
      SBACamera();
      SBACamera(const jafar::camera::CameraPinhole& _camera);
      SBACamera(const SBACamera& sbaCam);
      void setPinholeCamera(const jafar::camera::CameraPinhole& _camera);
      jafar::camera::CameraPinhole getPinholeCamera();
      void insertPose(const int& poseIndex, const jafar::geom::T3D& pose);
      jafar::geom::T3DQuaternion getPose(const int& poseIndex);
      inline int motionSize() const {return this->cameraMotion.size();};
      typedef std::map<int, geom::T3DQuaternion>::const_iterator MotionsIterator;
      inline std::map<int, jafar::geom::T3DQuaternion>::const_iterator motionBegin() const {
        return cameraMotion.begin();
      }
      inline std::map<int, jafar::geom::T3DQuaternion>::const_iterator motionEnd() const {
        return cameraMotion.end();
      }
      static void toTsaiNotation(const bundle::SBACamera& cam, double* jafarIntrinsics);
      static void toJafarNotation(bundle::SBACamera& cam, double* tsaiIntrinsics);
      void toVector(jblas::vec& v, const int& poseIndex);
      void toVector(jblas::vec& v, jafar::geom::T3DQuaternion pose);
      static SBACamera fromVector(jblas::vec& v, const int& poseId);
      static SBACamera fromVector(const jblas::vec& intrinsics, 
                                  const jblas::vec& extrinsics, 
                                  const int& poseId);
      void projectPointOnFrame(const jblas::vec3& inWorldCoordinates,
                               const jafar::geom::T3DQuaternion& pose, 
                               jblas::vec2& projection,
                               jblas::mat& transformJac,
                               jblas::mat& projectionJac);
      void projectPointOnFrame(const jblas::vec3& inWorldCoordinates,
                               const jafar::geom::T3DQuaternion& pose, 
                               jblas::vec2& projection);
      friend class SBACameras;
      friend std::ostream& operator<<(std::ostream& s, const jafar::bundle::SBACamera& c);

    };

    class SBACameras : public jafar::kernel::CSVFileSaveLoad {
      private:
      std::map<int, SBACamera> cameras;
      std::map<int, geom::T3DQuaternion*> poses;
      std::map<int, int> framesCameras;
      static size_t nframes;
      protected:
      void loadCSVFile(kernel::CSVFile& csvFile);
      void saveCSVFile(kernel::CSVFile& csvFile);
      public:
      SBACameras() : CSVFileSaveLoad() { nframes = 0; };
      typedef std::map<int, SBACamera>::const_iterator CamerasIterator;
      void insertCamera(const int& camId, const SBACamera& cam);
      SBACamera getCamera(const int& camId);
      inline std::map<int, SBACamera>::const_iterator begin(){
        return cameras.begin();
      };
      inline std::map<int, SBACamera>::const_iterator end(){
        return cameras.end();
      };
      inline size_t size(){
        return cameras.size();
      };
      size_t nbFrames();
      void insertPose(const int& poseIndex, jafar::geom::T3D* pose);
      jafar::geom::T3DQuaternion* getPose(const int& poseIndex);
//       int getCameraFromFrame(const int& frameId);
//       void setCameraFrame(const int& frameId, const int& camId);
      void setCameraIntrinsics(const int& camIndex, const jafar::camera::CameraPinhole& pCamera);
      std::map<int, geom::T3DQuaternion*> getPoses();
    };

    class SparseBundleAdjustment {
      private:
      SBACameras cams;
      SBAObservations obs;
      std::string obsFileName;
      std::string camFileName;
      std::map<int,int> frameCameraMap;
      int whichCameraId(const int& frameId);
      
      ublas::vector<jblas::vec> P;
      ublas::vector<jblas::vec> deltaP;
      ublas::vector<jblas::vec> PdeltaP;
      ublas::vector<jblas::vec> eab;
//       ublas::compressed_matrix<jblas::vec2> X;
//       ublas::compressed_matrix<jblas::vec2> hX;
//       ublas::compressed_matrix<jblas::vec2> e;
//       ublas::compressed_matrix<jblas::vec2>::iterator1 hxRowIt, xRowIt, eRowIt;
//       ublas::compressed_matrix<jblas::vec2>::iterator2 X_ij, hX_ij, e_ij;
      
      int nbCams; 
      int nbObs;
      int nbProj; 
      int nbVars;
      int stop;
      int iteration;

      public:
      static const double SBA_EPSILON;//       1E-12
      static const double SBA_EPSILON_SQ;//    ( (SBA_EPSILON)*(SBA_EPSILON) )
      static const double SBA_ONE_THIRD;//     0.3333333334
      static const double SBA_INIT_MU;//       1E-3
      static const int SBA_ITERATION_MAX;//    100

      SparseBundleAdjustment();

      SparseBundleAdjustment(const SBACameras& _cams,
                             const SBAObservations& _obs);

      SparseBundleAdjustment(const std::string& camFileName, 
                             const std::string& obsFileName);

      SparseBundleAdjustment(const std::string& camFileName,
                             const std::string& calFileName,
                             const std::string& obsFileName);
      
      SparseBundleAdjustment(const std::map<int, jblas::vec3>& _trackedObjects,
                             const jafar::premodeler::IFLists& _globalTracker,
                             std::map<int, jafar::geom::T3D *> _toOriginReferences,
                             const jafar::camera::CameraPinhole& pCamera);
      
      void loadCameras(const std::string& camFileName);

      void loadObservations(const std::string& obsFileName);

      void loadCalibration(const std::string& calFileName);

      void saveFiles(const std::string& _obsFileName, 
                     const std::string& _camFileName);

      void bundleAll();

      void explicitStopReason();

      //      double meanReprojectionError();

      ublas::vector<jblas::vec> getOptimized();
      ublas::vector<jblas::vec3> getOptimizedPoints();
      int getNbVariables();
      
      int getNbObseravtions();

      int getNbProjections();
      
      int getNbFrames();
    };

    std::ostream& operator<<(std::ostream& s, const jafar::bundle::SBAObservation& o);

  } // end namespace bundle
} // end namespace jafar
#endif