#include "MotionClassifierCore.h"


using namespace std;
using namespace rtm;


/**************************************************
 *
 *  VOPData : Visual Odometry Point Data
 *  移動物体分離機能付きビジュアルオトリにおいて必要な要素を持ったデータクラス
 *
 **************************************************/
VOPData::VOPData(){ this->reset();}

void VOPData::reset(){
  ntrack = -1;
  id = -1;
  isStatic = true;
  isEnvironment = false;

  cur_pos = pre_pos = ColumnVector(0);
  l_cov = l_icov = Matrix(0,0);

  worldpos = ColumnVector(0);
  worldcov = Matrix(0,0);

  rotpos = ColumnVector(0);

  w_poss.clear();
  w_covs.clear();
}



/* 注:初めての場合 worldposとworldcovを直接与えることを忘れずに */
void VOPData::init(const ColumnVector &lpos, const quaternion &quat,
		   const StereoParams &sprm, const Matrix qcov){
  ntrack = 0;
  id = 0;

  l_cov = makeStereoErrorCov(lpos, sprm.bl, sprm.f, sprm.su, sprm.sv, sprm.sd);
  w_poss.clear();
  w_covs.clear();
  cur_pos = lpos;
  updateMSWorldValues(lpos, quat, qcov);
}


/*
 * quaternionの共分散はポート入力の値ではなく，角速度のみに依存するものをいれる
 */
bool VOPData::update(const ColumnVector &pre, const ColumnVector &cur,
		     const StereoParams &sprm, const quaternion &quat,
		     const Matrix &qcov, const quaternion &dquat){
  ntrack++;

  /* 特徴点の更新 */
  pre_pos = pre; /* debug */
  cur_pos = cur;

  /* ステレオ誤差の共分散行列 */
  l_cov = makeStereoErrorCov(cur_pos, sprm.bl, sprm.f, sprm.su, sprm.sv, sprm.sd);
  l_icov = l_cov.inverse();

  //Matrix J = dpLocalPointbyQuat(pre_pos, dquat);
  Matrix J = dpWorldPointbyQuat(cur_pos, dquat.to_vector());
  SQCov = l_cov + J * qcov * J.transpose();
  iSQCov = SQCov.inverse();

  /* 慣性センサの姿勢で補正(yaw=0) */
  {
    ColumnVector rpy = quat2rpy(quat);
    rpy(2) = 0.0;
    quaternion rquat = rpy2quat(rpy);
    rotpos = rquat.rotate_vector(cur_pos);
  }

  updateMSWorldValues(cur, quat, qcov);

  return true;
}


/* l_covが先に計算されている必要性がある */
void VOPData::updateMSWorldValues(const ColumnVector &lpos, const quaternion &quat,
				  const Matrix qcov){
  /* 位置 */
  w_poss.push_front(D3(quat.rotate_vector(cur_pos)));

  /* ステレオ誤差 */
  Matrix J1 = quat2matrix(quat);
  Matrix src1 = J1 * l_cov * J1.transpose();
  w_st_cov = src1;

  /* ステレオ誤差 + 回転誤差 */
  Matrix J2 = dpWorldPointbyQuat(lpos, quat.to_vector());
  Matrix src2 = J2 * qcov * J2.transpose();
  w_covs.push_front(D33(src1 + src2));

  if(w_poss.size() > 101) w_poss.pop_back();
  if(w_covs.size() > 101) w_covs.pop_back();
}



/**************************************************
 *
 * voldata : Visual Odometry Object Data
 * 特徴点群をクラスタリングした物体のデータクラス
 *
 **************************************************/

VOLData::VOLData(){
  reset();
  emptyp = true;
}


void VOLData::reset(){
  /* empty=trueにするのはupdateLabelで */
  ntrack = -1;
  evals = centroid = ColumnVector(0);
  Cov = evecs = Matrix(0,0);
  isStatic = true;
  fidx.clear();
  w_poss.clear();
  w_covs.clear();
}


/******************************
 * 共分散行列を計算する
 *******************************/
void VOLData::getCovariance(vector<VOPData> &ftrs){
  int i;
  if(!fidx.empty()){
    /* 重心 */
    centroid = this->getCentroid(ftrs);

    /* 共分散行列 */
    int fn = static_cast<int>(fidx.size());
    Matrix Xd(3, fn);
    list<int>::iterator it=fidx.begin();
    for(i=0; i<fn; it++, i++){
      Xd.insert(ftrs[*it].rotpos - centroid, 0, i);
    }
    Cov = Xd * Xd.transpose() / static_cast<double>(fn);

    /* 固有値・固有ベクトル */
    EIG eig(Cov);
    ColumnVector _evals = real(eig.eigenvalues());
    Matrix _evecs = real(eig.eigenvectors());

    /* 降順に並び替え */
    int dlist[3];
    this->sortVectorElement(_evals, dlist);

    evecs = Matrix(3,3,0);
    evals = ColumnVector(3,0);
    for(i=0; i<3; i++){
      evecs.insert(_evecs.column((dlist[i])), 0, i);
      evals(i) = _evals(dlist[i]);
    }

    /* 小さすぎる固有値を変更 */
    bool toosmallp = false;
    for(i=0; i<3; i++){
      if(evals(i) < 250.0){ // 50 * 50 程度は欲しい
	evals(i) = 250.0;
    	toosmallp = true;
      }}
    if(toosmallp){
      DiagMatrix tmp(evals);
      Matrix tmp2(tmp); //省略するとエラー
      /* 共分散の再算出 */
      Cov = evecs * tmp2 * evecs.inverse();
    }

    /* 共分散逆行列 */
    invCov = Cov.inverse();
  }
  else{
    this->reset();
  }
}


ColumnVector VOLData::getCentroid(vector<VOPData> &ftrs){
  centroid = ColumnVector(3,0);
  int fn = static_cast<int>(fidx.size());
  list<int>::iterator it = fidx.begin();
  for( ; it!=fidx.end(); it++){
    centroid += ftrs[*it].rotpos;
  }
  centroid = centroid / static_cast<double>(fn);
  return centroid;
}


/******************************
 * 入力のベクトルの降順の位置をintの配列で返す
 ******************************/
void VOLData::sortVectorElement(const ColumnVector& src, int* down_list){
  int dim = src.rows();
  double max;
  double vmin = src.min() - 1.0;
  double pre_max= src.max() + 1.0;
  int tmp[dim];
  int nmulti = 0;
  for(int i=0; i<dim;){
    max = vmin;
    for(int j=0; j<dim; j++){
      if(src(j)>max && src(j)<pre_max){
	max = src(j);
      }}

    nmulti = 0;
    for(int j=0; j<dim; j++){
      if(src(j)==max){
	tmp[nmulti] = j;
	nmulti++;
      }}
    for(int j=0; j<nmulti; j++){
      down_list[i] = tmp[j];
      i++;
    }
    pre_max = max;
  }
}


/**************************************************
 *
 * MotionClassifierCore
 *
 *
 **************************************************/

MotionClassifierCore::MotionClassifierCore(){
  pthread_mutex_init(&mutex,NULL);
  separatep_ = true;
}


MotionClassifierCore::MotionClassifierCore(double dt){
  pthread_mutex_init(&mutex,NULL);
  separatep_ = true;
  this->init(dt);
}


MotionClassifierCore::~MotionClassifierCore(){
}


void MotionClassifierCore::init(double dt){
  /* 変数の初期化 */
  dt_ = dt;
  estco_ = coords();

  /* SLAM */
  fslam_.init(dt_);

  /* 特徴点群の確保 */
  ftrs_.assign(max_fn_, VOPData());
  labels_.assign(clst_maxn_, VOLData());

  /* ステレオカメラの情報 */
  sprm_.init(460.0, 90, 1.0, 1.0, 1.0);

  QCov_ = Matrix(4,4,1.0);
  pre_avel_ = ColumnVector(3,0);
}


/* 初期姿勢角を設定する */
void MotionClassifierCore::initStartOrient(const double* quat){
  ColumnVector X(13,0);
  for(int i=0; i<4; i++) X(i+3) = static_cast<double>(quat[i]);
  fslam_.setX(X);
  estco_.init(X.extract_n(0,7));

  /* debug */
  cout << "calibrated : " << endl;
  pquat_ = quaternion(X.extract(3,6));

  utimer_.start();
  utimer_.tick();
}


/* 初期位置を設定する */
void MotionClassifierCore::initFirstPosition(){
  
}


void MotionClassifierCore::convert(const double *flow, const double *avel, const double *quat,
				 const double *tvel, const double *odometry,
				 double *out_coords, double *out_ftrs){
  static int count =0;
  count ++;
  //cout << "========== @debug count : " << count << endl;
  //double dt = utimer_.tick();

  /* 入力データの更新 */
  setInputData(flow, avel, quat, tvel, odometry);

  /* 剛体分離 */
  separateObjects();

  /* 移動物体認識 */
  detectMovingObjects();

  // { /* debug */
  //   for(int i=0; i<clst_maxn_; i++){
  //     if(!labels_[i].fidx.empty()){
  // 	if(labels_[i].isStatic)
  // 	  cout << i << "th : static  : " << labels_[i].fidx.size() << endl;
  // 	else
  // 	  cout << i << "th : dynamic : " << labels_[i].fidx.size() << endl;
  //     }
  //   }}

  /* 推定用のデータの作成 */
  makeEstimateDatas();

  /******************************
   * Motion Estiamtor
   ******************************/
  coords me_co;
  Matrix me_cov(3,3,0);
  bool estmtnp = estimateMotion(me_co, me_cov);
  ColumnVector me_vel = me_co.worldpos/dt_;
  me_cov = me_cov / dt_;
  if(me_co.worldpos.norm(2.0) > 200) estmtnp = false;

  //estco_.transform(me_co, false); /* debug */

  /******************************
   * FastSLAM
   ******************************/
  /* 予測 */
  fslam_.predict();

  // /* 角速度の更新 */
  // fslam_.estZofAVel();
  // fslam_.setHofAVel();
  // fslam_.setQ(makeAVelCov(avel_));
  // fslam_.update(avel_);

  /******************************
   * 運動推定の更新
   ******************************/
  /* 運動推定角速度 */
  if(estmtnp){
    fslam_.estZofAVel();
    fslam_.setHofAVel();
    ColumnVector me_avel = dquat2avel(me_co.quat, dt_);
    fslam_.setQ(makeAVelCov(me_avel));
    fslam_.update(me_avel);

    /* 運動推定並進 */
    fslam_.estZofLVel();
    fslam_.setHofLVel();
    fslam_.setQ(me_cov);
    fslam_.update(me_vel);
  }

  /****************************************/

  /* 姿勢角の更新 */
  fslam_.estZofVertical();
  fslam_.setHofVertical();
  Matrix tmpcov = 0.000001 * make_unit_matrix(3);
  fslam_.setQ(tmpcov);
  ColumnVector tmpvec(3,0); tmpvec(2) = 1;
  fslam_.update(cquat_.inverse_rotate_vector(tmpvec));

  /* 特徴点による更新 */
  fslam_.updatebyPoints(curs_, wposs_, WCovs_);
  this->updateEstimateDatas();


  /* 結果の取得 */
  estco_.init(fslam_.getX().extract(0,6)); /* debug */

  // { /* debug */
  //   ColumnVector tmp(7,0);
  //   tmp.insert(cquat_.to_vector(), 3);
  //   estco_.init(tmp); /* debug */
  // }

  /******************************
   * 結果出力
   ******************************/
  this->writeResultCoords(out_coords);
  this->writeResultFeatures(out_ftrs);

  /* 更新分 */
  pquat_ = cquat_;
  pre_avel_ = avel_;
}



void MotionClassifierCore::setInputData(const double *flow, const double *avel, const double *quat,
				      const double *tvel, const double *odometry){
  /* 角速度の更新 */
  avel_ = makeColumnVectorfromArray(3, avel);
  //dquat_ = avel2dquat(avel_, dt_);

  /* 回転角の更新 */
  cquat_ = quaternion(makeColumnVectorfromArray(4, quat));
  dquat_ = getDQuat(pquat_, cquat_);

  /* 姿勢角の共分散行列 */
  for(int i=0; i<4; i++){
    for(int j=0; j<4; j++){
      QCov_(i,j) = quat[4+4*i+j];
    }}

  updateOdometryData(odometry);

  /* 特徴点の更新 */
  fn_ = setFeatures(flow);
}


/************************************
 * 3次元フローはトラックに失敗した回は必ずcount=0を返す(新しい特徴点は入らない)ので
 * count > 0 => VisionBoard上では継続してトラックされている
 **************************************/
int MotionClassifierCore::setFeatures(const double* flow){
  int idx;
  int fn = 0;

  ColumnVector pre(3,0);
  ColumnVector cur(3,0);

  Matrix qcov = makeAVelCov(avel_);
  Matrix J1 = dpQuatbyAVel(cquat_.to_vector(), dt_);
  qcov = J1 * qcov * J1.transpose(); /* quaternionの角速度誤差による共分散行列 */


  /* 前回用 */
  Matrix _qcov = makeAVelCov(pre_avel_);
  Matrix _J1 = dpQuatbyAVel(pquat_.to_vector(), dt_);
  _qcov = _J1 * _qcov * _J1.transpose(); /* quaternionの角速度誤差による共分散行列 */


  for(int i=0; i<max_fn_; i++){
    idx = 7 * i;
    if(flow[idx] > 0){ /* トラックに成功 */
      /* 特徴点位置 */
      pre(0) =   static_cast<double>(flow[idx+1+2]);
      pre(1) = - static_cast<double>(flow[idx+1+0]);
      pre(2) = - static_cast<double>(flow[idx+1+1]);
      cur(0) =   static_cast<double>(flow[idx+4+2]);
      cur(1) = - static_cast<double>(flow[idx+4+0]);
      cur(2) = - static_cast<double>(flow[idx+4+1]);

      /* 初めての点 */
      if(ftrs_[i].ntrack < 1){
	ftrs_[i].init(pre, pquat_, sprm_, _qcov);
	fslam_.makeNewWorldPoint(pre, ftrs_[i].worldpos, ftrs_[i].worldcov);
      }

      /* 更新 */
      if(ftrs_[i].update(pre, cur, sprm_, cquat_, qcov, dquat_)){
	fn++;
      }

      // /* debug 仮近いものを移動物体とする */
      // if(cur.norm(2.0) < 1500) ftrs_[i].isStatic = false;
    }
    else{ /* Fail Track */
      ftrs_[i].reset();
    }
  }

  return fn;
}


void MotionClassifierCore::updateOdometryData(const double* odometry){
  if(odometry[0] > 0.5) odometryp_ = true;
  else odometryp_ = false;

  if(odometryp_){
    if(odm_data_.ntrack<0){
      odm_data_.ntrack=0;
      D3 pos;
      D33 cov;
      for(int i=0; i<3; i++) cov(i,i) = pow(200.0,2);
      odm_data_.w_poss.push_front(pos);
      odm_data_.w_covs.push_front(cov);
    }
    odm_data_.ntrack++;
    ColumnVector l_dpos(3,0);
    for(int i=0; i<3; i++) l_dpos(i) = - odometry[i+1]/dt_; /* 特徴点の動きに合わせる */
    l_dpos = cquat_.rotate_vector(l_dpos);
    l_dpos(2) = 0;
    D3 pos = D3(l_dpos) + odm_data_.w_poss[0];

    D33 cov;
    for(int i=0; i<3; i++) cov(i,i) = pow(200.0, 2);
    odm_data_.w_poss.push_front(pos);
    odm_data_.w_covs.push_front(cov);
  }
  else{
    odm_data_.ntrack = -1;
    odm_data_.w_poss.clear();
    odm_data_.w_covs.clear();
  }
}


/******************************
 *
 *  剛体分離
 *
 ******************************/
void MotionClassifierCore::separateObjects(){
  /* 無所属を登録 */
  this->entry2Labels();

  /* ラベルの評価 */
  this->trackLabels();

  /* 新ラベルを作成 */
  //this->resetAllLabel(); /* debug */
  this->getNewLabels();


  // // ラベルを統合
  // this->fusionLabels();

  //debug
  // for(int i=0; i<clst_maxn_; i++){
  //   if(!labels_[i].fidx.empty()){
  //     int med_ntrack = getMedianNTrack(i, 4);
  //     cerr << "@separate " << i << "th cloud" << endl;
  //     cerr << "label track is " << labels_[i].ntrack << "  med_ntrack is " << med_ntrack << endl;
  //   }}

  /* 次回に残す情報を更新 */
  this->reupdateLabels(); /* for entry2labels */

}


void MotionClassifierCore::entry2Labels(){

  /* 前回の姿勢角(yaw=0) */
  ColumnVector rpy = quat2rpy(pquat_);
  rpy(2) = 0;
  quaternion rotquat = rpy2quat(rpy);

  /* ラベル=0の特徴点をラベル内か判定 */
  for(int i=0; i<max_fn_; i++){
    if(ftrs_[i].id == 0){
      ColumnVector rotpos = rotquat.rotate_vector(ftrs_[i].pre_pos);
      ColumnVector tmp;
      double min_dist = 10000;
      int min_idx = -1;
      double dist;

      /* 3Dマハラノビス距離の最近傍点を探索 */
      for(int j=1; j<clst_maxn_; j++){
	if(!labels_[j].fidx.empty()){
	  tmp = rotpos - labels_[j].centroid;
	  dist = tmp.transpose() * labels_[j].invCov * tmp;
	  //dist = tmp.norm(2.0);/* debug */
	  if(dist < min_dist){
	    min_dist = dist;
	    min_idx = j;
	  }}}


      int lid = getLabelingIDfromPos(rotpos);
      if( (min_idx > 0 && lid > 0) &&
	  checkAdj(labels_[min_idx].labeling_ids, lid)){
      	  // ( checkAdj(labels_[min_idx].labeling_ids, lid) ||
      	  //   min_dist < kDThre )){
	ftrs_[i].id = min_idx;
	ftrs_[i].isStatic = labels_[min_idx].isStatic;
      }}}
}


void MotionClassifierCore::trackLabels(){
  updateLabels();
  for(int i=1; i<clst_maxn_; i++){
    if(!labels_[i].emptyp){
      this->trackLabel(labels_[i]);
    }}
}


/*
 * ラベルの更新
 */
void MotionClassifierCore::updateLabels(){
  /* 特徴点情報を一回リセット */
  for(int i=0; i<clst_maxn_; i++){
    if(labels_[i].fidx.empty()) labels_[i].emptyp = true;
    else labels_[i].emptyp = false;
    labels_[i].fidx.clear();
  }

  /* 特徴点情報の更新 */
  for(int i=0; i<max_fn_; i++){
    if(ftrs_[i].ntrack>0){
      labels_[ftrs_[i].id].fidx.push_back(i);
    }}

  /* 特徴点が４点以上のものをラベルとして残す */
  for(int i=1; i<clst_maxn_; i++){
    if(labels_[i].fidx.size() >= 4){
      labels_[i].ntrack++;
    }
    else{
      list<int>::iterator fit = labels_[i].fidx.begin();
      for( ; fit!=labels_[i].fidx.end(); fit++ ) ftrs_[*fit].id = 0;
      labels_[i].reset();
    }}
}


bool MotionClassifierCore::trackLabel(VOLData& label){
  /**** はずれ値の処理 *****/
  removeOutlierbyFlow(label);

  /* MS世界座標系の変数の更新 */
  updateMSValuesofLabel(label);


  /******************************
   * 静止物体の剛体チェック
   ******************************/
  if(label.isStatic){
    if(!evalStaticObjects(label)){
      label.isStatic = false;
      list<int>::iterator it = label.fidx.begin();
      for( ; it!=label.fidx.end(); it++) ftrs_[*it].isStatic = false;
    }}

  /******************************
   * 移動物体の一物体性チェック
   ******************************/
  if(!label.isStatic){
    evalMovingObjectIdentity(label, 200.0);
  }

  return true;
}


/* はずれ値は1frameで激しく飛ぶもののみとする */
void MotionClassifierCore::removeOutlierbyFlow(VOLData& label){
  /**** 世界座標系でのフローの中央値の取得 */
  D3 med_flow;
  {
    list<double> flows[3];
    list<int>::iterator fit = label.fidx.begin();
    for( ; fit!=label.fidx.end(); fit++){
      D3 tmp(ftrs_[*fit].w_poss[0] - ftrs_[*fit].w_poss[1]);
      for(int i=0; i<3; i++) flows[i].push_back(tmp(i));
    }
    for(int i=0; i<3; i++) flows[i].sort(greater<double>());
    list<double>::iterator vit;
    for(int i=0; i<3; i++){
      vit=flows[i].begin();
      for(unsigned int j=0; j<flows[i].size()/2; j++){
	vit++;
      }
      med_flow(i) = *vit;
    }
  }

  /****
   * 中央値からのマハラノビス距離ではずれ値を検出(近傍点として回転誤差を無視する)
   ****/
  list<int>::iterator fit = label.fidx.begin();
  for( ; fit!=label.fidx.end(); ){
    D3 f_flow = ftrs_[*fit].w_poss[0] - ftrs_[*fit].w_poss[1];
    //double mhl = getMhlnbsDistance(ftrs_[*fit].wpnt_covs[0].inverse(), (f_flow - med_flow));
    D33 tmpcov;
    for(int i=0; i<3; i++) tmpcov(i,i) = pow(100.0, 2);
    D33 cov = tmpcov + ftrs_[*fit].w_st_cov;
    double mhl = getMhlnbsDistance(cov.inverse(), (f_flow - med_flow));
    if( mhl > 5){
      printf("@debug removeOutlier mhl = %f, ", mhl);
      cout << "pos :" << ftrs_[*fit].cur_pos.transpose() << endl;
      // printf("@debug remove  count:%d mahala:%f\n", ftrs_[*fit].ntrack ,mhl);
      // cout << "pos:  " << ftrs_[*fit].cur_pos.transpose() << endl;
      // cout << " flow :  " << (ftrs_[*fit].cur_pos-ftrs_[*fit].pre_pos).transpose() << endl;
      // cout << "fflow : " << f_flow << endl;
      // cout << "mflowq: " << med_flow << endl;
      // printf("\n");
      ftrs_[*fit].reset();
      fit = label.fidx.erase(fit);
    }
    else{
      fit++;
    }
  }
}


/******************************
 *
 * 静止とみなしている物体の評価
 * WM世界座標系において
 * 重心軌道とのマハラノビス距離のメディアンが大きい：移動物体（回転）
 * メディアンは小さいが結構な数が大きい：移動物体（1部）
 *
 ******************************/
bool MotionClassifierCore::evalStaticObjects(VOLData &label){
  const static double mthre = 1.5;
  int cf[4] = {1, 3, 10, 30};

  for(int i=0; i<4; i++){
    int fn, err_fn;
    double med_mhl;
    if(evalStaticObjectsFlow(label, cf[i], mthre, fn, err_fn, med_mhl) && fn >= 3){
      if(med_mhl > mthre){
	cout << "@debug evalStatic this label is Dynamic entire" << endl;
	return false;
      }
      else if (err_fn >= 3){
	cout << "@debug evalStatic this label is Dynamic partlly" << endl;
	return false;
      }
      else if (err_fn > 0){
	printf("@debug evalStatic this label is %d's error points\n", err_fn);
	removeStaticObjectsBadFlow(label, cf[i], mthre);
      }}}
  return true;
}


bool MotionClassifierCore::evalStaticObjectsFlow(VOLData &label, int nback, double mthre,
					       int &fn, int &err_fn, double &med_mhl){
  //fn: nback回以上フローがとれている点の数
  //err_fn: fnの中でマハラノビス距離がmthreを越えてしまった点の数
  //med_mhl: fnの各点のマハラノビス距離の中央値
  if( label.ntrack < nback ) return false;

  list<double> mhls;

  fn = 0;
  err_fn = 0;

  list<int>::iterator fit = label.fidx.begin();
  for( ; fit!=label.fidx.end(); fit++){
    if(ftrs_[*fit].ntrack >= nback){
      fn++;
      double mhl = getMhlDistanceGFlowandFFlow(label, ftrs_[*fit], nback);
      mhls.push_back(mhl);
      if(mhl > mthre) err_fn++;
    }}
  mhls.sort(greater<double>());
  list<double>::iterator mit = mhls.begin();
  for(unsigned int j=0; j<mhls.size()/2; j++) mit++;
  med_mhl = *mit;

  return true;
}


void MotionClassifierCore::removeStaticObjectsBadFlow(VOLData &label, int nback, double mthre){
  list<int>::iterator fit = label.fidx.begin();
  for( ; fit!=label.fidx.end(); ){
    if(ftrs_[*fit].ntrack >= nback){
      double mhl = getMhlDistanceGFlowandFFlow(label, ftrs_[*fit], nback);
      if(mhl > mthre){
	cout << "@debug remove bad flow: " << ftrs_[*fit].w_poss[0] << endl;
	ftrs_[*fit].reset();
	fit = label.fidx.erase(fit);
      }
      else fit++;
    }
    else fit++;
  }
}


double MotionClassifierCore::getMhlDistanceGFlowandFFlow(VOLData &label, VOPData &point, int nback){
  D33 allw_cov;
  for(int i=0; i<3; i++) allw_cov(i,i) = pow(150.0,2);

  D3 gflow = label.w_poss[0] - label.w_poss[nback];
  D3 fflow = point.w_poss[0] - point.w_poss[nback];
  D33 cov = allw_cov +
    point.w_covs[0] + point.w_covs[nback] + label.w_covs[0] + label.w_covs[nback];

  return (getMhlnbsDistance(cov.inverse(), gflow-fflow));
}





/******************************
 * 移動物体を特徴点が存在する領域において，再ラベリングする
 ******************************/
bool MotionClassifierCore::evalMovingObjectIdentity(VOLData &label, double size){
  //const static double size = 20.0; /* ラベリングの格子の大きさ */
  /* ラベルの領域の設定 */
  list<int>::iterator fit = label.fidx.begin();
  ColumnVector maxpos = ftrs_[*fit].rotpos.extract(0,1);
  ColumnVector minpos = ftrs_[*fit].rotpos.extract(0,1);
  for( ; fit!=label.fidx.end(); fit++){
    if( ftrs_[*fit].rotpos(0) > maxpos(0) ) maxpos(0) = ftrs_[*fit].rotpos(0);
    if( ftrs_[*fit].rotpos(1) > maxpos(1) ) maxpos(1) = ftrs_[*fit].rotpos(1);
    if( ftrs_[*fit].rotpos(0) < minpos(0) ) minpos(0) = ftrs_[*fit].rotpos(0);
    if( ftrs_[*fit].rotpos(1) < minpos(1) ) minpos(1) = ftrs_[*fit].rotpos(1);
  }

  /* ラベル領域の始点 */
  ColumnVector startpos(2,0);
  startpos(0) = size * static_cast<int>(minpos(0) / size);
  if( minpos(1) < 0 ) minpos(1) = minpos(1) - size;
  startpos(1) = size * static_cast<int>(minpos(1) / size);

  /* ラベル領域の数 */
  int nx = static_cast<int>((maxpos(0) - startpos(0))/size) + 1;
  int ny = static_cast<int>((maxpos(1) - startpos(1))/size) + 1;


  /* ラベリングクラスへの引数の設定 */
  unsigned char labeling_src[nx*ny];
  for(int i=0; i<nx*ny; i++) labeling_src[i] = static_cast<unsigned char>(0);
  for( fit=label.fidx.begin(); fit!=label.fidx.end(); fit++){
    int lid = getLabelingIDfromPos(ftrs_[*fit].rotpos, size, startpos, nx, ny);
    labeling_src[lid] = static_cast<unsigned char>(1);
  }
  short labeling_result[ny*nx];

  /* ラベリングの実行 */
  LabelingBS labeling;
  labeling.Exec(labeling_src, labeling_result ,ny, nx, false, 0);
  int labeln =labeling.GetNumOfResultRegions();

  /* 単体だった場合 */
  if( labeln == 1) return true;

  cout << "@debug mo is separated" << endl;



  /********************
   * 複数に分れた場合
   * 最も多いラベル以外をラベル０にする
   ********************/
  // /* 一度全てをstatic扱いにする(分れたどちらが移動物体かはわからないため) */
  // label.isStatic = true;
  // fit = label.fidx.begin();
  // for( ; fit!=label.fidx.end(); fit++){
  //   ftrs_[*fit].isStatic = true;
  // }


  /* ラベルに属する特徴点数 */
  int labelfn[labeln];
  for(int i=0; i<labeln; i++) labelfn[i] = 0;
  for( fit=label.fidx.begin(); fit!=label.fidx.end(); fit++ ){
    int lid = getLabelingIDfromPos(ftrs_[*fit].rotpos, size, startpos, nx, ny);
    //++labelfn[labeling_result[lid]];
    ++labelfn[-1+labeling_result[lid]];
    //printf("@debug hoge: result=%d\n", labeling_result[lid]);/* debug */
  }


  /* 属する特徴点が最も多いラベル */
  int max_id = 0;
  int max_fn = 0;
  for(int i=0; i<labeln; i++){
    if(labelfn[i] > max_fn){
      max_fn = labelfn[i];
      max_id = i;
    }}


  /* 最多特徴点数ラベル以外をラベル０に */
  for( fit=label.fidx.begin(); fit!=label.fidx.end(); ){
    int lid = getLabelingIDfromPos(ftrs_[*fit].rotpos, size, startpos, nx, ny);
    if(labeling_result[lid]-1 == max_id){
      fit++;
    } else{
      /* 特徴点側の更新 */
      ftrs_[*fit].id = 0;

      /* ラベルから削除 */
      fit = label.fidx.erase(fit);
    }}
  return false;
}


void MotionClassifierCore::updateMSValuesofLabel(VOLData &label){
  D3 gflow(0,0,0);
  D3 w_gpos(0,0,0);
  D33 w_gcov;
  {
    list<int>::iterator fit = label.fidx.begin();
    for( ; fit!=label.fidx.end(); fit++){
      gflow = gflow + D3(ftrs_[*fit].w_poss[0] - ftrs_[*fit].w_poss[1]);
      w_gpos = w_gpos + ftrs_[*fit].w_poss[0];
      w_gcov = w_gcov + ftrs_[*fit].w_covs[0].inverse();
    }
    double fn = static_cast<double>(label.fidx.size());
    gflow = gflow / fn;
    w_gpos = w_gpos / fn;
    w_gcov = w_gcov.inverse();
  }

  label.w_poss.push_front(gflow + label.w_poss[0]);
  label.w_covs.push_front(w_gcov);


  if(label.w_poss.size() > 101) label.w_poss.pop_back();
  if(label.w_covs.size() > 101) label.w_covs.pop_back();
}


void MotionClassifierCore::initMSValuesofLabel(VOLData &label){
  D3 w_gpos;
  D33 w_gcov;
  list<int>::iterator fit = label.fidx.begin();
  for( ; fit!=label.fidx.end(); fit++){
    w_gpos = w_gpos + ftrs_[*fit].w_poss[1];
    w_gcov = w_gcov + ftrs_[*fit].w_covs[1].inverse();
  }
  double fn = static_cast<double>(label.fidx.size());
  w_gpos = w_gpos / fn;
  w_gcov = w_gcov.inverse();

  label.w_poss.clear();
  label.w_covs.clear();
  label.w_poss.push_front(w_gpos);
  label.w_covs.push_front(w_gcov);
}



/* ftrs.rotposを元にcentroid,Cov, labeling_idsを更新 */
bool MotionClassifierCore::setLabelData(VOLData &label){
  /* 重心・共分散 */
  label.getCovariance(ftrs_);

  /* ラベリングの隣接チェック用 */
  label.labeling_ids.clear();
  list<int>::iterator it = label.fidx.begin();
  for( ; it!=label.fidx.end(); it++){
    int lid = getLabelingIDfromPos(ftrs_[*it].rotpos);
    if(lid >= 0) label.labeling_ids.push_back(lid);
  }
  label.labeling_ids.sort();
  label.labeling_ids.unique();

  return true;
}



bool MotionClassifierCore::checkAdj(list<int> ids, int tid){
  const static int ny = 20000 / LabelFrameSize_;
  //const static int nx = 10000 / LabelFrameSize_;

  list<int>::iterator it = ids.begin();
  for( ; it!=ids.end(); it++){
    int dx = (tid / ny) - (*it / ny);
    int dy = (tid % ny) - (*it % ny);
    if(abs(dx)<=1 && abs(dy)<=1) return true;
  }
  return false;
}



/* rotposの座標からラベリング分割領域でのIDを返す */
int MotionClassifierCore::getLabelingIDfromPos(const ColumnVector &rotpos){
  if( (rotpos(0) > 10000.0) || (abs(rotpos(1)) > 10000.0) ) return -1;


  const static int ny = 20000 / LabelFrameSize_;
  //const static int nx = 10000 / LabelFrameSize_;

  int lid = static_cast<int>(rotpos(0)/LabelFrameSize_) * ny;
  lid += static_cast<int>((rotpos(1)+10000)/ LabelFrameSize_);
  return lid;
}


/******************************
 *    end <==
 *    <======
 *    <==  st
 ******************************/
int MotionClassifierCore::getLabelingIDfromPos(const ColumnVector &rotpos, const double &size,
					     const ColumnVector &stpos,
					     const int &nx, const int &ny){
  int lid = static_cast<int>((rotpos(0) - stpos(0))/size) * ny;
  lid += static_cast<int>((rotpos(1) - stpos(1))/size);
  return lid;
}



void MotionClassifierCore::resetAllLabel(){
  for(int i=0; i<clst_maxn_; i++){
    labels_[i].reset();
    labels_[i].emptyp = true;
  }
  for(int i=0; i<max_fn_; i++){
    if(ftrs_[i].ntrack > 0){
      ftrs_[i].id = 0;
      labels_[0].fidx.push_back(i);
    }}
  //debug
  cout << "@resetAllLabel" << endl;
}


bool
MotionClassifierCore::GrantRigid(VOLData& label, list<double>& median_gap){
  int i,j,k;
  median_gap.clear();
  int lsize = static_cast<int>(label.fidx.size());
  double gaps[lsize][lsize];

  list<int>::iterator idx0 = label.fidx.begin();
  list<int>::iterator idx1 = label.fidx.begin();

  list<double> tmp;
  list<double>::iterator it;

  // 3次元特徴点の各距離の前フレームと現フレームとの差
  for(i=0; i<lsize; i++, idx0++){
    for(j=i, idx1=idx0; j<lsize; j++, idx1++){
      gaps[i][j] = gaps[j][i] =
	abs((ftrs_[(*idx0)].pre_pos -ftrs_[(*idx1)].pre_pos).norm(2.0) - (ftrs_[(*idx0)].cur_pos - ftrs_[(*idx1)].cur_pos).norm(2.0));
    }
    // 中央値の取得
    tmp.clear();
    for(j=0; j<lsize; j++){
      if(i!=j) tmp.push_back(gaps[i][j]);
    }
    tmp.sort(greater<double>());
    it = tmp.begin();
    for(k=0; k< lsize / 2; k++) it++;
    median_gap.push_back(*it);
  }
  return true;
}






int MotionClassifierCore::getNewLabels(){
  const int ny = 20000 / LabelFrameSize_;
  const int nx = 10000 / LabelFrameSize_;

  static unsigned char labeling_src[ny*nx];
  static short labeling_result[ny*nx];


  // /* debug */
  // //resetAllLabel();
  /* ラベル０を一度リセットして再登録 */
  labels_[0].fidx.clear();
  for(int i=0; i<max_fn_; i++){
    if(ftrs_[i].id == 0) labels_[0].fidx.push_back(i);
  }
  if(labels_[0].fidx.empty()) return -1;


  int fn = static_cast<int>(labels_[0].fidx.size());
  for(int i=0; i<nx*ny; i++) labeling_src[i] = static_cast<unsigned char>(0);
  list<int>::iterator fidx_it = labels_[0].fidx.begin();
  vector<int> l2f_idx;
  l2f_idx.reserve(fn);

  for( ; fidx_it!=labels_[0].fidx.end(); fidx_it++){
    // if(ftrs_[*fidx_it].rotpos(0) > 0 && ftrs_[*fidx_it].rotpos(0) < 10000 &&
    //    ftrs_[*fidx_it].rotpos(1) > -10000 && ftrs_[*fidx_it].rotpos(1) < 10000){
    int lid = getLabelingIDfromPos(ftrs_[*fidx_it].rotpos);
    if(lid > 0) labeling_src[lid] = (char) 1;
    else ftrs_[*fidx_it].reset(); /* 10m以上遠い点は信用しない */
  }

  /* ラベリングの実行 */
  LabelingBS labeling;
  labeling.Exec(labeling_src, labeling_result ,ny, nx, false, 0);

  /* ラベリング結果を特徴点の順番で取得 */
  vector<int> label_data;
  label_data.assign(max_fn_, -1);
  for(fidx_it=labels_[0].fidx.begin(); fidx_it!=labels_[0].fidx.end(); fidx_it++){
    if(ftrs_[*fidx_it].id==0){
      int lid = getLabelingIDfromPos(ftrs_[*fidx_it].rotpos);
      if(lid>0){
  	label_data[*fidx_it] = labeling_result[lid];
      }
      else label_data[*fidx_it] = -1;
    }
    else
      label_data[*fidx_it] = -1;
  }

  /* ラベリングのグループ数の取得 */
  int clst_n = labeling.GetNumOfResultRegions();

  /* iグループに属する特徴点のidxと数の取得 */
  vector<int> clsts[clst_n];
  for(int i=0; i<max_fn_; i++){
    if(label_data[i] > 0) clsts[label_data[i]-1].push_back(i); /* １スタートなので */
  }

  for(int i=0; i<clst_n; i++){
    if(clsts[i].size() < 4) { clsts[i].clear();}
  }
  int label_idx = 1;
  for(int i=0; i<clst_n; i++){
    if(!clsts[i].empty()){
      for( ; label_idx<clst_maxn_; label_idx++){
  	if(labels_[label_idx].emptyp){
  	  labels_[label_idx].emptyp = false;
  	  for(unsigned int j=0; j<clsts[i].size(); j++){ /* グループiに属するj番目*/
  	    ftrs_[clsts[i][j]].id = label_idx;
  	    labels_[label_idx].fidx.push_back(clsts[i][j]);
  	  }
  	  /* ラベルの登録 */
  	  labels_[label_idx].ntrack = 1;
  	  setLabelData(labels_[label_idx]);
	  initMSValuesofLabel(labels_[label_idx]);
	  updateMSValuesofLabel(labels_[label_idx]);

  	  // { /* debug */
  	  //   cout << "@debug label = " << label_idx << endl;
  	  //   list<int>::iterator it = labels_[label_idx].fidx.begin();
  	  //   for( ; it!=labels_[label_idx].fidx.end(); it++)
  	  //     cout << ftrs_[*it].rotpos.transpose() << endl;
  	  // }

  	  break;
  	}}}}
  return 0;
}



void MotionClassifierCore::reupdateLabels(){
  for(int i=1; i<clst_maxn_; i++){
    reupdateLabel(labels_[i]);
  }}


void MotionClassifierCore::reupdateLabel(VOLData& label){
  if(label.fidx.size() >= 4){
    setLabelData(label);
  }
  else{
    list<int>::iterator fit = label.fidx.begin();
    for( ; fit!=label.fidx.end(); fit++){
      ftrs_[*fit].id = 0;
    }
    label.reset();
  }}



void
MotionClassifierCore::fusionLabels(){
  //中間地点がマハラノビス距離のスレショルド以内＆
  //(最小固有値方向が近い or 最大固有値方向が近い)
  //const double kDThre = 7.815;
  //const double kDThre = 9.348;
  const double kDThre = 15.0;
  bool fusionp;

  ColumnVector  cen1, cen2, tmp, ceni2j;
  for(int i=1; i<clst_maxn_; i++){
    if(labels_[i].fidx.size() > 1){
      for(int j=i+1; j<clst_maxn_; j++){
	if(labels_[j].fidx.size() > 1){
	  fusionp = false;
	  /**** 基本パラム ****/
	  //法線
	  ColumnVector i_axis = labels_[i].evecs.column(1);
	  ColumnVector j_axis = labels_[j].evecs.column(1);
	  //重心
	  ColumnVector i_cen = labels_[i].centroid;
	  ColumnVector j_cen = labels_[j].centroid;

	  /**** 評価 ****/
	  // 法線の角度
	  double axis_val = abs(vector_ip(i_axis, j_axis));
	  // 平面の算出
	  double i_d =0; double j_d = 0;
	  for(int k=0; k<2; k++){
	    i_d -= i_axis(k)*i_cen(k);
	    j_d -= j_axis(k)*j_cen(k);
	  }
	  ColumnVector i_plane = i_axis.stack(ColumnVector(1, i_d));
	  ColumnVector j_plane = j_axis.stack(ColumnVector(1, j_d));

	  double i_height = getDistanceFromPntToPln(i_cen, j_plane);
	  double j_height = getDistanceFromPntToPln(j_cen, i_plane);
	  double hval = sqrt(i_height*i_height + j_height*j_height);

	  // 中間値のマハラノビス距離
	  ColumnVector tmp = (j_cen - i_cen) / 2;
	  double i_mahara = tmp.transpose()*labels_[i].invCov*tmp;//あやしい
	  double j_mahara = tmp.transpose()*labels_[j].invCov*tmp;

	  if(axis_val > 0.986 && hval < 200.0 && j_mahara < kDThre && i_mahara < kDThre){
	    fusionp = true;
	  }

	  if (fusionp){
	    printf("@fusionLabels fusioned\n");

	    // j=>i && jを消去 iを更新
	    if(labels_[i].ntrack >= labels_[i].ntrack){
	      list<int>::iterator iit = labels_[j].fidx.begin();
	      for(; iit!=labels_[j].fidx.end(); iit++){
		ftrs_[(*iit)].id=i;
		labels_[i].fidx.push_back(*iit);
	      }
	      labels_[j].reset();
	      labels_[i].getCovariance(ftrs_);
	    }
	    else{ // i=>j iを消去 jを更新 and break;
	      list<int>::iterator iit = labels_[i].fidx.begin();
	      for(; iit!=labels_[i].fidx.end(); iit++){
		ftrs_[(*iit)].id=j;
		labels_[j].fidx.push_back(*iit);
	      }
	      labels_[i].reset();
	      labels_[j].getCovariance(ftrs_);
	      break;
	    }}}}}}}



// 点と平面（直線）の距離 ax+by+cz+d=0のabcdがplaneに入る
double
MotionClassifierCore::getDistanceFromPntToPln(const ColumnVector &point, const ColumnVector &plane){
  if(point.rows() != plane.rows() -1){
    cerr << "@getDistanceFromPntToPln dimension is not good" << endl;
    return -1.0;
  }
  int row = point.rows();
  double height = 0;
  for(int i=0; i<row; i++) height += plane(i)*point(i);
  height += plane(row);
  height = abs(height);

  height /= plane.extract_n(0, row).norm(2.0);
  return height;
}



/******************************
 *
 * 移動物体分離
 *
 ******************************/
void MotionClassifierCore::detectMovingObjects(){
  /* ラベル０の情報の更新 */
  updateIndependLabel(labels_[0]);

  detectMOAmongLabels();

  // if(env_idx > 0){
  //   printf("@detectmo by environment\n");
  //   printf("idx = %d, fn = %d\n", env_idx, labels_[env_idx].fidx.size());
  //   detectMObyEnv(env_idx);
  //   //detectMObyCameraFlowWithEnv(env_idx);
  // } else if (labels_[0].fidx.size()>30){
  //   detectMObyEnv(0);
  // } else if (clst_n >10){
  //   detectMOAmongLabels();
  // } else{
  //   cerr << "@Estimate Motion can't calc Moving Detect" << endl;
  // }
}


void MotionClassifierCore::updateIndependLabel(VOLData &label){
  label.fidx.clear();
  for(int i=0; i<max_fn_; i++){
    if(ftrs_[i].id==0 && ftrs_[i].isStatic){
      labels_[0].fidx.push_back(i);
    }}

  if(label.fidx.size() < 10) label.reset();
  else{
    if(label.ntrack < 0){
      initMSValuesofLabel(label);
      label.ntrack = 0;
    }
    label.ntrack++;
    updateMSValuesofLabel(label);
  }
  /* updateMSValuesはただの平均なので共分散に応じて変える */
  // D3 vec;
  // list<int>::iterator fit = label.fidx.begin();
  // for( ; fit!=label.fidx.end(); fit++){
  //   D33 kcov = label.w_covs[0] * (ftrs_[*fit].w_covs[0]).inverse();
  //   vec = vec + kcov * (ftrs_[*fit].w_poss[0] - ftrs_[*fit].w_poss[1]);
  // }
  // label.w_poss[0] = vec;
}


int MotionClassifierCore::detectEnvironment(){
  const static double EnvEvalThre = 100.0*100.0;
  double max_eval = 0;
  int env_idx = 0;
  for(int i=1; i<clst_maxn_; i++){
    if(labels_[i].isStatic && labels_[i].fidx.size() > 15 && labels_[i].evals(0) > max_eval){
      max_eval = labels_[i].evals(0);
      env_idx = i;
    }}
  if(max_eval > EnvEvalThre)
    return env_idx;
  return -1;
}


int
MotionClassifierCore::detectMaxVarLabel(const double &min_var_thre, const int &min_fn){
  int ans_idx = -1;
  double max_eval = min_var_thre;
  for(int i=0; i<clst_maxn_; i++){
    if(static_cast<int>(labels_[i].fidx.size()) > min_fn
       && labels_[i].isStatic && labels_[i].evals(0) > max_eval){
      ans_idx = i;
      max_eval = labels_[i].evals(i);
    }}
  return ans_idx;
}



bool MotionClassifierCore::detectMOAmongLabels(){
  int cf[5] = {1, 3, 10, 30, 100};
  for(int cc=0; cc<5; cc++){
    int k = cf[cc];
    int lidx[clst_maxn_];
    int clst_n = 0;
    bool label0p_ = false;
    for(int i=0; i<clst_maxn_; i++){
      if(labels_[i].isStatic && labels_[i].ntrack>=k){
	if(i==0) label0p_ = true;
	lidx[clst_n] = i;
	clst_n++;
      }}

    /* グループ同士の運動の違い */
    int dim = clst_n;
    if(odometryp_ && odm_data_.ntrack>=k) dim++;
    Matrix DD(dim, dim, 0);
    Matrix DD2(dim, dim, 0);

    for(int i=0; i<clst_n; i++){
      for(int j=i+1; j<clst_n; j++){
	D3 dflow = labels_[lidx[i]].w_poss[0] - labels_[lidx[i]].w_poss[k];
	dflow = dflow - (labels_[lidx[j]].w_poss[0] - labels_[lidx[j]].w_poss[k]);

	D33 cov(pow(100, 2) * make_unit_matrix(3));
	cov = cov+
	  labels_[lidx[i]].w_covs[0] + labels_[lidx[i]].w_covs[k] +
	  labels_[lidx[j]].w_covs[0] + labels_[lidx[j]].w_covs[k];

	DD(i,j) = DD(j,i) = getMhlnbsDistance(cov.inverse(), dflow);
      }}

    /* オドメトリ情報との運動の違い */
    if(odometryp_ && odm_data_.ntrack>=k){
      for(int i=0; i<clst_n; i++){
	D3 dflow = odm_data_.w_poss[0] - odm_data_.w_poss[k];
	dflow = dflow - (labels_[lidx[i]].w_poss[0] - labels_[lidx[i]].w_poss[k]);
	D33 cov(pow(100, 2) * make_unit_matrix(3));
	cov = cov+
	  labels_[lidx[i]].w_covs[0] + labels_[lidx[i]].w_covs[k] +
	  odm_data_.w_covs[0] + odm_data_.w_covs[k];

	DD(i, clst_n) = DD(clst_n, i) = getMhlnbsDistance(cov.inverse(), dflow);
      }}

    /* 評価 */
    /* 下記のうち一つ選ぶ */

    /* 分散最大ラベルを静止物体として考え、それとの移動距離のずれがmthreを越していたら移動物体 */
    // //分散が最大となっているラベルを取得
    // int covsum[clst_n];
    // double max_cs = 0;
    // int max_cs_idx = -1;
    // for(int i=0; i< clst_n; i++){
    //   covsum[i] = labels_[lidx[i]].Cov(0,0)+labels_[lidx[i]].Cov(1,1)+labels_[lidx[i]].Cov(2,2);
    //   if(covsum[i] > max_cs){
    // 	max_cs = covsum[i];
    // 	max_cs_idx = i;
    //   }
    // }
    // double mthre = 10.0;
    // for(int i=0; i< clst_n; i++){
    //   if(i != max_cs_idx && DD(i, max_cs_idx) > mthre) {
    // 	regardDynamicLabel(lidx[i]);
    // 	cerr << "idx = " << lidx[i] << "is Dynamic" << endl;
    //   }
    // }

    // /* 他との移動距離のずれがmthreを越しているラベル個数割合がrthre以上なら移動物体 */
    // double mthre = 4.0;  //対人間10.0
    // double rthre = 0.4; //対人間0.8 -> 0.6
    // int mflg[clst_n];
    // for(int i=0; i<clst_n; i++){mflg[i] = 0;}
    // for(int i=0; i<clst_n; i++){
    //   for(int j=i+1; j<clst_n; j++){
    // 	if( DD(i,j) > mthre ){
    // 	  mflg[i]++;
    // 	  mflg[j]++;
    // 	}}}
    // for(int i=0; i<clst_n; i++){
    //   if(mflg[i] > clst_n * rthre && lidx[i]!=0){
    // 	regardDynamicLabel(lidx[i]);
    // 	cerr << "idx = " << lidx[i] << "is Dynamic" << endl;
    //   }
    // }

    /* 移動距離のずれがmthreを越している2つのラベルがあった場合、より他のと外れている方が移動物体 */
    double mthre = 10.0;
    for(int i=0; i<clst_n; i++){
      for(int j=i+1; j<clst_n; j++){
	if( DD(i,j) > mthre ){
	  double ave_i = 0;
	  double ave_j = 0;
	  for(int l=0; l<dim; l++){
	    ave_i += DD(i,l);
	    ave_j += DD(j,l);
	  }
	  cout << "@debug DD" << endl;
	  cout << DD << endl;
	  if(ave_i > ave_j && lidx[i]!=0){
	    regardDynamicLabel(lidx[i]);
	    cerr << "idx = " << lidx[i] << "is Dynamic" << endl;
	  }
	  else if (ave_j > ave_i){
	    regardDynamicLabel(lidx[j]);
	    cerr << "idx = " << lidx[j] << "is Dynamic" << endl;
	  }
	}}}
  }
  return true;
}


void MotionClassifierCore::regardDynamicLabel(int lidx){
  labels_[lidx].isStatic = false;
  list<int>::iterator it = labels_[lidx].fidx.begin();
  for( ; it!=labels_[lidx].fidx.end(); it++) ftrs_[*it].isStatic = false;
}


/********************
 *
 * Motion Estimation
 *
 ********************/
bool MotionClassifierCore::estimateMotion(coords &mco, Matrix &Cov){
  int loop_n = 10;
  unsigned int fn = pres_.size();
  //printf("@debug esti fn = %d\n", fn);
  if(fn > 9){
    pme_.estMotion(pres_, curs_, SCovs_, iSCovs_, iSQCovs_, loop_n, mco, Cov); /* debug */
    //mco = estmt_.estMotion(pres_, curs_, loop_n); /* debug */
    return true;
  }

  return false;
}


int MotionClassifierCore::makeEstimateDatas(){
  pres_.clear();
  curs_.clear();
  SCovs_.clear();
  iSCovs_.clear(); iSQCovs_.clear();
  wposs_.clear(); WCovs_.clear();

  int fn = 0;
  for(int i=0; i<max_fn_; i++){
    if(ftrs_[i].ntrack > 0 && ftrs_[i].isStatic) fn++;
  }


  pres_.reserve(fn); curs_.reserve(fn);
  SCovs_.reserve(fn); iSCovs_.reserve(fn); iSQCovs_.reserve(fn);
  wposs_.reserve(fn); WCovs_.reserve(fn);

  // int fn = 0;
  vector<VOPData>::iterator it = ftrs_.begin();

  for( ; it!=ftrs_.end(); it++){
    if((*it).ntrack>0 &&(*it).id!=-1 && (*it).isStatic
       && ( (*it).ntrack > 30 || (*it).cur_pos.norm(2.0) > 1500.0  )
       ){
      pres_.push_back((*it).pre_pos);
      curs_.push_back((*it).cur_pos);
      SCovs_.push_back((*it).l_cov);
      iSCovs_.push_back((*it).l_icov);
      iSQCovs_.push_back((*it).iSQCov);
      wposs_.push_back((*it).worldpos);
      WCovs_.push_back((*it).worldcov);
      fn++;
    }}

  //if(fn < 50) cout << "@debug fn = " << fn << endl;
  return fn;
}


int MotionClassifierCore::updateEstimateDatas(){
  vector<VOPData>::iterator it = ftrs_.begin();
  int fn = 0;
  for(int i=0; it!=ftrs_.end(); it++, i++){
    if((*it).ntrack>0 &&(*it).id!=-1 && (*it).isStatic
       && ( (*it).ntrack > 30 || (*it).cur_pos.norm(2.0) > 1500.0  ) //
       ){
      (*it).worldpos = wposs_[fn];
      (*it).worldcov = WCovs_[fn];
      fn++;
    }}
  return fn;
}


// EKFTranslationで一定以上離れている世界座標は信用ならないのでカウントリセット??
void MotionClassifierCore::removeErrorWorldMatch(vector<VOPData>& ftrs, vector<double>& eval_gaps,
					       vector<int>& ftrs_idx){
  // double kThre = 500.0;
  // int size = static_cast<int>(ftrs_idx.size());
  // int idx;
  // for(int i=0; i<size; i++){
  //   if(eval_gaps[i] > kThre){ // remove
  //     idx = ftrs_idx[i];
  //     ftrs_[idx].reset();
  //   }
  // }
}

void MotionClassifierCore::writeResultCoords(double *out_coords){
  /* 座標系 */
  ColumnVector tmpco = estco_.to_vector();
  for(int i=0; i<7; i++){
    out_coords[i] = static_cast<double>(tmpco(i));
  }
}

void MotionClassifierCore::writeResultFeatures(double *out_ftrs){
  /* 特徴点 */
  int idx;
  for(int i=0; i<max_fn_; i++){
    idx = 9 * i;

    if( ftrs_[i].ntrack > 0 ){
      out_ftrs[idx+0] = static_cast<double>(ftrs_[i].ntrack);
      out_ftrs[idx+1] = static_cast<double>(ftrs_[i].id);

      if(ftrs_[i].isStatic == true) out_ftrs[idx+2] = 0;
      else out_ftrs[idx+2] = 1;

      for(int j=0; j<3; j++){
	out_ftrs[idx+3+j] = static_cast<double>(ftrs_[i].worldpos(j));
	out_ftrs[idx+6+j] = static_cast<double>(ftrs_[i].cur_pos(j));
      }}
    else{
      //out_ftrs[idx+0] = out_ftrs[idx+1] = -1;
      for(int j=0; j<9; j++) out_ftrs[idx+j] = 0;
    }}
}


void MotionClassifierCore::setEstimationOptions(int kalman_type, bool separatep){
}


void MotionClassifierCore::detectMObyCameraFlowWithEnv(int env_idx){
  // // list<double> env_dus, env_dvs;
  // // { list<int>::iterator it = labels_[env_idx].fidx.begin();
  // //   for( ; it!=labels_[env_idx].fidx.end(); it++){
  // //     env_dus.push_back(ftrs_[*it].d_uv(0));
  // //     env_dvs.push_back(ftrs_[*it].d_uv(1));
  // //   }}
  // // env_dus.sort( greater<double>() );
  // // env_dvs.sort( greater<double>() );

  // // ColumnVector base_duv(2,0);
  // // { list<double>::iterator it = env_dus.begin();
  // //   for(int i=0; i<static_cast<int>(env_dus.size())/2; i++) it++;
  // //   base_duv(0) = (*it);
  // //   it = env_dvs.begin();
  // //   for(int i=0; i<static_cast<int>(env_dvs.size())/2; i++) it++;
  // //   base_duv(1) = (*it);
  // // }

  // // for(int i=0; i<clst_maxn_; i++){
  // //   /* 各ラベルに対して */
  // //   if(i!=env_idx && !labels_[i].fidx.empty()){
  // //     list<double> vals;
  // //     { list<int>::iterator it = labels_[i].fidx.begin();
  // // 	for( ; it!=labels_[i].fidx.end(); it++){
  // // 	  vals.push_back((ftrs_[*it].d_uv -base_duv).norm(2.0));
  // // 	}}
  // //     list<double>::iterator it = vals.begin();
  // //     for(unsigned int j=0; j<vals.size()/2; j++) it++;
  // //     double val = *it;

  // //     const static double Thre = 5;
  // //     printf("@bycamera %d th duv val is %f\n",i , val*500);
  // //   }
  // // }
}






