/**************************************************
 * 参考文献
 * "Estimating 3-D Location Parameters Using Dual Number Quaternions"
 * アルゴリズム、変数は論文に準拠
 *
 **************************************************/

#ifndef __DUAL_QUATERNION_EST_H__
#define __DUAL_QUATERNION_EST_H__

#include <vector>
#include <octave_rtmex/octave_rtmex.h>
#include <utils/UtilTimer.h>
#include "VOUtil.h"

using namespace std;

namespace rtm{
  class DualQuaternionEst {
  public:
    DualQuaternionEst();
    ~DualQuaternionEst();
    coords estMotion(vector<ColumnVector> &pres, vector<ColumnVector> &curs, int nrnsc);
    void setCFFCMode(int mode){ cffc_mode_ = mode;}
  private:
    void selectFeatures(const vector<ColumnVector> &src,
			list<int> slist, vector<ColumnVector> &dst);
    coords estMotionOne(vector<ColumnVector> &pres, vector<ColumnVector> &curs);
    Matrix makeQmat(const ColumnVector& vec);
    Matrix makeWmat(const ColumnVector& vec);
    Matrix makeWQmat(const ColumnVector& vec, bool qmatp);
    Matrix makeSkewMatrix(const ColumnVector& vec);
    void makeStereoCovsfromPos(const vector<ColumnVector> &points, vector<Matrix> &covs);
    double evalEstimation(const vector<ColumnVector> pres, const vector<ColumnVector> curs,
			  const vector<Matrix> covs, const coords &co);
    double getMahalaDist(const ColumnVector &vec1, const ColumnVector &vec2,const Matrix &cov);
    Matrix makeStereoCovfromPos(const ColumnVector &point);

    UtilTimer utimer_;

    /* */
    int cffc_mode_;
  };
};

#endif
