/*
 * Software License Agreement (New BSD License)
 *
 * Copyright (c) 2013, Keith Leung, Felipe Inostroza
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Advanced Mining Technology Center (AMTC), the
 *       Universidad de Chile, nor the names of its contributors may be
 *       used to endorse or promote products derived from this software without
 *       specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AMTC, UNIVERSIDAD DE CHILE, OR THE COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef RBPHDFILTER_HPP
#define RBPHDFILTER_HPP

#include <boost/timer/timer.hpp>
#include <Eigen/Core>
#include "GaussianMixture.hpp"
#include "ParticleFilter.hpp"
#include "KalmanFilter.hpp"
#include <math.h>
#include <vector>

#include <stdio.h>

/**
 *  \class RBPHDFilter
 *  \brief Rao-Blackwellized Probability Hypothesis Density Filter class
 *
 *  This class implements the Rao-Bloackwellized Probability Hypothesis Density
 *  filter. The constructor of this class will internally instantiate the
 *  process model for both the robot and landmarks, the measurement model,
 *  and the Kalman filter. Users have access to these through pointers that
 *  can be obtained by calling the appropraite get function.
 *
 *  \tparam RobotProcessModel A robot process model derived from ProcessModel
 *  \tparam LmkProcessModel A landmark process model derived from ProcessModel
 *  \tparam MeasurementModel A sensor model derived from MeasurementModel
 *  \tparam KalmanFilter A Kalman filter that uses LmkProcessModel and MeasurementModel
 *  \author Keith Leung, Felipe Inostroza
 */

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter>
class RBPHDFilter : public ParticleFilter<RobotProcessModel, MeasurementModel>
{
public:

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef typename RobotProcessModel::TState TPose;
  typedef typename RobotProcessModel::TInput TInput;
  typedef typename MeasurementModel::TLandmark TLandmark;
  typedef typename MeasurementModel::TMeasurement TMeasurement;
  typedef typename GaussianMixture<TLandmark>::Gaussian TGaussian;

  /**
   * \brief Configurations for this RBPHDFilter
   */
  struct Config{

    /**  New birth Gaussians are set with this weight */
    double birthGaussianWeight_;

    /**  New Gaussians are only created during map update if the innovation mahalanobis distance
	 is less than this threshold */
    double newGaussianCreateInnovMDThreshold_;

    /**  number of map states to use for evaluating particle weight
	 0 => empty-set strategy,
	 1 => single-feature strategy,
	 >1 => multi-feature strategy
    */
    int importanceWeightingEvalPointCount_;

    /** The mahalanobis distance threshold used to determine if a possible meaurement-landmark
     *  pairing is significant to worth considering
     */
    double importanceWeightingMeasurementLikelihoodMDThreshold_;

    /** Gaussian merging Mahalanobis distance threshold */
    double gaussianMergingThreshold_;

    /** Gaussian merging covariance inflation factor */
    double gaussianMergingCovarianceInflationFactor_;

    /** Gaussian pruning weight threshold, below which Gaussians are eliminated from a Gaussian mixture */
    double gaussianPruningThreshold_;

    /** Minimum timeteps betwen resampling of particles*/
    double minInterSampleTimesteps_;

    /** If true, timing information is written to the console every update*/
    bool reportTimingInfo_;

    /** Use the particle weighting strategty from Single-cluster PHD Filtering by Lee, et. al. */
    bool useClusterProcess_;

  } config;

  /**
   * Constructor
   * \param n number of particles
   * \param initState initial state of particles
   */
  RBPHDFilter(int n);

  /** Destructor */
  ~RBPHDFilter();

  /**
   * Get the landmark process model
   * \return pointer to the landmark process model
   */
  LmkProcessModel* getLmkProcessModel();

  /**
   * Predict the robot trajectory using the lastest odometry data
   * \param[in] input
   * \param[in] currentTimestep current timestep;
   */
  void predict( TInput u, int currentTimestep );

  /**
   * Update the map, calculate importance weighting, sample if necessary, and
   * create new birth Gaussians.
   * \param[in] Z set of measurements to use for the update, placed in a std vector, which
   * gets cleared after the function call.
   * \param[in] currentTimestep current timestep;
   */
  void update( std::vector<TMeasurement> &Z, int currentTimestep );

  /**
   * Get the size of the Gaussian mixture for a particle
   * \param[in] i particle index
   * \return size if index is valid, else -1
   */
  int getGMSize(int i);

  /**
   * Get the position, covariance, and weight of a Gaussian in particle i's Gaussin mixture
   * \param[in] i particle index
   * \param[in] m Gaussian index
   * \param[out] u mean
   * \param[out] S covariance
   * \param[out] w weight
   * \return false if the indices specified are invalid
   */
  bool getLandmark(const int i, const int m,
		   typename TLandmark::Vec &u,
		   typename TLandmark::Mat &S,
		   double &w);

  /**
   * Get the pointer to the Kalman Filter used for updating the map
   * \return pointer to the Kalman Filter
   */
  KalmanFilter* getKalmanFilter();

  /** Function for testing purposes only */
  void setParticlePose(int i, TPose &p);

private:

  KalmanFilter *kfPtr_; /**< pointer to the Kalman filter */
  LmkProcessModel *lmkModelPtr_; /**< pointer to landmark process model */

  std::vector< GaussianMixture<TLandmark>* > maps_; /**< Particle dependent maps */

  /** indices of unused measurement for each particle for creating birth Gaussians */
  std::vector< std::vector<unsigned int> > unused_measurements_;

  int k_currentTimestep_; /**< current time */
  int k_lastResample_; /**< last resample time */

  /**
   * Add birth Gaussians for each particle's map using unused_measurements_
   */
  void addBirthGaussians();

  /**
   * Update the map with the measurements in measurements_
   * Existing landmarks with probability of detection > 0 will have their Gaussian
   * mixture weight reduced to account for missed detection.
   * For every landmark-measurement pair with probability of detection > 0,
   * a new landmark will be created.
   */
  void updateMap();

  /**
   * Importance weighting. Overrides the abstract function in ParticleFilter
   */
  void importanceWeighting();

  /**
   * Random Finite Set measurement likelihood evaluation
   * \brief The current measurements in measurements_ are used to determine the
   * RFS measurement likelihood given a set of landmarks
   * \param[in] particleIdx particle for which the likelihood is calcuated
   * \param[in] indices of evaluation points in maps_[particleIdx]
   * \param[in] probability of detection of evaluation point
   * \return measurement likelihood
   */
  double rfsMeasurementLikelihood( const int particleIdx,
				   std::vector<unsigned int> &evalPtIdx,
				   std::vector<double> &evalPtPd );

  /**
   * Calculate the sum of all permutations of measurement likelihood from a likelihood
   * table generated from within rfsMeasurementLikelihood
   * \param[in] likelihoodTab likelihood table generated within rfsMeasurementLikelihood
   * \para,[in] A vector of measurement indices (columns) to consider in the likelihoodTab
   * \return sum of all permutations from the given likelihood table
   */
  double rfsMeasurementLikelihoodPermutations( std::vector< double* > &likelihoodTab,
					       std::vector< int > &Z_NoClutter);

  /** Checks the Gaussian mixture maps for all particles for errors
   *  \return true if there are no errors
   */
  bool checkMapIntegrity();

};

////////// Implementation //////////

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter >
RBPHDFilter< RobotProcessModel, LmkProcessModel, MeasurementModel, KalmanFilter >::RBPHDFilter(int n)
  : ParticleFilter<RobotProcessModel, MeasurementModel>(n)
{

  lmkModelPtr_ = new LmkProcessModel;
  kfPtr_ = new KalmanFilter(lmkModelPtr_, this->getMeasurementModel());

  for(int i = 0; i < n; i++){
    printf("Creating map structure for particle %d\n", i);
    maps_.push_back( new GaussianMixture<TLandmark>() );
    unused_measurements_.push_back( std::vector<unsigned int>() );
  }

  config.birthGaussianWeight_ = 0.25;
  config.gaussianMergingThreshold_ = 0.5;
  config.gaussianMergingCovarianceInflationFactor_ = 1.5;
  config.gaussianPruningThreshold_ = 0.2;
  config.importanceWeightingEvalPointCount_ = 8;
  config.importanceWeightingMeasurementLikelihoodMDThreshold_ = 3.0;
  config.newGaussianCreateInnovMDThreshold_ = 0.2;
  config.minInterSampleTimesteps_ = 5;
  config.reportTimingInfo_ = false;

  k_lastResample_ = -10;
}

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter >
RBPHDFilter< RobotProcessModel, LmkProcessModel, MeasurementModel, KalmanFilter >::~RBPHDFilter(){

  for(int i = 0; i < maps_.size(); i++){
    delete maps_[i];
  }
  delete kfPtr_;
  delete lmkModelPtr_;
}

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter >
LmkProcessModel* RBPHDFilter< RobotProcessModel, LmkProcessModel, MeasurementModel, KalmanFilter >::getLmkProcessModel(){
  return lmkModelPtr_;
}

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter >
void RBPHDFilter< RobotProcessModel, LmkProcessModel, MeasurementModel, KalmanFilter >::predict( TInput u,
												 int currentTimestep){

  boost::timer::auto_cpu_timer *timer = NULL;
  if(config.reportTimingInfo_)
    timer = new boost::timer::auto_cpu_timer(6, "Predict time: %ws\n");

  // Add birth Gaussians using pose before prediction
  addBirthGaussians();

  // propagate particles
  k_currentTimestep_ = currentTimestep;
  this->propagate(u);

  // propagate landmarks
  for( int i = 0; i < this->nParticles_; i++ ){
    for( int m = 0; m < maps_[i]->getGaussianCount(); m++){
      TLandmark *plm;
      maps_[i]->getGaussian(m, plm);
      lmkModelPtr_->staticStep(*plm, *plm);
    }
  }

  if(timer != NULL)
    delete timer;
}

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter >
void RBPHDFilter< RobotProcessModel, LmkProcessModel, MeasurementModel, KalmanFilter >::update( std::vector<TMeasurement> &Z,
												int currentTimestep){

  boost::timer::auto_cpu_timer *timer_mapUpdate = NULL;
  boost::timer::auto_cpu_timer *timer_particleWeighting = NULL;
  boost::timer::auto_cpu_timer *timer_mapMerge = NULL;
  boost::timer::auto_cpu_timer *timer_mapPrune = NULL;
  boost::timer::auto_cpu_timer *timer_particleResample = NULL;

  k_currentTimestep_ = currentTimestep;

  this->setMeasurements( Z ); // Z gets cleared after this call, measurements now stored in this->measurements_

  ////////// Map Update //////////
  if(config.reportTimingInfo_){
    timer_mapUpdate = new boost::timer::auto_cpu_timer(6, "Map update time: %ws\n");
  }
  updateMap();
  if(timer_mapUpdate != NULL)
    delete timer_mapUpdate;

  ////////// Particle Weighintg //////////
  if(config.reportTimingInfo_){
    timer_particleWeighting = new boost::timer::auto_cpu_timer(6, "Particle weighting time: %ws\n");
  }
  if(!config.useClusterProcess_){
    importanceWeighting();
  }
  if(timer_particleWeighting != NULL)
    delete timer_particleWeighting;

  //////////// Merge and prune //////////
  int maxMapSize = -1;
  int i_maxMapSize = -1;
  if(config.reportTimingInfo_){
    timer_mapMerge = new boost::timer::auto_cpu_timer(6, "Map merge time: %ws\n");
  }
  for( int i = 0; i < this->nParticles_; i++){
    maps_[i]->merge( config.gaussianMergingThreshold_,
		     config.gaussianMergingCovarianceInflationFactor_);
  }
  if(timer_mapMerge != NULL)
    delete timer_mapMerge;

  if(config.reportTimingInfo_){
    timer_mapPrune = new boost::timer::auto_cpu_timer(6, "Map prune time: %ws\n");
  }
  for( int i = 0; i < maps_.size(); i++){ // maps_size is same as number of particles
    maps_[i]->prune( config.gaussianPruningThreshold_ );
  }
  if(timer_mapPrune != NULL)
    delete timer_mapPrune;

  //////////// Particle resampling //////////
  if(config.reportTimingInfo_){
    timer_particleResample = new boost::timer::auto_cpu_timer(6, "Particle resample time: %ws\n");
  }
  bool resampleOccured = false;
  if( k_currentTimestep_ - k_lastResample_ >= config.minInterSampleTimesteps_){
    resampleOccured = this->resample();
  }

  if( resampleOccured ){
    k_lastResample_ = k_currentTimestep_;
  }else{
    this->normalizeWeights();
  }

  if( resampleOccured){   // reassign maps as well according to resampling of particles

    std::vector< GaussianMixture<TLandmark>* > maps_temp( maps_.size(), NULL );
    std::vector< int > useCount ( maps_.size(), 0);

    // Note which GMs get used and how many times
    for(int i = 0; i < this->nParticles_; i++){
      int j = this->particleSet_[i]->getParentId();
      useCount[j]++;
    }

    // for maps that get used, make a (pointer copy) before doing any overwriting
    // Also rid of the maps that die along with particles
    for(int i = 0; i < this->nParticles_; i++){
      if( useCount[i] > 0){
	maps_temp[i] = maps_[i];
      }else{
	delete maps_[i];
      }
    }

    // Copy GMs
    for(int i = 0; i < this->nParticles_; i++){
      int j = this->particleSet_[i]->getParentId();

      if( useCount[j] == 1){ // map j is only copied over to map i and not to any other particle's map

	maps_[i] = maps_temp[j];
	maps_temp[j] = NULL;

      }else{ // GM_j is used by more than 1 particle, need to allocate memory for copying map

	maps_[i] = new GaussianMixture<TLandmark>;
	maps_temp[j]->copyTo( maps_[i] );
      }
      useCount[j]--;
    }

  }
  if(timer_particleResample != NULL)
    delete timer_particleResample;

}

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter >
void RBPHDFilter< RobotProcessModel, LmkProcessModel, MeasurementModel, KalmanFilter >::updateMap(){

  const unsigned int startIdx = 0;
  const unsigned int stopIdx = this->nParticles_;

  const unsigned int nZ = this->measurements_.size();
  typename ParticleFilter<RobotProcessModel, MeasurementModel>::TParticleSet::iterator particles_it = this->particleSet_.begin();
  typename std::vector< GaussianMixture<TLandmark>* >::iterator maps_it = maps_.begin();
  std::vector< std::vector<unsigned int> >::iterator unused_measurements_it = unused_measurements_.begin();
  const typename std::vector< TMeasurement >::iterator measurements_it_start = this->measurements_.begin();
  typename std::vector< TMeasurement >::iterator measurements_it = measurements_it_start;
  particles_it += startIdx;
  maps_it += startIdx;
  unused_measurements_it += startIdx;

  for(unsigned int i = startIdx; i < stopIdx; i++, particles_it++, maps_it++, unused_measurements_it++){

    //---------- 1. setup / book-keeping ----------

    const unsigned int nM = (*maps_it)->getGaussianCount();
    unused_measurements_it->clear();
    if(nM == 0){ // No existing landmark case -> flag all measurements as unused and go to next particles
      for(int z = 0; z < nZ; z++){
	unused_measurements_it->push_back( z );
      }
      continue;
    }
    double Pd[nM];
    int landmarkCloseToSensingLimit[nM];

    // For cluster process particle weighting
    double w_km_sum = std::numeric_limits<double>::denorm_min();
    double likelihoodProd = 1;
    if(config.useClusterProcess_){
      for(int m = 0; m < nM; m++){
	w_km_sum += (*maps_it)->getWeight(m);
      }
    }

    // Mn x nZ table for Gaussian weighting
    double** weightingTable = new double* [ nM ];
    TLandmark*** newLandmarkPointer = new TLandmark** [ nM ];
    for( int n = 0; n < nM; n++ ){
      weightingTable[n] = new double [ nZ ];
      newLandmarkPointer[n] = new TLandmark* [ nZ ];
    }
    for(int m = 0; m < nM; m++){
      for(int z = 0; z < nZ; z++){
	newLandmarkPointer[m][z] = NULL;
	weightingTable[m][z] = 0;
      }
    }

    //----------  2. Kalman Filter map update ----------

    TPose *pose = new TPose;
    (*particles_it)->getPose(*pose);

    TLandmark* lmNew = NULL;

    for(unsigned int m = 0; m < nM; m++){

      TLandmark* lm = (*maps_it)->getGaussian(m);
      bool isCloseToSensingLimit;
      Pd[m] = this->pMeasurementModel_->probabilityOfDetection( *pose, *lm,
								isCloseToSensingLimit);
      landmarkCloseToSensingLimit[m] = ( isCloseToSensingLimit ) ? 1 : 0;
      double w_km = (*maps_it)->getWeight(m);
      double Pd_times_w_km = Pd[m] * w_km;

      if(Pd[m] != 0){
	int z = 0;
	for(z = 0, measurements_it = measurements_it_start; z < nZ; z++, measurements_it++){

	  if(lmNew == NULL)
	    lmNew = new TLandmark;

	  newLandmarkPointer[m][z] = NULL;
	  weightingTable[m][z] = 0;
	  double innovationLikelihood = 0;
	  double innovationMahalanobisDist2 = 0;
	  double threshold = config.newGaussianCreateInnovMDThreshold_ * config.newGaussianCreateInnovMDThreshold_;

	  // RUN KF, create new landmark for likely updates but do not add to map_[i] yet
	  // because we cannot determine actual weight until the entire weighting table is
	  // filled in
	  bool updateMade = kfPtr_->correct(*pose, *measurements_it, *lm, *lmNew,
					    &innovationLikelihood, &innovationMahalanobisDist2);

	  if ( !updateMade || innovationMahalanobisDist2 > threshold ){
	    newLandmarkPointer[m][z] = NULL;
	    weightingTable[m][z] = 0;
	  }else{
	    newLandmarkPointer[m][z] = lmNew;
	    lmNew = NULL;
	    weightingTable[m][z] = Pd_times_w_km * innovationLikelihood;
	  }

	} // z forloop end

      }else{ // Pd = 0

	for(int z = 0; z < nZ; z++){

	  newLandmarkPointer[m][z] = NULL;
	  weightingTable[m][z] = 0;

	}
      }
    }
    if(lmNew != NULL)
      delete lmNew;

    // Now calculate the weight of each new Gaussian
    int z = 0;
    for(z = 0, measurements_it = measurements_it_start; z < nZ; z++, measurements_it++){

      double clutter = this->pMeasurementModel_->clutterIntensity( *measurements_it, nZ );
      double sum = clutter;
      for(unsigned int m = 0; m < nM; m++){
	sum += weightingTable[m][z];
      }

      if(config.useClusterProcess_){
	likelihoodProd *= sum;
      }

      for(unsigned int m = 0; m < nM; m++){
	weightingTable[m][z] /= sum;
      }
    }
    if(config.useClusterProcess_){
      double prev_particle_i_weight = this->particleSet_[i]->getWeight();
      this->particleSet_[i]->setWeight( exp(w_km_sum) * likelihoodProd);
    }

    // ---------- 3. Add new Gaussians to map  ----------
    // New Gaussians will have indices >= nM
    for(int m = 0; m < nM; m++){
      for(int z = 0; z < nZ; z++){
	if(newLandmarkPointer[m][z] != NULL && weightingTable[m][z] > 0){
	  (*maps_it)->addGaussian( newLandmarkPointer[m][z], weightingTable[m][z]);
	}
      }
    }

    //----------  4. Determine weights for existing Gaussians (missed detection) ----------
    for(int m = 0; m < nM; m++){

      double w_km = (*maps_it)->getWeight(m);
      double w_k = (1 - Pd[m]) * w_km;

      // For landmarks close to sensing limit
      if (landmarkCloseToSensingLimit[m] == 1){
	double weight_sum_m = 0;
	for(int z = 0; z < nZ; z++){
	  weight_sum_m += weightingTable[m][z];
	}
	double delta_w = Pd[m] * w_km - weight_sum_m;
	if( delta_w > 0 ){
	  w_k += delta_w;
	}
      }

      (*maps_it)->setWeight(m, w_k);
    }

    //----------  5. Identify unused measurements for adding birth Gaussians later ----------
    unused_measurements_it->clear();
    for(int z = 0; z < nZ; z++){
      int useCount = 0;
      for(int m = 0; m < nM; m++){
	if (weightingTable[m][z] != 0){
	  useCount++;
	}
      }
      if (useCount == 0)
	unused_measurements_it->push_back( z );
    }

    //----------  6. Cleanup - Free memory ----------
    delete pose;

    for( int n = 0; n < nM; n++ ){
      delete[] weightingTable[n];
      delete[] newLandmarkPointer[n];
    }
    delete[] weightingTable;
    delete[] newLandmarkPointer;

  }

}

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter >
void RBPHDFilter< RobotProcessModel, LmkProcessModel, MeasurementModel, KalmanFilter >::importanceWeighting(){

  for(int i = 0; i < this->nParticles_; i++){

    //printf("Importance weighting for particle %d\n", i);
    TPose x;
    this->particleSet_[i]->getPose( x );

    // 1. select evaluation points from highest-weighted Gaussians after update, that are within sensor FOV
    const unsigned int nM = maps_[i]->getGaussianCount();
    int nEvalPoints = config.importanceWeightingEvalPointCount_ > nM ? nM : config.importanceWeightingEvalPointCount_ ;
    std::vector<unsigned int> evalPointIdx;
    std::vector<double> evalPointPd;
    evalPointIdx.reserve(nEvalPoints);
    evalPointPd.reserve(nEvalPoints);
    if( nEvalPoints == 0 ){
      this->particleSet_[i]->setWeight( std::numeric_limits<double>::denorm_min() );
      continue;
    }
    maps_[i]->sortByWeight(); // sort by weight so that we can pick off the top nEvalPoints Gaussians
    for(int m = 0; m < nM; m++){
      TLandmark* plm_temp;
      double w, w_prev;
      bool closeToSensingLim;
      maps_[i]->getGaussian(m, plm_temp, w, w_prev);
      double Pd = this->pMeasurementModel_->probabilityOfDetection(x,*plm_temp, closeToSensingLim);
      if( Pd > 0 ){
	evalPointIdx.push_back(m);
	evalPointPd.push_back(Pd);
      }
      if(evalPointIdx.size() >= nEvalPoints)
	break;
    }
    nEvalPoints = evalPointIdx.size();

    // 2. evaluate sum of Gaussian weights
    double gaussianWeightSumBeforeUpdate = 0;
    double gaussianWeightSumAfterUpdate = 0;
    for(int m = 0; m < nM; m++){
      TLandmark* plm_temp;
      double w, w_prev;
      maps_[i]->getGaussian(m, plm_temp, w, w_prev); // for newly created Gaussians, w_prev = 0
      gaussianWeightSumBeforeUpdate += w_prev;
      gaussianWeightSumAfterUpdate += w;
      //printf("w_[%d] = %f\n", i, w);
    }
    // Check for NaN
    if( gaussianWeightSumBeforeUpdate != gaussianWeightSumBeforeUpdate ||
	gaussianWeightSumAfterUpdate != gaussianWeightSumAfterUpdate ){
      //printf("Particle %d map size before update = %f\n", i, gaussianWeightSumBeforeUpdate);
      //printf("Particle %d map size after update = %f\n", i, gaussianWeightSumAfterUpdate);
    }
    //printf("Particle %d map size after - before update = %f\n", i, gaussianWeightSumAfterUpdate - gaussianWeightSumBeforeUpdate);

    // 3. evaluate intensity function at eval points and take their product
    double intensityProd_beforeUpdate = 1;
    double intensityProd_afterUpdate = 1;
    for(int e = 0; e < nEvalPoints; e++){

      int p = evalPointIdx[e];
      TLandmark* lm_evalPt;
      double w_temp;
      maps_[i]->getGaussian(p, lm_evalPt, w_temp);

      double intensity_at_evalPt_beforeUpdate = std::numeric_limits<double>::denorm_min();
      double intensity_at_evalPt_afterUpdate = std::numeric_limits<double>::denorm_min();

      for(int m = 0; m < nM; m++){
	TLandmark* plm;
	double w, w_prev;
	maps_[i]->getGaussian(m, plm, w, w_prev);
	// New Gaussians from update will have w_prev = 0
	// Out Gaussians (missed-detection) will not have been updated, but weights will have changed
	double likelihood = plm->evalGaussianLikelihood( *lm_evalPt );
	intensity_at_evalPt_beforeUpdate += w_prev * likelihood; // w_prev for newly created Gaussians are 0
	intensity_at_evalPt_afterUpdate += w * likelihood;
	// NaN check
	if( likelihood != likelihood ||
	    intensity_at_evalPt_beforeUpdate != intensity_at_evalPt_beforeUpdate ||
	    intensity_at_evalPt_afterUpdate != intensity_at_evalPt_afterUpdate){
	  printf("Particle %d map intensity error for eval point %d\n", i, m);
	  printf("intensity before update = %f\n", intensity_at_evalPt_beforeUpdate);
	  printf("intensity after update = %f\n", intensity_at_evalPt_afterUpdate);
	}
      }
      intensityProd_beforeUpdate *= intensity_at_evalPt_beforeUpdate;
      intensityProd_afterUpdate *= intensity_at_evalPt_afterUpdate;
      // NaN Check
      if( intensityProd_beforeUpdate != intensityProd_beforeUpdate ||
	  intensityProd_afterUpdate != intensityProd_afterUpdate ){
	printf("Particle %d map intensity product error\n", i);
	printf("intensity product before update = %f\n", intensityProd_beforeUpdate);
	printf("intensity product after update = %f\n", intensityProd_afterUpdate);
      }
    }
    //printf("Particle %d intensity product before / after update = %f\n", i, intensityProd_beforeUpdate / intensityProd_afterUpdate);

    // 4. calculate measurement likelihood at eval points
    // note that rfsMeasurementLikelihood uses maps_[i] which is already sorted by weight
    double measurementLikelihood = rfsMeasurementLikelihood( i, evalPointIdx, evalPointPd );
    //printf("Particle %d measurement likelihood = %f\n", i, measurementLikelihood);

    // 5. calculate overall weight
    double overall_weight = measurementLikelihood * intensityProd_beforeUpdate / intensityProd_afterUpdate *
      exp( gaussianWeightSumAfterUpdate - gaussianWeightSumBeforeUpdate);

    double prev_weight = this->particleSet_[i]->getWeight();
    this->particleSet_[i]->setWeight( overall_weight * prev_weight );
    //printf("Particle %d overall weight = %f\n\n", i, overall_weight);

  }

}

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter >
double RBPHDFilter< RobotProcessModel, LmkProcessModel, MeasurementModel, KalmanFilter >::
rfsMeasurementLikelihood( const int particleIdx,
			  std::vector<unsigned int> &evalPtIdx,
			  std::vector<double> &evalPtPd ){
  // eval points are first nEvalPoints elements of maps_[i], which are already ordered by weight;

  const int i = particleIdx;
  const int nM = evalPtIdx.size();
  const int nZ = this->measurements_.size();

  // Fill in likelihood table
  TPose x;
  this->particleSet_[i]->getPose( x );
  std::vector< double* > likelihoodTab;
  likelihoodTab.reserve(nM);
  for( int m = 0; m < nM; m++ ){

    double* row = new double[nZ];

    TLandmark* evalPt;
    maps_[i]->getGaussian( evalPtIdx[m], evalPt );
    double Pd = evalPtPd[m];

    double row_sum = 0;
    for( int z = 0; z < nZ; z++ ){

      TMeasurement expected_z;
      this->pMeasurementModel_->measure( x, *evalPt, expected_z);
      //TMeasurement actual_z = this->measurements_[z];

      double md2;
      double threshold = config.importanceWeightingMeasurementLikelihoodMDThreshold_;
      threshold *= threshold;
      double likelihood = this->measurements_[z].evalGaussianLikelihood( expected_z, &md2);
      if( md2 <= threshold ){
	row[z] = likelihood * Pd;
	row_sum += row[z];
      }else{
	row[z] = 0;
      }

    }

    // Check to see if likelihood to all measurements is 0, if so remove this eval point
    // as it will not contribute anything to the output likelihood
    if( row_sum == 0 ){
      delete[] row;
    }else{
      likelihoodTab.push_back(row);
    }

  }
  const int likelihoodTabSizeWithoutClutter = likelihoodTab.size();

  // Check measurements (columns) with 0 likelihood to all eval points
  // if so, that measurement is considered clutter
  int nClutter = 0;
  double clutterLikelihood = 1; // we will multiply the likelihood sum of all d.a. permuations with this at the end
  std::vector<int> z_noClutter; // we only want the non-clutter measurements when we permutate over all data assocation pairs later
  z_noClutter.reserve(nZ);

  for( int z = 0; z < nZ; z++ ){
    double isClutter = true;
    for( int m = 0; m < likelihoodTab.size(); m++ ){
      if( likelihoodTab[m][z] > 0 ){
	isClutter = false;
	break;
      }
    }
    if( isClutter ){
      nClutter++;
      // TMeasurement actual_z = this->measurements_[z];
      clutterLikelihood *= this->pMeasurementModel_->clutterIntensity(this->measurements_[z], nZ);;
    }else{
      z_noClutter.push_back(z);
    }
  }

  // If the number of measurements is greater than the number of
  // eval points, then some measurements have to be assigned as clutter.
  // We will add extra rows for these assignments in likelihoodTab
  double *clutterRow = NULL;
  int nR = z_noClutter.size() - likelihoodTab.size();
  if (nR > 0){
    clutterRow = new double[nZ];
    for( int z = 0; z < nZ; z++ ){
      //TMeasurement actual_z = this->measurements_[z];
      clutterRow[z] = this->pMeasurementModel_->clutterIntensity(this->measurements_[z], nZ);
    }
    for( int r = 0; r < nR; r++ ){
      likelihoodTab.push_back(clutterRow);
    }
  }

  // Go through all permutations of eval point - measurement pairs
  // to calculate the likelihood
  double likelihood = 0;
  while (likelihood == 0){

    if( likelihoodTab.size() == 0 ){
      likelihood = 1;
      break;
    }

    likelihood = rfsMeasurementLikelihoodPermutations( likelihoodTab, z_noClutter);
    if( likelihood != likelihood ){
      printf("RFS Measurement likelihood = %f", likelihood);
    }

    if( likelihood == 0 ){

      // Add another row to of clutter to likelihoodTab
      // printf("Adding clutter row for RFS measurement likelihood calculation\n");
      if( clutterRow == NULL ){
	clutterRow = new double[nZ];
	for( int z = 0; z < nZ; z++ ){
	  //TMeasurement actual_z = this->measurements_[z];
	  clutterRow[z] = this->pMeasurementModel_->clutterIntensity(this->measurements_[z], nZ);
	}
      }
      likelihoodTab.push_back( clutterRow );

    }

  }

  // Deallocate likelihood table
  for( int m = 0; m < likelihoodTabSizeWithoutClutter; m++ ){
    delete[] likelihoodTab[m];
  }
  if( clutterRow != NULL )
    delete[] clutterRow;

  if (nClutter > 0){
    likelihood /= this->pMeasurementModel_->clutterIntensityIntegral( nZ );
    likelihood *= clutterLikelihood;
  }

  return likelihood;
}

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter >
double RBPHDFilter< RobotProcessModel, LmkProcessModel, MeasurementModel, KalmanFilter >::
rfsMeasurementLikelihoodPermutations( std::vector< double* > &likelihoodTab,
				      std::vector< int > &Z_NoClutter){
  // Note that nM is always >= nZ
  // We will find all eval point permutations of (0, 1, 2, ... , nM - 1)
  // and use the first nZ of each permutation to calculate the likelihood

  // A function required for sorting
  struct sort{
    static bool descend(int i, int j){ return (i > j); }
  };

  const int nM = likelihoodTab.size();
  const int nZ = Z_NoClutter.size();
  double allPermutationLikelihood = 0;
  bool lastPermutationSequence = false;
  std::vector<int> currentPermutation(nM);
  for(int m = 0; m < nM; m++){
    currentPermutation[m] = m;
  }

  while( !lastPermutationSequence ){

    // find the likelihood of the current permutation

    double currentPermutationLikelihood  = 1;
    for(int z = 0; z < nZ; z++){
      int m = currentPermutation[ z ];
      currentPermutationLikelihood *= likelihoodTab[m][ Z_NoClutter[z] ];

      // Fast-forward permutation if we know that following sequences will also
      // have 0 likelihood
      if( currentPermutationLikelihood == 0 && z < nZ - 1){
	std::sort(currentPermutation.begin() + z + 1, currentPermutation.end(), sort::descend);
	break;
      }
    }

    allPermutationLikelihood += currentPermutationLikelihood;

    // Fast-forward if nM > nZ (i.e., the last nM - nZ elements in the permutation sequence does not matter)
    if( nM > nZ ){
      std::sort(currentPermutation.begin() + nZ, currentPermutation.end(), sort::descend);
    }

    // Generate the next permutation sequence
    for(int m = nM - 2; m >= -1; m--){

      if( m == -1){
	lastPermutationSequence = true;
	break;
      }

      // Find the highest index m such that currentPermutation[m] < currentPermutation[m+1]
      if(currentPermutation[m] < currentPermutation[ m + 1 ]){

	// Find highest index i such that currentPermutation[i] > currentPermutation[m]
	// then swap the elements
	for(int i = nM - 1; i >= 0; i--){
	  if( currentPermutation[i] > currentPermutation[m] ){
	    int temp = currentPermutation[i];
	    currentPermutation[i] = currentPermutation[m];
	    currentPermutation[m] = temp;
	    break;
	  }
	}

	// reverse order of elements after currentPermutation[m]
	int nElementsToSwap = nM - (m + 1);
	int elementsToSwapMidPt = nElementsToSwap / 2;
	int idx1 = m + 1;
	int idx2 = nM - 1;
	for(int i = 1; i <= elementsToSwapMidPt; i++){
	  int temp = currentPermutation[idx1];
	  currentPermutation[idx1] = currentPermutation[idx2];
	  currentPermutation[idx2] = temp;
	  idx1++;
	  idx2--;
	}

	break;
      }

    }

    // now we should have the next permutation sequence
  }

  return allPermutationLikelihood;

}

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter >
void RBPHDFilter< RobotProcessModel, LmkProcessModel, MeasurementModel, KalmanFilter >::addBirthGaussians(){

  for(int i = 0; i < this->nParticles_; i++){

    while( unused_measurements_[i].size() > 0){

      // get measurement
      int unused_idx = unused_measurements_[i].back();
      TMeasurement unused_z = this->measurements_[unused_idx];
      unused_measurements_[i].pop_back();

      // use inverse measurement model to get landmark
      TPose robot_pose;
      TLandmark landmark_pos;
      this->particleSet_[i]->getPose(robot_pose);
      this->pMeasurementModel_->inverseMeasure( robot_pose,  unused_z, landmark_pos );

      // add birth landmark to Gaussian mixture (last param = true to allocate mem)
      maps_[i]->addGaussian( &landmark_pos, config.birthGaussianWeight_, true);

    }

  }

}

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter >
bool RBPHDFilter< RobotProcessModel, LmkProcessModel, MeasurementModel, KalmanFilter >::checkMapIntegrity(){

  for( int i = 0; i < maps_.size(); i++ ){

    unsigned int nM = maps_[i]->getGaussianCount();
    for(int m = 0; m < nM; m++){

      TLandmark *lm;
      typename TLandmark::Vec lm_x;
      typename TLandmark::Mat lm_S;
      double w;

      maps_[i]->getGaussian(m, lm, w);
      if( lm != NULL ){
	lm->get(lm_x, lm_S);

	bool vecError = false;
	for( int c = 0; c < lm_x.rows(); c++){
	  if(lm_x(c) != lm_x(c)){
	    vecError = true;
	    break;
	  }
	}
	if(vecError){
	  printf("particle %d, landmark index %d, vector error\n", i, m);
	  std::cout << lm_x << std::endl;
	  return false;
	}

	bool matError = false;
	for( int r = 0; r < lm_S.rows(); r++){
	  for( int c = 0; c < lm_S.cols(); c++){
	    if(lm_S(r,c) != lm_S(r,c)){
	      matError = true;
	      break;
	    }
	  }
	}
	lm_x.setOnes();
	double posDefCheck = lm_x.transpose() * lm_S * lm_x;
	if( posDefCheck != posDefCheck || posDefCheck <= 0){
	  matError = true;
	}
	if(matError){
	  printf("particle %d, landmark index %d, covariance error\n", i, m);
	  std::cout << lm_S << std::endl;
	  return false;
	}

	if( w != w ){
	  printf("particle %d, landmark index %d, w = %f\n", i, m, w);
	  return false;
	}

      }

    }

  }

  return true;

}

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter >
int RBPHDFilter< RobotProcessModel, LmkProcessModel, MeasurementModel, KalmanFilter >::getGMSize(int i){

  if( i >= 0 && i < maps_.size() )
    return ( this->maps_[i]->getGaussianCount() );
  else
    return -1;
}

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter >
bool RBPHDFilter< RobotProcessModel, LmkProcessModel, MeasurementModel, KalmanFilter >::
getLandmark(const int i, const int m,
	    typename TLandmark::Vec &u,
	    typename TLandmark::Mat &S,
	    double &w)
{

  int sz = getGMSize(i);
  if( sz == -1 || (m < 0) || (m >= sz) )
    {
      return false;
    }
    TLandmark *plm;
    maps_[i]->getGaussian(m, plm, w);
    plm->get(u, S);
    return true;
}

#endif

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter >
void RBPHDFilter< RobotProcessModel, LmkProcessModel, MeasurementModel, KalmanFilter >::
setParticlePose(int i, TPose &p){

  this->particleSet_[i]->setPose(p);

}

template< class RobotProcessModel, class LmkProcessModel, class MeasurementModel, class KalmanFilter >
KalmanFilter* RBPHDFilter< RobotProcessModel, LmkProcessModel, MeasurementModel, KalmanFilter >::getKalmanFilter(){
  return kfPtr_;
}


#include <memory>
#include <iostream>
#include <fstream>

#include <deal.II/base/conditional_ostream.h>
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/mpi.h>

#include "framework/builder/framework_builder.hpp"
#include "problem/parameters_dealii_handler.h"
#include "utility/runtime/runtime_helper.h"

int main(int argc, char* argv[]) {
  try {
    bart::utility::runtime::RuntimeHelper runtime_helper("0.2.0");
    runtime_helper.ParseArguments(argc, argv);

    if (runtime_helper.show_help()) {
      std::cout << runtime_helper.HelpMessage() << std::endl;
      return EXIT_FAILURE;
    }

    std::cout << runtime_helper.ProgramHeader() << std::endl;
    std::cout << "\nInitializing MPI\n";
    dealii::Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv, 1);
    std::cout << "\nMPI Initialized\n";

    namespace MPI = dealii::Utilities::MPI;
    const std::string filename{runtime_helper.filename()};
    const int n_processes = MPI::n_mpi_processes(MPI_COMM_WORLD);
    const int process_id =  MPI::this_mpi_process(MPI_COMM_WORLD);

    bart::problem::ParametersDealiiHandler prm;
    dealii::ParameterHandler d2_prm;

    prm.SetUp(d2_prm);
    d2_prm.parse_input(filename, "");
    prm.Parse(d2_prm);

    double k_eff_final;

    // Open file for output, if there are multiple processes they will end with
    // a number indicating the process number.
    std::ofstream output_stream, master_output_stream;
    const std::string output_filename_base{prm.OutputFilenameBase()};
    if (n_processes > 1) {
      const std::string full_filename = output_filename_base + dealii::Utilities::int_to_string(process_id, 4);
      output_stream.open((full_filename + ".vtu").c_str());
      master_output_stream.open(output_filename_base + ".pvtu");
    } else {
      output_stream.open((output_filename_base + ".vtu").c_str());
    }
    std::vector<std::string> filenames;
    // Write master pvtu record if multiple files are required
    if ((n_processes > 1) && (process_id == 0)) {
      for (int process = 0; process < n_processes; ++process) {
        const std::string full_filename =
            output_filename_base + dealii::Utilities::int_to_string(process, 4);
        filenames.push_back(full_filename + ".vtu");
      }
    }

    // Framework pointer
    std::unique_ptr<bart::framework::FrameworkI> framework_ptr;

    switch(prm.SpatialDimension()) {
      case 1: {
        bart::framework::builder::FrameworkBuilder<1> builder;
        framework_ptr = builder.BuildFramework("main", prm);
        break;
      }
      case 2: {
        bart::framework::builder::FrameworkBuilder<2> builder;
        framework_ptr = builder.BuildFramework("main", prm);
        break;
      }
      case 3: {
        bart::framework::builder::FrameworkBuilder<3> builder;
        framework_ptr = builder.BuildFramework("main", prm);
        break;
      }
    }

    // Pause if needed before solve
    if (runtime_helper.do_pause()) {
      std::cout << "Press <Enter> to begin solve...";
      std::cin.ignore();
    }

    framework_ptr->SolveSystem();
    if (prm.DoDiscreteFourierTransformOfError()) {

      std::unique_ptr<bart::framework::FrameworkI> fourier_framework_ptr;
      switch (prm.SpatialDimension()) {
        case 1: {
          bart::framework::builder::FrameworkBuilder<1> builder;
          fourier_framework_ptr = builder.BuildFramework("fourier", prm,
                                                         framework_ptr->system()->current_moments.get());
          break;
        }
        case 2: {
          bart::framework::builder::FrameworkBuilder<2> builder;
          fourier_framework_ptr = builder.BuildFramework("fourier", prm,
                                                         framework_ptr->system()->current_moments.get());
          break;
        }
        case 3: {
          bart::framework::builder::FrameworkBuilder<3> builder;
          fourier_framework_ptr = builder.BuildFramework("fourier", prm,
                                                         framework_ptr->system()->current_moments.get());
          break;
        }
      }
      framework_ptr.release();

      fourier_framework_ptr->SolveSystem();
      fourier_framework_ptr->OutputResults(output_stream);
      if (n_processes > 1)
        fourier_framework_ptr->OutputMasterFile(master_output_stream, filenames, process_id);
      k_eff_final = fourier_framework_ptr->system()->k_effective.value_or(0);
    } else {
      framework_ptr->OutputResults(output_stream);
      if (n_processes > 1)
        framework_ptr->OutputMasterFile(master_output_stream, filenames, process_id);
      k_eff_final = framework_ptr->system()->k_effective.value_or(0);
    }

    std::cout << "Final k_effective: " << k_eff_final << std::endl;

  } catch (std::exception &exc) {
    std::cerr << std::endl << std::endl
              << "----------------------------------------------------"
              << std::endl;
    std::cerr << "Exception on processing: " << std::endl
              << exc.what() << std::endl
              << "Aborting!" << std::endl
              << "----------------------------------------------------"
              << std::endl;
    return 1;
  } catch (...) {
    std::cerr << std::endl << std::endl
              << "----------------------------------------------------"
              << std::endl;
    std::cerr << "Unknown exception!" << std::endl
              << "Aborting!" << std::endl
              << "----------------------------------------------------"
              << std::endl;
    return 1;
  }
  return 0;
}


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

#include "local_parameterization_se3.hpp"

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

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

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

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

#define PARETO_ALPHA 1

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

};

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

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

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

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

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

}  // namespace internal
}  // namespace Eigen

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

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

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

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

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

  Sophus::SE3d T_aw;
};

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

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

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

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

    residuals = point_b_prime - point_a_prime;
    return true;
  }

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

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

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

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

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

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

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

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

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

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

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

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

    return true;
  }

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

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

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

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

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

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

  ~R2EM_CauchyUniform() {}

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

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

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

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

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

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

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

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

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

  };

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

  void EstimateLAMBDA() {

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

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

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

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

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

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

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

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

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

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

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

  }

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

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

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

// Expectation steps
  void Expectation(int iteration_num) {

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

      FramedMatches match = odometry_txt_.data_[i];

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

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

    }

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

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

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

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

        }
    }

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

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

  PCLTrajectory camera_poses_for_one_loop;
  void yujie(int index_loop) {

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

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

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

      FramedMatches match = odometry_txt_.data_[i];

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

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

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

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

        }

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

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

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

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

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

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

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

    // Solve

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

  }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    // if (argc > 3) {

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  return 0;
}