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

#ifndef __PROB_MOTION_EST_H__
#define __PROB_MOTION_EST_H__

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

using namespace std;

namespace rtm{
  class ProbMotionEst {
  public:
    ProbMotionEst();
    ~ProbMotionEst();

    void estMotion(const vector<ColumnVector> &pres, const vector<ColumnVector> &curs,
		   const quaternion &dquat, int nrnsc,
		   coords &ans_co, Matrix &ans_Cov);

    void estMotion(const vector<ColumnVector> &pres, const vector<ColumnVector> &curs,
		   const vector<Matrix> &SCovs, const vector<Matrix> &iSCovs,
		   const vector<Matrix> &iSQCovs, const int nrnsc,
		   coords &ans_co, Matrix &ans_Cov);

  private:
    void makeInverseCovsfromStereoandDQuat(const vector<ColumnVector> &pres,
					   const vector<ColumnVector> &curs,
					   const vector<Matrix> &SCovs,
					   const quaternion &dquat,
					   vector<Matrix> &invCovs);

    void estMotionOne(const vector<ColumnVector> &pres, const vector<ColumnVector> &curs,
		      const vector<Matrix> &iCwSQs, coords &co, Matrix &Cov);


    Matrix makeQmat(const ColumnVector& vec);
    Matrix makeWmat(const ColumnVector& vec);
    Matrix makeWQmat(const ColumnVector& vec, bool qmatp);
    Matrix makeSkewMatrix(const ColumnVector& vec);
    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);
  };
};

#endif
