D:/Travail/These/Determination caracteristiques camera/GSoC/SfM/src/bundle_related.h

#ifndef _GSOC_SFM_POINTS_BUNDLE_RELATED_H
#define _GSOC_SFM_POINTS_BUNDLE_RELATED_H 1

#include <vector>
#include <sba.h>
#include "libmv/numeric/numeric.h"
#include "libmv/base/vector.h"

//A lot of these functions are extracted from Lourakis' SBA:
//http://www.ics.forth.gr/~lourakis/sba/

#include "macro.h" //SFM_EXPORTS

namespace OpencvSfM{
  class SFM_EXPORTS SequenceAnalyzer;
  class SFM_EXPORTS PointOfView;

  struct bundle_datas
  {
    libmv::vector< int >& idx;
    libmv::vector< libmv::Mat3 >& intraParams;
    libmv::vector< Eigen::Quaterniond >& rotations;
    libmv::vector< libmv::Vec3 >& translations;
    double* points3D;
    int cnp;
    int pnp;
    int mnp;
    int ncon;
    int mcon;

    bundle_datas(libmv::vector< int >& index,
      libmv::vector< libmv::Mat3 >& i,
      libmv::vector< Eigen::Quaterniond >& r,
      libmv::vector< libmv::Vec3 >& t,
      int c, int p, int mp, int n, int m)
      :idx(index),intraParams(i),rotations(r),translations(t),
      cnp(c), pnp(p),mnp(mp), mcon(m),ncon(n)
    {
      points3D = NULL;
    }
  };

  void SFM_EXPORTS full_bundle( SequenceAnalyzer &sequence,
    std::vector<PointOfView>& cameras);

  void calcImgProjFullR(double a[5],double qr0[4],double t[3],double M[3],
    double n[2]);


  inline static void quatMultFast(double q1[4], double q2[4], double p[4])
  {
    double t1, t2, t3, t4, t5, t6, t7, t8, t9;
    //double t10, t11, t12;

    t1=(q1[0]+q1[1])*(q2[0]+q2[1]);
    t2=(q1[3]-q1[2])*(q2[2]-q2[3]);
    t3=(q1[1]-q1[0])*(q2[2]+q2[3]);
    t4=(q1[2]+q1[3])*(q2[1]-q2[0]);
    t5=(q1[1]+q1[3])*(q2[1]+q2[2]);
    t6=(q1[1]-q1[3])*(q2[1]-q2[2]);
    t7=(q1[0]+q1[2])*(q2[0]-q2[3]);
    t8=(q1[0]-q1[2])*(q2[0]+q2[3]);


    t9=0.5*(t5-t6+t7+t8);
    p[0]= t2 + t9-t5;
    p[1]= t1 - t9-t6;
    p[2]=-t3 + t9-t8;
    p[3]=-t4 + t9-t7;
  }

  void img_projsRTS_x(/*cameras and points*/ double *p,
  /*sparse matrix of 2D points*/ struct sba_crsm *idxij,
    /*tmp vector*/int *rcidxs, /*tmp vector*/int *rcsubs,
    /*out vect*/double *hx,
    void *adata);

  void img_projsKRTS_x(/*cameras and points*/ double *p,
  /*sparse matrix of 2D points*/ struct sba_crsm *idxij,
    /*tmp vector*/int *rcidxs, /*tmp vector*/int *rcsubs,
    /*out vect*/double *hx,
    void *adata);

  void img_projsRT_x(/*cameras and points*/ double *p,
  /*sparse matrix of 2D points*/ struct sba_crsm *idxij,
    /*tmp vector*/int *rcidxs, /*tmp vector*/int *rcsubs,
    /*out vect*/double *hx,
    void *adata);
  void calcImgProjJacRTS(double a[5],double qr0[4],double v[3],double t[3],
    double M[3],double jacmRT[2][6],double jacmS[2][3]);

  void img_projsRTS_jac_x(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata);
}

#endif