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

#include "local_parameterization_se3.hpp"

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

#include <iomanip>
#include <algorithm>

#define SIGMA 0.5
#define SIGMA_two_EM 0.2
#define NUM_ITERATION_EM 1000
#define PI 3.1415926535897
#define Median_inlier_loss   3                //调节公式(13)中的圆符号         change round signal

#define PARETO_ALPHA 1

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

typedef Eigen::Matrix< double, 6, 6, Eigen::RowMajor > InformationMatrix;
typedef Eigen::Matrix< double, 6, 1> Vector6d;
typedef Eigen::Matrix< int, Eigen::Dynamic, 3, Eigen::RowMajor > Semantic_lable_read;   //元素为:语义一致的标签位、语义标签1、语义标签2     the flag if semantic labels are the same  label_1  and label_2
typedef Eigen::Matrix< double, Eigen::Dynamic, 6, 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;    //保存语义混淆矩阵     save  semantic matrix

  void LoadFromFile_semantic_p(const char*  filename ) {          //load  semantic matrix
    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;          //存储每个点对 语义是否对应的标志位    save the flag if semantic labels are the same for each point pair
  std::vector<int> label_one_;         //每对对应点第一个点的语义标签值     the semantic label of the first point for each point pair
  std::vector<int> label_two_;        //每对对应点第二个点的语义标签值     the semantic label of the second point for each point pair
  FramedMatches( int id1, int id2, int f, int old_cor,int new_cor,float ratio,Eigen::Matrix4d trans, PairMatrix pairs,Semantic_lable_read sem)
  : id1_(id1), id2_(id2), frame_(f), old_correspondence_num_(old_cor),new_correspondence_num_(new_cor),ratio_(ratio),transformation_(trans)
  {
    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]=sem(i,0);
      label_one_[i]=sem(i,1);
      label_two_[i]=sem(i,2);
    }
  }
};

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

  void LoadFromFile(const char* filename) {// load files of odometry and loop constraints
    data_.clear();
    int id1, id2, frames=22, old_correspondence_num, new_correspondence_num;
    float ratio;
    Eigen::Matrix4d trans;
    PairMatrix pairs;
    Semantic_lable_read sem;
    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 ] != '#' ) {
            sscanf( buffer, "%d %d %d %d %f", &id1, &id2, &old_correspondence_num, &new_correspondence_num, &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;
          pairs.resize(old_correspondence_num, 6);
          sem.resize(old_correspondence_num, 3);
          for (int i = 0; i < old_correspondence_num; i++) {
            fgets( buffer, 1024, f );
            sscanf( buffer, "%lf %lf %lf %lf %lf %lf %d %d %d", &pairs(i,0), &pairs(i,1), &pairs(i,2), &pairs(i,3), \
                                                           &pairs(i,4), &pairs(i,5), &sem(i,0),&sem(i,1),&sem(i,2));
          }
            data_.push_back( FramedMatches( id1, id2, frames, old_correspondence_num,new_correspondence_num,ratio,trans, pairs,sem) );
          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)));
  }

  // 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 ) {      // load init poses file
    data_.clear();
    index_ = 0;
    int id1, id2, frame;
    Eigen::Matrix4d trans;
    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", &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 ) );
        }
      }
      std::cout << "pose num is:"<<data_.size()  << std::endl;
      fclose( f );
    }
  }

  void SaveToFile(const char* filename ) {               // save output poses files
    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 {        // load information matrix files ,not used in cauchy
  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

// test examples of 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;
};

//  GaussianFunctor is not used in this work.
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_;
};

// define CauchyFunctor
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  : converge for first EM
  double lambda_;
  //  lambda_second_ : converge for second EM
  double  lambda_second_;
  double U;
  Eigen::Matrix<double, 6, 6> covarance_;

  // read from file
  PCLMatches odometry_txt_;
  PCLMatches loop_txt_;

  // blocks for ceres
  PCLTrajectory camera_poses_;
  std::vector< std::vector<double> > L_;     //单个loop正确的概率值(语义)      correct probability of a loop constraint
  double sum_L_;   //用于检测 first EM 收敛性      for test convergence of first EM
  std::vector< std::vector<double> > L_P_;    //所有odometry、loop中每个点对正确的概率      correct probability of a point pair in a constraint
  double sum_L_P_;   //用于检测 second EM收敛性    for test convergence of second EM
  std::vector< std::vector<int> > L_vector;     //单个loop是否正确(根据相同向量对的个数): 0表示不正确, 1: 表示暂时正确    judge whether a loop is correct from vector filter model
  std::vector< std::vector<int> > vector_true;         //根据向量对是否相同：所有odometry、loop中每个点对是否正确: 0表示不正确，1表示正确  judge whether a point pair in a loop is correct from vector filter model

  // convergence condition  for first EM
  double last_lambda_;
  PCLTrajectory last_camera_poses_;
  Eigen::Matrix<double, 6, 6> last_covarance_;
  // convergence condition  for second EM
  double last_lambda_second_;

  R2EM_CauchyUniform(double lambda)
  : lambda_(lambda){
    last_lambda_ = -1.;
    lambda_second_=0.99;
    last_lambda_second_ = -1.;
  }
  ~R2EM_CauchyUniform() {}

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

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

  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() {          // init poses of submaps
    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 ) );
    }

    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() {              //estimate  cauchy loss for odometry and loop constraints

    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];

      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));
      }
      assert(match.pairs_.size() > 0);
      average_loss[i] = sum_cauchy_loss / match.pairs_.size();
      total_loss[i] = sum_cauchy_loss;
    }

    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) {       //save correct constraints
    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 );
  }

  // first Expectation step
  void Expectation(int iteration_num)
  {
    //计算odometry每个点正确的概率    calculate correct probabilities of point pairs in each odometry constraint
    // for (int i = 0; i < NumOdometryConstraints(); i++)
    // {
    //   std::cout << "num of correct odometry:  " << i<< std::endl;

    //   FramedMatches match = odometry_txt_.data_[i];

    //   assert(match.id1_ < match.id2_);
    //   int id1 = match.id1_;
    //   int id2 = match.id2_;
    //     if(id1 !=60)         //
    //     {
    //       for (std::size_t j = 0; j < match.pairs_.size() && j<200; 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;

    //           L_P_[i+NumLoopClosureConstraints()][j]= U / (U + exp(2 * log (1. + vec_d.squaredNorm() / (SIGMA_two_EM * SIGMA_two_EM))));

    //           if(iteration_num==0)   L_P_[i+NumLoopClosureConstraints()][j]=1;
    //       }
    //     }

    // }
    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;
    for (int i = 0; i < NumLoopClosureConstraints(); i ++) {

      FramedMatches match = loop_txt_.data_[i];

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

      if(L_vector[id1][id2]==0) continue;   //根据向量对一致的数量判断 一个loop是否为正确的loop  不正确的loop不参与优化  judge if a loop is correct by vector filter model
      // 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++) {

          if(vector_true[i][j]==0 ) continue;     //向量对不一致的匹配对 直接不参与 正确概率计算 和 优化     if a point pair  is false by vector filter model , continue

        Eigen::Vector3d vec_d = camera_poses_.data_[id1].transformation_se3_ * match.pairs_[j].first
                              -camera_poses_.data_[id2].transformation_se3_ * match.pairs_[j].second;
        if(1)
        {
          sum_cauchy_loss += log (1. + vec_d.squaredNorm() / (SIGMA * SIGMA));
        }
        else
        {
           sum_cauchy_loss+=0;
        }
      }
      if (match.pairs_.size() > 0) {
        sum_cauchy_loss /= match.pairs_.size();
      }

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

        std::cout << id1 <<" " << id2 << std::endl;
        std::cout << log (1. + (camera_poses_.data_[id1].transformation_se3_ * match.pairs_[0].first
                              -camera_poses_.data_[id2].transformation_se3_ * match.pairs_[0].second).squaredNorm() / (SIGMA * SIGMA))*L_[id1][id2] << std::endl;

      //对于两幅地图的配准，在第一次EM迭代时，所有loop-closure的概率都设为1。第一次迭代仍然考虑语义正确点对占比的因素。  correct P of loops =1  for the first E step,and consider semantic ratio
       if(iteration_num ==0)
       {
        L_[id1][id2]=1;
        L_[id1][id2]=L_[id1][id2]*exp(-(match.ratio_-1)*(match.ratio_-1)/(2*0.1));
       }

      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) << 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;
  }

  //first Maximization step
  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_ ++;

    // 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
    int  diuqi=0 ;
    // odometry cases
    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)         //
        {
          diuqi=0 ;        // output numbers of filtered point pairs in each odometry
          for (std::size_t j = 0; j < match.pairs_.size() && j<200; j++)
          {
            // std::cout << "label is "<<  match.semantic_label[j] << " match.vector_true[j] " << vector_true[i+NumLoopClosureConstraints()][j]<< std::endl;

            if(vector_true[i+NumLoopClosureConstraints()][j]==0)          //
            {
              // std::cout << "丢弃：" <<match.pairs_[j].first[0] << "  " << match.pairs_[j].first[1] << "  " <<match.pairs_[j].first[2] << " second: " <<
              //  match.pairs_[j].second[0] << "  " << match.pairs_[j].second[1] << "  " <<match.pairs_[j].second[2] << std::endl;
              diuqi++ ;

            }

            if(vector_true[i+NumLoopClosureConstraints()][j]==1)          //
            {
              // std::cout << match.pairs_[j].first[0] << "  " << match.pairs_[j].first[1] << "  " <<match.pairs_[j].first[2] << " second: " <<
              //  match.pairs_[j].second[0] << "  " << match.pairs_[j].second[1] << "  " <<match.pairs_[j].second[2] << std::endl;
            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());
            }
          }
          std::cout << id1 <<"is: "<<diuqi <<std::endl;
        }

    }

    for (int i = 0; i < NumLoopClosureConstraints(); i++)
    {
      FramedMatches match = loop_txt_.data_[i];

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

        if(L_vector[id1][id2]==0) continue; //根据向量对一致的数量判断 一个loop是否为正确的loop  不正确的loop不参与优化      for filtered loops by vector filter model ,continue

        for (std::size_t j = 0; j < match.pairs_.size() ; j++)
        {
          if(match.semantic_label[j]==1 && vector_true[i][j]==1)
          {
          // {
          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) {
            //目标函数，每个odometry和loop应该除以 各自特征点对的数量。(cauchy源代码可能因为特征点数量相同没有除)
            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 && vector_true[i][j]==1)
          {
            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) {
           if (label_one!=100&&label_two!=100) {
            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;
  }

void Vector()       //计算一个loop中正确向量的个数   vector filter model
  {
      int l_odometry_ave=0;
    for (int i = 0; i < NumOdometryConstraints(); i++)
    {
      int l=0;
      // std::cout << "num of correct odometry:  " << i<< std::endl;

      FramedMatches match = odometry_txt_.data_[i];

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

       int  num_vector[match.pairs_.size()]={0};

        if(1)         // judge for a point pair in each odometry
        {
          for (std::size_t j = 0; j < match.pairs_.size()-1 && j<200; j++)
          for (std::size_t k = j+1; k < match.pairs_.size() && k<200; k++)
          {

              Eigen::Vector3d vec_d_1 = (match.pairs_[k].first - match.pairs_[j].first);
              Eigen::Vector3d vec_d_2 = (match.pairs_[k].second - match.pairs_[j].second);

              float sqrt_1=sqrt(vec_d_1[0]*vec_d_1[0]+vec_d_1[1]*vec_d_1[1]+vec_d_1[2]*vec_d_1[2]);
              float sqrt_2=sqrt(vec_d_2[0]*vec_d_2[0]+vec_d_2[1]*vec_d_2[1]+vec_d_2[2]*vec_d_2[2]);

              if(abs(sqrt_1-sqrt_2)<0.2)
              {
                // l++;
                num_vector[j]++;
                num_vector[k]++;
                // vector_true[i+NumLoopClosureConstraints()][j]=1;
                // vector_true[i+NumLoopClosureConstraints()][k]=1;
              }
          }
          // judge for a odometry constraint
          for(int m=0;m<match.pairs_.size();m++)
          {
            // std::cout << num_vector[m] << std::endl;
            if(num_vector[m]>match.pairs_.size()/4)
            {
              vector_true[i+NumLoopClosureConstraints()][m]=1;
              l++;
            }
          }

        }
        l_odometry_ave+=l;
        // std:: cout << l << std::endl;
    }
    l_odometry_ave=l_odometry_ave/NumOdometryConstraints();      //所有odometry中相同向量对数量的平均数
    // std:: cout << "l_odometry_ave "<<l_odometry_ave << std::endl;
    for (int i = 0; i < NumLoopClosureConstraints(); i++)
    {
      int l=0;
      FramedMatches match = loop_txt_.data_[i];

      int  num_vector[match.pairs_.size()]={0};

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

      //judge for a point pair in a loop
          for (std::size_t j = 0; j < match.pairs_.size()-1 && j<200; j++)
          for (std::size_t k = j+1; k < match.pairs_.size() && k<200; k++)
          {

              Eigen::Vector3d vec_d_1 = (match.pairs_[k].first - match.pairs_[j].first);
              Eigen::Vector3d vec_d_2 = (match.pairs_[k].second - match.pairs_[j].second);

              float sqrt_1=sqrt(vec_d_1[0]*vec_d_1[0]+vec_d_1[1]*vec_d_1[1]+vec_d_1[2]*vec_d_1[2]);
              float sqrt_2=sqrt(vec_d_2[0]*vec_d_2[0]+vec_d_2[1]*vec_d_2[1]+vec_d_2[2]*vec_d_2[2]);

              if(abs(sqrt_1-sqrt_2)<0.1)
              {
                // l++;
                num_vector[j]++;
                num_vector[k]++;
              }

          }
          // judge for a loop
          for(int m=0;m<match.pairs_.size();m++)
          {
            if(num_vector[m]>match.pairs_.size()/4)
            {
                          // std::cout << num_vector[m] << std::endl;
              vector_true[i][m]=1;
              l++;
            }
          }
        // std:: cout << l << std::endl;
        if (l<l_odometry_ave*0.2)
        {
          // std:: cout << "outlier loop" << std::endl;
          L_vector[id1][id2]=0;
        }
        else    L_vector[id1][id2]=1;
        // std:: cout << " " << std::endl;
    }
  }

  // second E step  filter false point pairs in correct constraints
  void Expectation_point(int iteration_num)       //再次使用EM算法，筛除正确loop 和 odometry中的错误点对
  {
    sum_L_P_=0. ;
    for (int i = 0; i < NumOdometryConstraints(); i++)
    {
      // std::cout << "num of correct odometry:  " << i<< std::endl;

      FramedMatches match = odometry_txt_.data_[i];

      assert(match.id1_ < match.id2_);
      int id1 = match.id1_;
      int id2 = match.id2_;
      // std::cout << id1 << "   " << id2 <<std::endl;
        if(1)
        {
          // odometry cases
          for (std::size_t j = 0; j < match.pairs_.size() && j<200; j++)
          {
              if(vector_true[i+NumLoopClosureConstraints()][j]==0)  continue;

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

              L_P_[i+NumLoopClosureConstraints()][j]= U / (U + exp(2 * log (1. + vec_d.squaredNorm() / (SIGMA_two_EM * SIGMA_two_EM))));

              //if(iteration_num==0)   L_P_[i+NumLoopClosureConstraints()][j]=1;

              sum_L_P_ +=L_P_[i+NumLoopClosureConstraints()][j] ;
          }
        }

    }
    // loop cases
    for (int i = 0; i < NumLoopClosureConstraints(); i ++)
    {
      FramedMatches match = loop_txt_.data_[i];

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

      if(L_vector[id1][id2]==0)  continue;       //根据向量 去除错误loop
      if(L_[id1][id2] < TRUST_ODOMETRY*0.2 )  continue;           //只考虑正确loop中错误点对的筛除

      for (std::size_t j = 0; j < match.pairs_.size(); j++)
      {
        if(vector_true[i][j]==0)  continue;

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

        L_P_[i][j]= U / (U + exp(2 * log (1. + vec_d.squaredNorm() / (SIGMA_two_EM * SIGMA_two_EM))));
        // if(iteration_num==0)   L_P_[i][j]=1;
        // std::cout << "U:  " << U<< std::endl;
        sum_L_P_ +=L_P_[i][j] ;
        // std::cout << "num of correct loop:  " << i<< std::endl;
        // std::cout << "correct probability of point "<<j <<" is: "<< L_P_[i][j]<< std::endl;
        // std::cout << "point first is :" << match.pairs_[j].first << "point second is :" << match.pairs_[j].second <<std::endl;
      }

    }
  }

// second M step filter false point pairs in correct constraints
void Maximization_point() {
    last_lambda_second_ = lambda_second_;
    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_ ++;

    // Maximize lambda
    lambda_second_ = sum_L_P_ / 50;

    // 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);
    }

    //dometry cases
    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<200; j++)
          {

            if(L_P_[i+NumLoopClosureConstraints()][j]<0.8) continue;
            else L_P_[i+NumLoopClosureConstraints()][j]=1;

            if(match.semantic_label[j]==1 && vector_true[i+NumLoopClosureConstraints()][j]==1)
            {
            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::ScaledLoss(new ceres::CauchyLoss(SIGMA_two_EM), L_P_[i+NumLoopClosureConstraints()][j], ceres::TAKE_OWNERSHIP),
                                  camera_poses_.data_[id1].transformation_se3_.data(),
                                  camera_poses_.data_[id2].transformation_se3_.data());
            }
          }
        }

    }

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

      FramedMatches match = loop_txt_.data_[i];

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

       if(L_[id1][id2] < TRUST_ODOMETRY*0.2)  continue;          //不进行outlier loop的优化
       if(L_vector[id1][id2]==0) continue;    //向量判断   错误loop直接不参与优化

        for (std::size_t j = 0; j < match.pairs_.size(); j++)
        {
          if(L_[id1][id2] >= TRUST_ODOMETRY*0.2)
          {
              if(L_P_[i][j]<0.8) continue;
               else L_P_[i][j]=1;
          }
          if(match.semantic_label[j]==1 && vector_true[i][j]==1)   //语义+向量  semantic information and vector consistency
          {
          // {
          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));
            problem.AddResidualBlock(cost_loop,
                                  new ceres::ScaledLoss(new ceres::CauchyLoss(SIGMA_two_EM), L_P_[i][j], ceres::TAKE_OWNERSHIP),
                                  camera_poses_.data_[id1].transformation_se3_.data(),
                                  camera_poses_.data_[id2].transformation_se3_.data());
          }

          if(match.semantic_label[j]==0 && vector_true[i][j]==1)          //语义+向量
          {
            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) {
            if(label_one!=100 && label_two!=100){
            problem.AddResidualBlock(cost_loop,
                                  new ceres::ScaledLoss(new ceres::CauchyLoss(SIGMA_two_EM), L_P_[i][j]*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;
  }
  int NumPoses() {
    return camera_poses_.data_.size();
  }

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

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

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

    std::cout << "new lambda : "<< lambda_ << std::endl;
    if (fabs(lambda_ - last_lambda_) > 0.001) {
      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 IsConverged_second() {
    if (last_lambda_second_ < 0) {
      // not started yet
      return false;
    }

    std::cout << "new lambda_second : "<< lambda_second_ << std::endl;
    if (fabs(lambda_second_ - last_lambda_second_) > 0.01) {
      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) {

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

  if (argc == 7)
  {

    // converge  detect for EM first and second initial
    double lambda = 0.99;
    R2EM_CauchyUniform r2em_c(lambda);

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

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

    std::cout << "InitCameraPosesWithZhou" << std::endl;
    r2em_c.InitCameraPosesWithZhou(argv[3]);

    //初始化
  r2em_c.L_P_.resize(r2em_c.NumLoopClosureConstraints()+r2em_c.NumOdometryConstraints());
    for (int i = 0; i < r2em_c.NumLoopClosureConstraints()+r2em_c.NumOdometryConstraints(); i++) {
      r2em_c.L_P_[i].resize(200);
    }

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

  r2em_c.vector_true.resize(r2em_c.NumLoopClosureConstraints()+r2em_c.NumOdometryConstraints());
    for (int i = 0; i < r2em_c.NumLoopClosureConstraints()+r2em_c.NumOdometryConstraints(); i++) {
      r2em_c.vector_true[i].resize(200);
    }

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

    LoadFromFile_semantic_p(argv[4]);
  std::cout << "semantic probability matrix" << std::endl;
    r2em_c.Vector();
    std::cout << "START EM" << std::endl;

    //第一次EM迭代，用于筛除错误的loop    first EM iteration
    for (int i = 0 ; i < NUM_ITERATION_EM; i ++) {
      r2em_c.Expectation(i);
      r2em_c.Maximization();

      std::cout << "check 1 " << std::endl;

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

    r2em_c.SaveCameraPoses("/home/tang/undergraduate/RobustPCLReconstruction-cauchy_em/data/cauchy/poses_first_EM.txt");
    //第二次EM迭代，用于筛除正确loop中的错误点对
    //second EM iteration
    for (int i = 0 ; i < NUM_ITERATION_EM; i ++) {
      r2em_c.Expectation_point(i);
      r2em_c.Maximization_point();

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

      std::cout << "check 1 " << std::endl;

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

      r2em_c.SaveCameraPoses(argv[5]);
      std::cout << "final poses are saved to " << argv[5] << std::endl;

      r2em_c.SaveLinks(argv[6]);
      std::cout << "links are saved to " << argv[6] << std::endl;

    std::cout << "there are " << r2em_c.NumPoses() << " poses" << std::endl;
    std::cout << "there are " << r2em_c.NumLoopClosureConstraints() << " loops" << std::endl;

    // efficiency
    std::cout << "first EM time is : " << double((finish_1-start)/1000) << "ms" << std::endl;
    std::cout << "second EM time is : " << double((finish_2-finish_1)/1000) << "ms" << std::endl;
    // std::cout << start << std::endl;
    // std::cout << finish_1 << std::endl;
    // std::cout << finish_2 << std::endl;
    //  std::cout << CLOCKS_PER_SEC << std::endl;

  } else {
    std::cout << "input format is : odom.txt loop.txt init.txt final.txt link.txt" << std::endl;
  }

  return 0;
}


#ifndef STAN_MATH_TORSTEN_PKMODELTWOCPT2_HPP
#define STAN_MATH_TORSTEN_PKMODELTWOCPT2_HPP

#include <Eigen/Dense>
#include <stan/math/prim/scal/err/check_greater_or_equal.hpp>
#include <boost/math/tools/promotion.hpp>
#include <stan/math/torsten/event_solver.hpp>
#include <stan/math/torsten/events_manager.hpp>
#include <stan/math/torsten/PKModel/PKModel.hpp>
#include <stan/math/torsten/PKModel/Pred/Pred1_twoCpt.hpp>
#include <stan/math/torsten/PKModel/Pred/PredSS_twoCpt.hpp>
#include <stan/math/torsten/pmx_twocpt_model.hpp>
#include <stan/math/torsten/pmx_population_check.hpp>
#include <stan/math/torsten/return_type.hpp>
#include <string>
#include <vector>

namespace torsten {

/**
 * Computes the predicted amounts in each compartment at each event
 * for a two compartment model with first oder absorption.
 *
 * @tparam T0 type of scalar for time of events.
 * @tparam T1 type of scalar for amount at each event.
 * @tparam T2 type of scalar for rate at each event.
 * @tparam T3 type of scalar for inter-dose inteveral at each event.
 * @tparam T4 type of scalars for the model parameters.
 * @tparam T5 type of scalars for the
 * @param[in] pMatrix parameters at each event
 * @param[in] time times of events
 * @param[in] amt amount at each event
 * @param[in] rate rate at each event
 * @param[in] ii inter-dose interval at each event
 * @param[in] evid event identity:
 *                    (0) observation
 *                    (1) dosing
 *                    (2) other
 *                    (3) reset
 *                    (4) reset AND dosing
 * @param[in] cmt compartment number at each event
 * @param[in] addl additional dosing at each event
 * @param[in] ss steady state approximation at each event (0: no, 1: yes)
 * @return a matrix with predicted amount in each compartment
 *         at each event.
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6>
Eigen::Matrix<typename torsten::return_t<T0, T1, T2, T3, T4, T5, T6>::type,
              Eigen::Dynamic, Eigen::Dynamic>
pmx_solve_twocpt(const std::vector<T0>& time,
              const std::vector<T1>& amt,
              const std::vector<T2>& rate,
              const std::vector<T3>& ii,
              const std::vector<int>& evid,
              const std::vector<int>& cmt,
              const std::vector<int>& addl,
              const std::vector<int>& ss,
              const std::vector<std::vector<T4> >& pMatrix,
              const std::vector<std::vector<T5> >& biovar,
              const std::vector<std::vector<T6> >& tlag) {
  using std::vector;
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using boost::math::tools::promote_args;
  using stan::math::check_positive_finite;
  using refactor::PKRec;

  int nCmt = refactor::PMXTwoCptModel<double, double, double, double>::Ncmt;
  int nParms = refactor::PMXTwoCptModel<double, double, double, double>::Npar;
  static const char* function("pmx_solve_twocpt");

  // Check arguments
  torsten::pmx_check(time, amt, rate, ii, evid, cmt, addl, ss,
                pMatrix, biovar, tlag, function);
  for (size_t i = 0; i < pMatrix.size(); i++) {
    check_positive_finite(function, "PK parameter CL", pMatrix[i][0]);
    check_positive_finite(function, "PK parameter Q", pMatrix[i][1]);
    check_positive_finite(function, "PK parameter V2", pMatrix[i][2]);
    check_positive_finite(function, "PK parameter V3", pMatrix[i][3]);
  }
  std::string message4 = ", but must equal the number of parameters in the model: " // NOLINT
    + boost::lexical_cast<std::string>(nParms) + "!";
  const char* length_error4 = message4.c_str();
  if (!(pMatrix[0].size() == (size_t) nParms))
    stan::math::invalid_argument(function,
    "The number of parameters per event (length of a vector in the first argument) is", // NOLINT
    pMatrix[0].size(), "", length_error4);

  // FIX ME - we want to check every array of pMatrix, not
  // just the first one (at index 0)
  std::string message5 = ", but must equal the number of parameters in the model: " // NOLINT
  + boost::lexical_cast<std::string>(nParms) + "!";
  const char* length_error5 = message5.c_str();
  if (!(pMatrix[0].size() == (size_t) nParms))
    stan::math::invalid_argument(function,
    "The number of parameters per event (length of a vector in the ninth argument) is", // NOLINT
    pMatrix[0].size(), "", length_error5);

  std::string message6 = ", but must equal the number of compartments in the model: " // NOLINT
  + boost::lexical_cast<std::string>(nCmt) + "!";
  const char* length_error6 = message6.c_str();
  if (!(biovar[0].size() == (size_t) nCmt))
    stan::math::invalid_argument(function,
    "The number of biovariability parameters per event (length of a vector in the tenth argument) is", // NOLINT
    biovar[0].size(), "", length_error6);

  if (!(tlag[0].size() == (size_t) nCmt))
    stan::math::invalid_argument(function,
    "The number of lag times parameters per event (length of a vector in the eleventh argument) is", // NOLINT
    tlag[0].size(), "", length_error5);

  // Construct dummy matrix for last argument of pred
  Matrix<T4, Dynamic, Dynamic> dummy_system;
  vector<Matrix<T4, Dynamic, Dynamic> >
    dummy_systems(1, dummy_system);

#ifdef OLD_TORSTEN
  return Pred(time, amt, rate, ii, evid, cmt, addl, ss,
              pMatrix, biovar, tlag,
              nCmt, dummy_systems,
              Pred1_twoCpt(), PredSS_twoCpt());
#else
  using ER = NONMENEventsRecord<T0, T1, T2, T3, std::vector<T4>, T5, T6>;
  using EM = EventsManager<ER>;
  const ER events_rec(nCmt, time, amt, rate, ii, evid, cmt, addl, ss, pMatrix, biovar, tlag);

  Matrix<typename EM::T_scalar, Dynamic, Dynamic> pred =
    Matrix<typename EM::T_scalar, Dynamic, Dynamic>::Zero(events_rec.num_event_times(), EM::nCmt(events_rec));

  using model_type = refactor::PMXTwoCptModel<typename EM::T_time, typename EM::T_scalar, typename EM::T_rate, typename EM::T_par>;
  EventSolver<model_type> pr;
  pr.pred(0, events_rec, pred);
  return pred;

#endif
}

/**
 * Overload function to allow user to pass an std::vector for
 * pMatrix/bioavailability/tlag
 */
  template <typename T0, typename T1, typename T2, typename T3,
            typename T_par, typename T_biovar, typename T_tlag,
               typename
            std::enable_if_t<
              !(torsten::is_std_vector<T_par>::value && torsten::is_std_vector<T_biovar>::value && torsten::is_std_vector<T_tlag>::value)>* = nullptr> //NOLINT
  Eigen::Matrix <typename torsten::return_t<T0, T1, T2, T3,
                                            typename torsten::value_type<T_par>::type,
                                            typename torsten::value_type<T_biovar>::type,
                                            typename torsten::value_type<T_tlag>::type>::type,
                 Eigen::Dynamic, Eigen::Dynamic>
  pmx_solve_twocpt(const std::vector<T0>& time,
                   const std::vector<T1>& amt,
                   const std::vector<T2>& rate,
                   const std::vector<T3>& ii,
                   const std::vector<int>& evid,
                   const std::vector<int>& cmt,
                   const std::vector<int>& addl,
                   const std::vector<int>& ss,
                   const std::vector<T_par>& pMatrix,
                   const std::vector<T_biovar>& biovar,
                   const std::vector<T_tlag>& tlag) {
    auto param_ = torsten::to_array_2d(pMatrix);
    auto biovar_ = torsten::to_array_2d(biovar);
    auto tlag_ = torsten::to_array_2d(tlag);

    return pmx_solve_twocpt(time, amt, rate, ii, evid, cmt, addl, ss,
                            param_, biovar_, tlag_);
  }

  // old version
  template <typename T0, typename T1, typename T2, typename T3, typename T4,
            typename T5, typename T6>
  Eigen::Matrix <typename torsten::return_t<T0, T1, T2, T3, T4, T5, T6>::type,
                 Eigen::Dynamic, Eigen::Dynamic>
  PKModelTwoCpt(const std::vector<T0>& time,
                const std::vector<T1>& amt,
                const std::vector<T2>& rate,
                const std::vector<T3>& ii,
                const std::vector<int>& evid,
                const std::vector<int>& cmt,
                const std::vector<int>& addl,
                const std::vector<int>& ss,
                const std::vector<std::vector<T4> >& pMatrix,
                const std::vector<std::vector<T5> >& biovar,
                const std::vector<std::vector<T6> >& tlag) {
    auto x = pmx_solve_twocpt(time, amt, rate, ii, evid, cmt, addl, ss,
                              pMatrix, biovar, tlag);
    return x.transpose();
  }

  template <typename T0, typename T1, typename T2, typename T3,
            typename T_par, typename T_biovar, typename T_tlag,
               typename
            std::enable_if_t<
              !(torsten::is_std_vector<T_par>::value && torsten::is_std_vector<T_biovar>::value && torsten::is_std_vector<T_tlag>::value)>* = nullptr> //NOLINT
  Eigen::Matrix <typename torsten::return_t<T0, T1, T2, T3,
                                            typename torsten::value_type<T_par>::type,
                                            typename torsten::value_type<T_biovar>::type,
                                            typename torsten::value_type<T_tlag>::type>::type,
                 Eigen::Dynamic, Eigen::Dynamic>
  PKModelTwoCpt(const std::vector<T0>& time,
                   const std::vector<T1>& amt,
                   const std::vector<T2>& rate,
                   const std::vector<T3>& ii,
                   const std::vector<int>& evid,
                   const std::vector<int>& cmt,
                   const std::vector<int>& addl,
                   const std::vector<int>& ss,
                   const std::vector<T_par>& pMatrix,
                   const std::vector<T_biovar>& biovar,
                   const std::vector<T_tlag>& tlag) {
    auto x = pmx_solve_twocpt(time, amt, rate, ii, evid, cmt, addl, ss,
                            pMatrix, biovar, tlag);
    return x.transpose();
  }

  /*
   * For population models, we follow the call signature
   * but add the arrays of the length of each individual's data.
   * The size of that vector is the size of
   * the population.
   */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6>
Eigen::Matrix<typename EventsManager<NONMENEventsRecord<T0, T1, T2, T3, std::vector<T4>, T5, T6> >::T_scalar, // NOLINT
              Eigen::Dynamic, Eigen::Dynamic>
pmx_solve_group_twocpt(const std::vector<int>& len,
                       const std::vector<T0>& time,
                       const std::vector<T1>& amt,
                       const std::vector<T2>& rate,
                       const std::vector<T3>& ii,
                       const std::vector<int>& evid,
                       const std::vector<int>& cmt,
                       const std::vector<int>& addl,
                       const std::vector<int>& ss,
                       const std::vector<std::vector<T4> >& pMatrix,
                       const std::vector<std::vector<T5> >& biovar,
                       const std::vector<std::vector<T6> >& tlag) {
  using ER = NONMENEventsRecord<T0, T1, T2, T3, std::vector<T4>, T5, T6>;
  using EM = EventsManager<ER>;

  int nCmt = refactor::PMXTwoCptModel<double, double, double, double>::Ncmt;
  ER events_rec(nCmt, len, time, amt, rate, ii, evid, cmt, addl, ss, pMatrix, biovar, tlag);

  static const char* caller("pmx_solve_group_twocpt");
  torsten::pmx_population_check(len, time, amt, rate, ii, evid, cmt, addl, ss,
                                pMatrix, biovar, tlag, caller);

  using model_type = refactor::PMXTwoCptModel<typename EM::T_time, typename EM::T_scalar, typename EM::T_rate, typename EM::T_par>;
  EventSolver<model_type> pr;

  Eigen::Matrix<typename EM::T_scalar, -1, -1> pred(events_rec.total_num_event_times, nCmt);

  pr.pred(events_rec, pred);

  return pred;
}

}
#endif


#pragma once
#include <fc/crypto/elliptic.hpp>
#include <boost/config.hpp>

#ifdef _MSC_VER
extern "C"{
    struct ECDSA_SIG_st {
        BIGNUM *r;
        BIGNUM *s;
    };
}
#endif

/* public_key_impl implementation based on openssl
 * used by mixed + openssl
 */

namespace fc { namespace ecc { namespace detail {

void _init_lib();

class public_key_impl
{
    public:
        public_key_impl() BOOST_NOEXCEPT;
        public_key_impl( const public_key_impl& cpy ) BOOST_NOEXCEPT;
        public_key_impl( public_key_impl&& cpy ) BOOST_NOEXCEPT;
        ~public_key_impl() BOOST_NOEXCEPT;

        public_key_impl& operator=( const public_key_impl& pk ) BOOST_NOEXCEPT;

        public_key_impl& operator=( public_key_impl&& pk ) BOOST_NOEXCEPT;

        static int ECDSA_SIG_recover_key_GFp(EC_KEY *eckey, ECDSA_SIG *ecsig, const unsigned char *msg, int msglen, int recid, int check);

        EC_KEY* _key = nullptr;

    private:
        void free_key() BOOST_NOEXCEPT;
};

}}}


#include <ros/ros.h>
#include <boost/thread.hpp>
#include <armadillo>
#include <geometry_msgs/Twist.h>
#include <string>

//Includes specific to this package
#include <jog_controller/twist_to_jog_frame.h>
#include "jog_msgs/JogFrame.h"
#include "jog_msgs/ControllerStatus.h"
#include "jog_msgs/GetTargetList.h"
#include "jog_msgs/SetTarget.h"
#include "jog_msgs/SetCollisionAvoidance.h"

namespace jog_controller
{

TwistToJogFrame::TwistToJogFrame()
{
  ros::NodeHandle nh, pnh("~");
  jog_frame_pub_ = nh.advertise<jog_msgs::JogFrame>("jog_frame", 1);
  get_target_frame_list_srv_ = nh.advertiseService("/twist_to_jog_frame/get_target_frame_list", &TwistToJogFrame::getTargetFrameList, this);
  set_controller_status_srv_ = nh.advertiseService("/twist_to_jog_frame/set_controller_status", &TwistToJogFrame::setControllerStatus, this);
  set_target_frame_srv_ = nh.advertiseService("/twist_to_jog_frame/set_target_frame", &TwistToJogFrame::setTargetFrame, this);
  set_target_link_srv_ = nh.advertiseService("/twist_to_jog_frame/set_target_link", &TwistToJogFrame::setTargetLink, this);
  pnh.getParam("/jog_frame_node/group_names", group_names_);
  pnh.getParam("/jog_frame_node/link_names", link_names_);
  pnh.getParam("group_name", group_name_);
  pnh.getParam("link_name", link_name_);
  pnh.getParam("frame_id", frame_id_);
  pnh.getParam("rotate_axes", rotate_axes_);
  std::string str_rotation_matrix_;
  pnh.getParam("rotation_matrix", str_rotation_matrix_);
  rotation_matrix_ = arma::mat(str_rotation_matrix_);
  pnh.getParam("dominant_axis_mode", dominant_axis_mode_);
  pnh.getParam("scale_linear", scale_linear_);
  pnh.getParam("scale_angular", scale_angular_);
  pnh.getParam("sub_topic", sub_topic_);
  pnh.param("avoid_collisions", avoid_collisions_, true);
  pnh.getParam("controller_enabled", controller_enabled_);
  twist_sub_ = nh.subscribe(sub_topic_, 10, &TwistToJogFrame::twistCallback, this);
  ros::topic::waitForMessage<geometry_msgs::Twist>(sub_topic_);
}

void TwistToJogFrame::keepOnlyDominantAxis(arma::vec6 &twist_p)
{
  double min = twist_p.min();
  double max = twist_p.max();
  double min_max = max + min;
  if(min_max > 0){
    arma::uword i = twist_p.index_max();
    twist_p.zeros();
    twist_p.at(i) = max;
  } else {
    arma::uword i = twist_p.index_min();
    twist_p.zeros();
    twist_p.at(i) = min;
  }
}

void TwistToJogFrame::rotateAxes(arma::vec6 &twist_p)
{
  arma::vec linear = arma::vec3({twist_p.at(0), twist_p.at(1), twist_p.at(2)});
  arma::vec angular = arma::vec3({twist_p.at(3), twist_p.at(4), twist_p.at(5)});

  linear = rotation_matrix_ * linear;
  angular = rotation_matrix_ * angular;

  //Write in original vector
  for(arma::uword i = 0; i < 3; i++){
    twist_p[i] = linear[i];
    twist_p[i+3] = angular[i];
  }
}

bool TwistToJogFrame::hasDuplicate(const arma::vec6 &twist_p)
{
  arma::vec nz_tmp = arma::nonzeros(twist_p);
  arma::vec unique_tmp = arma::unique(nz_tmp);
  return size(unique_tmp) != size(nz_tmp);
}

void TwistToJogFrame::scaleCommand(arma::vec6 &twist_p)
{
  for(arma::uword i = 0; i < 3; i++)
  {
    // x,y,z
    twist_p.at(i) = twist_p.at(i) * scale_linear_;
    // rx,ry,rz
    twist_p.at(i+3) = twist_p.at(i+3) * scale_angular_;
  }
}

void TwistToJogFrame::twistCallback(const geometry_msgs::TwistConstPtr &twist)
{
  boost::mutex::scoped_lock lock(mutex_);

  // publish
  jog_msgs::JogFrame msg;
  msg.header.stamp = ros::Time::now();
  msg.header.frame_id = frame_id_;
  msg.group_name = group_name_;
  msg.link_name = link_name_;

  arma::vec6 v_twist = arma::vec6({twist->linear.x, twist->linear.y, twist->linear.z, twist->angular.x, twist->angular.y, twist->angular.z});

  if(rotate_axes_)
  {
    rotateAxes(v_twist);
  }

  scaleCommand(v_twist);

  if(dominant_axis_mode_)
  {
    if(hasDuplicate(v_twist))
    {
      v_twist.zeros(); //handling the edge case
    } else {
      keepOnlyDominantAxis(v_twist);
    }
  }

  msg.avoid_collisions = avoid_collisions_;

  // Publish if the button is enabled and if at least one of the commands is different from zero.
  if(controller_enabled_ && arma::any(v_twist))
  {
    //Adding linear and angular values to the msg.
    msg.linear_delta.x = v_twist.at(0);
    msg.linear_delta.y = v_twist.at(1);
    msg.linear_delta.z = v_twist.at(2);
    msg.angular_delta.x = v_twist.at(3);
    msg.angular_delta.y = v_twist.at(4);
    msg.angular_delta.z = v_twist.at(5);

    jog_frame_pub_.publish(msg);
  }
}

bool TwistToJogFrame::getTargetFrameList(jog_msgs::GetTargetListRequest &req, jog_msgs::GetTargetListResponse &res)
{
  res.target = link_names_;
  return true;
}

//callback for controller_enable service
bool TwistToJogFrame::setControllerStatus(jog_msgs::ControllerStatusRequest &req, jog_msgs::ControllerStatusResponse &res)
{
  controller_enabled_ = req.status;
  return true;
}

bool TwistToJogFrame::setTargetFrame(jog_msgs::SetTargetRequest &target_frame, jog_msgs::SetTargetResponse &res)
{
  frame_id_ = target_frame.name;
  return true;
}

bool TwistToJogFrame::setTargetLink(jog_msgs::SetTargetRequest &target_link, jog_msgs::SetTargetResponse &res)
{
  link_name_ = target_link.name;
  return true;
}

bool TwistToJogFrame::setCollisionAvoidance(jog_msgs::SetCollisionAvoidanceRequest &req, jog_msgs::SetCollisionAvoidanceResponse &res)
{
  avoid_collisions_ = req.status;
  return true;
}

}  // namespace jog_controller

/**
 * @brief Main function of the node
 */
int main(int argc, char **argv)
{
  ros::init(argc, argv, "twist_to_jog_frame");
  jog_controller::TwistToJogFrame node;

  ros::Rate loop_rate(10);
  while ( ros::ok() )
  {
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_DSL_FUNCTIONS_PACK_TERMINAL_HPP_INCLUDED
#define BOOST_SIMD_DSL_FUNCTIONS_PACK_TERMINAL_HPP_INCLUDED

#include <boost/simd/dsl/functions/terminal.hpp>
#include <boost/simd/sdk/functor/preprocessor/call.hpp>
#include <boost/proto/traits.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_TPL( boost::simd::tag::terminal_,tag::cpu_
                                , (class Value)(class State)
                                  (class Data)(std::size_t N)
                                , ((array_<scalar_< arithmetic_<Value > >,N>))
                                  ((target_<array_<scalar_< arithmetic_<State> >,N> >))
                                  (scalar_< integer_<Data> >)
                                )
{
    typedef typename Value::value_type result_type;

    BOOST_DISPATCH_FORCE_INLINE result_type
    operator()( Value const& v, State const&, Data const& p ) const
    {
      return v[p];
    }
  };
} } }

#endif


// All content Copyright (C) 2018 Genomics plc
#include "variant/type/variant.hpp"

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <string>

using wecall::utils::ReferenceSequence;
using wecall::caller::Region;

BOOST_AUTO_TEST_CASE( mnp )
{
    using namespace wecall::variant;

    std::string contig = "1";
    const std::string removed( "ABCD" );
    const std::string added( "HELL" );

    const auto referenceSequence =
        std::make_shared< ReferenceSequence >( Region( contig, 1000000, 1000000 + removed.size() ), removed );
    varPtr_t theMNP = std::make_shared< Variant >( referenceSequence, referenceSequence->region(), added );

    BOOST_CHECK_EQUAL( theMNP->sequence(), added );
    BOOST_CHECK_EQUAL( theMNP->refSequence().sequence(), removed );

    BOOST_CHECK_EQUAL( theMNP->start(), 1000000 );
    BOOST_CHECK_EQUAL( theMNP->end(), 1000004 );

    auto nDiffs = 4;
    auto expectedPrior = 5e-5 * pow( 0.1, nDiffs - 1 ) * ( 1.0 - 0.1 );

    wecall::variant::setDefaultPriors( {theMNP} );
    BOOST_CHECK_CLOSE( theMNP->prior(), expectedPrior, 1e-5 );
}

BOOST_AUTO_TEST_CASE( testMnpLeftAlignDoesntChangePosition )
{
    using namespace wecall::variant;

    std::string contig = "1";
    const std::string seq = "TT";

    const auto referenceSequence = std::make_shared< ReferenceSequence >( Region( contig, 0, 10 ), "ATATATATAA" );

    varPtr_t theMnp = std::make_shared< Variant >( referenceSequence, Region( contig, 8, 10 ), seq );

    theMnp->getLeftAligned( 0 );

    BOOST_CHECK_EQUAL( theMnp->start(), 8 );
}


// Boost.Geometry Index
//
// boxes union/intersection area/volume
//
// Copyright (c) 2011-2014 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_INTERSECTION_CONTENT_HPP
#define BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_INTERSECTION_CONTENT_HPP

#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/strategies/intersection.hpp>
#include <boost/geometry/index/detail/algorithms/content.hpp>

namespace boost { namespace geometry { namespace index { namespace detail {

/**
 * \brief Compute the area of the intersection of b1 and b2
 */
template <typename Box>
inline typename default_content_result<Box>::type intersection_content(Box const& box1, Box const& box2)
{
    if ( geometry::intersects(box1, box2) )
    {
        Box box_intersection;
        if ( geometry::intersection(box1, box2, box_intersection) )
            return detail::content(box_intersection);
    }
    return 0;
}

}}}} // namespace boost::geometry::index::detail

#endif // BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_INTERSECTION_CONTENT_HPP


#include <boost/hana.hpp>
#include <boost/utility/enable_if.hpp>
#include <string>
#include <iostream>

struct person {
    BOOST_HANA_DEFINE_STRUCT(person,
        (std::string, name),
        (int, age)
    );
};

template <class T>
typename boost::disable_if<boost::hana::Foldable<T>>::type serialize(T t) { std::cout << t << '\n'; }

template <class T>
typename boost::enable_if<boost::hana::Foldable<T>>::type serialize(T t) { boost::hana::for_each(t, [](auto pair){ std::cout << boost::hana::second(pair) << '\n'; }); }

int main()
{
    person tom{ "Tom", 35 };
    serialize(tom);

    std::string s{"String"};
    serialize(s);

    int i{1};
    serialize(i);
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include <boost/preprocessor/control/if.hpp>

#include "DataStructures/DataBox/PrefixHelpers.hpp"
#include "DataStructures/DataBox/Prefixes.hpp"
#include "Evolution/DiscontinuousGalerkin/Actions/VolumeTermsImpl.tpp"
#include "Evolution/Systems/NewtonianEuler/System.hpp"
#include "Evolution/Systems/NewtonianEuler/TimeDerivativeTerms.hpp"
#include "PointwiseFunctions/AnalyticData/NewtonianEuler/KhInstability.hpp"
#include "PointwiseFunctions/AnalyticSolutions/NewtonianEuler/IsentropicVortex.hpp"
#include "PointwiseFunctions/AnalyticSolutions/NewtonianEuler/LaneEmdenStar.hpp"
#include "PointwiseFunctions/AnalyticSolutions/NewtonianEuler/RiemannProblem.hpp"
#include "PointwiseFunctions/AnalyticSolutions/NewtonianEuler/SmoothFlow.hpp"
#include "Utilities/GenerateInstantiations.hpp"

namespace evolution::dg::Actions::detail {
#define DIM(data) BOOST_PP_TUPLE_ELEM(0, data)
#define INITIAL_DATA_TYPE(data) BOOST_PP_TUPLE_ELEM(1, data)<DIM(data)>
#define SYSTEM(data) \
  ::NewtonianEuler::System<DIM(data), INITIAL_DATA_TYPE(data)>

#define INSTANTIATION(r, data)                                                \
  template void volume_terms<::NewtonianEuler::TimeDerivativeTerms<           \
      DIM(data), INITIAL_DATA_TYPE(data)>>(                                   \
      const gsl::not_null<Variables<db::wrap_tags_in<                         \
          ::Tags::dt, typename SYSTEM(data)::variables_tag::tags_list>>*>     \
          dt_vars_ptr,                                                        \
      const gsl::not_null<Variables<db::wrap_tags_in<                         \
          ::Tags::Flux, typename SYSTEM(data)::flux_variables,                \
          tmpl::size_t<DIM(data)>, Frame::Inertial>>*>                        \
          volume_fluxes,                                                      \
      const gsl::not_null<Variables<db::wrap_tags_in<                         \
          ::Tags::deriv, typename SYSTEM(data)::gradient_variables,           \
          tmpl::size_t<DIM(data)>, Frame::Inertial>>*>                        \
          partial_derivs,                                                     \
      const gsl::not_null<Variables<typename SYSTEM(                          \
          data)::compute_volume_time_derivative_terms::temporary_tags>*>      \
          temporaries,                                                        \
      const Variables<typename SYSTEM(data)::variables_tag::tags_list>&       \
          evolved_vars,                                                       \
      const ::dg::Formulation dg_formulation, const Mesh<DIM(data)>& mesh,    \
      [[maybe_unused]] const tnsr::I<DataVector, DIM(data), Frame::Inertial>& \
          inertial_coordinates,                                               \
      const InverseJacobian<DataVector, DIM(data), Frame::ElementLogical,     \
                            Frame::Inertial>&                                 \
          logical_to_inertial_inverse_jacobian,                               \
      [[maybe_unused]] const Scalar<DataVector>* const det_inverse_jacobian,  \
      const std::optional<tnsr::I<DataVector, DIM(data), Frame::Inertial>>&   \
          mesh_velocity,                                                      \
      const std::optional<Scalar<DataVector>>& div_mesh_velocity,             \
      const tnsr::I<DataVector, DIM(data)>& momentum_density,                 \
      const Scalar<DataVector>& energy_density,                               \
      const tnsr::I<DataVector, DIM(data)>& velocity,                         \
      const Scalar<DataVector>& pressure);

GENERATE_INSTANTIATIONS(INSTANTIATION, (1, 2, 3),
                        (::NewtonianEuler::Solutions::RiemannProblem,
                         ::NewtonianEuler::Solutions::SmoothFlow))

GENERATE_INSTANTIATIONS(INSTANTIATION, (2, 3),
                        (::NewtonianEuler::AnalyticData::KhInstability))

GENERATE_INSTANTIATIONS(INSTANTIATION, (2),
                        (::NewtonianEuler::Solutions::IsentropicVortex))

#undef INSTANTIATION
#undef SYSTEM
#undef INITIAL_DATA_TYPE
#undef DIM

using isentropic_vortex_3d_system = ::NewtonianEuler::System<
    3, ::NewtonianEuler::Solutions::IsentropicVortex<3>>;

template void volume_terms<::NewtonianEuler::TimeDerivativeTerms<
    3, ::NewtonianEuler::Solutions::IsentropicVortex<3>>>(
    const gsl::not_null<Variables<db::wrap_tags_in<
        ::Tags::dt, isentropic_vortex_3d_system::variables_tag::tags_list>>*>
        dt_vars_ptr,
    const gsl::not_null<Variables<db::wrap_tags_in<
        ::Tags::Flux, isentropic_vortex_3d_system::flux_variables,
        tmpl::size_t<3>, Frame::Inertial>>*>
        volume_fluxes,
    const gsl::not_null<Variables<db::wrap_tags_in<
        ::Tags::deriv, isentropic_vortex_3d_system::gradient_variables,
        tmpl::size_t<3>, Frame::Inertial>>*>
        partial_derivs,
    const gsl::not_null<
        Variables<isentropic_vortex_3d_system::
                      compute_volume_time_derivative_terms::temporary_tags>*>
        temporaries,
    const Variables<isentropic_vortex_3d_system::variables_tag::tags_list>&
        evolved_vars,
    const ::dg::Formulation dg_formulation, const Mesh<3>& mesh,
    [[maybe_unused]] const tnsr::I<DataVector, 3, Frame::Inertial>&
        inertial_coordinates,
    const InverseJacobian<DataVector, 3, Frame::ElementLogical,
                          Frame::Inertial>&
        logical_to_inertial_inverse_jacobian,
    [[maybe_unused]] const Scalar<DataVector>* const det_inverse_jacobian,
    const std::optional<tnsr::I<DataVector, 3, Frame::Inertial>>& mesh_velocity,
    const std::optional<Scalar<DataVector>>& div_mesh_velocity,
    const tnsr::I<DataVector, 3>& momentum_density,
    const Scalar<DataVector>& energy_density,
    const tnsr::I<DataVector, 3>& velocity, const Scalar<DataVector>& pressure,
    const NewtonianEuler::Sources::VortexPerturbation& source,
    const NewtonianEuler::Solutions::IsentropicVortex<3>& vortex,
    const tnsr::I<DataVector, 3>& x, const double& time);

using lane_emden_star_system =
    ::NewtonianEuler::System<3, ::NewtonianEuler::Solutions::LaneEmdenStar>;

template void volume_terms<::NewtonianEuler::TimeDerivativeTerms<
    3, ::NewtonianEuler::Solutions::LaneEmdenStar>>(
    const gsl::not_null<Variables<db::wrap_tags_in<
        ::Tags::dt,
        typename lane_emden_star_system::variables_tag::tags_list>>*>
        dt_vars_ptr,
    const gsl::not_null<Variables<db::wrap_tags_in<
        ::Tags::Flux, typename lane_emden_star_system::flux_variables,
        tmpl::size_t<3>, Frame::Inertial>>*>
        volume_fluxes,
    const gsl::not_null<Variables<db::wrap_tags_in<
        ::Tags::deriv, typename lane_emden_star_system::gradient_variables,
        tmpl::size_t<3>, Frame::Inertial>>*>
        partial_derivs,
    const gsl::not_null<
        Variables<typename lane_emden_star_system::
                      compute_volume_time_derivative_terms::temporary_tags>*>
        temporaries,
    const Variables<typename lane_emden_star_system::variables_tag::tags_list>&
        evolved_vars,
    const ::dg::Formulation dg_formulation, const Mesh<3>& mesh,
    [[maybe_unused]] const tnsr::I<DataVector, 3, Frame::Inertial>&
        inertial_coordinates,
    const InverseJacobian<DataVector, 3, Frame::ElementLogical,
                          Frame::Inertial>&
        logical_to_inertial_inverse_jacobian,
    [[maybe_unused]] const Scalar<DataVector>* const det_inverse_jacobian,
    const std::optional<tnsr::I<DataVector, 3, Frame::Inertial>>& mesh_velocity,
    const std::optional<Scalar<DataVector>>& div_mesh_velocity,
    const tnsr::I<DataVector, 3>& momentum_density,
    const Scalar<DataVector>& energy_density,
    const tnsr::I<DataVector, 3>& velocity, const Scalar<DataVector>& pressure,
    const NewtonianEuler::Sources::LaneEmdenGravitationalField& source,
    const Scalar<DataVector>& mass_density_cons_for_source,
    const tnsr::I<DataVector, 3>& momentum_density_for_source,
    const NewtonianEuler::Solutions::LaneEmdenStar& star,
    const tnsr::I<DataVector, 3>& x);
}  // namespace evolution::dg::Actions::detail


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/xray/sampling_base.h>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/return_internal_reference.hpp>

namespace cctbx { namespace xray { namespace boost_python {

namespace {

  struct sampling_base_wrappers
  {
    typedef sampling_base<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      typedef return_internal_reference<> rir;
      class_<w_t>("sampling_base", no_init)
        .def("unit_cell", &w_t::unit_cell, rir())
        .def("u_base", &w_t::u_base)
        .def("u_extra", &w_t::u_extra)
        .def("u_min", &w_t::u_min)
        .def("wing_cutoff", &w_t::wing_cutoff)
        .def("exp_table_one_over_step_size",
          &w_t::exp_table_one_over_step_size)
        .def("tolerance_positive_definite",
          &w_t::tolerance_positive_definite)
        .def("n_scatterers_passed", &w_t::n_scatterers_passed)
        .def("n_contributing_scatterers", &w_t::n_contributing_scatterers)
        .def("n_anomalous_scatterers", &w_t::n_anomalous_scatterers)
        .def("anomalous_flag", &w_t::anomalous_flag)
        .def("exp_table_size", &w_t::exp_table_size)
        .def("max_sampling_box_n_points", &w_t::max_sampling_box_n_points)
        .def("sum_sampling_box_n_points", &w_t::sum_sampling_box_n_points)
        .def("ave_sampling_box_n_points", &w_t::ave_sampling_box_n_points)
        .def("max_sampling_box_edges", &w_t::max_sampling_box_edges, ccr())
        .def("max_sampling_box_edges_frac", &w_t::max_sampling_box_edges_frac)
        .def("excessive_sampling_radius_i_seqs",
          &w_t::excessive_sampling_radius_i_seqs, ccr())
      ;
    }
  };

} // namespace <anoymous>

  void wrap_sampling_base()
  {
    using namespace boost::python;

    def("calc_u_base", calc_u_base, (
      arg("d_min"),
      arg("grid_resolution_factor"),
      arg("quality_factor")=100,
      arg("max_u_base")=adptbx::b_as_u(1000)));

    def("apply_u_extra",
      (void(*)(
        uctbx::unit_cell const&,
        double const&,
        af::const_ref<miller::index<> > const&,
        af::ref<std::complex<double> > const&,
        double const&)) apply_u_extra, (
          arg("unit_cell"),
          arg("u_extra"),
          arg("miller_indices"),
          arg("structure_factors"),
          arg("multiplier")=1));

    sampling_base_wrappers::wrap();
  }

}}} // namespace cctbx::xray::boost_python