/**
 * @author Tiago de Freitas Pereira <tiago.pereira@idiap.ch>
 * @date Tue 13 Jan 16:50:00 2015
 *
 * @brief Python API for bob::learn::em
 *
 * Copyright (C) 2011-2014 Idiap Research Institute, Martigny, Switzerland
 */

#include "main.h"
#include <boost/assign.hpp>

/******************************************************************/
/************ Constructor Section *********************************/
/******************************************************************/

// InitializationMethod type conversion

#if BOOST_VERSION >= 104700
  static const std::map<std::string, bob::learn::em::KMeansTrainer::InitializationMethod> IM = boost::assign::map_list_of
  ("RANDOM",  bob::learn::em::KMeansTrainer::InitializationMethod::RANDOM)
  ("RANDOM_NO_DUPLICATE", bob::learn::em::KMeansTrainer::InitializationMethod::RANDOM_NO_DUPLICATE)
  ("KMEANS_PLUS_PLUS", bob::learn::em::KMeansTrainer::InitializationMethod::KMEANS_PLUS_PLUS)
  ;
#else
  static const std::map<std::string, bob::learn::em::KMeansTrainer::InitializationMethod> IM = boost::assign::map_list_of
  ("RANDOM",  bob::learn::em::KMeansTrainer::InitializationMethod::RANDOM),
  ("RANDOM_NO_DUPLICATE", bob::learn::em::KMeansTrainer::InitializationMethod::RANDOM_NO_DUPLICATE)
  ;
#endif

static inline bob::learn::em::KMeansTrainer::InitializationMethod string2IM(const std::string& o){            /* converts string to InitializationMethod type */
  auto it = IM.find(o);
  if (it == IM.end()) throw std::runtime_error("The given InitializationMethod '" + o + "' is not known; choose one of ('RANDOM', 'RANDOM_NO_DUPLICATE', 'KMEANS_PLUS_PLUS')");
  else return it->second;
}
static inline const std::string& IM2string(bob::learn::em::KMeansTrainer::InitializationMethod o){            /* converts InitializationMethod type to string */
  for (auto it = IM.begin(); it != IM.end(); ++it) if (it->second == o) return it->first;
  throw std::runtime_error("The given InitializationMethod type is not known");
}

static auto KMeansTrainer_doc = bob::extension::ClassDoc(
  BOB_EXT_MODULE_PREFIX ".KMeansTrainer",
  "Trains a KMeans clustering :ref:`k-means <kmeans>`."
  "This class implements the expectation-maximization algorithm for a k-means."
  "See Section 9.1 of Bishop, \"Pattern recognition and machine learning\", 2006"
  "It uses a random initialization of the means followed by the expectation-maximization algorithm"
).add_constructor(
  bob::extension::FunctionDoc(
    "__init__",
    "Creates a KMeansTrainer",
    "",
    true
  )
  .add_prototype("initialization_method","")
  .add_prototype("other","")
  .add_prototype("","")

  .add_parameter("initialization_method", "str", "The initialization method of the means.\nPossible values are: 'RANDOM', 'RANDOM_NO_DUPLICATE', 'KMEANS_PLUS_PLUS' ")
  .add_parameter("other", ":py:class:`bob.learn.em.KMeansTrainer`", "A KMeansTrainer object to be copied.")

);

static int PyBobLearnEMKMeansTrainer_init_copy(PyBobLearnEMKMeansTrainerObject* self, PyObject* args, PyObject* kwargs) {

  char** kwlist = KMeansTrainer_doc.kwlist(1);
  PyBobLearnEMKMeansTrainerObject* tt;
  if (!PyArg_ParseTupleAndKeywords(args, kwargs, "O!", kwlist, &PyBobLearnEMKMeansTrainer_Type, &tt)){
    KMeansTrainer_doc.print_usage();
    return -1;
  }

  self->cxx.reset(new bob::learn::em::KMeansTrainer(*tt->cxx));
  return 0;
}

static int PyBobLearnEMKMeansTrainer_init_str(PyBobLearnEMKMeansTrainerObject* self, PyObject* args, PyObject* kwargs) {

  char** kwlist = KMeansTrainer_doc.kwlist(0);
  char* value;
  if (!PyArg_ParseTupleAndKeywords(args, kwargs, "s", kwlist, &value)){
    KMeansTrainer_doc.print_usage();
    return -1;
  }
  self->cxx.reset(new bob::learn::em::KMeansTrainer(string2IM(std::string(value))));
  return 0;
}

static int PyBobLearnEMKMeansTrainer_init(PyBobLearnEMKMeansTrainerObject* self, PyObject* args, PyObject* kwargs) {
  BOB_TRY

  int nargs = (args?PyTuple_Size(args):0) + (kwargs?PyDict_Size(kwargs):0);

  switch (nargs) {

    case 0:{ //default initializer ()
      self->cxx.reset(new bob::learn::em::KMeansTrainer());
      return 0;
    }
    case 1:{
      //Reading the input argument
      PyObject* arg = 0;
      if (PyTuple_Size(args))
        arg = PyTuple_GET_ITEM(args, 0);
      else {
        PyObject* tmp = PyDict_Values(kwargs);
        auto tmp_ = make_safe(tmp);
        arg = PyList_GET_ITEM(tmp, 0);
      }

      // If the constructor input is KMeansTrainer object
      if (PyBobLearnEMKMeansTrainer_Check(arg))
        return PyBobLearnEMKMeansTrainer_init_copy(self, args, kwargs);
      else if(PyString_Check(arg))
        return PyBobLearnEMKMeansTrainer_init_str(self, args, kwargs);
    }
    default:{
      PyErr_Format(PyExc_RuntimeError, "number of arguments mismatch - %s requires 0 or 1 arguments, but you provided %d (see help)", Py_TYPE(self)->tp_name, nargs);
      KMeansTrainer_doc.print_usage();
      return -1;
    }
  }
  BOB_CATCH_MEMBER("cannot create KMeansTrainer", -1)
  return 0;
}

static void PyBobLearnEMKMeansTrainer_delete(PyBobLearnEMKMeansTrainerObject* self) {
  self->cxx.reset();
  Py_TYPE(self)->tp_free((PyObject*)self);
}

int PyBobLearnEMKMeansTrainer_Check(PyObject* o) {
  return PyObject_IsInstance(o, reinterpret_cast<PyObject*>(&PyBobLearnEMKMeansTrainer_Type));
}

static PyObject* PyBobLearnEMKMeansTrainer_RichCompare(PyBobLearnEMKMeansTrainerObject* self, PyObject* other, int op) {
  BOB_TRY

  if (!PyBobLearnEMKMeansTrainer_Check(other)) {
    PyErr_Format(PyExc_TypeError, "cannot compare `%s' with `%s'", Py_TYPE(self)->tp_name, Py_TYPE(other)->tp_name);
    return 0;
  }
  auto other_ = reinterpret_cast<PyBobLearnEMKMeansTrainerObject*>(other);
  switch (op) {
    case Py_EQ:
      if (*self->cxx==*other_->cxx) Py_RETURN_TRUE; else Py_RETURN_FALSE;
    case Py_NE:
      if (*self->cxx==*other_->cxx) Py_RETURN_FALSE; else Py_RETURN_TRUE;
    default:
      Py_INCREF(Py_NotImplemented);
      return Py_NotImplemented;
  }
  BOB_CATCH_MEMBER("cannot compare KMeansTrainer objects", 0)
}

/******************************************************************/
/************ Variables Section ***********************************/
/******************************************************************/

/***** initialization_method *****/
static auto initialization_method = bob::extension::VariableDoc(
  "initialization_method",
  "str",
  "Initialization method.",
  "Possible values:\n"
  " `RANDOM`: Random initialization \n\n"
  " `RANDOM_NO_DUPLICATE`: Random initialization without repetition \n\n"
  " `KMEANS_PLUS_PLUS`: Apply the kmeans++ initialization http://en.wikipedia.org/wiki/K-means%2B%2B  \n\n"
);
PyObject* PyBobLearnEMKMeansTrainer_getInitializationMethod(PyBobLearnEMKMeansTrainerObject* self, void*) {
  BOB_TRY
  return Py_BuildValue("s", IM2string(self->cxx->getInitializationMethod()).c_str());
  BOB_CATCH_MEMBER("initialization method could not be read", 0)
}
int PyBobLearnEMKMeansTrainer_setInitializationMethod(PyBobLearnEMKMeansTrainerObject* self, PyObject* value, void*) {
  BOB_TRY

  if (!PyString_Check(value)){
    PyErr_Format(PyExc_RuntimeError, "%s %s expects an str", Py_TYPE(self)->tp_name, initialization_method.name());
    return -1;
  }
  self->cxx->setInitializationMethod(string2IM(PyString_AS_STRING(value)));

  return 0;
  BOB_CATCH_MEMBER("initialization method could not be set", -1)
}

/***** zeroeth_order_statistics *****/
static auto zeroeth_order_statistics = bob::extension::VariableDoc(
  "zeroeth_order_statistics",
  "array_like <float, 1D>",
  "Returns the internal statistics. Useful to parallelize the E-step",
  ""
);
PyObject* PyBobLearnEMKMeansTrainer_getZeroethOrderStatistics(PyBobLearnEMKMeansTrainerObject* self, void*){
  BOB_TRY
  return PyBlitzArrayCxx_AsConstNumpy(self->cxx->getZeroethOrderStats());
  BOB_CATCH_MEMBER("zeroeth_order_statistics could not be read", 0)
}
int PyBobLearnEMKMeansTrainer_setZeroethOrderStatistics(PyBobLearnEMKMeansTrainerObject* self, PyObject* value, void*){
  BOB_TRY
  PyBlitzArrayObject* o;
  if (!PyBlitzArray_Converter(value, &o)){
    PyErr_Format(PyExc_RuntimeError, "%s %s expects a 1D array of floats", Py_TYPE(self)->tp_name, zeroeth_order_statistics.name());
    return -1;
  }
  auto o_ = make_safe(o);
  auto b = PyBlitzArrayCxx_AsBlitz<double,1>(o, "zeroeth_order_statistics");
  if (!b) return -1;
  self->cxx->setZeroethOrderStats(*b);
  return 0;
  BOB_CATCH_MEMBER("zeroeth_order_statistics could not be set", -1)
}

/***** first_order_statistics *****/
static auto first_order_statistics = bob::extension::VariableDoc(
  "first_order_statistics",
  "array_like <float, 2D>",
  "Returns the internal statistics. Useful to parallelize the E-step",
  ""
);
PyObject* PyBobLearnEMKMeansTrainer_getFirstOrderStatistics(PyBobLearnEMKMeansTrainerObject* self, void*){
  BOB_TRY
  return PyBlitzArrayCxx_AsConstNumpy(self->cxx->getFirstOrderStats());
  BOB_CATCH_MEMBER("first_order_statistics could not be read", 0)
}
int PyBobLearnEMKMeansTrainer_setFirstOrderStatistics(PyBobLearnEMKMeansTrainerObject* self, PyObject* value, void*){
  BOB_TRY
  PyBlitzArrayObject* o;
  if (!PyBlitzArray_Converter(value, &o)){
    PyErr_Format(PyExc_RuntimeError, "%s %s expects a 1D array of floats", Py_TYPE(self)->tp_name, first_order_statistics.name());
    return -1;
  }
  auto o_ = make_safe(o);
  auto b = PyBlitzArrayCxx_AsBlitz<double,2>(o, "first_order_statistics");
  if (!b) return -1;
  self->cxx->setFirstOrderStats(*b);
  return 0;
  BOB_CATCH_MEMBER("first_order_statistics could not be set", -1)
}

/***** average_min_distance *****/
static auto average_min_distance = bob::extension::VariableDoc(
  "average_min_distance",
  "str",
  "Average min (square Euclidean) distance. Useful to parallelize the E-step.",
  ""
);
PyObject* PyBobLearnEMKMeansTrainer_getAverageMinDistance(PyBobLearnEMKMeansTrainerObject* self, void*) {
  BOB_TRY
  return Py_BuildValue("d", self->cxx->getAverageMinDistance());
  BOB_CATCH_MEMBER("Average Min Distance method could not be read", 0)
}
int PyBobLearnEMKMeansTrainer_setAverageMinDistance(PyBobLearnEMKMeansTrainerObject* self, PyObject* value, void*) {
  BOB_TRY

  if (!PyBob_NumberCheck(value)){
    PyErr_Format(PyExc_RuntimeError, "%s %s expects an double", Py_TYPE(self)->tp_name, average_min_distance.name());
    return -1;
  }
  self->cxx->setAverageMinDistance(PyFloat_AS_DOUBLE(value));

  return 0;
  BOB_CATCH_MEMBER("Average Min Distance could not be set", -1)
}

static PyGetSetDef PyBobLearnEMKMeansTrainer_getseters[] = {
  {
   initialization_method.name(),
   (getter)PyBobLearnEMKMeansTrainer_getInitializationMethod,
   (setter)PyBobLearnEMKMeansTrainer_setInitializationMethod,
   initialization_method.doc(),
   0
  },
  {
   zeroeth_order_statistics.name(),
   (getter)PyBobLearnEMKMeansTrainer_getZeroethOrderStatistics,
   (setter)PyBobLearnEMKMeansTrainer_setZeroethOrderStatistics,
   zeroeth_order_statistics.doc(),
   0
  },
  {
   first_order_statistics.name(),
   (getter)PyBobLearnEMKMeansTrainer_getFirstOrderStatistics,
   (setter)PyBobLearnEMKMeansTrainer_setFirstOrderStatistics,
   first_order_statistics.doc(),
   0
  },
  {
   average_min_distance.name(),
   (getter)PyBobLearnEMKMeansTrainer_getAverageMinDistance,
   (setter)PyBobLearnEMKMeansTrainer_setAverageMinDistance,
   average_min_distance.doc(),
   0
  },
  {0}  // Sentinel
};

/******************************************************************/
/************ Functions Section ***********************************/
/******************************************************************/

/*** initialize ***/
static auto initialize = bob::extension::FunctionDoc(
  "initialize",
  "Initialise the means randomly",
  "Data is split into as many chunks as there are means, then each mean is set to a random example within each chunk.",
  true
)
.add_prototype("kmeans_machine, data, [rng]")
.add_parameter("kmeans_machine", ":py:class:`bob.learn.em.KMeansMachine`", "KMeansMachine Object")
.add_parameter("data", "array_like <float, 2D>", "Input data")
.add_parameter("rng", ":py:class:`bob.core.random.mt19937`", "The Mersenne Twister mt19937 random generator used for the initialization of subspaces/arrays before the EM loop.");
static PyObject* PyBobLearnEMKMeansTrainer_initialize(PyBobLearnEMKMeansTrainerObject* self, PyObject* args, PyObject* kwargs) {
  BOB_TRY

  /* Parses input arguments in a single shot */
  char** kwlist = initialize.kwlist(0);

  PyBobLearnEMKMeansMachineObject* kmeans_machine = 0;
  PyBlitzArrayObject* data                        = 0;
  PyBoostMt19937Object* rng = 0;

  if (!PyArg_ParseTupleAndKeywords(args, kwargs, "O!O&|O!", kwlist, &PyBobLearnEMKMeansMachine_Type, &kmeans_machine,
                                                                 &PyBlitzArray_Converter, &data,
                                                                  &PyBoostMt19937_Type, &rng)) return 0;
  auto data_ = make_safe(data);

  // perform check on the input
  if (data->type_num != NPY_FLOAT64){
    PyErr_Format(PyExc_TypeError, "`%s' only supports 64-bit float arrays for input array `%s`", Py_TYPE(self)->tp_name, initialize.name());
    return 0;
  }

  if (data->ndim != 2){
    PyErr_Format(PyExc_TypeError, "`%s' only processes 2D arrays of float64 for `%s`", Py_TYPE(self)->tp_name, initialize.name());
    return 0;
  }

  if (data->shape[1] != (Py_ssize_t)kmeans_machine->cxx->getNInputs() ) {
    PyErr_Format(PyExc_TypeError, "`%s' 2D `input` array should have the shape [N, %" PY_FORMAT_SIZE_T "d] not [N, %" PY_FORMAT_SIZE_T "d] for `%s`", Py_TYPE(self)->tp_name, kmeans_machine->cxx->getNInputs(), data->shape[1], initialize.name());
    return 0;
  }

  if(rng){
    self->cxx->setRng(rng->rng);
  }

  self->cxx->initialize(*kmeans_machine->cxx, *PyBlitzArrayCxx_AsBlitz<double,2>(data));

  BOB_CATCH_MEMBER("cannot perform the initialize method", 0)

  Py_RETURN_NONE;
}

/*** e_step ***/
static auto e_step = bob::extension::FunctionDoc(
  "e_step",
  "Compute the E-step, which is basically the distances ",
  "Accumulate across the dataset:"
  " -zeroeth and first order statistics"
  " -average (Square Euclidean) distance from the closest mean",
  true
)
.add_prototype("kmeans_machine,data")
.add_parameter("kmeans_machine", ":py:class:`bob.learn.em.KMeansMachine`", "KMeansMachine Object")
.add_parameter("data", "array_like <float, 2D>", "Input data");
static PyObject* PyBobLearnEMKMeansTrainer_e_step(PyBobLearnEMKMeansTrainerObject* self, PyObject* args, PyObject* kwargs) {
  BOB_TRY

  /* Parses input arguments in a single shot */
  char** kwlist = e_step.kwlist(0);

  PyBobLearnEMKMeansMachineObject* kmeans_machine;
  PyBlitzArrayObject* data = 0;
  if (!PyArg_ParseTupleAndKeywords(args, kwargs, "O!O&", kwlist, &PyBobLearnEMKMeansMachine_Type, &kmeans_machine,
                                                                 &PyBlitzArray_Converter, &data)) return 0;
  auto data_ = make_safe(data);

  if (data->type_num != NPY_FLOAT64){
    PyErr_Format(PyExc_TypeError, "`%s' only supports 64-bit float arrays for input array `%s`", Py_TYPE(self)->tp_name, e_step.name());
    return 0;
  }

  if (data->ndim != 2){
    PyErr_Format(PyExc_TypeError, "`%s' only processes 2D arrays of float64 for `%s`", Py_TYPE(self)->tp_name, e_step.name());
    return 0;
  }

  if (data->shape[1] != (Py_ssize_t)kmeans_machine->cxx->getNInputs() ) {
    PyErr_Format(PyExc_TypeError, "`%s' 2D `input` array should have the shape [N, %" PY_FORMAT_SIZE_T "d] not [N, %" PY_FORMAT_SIZE_T "d] for `%s`", Py_TYPE(self)->tp_name, kmeans_machine->cxx->getNInputs(), data->shape[1], e_step.name());
    return 0;
  }

  auto state = PyEval_SaveThread();
  self->cxx->eStep(*kmeans_machine->cxx, *PyBlitzArrayCxx_AsBlitz<double,2>(data));
  PyEval_RestoreThread(state);

  BOB_CATCH_MEMBER("cannot perform the e_step method", 0)

  Py_RETURN_NONE;
}

/*** m_step ***/
static auto m_step = bob::extension::FunctionDoc(
  "m_step",
  "Updates the mean based on the statistics from the E-step",
  0,
  true
)
.add_prototype("kmeans_machine, [data]")
.add_parameter("kmeans_machine", ":py:class:`bob.learn.em.KMeansMachine`", "KMeansMachine Object")
.add_parameter("data", "object", "Ignored.");
static PyObject* PyBobLearnEMKMeansTrainer_m_step(PyBobLearnEMKMeansTrainerObject* self, PyObject* args, PyObject* kwargs) {
  BOB_TRY

  /* Parses input arguments in a single shot */
  char** kwlist = m_step.kwlist(0);

  PyBobLearnEMKMeansMachineObject* kmeans_machine;
  PyObject* data = 0;
  if (!PyArg_ParseTupleAndKeywords(args, kwargs, "O!|O", kwlist, &PyBobLearnEMKMeansMachine_Type, &kmeans_machine,
                                                                 &data)) return 0;
  self->cxx->mStep(*kmeans_machine->cxx);

  BOB_CATCH_MEMBER("cannot perform the m_step method", 0)

  Py_RETURN_NONE;
}

/*** computeLikelihood ***/
static auto compute_likelihood = bob::extension::FunctionDoc(
  "compute_likelihood",
  "This functions returns the average min (Square Euclidean) distance (average distance to the closest mean)",
  0,
  true
)
.add_prototype("kmeans_machine")
.add_parameter("kmeans_machine", ":py:class:`bob.learn.em.KMeansMachine`", "KMeansMachine Object");
static PyObject* PyBobLearnEMKMeansTrainer_compute_likelihood(PyBobLearnEMKMeansTrainerObject* self, PyObject* args, PyObject* kwargs) {
  BOB_TRY

  /* Parses input arguments in a single shot */
  char** kwlist = compute_likelihood.kwlist(0);

  PyBobLearnEMKMeansMachineObject* kmeans_machine;
  if (!PyArg_ParseTupleAndKeywords(args, kwargs, "O!", kwlist, &PyBobLearnEMKMeansMachine_Type, &kmeans_machine)) return 0;

  double value = self->cxx->computeLikelihood(*kmeans_machine->cxx);
  return Py_BuildValue("d", value);

  BOB_CATCH_MEMBER("cannot perform the computeLikelihood method", 0)
}

/*** reset_accumulators ***/
static auto reset_accumulators = bob::extension::FunctionDoc(
  "reset_accumulators",
  "Reset the statistics accumulators to the correct size and a value of zero.",
  0,
  true
)
.add_prototype("kmeans_machine")
.add_parameter("kmeans_machine", ":py:class:`bob.learn.em.KMeansMachine`", "KMeansMachine Object");
static PyObject* PyBobLearnEMKMeansTrainer_reset_accumulators(PyBobLearnEMKMeansTrainerObject* self, PyObject* args, PyObject* kwargs) {
  BOB_TRY

  /* Parses input arguments in a single shot */
  char** kwlist = reset_accumulators.kwlist(0);

  PyBobLearnEMKMeansMachineObject* kmeans_machine;
  if (!PyArg_ParseTupleAndKeywords(args, kwargs, "O!", kwlist, &PyBobLearnEMKMeansMachine_Type, &kmeans_machine)) return 0;

  self->cxx->resetAccumulators(*kmeans_machine->cxx);
  Py_RETURN_NONE;

  BOB_CATCH_MEMBER("cannot perform the reset_accumulators method", 0)
}

static PyMethodDef PyBobLearnEMKMeansTrainer_methods[] = {
  {
    initialize.name(),
    (PyCFunction)PyBobLearnEMKMeansTrainer_initialize,
    METH_VARARGS|METH_KEYWORDS,
    initialize.doc()
  },
  {
    e_step.name(),
    (PyCFunction)PyBobLearnEMKMeansTrainer_e_step,
    METH_VARARGS|METH_KEYWORDS,
    e_step.doc()
  },
  {
    m_step.name(),
    (PyCFunction)PyBobLearnEMKMeansTrainer_m_step,
    METH_VARARGS|METH_KEYWORDS,
    m_step.doc()
  },
  {
    compute_likelihood.name(),
    (PyCFunction)PyBobLearnEMKMeansTrainer_compute_likelihood,
    METH_VARARGS|METH_KEYWORDS,
    compute_likelihood.doc()
  },
  {
    reset_accumulators.name(),
    (PyCFunction)PyBobLearnEMKMeansTrainer_reset_accumulators,
    METH_VARARGS|METH_KEYWORDS,
    reset_accumulators.doc()
  },
  {0} /* Sentinel */
};

/******************************************************************/
/************ Module Section **************************************/
/******************************************************************/

// Define the Gaussian type struct; will be initialized later
PyTypeObject PyBobLearnEMKMeansTrainer_Type = {
  PyVarObject_HEAD_INIT(0,0)
  0
};

bool init_BobLearnEMKMeansTrainer(PyObject* module)
{
  // initialize the type struct
  PyBobLearnEMKMeansTrainer_Type.tp_name = KMeansTrainer_doc.name();
  PyBobLearnEMKMeansTrainer_Type.tp_basicsize = sizeof(PyBobLearnEMKMeansTrainerObject);
  PyBobLearnEMKMeansTrainer_Type.tp_flags = Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE;//Enable the class inheritance
  PyBobLearnEMKMeansTrainer_Type.tp_doc = KMeansTrainer_doc.doc();

  // set the functions
  PyBobLearnEMKMeansTrainer_Type.tp_new = PyType_GenericNew;
  PyBobLearnEMKMeansTrainer_Type.tp_init = reinterpret_cast<initproc>(PyBobLearnEMKMeansTrainer_init);
  PyBobLearnEMKMeansTrainer_Type.tp_dealloc = reinterpret_cast<destructor>(PyBobLearnEMKMeansTrainer_delete);
  PyBobLearnEMKMeansTrainer_Type.tp_richcompare = reinterpret_cast<richcmpfunc>(PyBobLearnEMKMeansTrainer_RichCompare);
  PyBobLearnEMKMeansTrainer_Type.tp_methods = PyBobLearnEMKMeansTrainer_methods;
  PyBobLearnEMKMeansTrainer_Type.tp_getset = PyBobLearnEMKMeansTrainer_getseters;
  PyBobLearnEMKMeansTrainer_Type.tp_call = reinterpret_cast<ternaryfunc>(PyBobLearnEMKMeansTrainer_compute_likelihood);

  // check that everything is fine
  if (PyType_Ready(&PyBobLearnEMKMeansTrainer_Type) < 0) return false;

  // add the type to the module
  Py_INCREF(&PyBobLearnEMKMeansTrainer_Type);
  return PyModule_AddObject(module, "KMeansTrainer", (PyObject*)&PyBobLearnEMKMeansTrainer_Type) >= 0;
}


#pragma once

#include <Eigen/Core>

template <typename Derived, typename Scalar_t, int tangent_dim>
class ManifoldElement
{
 public:
  using Scalar = Scalar_t;
  using TangentVec = Eigen::Matrix<Scalar, tangent_dim, 1>;

  ManifoldElement() = default;
  virtual ~ManifoldElement() = default;

  // See https://stackoverflow.com/a/4782927 or
  // https://en.cppreference.com/w/cpp/language/rule_of_three#Rule_of_zero
  ManifoldElement(const ManifoldElement &) = default; // Copy constructor
  ManifoldElement(ManifoldElement &&) = default;      // Move constructor
  ManifoldElement &operator=(const ManifoldElement &) & =
      default; // Copy assignment operator
  ManifoldElement &operator=(ManifoldElement &&) & =
      default; // Move assignment operator

  virtual Derived operator+(const TangentVec &diff) const = 0;
  virtual TangentVec operator-(const Derived &element) const = 0;

  static constexpr unsigned int tangent_dim_ = tangent_dim;
};


#include <iostream>
#include "gdal_priv.h"
#include "cpl_conv.h" // for CPLMalloc()
#include <boost/array.hpp>
#include <boost/foreach.hpp>
#define MAX_BANDS (30)
#include <limits.h>
#include <float.h>
#include <stdio.h>
#include <stdlib.h>
#include <getopt.h>
#include <math.h>

//Open
GDALDataset  *gdal_open( char *filename) {
 	GDALDataset  *poDataset;

    	poDataset = (GDALDataset *) GDALOpen( filename, GA_ReadOnly );
    	if( poDataset == NULL )
    	{
		std::cerr << "Could not open \'" << filename << "\'\n";
		exit(-1);
	}

 	return poDataset;
}

int ussage(char *name) {
	 std::cerr << "Ussage. TBD.\n";
         exit(-1);
}

//Get bands, put in vector, return. Dupilcates vector, but not the end of the world..
std::vector<GDALRasterBand  *> get_bands( GDALDataset  *poDataset)
  {
	std::vector<GDALRasterBand  *> bands;
	for (int i=0; i< poDataset->GetRasterCount(); i++) {
		 bands.push_back( poDataset->GetRasterBand(i+1));
	}

	return bands;
  }

void gdal_init() {
	 GDALAllRegister();
}

#define YAML_INDENT ("  ")
//does the actual work. valid_start and valid_stop are not currently used..
//if nodatad==NAN, then look at the data for nodata values.
void examine_data ( std::vector<GDALRasterBand  *> bands, double nodatad, double *valid_start=NULL, double *valid_stop=NULL ) {
	float 		*pafScanline;
	int 		*paiScanline;
	int		nodata;
	float		nodataf;
	bool		floating_point=false;
	int		nXSize;
	int		nYSize;
	int 		pbSuccess = NULL;

	//TAKE NOTE!
	//if nodatad == NAN, then get nodata value directly from tiff.
	if (isnan(nodatad) )
		nodatad = bands[0]->GetNoDataValue();

	//less typing..
	nXSize=bands[0]->GetXSize();
	nYSize=bands[0]->GetYSize();
	//nYSize=2;

	//floating point flag..
	if (bands[0]->GetRasterDataType()==GDT_Float32 || bands[0]->GetRasterDataType()==GDT_Float64) floating_point = true;

	if (floating_point) {
		nodataf = (float)nodatad;
        	pafScanline = (float *) CPLMalloc(sizeof(float)*nXSize);
		}
	else {
		nodata = (int)nodatad;
		paiScanline = (int *) CPLMalloc(sizeof(float)*nXSize);
	}

	//start of yml output..
	std::cout << "bands:" << "\n";

        BOOST_FOREACH( GDALRasterBand  *band, bands )
        {
		//stats..
        	unsigned long   valid_data=0;
        	float           fmax=FLT_MIN,fmin=FLT_MAX;
        	int             max=INT_MIN,min=INT_MAX;

		for(int y=0; y <  nYSize; y++) {
			if (floating_point) {
				band->RasterIO( GF_Read, 0, y, nXSize, 1, pafScanline, nXSize, 1, GDT_Float32, 0, 0 );
				for(int x=0; x < nXSize;x++) {
					if (pafScanline[x] == nodataf) continue;
					if ( fmin > pafScanline[x] ) fmin =  pafScanline[x];
					else if ( fmax < pafScanline[x] ) fmax =  pafScanline[x];
					valid_data++;
                                }
			} else {
				band->RasterIO( GF_Read, 0, y, nXSize, 1, paiScanline, nXSize, 1, GDT_Int32, 0, 0 );
				for(int x=0; x < nXSize;x++) {
					if (paiScanline[x] == nodata) continue;
                                        if ( min > paiScanline[x] ) min =  paiScanline[x];
                                        else if ( max < paiScanline[x] ) max =  paiScanline[x];
					valid_data ++;
				}
			}

		}

		//print YAML stats..
		std::cout << "- " << "band_no: " << band->GetBand() << "\n";
		std::cout << YAML_INDENT << "overviews: " << band->GetOverviewCount() << "\n";
		std::cout << YAML_INDENT << "valid_pixels: " << valid_data << "\n";
		std::cout << YAML_INDENT << "nodata_pixels: " << nYSize*nXSize-valid_data << "\n";
		std::cout << YAML_INDENT << "nodata_value: " << nodatad << "\n";
		if ( floating_point ) {
			std::cout << YAML_INDENT << "min: " << fmin << "\n";
			std::cout << YAML_INDENT << "max: " << fmax << "\n";
		} else {
			std::cout << YAML_INDENT << "min: " << min << "\n";
                       	std::cout << YAML_INDENT << "max: " << max << "\n";
		}
        }

}

//export some basic stats..
void get_basic_stats( GDALDataset  *poDataset) {
	double padfTransform[6];

	//get sizes
	std::cout << "width: " << poDataset->GetRasterXSize() << "\n";
	std::cout << "height: " << poDataset->GetRasterYSize() << "\n";
	//get projection
	std::cout << "projection: " << "'" << poDataset->GetProjectionRef() << "' \n";
	std::cout << "geo_transform: " << "\n";
	poDataset->GetGeoTransform(padfTransform);
	BOOST_FOREACH (double v,padfTransform )
		std::cout << "- " << v << "\n";
}

void ussage(){
	std::cout << "This tool provides basic information about an image in yaml format.\n";
        std::cout << "Ussage:\n";
        std::cout << "\t\timage_info (-h|--help) (-n|nodata <nodata_value>) input_file\n";
        exit(0);
}

void parse_command_line(int argc, char **argv, char *infile, double &nodata) {
	int c;

       	while (1)
         {
           static struct option long_options[] =
             {
               /* These options set a flag. */
               {"help", no_argument, 0, 'v'},
               {"nodata",  required_argument, 0, 'n'},
               {0, 0, 0, 0}
             };
           /* getopt_long stores the option index here. */
           int option_index = 0;

           c = getopt_long (argc, argv, "h:n:",
                            long_options, &option_index);

           /* Detect the end of the options. */
           if (c == -1)
             break;

           switch (c)
             {
             case 'h':
		ussage();
		exit(0);
             case 'n':
		nodata = atof(optarg);
               break;
             }
         }

	if (optind + 1== argc)
         {
		strcpy(infile,argv[optind]);
		std::cout << "infile: " << infile << "\n";
         }
	else
	 {
		ussage();
	 }

}

int main (int argc, char **argv) {
	GDALDataset  *poDataset;
	GDALRasterBand  *poBand;
	char infile[1024];
	double nodata=NAN;
	std::vector<GDALRasterBand  *> bands;

	parse_command_line(argc,argv,infile,nodata);

	gdal_init();

	poDataset = gdal_open(infile);

	get_basic_stats(poDataset);
	bands = get_bands(poDataset);

	examine_data(bands, nodata);
}


#ifndef NETWORKS_HPP
#define NETWORKS_HPP

#include <vector>
// For External Library
#include <torch/torch.h>
#include <boost/program_options.hpp>

// Define Namespace
namespace nn = torch::nn;
namespace po = boost::program_options;

// Function Prototype
void weights_init(nn::Module &m);
torch::Tensor UpSampling(torch::Tensor x, const std::vector<long int> shape);

// -------------------------------------------------
// struct{YOLOv3Impl}(nn::Module)
// -------------------------------------------------
struct YOLOv3Impl : nn::Module{
private:
    nn::Sequential stage1, stage2, stage3, stage4, stage5, stage6;
    nn::Sequential stageA, stageB, stageC;
public:
    YOLOv3Impl(){}
    YOLOv3Impl(po::variables_map &vm);
    std::vector<torch::Tensor> forward(torch::Tensor x);
};

// -------------------------------------------------
// struct{ResBlockImpl}(nn::Module)
// -------------------------------------------------
struct ResBlockImpl : nn::Module{
private:
    nn::Sequential sq;
public:
    ResBlockImpl(){}
    ResBlockImpl(const size_t outside_nc, const size_t inside_nc);
    torch::Tensor forward(torch::Tensor x);
};

// -------------------------------------------------
// struct{ConvBlockImpl}(nn::Module)
// -------------------------------------------------
struct ConvBlockImpl : nn::Module{
private:
    nn::Sequential sq;
public:
    ConvBlockImpl(){}
    ConvBlockImpl(const size_t in_nc, const size_t out_nc, const size_t ksize, const size_t stride, const size_t pad, const bool BN, const bool LReLU, const bool bias=false);
    torch::Tensor forward(torch::Tensor x);
};

TORCH_MODULE(YOLOv3);
TORCH_MODULE(ResBlock);
TORCH_MODULE(ConvBlock);

#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2013 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file gaussian1dmodel.hpp
    \brief basic interface for one factor interest rate models
*/

// uncomment to enable NTL support (see below for more details and references)
// #define GAUSS1D_ENABLE_NTL

#ifndef quantlib_gaussian1dmodel_hpp
#define quantlib_gaussian1dmodel_hpp

#include <ql/models/model.hpp>
#include <ql/models/parameter.hpp>
#include <ql/math/interpolation.hpp>
#include <ql/math/interpolations/cubicinterpolation.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/indexes/swapindex.hpp>
#include <ql/instruments/vanillaswap.hpp>
#include <ql/time/date.hpp>
#include <ql/time/period.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/stochasticprocess.hpp>
#include <ql/utilities/null.hpp>
#include <ql/patterns/lazyobject.hpp>

#ifdef GAUSS1D_ENABLE_NTL
    #include <boost/math/bindings/rr.hpp>
#endif

#include <boost/math/special_functions/erf.hpp>

namespace QuantLib {

    /*! One factor interest rate model interface class
        The only methods that must be implemented by subclasses
        are the numeraire and zerobond methods for an input array
        of state variable values. The variable $y$ is understood
        to be the standardized (zero mean, unit variance) version
        of the model's original state variable $x$.

        NTL support may be enabled by defining GAUSS1D_ENABLE_NTL in this
        file. For details on NTL see
                 http://www.shoup.net/ntl/

        \warning the variance of the state process conditional on
        $x(t)=x$ must be independent of the value of $x$

    */

    class Gaussian1dModel : public TermStructureConsistentModel,
                            public LazyObject {

      public:

        const boost::shared_ptr<StochasticProcess1D> stateProcess() const;

        const Real numeraire(const Time t, const Real y = 0.0,
                             const Handle<YieldTermStructure> &yts =
                                 Handle<YieldTermStructure>()) const;

        const Real zerobond(const Time T, const Time t = 0.0,
                            const Real y = 0.0,
                            const Handle<YieldTermStructure> &yts =
                                Handle<YieldTermStructure>()) const;

        const Real numeraire(const Date &referenceDate, const Real y = 0.0,
                             const Handle<YieldTermStructure> &yts =
                                 Handle<YieldTermStructure>()) const;

        const Real zerobond(const Date &maturity,
                            const Date &referenceDate = Null<Date>(),
                            const Real y = 0.0,
                            const Handle<YieldTermStructure> &yts =
                                Handle<YieldTermStructure>()) const;

        const Real zerobondOption(
            const Option::Type &type, const Date &expiry, const Date &valueDate,
            const Date &maturity, const Rate strike,
            const Date &referenceDate = Null<Date>(), const Real y = 0.0,
            const Handle<YieldTermStructure> &yts =
                Handle<YieldTermStructure>(),
            const Real yStdDevs = 7.0, const Size yGridPoints = 64,
            const bool extrapolatePayoff = true,
            const bool flatPayoffExtrapolation = false) const;

        const Real forwardRate(const Date &fixing,
                               const Date &referenceDate = Null<Date>(),
                               const Real y = 0.0,
                               boost::shared_ptr<IborIndex> iborIdx =
                                   boost::shared_ptr<IborIndex>()) const;

        const Real swapRate(const Date &fixing, const Period &tenor,
                            const Date &referenceDate = Null<Date>(),
                            const Real y = 0.0,
                            boost::shared_ptr<SwapIndex> swapIdx =
                                boost::shared_ptr<SwapIndex>()) const;

        const Real swapAnnuity(const Date &fixing, const Period &tenor,
                               const Date &referenceDate = Null<Date>(),
                               const Real y = 0.0,
                               boost::shared_ptr<SwapIndex> swapIdx =
                                   boost::shared_ptr<SwapIndex>()) const;

        /*! Computes the integral
        \f[ {2\pi}^{-0.5} \int_{a}^{b} p(x) \exp{-0.5*x*x} \mathrm{d}x \f]
        with
        \f[ p(x) = ax^4+bx^3+cx^2+dx+e \f].
        */
        const static Real gaussianPolynomialIntegral(const Real a, const Real b,
                                                     const Real c, const Real d,
                                                     const Real e,
                                                     const Real x0,
                                                     const Real x1);

        /*! Computes the integral
        \f[ {2\pi}^{-0.5} \int_{a}^{b} p(x) \exp{-0.5*x*x} \mathrm{d}x \f]
        with
        \f[ p(x) = a(x-h)^4+b(x-h)^3+c(x-h)^2+d(x-h)+e \f].
        */
        const static Real gaussianShiftedPolynomialIntegral(
            const Real a, const Real b, const Real c, const Real d,
            const Real e, const Real h, const Real x0, const Real x1);

        /*! Generates a grid of values for the standardized state variable $y$
           at time $T$
            conditional on $y(t)=y$, covering yStdDevs standard deviations
           consisting of
            2*gridPoints+1 points */

        const Disposable<Array> yGrid(const Real yStdDevs, const int gridPoints,
                                      const Real T = 1.0, const Real t = 0,
                                      const Real y = 0) const;

      protected:

        // we let derived classes register with the termstructure
        Gaussian1dModel(const Handle<YieldTermStructure> &yieldTermStructure)
            : TermStructureConsistentModel(yieldTermStructure) {}

        virtual ~Gaussian1dModel() {}

        virtual const Real
        numeraireImpl(const Time t, const Real y,
                      const Handle<YieldTermStructure> &yts) const = 0;

        virtual const Real
        zerobondImpl(const Time T, const Time t, const Real y,
                     const Handle<YieldTermStructure> &yts) const = 0;

        void performCalculations() const {}

        void generateArguments() {
            calculate();
            notifyObservers();
        }

        boost::shared_ptr<StochasticProcess1D> stateProcess_;
    };

    inline const boost::shared_ptr<StochasticProcess1D>
    Gaussian1dModel::stateProcess() const {

        QL_REQUIRE(stateProcess_ != NULL, "state process not set");
        return stateProcess_;

    }

    inline const Real
    Gaussian1dModel::numeraire(const Time t, const Real y,
                               const Handle<YieldTermStructure> &yts) const {

        return numeraireImpl(t, y, yts);
    }

    inline const Real
    Gaussian1dModel::zerobond(const Time T, const Time t, const Real y,
                              const Handle<YieldTermStructure> &yts) const {

        return zerobondImpl(T, t, y, yts);
    }

    inline const Real
    Gaussian1dModel::numeraire(const Date &referenceDate, const Real y,
                               const Handle<YieldTermStructure> &yts) const {

        return numeraire(termStructure()->timeFromReference(referenceDate), y,
                         yts);
    }

    inline const Real
    Gaussian1dModel::zerobond(const Date &maturity, const Date &referenceDate,
                              const Real y,
                              const Handle<YieldTermStructure> &yts) const {

        return zerobond(termStructure()->timeFromReference(maturity),
                        referenceDate != Null<Date>()
                            ? termStructure()->timeFromReference(referenceDate)
                            : 0.0,
                        y, yts);
    }

}

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_ADDS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_ADDS_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/adds.hpp>
#include <boost/mpl/logical.hpp>
#include <boost/simd/toolbox/arithmetic/functions/adds.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/is_greater.hpp>
#include <boost/simd/include/functions/simd/is_gtz.hpp>
#include <boost/simd/include/functions/simd/logical_and.hpp>
#include <boost/simd/include/functions/simd/logical_or.hpp>
#include <boost/simd/include/functions/simd/logical_not.hpp>
//#include <boost/simd/include/functions/simd/logical_notand.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/min.hpp>
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::adds_, tag::cpu_,
                          (A0)(X),
                          ((simd_<floating_<A0>,X>))
                          ((simd_<floating_<A0>,X>))
                         )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return a0+a1; }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::adds_, tag::cpu_,
                          (A0)(X),
                          ((simd_<uint_<A0>,X>))
                          ((simd_<uint_<A0>,X>))
                         )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      A0 a0pa1 = a0+a1;
      return if_else(lt(a0pa1, a0), Valmax<A0>(), a0pa1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::adds_, tag::cpu_,
                          (A0)(X),
                          ((simd_<int_<A0>,X>))
                          ((simd_<int_<A0>,X>))
                         )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::as_logical<A0>::type bA0;
      bA0 gtza0 = is_gtz(a0);
      bA0 gtza1 = is_gtz(a1);
      A0 a0pa1 = a0+a1;
      bA0 test1 = logical_and(logical_and(gtza0, gtza1), lt(a0pa1, a0));
      bA0 test2 = logical_and(logical_not(logical_or(gtza0, gtza1)), gt(a0pa1,a0)); //logical_notand
      return if_else(test1,Valmax<A0>(),if_else(test2,Valmin<A0>(),a0pa1));
    }
  };
} } }
#endif


#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <algorithm>
#include <boost/timer.hpp>

#include "myslam/config.h"
#include "myslam/visual_odometry.h"
#include "myslam/g2o_types.h"
namespace myslam
{

VisualOdometry::VisualOdometry() : state_(INITIALIZING), ref_(nullptr), curr_(nullptr), map_(new Map), num_lost_(0), num_inliers_(0)
{
    num_of_features_ = Config::get<int>("number_of_features");
    scale_factor_ = Config::get<double>("scale_factor");
    level_pyramid_ = Config::get<int>("level_pyramid");
    match_ratio_ = Config::get<float>("match_ratio");
    max_num_lost_ = Config::get<float>("max_num_lost");
    min_inliers_ = Config::get<int>("min_inliers");
    key_frame_min_rot = Config::get<double>("keyframe_rotation");
    key_frame_min_trans = Config::get<double>("keyframe_translation");
    orb_ = cv::ORB::create(num_of_features_, scale_factor_, level_pyramid_);
}

VisualOdometry::~VisualOdometry()
{
}

bool VisualOdometry::addFrame(Frame::Ptr frame)
{
    switch (state_)
    {
    case INITIALIZING:
    {
        state_ = OK;
        ref_ = curr_ = frame;
        map_->insertKeyFrame(frame);
        extractKeyPoints();
        computeDescriptors();
        setRef3DPoints();
        break;
    }
    case OK:
    {
        curr_ = frame;
        extractKeyPoints();
        computeDescriptors();
        featureMatching();
        poseEstimationPnP();
        if (checkEstimatedPose())
        {
            curr_->T_c_w_ = T_c_r_estimated_ * ref_->T_c_w_; //T_cw=T_cr*T_rw
            ref_ = curr_;
            setRef3DPoints();
            num_lost_ = 0;
            if (checkKeyFrame())
                addKeyFrame();
        }
        else
        {
            num_lost_++;
            if (num_lost_ > max_num_lost_)
                state_ = LOST;
            return false;
        }
        break;
    }
    case LOST:
    {
        cout << "vo has lost" << endl;
        break;
    }
    }
    return true;
}

void VisualOdometry::extractKeyPoints()
{
    orb_->detect(curr_->color_, keypoints_curr_);
}

void VisualOdometry::computeDescriptors()
{
    orb_->compute(curr_->color_, keypoints_curr_, descriptors_curr_);
}

void VisualOdometry::featureMatching()
{
    vector<cv::DMatch> matches;
    cv::BFMatcher matcher(cv::NORM_HAMMING);
    matcher.match(descriptors_ref_, descriptors_curr_, matches);

    //定义lambda函数，从matches里，按照其中元素的cv::DMatch.distance找到最小距离的元素
    //得到它的最小距离
    float min_dis = std::min_element(
                        matches.begin(), matches.end(),
                        [](const cv::DMatch &m1, const cv::DMatch &m2) {
                            return m1.distance < m2.distance;
                        })
                        ->distance;

    feature_matches_.clear();

    // 找到距离合适的匹配
    // 距离小于30或min_dis的match_ratio倍，算匹配上
    for (cv::DMatch &m : matches)
    {
        if (m.distance < max<float>(min_dis * match_ratio_, 30.0))
        {
            feature_matches_.push_back(m);
        }
    }
    cout << "good matches:" << feature_matches_.size() << endl;
}

void VisualOdometry::setRef3DPoints()
{
    pts_3d_ref_.clear();
    descriptors_ref_ = Mat();
    //调用它的时候，已经把curr_赋给ref_了，所以此时res_实际上是curr_。
    // 找当前帧所有关键点，看能不能有深度，有的话计算相机坐标系的三维坐标，
    // 并且把由深度的描述子保留给ref_。
    // 但其实，把ref和curr统一是不是更好？
    for (size_t i = 0; i < keypoints_curr_.size(); i++)
    {
        double d = ref_->findDepth(keypoints_curr_[i]);
        if (d > 0)
        {
            //cout<<keypoints_curr_[i].pt.x<<' '<<keypoints_curr_[i].pt.y<<' '<<d<<' ';
            Vector3d p_cam=ref_->camera_->pixel2camera(
                Vector2d(keypoints_curr_[i].pt.x,keypoints_curr_[i].pt.y),d);
            pts_3d_ref_.push_back(cv::Point3f(p_cam(0, 0), p_cam(1, 0), p_cam(2, 0)));
            //cout<<p_cam(0, 0)<<' '<<p_cam(1, 0)<<' '<<p_cam(2, 0)<<endl;
            descriptors_ref_.push_back(descriptors_curr_.row(i));
        }
    }
}

void VisualOdometry::poseEstimationPnP()
{
    vector<cv::Point3f> pts3d;
    vector<cv::Point2f> pts2d;
    // query是要匹配的描述子，train是被匹配的描述子
    for (cv::DMatch m : feature_matches_)
    {
        pts3d.push_back(pts_3d_ref_[m.queryIdx]);
        pts2d.push_back(keypoints_curr_[m.trainIdx].pt);
    }
    // 内参
    Mat K = (cv::Mat_<double>(3, 3) << ref_->camera_->fx_, 0, ref_->camera_->cx_,
             0, ref_->camera_->fy_, ref_->camera_->cy_,
             0, 0, 1);
    //输出
    // rvec ，tvec 估计的旋转矩阵和平移矩阵
    Mat rvec, tvec, inliers;
    cv::solvePnPRansac(pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers);
    num_inliers_ = inliers.rows;
    cout << "pnp inliers:" << num_inliers_ << endl;
    T_c_r_estimated_ = SE3(
        SO3(rvec.at<double>(0,0), rvec.at<double>(1,0), rvec.at<double>(2,0)),
        Vector3d( tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0))
    );
    // 使用bundle adjestment优化位姿
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;
    Block::LinearSolverType* linearSolver=new g2o::LinearSolverDense<Block::PoseMatrixType>();
    Block* solver_ptr=new Block(linearSolver);
    g2o::OptimizationAlgorithmLevenberg* solver=new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

    // 优化变量，一个顶点，位姿
    g2o::VertexSE3Expmap* pose=new g2o::VertexSE3Expmap();
    pose->setId(0);
    // 设定初值
    pose->setEstimate(g2o::SE3Quat(
        T_c_r_estimated_.rotation_matrix(),
        T_c_r_estimated_.translation()
    ));
    optimizer.addVertex(pose);

    // 边
    for(int i=0;i<inliers.rows;i++)
    {
        // 对每一个内点
        int index=inliers.at<int>(i,0);
        EdgeProjectXYZ2UVPoseOnly* edge=new EdgeProjectXYZ2UVPoseOnly();
        edge->setId(i);
        // 一元边，只连接位姿顶点
        edge->setVertex(0,pose);
        edge->camera_=curr_->camera_.get();
        // 三维点坐标，把它重投影和下面的measurement计算误差
        edge->point_=Vector3d(pts3d[index].x,pts3d[index].y,pts3d[index].z);
        // 观测到的值
        edge->setMeasurement(Vector2d(pts2d[index].x,pts2d[index].y));
        edge->setInformation(Eigen::Matrix2d::Identity());
        optimizer.addEdge(edge);
    }

    optimizer.initializeOptimization();
    optimizer.optimize(10);

    T_c_r_estimated_=SE3(
        pose->estimate().rotation(),
        pose->estimate().translation()
    );
}

bool VisualOdometry::checkEstimatedPose()
{
    if (num_inliers_ < min_inliers_)
    {
        cout << "reject because inlier is too small:" << num_inliers_ << endl;
        return false;
    }
    Sophus::Vector6d d = T_c_r_estimated_.log();
    if (d.norm() > 5.0)
    {
        cout << "reject because motion is too large:" << d.norm() << endl;
        return false;
    }
    return true;
}

bool VisualOdometry::checkKeyFrame()
{
    Sophus::Vector6d d = T_c_r_estimated_.log();
    Vector3d trans = d.head<3>();
    Vector3d rot = d.tail<3>();
    if (rot.norm() > key_frame_min_rot || trans.norm() > key_frame_min_trans)
        return true;
    return false;
}

void VisualOdometry::addKeyFrame()
{
    cout << "adding a key frame" << endl;
    map_->insertKeyFrame(curr_);
}

} // namespace myslam