#include <ceres/ceres.h>
#include <iostream>
#include<time.h>
#include <sophus/se3.hpp>

#include "local_parameterization_se3.hpp"

#include "glog/logging.h"
#include <vector>
#include <fstream>
#include <Eigen/Core>

#include <iomanip>
#include <algorithm>
#include<math.h>

// #define NUM_PAIRS 200
// #define NUM_PAIRS_NEEDED 50
// #define NUM_LOOPS_NEEDED 1770
// #define NUM_CAMERAS 1770

#define SIGMA 0.5
#define NUM_ITERATION_EM 50
#define PI 3.1415926535897
#define Median_inlier_loss   3                //调节公式(13)中的圆符号

#define PARETO_ALPHA 1

#define ZIQUAN true
#define NUM_FEATURE_MATCHES_PER_CONSTRAINT 200
#define TRUST_ODOMETRY 0.9

// typedef std::pair< int, int > IntPair;
typedef Eigen::Matrix< double, 6, 6, Eigen::RowMajor > InformationMatrix;
typedef Eigen::Matrix< double, 6, 1> Vector6d;
typedef Eigen::Matrix< double, Eigen::Dynamic, 7, Eigen::RowMajor > PairMatrix;

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;

/*
  ZHOU's convension
*/
std::vector<std::vector<double>>  semantic_p;    //保存语义混淆矩阵

  void LoadFromFile_semantic_p(const char*  filename ) {
    semantic_p.clear();
    semantic_p.resize(24);
    for(int i=0;i<24;i++)
    {
      semantic_p[i].resize(24);
    }
    FILE * f = fopen( filename, "r" );
    int i=0;
    if ( f != NULL ) {
      char buffer[1024];
      while ( fgets( buffer, 1024, f ) != NULL &&i<24)
      {
        if ( strlen( buffer ) > 0 && buffer[ 0 ] != '#' )
        {
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",  \
           &semantic_p[i][0],&semantic_p[i][1],&semantic_p[i][2], &semantic_p[i][3], &semantic_p[i][4], &semantic_p[i][5],\
          &semantic_p[i][6],&semantic_p[i][7],&semantic_p[i][8], &semantic_p[i][9], &semantic_p[i][10], &semantic_p[i][11],\
          &semantic_p[i][12],&semantic_p[i][13],&semantic_p[i][14], &semantic_p[i][15], &semantic_p[i][16], &semantic_p[i][17],\
          &semantic_p[i][18],&semantic_p[i][19],&semantic_p[i][20], &semantic_p[i][21], &semantic_p[i][22], &semantic_p[i][23]);
        }
        i++;
      }
      fclose( f );
    }
  }

struct FramedMatches {
  int id1_;
  int id2_;
  int frame_;
  int old_correspondence_num_;
  int new_correspondence_num_;
  float ratio_;
  Eigen::Matrix4d transformation_;
  std::vector< std::pair<Eigen::Vector3d, Eigen::Vector3d> > pairs_;
  std::vector<int> semantic_label;          //存储每个点对 语义是否对应的标志位
  std::vector<int> label_one_;         //每对对应点第一个点的语义标签值
  std::vector<int> label_two_;        //每对对应点第二个点的语义标签值
  FramedMatches( int id1, int id2, int f, int old_cor,int new_cor,float ratio,Eigen::Matrix4d trans, PairMatrix pairs,int label_one,int label_two)
  : id1_(id1), id2_(id2), frame_(f), old_correspondence_num_(old_cor),new_correspondence_num_(new_cor),ratio_(ratio),transformation_(trans)
  {
    std::cout<<"here"<<std::endl;
    int num_pairs = pairs.rows();
    pairs_.resize(num_pairs);
    semantic_label.resize(num_pairs);
    label_one_.resize(num_pairs);
    label_two_.resize(num_pairs);
    for (int i = 0; i < num_pairs;  i++) {
      pairs_[i] = std::make_pair(Eigen::Vector3d(pairs(i,0), pairs(i,1), pairs(i,2)),
                                 Eigen::Vector3d(pairs(i,3), pairs(i,4), pairs(i,5)));
      semantic_label[i]=pairs(i,6);
      label_one_[i]=label_one;
      label_two_[i]=label_two;
    }
  }
};

struct PCLMatches {
  std::vector< FramedMatches > data_;

  void LoadFromFile(const char* filename) {// , int num = -1, float min_ratio = -1) {
    //std::cout<<"here"<<std::endl;
    data_.clear();
    int id1, id2, frames=22, old_correspondence_num, new_correspondence_num;
    float ratio;
    Eigen::Matrix4d trans;
    PairMatrix pairs;
    int label_one,label_two;   //记录每对对应点的语义标签值
    FILE * f = fopen( filename, "r" );
    if ( f != NULL ) {
      char buffer[1024];
      // int counter = 0;
      // while ( (num < 0 || counter <= num) && fgets( buffer, 1024, f ) != NULL ) {
      while ( fgets( buffer, 1024, f ) != NULL ) {
        if ( strlen( buffer ) > 0 && buffer[ 0 ] != '#' ) {
          // if (ZIQUAN) {       ratio=new_correspondence_num/old_correspondence_num
            sscanf( buffer, "%d %d %d %d %f", &id1, &id2, &old_correspondence_num, &new_correspondence_num, &ratio);
          // } else {
            // sscanf( buffer, "%d %d %d %d %d %f", &id1, &id2, &frames, &count_in, &count_out, &ratio);
          // }
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf", &trans(0,0), &trans(0,1), &trans(0,2), &trans(0,3) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf", &trans(1,0), &trans(1,1), &trans(1,2), &trans(1,3) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf", &trans(2,0), &trans(2,1), &trans(2,2), &trans(2,3) );
          trans(3,0) = 0; trans(3,1) = 0; trans(3,2) = 0; trans(3,3) = 1;
          // if (ZIQUAN) {
          //   count_in = 200;
          // }
          pairs.resize(old_correspondence_num, 7);

          for (int i = 0; i < old_correspondence_num; i++) {
            fgets( buffer, 1024, f );
            sscanf( buffer, "%lf %lf %lf %lf %lf %lf %lf %d %d", &pairs(i,0), &pairs(i,1), &pairs(i,2), &pairs(i,3), \
                                                           &pairs(i,4), &pairs(i,5), &pairs(i,6),&label_one,&label_two);
          }
          // if (ratio > min_ratio) {
            std::cout << "here" << std::endl;
            data_.push_back( FramedMatches( id1, id2, frames, old_correspondence_num,new_correspondence_num,ratio,trans, pairs ,label_one,label_two) );
            // counter ++ ;
          // }
          std::cout << id1 << " and " << id2  << std::endl;
        }
      }
      fclose( f );
    }
  }
};

struct FramedTransformation {
  int id1_;
  int id2_;
  int frame_;
  Eigen::Matrix4d transformation_;      // pose
  Sophus::SE3d transformation_se3_;     // useful for both pose and link
  // std::vector< std::pair<Eigen::Vector3d, Eigen::Vector3d> > pairs_;

  FramedTransformation( int id1, int id2, int f, Eigen::Matrix4d t)
    : id1_( id1 ), id2_( id2 ), frame_( f ), transformation_( t )
  {
    Eigen::Quaterniond q;
    q = t.block<3,3>(0,0);
    transformation_se3_ = Sophus::SE3d(q, Sophus::SE3d::Point(t(0,3), t(1,3), t(2,3)));
    // pairs_.resize(NUM_PAIRS_NEEDED);
    // for (int i = 0; i < NUM_PAIRS_NEEDED; i++) {
    //   pairs_[i] = std::make_pair(Eigen::Vector3d(pairs(i,0), pairs(i,1), pairs(i,2)),
    //                              Eigen::Vector3d(pairs(i,3), pairs(i,4), pairs(i,5)));
    // }
  }

  // only use this constructer to check for convergence
  FramedTransformation(int id1, int id2, int f, Sophus::SE3d t)
    : id1_( id1 ), id2_( id2 ), frame_( f ), transformation_se3_( t )
    {
      transformation_ = t.matrix();
    }
};

struct PCLTrajectory {
  std::vector< FramedTransformation > data_;
  int index_;

  void LoadFromFile(const char* filename ) {
    data_.clear();
    index_ = 0;
    int id1, id2, frame;
    Eigen::Matrix4d trans;
    FILE * f = fopen( filename, "r" );
    if ( f != NULL ) {
      char buffer[1024];
      int i=0;
      while ( fgets( buffer, 1024, f ) != NULL ) {
        if ( strlen( buffer ) > 0 && buffer[ 0 ] != '#' ) {
          sscanf( buffer, "%d %d %d", &id1, &id2, &frame);
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf", &trans(0,0), &trans(0,1), &trans(0,2), &trans(0,3) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf", &trans(1,0), &trans(1,1), &trans(1,2), &trans(1,3) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf", &trans(2,0), &trans(2,1), &trans(2,2), &trans(2,3) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf", &trans(3,0), &trans(3,1), &trans(3,2), &trans(3,3) );
          data_.push_back( FramedTransformation( id1, id2, frame, trans ) );
          i++;
          // trans(3,0) = 0; trans(3,1) = 0; trans(3,2) = 0; trans(3,3) = 1;
          // for (int i = 0; i < NUM_PAIRS; i++) {
          //   fgets( buffer, 1024, f );
          //   sscanf( buffer, "%lf %lf %lf %lf %lf %lf %lf", &pairs(i,0), &pairs(i,1), &pairs(i,2), &pairs(i,3),
          //                                                  &pairs(i,4), &pairs(i,5), &pairs(i,6));
          // }
          // if (ratio > min_ratio) {
          //   data_.push_back( FramedTransformation( id1, id2, 1770, trans ) );
          //   counter ++ ;
          // }
          // std::cout << id1 << "\t" << id2  << std::endl;
        }
      }
      std::cout << "pose num is:"<<data_.size()  << std::endl;
      std::cout << "i is:"<<i  << std::endl;
      fclose( f );
    }
  }

  void SaveToFile(const char* filename ) {
    FILE * f = fopen( filename, "w" );
    for ( int i = 0; i < ( int )data_.size(); i++ ) {
      Sophus::SE3d trans_se3 = data_[ i ].transformation_se3_;
      Eigen::Matrix4d trans = trans_se3.matrix();
      fprintf( f, "%d\t%d\t%d\n", data_[ i ].id1_, data_[ i ].id2_, data_[ i ].frame_ );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(0,0), trans(0,1), trans(0,2), trans(0,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(1,0), trans(1,1), trans(1,2), trans(1,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(2,0), trans(2,1), trans(2,2), trans(2,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(3,0), trans(3,1), trans(3,2), trans(3,3) );
    }
    fclose( f );
  }

};

struct FramedInformation {
  int id1_;
  int id2_;
  int frame_;
  InformationMatrix information_;
  FramedInformation( int id1, int id2, int f, InformationMatrix t )
    : id1_( id1 ), id2_( id2 ), frame_( f ), information_( t )
  {}
};

struct PCLInformation {
  std::vector< FramedInformation > data_;

  void LoadFromFile(const char*  filename ) {
    data_.clear();
    int id1, id2, frame;
    InformationMatrix info;
    FILE * f = fopen( filename, "r" );
    if ( f != NULL ) {
      char buffer[1024];
      while ( fgets( buffer, 1024, f ) != NULL ) {
        if ( strlen( buffer ) > 0 && buffer[ 0 ] != '#' ) {
          sscanf( buffer, "%d %d %d", &id1, &id2, &frame);
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(0,0), &info(0,1), &info(0,2), &info(0,3), &info(0,4), &info(0,5) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(1,0), &info(1,1), &info(1,2), &info(1,3), &info(1,4), &info(1,5) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(2,0), &info(2,1), &info(2,2), &info(2,3), &info(2,4), &info(2,5) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(3,0), &info(3,1), &info(3,2), &info(3,3), &info(3,4), &info(3,5) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(4,0), &info(4,1), &info(4,2), &info(4,3), &info(4,4), &info(4,5) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(5,0), &info(5,1), &info(5,2), &info(5,3), &info(5,4), &info(5,5) );
          data_.push_back( FramedInformation( id1, id2, frame, info ) );
          // std::cout << id1 << "\t" << id2  << std::endl;
        }
      }
      fclose( f );
    }
  }
};

// Eigen's ostream operator is not compatible with ceres::Jet types.
// In particular, Eigen assumes that the scalar type (here Jet<T,N>) can be
// casted to an arithmetic type, which is not true for ceres::Jet.
// Unfortunatly, the ceres::Jet class does not define a conversion
// operator (http://en.cppreference.com/w/cpp/language/cast_operator).
//
// This workaround creates a template specilization for Eigen's cast_impl,
// when casting from a ceres::Jet type. It relies on Eigen's internal API and
// might break with future versions of Eigen.
namespace Eigen {
namespace internal {

template <class T, int N, typename NewType>
struct cast_impl<ceres::Jet<T, N>, NewType> {
  EIGEN_DEVICE_FUNC
  static inline NewType run(ceres::Jet<T, N> const& x) {
    return static_cast<NewType>(x.a);
  }
};

}  // namespace internal
}  // namespace Eigen

struct TestSE3CostFunctor {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  TestSE3CostFunctor(Sophus::SE3d T_aw) : T_aw(T_aw) {}

  template <class T>
  bool operator()(T const* const sT_wa, T* sResiduals) const {
    Eigen::Map<Sophus::SE3<T> const> const T_wa(sT_wa);
    Eigen::Map<Eigen::Matrix<T, 6, 1> > residuals(sResiduals);

    // We are able to mix Sophus types with doubles and Jet types withou needing
    // to cast to T.
    residuals = (T_aw * T_wa).log();
    // std::cout << residuals << std::endl;

    // Reverse order of multiplication. This forces the compiler to verify that
    // (Jet, double) and (double, Jet) SE3 multiplication work correctly.
    // residuals = (T_wa * T_aw).log();

    // Finally, ensure that Jet-to-Jet multiplication works.
    // residuals = (T_wa * T_aw.cast<T>()).log();
    return true;
  }

  Sophus::SE3d T_aw;
};

struct TestPointCostFunctor {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  TestPointCostFunctor(Sophus::SE3d T_aw, Eigen::Vector3d point_a)
      : T_aw(T_aw), point_a(point_a) {}

  template <class T>
  bool operator()(T const* const sT_wa, T const* const spoint_b,
                  T* sResiduals) const {
    using Vector3T = Eigen::Matrix<T, 3, 1>;
    Eigen::Map<Sophus::SE3<T> const> const T_wa(sT_wa);
    Eigen::Map<Vector3T const> point_b(spoint_b);
    Eigen::Map<Vector3T> residuals(sResiduals);

    // Multiply SE3d by Jet Vector3.
    Vector3T point_b_prime = T_aw * point_b;
    // Ensure Jet SE3 multiplication with Jet Vector3.
    // point_b_prime = T_aw.cast<T>() * point_b;

    // Multiply Jet SE3 with Vector3d.
    Vector3T point_a_prime = T_wa * point_a;
    // Ensure Jet SE3 multiplication with Jet Vector3.
    // point_a_prime = T_wa * point_a.cast<T>();

    residuals = point_b_prime - point_a_prime;
    return true;
  }

  Sophus::SE3d T_aw;
  Eigen::Vector3d point_a;
};

struct GaussianFunctor {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  GaussianFunctor(Sophus::SE3d trans_next_to_current, InformationMatrix info, int id1, int id2)
  : trans_next_to_current_(trans_next_to_current), info_(info), id1_(id1), id2_(id2) {}

  template <typename T>
  bool operator()(T const* const sT_current, T const* const sT_next,
                  T* residual) const {
    // using Vector3T = Eigen::Matrix<T, 3, 1>;
    Eigen::Map<Sophus::SE3<T> const> const T_current(sT_current);
    Eigen::Map<Sophus::SE3<T> const> const T_next(sT_next);
    // Eigen::Map<Vector3T> residuals(sResiduals);

    // Eigen::Matrix<T,4,4> M_xi = (trans_next_to_current_ * T_next.inverse() * T_current).matrix();
    Sophus::SE3<T> xi_7T = trans_next_to_current_
                          * T_next.inverse()
                          * T_current;

    Eigen::Matrix<T,6,1> xi;
    // xi << M_xi(0,3), M_xi(1,3), M_xi(2,3), q.x(), q.y(), q.z();
    xi << xi_7T.data()[4], xi_7T.data()[5], xi_7T.data()[6], xi_7T.data()[0], xi_7T.data()[1], xi_7T.data()[2];

    Eigen::Matrix<T,1,1> sq_error = xi.transpose() * info_ * xi;
    // double sq_error = (xi.transpose() * info_ * xi)[0];

    residual[0] = T( sqrt(sq_error(0,0)) );
    // residual[0] = T( sqrt(sq_error(0,0) / T(2.)) / T(SIGMA) );

    // std::cout << id1_ << ", " << id2_ << " : " << residual[0] << std::endl;
    return true;
  }

private:
  const Sophus::SE3d trans_next_to_current_;
  const InformationMatrix info_;
  const int id1_, id2_;
};

struct CauchyFunctor {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  CauchyFunctor(Eigen::Vector3d point_p, Eigen::Vector3d point_q,
                int id1, int id2)
  : point_p_(point_p), point_q_(point_q),
    id1_(id1), id2_(id2) {}

  template <typename T>
  bool operator()(T const* const sT_current, T const* const sT_next,
                  T* sResiduals) const {
    // using Vector3T = Eigen::Matrix<T, 3, 1>;
    Eigen::Map<Sophus::SE3<T> const> const T_current(sT_current);
    Eigen::Map<Sophus::SE3<T> const> const T_next(sT_next);
    Eigen::Map<Eigen::Matrix<T, 3, 1> > residuals(sResiduals);

    residuals = (T_current * point_p_ - T_next * point_q_);

    return true;
  }

private:
  const Eigen::Vector3d point_p_, point_q_;
  const int id1_, id2_;
};

class R2EM_CauchyUniform {
public:
  // lambda
  double lambda_;
  double U;
  // double u_;
  Eigen::Matrix<double, 6, 6> covarance_;

  // read from file
  // PCLTrajectory odometry_log_;
  // PCLInformation odometry_info_;
  PCLMatches odometry_txt_;
  // PCLTrajectory loop_log_;
  PCLMatches loop_txt_;
  // PCLInformation loop_info_;

  // blocks for ceres
  PCLTrajectory camera_poses_;
  std::vector< std::vector<double> > L_;
  double sum_L_;

  // convergence condition
  double last_lambda_;
  PCLTrajectory last_camera_poses_;
  Eigen::Matrix<double, 6, 6> last_covarance_;

  R2EM_CauchyUniform(double lambda)
  : lambda_(lambda) {
    last_lambda_ = -1.;
  }

  ~R2EM_CauchyUniform() {}

  // void LoadOdometryLog(const char* filename) {
  //   odometry_log_.LoadFromFile(filename);
  // }

  // void LoadOdometryInfo(const char* filename) {
  //   odometry_info_.LoadFromFile(filename);
  // }

  void LoadOdometryTxt(const char* filename) {
    odometry_txt_.LoadFromFile(filename);//, -1);
  }

  void LoadLoopTxt(const char* filename) {
    loop_txt_.LoadFromFile(filename);//, -1);
  }

  // void LoadLoopInfo(const char* filename) {
  //   loop_info_.LoadFromFile(filename);
  // }

  void InitCameraPosesWithZhou(const char* filename) {
    camera_poses_.LoadFromFile(filename);
    L_.resize(NumPoses());
    for (int i = 0; i < NumPoses(); i++) {
      L_[i].resize(NumPoses());
    }
  }

  void InitCameraPoses() {
    camera_poses_.data_.clear();
    camera_poses_.index_ = 0;
    Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
    camera_poses_.data_.push_back( FramedTransformation( 0, 0, 1, pose ) );

    for( std::vector< FramedMatches >::iterator it = odometry_txt_.data_.begin();
      it != odometry_txt_.data_.end(); it ++)
    {
      pose = pose * it->transformation_;
      camera_poses_.data_.push_back( FramedTransformation( it->id2_, it->id2_, it->id2_ + 1, pose ) );
    }
    // std::cout << "odometry_log_.data_.size(): " << odometry_log_.data_.size() << std::endl;

    L_.resize(NumPoses());
    for (int i = 0; i < NumPoses(); i++) {
      L_[i].resize(NumPoses());
    }

  };

  void SaveCameraPoses(const char* filename) {
    camera_poses_.SaveToFile(filename);
  }

  void EstimateLAMBDA() {

    std::cout << "In EstimateLAMBDA : " << NumOdometryConstraints() << std::endl;
    std::vector<double> average_loss(NumOdometryConstraints());
    std::vector<double> total_loss(NumOdometryConstraints());
    for (int i = 0; i < NumOdometryConstraints(); i ++) {

      FramedMatches match = odometry_txt_.data_[i];
      // FramedInformation info = odometry_info_.data_[i];

      assert(match.id1_ < match.id2_);
      int id1 = match.id1_;
      int id2 = match.id2_;

      std::cout << "In EstimateLAMBDA : " << i << "th OdometryConstraint " << id1 << " with " << id2 << std::endl;

      Eigen::Quaterniond q;
      q = match.transformation_.block<3,3>(0,0);
      Sophus::SE3d transformation_se3_ = Sophus::SE3d(q, Sophus::SE3d::Point(match.transformation_(0,3), match.transformation_(1,3), match.transformation_(2,3)));

      // compute the sum of cauchy loss
      double sum_cauchy_loss = 0.;
      for (std::size_t j = 0; j < match.pairs_.size(); j++) {
        Eigen::Vector3d vec_d = camera_poses_.data_[id1].transformation_se3_ * match.pairs_[j].first
                              - camera_poses_.data_[id2].transformation_se3_ * match.pairs_[j].second;
        sum_cauchy_loss += log (1. + vec_d.squaredNorm() / (SIGMA * SIGMA));
        // std::cout << j << "/" << loop_log_.data_.size() << " : " << sum_cauchy_loss << std::endl;
      }
      assert(match.pairs_.size() > 0);
      average_loss[i] = sum_cauchy_loss / match.pairs_.size();
      total_loss[i] = sum_cauchy_loss;
      std::cout << "match.pairs_.size() is " << match.pairs_.size() << '\n';
      std::cout << "sum_cauchy_loss " << sum_cauchy_loss << '\n';
      std::cout << "average_loss[i] " << average_loss[i] << '\n';
    }

    for (int i = 0; i < NumOdometryConstraints(); i ++) {
      FramedMatches match = odometry_txt_.data_[i];
      // FramedInformation info = odometry_info_.data_[i];

      assert(match.id1_ < match.id2_);
      int id1 = match.id1_;
      int id2 = match.id2_;

      // double expterm = exp( - log(U) + (1. + PARETO_ALPHA) * log(average_loss[i]));
      double expterm = exp( - log(U) + (1. + PARETO_ALPHA) * (average_loss[i]));
      // L_[id1][id2] = 1./ (1. + expterm);
      std::cout << "(" << std::setw(3) << id1 << "," << std::setw(3) << id2 << ") : "
                << std::setw(15) << average_loss[i] << "(" <<  total_loss[i] << ")"
                << std::setw(15) << average_loss[i] * average_loss[i]
                << std::setw(15) << expterm <<  " --> "
                << std::setw(15) << 1./ (1. + expterm)
                << std::setw(15) << match.pairs_.size()
                // << std::setw(15) << U_big
                << std::endl;
    }
    std::nth_element(average_loss.begin(), average_loss.begin() + average_loss.size()/2, average_loss.end());
    std::nth_element(total_loss.begin(), total_loss.begin() + total_loss.size()/2, total_loss.end());
    // double median_inlier_loss = average_loss[average_loss.size()/2];

    double median_inlier_loss = exp(2 * average_loss[average_loss.size()/2]);
    std::cout << "average_loss[average_loss.size()/2] " << average_loss[average_loss.size()/2] << '\n';

    // double median_total_loss = total_loss[average_loss.size()/2];
    std::cout << "The median is " << median_inlier_loss << '\n';
    // U =  TRUST_ODOMETRY / (1-TRUST_ODOMETRY) * median_inlier_loss * median_inlier_loss;
    U =  TRUST_ODOMETRY / (1-TRUST_ODOMETRY) * median_inlier_loss;
    // double U_big =  TRUST_ODOMETRY / (1-TRUST_ODOMETRY) * median_total_loss * median_total_loss;

  }

  void SaveLinks(const char* filename) {
    FILE * f = fopen( filename, "w" );

    for ( int i = 0; i < NumOdometryConstraints(); i++ ) {
      Eigen::Matrix4d trans = odometry_txt_.data_[ i ].transformation_;
      // Eigen::Matrix4d trans = trans_se3.matrix();
      fprintf( f, "%d\t%d\t%d\n", odometry_txt_.data_[ i ].id1_, odometry_txt_.data_[ i ].id2_, odometry_txt_.data_[ i ].frame_ );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(0,0), trans(0,1), trans(0,2), trans(0,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(1,0), trans(1,1), trans(1,2), trans(1,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(2,0), trans(2,1), trans(2,2), trans(2,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(3,0), trans(3,1), trans(3,2), trans(3,3) );
    }

    for (int i = 0; i < NumLoopClosureConstraints(); i++ ) {
      Eigen::Matrix4d trans = loop_txt_.data_[ i ].transformation_;
      int id1 = loop_txt_.data_[ i ].id1_;
      int id2 = loop_txt_.data_[ i ].id2_;
      assert(id1 < id2);
      if (id1 != id2 - 1 && L_[id1][id2] > 0.8 * TRUST_ODOMETRY) {
        fprintf( f, "%d\t%d\t%d\n", loop_txt_.data_[ i ].id1_, loop_txt_.data_[ i ].id2_, loop_txt_.data_[ i ].frame_ );
        fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(0,0), trans(0,1), trans(0,2), trans(0,3) );
        fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(1,0), trans(1,1), trans(1,2), trans(1,3) );
        fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(2,0), trans(2,1), trans(2,2), trans(2,3) );
        fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(3,0), trans(3,1), trans(3,2), trans(3,3) );
      }
    }
    fclose( f );
  }

// Expectation steps
  void Expectation(int iteration_num) {

    sum_L_ = 0.;
    std::vector<int> healthy_counter(20);
    for (int ii = 0; ii < 20; ii ++) {
      healthy_counter[ii] = 0;
    }

  int maxnum_loop_corres=0;
  //寻找所有loop中最大对应点对数
  for (int i = 0; i < NumLoopClosureConstraints(); i ++)
  {
    FramedMatches match = loop_txt_.data_[i];
    if(match.old_correspondence_num_>maxnum_loop_corres)
    {
      maxnum_loop_corres = match.old_correspondence_num_;
    }
  }

    for (int i = 0; i < NumLoopClosureConstraints(); i ++) {

      FramedMatches match = loop_txt_.data_[i];
      // FramedTransformation trans = loop_log_.data_[i];
      // FramedInformation info = loop_info_.data_[i];

      assert(match.id1_ < match.id2_);
      int id1 = match.id1_;
      int id2 = match.id2_;
      // std::cout << id1 << " ? " << id2 <<std::endl;

      // compute the sum of cauchy loss
      double sum_cauchy_loss = 0.;
      for (std::size_t j = 0; j < match.pairs_.size(); j++) {

        Eigen::Vector3d vec_d = camera_poses_.data_[id1].transformation_se3_ * match.pairs_[j].first
                              -camera_poses_.data_[id2].transformation_se3_ * match.pairs_[j].second;

          sum_cauchy_loss += log (1. + vec_d.squaredNorm() / (SIGMA * SIGMA));

        // std::cout << j << "/" << loop_log_.data_.size() << " : " << sum_cauchy_loss << std::endl;
      }
      if (match.pairs_.size() > 0) {
        sum_cauchy_loss /= match.pairs_.size();
      }

      // double expterm = exp( - log(U) + (1. + PARETO_ALPHA) * log(sum_cauchy_loss));
      // L_[id1][id2] = 1./ (1. + expterm);

      // U =  TRUST_ODOMETRY / (1-TRUST_ODOMETRY) * Median_inlier_loss;
      L_[id1][id2] = U / (U + exp(2 * sum_cauchy_loss));

      //对于两幅地图的配准，在第一次EM迭代时，所有loop-closure的概率都设为1。第一次迭代仍然考虑语义正确点对占比的因素。
      // if(iteration_num ==0) L_[id1][id2]=1;

      //ADD BY YUJIE
      //  if(match.ratio_<0.5 ) L_[id1][id2]=L_[id1][id2]*match.ratio_;
      //match.ratio_用高斯函数表示，达到这样的效果：当match.ratio较小时：0.2，高斯函数值很小，为0.04；当match.ratio较大时：0.5，高斯函数值较大，为0.28

          // L_[id1][id2]=L_[id1][id2]*exp(-(match.ratio_-1)*(match.ratio_-1)/(2*0.1));
        // std::cout << "match.ratio"   << match.ratio_ <<std::endl;
        // std::cout << "gaussian_of_match.ratio"   << exp(-(match.ratio_-1)*(match.ratio_-1)/(2*0.1)) <<std::endl;
      // L_[id1][id2]=L_[id1][id2]*match.old_correspondence_num_/maxnum_loop_corres;

      std::cout << "(" << std::setw(3) << id1 << "," << std::setw(3) << id2 << ") : "
                << std::setw(15) << sum_cauchy_loss
                << std::setw(15) << sum_cauchy_loss * sum_cauchy_loss
                // << std::setw(15) << expterm <<  " --> "
                << std::setw(15) << exp(2 * sum_cauchy_loss / match.pairs_.size()) <<  " --> "
                << std::setw(15) <<  L_[id1][id2]
                << std::endl;

      for (int ii = 0; ii < 20; ii ++) {
        if (L_[id1][id2] > 0.05 * ii && L_[id1][id2] <= 0.05 * (ii+1)){
          healthy_counter[ii] ++;
        }
      }

      sum_L_ += L_[id1][id2];
    }

    std::cout << "\t\tsum_L_ is " << sum_L_ << std::endl;
    std::cout << "\t\tHealthy is ";
    for (int ii = 0; ii < 20; ii ++) {
       std::cout << "\t(" << (0.05 * ii) << ")\t" << healthy_counter[ii] ;
    }
    std::cout << "\t(1.0)\t" << std::endl;
    std::cout << "\t\tTotal is " << NumLoopClosureConstraints() << std::endl;
    std::cout << "\t\testimated LAMBDA is " << U << std::endl;
  }

  void Maximization() {
    last_lambda_ = lambda_;
    last_camera_poses_.index_ = camera_poses_.index_;
    last_camera_poses_.data_.clear();
    for (int i = 0; i < NumPoses(); i++) {
      last_camera_poses_.data_.push_back(
        FramedTransformation (camera_poses_.data_[i].id1_,
                              camera_poses_.data_[i].id2_,
                              camera_poses_.data_[i].frame_,
                              camera_poses_.data_[i].transformation_se3_));
    }
    camera_poses_.index_ ++;
    // last_covarance_ = covarance_;

    // Maximize lambda
    lambda_ = sum_L_ / NumLoopClosureConstraints();

    // Maximize pose
    // Build the problem.
    ceres::Problem problem;

    // Specify local update rule for our parameter
    for (std::vector< FramedTransformation >::iterator it = camera_poses_.data_.begin();
         it != camera_poses_.data_.end(); it++ ) {
      problem.AddParameterBlock(it->transformation_se3_.data(), Sophus::SE3d::num_parameters,
                                new Sophus::test::LocalParameterizationSE3);
    }

    // Create and add cost functions. Derivatives will be evaluated via
    // automatic differentiation                                //YUJIE暂时不对odometry进行优化
    for (int i = 0; i < NumOdometryConstraints(); i++)
    {

      // FramedTransformation trans = odometry_log_.data_[i];
      // FramedInformation info = odometry_info_.data_[i];

      // assert(trans.id1_ == info.id1_ && trans.id2_ == info.id2_ && trans.id1_ < trans.id2_);
      // int id1 = info.id1_;
      // int id2 = info.id2_;

      // if (info.information_(0,0) <= 1)
      //   continue;

      // ceres::CostFunction* cost_odometry =
      //     new ceres::AutoDiffCostFunction<GaussianFunctor, 1,
      //                                     Sophus::SE3d::num_parameters,
      //                                     Sophus::SE3d::num_parameters>(
      //         new GaussianFunctor(trans.transformation_se3_, info.information_, id1, id2));
      // problem.AddResidualBlock(cost_odometry, NULL,
      //                          camera_poses_.data_[id1].transformation_se3_.data(),
      //                          camera_poses_.data_[id2].transformation_se3_.data());

      FramedMatches match = odometry_txt_.data_[i];

      assert(match.id1_ < match.id2_);
      int id1 = match.id1_;
      int id2 = match.id2_;
        if(1)         ///////////////////////////////////////////////////////////
        {
          for (std::size_t j = 0; j < match.pairs_.size() && j<200; j++)
          {
            ceres::CostFunction* cost_odometry =
                new ceres::AutoDiffCostFunction<CauchyFunctor, 3,
                                              Sophus::SE3d::num_parameters,
                                              Sophus::SE3d::num_parameters>(
                    new CauchyFunctor(match.pairs_[j].first, match.pairs_[j].second,
                                      id1, id2));
            problem.AddResidualBlock(cost_odometry,
                                    new ceres::CauchyLoss(SIGMA),
                                    camera_poses_.data_[id1].transformation_se3_.data(),
                                    camera_poses_.data_[id2].transformation_se3_.data());

            //目标函数，每个odometry和loop应该除以 各自特征点对的数量。(cauchy源代码可能因为特征点数量相同没有除)
            // problem.AddResidualBlock(cost_odometry,
            //                       new ceres::ScaledLoss(new ceres::CauchyLoss(SIGMA), 1/match.old_correspondence_num_, ceres::TAKE_OWNERSHIP),
            //                       camera_poses_.data_[id1].transformation_se3_.data(),
            //                       camera_poses_.data_[id2].transformation_se3_.data());
          }
        }

    }

    for (int i = 0; i < NumLoopClosureConstraints(); i++)
    {

      // std::cout << "Maximization + loops " << (i+1) << " / " << NumLoopClosureConstraints() <<  std::endl;
      // FramedTransformation trans = loop_log_.data_[i];
      // FramedInformation info = loop_info_.data_[i];
      FramedMatches match = loop_txt_.data_[i];
      // std::cout << "match.old_correspondence_num_: " << match.old_correspondence_num_ << std::endl;
      assert(match.id1_ < match.id2_);
      int id1 = match.id1_;
      int id2 = match.id2_;

        for (std::size_t j = 0; j < match.pairs_.size(); j++)
        {
          // {
          ceres::CostFunction* cost_loop =
              new ceres::AutoDiffCostFunction<CauchyFunctor, 3,
                                              Sophus::SE3d::num_parameters,
                                              Sophus::SE3d::num_parameters>(
                  new CauchyFunctor(match.pairs_[j].first, match.pairs_[j].second,
                                    id1, id2));
          //  if (L_[id1][id2] >= TRUST_ODOMETRY * 0.5) {
            problem.AddResidualBlock(cost_loop,
                                  new ceres::ScaledLoss(new ceres::CauchyLoss(SIGMA), L_[id1][id2], ceres::TAKE_OWNERSHIP),
                                  camera_poses_.data_[id1].transformation_se3_.data(),
                                  camera_poses_.data_[id2].transformation_se3_.data());

          // if(match.semantic_label[j]==0)
          // {
          //   int label_one=match.label_one_[j];
          //   int label_two=match.label_two_[j];
          // ceres::CostFunction* cost_loop =
          //     new ceres::AutoDiffCostFunction<CauchyFunctor, 3,
          //                                     Sophus::SE3d::num_parameters,
          //                                     Sophus::SE3d::num_parameters>(
          //         new CauchyFunctor(match.pairs_[j].first, match.pairs_[j].second,
          //                           id1, id2));
          // //  if (L_[id1][id2] >= TRUST_ODOMETRY * 0.5) {
          //   problem.AddResidualBlock(cost_loop,
          //                         new ceres::ScaledLoss(new ceres::CauchyLoss(SIGMA), L_[id1][id2]*semantic_p[label_one][label_two], ceres::TAKE_OWNERSHIP),
          //                         camera_poses_.data_[id1].transformation_se3_.data(),
          //                         camera_poses_.data_[id2].transformation_se3_.data());
          // }

        }
    }

    // Set solver options (precision / method)
    ceres::Solver::Options options;
    // options.max_num_iterations = 1000;
    options.gradient_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
    options.function_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
    // options.linear_solver_type = ceres::DENSE_QR;
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

    // Solve
    std::cout << "--------------SOLVING----------------ITERATION " << camera_poses_.index_ << std::endl;
    ceres::Solver::Summary summary;
    Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << std::endl;
    std::cout << "--------------  DONE ----------------ITERATION " << camera_poses_.index_ << std::endl;
  }

  PCLTrajectory camera_poses_for_one_loop;
  void yujie(int index_loop) {

    camera_poses_for_one_loop.data_.clear();
    for (int i = 0; i < NumPoses(); i++)
    {
      camera_poses_for_one_loop.data_.push_back(
        FramedTransformation (camera_poses_.data_[i].id1_,
                              camera_poses_.data_[i].id2_,
                              camera_poses_.data_[i].frame_,
                              camera_poses_.data_[i].transformation_se3_));
    }
    // last_covarance_ = covarance_;

    // Maximize pose
    // Build the problem.
    ceres::Problem problem;

    for (int i = 0; i < NumOdometryConstraints(); i++)
    {

      FramedMatches match = odometry_txt_.data_[i];

      assert(match.id1_ < match.id2_);
      int id1 = match.id1_;
      int id2 = match.id2_;
        if(1)         ///////////////////////////////////////////////////////////
        {
          for (std::size_t j = 0; j < match.pairs_.size(); j++)
          {
            ceres::CostFunction* cost_odometry =
                new ceres::AutoDiffCostFunction<CauchyFunctor, 3,
                                              Sophus::SE3d::num_parameters,
                                              Sophus::SE3d::num_parameters>(
                    new CauchyFunctor(match.pairs_[j].first, match.pairs_[j].second,
                                      id1, id2));
            problem.AddResidualBlock(cost_odometry,
                                    new ceres::CauchyLoss(SIGMA),
                                    camera_poses_for_one_loop.data_[id1].transformation_se3_.data(),
                                    camera_poses_for_one_loop.data_[id2].transformation_se3_.data());
          }
        }
    }

    // 一个loop
    for (std::vector< FramedTransformation >::iterator it = camera_poses_for_one_loop.data_.begin();
         it != camera_poses_for_one_loop.data_.end(); it++ ) {
      problem.AddParameterBlock(it->transformation_se3_.data(), Sophus::SE3d::num_parameters,
                                new Sophus::test::LocalParameterizationSE3);
    }

      // std::cout << "Maximization + loops " << (i+1) << " / " << NumLoopClosureConstraints() <<  std::endl;
      // FramedTransformation trans = loop_log_.data_[i];
      // FramedInformation info = loop_info_.data_[i];
      FramedMatches match = loop_txt_.data_[index_loop];
      std::cout <<  "the loopConstraint " << match.id1_ << " with " << match.id2_ << "\n"<<std::endl;
      // std::cout << "match.old_correspondence_num_: " << match.old_correspondence_num_ << std::endl;
      assert(match.id1_ < match.id2_);
      int id1 = match.id1_;
      int id2 = match.id2_;

        for (std::size_t j = 0; j < match.pairs_.size(); j++)
        {
          // {
          ceres::CostFunction* cost_loop =
              new ceres::AutoDiffCostFunction<CauchyFunctor, 3,
                                              Sophus::SE3d::num_parameters,
                                              Sophus::SE3d::num_parameters>(
                  new CauchyFunctor(match.pairs_[j].first, match.pairs_[j].second,
                                    id1, id2));
            if (match.semantic_label[j]==1)                       //加入语义筛除
            {
            problem.AddResidualBlock(cost_loop,
                                    new ceres::CauchyLoss(SIGMA),
                                  camera_poses_for_one_loop.data_[id1].transformation_se3_.data(),
                                  camera_poses_for_one_loop.data_[id2].transformation_se3_.data());
            }

        }

        for (int i = 0; i < NumLoopClosureConstraints(); i ++) {

      FramedMatches match = loop_txt_.data_[i];
      // FramedInformation info = odometry_info_.data_[i];

      assert(match.id1_ < match.id2_);
      int id1 = match.id1_;
      int id2 = match.id2_;

      std::cout << "In EstimateLAMBDA : " << i << "th loopConstraint " << id1 << " with " << id2 << std::endl;

      Eigen::Quaterniond q;
      q = match.transformation_.block<3,3>(0,0);
      Sophus::SE3d transformation_se3_ = Sophus::SE3d(q, Sophus::SE3d::Point(match.transformation_(0,3), match.transformation_(1,3), match.transformation_(2,3)));

      // compute the sum of cauchy loss
      double sum_cauchy_loss = 0.;
      for (std::size_t j = 0; j < match.pairs_.size(); j++) {
        Eigen::Vector3d vec_d = camera_poses_for_one_loop.data_[id1].transformation_se3_ * match.pairs_[j].first
                              - camera_poses_for_one_loop.data_[id2].transformation_se3_ * match.pairs_[j].second;
         std::cout<<sqrt(vec_d.squaredNorm())<<' ';
        // std::cout << j << "/" << loop_log_.data_.size() << " : " << sum_cauchy_loss << std::endl;
      }
      std::cout<<std::endl;
    }

    // Set solver options (precision / method)
    ceres::Solver::Options options;
    // options.max_num_iterations = 1000;
    options.gradient_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
    options.function_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
    // options.linear_solver_type = ceres::DENSE_QR;
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

    // Solve

    ceres::Solver::Summary summary;
    Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << std::endl;

  }

  int NumPoses() {
    return camera_poses_.data_.size();
  }

  int NumOdometryConstraints() {
    return odometry_txt_.data_.size();
  }

  int NumLoopClosureConstraints() {
    return loop_txt_.data_.size();
  }

  bool IsConverged() {
    if (last_lambda_ < 0) {
      // not started yet
      return false;
    }

    std::cout << "new lambda : "<< lambda_ << std::endl;
    if (fabs(lambda_ - last_lambda_) > 0.00001) {
      std::cout << "lambda was updated" << std::endl;
      return false;
    }

    for (int i = 0; i < NumPoses(); i++)
    {
      // std::cout << "checking converging pose : ";
      Sophus::SE3d last_pose = last_camera_poses_.data_[i].transformation_se3_;
      Sophus::SE3d current_pose = camera_poses_.data_[i].transformation_se3_;

      double const mse = (last_pose.inverse() * current_pose).log().squaredNorm();
      bool const converged = mse < 10. * Sophus::Constants<double>::epsilon();

      std::cout << i << "(" << mse << ", " << converged << "," << last_camera_poses_.index_ << "),";
      if (!converged) {
        std::cout << std::endl;
        return false;
      }
    }

    std::cout << std::endl;
    return true;
  }
};

bool test(Sophus::SE3d const& T_w_targ, Sophus::SE3d const& T_w_init,
          Sophus::SE3d::Point const& point_a_init,
          Sophus::SE3d::Point const& point_b) {
  static constexpr int kNumPointParameters = 3;

  // Optimisation parameters.
  Sophus::SE3d T_wr = T_w_init;
  Sophus::SE3d::Point point_a = point_a_init;

  // Build the problem.
  ceres::Problem problem;

  // Specify local update rule for our parameter
  problem.AddParameterBlock(T_wr.data(), Sophus::SE3d::num_parameters,
                            new Sophus::test::LocalParameterizationSE3);

  // Create and add cost functions. Derivatives will be evaluated via
  // automatic differentiation
  ceres::CostFunction* cost_function1 =
      new ceres::AutoDiffCostFunction<TestSE3CostFunctor, Sophus::SE3d::DoF,
                                      Sophus::SE3d::num_parameters>(
          new TestSE3CostFunctor(T_w_targ.inverse()));
  problem.AddResidualBlock(cost_function1, NULL, T_wr.data());

  ceres::CostFunction* cost_function2 =
      new ceres::AutoDiffCostFunction<TestPointCostFunctor, kNumPointParameters,
                                      Sophus::SE3d::num_parameters,
                                      kNumPointParameters>(
          new TestPointCostFunctor(T_w_targ.inverse(), point_b));
  problem.AddResidualBlock(cost_function2, NULL, T_wr.data(), point_a.data());

  // Set solver options (precision / method)
  ceres::Solver::Options options;
  options.gradient_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
  options.function_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
  options.linear_solver_type = ceres::DENSE_QR;

  // Solve
  ceres::Solver::Summary summary;
  Solve(options, &problem, &summary);
  std::cout << summary.BriefReport() << std::endl;

  // Difference between target and parameter
  double const mse = (T_w_targ.inverse() * T_wr).log().squaredNorm();
  bool const passed = mse < 10. * Sophus::Constants<double>::epsilon();
  return passed;
}

template <typename Scalar>
bool CreateSE3FromMatrix(Eigen::Matrix<Scalar, 4, 4> mat) {
  auto se3 = Sophus::SE3<Scalar>(mat);
  se3 = se3;
  return true;
}

int main(int argc, char** argv) {
  // using SE3Type = Sophus::SE3<double>;
  // using SO3Type = Sophus::SO3<double>;
  // using Point = SE3Type::Point;
  // double const kPi = Sophus::Constants<double>::pi();

  // std::vector<SE3Type> se3_vec;
  // se3_vec.push_back(
  //     SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), Point(0, 0, 0)));
  // se3_vec.push_back(
  //     SE3Type(SO3Type::exp(Point(0.2, 0.5, -1.0)), Point(10, 0, 0)));
  // se3_vec.push_back(
  //     SE3Type(SO3Type::exp(Point(0., 0., 0.)), Point(0, 100, 5)));
  // se3_vec.push_back(
  //     SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), Point(0, 0, 0)));
  // se3_vec.push_back(
  //     SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), Point(0, -0.00000001, 0.0000000001)));
  // se3_vec.push_back(
  //     SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), Point(0.01, 0, 0)));
  // se3_vec.push_back(
  //     SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(4, -5, 0)));
  // se3_vec.push_back(
  //     SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), Point(0, 0, 0)) *
  //     SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(0, 0, 0)) *
  //     SE3Type(SO3Type::exp(Point(-0.2, -0.5, -0.0)), Point(0, 0, 0)));
  // se3_vec.push_back(
  //     SE3Type(SO3Type::exp(Point(0.3, 0.5, 0.1)), Point(2, 0, -7)) *
  //     SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(0, 0, 0)) *
  //     SE3Type(SO3Type::exp(Point(-0.3, -0.5, -0.1)), Point(0, 6, 0)));

  // std::vector<Point> point_vec;
  // point_vec.emplace_back(1.012, 2.73, -1.4);
  // point_vec.emplace_back(9.2, -7.3, -4.4);
  // point_vec.emplace_back(2.5, 0.1, 9.1);
  // point_vec.emplace_back(12.3, 1.9, 3.8);
  // point_vec.emplace_back(-3.21, 3.42, 2.3);
  // point_vec.emplace_back(-8.0, 6.1, -1.1);
  // point_vec.emplace_back(0.0, 2.5, 5.9);
  // point_vec.emplace_back(7.1, 7.8, -14);
  // point_vec.emplace_back(5.8, 9.2, 0.0);

  // for (size_t i = 0; i < se3_vec.size(); ++i) {
  //   const int other_index = (i + 3) % se3_vec.size();
  //   bool const passed = test(se3_vec[i], se3_vec[other_index], point_vec[i],
  //                            point_vec[other_index]);
  //   if (!passed) {
  //     std::cerr << "failed!" << std::endl << std::endl;
  //     exit(-1);
  //   }
  // }

  // Eigen::Matrix<ceres::Jet<double, 28>, 4, 4> mat;
  // mat.setIdentity();
  // std::cout << CreateSE3FromMatrix(mat) << std::endl;

  std::cout << "cauchy_em_no_semantic started" << std::endl;
    clock_t start = clock() ;

  if (argc == 7)
  {
    // double u = 1.0 / 57;
    double lambda = 0.99;
    R2EM_CauchyUniform r2em_c(lambda);

    // std::cout << "load" << std::endl;
    // r2em_c.LoadOdometryLog(argv[1]);

    // std::cout << "init" << std::endl;
    // r2em_c.LoadOdometryInfo(argv[2]);

    std::cout << "LoadOdometryTxt" << std::endl;
    r2em_c.LoadOdometryTxt(argv[1]);

    std::cout << "LoadLoopTxt" << std::endl;
    r2em_c.LoadLoopTxt(argv[2]);
    // r2em_c.LoadLoopInfo(argv[4]);

    // if (argc > 3) {

    std::cout << "InitCameraPosesWithZhou" << std::endl;
    r2em_c.InitCameraPosesWithZhou(argv[3]);
    // } else {
      // r2em_c.InitCameraPoses();
    // }

    std::cout << "EstimateLAMBDA" << std::endl;
    r2em_c.EstimateLAMBDA();

    //r2em_c.SaveCameraPoses("/home/ziquan/my_ws/init_poses.txt");
    //std::cout << "save to /home/ziquan/my_ws/init_poses.txt" << std::endl;
    LoadFromFile_semantic_p(argv[4]);
  std::cout << "semantic probability matrix" << std::endl;

    std::cout << "START EM" << std::endl;
  // add by yujie
  //  for(int i=0;i<r2em_c.NumLoopClosureConstraints();i++)
  //  {
  //    r2em_c.yujie(i);
  //  }

    for (int i = 0 ; i < NUM_ITERATION_EM; i ++) {
      r2em_c.Expectation(i);
      r2em_c.Maximization();

std::string iteration_pose="/home/tang/undergraduate/RobustPCLReconstruction-cauchy_em/data/cauchy/多机/07-10%正确-各代位姿/pose_nosem"+std::to_string(i)+".txt";
  r2em_c.SaveCameraPoses(iteration_pose.c_str());

      std::cout << "check 1 " << std::endl;
      if (i == 0)
        //r2em_c.SaveCameraPoses("/home/ziquan/my_ws/final_poses_em_0th.txt");

      if (i == 15)
        // r2em_c.SaveCameraPoses("/home/tang/final_poses_em_5nd.txt");
        {
        // std::cout << "Fail" << std::endl;
        //   r2em_c.SaveCameraPoses(argv[5]);
        //   break;
        }

      if (i == 100)
        // r2em_c.SaveCameraPoses("final_poses_em_100th.txt");

      std::cout << "check EM converges in iteration " << i << std::endl;
      if (r2em_c.IsConverged()) {
        std::cout << "EM converges! " << std::endl;
        break;
      }
    }
        clock_t finish = clock() ;

    // if (argc > 3) {
      r2em_c.SaveCameraPoses(argv[5]);
      std::cout << "final poses are saved to " << argv[5] << std::endl;
    // } else {

    //   r2em_c.SaveCameraPoses("/home/ziquan/my_ws/final_poses_em.txt");
    //   std::cout << "final poses are saved to /home/ziquan/my_ws/final_poses_em.txt" << std::endl;
    // }

      r2em_c.SaveLinks(argv[6]);
      std::cout << "links are saved to " << argv[6] << std::endl;
    // r2em_c.SaveLinks("/home/ziquan/my_ws/reg_refine_all_em.log");
    // std::cout << "links are saved to /home/ziquan/my_ws/reg_refine_all_em.log" << std::endl;
    std::cout << "cauchy_em_no_semantic ended" << std::endl;
    std::cout << "there are " << r2em_c.NumPoses() << " poses" << std::endl;
    // std::cout << "there are " << r2em_c.NumOdometryConstraints() << " odometry constraints" << std::endl;
    std::cout << "there are " << r2em_c.NumLoopClosureConstraints() << " loops" << std::endl;

  std::cout << "first EM time is : " << double((finish-start)/1000) << "ms" << std::endl;
  } else {
    std::cout << "input format is : odom.txt loop.txt init.txt final.txt link.txt" << std::endl;
  }

  return 0;
}


/*
 * File:   Simulation.cpp
 * Author: ben
 *
 * Created on September 21, 2012, 11:18 AM
 */

#include "Simulation.h"
#include "Utils.h"
#include "moves/MoveFactory.h"
#include "Boltzmann.h"
#include "observables/Observable.h"

#include <boost/assign/std.hpp>
#include <boost/unordered_map.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/array.hpp>
#include <fstream>
#include <sstream>
#include <ctime>

using namespace boost::assign;

Simulation::Simulation() :
k(READ_CONF("mc.k", 10)),
z(READ_CONF("mc.z", 20)),
measureAtVolume(READ_CONF("general.measureAtVolume", true)),
drift(READ_CONF("volume.drift", false)),
volumeStart(READ_CONF("volume.start", 500)),
volumeEnd(READ_CONF("volume.end", 10000)),
skip(READ_CONF("volume.skip", 100)),
drop(READ_CONF("general.drop", 0)) {
    /* Initialize random number generator */
    setSeed(std::time(0));
    moveFactory = new MoveFactory(*this);
}

Simulation::Simulation(const Simulation& orig) {
}

Simulation::~Simulation() {
    clearTriangulation();
    delete moveFactory;

    foreach(Observable* obs, observables) {
        delete obs;
    }
}

void Simulation::clearTriangulation() {
    TriSet deleted;

    foreach(Vertex* v, vertices) {

        foreach(Triangle* t, v->getTriangles()) {
            if (deleted.find(t) == deleted.end()) {
                delete t;
                deleted.insert(t);
            }
        }
        delete v;
    }

    TTSCount = 0;
    SSTCount = 0;
    vertices.clear();
}

void Simulation::readFromFile(const char* filename) {
    std::ifstream file(filename);
    if (!file.is_open()) {
        std::cout << "Unable to open file '" << filename << "'" << std::endl;
        return;
    }

    char type;
    int vCount, vertA, vertB, vertC;

    file >> vCount;
    Vertex * vertex_array[vCount];

    clearTriangulation(); // clear old grid

    for (int i = 0; i < vCount; i++) {
        vertex_array[i] = new Vertex();
        vertices.push_back(vertex_array[i]);
    }

    // TODO: check if the triangulation is valid?
    while (file >> type >> vertA >> vertB >> vertC) {
        new Triangle(type == 'T' ? Triangle::TTS : Triangle::SST, vertex_array[vertA],
                vertex_array[vertB], vertex_array[vertC]);

        if (type == 'T') {
            TTSCount++;
        } else {
            SSTCount++;
        }
    }

    file.close();
}

void Simulation::writeToFile(const char* filename) {
    std::ofstream file(filename);
    if (!file.is_open()) {
        std::cout << "Unable to open file '" << filename << "'" << std::endl;
        return;
    }

    int vCount = 0;
    typedef boost::unordered_map<Vertex*, int> vertex_map;
    vertex_map vertexMap;

    foreach(Vertex* v, vertices) {
        vertexMap.insert(vertex_map::value_type(v, vCount));
        vCount++;
    }

    file << vCount << std::endl;

    TriSet visitedTri;

    foreach(Vertex* v, vertices) {

        foreach(Triangle* t, v->getTriangles()) {
            if (visitedTri.find(t) != visitedTri.end()) {
                continue;
            }
            visitedTri.insert(t);

            if (t->getType() == Triangle::TTS) {
                file << "T";
            } else {
                file << "S";
            }

            for (int i = 0; i < 3; i++) {
                file << " " << vertexMap[t->getVertex(i)];
            }

            file << std::endl;
        }
    }

    file.close();
}

void Simulation::generateInitialTriangulation(int N, int T) {
    Vertex * vertices[N * T];
    Triangle * triangles[N * T * 2]; // TODO: remove, unnecessary

    /* Create vertices */
    for (int t = 0; t < T * N; t++) {
        vertices[t] = new Vertex();
        this->vertices.push_back(vertices[t]);
    }

    /* Create foliation */
    for (int t = 0; t < T; t++) {
        for (int s = 0; s < N; s++) {
            Triangle* u = new Triangle(Triangle::TTS, vertices[t * N + s], vertices[((t + 1) % T) * N + s],
                    vertices[t * N + (s + 1) % N]);
            Triangle* v = new Triangle(Triangle::TTS, vertices[((t + 1) % T) * N + (s + 1) % N], vertices[t * N + (s + 1) % N],
                    vertices[((t + 1) % T) * N + s]);
            triangles[t * 2 * N + 2 * s] = u;
            triangles[t * 2 * N + 2 * s + 1] = v;
        }
    }

    TTSCount = N * T * 2;
    SSTCount = 0;
}

Vertex* Simulation::getRandomVertex(const std::vector<Vertex*>& vertices) {
    BOOST_ASSERT(vertices.size() > 0);
    boost::uniform_int<> uint(0, vertices.size() - 1);
    return vertices[uint(rng)];
}

void Simulation::collectTriangles(TriSet& triSet, Vertex* v, int depth) {
    if (depth <= 0) {
        triSet += v->getTriangles();
        return;
    }

    triSet += v->getTriangles();

    foreach(Vertex* u, v->getNeighbouringVertices()) {
        collectTriangles(triSet, u, depth - 1);
    }
}

void Simulation::drawPartialTriangulation(const char* filename, Vertex* v, const TriSet& tri) {
    std::ofstream dotFile(filename);
    if (!dotFile.is_open()) {
        std::cout << "Unable to open file '" << filename << "'" << std::endl;
        return;
    }

    dotFile << "strict graph G {" << std::endl << "node[shape=point];" << std::endl;
    dotFile << "n" << v << "[color=green];" << std::endl;

    foreach(Triangle* t, tri) {
        for (int i = 0; i < 3; i++) {
            dotFile << "n" << t->getVertex(i) << "--" << "n" << t->getVertex((i + 1) % 3);
            if (t->isTimelike(i)) {
                dotFile << " [color=red];" << std::endl;
            } else {
                dotFile << " [color=blue];" << std::endl;
            }
        }
    }

    dotFile << "}";
    dotFile.close();
}

// FIXME
void Simulation::checkLinkOverlap() {
  /*  for (unsigned int i = 0; i < vertices.size() - 1; i++) {
        for (unsigned int j = i + 1; j < vertices.size(); j++) {
            TriSet t;

            TriSet t = vertices[i]->getTriangles() & vertices[j]->getTriangles();

            if (t.size() != 2 && t.size() != 0) {
                std::cerr << "Link duplicates: " << i << " " << j << " " << t.size() << std::endl;
                BOOST_ASSERT(false);
            }
        }
    }*/
}

void Simulation::printTriangleConnectivity(Triangle* t) {
    typedef boost::tuple<Triangle*, Vertex*, Vertex*> curpos;

    int newId = 0;
    boost::unordered_map<Triangle*, int> tri;
    std::queue<curpos> neighbours;

    // add current triangle and first neighbour
    neighbours.push(
            boost::make_tuple(t, t->getVertex(0), t->getVertex(1)));
    neighbours.push(
            boost::make_tuple(t->getNeighbour(0), t->getVertex(0), t->getVertex(1)));

    while (!neighbours.empty()) {
        curpos cur = neighbours.front();
        neighbours.pop();

        Triangle* t = cur.get < 0 > ();

        boost::unordered_map<Triangle*, int>::iterator res = tri.find(t);
        if (res != tri.end()) {
            std::cout << res->second << std::endl;
            continue;
        }

        // create character of tri a - b - c
        Vertex* a = cur.get < 1 > ();
        Vertex* b = cur.get < 2 > ();
        Vertex* c = t->getThirdVertex(a, b);

        std::cout << (t->isTimelike(a, b) ? "T" : "S") <<
                (t->isTimelike(b, c) ? "T" : "S") <<
                (t->isTimelike(c, a) ? "T" : "S") << ": " << newId << std::endl;

        tri[t] = newId;
        newId++;

        // add the two other neighbours to the stack
        neighbours.push(boost::make_tuple(t->getNeighbour(b, c),
                b, c));
        neighbours.push(boost::make_tuple(t->getNeighbour(c, a),
                c, a));
    }
}

std::vector<int> Simulation::createID(Triangle* t) {
    typedef boost::tuple<Triangle*, Vertex*, Vertex*> curpos;
    std::vector<int> id;

    int newId = 8; // start at 8, 8 > character
    boost::unordered_map<Triangle*, int> tri;
    std::queue<curpos> neighbours;

    // add current triangle and first neighbour
    neighbours.push(
            boost::make_tuple(t, t->getVertex(0), t->getVertex(1)));
    neighbours.push(
            boost::make_tuple(t->getNeighbour(0), t->getVertex(0), t->getVertex(1)));

    while (!neighbours.empty()) {
        curpos cur = neighbours.front();
        neighbours.pop();

        Triangle* t = cur.get < 0 > ();

        boost::unordered_map<Triangle*, int>::iterator res = tri.find(t);
        if (res != tri.end()) {
            id.push_back(res->second);
            continue;
        }

        // create character of tri a - b - c
        Vertex* a = cur.get < 1 > ();
        Vertex* b = cur.get < 2 > ();
        Vertex* c = t->getThirdVertex(a, b);

        int character = t->isTimelike(a, b) * 4 + t->isTimelike(b, c) * 2 +
                t->isTimelike(c, a);

        id.push_back(character);
        tri[t] = newId;
        newId++;

        // add the two other neighbours to the stack
        neighbours.push(boost::make_tuple(t->getNeighbour(b, c),
                b, c));
        neighbours.push(boost::make_tuple(t->getNeighbour(c, a),
                c, a));
    }

    return id;
}

void Simulation::Metropolis(double alpha, unsigned int volume, double
        deltaVolume, unsigned int numSweeps, unsigned int sweepLength) {
    unsigned long long moveRejectedBecauseImpossible = 0, moveRejectedBecauseDetBal = 0;
    double bias = 0; // count the bias of the system size
    double lambda = 5;
    bool measured = false;

    /*BoltzmannTester boltzmannTester;
    Triangle* fixed = *vertices[0]->getTriangles().begin();
    moveFactory->setFixedTriangle(fixed);
    std::vector<int> id = createID(fixed);*/

    //std::ofstream ratio("tri_ratio.dat"); // TODO: make observable
    //std::ofstream lambda_measure("lambda.dat");

    if (drift) {
        volume = volumeStart;
    }

    for (unsigned long sweep = 0; sweep < numSweeps; sweep++) {
        if (drift && sweep % skip == 0) {
            volume += (volumeEnd - volumeStart) / (numSweeps / skip);
        }

        /*
        if (sweep % 10 == 0) { // for testing
            boltzmannTester.printFrequencies(lambda, alpha);
        }*/

        // for testing
        //ratio << TTSCount / (double) (SSTCount + TTSCount) << " " << TTSCount << " " << SSTCount << " "
        //        << 2 * vertices.size() << std::endl;

        measured = false;
        for (unsigned int i = 0; i < sweepLength; i++) {
            /* Measure observables when the volume is right */
            if (((!measureAtVolume && i == 0) ||
                    (measureAtVolume && vertices.size() * 2 == volume && !measured
                    && i > 0.4 * sweepLength && i < 0.6 * sweepLength)) &&
                    ((drift && (sweep % skip) > drop) || (!drift && sweep > drop))) {

                foreach(Observable* o, observables) {
                    o->measure(vertices);
                }

                measured = true;
            }

            Move* move = moveFactory->createRandomMove(*this);

            // some random moves can be impossible and to simplify the
            // probability checks, we can do this explicit check
            if (!move->isMovePossible(vertices)) {
                //boltzmannTester.addStateId(id);
                moveFactory->setMoveImpossible();
                moveRejectedBecauseImpossible++;
                continue;
            }

            /* acceptance = P(x') / P(x) * Q(x | x') / Q(x' | x) */
            double acceptance = move->getMoveProbability(lambda, alpha) *
                    move->getInverseTransitionProbability(vertices) /
                    move->getTransitionProbability(vertices);

            /* add quadratic volume fixing term */
            double delta = move->getDeltaSST() + move->getDeltaTTS();
            acceptance *= exp(-deltaVolume * delta * (4.0 *
                    (double) vertices.size() + delta - 2.0 * (double) volume));

            if (acceptance > 1 || getRandomNumber() < acceptance) {
                move->execute(vertices);
                moveFactory->setMoveAccepted();

                TTSCount += move->getDeltaTTS();
                SSTCount += move->getDeltaSST();
                //id = createID(fixed);
            } else {
                moveRejectedBecauseDetBal++;
                moveFactory->setMoveRejected();
            }

            bias += 2.0 * (double) vertices.size() - (double) volume;
            //boltzmannTester.addStateId(id);
        }

        /* Update lambda. Lower lambda means more growth, so the signs are
         * reversed. */
        bias *= z / (double) sweepLength / (double) volume;
        lambda += k * (bias * bias * bias + bias);
        lambda = lambda < 0 ? 0 : lambda;

        std::cout << "Real run " << sweep << ", lambda: " << lambda << ", delta: " << k * (bias * bias * bias + bias)
                << ", bias: " << bias / z * 100 << "%" << std::endl;
        //lambda_measure << lambda << std::endl;
        bias = 0;
    }

    /* Write some statistics*/
    std::cout << "Rejected impossible: " << moveRejectedBecauseImpossible
            << ", " << 100 * moveRejectedBecauseImpossible /
            ((float) sweepLength * (float) numSweeps) << "%" << std::endl;
    std::cout << "Rejected detailed balance: " << moveRejectedBecauseDetBal
            << ", " << 100 * moveRejectedBecauseDetBal /
            ((float) sweepLength * (float) numSweeps - (float) moveRejectedBecauseImpossible)
            << "%" << std::endl;
};


#include "imagewindowmapper.h"
//#include <boost/math/special_functions/round.hpp>
#include "rectcalc.h"
#include "Geometry/Matrix33.h"
#include "Geometry/Angle.h"
#include "assert.h"
#define FLOAT_ZERO 1.0e-30
//using boost::math::lround;

/*int round(double val)
{
    return (val > 0.0) ? floor(val + 0.5) : ceil(val - 0.5);
}*/
using namespace Geometry;
ImageWindowMapper::ImageWindowMapper()
:m_sizePane (0, 0)
, m_sizeImage(0, 0)
, m_sizeWnd(0, 0)
, _window_offset(0, 0)
{
    m_dAngle = 0.0;
    m_bVFlip = false;
    m_bHFlip = false;
    m_dZoom = 1.0;
    m_dFovX = 0.0;
    m_dFovY = 0.0;
    m_bUpdated = false;
}

ImageWindowMapper::~ImageWindowMapper()
{
}

bool ImageWindowMapper::UpdateMapper()
{
    if (m_bUpdated)
    {
        return true;
    }
    if (m_sizeImage.width() <= 0 || m_sizeImage.height() <= 0 ||
        m_dFovX < FLOAT_ZERO || m_dFovY < FLOAT_ZERO ||
        m_sizeWnd.width() <= 0 || m_sizeWnd.height() <= 0)
    {
        CMatrix33 identity;
        _voxel_to_window = identity;
        _window_to_voxel = identity;
        _physical_to_window = identity;
        _window_to_physical = identity;
        _voxel_to_physical = identity;
        _physical_to_voxel = identity;

        m_bUpdated = true;
        return false;
    }
    _voxel_to_window = CalcVoxelToWindow();
    _voxel_to_window.GetInverse(_window_to_voxel);

    CMatrix33 physical_to_voxel = CalcPhysicalToVoxel();

    _physical_to_window = _voxel_to_window * physical_to_voxel;
    _physical_to_window.GetInverse(_window_to_physical);

    _voxel_to_physical = _window_to_physical * _voxel_to_window;
    _physical_to_voxel = _window_to_voxel * _physical_to_window;

    m_bUpdated = true;
    return true;
}

QPoint ImageWindowMapper::WindowToVoxel(const QPoint& ptWindow) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();

    Point2D p = _window_to_voxel * Point2D(ptWindow.x(), ptWindow.y());

  //  return QPoint(lround(p.x), lround(p.y));
    return QPoint(p.x, p.y);
}

/*int int_round(double v)
{
    try
    {
        return boost::math::iround(v);
    }
    catch (std::exception& e)
    {
        LOG_ERROR(CString(e.what()));
        if (v < 0)
        {
            return INT_MIN;
        }
        else
        {
            return INT_MAX;
        }
    }
}*/

QPoint ImageWindowMapper::PhysicalToWindow(const Point2D& ptPhysical) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();
    Point2D p = _physical_to_window * ptPhysical;

    //return QPoint(int_round(p.x) - _window_offset.cx, int_round(p.y) - _window_offset.cy);
    return QPoint(p.x - _window_offset.width(), p.y - _window_offset.height());
}

QSize ImageWindowMapper::PhysicalToWindow(const Vector2D& size_physical) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();

    auto size = _physical_to_window * size_physical;

    //return QSize(int_round(size.x), int_round(size.y));
    return QSize(size.cx, size.cy);
}

Point2D ImageWindowMapper::PhysicalToWindow2D(const Point2D& ptPhysical) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();
    Point2D pt = _physical_to_window * ptPhysical;
    pt.x = pt.x - _window_offset.width();
    pt.y = pt.y - _window_offset.height();
    return pt;
}

Vector2D ImageWindowMapper::PhysicalToWindow2D(const Vector2D& vPhysical) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();
    Vector2D size = _physical_to_window * vPhysical;
    return size;
}

Point2D ImageWindowMapper::WindowToPhysical(const QPoint& ptWindow) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();

    Point2D pt_window(ptWindow.x() + _window_offset.width(),
                      ptWindow.y() + _window_offset.height());
    Point2D pt = _window_to_physical * pt_window;
    return pt;
}

Vector2D ImageWindowMapper::WindowToPhysical(const QSize& szWindow) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();

    Vector2D size_window(szWindow.width(), szWindow.height());
    Vector2D size2d = _window_to_physical * size_window;
    return size2d;
}

QPoint ImageWindowMapper::PhysicalToVoxel(const Point2D& ptPhysical) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();
    assert(m_dFovX > FLOAT_ZERO && m_dFovY > FLOAT_ZERO);
    Point2D pt = CalcPhysicalToVoxel() * ptPhysical;
  //  QPoint ptVoxel(boost::math::lround(pt.x), boost::math::lround(pt.y));
    QPoint ptVoxel(pt.x, pt.y);
    return ptVoxel;
}

Point2D ImageWindowMapper::PhysicalToDoubleVoxel(const Point2D& ptPhysical) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();
    assert(m_dFovX > FLOAT_ZERO && m_dFovY > FLOAT_ZERO);
    Point2D pt = CalcPhysicalToVoxel() * ptPhysical;
    return pt;
}

Point2D ImageWindowMapper::VoxelToPhysical(const Point2D& voxel) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();

    return _voxel_to_physical * voxel;
}

QPoint ImageWindowMapper::VoxelToWindow(const Point2D& voxel_point) const
{
    const_cast<ImageWindowMapper*>(this)->UpdateMapper();
    auto window_point = _voxel_to_window * voxel_point;

   // return QPoint(int_round(window_point.x), int_round(window_point.y));
    return QPoint(window_point.x, window_point.y);
}

void ImageWindowMapper::SetFOV(double dFovX, double dFovY)
{
    if (abs(m_dFovX - dFovX) < 0.000001 && abs(m_dFovY - dFovY) < 0.000001)
    {
        return;
    }
    m_dFovX = dFovX;
    m_dFovY = dFovY;
    m_bUpdated = false;
}

void ImageWindowMapper::SetImageSize(QSize size)
{
    if (m_sizeImage == size)
    {
        return;
    }

    m_sizeImage = size;
    m_bUpdated = false;
}

void ImageWindowMapper::SetWindowSize(QSize size)
{
    if (m_sizeWnd == size)
    {
        return;
    }

    m_sizeWnd = size;
    m_bUpdated = false;
}

void ImageWindowMapper::SetWindowOffset(const QSize& offset)
{
    if (_window_offset == offset)
    {
        return;
    }

    _window_offset = offset;
    m_bUpdated = false;
}

CMatrix33 ImageWindowMapper::CalcVoxelToWindow() const
{
    CMatrix33 rotation;
    rotation.MakeRotate(CRadian(-m_dAngle));

    CMatrix33 flip;
    if (m_bHFlip)
    {
        flip = flip * CMatrix33::FromBasisVectors(Vector2D(-1.0, 0.0), Vector2D(0.0, 1.0));
    }
    if (m_bVFlip)
    {
        flip = flip * CMatrix33::FromBasisVectors(Vector2D(1.0, 0.0), Vector2D(0.0, -1.0));
    }

    CMatrix33 pane;
    pane.MakeTranslate(Vector2D(m_sizePane.x, m_sizePane.y));

    QSize image_rect_size = RectCalc::GetFitSize(m_sizeImage, m_sizeWnd);
    double fit_ratio = image_rect_size.width()/ static_cast<double>(m_sizeImage.width());
    CMatrix33 zoom;
    zoom.MakeScale(Vector2D(m_dZoom * fit_ratio, m_dZoom * fit_ratio));

    CMatrix33 image_center_to_first;
    image_center_to_first.MakeTranslate(Vector2D(-(m_sizeImage.width() - 1) / 2.0, -(m_sizeImage.height() - 1) / 2.0));

    CMatrix33 viweport_first_to_center;
    viweport_first_to_center.MakeTranslate(Vector2D((m_sizeWnd.width() - 1) / 2.0, (m_sizeWnd.height() - 1) / 2.0));

    return viweport_first_to_center * zoom * pane * flip * rotation * image_center_to_first;
}

CMatrix33 ImageWindowMapper::CalcPhysicalToVoxel() const
{
    CMatrix33 scale;
    scale.MakeScale(Vector2D(m_sizeImage.width() / m_dFovX, m_sizeImage.height() / m_dFovY));

    CMatrix33 translate;
    translate.MakeTranslate(Vector2D(m_sizeImage.width() / 2.0, m_sizeImage.height() / 2.0));

    return translate * scale;
}

void ImageWindowMapper::SetRotationAngle( double angle )
{
    if (m_dAngle == angle)
    {
        return;
    }

    m_dAngle = angle;
    m_bUpdated = false;
}

double ImageWindowMapper::GetRotationAngle() const
{
    return m_dAngle;
}

bool ImageWindowMapper::IsVerticalFlip() const
{
    return m_bVFlip;
}

void ImageWindowMapper::SetVerticalFlip( bool flip )
{
    if (m_bVFlip == flip)
    {
        return;
    }

    m_bVFlip = flip;
    m_bUpdated = false;
}

bool ImageWindowMapper::IsHorizontalFlip() const
{
    return m_bHFlip;
}

void ImageWindowMapper::SetHorizontalFlip( bool flip )
{
    if (m_bHFlip == flip)
    {
        return;
    }

    m_bHFlip = flip;
    m_bUpdated = false;
}

void ImageWindowMapper::SetZoom( double dZoom )
{
    if (m_dZoom == dZoom)
    {
        return;
    }

    m_dZoom = dZoom;
    m_bUpdated = false;
}

double ImageWindowMapper::GetZoom() const
{
    return m_dZoom;
}

void ImageWindowMapper::SetPane(const Vector2D& pane)
{
    if (m_sizePane.x == pane.cx && m_sizePane.y == pane.cy)
    {
        return;
    }

    m_sizePane = Point2D(pane.cx, pane.cy);
    m_bUpdated = false;
}

Vector2D ImageWindowMapper::GetPane() const
{
    return Vector2D(m_sizePane.x, m_sizePane.y);
}

void ImageWindowMapper::HorizontalFlip()
{
    m_bHFlip = !m_bHFlip;
    m_sizePane.x *=  -1;
    m_bUpdated = false;
}

void ImageWindowMapper::VerticalFlip()
{
    m_bVFlip = ! m_bVFlip;
    m_sizePane.y *= -1;
    m_bUpdated = false;
}

void ImageWindowMapper::Rotate( double angle )
{
    Vector2D old_pane(m_sizePane.x, m_sizePane.y);
    m_sizePane = Point2D(old_pane.cx * cos(angle) + old_pane.cy * sin(angle),
        -old_pane.cx * sin(angle) + old_pane.cy * cos(angle));

    angle = (m_bHFlip == m_bVFlip) ? angle : - angle;
    m_dAngle += angle;

    m_bUpdated = false;
}

void ImageWindowMapper::Zoom( double multiple )
{
    SetZoom(m_dZoom * multiple);
}

void ImageWindowMapper::Pane( const Vector2D& offset )
{
    if (m_sizeImage.width() == 0 || m_sizeImage.height() == 0)
    {
        return ;
    }

    QSize image_rect_size = RectCalc::GetFitSize(m_sizeImage, m_sizeWnd);
    double effective_zoom = m_sizeImage.width() / (m_dZoom * image_rect_size.width());

    m_sizePane = Point2D(m_sizePane.x + offset.cx * effective_zoom, m_sizePane.y + offset.cy * effective_zoom);
    Point2D max_pane ((m_sizeImage.width() * 4.0) / 5, (m_sizeImage.height() * 4.0) / 5);
    if (m_sizePane.x > max_pane.x)
    {
        m_sizePane.x = max_pane.x;
    }
    else if (m_sizePane.x < -max_pane.x)
    {
        m_sizePane.x = -max_pane.x;
    }
    if (m_sizePane.y > max_pane.y)
    {
        m_sizePane.y = max_pane.y;
    }
    else if (m_sizePane.y < -max_pane.y)
    {
        m_sizePane.y = -max_pane.y;
    }
    m_bUpdated = false;
}

bool ImageWindowMapper::operator==( const ImageWindowMapper& rhs )
{
    return ((fabs(m_dAngle - rhs.m_dAngle) <= 1.0e-5) &&
        m_bHFlip == rhs.m_bHFlip &&
        m_bVFlip == rhs.m_bVFlip &&
        (fabs(m_dZoom - rhs.m_dZoom) <= 1.0e-5) &&
        m_sizePane == rhs.m_sizePane);
}

void ImageWindowMapper::Reset()
{
    m_dZoom = 1.0;
    m_dAngle = 0.0;
    m_sizePane = Point2D(0.0, 0.0);
    m_bHFlip = false;
    m_bVFlip = false;

    m_bUpdated = false;
}