//   GAMBIT: Global and Modular BSM Inference Tool
//   *********************************************
///  \file
///
///  Type definition and function declaration header for decay chain setup.
///
///
///  *********************************************
///
///  Authors (add name and date if you modify):
///
///  \author Lars A. Dal
///          (l.a.dal@fys.uio.no)
///  \date 2014 Oct, Nov, Dec
///  \date 2015 Jan
///  *********************************************

#ifndef __decay_chain_hpp__
#define __decay_chain_hpp__

#include <vector>
#include <unordered_map>
#include <string>
#include <set>
#include <boost/shared_ptr.hpp>
#include "gambit/Utils/threadsafe_rng.hpp"

namespace Gambit
{
    namespace DarkBit
    {
        struct TH_Channel;
        class TH_ProcessCatalog;
        class SimYieldTable;
        namespace DecayChain
        {
            using std::vector;
            using std::ofstream;
            using std::ostream;
            using std::string;
            using boost::shared_ptr;
            using std::unordered_map;
            using std::set;

            //  *********************************************
            //  Generic 3-vector class
            //  *********************************************
            class vec3
            {
                public:
                    double vals[3];
                    // Constructors
                    vec3(){}
                    vec3(double v0, double v1, double v2) {vals[0]=v0;vals[1]=v1;vals[2]=v2;}
                    vec3(double v0){vals[0]=v0;vals[1]=v0;vals[2]=v0;}
                    // Get the length of the vector
                    double length() const;
                    // Normalize vector to unit length
                    void normalize();
                    // Normalize vector to length len
                    void normalize(const double len);
                    // Operators
                    double& operator[](int i){ return vals[i];}
                    double operator[](int i)const{ return vals[i];}
                    vec3 operator-()const{return vec3(-vals[0],-vals[1],-vals[2]);	}
            };
            vec3 operator* (double x, const vec3 &y);
            vec3 operator* (const vec3 &y, double x);
            vec3 operator/ (const vec3 &y, double x);
            ostream& operator<<(ostream& os, const vec3& v);
            double dot(const vec3 &a, const vec3 &b);

            //  *********************************************
            //  Generic 4-vector class
            //  *********************************************
            class vec4
            {
                public:
                    double vals[4];
                    // Constructors
                    vec4(){}
                    vec4(double v0, double v1, double v2, double v3){vals[0]=v0;vals[1]=v1;vals[2]=v2;vals[3]=v3;}
                    vec4(double v0){vals[0]=v0;vals[1]=v0;vals[2]=v0;vals[3]=v0;}
                    vec4(double v0, vec3 v){vals[0]=v0;vals[1]=v[0];vals[2]=v[1];vals[3]=v[2];}
                    // Returns vec3 containting elements 1,2,3
                    vec3 xyz() const{return vec3(vals[1],vals[2],vals[3]);}
                    // Operators
                    double& operator[](int i){ return vals[i];}
                    double operator[](int i)const{ return vals[i];}
                    vec4 operator-(){return vec4(-vals[0],-vals[1],-vals[2],-vals[3]);}
            };
            vec4 operator* (double x, const vec4 &y);
            vec4 operator* (const vec4 &y, double x);
            vec4 operator+ (const vec4 &x, const vec4 &y);
            vec4 operator- (const vec4 &x, const vec4 &y);
            ostream& operator<<(ostream& os, const vec4& v);
            double dot(const vec4 &a, const vec4 &b);
            // Construct an energy-momentum 4-vector from the given 3-momentum and mass
            vec4 Ep4vec(const vec3 p, double m);

            //  *********************************************
            //  Generic 4x4 class
            //  *********************************************
            class mat4
            {
                public:
                    double vals[4][4];
                    mat4(){};
                    mat4(   double v00, double v01, double v02, double v03,
                            double v10, double v11, double v12, double v13,
                            double v20, double v21, double v22, double v23,
                            double v30, double v31, double v32, double v33);
                    mat4(double v);
                    // Identity matrix
                    static mat4 identity();
            };
            vec4 operator* (const mat4 &m, const vec4 &v);
            mat4 operator* (const mat4 &m1, const mat4 &m2);
            ostream& operator<<(ostream& os, const mat4& m);

            //  *********************************************
            //  Utility functions
            //  *********************************************
            // Generate a random number between -1 and 1
            double rand_m1_1();
            // Generate a random number between 0 and 1
            inline double rand_0_1(){return Random::draw();}
            // Generate a 3-vector to a random point on the unit sphere
            vec3 randOnSphere();
            // Calculate Lorentz boost matrix corresponding to beta_xyz.
            // Always calculate gamma=E/m and use the second version when possible, as this is far more numerically stable.
            void lorentzMatrix(const vec3 &beta_xyz, mat4 &mat);
            void lorentzMatrix(const vec3 &beta_xyz, mat4 &mat, double gamma);
            // Boost inVec according to beta_xyz.
            vec4 lorentzBoost(const vec4 &inVec, const vec3 &beta_xyz);
            // Boost inVec to the frame where a particle at rest (in this frame) would have 4-momentum p_parent
            vec4 p_parentFrame(const vec4 &inVec, const vec4 &p_parent);
            // Get Lorentz matrix for boosting to parent frame
            void boostMatrixParentFrame(mat4 &mat, vec4 &p_parent);
            // Calculate the invariant mass of the given 4-vector pair
            double invariantMass(const vec4 &a, const vec4 &b);

            //  *********************************************
            //  Class containing all the allowed decay channels of a single particle,
            //  as well as the particle mass.
            //  Contains functonality for picking random decays according to their decay widths
            //  *********************************************
            class DecayTableEntry
            {
                friend class DecayTable;
                public:
                    // Particle mass
                    const double m;
                    // Is the particle stable in the context of the decay chain?
                    bool stable;
                    // Flags indicating whether or not the various decay states are endpoints
                    unordered_map<const TH_Channel*, bool> endpointFlags;
                    // Constructor
                    DecayTableEntry(string pID, double m, bool stable) :
                        m(m), stable(stable), enabledWidth(0),
                        totalWidth(0), invisibleWidth(0), useForcedTotalWidth(false), forcedTotalWidth(0), pID(pID), randInit(false){}
                    // Dummy constructor (for the DecayTable map)
                    DecayTableEntry() :
                        m(0), stable(false), enabledWidth(0),
                        totalWidth(0), invisibleWidth(0), useForcedTotalWidth(false), forcedTotalWidth(0), pID(""),  randInit(false){}
                    // Pick a random decay channel
                    bool randomDecay(const TH_Channel* &decay) const;
                    // Update decay widths and if necessary Monte Carlo table
                    void update();
                    // Functions for checking if a decay exists in the decay tables
                    bool isEnabled(const TH_Channel *in) const;
                    bool isDisabled(const TH_Channel *in) const;
                    bool isRegistered(const TH_Channel *in) const;
                    // Add decay channel as enabled or disabled
                    void addChannel(const TH_Channel *in);
                    void addDisabled(const TH_Channel *in);
                    void setInvisibleWidth(double width);
                    // Functions for enabling/disabling (registered) decays.
                    // Returns false if decay is in the list of disabled/enabled decays
                    bool enableDecay(const TH_Channel *in);
                    bool disableDecay(const TH_Channel *in);
                    // Get branching ratio of enabled decays
                    double getEnabledBranching() const;
                    // Set total decay width to a fixed value
                    void forceTotalWidth(bool enabled, double width);
                    // Get total decay width
                    double getTotalWidth() const;
                    bool hasEnabledDecays() const;
                private:
                    // Lists of decays
                    vector<const TH_Channel*> enabledDecays;
                    vector<const TH_Channel*> disabledDecays;
                    // Table used for picking random decays
                    mutable vector<double> randLims;
                    // Decay widths of enabled channels and all channels
                    double enabledWidth;
                    double totalWidth;
                    double invisibleWidth;
                    bool useForcedTotalWidth;
                    double forcedTotalWidth;
                    // String particle identifier
                    string pID;
                    // Initialization status of Monte Carlo table
                    mutable bool randInit;
                    // Function used in picking a random decay
                    int findChannelIdx(double pick) const;
                    // Generate table for picking a random decay in Monte Carlo decay chains
                    void generateRandTable() const;
            };

            //  *********************************************
            //  Table of all particles and their decay channels.
            //  Uses particle PID as array index
            //  *********************************************
            class DecayTable
            {
                public:
                    DecayTable(const TH_ProcessCatalog &cat, const SimYieldTable &tab, set<string> disabledList);
                    DecayTable(){};
                    bool hasEntry(string) const;
                    // Add particle to decay table, specifying particle ID, mass and whether or not it should be decayed in decay chains
                    void addEntry(string pID, double m, bool stable);
                    void addEntry(string pID, DecayTableEntry entry);
                    bool randomDecay(string pID, const TH_Channel* &decay) const;
                    const DecayTableEntry& operator[](string i) const;
                    // Retrieve width of decay channel
                    static double getWidth(const TH_Channel *ch);
                    // Print the decay table (to cout)
                    void printTable() const;
                private:
                    unordered_map<string,DecayTableEntry> table;
            };

            //  *********************************************
            //  The main decay chain class.
            //  Each link (particle) in the decay chain is an instance of this class,
            //  with pointers to its parent and child links.
            //  *********************************************
            class ChainParticle
            {
                public:
                    // Invariant (rest) mass
                    const double m;
                    // Constructor for the base node (top particle in the decay chain).
                    ChainParticle(vec3 ipLab, const DecayTable *dc, string pID);
                    // Iteratively add random links to the decay chain by Monte Carlo to a maximum length of maxSteps or minimum energy of Emin.
                    // Use negative numbers to turn off limits
                    void generateDecayChainMC(int maxSteps, double Emin);
                    // Draw new angles for the decay products in this and all subsequent links of the decay chain
                    void reDrawAngles();
                    // Remove all subsequent links in the decay chain
                    void cutChain();
                    // Boost a given 4-momentum from this frame to the lab frame.
                    vec4 p_to_Lab(const vec4 &p) const;
                    // Calculate 4-momentum of this particle in the lab frame.
                    vec4 p_Lab() const;
                    // Calculate the energy of this particle in the lab frame. Faster than calculating the full 4-momentum.
                    double E_Lab() const;
                    // Iteratively collect decay chain endpoint states. Optional argument to only collect particles of certain types.
                    // Note that this function will not collect decay products of endpoint states. These must be extracted manually.
                    void collectEndpointStates(vector<const ChainParticle*> &endpointStates, bool includeAborted, string ipID="") const;
                    // Get number of child particles
                    int getnChildren() const {return nChildren;}
                    // Get child particle
                    const ChainParticle* operator[](int i) const;
                    // Get parent particle
                    const ChainParticle* getParent() const;
                    // Get energy in parent frame
                    double E_parentFrame() const;
                    // Get particle ID
                    string getpID() const {return pID;}
                    // Print the decay chain (to cout)
                    void printChain() const;
                    // Get weight factor (see description of the weight variable)
                    double getWeight() const {return weight;}
                    // Get boost between lab frame and CoM frame of this particle
                    void getBoost(double& gamma, double& beta) const;
                    // Get pointer to decay table
                    const DecayTable* getDecayTable() const {return decayTable;}
                    // Destructor
                    ~ChainParticle();
                private:
                    // Helper function for printChain()
                    bool printChain(int generation, vector<int> ancestry) const;
                    // How much the decay chain (to this point) should be weighted down due to
                    // missing/disabled decay channels. Only relevant for Monte Carlo generated chains.
                    double weight;
                    // Pointer to decay table
                    const DecayTable *decayTable;
                    // Lorentz matrices
                    mat4 boostToParentFrame;
                    mat4 boostToLabFrame;
                    // 4-momentum in parent's rest frame
                    vec4 p_parent;
                    // Particle identifier
                    string pID;
                    // How many ancestors do I have?
                    int chainGeneration;
                    // Has this particle been kept from decaying by an energy or chain length cut?
                    bool abortedDecay;
                    // Is this particle stable, or has it decayed to a final state consisting entirely of stable particles?
                    // If so, this particle is considered a final state, and any children must be extracted manually after collecting the final states.
                    // This is to avoid having final states that can't be treated as free particles (such as quarks).
                    bool isEndpoint;
                    // Number of child particles
                    int nChildren;
                    // Pointers to parent and child particles
                    ChainParticle *parent;
                    vector<ChainParticle*> children;
                    // Function for updating the Lorentz boost matrices according to a new 4-momentum.
                    void update(vec4 &ip_parent);
                    // Constructor used by member functions during chain generation.
                    ChainParticle(const vec4 &pp, double m, double weight, const DecayTable *dc, ChainParticle *parent, int chainGeneration, string pID);
                    // Disable copy constructor and assignment operator. These would cause mayhem.
                    ChainParticle(const ChainParticle&);
                    ChainParticle & operator=(const ChainParticle&);
            };
            typedef std::vector<const Gambit::DarkBit::DecayChain::ChainParticle*> ChainParticleVector;

            // Container for passing around ChainParticle objects.
            struct ChainContainer
            {
                ChainContainer(){}
                ChainContainer(shared_ptr<ChainParticle> ch) : chain(ch) {}
                shared_ptr<const ChainParticle> chain;
            };

        }
    }
}

#endif // defined __decay_chain_hpp__


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// 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_STRATEGY_AGNOSTIC_POINT_IN_POLY_WINDING_HPP
#define BOOST_GEOMETRY_STRATEGY_AGNOSTIC_POINT_IN_POLY_WINDING_HPP

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/strategies/side.hpp>
#include <boost/geometry/strategies/within.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace within
{

/*!
\brief Within detection using winding rule
\ingroup strategies
\tparam Point \tparam_point
\tparam PointOfSegment \tparam_segment_point
\tparam CalculationType \tparam_calculation
\author Barend Gehrels
\note The implementation is inspired by terralib http://www.terralib.org (LGPL)
\note but totally revised afterwards, especially for cases on segments
\note Only dependant on "side", -> agnostic, suitable for spherical/latlong

\qbk{
[heading See also]
[link geometry.reference.algorithms.within.within_3_with_strategy within (with strategy)]
}
 */
template
<
    typename Point,
    typename PointOfSegment = Point,
    typename CalculationType = void
>
class winding
{
    typedef typename select_calculation_type
        <
            Point,
            PointOfSegment,
            CalculationType
        >::type calculation_type;

    typedef typename strategy::side::services::default_strategy
        <
            typename cs_tag<Point>::type
        >::type strategy_side_type;

    /*! subclass to keep state */
    class counter
    {
        int m_count;
        bool m_touches;

        inline int code() const
        {
            return m_touches ? 0 : m_count == 0 ? -1 : 1;
        }

    public :
        friend class winding;

        inline counter()
            : m_count(0)
            , m_touches(false)
        {}

    };

    template <size_t D>
    static inline int check_touch(Point const& point,
                PointOfSegment const& seg1, PointOfSegment const& seg2,
                counter& state)
    {
        calculation_type const p = get<D>(point);
        calculation_type const s1 = get<D>(seg1);
        calculation_type const s2 = get<D>(seg2);
        if ((s1 <= p && s2 >= p) || (s2 <= p && s1 >= p))
        {
            state.m_touches = true;
        }
        return 0;
    }

    template <size_t D>
    static inline int check_segment(Point const& point,
                PointOfSegment const& seg1, PointOfSegment const& seg2,
                counter& state)
    {
        calculation_type const p = get<D>(point);
        calculation_type const s1 = get<D>(seg1);
        calculation_type const s2 = get<D>(seg2);

        // Check if one of segment endpoints is at same level of point
        bool eq1 = math::equals(s1, p);
        bool eq2 = math::equals(s2, p);

        if (eq1 && eq2)
        {
            // Both equal p -> segment is horizontal (or vertical for D=0)
            // The only thing which has to be done is check if point is ON segment
            return check_touch<1 - D>(point, seg1, seg2,state);
        }

        return
              eq1 ? (s2 > p ?  1 : -1)  // Point on level s1, UP/DOWN depending on s2
            : eq2 ? (s1 > p ? -1 :  1)  // idem
            : s1 < p && s2 > p ?  2     // Point between s1 -> s2 --> UP
            : s2 < p && s1 > p ? -2     // Point between s2 -> s1 --> DOWN
            : 0;
    }

public :

    // Typedefs and static methods to fulfill the concept
    typedef Point point_type;
    typedef PointOfSegment segment_point_type;
    typedef counter state_type;

    static inline bool apply(Point const& point,
                PointOfSegment const& s1, PointOfSegment const& s2,
                counter& state)
    {
        int count = check_segment<1>(point, s1, s2, state);
        if (count != 0)
        {
            int side = strategy_side_type::apply(s1, s2, point);
            if (side == 0)
            {
                // Point is lying on segment
                state.m_touches = true;
                state.m_count = 0;
                return false;
            }

            // Side is NEG for right, POS for left.
            // The count is -2 for down, 2 for up (or -1/1)
            // Side positive thus means UP and LEFTSIDE or DOWN and RIGHTSIDE
            // See accompagnying figure (TODO)
            if (side * count > 0)
            {
                state.m_count += count;
            }
        }
        return ! state.m_touches;
    }

    static inline int result(counter const& state)
    {
        return state.code();
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

// Register using "areal_tag" for ring, polygon, multi-polygon
template <typename Point, typename PointOfSegment>
struct default_strategy<point_tag, areal_tag, cartesian_tag, cartesian_tag, Point, PointOfSegment>
{
    typedef winding<Point, PointOfSegment> type;
};

template <typename Point, typename PointOfSegment>
struct default_strategy<point_tag, areal_tag, spherical_polar_tag, spherical_polar_tag, Point, PointOfSegment>
{
    typedef winding<Point, PointOfSegment> type;
};

template <typename Point, typename PointOfSegment>
struct default_strategy<point_tag, areal_tag, spherical_equatorial_tag, spherical_equatorial_tag, Point, PointOfSegment>
{
    typedef winding<Point, PointOfSegment> type;
};

} // namespace services

#endif

}} // namespace strategy::within

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_AGNOSTIC_POINT_IN_POLY_WINDING_HPP


#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