text:
//#ifdef _MPI
//#include "mpi_csr_comm.h"
//#include "mpi_csr_matrix.h"
//#endif  // #ifdef _MPI

#ifdef _MPI_MY
#include "mpi_mesh_grdecl.h"
#endif

//#include "bs_import_common.h"

#include "jacobian.h"
#include "calc_model.h"

//#include "py_mpi_vector.h"
//#include "py_mpi_csr_matrix.h"

#include "event_manager.h"
#include "py_event_manager.h"

#include "reservoir_simulator.h"
#include "py_reservoir_simulator.h"

#include "py_calc_model.h"

#include "data_storage_interface.h"
#include "py_data_storage_interface.h"

#include "calc_well.h"
#include "py_calc_well.h"
#include "facility_manager.h"

#include "well_controller.h"
#include "well_limit_operation.h"

#include "calc_rho.h"
#include "calc_well_pressure.h"

#include "py_event_base.h"

#include "py_facility_manager.h"
#include "py_reservoir.h"

#include "py_jacobian.h"
#include "default_well.h"
#include "py_well_factory.h"
#include "py_default_wells.h"

#include "fi_params.h"

#include "prepare_fpu.h"

#include "explicit_model.hpp"
#include "equil_model.hpp"
#include "event_manager_keywords.hpp"
#include "equil_model_depth.h"

#include "py_equil_model.h"
//#include "well_results_storage.h"
//#include "fip_results_storage.h"

// FIXME:
//#include "csr_ilu_cfl.h"
//#include "py_csr_ilu_cfl_prec.h"

#include <boost/shared_ptr.hpp>

using namespace blue_sky;

#ifdef BSPY_EXPORTING_PLUGIN
using namespace blue_sky::python;
using namespace boost::python;
#endif

namespace blue_sky
{
  BLUE_SKY_PLUGIN_DESCRIPTOR_EXT ("bs_bos_core", "1.0.0", "Blue Sky BOS Core Plugin", "Blue Sky realization of Black Oil Simulator", "bs_bos_core")

//  struct BS_API_PLUGIN test_y;
//
//  struct BS_API_PLUGIN test_i
//  {
//    virtual void method () = 0;
//
//    virtual ~test_i () {}
//  };
//
//  struct BS_API_PLUGIN test_x : test_i
//  {
//    virtual ~test_x ()
//    {
//      printf ("~test_x\n");
//    }
//    virtual void method ()
//    {
//      printf ("test_x::method\n");
//    }
//
//    virtual void m (test_y *)
//    {
//      printf ("test_x::m\n");
//    }
//  };
//  struct BS_API_PLUGIN test_z : public test_x
//  {
//    virtual ~test_z ()
//    {
//      printf ("~test_z\n");
//    }
//    virtual void method ()
//    {
//      printf ("test_z::method\n");
//    }
//
//    virtual void m (test_y *)
//    {
//      printf ("test_z::m\n");
//    }
//  };
//
//  template <typename T>
//  struct BS_API_PLUGIN test_wrapper : T, wrapper <T>
//  {
//    ~test_wrapper ()
//    {
//      printf ("~test_wrapper\n");
//    }
//
//    test_wrapper (T *t)
//    :t_ (t)
//    {
//
//    }
//
//    void method ()
//    {
//      if (override f = this->get_override ("method"))
//        {
//          f ();
//        }
//      else
//        {
//          t_->method ();
//        }
//    }
//    void method_default ()
//    {
//      printf ("method_default\n");
//    }
//
//    void m (test_y *y)
//    {
//      if (override f = this->get_override ("m"))
//        {
//          f (*y);
//        }
//      else
//        {
//          t_->m (y);
//        }
//    }
//
//    T *t_;
//  };
//
//  template <typename T>
//  static
//  smart_ptr <test_wrapper <T>, false>
//  make_test_x_wrapper ()
//  {
//    printf ("make_test_x_wrapper\n");
//    return new test_wrapper <T> ();
//  }
//
//  template <typename T>
//  static
//  smart_ptr <test_wrapper <T>, false>
//  make_test_x_wrapper_2 (T *i)
//  {
//    printf ("make_test_x_wrapper_2\n");
//    return new test_wrapper <T> (i);
//  }
//
//  struct BS_API_PLUGIN test_y
//  {
//    virtual ~test_y ()
//    {
//      printf ("~test_y\n");
//    }
//    virtual void method (test_x *x)
//    {
//      printf ("test_y_x::method\n");
//      x->method ();
//    }
//    virtual void mz (test_z *x)
//    {
//      printf ("test_y_z::method\n");
//      x->method ();
//    }
//    virtual void mi (test_i *x)
//    {
//      printf ("test_y_i::method\n");
//      x->method ();
//    }
//    virtual void m2 (const smart_ptr <test_wrapper <test_i>, false> &x)
//    {
//      printf ("test_y::m2\n");
//      x->method ();
//    }
//
//    virtual void m3 (smart_ptr <test_wrapper <test_x>, false> &x)
//    {
//      printf ("test_y::m3\n");
//      x->method ();
//    }
//  };
//
//
//  template <typename T> inline T * get_pointer(smart_ptr <T, false> const & p)
//  {
//    return const_cast <T *> (p.get());
//  }
//
//  using namespace boost::python;
//
//  template <typename T>
//  object init__ (object py_obj)
//  {
//    printf ("init__\n");
//    object return_value = call_method<object>(py_obj.ptr(), "__cons__");
//    initialize_wrapper (py_obj.ptr (), extract <test_wrapper <T> *> (py_obj));
//    return return_value;
//  }
//
//  template <typename T>
//  object init__2 (object py_obj, T *i)
//  {
//    printf ("init__\n");
//    object return_value = call_method<object>(py_obj.ptr(), "__cons__", i);
//    initialize_wrapper (py_obj.ptr (), extract <test_wrapper <T> *> (py_obj));
//    return return_value;
//  }
//
//  void
//  py_export_test_x ()
//  {
//    class_ <test_i, boost::noncopyable> ("test_i", no_init)
//      ;
//
//    class_ <test_x/*, test_wrapper <test_x>*/, bases <test_i>/*, boost::noncopyable*/> ("test_x"/*, boost::python::no_init*/)
//      .def ("method", &test_x::method)//, &test_wrapper<test_x>::method_default)
//      .def ("m", &test_x::m)//, &test_wrapper<test_x>::method_default)
//      //.def ("__cons__", make_constructor (make_test_x_wrapper <test_x>))
//      //.def ("__init__", init__ <test_x>)
//      ;
//    class_ <test_z/*, test_wrapper <test_z>*/, bases </*test_wrapper <*/test_x/*>*/ >/*, boost::noncopyable*/> ("test_z"/*, boost::python::no_init*/)
//      //.def ("__cons__", make_constructor (make_test_x_wrapper <test_z>))
//      //.def ("__init__", init__ <test_z>)
////      .def ("m", &test_z::method)//, &test_wrapper <test_z>::method_default)
//      //.def ("method", &test_z::method)//, &test_wrapper <test_z>::method_default)
//      ;
//
//    class_ <test_wrapper <test_x>, bases <test_i>, boost::noncopyable> ("test_x_wrapper", boost::python::no_init)
//      .def ("__cons__", make_constructor (make_test_x_wrapper_2 <test_x>))
//      .def ("__init__", init__2 <test_x>)
//      .def ("method", &test_wrapper <test_x>::method)//, &test_wrapper <test_z>::method_default)
//      .def ("m", &test_wrapper <test_x>::m)//, &test_wrapper <test_z>::method_default)
//      ;
//
//    class_ <test_wrapper <test_z>, bases <test_x>, boost::noncopyable> ("test_z_wrapper", boost::python::no_init)
//      .def ("__cons__", make_constructor (make_test_x_wrapper_2 <test_z>))
//      .def ("__init__", init__2 <test_z>)
//      .def ("method", &test_wrapper <test_z>::method)//, &test_wrapper <test_z>::method_default)
//      .def ("m", &test_wrapper <test_z>::m)//, &test_wrapper <test_z>::method_default)
//      ;
//
//    class_ <test_y, boost::noncopyable> ("test_y")
//      .def ("method", &test_y::method)
//      //.def ("method", &test_y::mz)
//      .def ("mz", &test_y::mz)
//      .def ("mi", &test_y::mi)
//      .def ("m2", &test_y::m2)
//      .def ("m3", &test_y::m3)
//      ;
//
//    typedef smart_ptr <test_x, false> test_x_t;
//    typedef smart_ptr <test_wrapper <test_x>, false> test_x_wrapper_t;
//    register_ptr_to_python<test_x_t>();
//    register_ptr_to_python<test_x_wrapper_t>();
//
//    typedef smart_ptr <test_z, false> test_z_t;
//    typedef smart_ptr <test_wrapper <test_z>, false> test_z_wrapper_t;
//    register_ptr_to_python<test_z_t>();
//    register_ptr_to_python<test_z_wrapper_t>();
//
//    register_ptr_to_python<test_y *>();
//    register_ptr_to_python<test_i *>();
//    register_ptr_to_python<smart_ptr <test_i, false> >();
//  }

  namespace {
  bool
  register_types (const plugin_descriptor &pd)
  {
    bool res = true;

    //////////////////////////////Events/////////////////////////////////////////////
    res &= event_base_register_types (pd);
    res &= BS_KERNEL.register_type (pd, event_manager::bs_type ()); BS_ASSERT (res);

    //////////////////////////////Linear Solver///////////////////////////////
// FIXME:
//#ifdef BS_BOS_CORE_USE_CSR_ILU_CFL_PREC
//    res &= blue_sky::give_kernel::Instance().register_type(*bs_init.pd_, csr_ilu_cfl_prec<base_strategy_fi>::bs_type());
//    BS_ASSERT (res);
//    res &= blue_sky::give_kernel::Instance().register_type(*bs_init.pd_, csr_ilu_cfl_prec<base_strategy_di>::bs_type());
//    BS_ASSERT (res);
//    res &= blue_sky::give_kernel::Instance().register_type(*bs_init.pd_, csr_ilu_cfl_prec<base_strategy_mixi>::bs_type());
//    BS_ASSERT (res);
//#endif

//#ifdef _MPI
//    res &= blue_sky::mpi_csr_matrix_register_type (pd);
//    BS_ASSERT (res);
//    res &= blue_sky::mpi_csr_comm_register_type (pd);
//    BS_ASSERT (res);
//#endif

    //res &= BS_KERNEL.register_type (pd, well_results_storage::bs_type ()); BS_ASSERT (res);
    //res &= BS_KERNEL.register_type (pd, fip_results_storage::bs_type ()); BS_ASSERT (res);

    res &= calc_rho_register_types (pd);            BS_ASSERT (res);
    res &= calc_well_pressure_register_types (pd);  BS_ASSERT (res);
    res &= calc_well_register_types (pd);           BS_ASSERT (res);
    res &= wells::default_well_register_types (pd); BS_ASSERT (res);

    res &= facility_manager_register_type (pd);
    BS_ASSERT (res);
    res &= well_factory_register_type (pd);
    BS_ASSERT (res);
    res &= wells::well_controller_factory_register_type (pd);
    BS_ASSERT (res);
    res &= wells::well_limit_operation_factory_register_type (pd);
    BS_ASSERT (res);

	#ifdef BSPY_EXPORTING_PLUGIN
    res &= data_storage_proxy_register_type (pd);
    BS_ASSERT (res);
	#endif

    res &= data_storage_register_type (pd);
    BS_ASSERT (res);
    res &= data_serializer_register_type (pd);
    BS_ASSERT (res);
    res &= data_storage_interface_register_type (pd);
    BS_ASSERT (res);

    res &= reservoir_simulator_register_types (pd);
    BS_ASSERT (res);

    res &= BS_KERNEL.register_type (pd, explicit_model::bs_type ()); BS_ASSERT (res);
    res &= BS_KERNEL.register_type (pd, equil_model::bs_type ()); BS_ASSERT (res);

    res &= BS_KERNEL.register_type (pd, event_manager_keywords::bs_type ()); BS_ASSERT (res);

    res &= BS_KERNEL.register_type (pd, equil_model_depth::bs_type ()); BS_ASSERT (res);

    // force serialization typeinfo registering
    //serialize_register_eti< equil_model_depth >();

    return res;
  }
  }

  BLUE_SKY_REGISTER_PLUGIN_FUN
  {

    //bool res = true;
    return register_types (*bs_init.pd_);

  }
}//bs
//
//namespace blue_sky {
//  template <typename bcsr_d, typename bcsr_f>
//  void
//  convert_double_to_float (bcsr_d *bcsr_mx_d, bcsr_f *bcsr_mx_f)
//  {
//    //typedef typename mx_t::i_type_t i_type;
//    //typedef typename mx_t::fp_type_t fp_type;
//
//    typedef typename bcsr_d::index_t index_t;
//    typedef typename bcsr_d::item_t item_t;
//    typedef typename bcsr_d::item_array_t item_array_t;
//    typedef typename bcsr_d::index_array_t index_array_t;
//
//    //typedef typename bcsr_d::index_t index_t;
//    typedef typename bcsr_f::item_t rhs_item_t;
//
//    const index_t *rows_d = &bcsr_mx_d->get_rows_ptr ()[0];
//    const index_t *cols_d = &bcsr_mx_d->get_cols_ind ()[0];
//    const item_t *val_d   = &bcsr_mx_d->get_values ()[0];
//
//    index_t *rows_f = &bcsr_mx_f->get_rows_ptr ()[0];
//    index_t *cols_f = &bcsr_mx_f->get_cols_ind ()[0];
//    rhs_item_t *val_f   = &bcsr_mx_f->get_values ()[0];
//
//    typedef typename bcsr_d::i_type_t i_type;
//
//    bcsr_mx_f->alloc_rows_ptr (bcsr_mx_d->n_rows, 0);
//    bcsr_mx_f->alloc_cols_ind_and_values (bcsr_mx_d->get_n_non_zeros ());
//
//    bcsr_mx_f->get_rows_ptr ().assign (bcsr_mx_d->get_rows_ptr ().begin (), bcsr_mx_d->get_rows_ptr ().end ());
//    bcsr_mx_f->get_diag_ind ().assign (bcsr_mx_d->get_diag_ind ().begin (), bcsr_mx_d->get_diag_ind ().end ());
//    bcsr_mx_f->get_cols_ind ().assign (bcsr_mx_d->get_cols_ind ().begin (), bcsr_mx_d->get_cols_ind ().end ());
//    bcsr_mx_f->get_values ().assign (bcsr_mx_d->get_values ().begin (), bcsr_mx_d->get_values ().end ());
//
//    bcsr_mx_f->n_rows = bcsr_mx_d->n_rows;
//    bcsr_mx_f->n_cols = bcsr_mx_d->n_cols;
//    bcsr_mx_f->n_block_size = bcsr_mx_d->n_block_size;
//
//    index_array_t tmp_rows_ptr, tmp_diag_ind, tmp_cols_ind;
//    item_array_t tmp_values;
//    std::swap (bcsr_mx_d->get_rows_ptr (), tmp_rows_ptr);
//    std::swap (bcsr_mx_d->get_diag_ind (), tmp_diag_ind);
//    std::swap (bcsr_mx_d->get_cols_ind (), tmp_cols_ind);
//    std::swap (bcsr_mx_d->get_values (), tmp_values);
//  }
//
//  template <typename py_bcsr_d, typename py_bcsr_f>
//  void
//  py_convert_double_to_float (py_bcsr_d &bcsr_d, py_bcsr_f &bcsr_f)
//  {
//    convert_double_to_float (&(*bcsr_d.template get_lspx <typename py_bcsr_d::matrix_t> ()),
//      &(*bcsr_f.template get_lspx <typename py_bcsr_f::matrix_t> ()));
//  }
//}

#ifdef BSPY_EXPORTING_PLUGIN
namespace {
  void
  init_py_subsystem ()
  {
    using namespace boost::python;

  //#ifdef _MPI
  //  python::py_export_mpi_vector ();
  //  python::py_export_mpi_csr_matrix ();
  //#endif

	python::py_export_equil_model();

	py_export_fi_params ();

    py_export_events ();
    py_export_event_manager();

    python::py_export_reservoir_simulator ();
    python::py_export_calc_model ();
    // we should export enum only for one instance of reservoir simulator
    //reservoir_simulator<base_strategy_di>::py_export_signals_enum (); //!TODO:

    python::py_export_calc_well ();

    python::py_export_data_storage_interface ();

    python::py_export_jacobian ();

    python::py_export_facility_manager ();
    python::py_export_reservoir ();

    python::py_export_well_factories ();

  // FIXME:
  //#ifdef BS_BOS_CORE_USE_CSR_ILU_CFL_PREC
  //  python::py_export_csr_ilu_cfl_prec ();
  //#endif

    python::py_export_default_wells ();

    def ("enable_fpu_exceptions", blue_sky::tools::prepare_fpu::enable_exceptions);

    //def ("convert_double_to_float", py_convert_double_to_float <python::py_bcsr_matrix <base_strategy_di::item_array_t, base_strategy_di::index_array_t>, python::py_bcsr_matrix <base_strategy_fi::item_array_t, base_strategy_fi::index_array_t> >);

    //py_export_test_x ();
  }
}
BLUE_SKY_INIT_PY_FUN
{
  init_py_subsystem ();
}
#ifdef _DEBUG
BOOST_PYTHON_MODULE (bs_bos_core_d)
#else
BOOST_PYTHON_MODULE (bs_bos_core)
#endif
{
  init_py_subsystem ();
  std::cout << &BS_KERNEL << std::endl;
  bool res = blue_sky::register_types (*blue_sky::bs_get_plugin_descriptor ());
  if (!res)
    throw "Can't register bs_bos_core types";
}

#endif //BSPY_EXPORT_PLUGIN

max_stars_repo_name: bs-eagle/bs-eagle

max_issues_repo_name: bs-eagle/bs-eagle

max_forks_repo_name: bs-eagle/bs-eagle


text:
/* Copyright 2015 Google Inc. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

// Computing label Op

#include <stdio.h>
#include <cfloat>
#include <math.h>
#include <time.h>

#include "types.h"
#include "sampler2D.h"
#include "ransac.h"
#include "Hypothesis.h"
#include "detection.h"
#include <Eigen/Geometry>

#include "third_party/eigen3/unsupported/Eigen/CXX11/Tensor"
#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor_shape.h"

using namespace tensorflow;
typedef Eigen::ThreadPoolDevice CPUDevice;

REGISTER_OP("Houghvoting")
    .Attr("T: {float, double}")
    .Attr("is_train: int")
    .Input("bottom_label: int32")
    .Input("bottom_vertex: T")
    .Input("bottom_extents: T")
    .Input("bottom_meta_data: T")
    .Input("bottom_gt: T")
    .Output("top_box: T")
    .Output("top_pose: T")
    .Output("top_target: T")
    .Output("top_weight: T");

REGISTER_OP("HoughvotingGrad")
    .Attr("T: {float, double}")
    .Input("bottom_label: int32")
    .Input("bottom_vertex: T")
    .Input("grad: T")
    .Output("output_label: T")
    .Output("output_vertex: T");

/**
 * @brief Data used in NLOpt callback loop.
 */
struct DataForOpt
{
  int imageWidth;
  int imageHeight;
  float rx, ry;
  cv::Rect bb2D;
  std::vector<cv::Point3f> bb3D;
  cv::Mat_<float> camMat;
};

void getLabels(const int* label_map, std::vector<std::vector<int>>& labels, std::vector<int>& object_ids, int width, int height, int num_classes, int minArea);
void getBb3Ds(const float* extents, std::vector<std::vector<cv::Point3f>>& bb3Ds, int num_classes);
inline bool samplePoint2D(jp::id_t objID, std::vector<cv::Point2f>& eyePts, std::vector<cv::Point2f>& objPts, std::vector<float>& distances, const cv::Point2f& pt2D, const float* vertmap, int width, int num_classes);
std::vector<TransHyp*> getWorkingQueue(std::map<jp::id_t, std::vector<TransHyp>>& hypMap, int maxIt);
inline float point2line(cv::Point2d x, cv::Point2f n, cv::Point2f p);
inline void countInliers2D(TransHyp& hyp, const float * vertmap, const std::vector<std::vector<int>>& labels, float inlierThreshold, int width, int num_classes, int pixelBatch);
inline void updateHyp2D(TransHyp& hyp, int maxPixels);
inline void filterInliers2D(TransHyp& hyp, int maxInliers);
inline cv::Point2f getMode2D(jp::id_t objID, const cv::Point2f& pt, const float* vertmap, float & distance, int width, int num_classes);
static double optEnergy(const std::vector<double> &pose, std::vector<double> &grad, void *data);
double poseWithOpt(std::vector<double> & vec, DataForOpt data, int iterations);
void estimateCenter(const int* labelmap, const float* vertmap, std::vector<std::vector<cv::Point3f>> bb3Ds, int batch, int height, int width, int num_classes, int is_train,
  float fx, float fy, float px, float py, std::vector<cv::Vec<float, 13> >& outputs);
void compute_target_weight(int height, int width, float* target, float* weight, std::vector<std::vector<cv::Point3f>> bb3Ds, const float* poses_gt, int num_gt, int num_classes, float fx, float fy, float px, float py, std::vector<cv::Vec<float, 13> > outputs);
inline void compute_width_height(TransHyp& hyp, const float* vertmap, const std::vector<std::vector<int>>& labels, float inlierThreshold, int width, int num_classes);

template <typename Device, typename T>
class HoughvotingOp : public OpKernel {
 public:
  explicit HoughvotingOp(OpKernelConstruction* context) : OpKernel(context) {
    // Get the pool height
    OP_REQUIRES_OK(context,
                   context->GetAttr("is_train", &is_train_));
    // Check that pooled_height is positive
    OP_REQUIRES(context, is_train_ >= 0,
                errors::InvalidArgument("Need is_train >= 0, got ",
                                        is_train_));
  }

  // bottom_label: (batch_size, height, width)
  // bottom_vertex: (batch_size, height, width, 3 * num_classes)
  // top_box: (num, 6) i.e., batch_index, cls, x1, y1, x2, y2
  void Compute(OpKernelContext* context) override
  {
    // Grab the input tensor
    const Tensor& bottom_label = context->input(0);
    const Tensor& bottom_vertex = context->input(1);
    const Tensor& bottom_extents = context->input(2);

    // format of the meta_data
    // intrinsic matrix: meta_data[0 ~ 8]
    // inverse intrinsic matrix: meta_data[9 ~ 17]
    // pose_world2live: meta_data[18 ~ 29]
    // pose_live2world: meta_data[30 ~ 41]
    // voxel step size: meta_data[42, 43, 44]
    // voxel min value: meta_data[45, 46, 47]
    const Tensor& bottom_meta_data = context->input(3);
    auto meta_data = bottom_meta_data.flat<T>();

    const Tensor& bottom_gt = context->input(4);
    const float* gt = bottom_gt.flat<float>().data();

    // data should have 5 dimensions.
    OP_REQUIRES(context, bottom_label.dims() == 3,
                errors::InvalidArgument("label must be 3-dimensional"));

    OP_REQUIRES(context, bottom_vertex.dims() == 4,
                errors::InvalidArgument("vertex must be 4-dimensional"));

    // batch size
    int batch_size = bottom_label.dim_size(0);
    // height
    int height = bottom_label.dim_size(1);
    // width
    int width = bottom_label.dim_size(2);
    // num of classes
    int num_classes = bottom_vertex.dim_size(3) / VERTEX_CHANNELS;
    int num_meta_data = bottom_meta_data.dim_size(3);
    int num_gt = bottom_gt.dim_size(0);

    // for each image, run hough voting
    std::vector<cv::Vec<float, 13> > outputs;
    const float* extents = bottom_extents.flat<float>().data();

    // bb3Ds
    std::vector<std::vector<cv::Point3f>> bb3Ds;
    getBb3Ds(extents, bb3Ds, num_classes);

    int index_meta_data = 0;
    float fx, fy, px, py;
    for (int n = 0; n < batch_size; n++)
    {
      const int* labelmap = bottom_label.flat<int>().data() + n * height * width;
      const float* vertmap = bottom_vertex.flat<float>().data() + n * height * width * VERTEX_CHANNELS * num_classes;
      fx = meta_data(index_meta_data + 0);
      fy = meta_data(index_meta_data + 4);
      px = meta_data(index_meta_data + 2);
      py = meta_data(index_meta_data + 5);

      estimateCenter(labelmap, vertmap, bb3Ds, n, height, width, num_classes, is_train_, fx, fy, px, py, outputs);

      index_meta_data += num_meta_data;
    }

    if (outputs.size() == 0)
    {
      std::cout << "no detection" << std::endl;
      // add a dummy detection to the output
      cv::Vec<float, 13> roi;
      roi(0) = 0;
      roi(1) = -1;
      roi(2) = 0;
      roi(3) = 0;
      roi(4) = 1;
      roi(5) = 1;
      roi(6) = 1;
      outputs.push_back(roi);
    }

    // Create output tensors
    // top_box
    int dims[2];
    dims[0] = outputs.size();
    dims[1] = 6;
    TensorShape output_shape;
    TensorShapeUtils::MakeShape(dims, 2, &output_shape);

    Tensor* top_box_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(0, output_shape, &top_box_tensor));
    float* top_box = top_box_tensor->template flat<float>().data();

    // top_pose
    dims[1] = 7;
    TensorShape output_shape_pose;
    TensorShapeUtils::MakeShape(dims, 2, &output_shape_pose);

    Tensor* top_pose_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(1, output_shape_pose, &top_pose_tensor));
    float* top_pose = top_pose_tensor->template flat<float>().data();

    // top target
    dims[1] = 4 * num_classes;
    TensorShape output_shape_target;
    TensorShapeUtils::MakeShape(dims, 2, &output_shape_target);

    Tensor* top_target_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(2, output_shape_target, &top_target_tensor));
    float* top_target = top_target_tensor->template flat<float>().data();
    memset(top_target, 0, outputs.size() * 4 * num_classes *sizeof(T));

    // top weight
    Tensor* top_weight_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(3, output_shape_target, &top_weight_tensor));
    float* top_weight = top_weight_tensor->template flat<float>().data();
    memset(top_weight, 0, outputs.size() * 4 * num_classes *sizeof(T));

    for(int n = 0; n < outputs.size(); n++)
    {
      cv::Vec<float, 13> roi = outputs[n];

      for (int i = 0; i < 6; i++)
        top_box[n * 6 + i] = roi(i);

      for (int i = 0; i < 7; i++)
        top_pose[n * 7 + i] = roi(6 + i);
    }

    if (is_train_)
      compute_target_weight(height, width, top_target, top_weight, bb3Ds, gt, num_gt, num_classes, fx, fy, px, py, outputs);
  }
 private:
  int is_train_;
};

REGISTER_KERNEL_BUILDER(Name("Houghvoting").Device(DEVICE_CPU).TypeConstraint<float>("T"), HoughvotingOp<CPUDevice, float>);

// compute gradient
template <class Device, class T>
class HoughvotingGradOp : public OpKernel {
 public:
  explicit HoughvotingGradOp(OpKernelConstruction* context) : OpKernel(context) {
  }

  void Compute(OpKernelContext* context) override
  {
    // Grab the input tensor
    const Tensor& bottom_label = context->input(0);
    const Tensor& bottom_vertex = context->input(1);

    // data should have 5 dimensions.
    OP_REQUIRES(context, bottom_label.dims() == 3,
                errors::InvalidArgument("label must be 3-dimensional"));

    OP_REQUIRES(context, bottom_vertex.dims() == 4,
                errors::InvalidArgument("vertex must be 4-dimensional"));

    // batch size
    int batch_size = bottom_label.dim_size(0);
    // height
    int height = bottom_label.dim_size(1);
    // width
    int width = bottom_label.dim_size(2);
    // num of classes
    int num_classes = bottom_vertex.dim_size(3) / VERTEX_CHANNELS;

    // construct the output shape
    TensorShape output_shape = bottom_label.shape();
    Tensor* top_label_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(0, output_shape, &top_label_tensor));
    T* top_label = top_label_tensor->template flat<T>().data();
    memset(top_label, 0, batch_size * height * width * sizeof(T));

    TensorShape output_shape_1 = bottom_vertex.shape();
    Tensor* top_vertex_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(1, output_shape_1, &top_vertex_tensor));
    T* top_vertex = top_vertex_tensor->template flat<T>().data();
    memset(top_vertex, 0, batch_size * height * width * 2 * num_classes *sizeof(T));
  }
};

REGISTER_KERNEL_BUILDER(Name("HoughvotingGrad").Device(DEVICE_CPU).TypeConstraint<float>("T"), HoughvotingGradOp<CPUDevice, float>);

// get label lists
void getLabels(const int* label_map, std::vector<std::vector<int>>& labels, std::vector<int>& object_ids, int width, int height, int num_classes, int minArea)
{
  for(int i = 0; i < num_classes; i++)
    labels.push_back( std::vector<int>() );

  // for each pixel
  #pragma omp parallel for
  for(int x = 0; x < width; x++)
  for(int y = 0; y < height; y++)
  {
    int label = label_map[y * width + x];
    labels[label].push_back(y * width + x);
  }

  for(int i = 1; i < num_classes; i++)
  {
    if (labels[i].size() > minArea)
    {
      object_ids.push_back(i);
    }
  }
}

// get 3D bounding boxes
void getBb3Ds(const float* extents, std::vector<std::vector<cv::Point3f>>& bb3Ds, int num_classes)
{
  // for each object
  for (int i = 1; i < num_classes; i++)
  {
    cv::Vec<float, 3> extent;
    extent(0) = extents[i * 3];
    extent(1) = extents[i * 3 + 1];
    extent(2) = extents[i * 3 + 2];

    bb3Ds.push_back(getBB3D(extent));
  }
}

inline cv::Point2f getMode2D(jp::id_t objID, const cv::Point2f& pt, const float* vertmap, float & distance, int width, int num_classes)
{
  int channel = VERTEX_CHANNELS * objID;
  int offset = channel + VERTEX_CHANNELS * num_classes * (pt.y * width + pt.x);

  jp::coord2_t mode;
  mode(0) = vertmap[offset];
  mode(1) = vertmap[offset + 1];
  distance = vertmap[offset + 2];

  return cv::Point2f(mode(0), mode(1));
}

inline bool samplePoint2D(jp::id_t objID, std::vector<cv::Point2f>& eyePts, std::vector<cv::Point2f>& objPts, std::vector<float>& distances, const cv::Point2f& pt2D, const float* vertmap, int width, int num_classes)
{
  float distance;
  cv::Point2f obj = getMode2D(objID, pt2D, vertmap, distance, width, num_classes); // read out object coordinate

  eyePts.push_back(pt2D);
  objPts.push_back(obj);
  distances.push_back(distance);

  if (distance < 0)
    return false;
  else
    return true;
}

/**
 * @brief Creates a list of pose hypothesis (potentially belonging to multiple objects) which still have to be processed (e.g. refined).
 *
 * The method includes all remaining hypotheses of an object if there is still more than one, or if there is only one remaining but it still needs to be refined.
 *
 * @param hypMap Map of object ID to a list of hypotheses for that object.
 * @param maxIt Each hypotheses should be at least this often refined.
 * @return std::vector< Ransac3D::TransHyp*, std::allocator< void > > List of hypotheses to be processed further.
*/
std::vector<TransHyp*> getWorkingQueue(std::map<jp::id_t, std::vector<TransHyp>>& hypMap, int maxIt, int is_train)
{
  std::vector<TransHyp*> workingQueue;

  if (is_train)
  {
    for(auto it = hypMap.begin(); it != hypMap.end(); it++)
    for(int h = 0; h < it->second.size(); h++)
      if(it->second[h].refSteps < maxIt)
        workingQueue.push_back(&(it->second[h]));
  }
  else
  {
    for(auto it = hypMap.begin(); it != hypMap.end(); it++)
    for(int h = 0; h < it->second.size(); h++)
      if(it->second.size() > 1 || it->second[h].refSteps < maxIt) //exclude a hypothesis if it is the only one remaining for an object and it has been refined enough already
        workingQueue.push_back(&(it->second[h]));
  }

  return workingQueue;
}

inline float point2line(cv::Point2d x, cv::Point2f n, cv::Point2f p)
{
  float n1 = -n.y;
  float n2 = n.x;
  float p1 = p.x;
  float p2 = p.y;
  float x1 = x.x;
  float x2 = x.y;

  return fabs(n1 * (x1 - p1) + n2 * (x2 - p2)) / sqrt(n1 * n1 + n2 * n2);
}

inline float angle_distance(cv::Point2f x, cv::Point2f n, cv::Point2f p)
{
  return n.dot(x - p);
}

inline void countInliers2D(TransHyp& hyp, const float * vertmap, const std::vector<std::vector<int>>& labels, float inlierThreshold, int width, int num_classes, int pixelBatch)
{
  // reset data of last RANSAC iteration
  hyp.inlierPts2D.clear();
  hyp.inliers = 0;

  hyp.effPixels = 0; // num of pixels drawn
  hyp.maxPixels += pixelBatch; // max num of pixels to be drawn

  int maxPt = labels[hyp.objID].size(); // num of pixels of this class
  float successRate = hyp.maxPixels / (float) maxPt; // probability to accept a pixel

  std::mt19937 generator;
  std::negative_binomial_distribution<int> distribution(1, successRate); // lets you skip a number of pixels until you encounter the next pixel to accept

  for(unsigned ptIdx = 0; ptIdx < maxPt;)
  {
    int index = labels[hyp.objID][ptIdx];
    cv::Point2d pt2D(index % width, index / width);

    hyp.effPixels++;

    // read out object coordinate
    float distance;
    cv::Point2d obj = getMode2D(hyp.objID, pt2D, vertmap, distance, width, num_classes);

    // inlier check
    float d = cv::norm(hyp.center - pt2D);
    if(point2line(hyp.center, obj, pt2D) < inlierThreshold && angle_distance(hyp.center, obj, pt2D) > 0 && d < std::max(hyp.bb.width, hyp.bb.height))
    {
      hyp.inlierPts2D.push_back(std::pair<cv::Point2d, cv::Point2d>(obj, pt2D)); // store object coordinate - camera coordinate correspondence
      hyp.inliers++; // keep track of the number of inliers (correspondences might be thinned out for speed later)
    }

    // advance to the next accepted pixel
    if(successRate < 1)
      ptIdx += std::max(1, distribution(generator));
    else
      ptIdx++;
  }
}

inline void compute_width_height(TransHyp& hyp, const float* vertmap, const std::vector<std::vector<int>>& labels, float inlierThreshold, int width, int num_classes)
{
  float w = -1;
  float h = -1;
  int maxPt = labels[hyp.objID].size(); // num of pixels of this class

  for(unsigned ptIdx = 0; ptIdx < maxPt; ptIdx++)
  {
    int index = labels[hyp.objID][ptIdx];
    cv::Point2d pt2D(index % width, index / width);

    // read out object coordinate
    float distance;
    cv::Point2d obj = getMode2D(hyp.objID, pt2D, vertmap, distance, width, num_classes);

    // inlier check
    float d = cv::norm(hyp.center - pt2D);
    if(point2line(hyp.center, obj, pt2D) < inlierThreshold && angle_distance(hyp.center, obj, pt2D) > 0 && d < std::max(hyp.bb.width, hyp.bb.height))
    {
      float x = fabs(pt2D.x - hyp.center.x);
      float y = fabs(pt2D.y - hyp.center.y);
      if (x > w)
        w = x;
      if (y > h)
        h = y;
    }
  }
  hyp.width_ = 2 * w;
  hyp.height_ = 2 * h;
}

inline void updateHyp2D(TransHyp& hyp, int maxPixels)
{
  if(hyp.inlierPts2D.size() < 4) return;
  filterInliers2D(hyp, maxPixels); // limit the number of correspondences

  // data conversion
  cv::Point2d center = hyp.center;
  Hypothesis trans(center);

  // recalculate pose
  trans.calcCenter(hyp.inlierPts2D);
  hyp.center = trans.getCenter();
}

inline void filterInliers2D(TransHyp& hyp, int maxInliers)
{
  if(hyp.inlierPts2D.size() < maxInliers) return; // maximum number not reached, do nothing

  std::vector<std::pair<cv::Point2d, cv::Point2d>> inlierPts; // filtered list of inlier correspondences

  // select random correspondences to keep
  for(unsigned i = 0; i < maxInliers; i++)
  {
    int idx = irand(0, hyp.inlierPts2D.size());

    inlierPts.push_back(hyp.inlierPts2D[idx]);
  }

  hyp.inlierPts2D = inlierPts;
}

void estimateCenter(const int* labelmap, const float* vertmap, std::vector<std::vector<cv::Point3f>> bb3Ds, int batch, int height, int width, int num_classes, int is_train,
  float fx, float fy, float px, float py, std::vector<cv::Vec<float, 13> >& outputs)
{
  //set parameters, see documentation of GlobalProperties
  int maxIterations = 10000000;
  float minArea = 400; // a hypothesis covering less projected area (2D bounding box) can be discarded (too small to estimate anything reasonable)
  float minDist2D = 10;
  float inlierThreshold3D = 0.5;
  int ransacIterations;  // 256
  int poseIterations = 100;
  int preemptiveBatch;  // 1000
  int maxPixels;  // 1000
  int refIt;  // 8

  if (is_train)
  {
    ransacIterations = 256;
    preemptiveBatch = 100;
    maxPixels = 1000;
    refIt = 4;
  }
  else
  {
    ransacIterations = 256 * 1;
    preemptiveBatch = 100 * 1;
    maxPixels = 1000 * 1;
    refIt = 8;
  }

  // labels
  std::vector<std::vector<int>> labels;
  std::vector<int> object_ids;
  getLabels(labelmap, labels, object_ids, width, height, num_classes, minArea);

  // camera matrix
  cv::Mat_<float> camMat = cv::Mat_<float>::zeros(3, 3);
  camMat(0, 0) = fx;
  camMat(1, 1) = fy;
  camMat(2, 2) = 1.f;
  camMat(0, 2) = px;
  camMat(1, 2) = py;

  if (object_ids.size() == 0)
    return;

  int imageWidth = width;
  int imageHeight = height;

  // hold for each object a list of pose hypothesis, these are optimized until only one remains per object
  std::map<jp::id_t, std::vector<TransHyp>> hypMap;

  // sample initial pose hypotheses
  #pragma omp parallel for
  for(unsigned h = 0; h < ransacIterations; h++)
  for(unsigned i = 0; i < maxIterations; i++)
  {
    // camera coordinate - object coordinate correspondences
    std::vector<cv::Point2f> eyePts;
    std::vector<cv::Point2f> objPts;
    std::vector<float> distances;

    // sample first point and choose object ID
    jp::id_t objID = object_ids[irand(0, object_ids.size())];

    if(objID == 0) continue;

    int pindex = irand(0, labels[objID].size());
    int index = labels[objID][pindex];
    cv::Point2f pt1(index % width, index / width);

    // sample first correspondence
    if(!samplePoint2D(objID, eyePts, objPts, distances, pt1, vertmap, width, num_classes))
      continue;

    // sample other points in search radius, discard hypothesis if minimum distance constrains are violated
    pindex = irand(0, labels[objID].size());
    index = labels[objID][pindex];
    cv::Point2f pt2(index % width, index / width);

    if (cv::norm(pt1 - pt2) < minDist2D)
      continue;

    if(!samplePoint2D(objID, eyePts, objPts, distances, pt2, vertmap, width, num_classes))
      continue;

    // reconstruct
    std::vector<std::pair<cv::Point2d, cv::Point2d>> pts2D;
    float distance = 0;
    for(unsigned j = 0; j < eyePts.size(); j++)
    {
      pts2D.push_back(std::pair<cv::Point2d, cv::Point2d>(
      cv::Point2d(objPts[j].x, objPts[j].y),
      cv::Point2d(eyePts[j].x, eyePts[j].y)
      ));
      distance += distances[j];
    }
    distance /= distances.size();

    Hypothesis trans(pts2D);

    // center
    cv::Point2d center = trans.getCenter();
    int x = int(center.x);
    int y = int(center.y);
    if (num_classes > 2 && x >= 0 && x < width && y >= 0 && y < height)
    {
      if (labelmap[y * width + x] == 0)
        continue;
    }

    // create a hypothesis object to store meta data
    TransHyp hyp(objID, center);

    // estimate a projection
    cv::Mat tvec(3, 1, CV_64F);
    cv::Mat rvec(3, 1, CV_64F);
    for(int j = 0; j < 3; j++)
    {
      tvec.at<double>(j, 0) = 0;
      rvec.at<double>(j, 0) = 0;
    }
    tvec.at<double>(2, 0) = distance;
    jp::cv_trans_t pose(rvec, tvec);

    std::vector<cv::Point2f> bb2D;
    cv::projectPoints(bb3Ds[objID-1], pose.first, pose.second, camMat, cv::Mat(), bb2D);

    // get min-max of projected vertices
    int minX = 10000000;
    int maxX = -10000000;
    int minY = 10000000;
    int maxY = -10000000;

    for(unsigned j = 0; j < bb2D.size(); j++)
    {
	minX = std::min((float) minX, bb2D[j].x);
	minY = std::min((float) minY, bb2D[j].y);
	maxX = std::max((float) maxX, bb2D[j].x);
	maxY = std::max((float) maxY, bb2D[j].y);
    }
    hyp.bb = cv::Rect(0, 0, (maxX - minX + 1), (maxY - minY + 1));

    cv::Point2f c;
    c.x = center.x;
    c.y = center.y;
    if (cv::norm(pt1 - c) > std::max(hyp.bb.width, hyp.bb.height) || cv::norm(pt2 - c) > std::max(hyp.bb.width, hyp.bb.height))
      continue;

    #pragma omp critical
    {
      hypMap[objID].push_back(hyp);
    }

    break;
  }

  // create a list of all objects where hyptheses have been found
  std::vector<jp::id_t> objList;
  for(std::pair<jp::id_t, std::vector<TransHyp>> hypPair : hypMap)
  {
    objList.push_back(hypPair.first);
  }

  // create a working queue of all hypotheses to process
  std::vector<TransHyp*> workingQueue = getWorkingQueue(hypMap, refIt, is_train);

  // main preemptive RANSAC loop, it will stop if there is max one hypothesis per object remaining which has been refined a minimal number of times
  while(!workingQueue.empty())
  {
    // draw a batch of pixels and check for inliers, the number of pixels looked at is increased in each iteration
    #pragma omp parallel for
    for(int h = 0; h < workingQueue.size(); h++)
      countInliers2D(*(workingQueue[h]), vertmap, labels, inlierThreshold3D, width, num_classes, preemptiveBatch);

    // sort hypothesis according to inlier count and discard bad half
    #pragma omp parallel for
    for(unsigned o = 0; o < objList.size(); o++)
    {
      jp::id_t objID = objList[o];
      if(hypMap[objID].size() > 1)
      {
	std::sort(hypMap[objID].begin(), hypMap[objID].end());
	hypMap[objID].erase(hypMap[objID].begin() + hypMap[objID].size() / 2, hypMap[objID].end());
      }
    }
    workingQueue = getWorkingQueue(hypMap, refIt, is_train);

    // refine
    #pragma omp parallel for
    for(int h = 0; h < workingQueue.size(); h++)
    {
      updateHyp2D(*(workingQueue[h]), maxPixels);
      workingQueue[h]->refSteps++;
    }

    workingQueue = getWorkingQueue(hypMap, refIt, is_train);
  }

  #pragma omp parallel for
  for(auto it = hypMap.begin(); it != hypMap.end(); it++)
  for(int h = 0; h < it->second.size(); h++)
  {
    cv::Vec<float, 13> roi;
    roi(0) = batch;
    roi(1) = it->second[h].objID;

    // backproject the center
    cv::Point2d center = it->second[h].center;
    float rx = (center.x - px) / fx;
    float ry = (center.y - py) / fy;
    float distance = it->second[h].compute_distance(vertmap, num_classes, width);

    // initial pose
    std::vector<double> vec(6);
    vec[0] = 0.0;
    vec[1] = 0.0;
    vec[2] = 0.0;
    vec[3] = rx * distance;
    vec[4] = ry * distance;
    vec[5] = distance;

    // convert pose to our format
    cv::Mat tvec(3, 1, CV_64F);
    cv::Mat rvec(3, 1, CV_64F);

    for(int i = 0; i < 6; i++)
    {
      if(i > 2)
        tvec.at<double>(i-3, 0) = vec[i];
      else
        rvec.at<double>(i, 0) = vec[i];
    }

    jp::cv_trans_t trans(rvec, tvec);
    jp::jp_trans_t pose = jp::cv2our(trans);

    // convert to quarternion
    cv::Mat pose_t;
    cv::transpose(pose.first, pose_t);
    Eigen::Map<Eigen::Matrix3d> eigenT( (double*)pose_t.data );
    Eigen::Quaterniond quaternion(eigenT);

    compute_width_height(it->second[h], vertmap, labels, inlierThreshold3D, width, num_classes);
    float scale = 0.05;
    roi(2) = center.x - it->second[h].width_ * (0.5 + scale);
    roi(3) = center.y - it->second[h].height_ * (0.5 + scale);
    roi(4) = center.x + it->second[h].width_ * (0.5 + scale);
    roi(5) = center.y + it->second[h].height_ * (0.5 + scale);

    roi(6) = quaternion.w();
    roi(7) = quaternion.x();
    roi(8) = quaternion.y();
    roi(9) = quaternion.z();
    roi(10) = pose.second.x;
    roi(11) = pose.second.y;
    roi(12) = pose.second.z;

    /*
    std::cout << pose.first << std::endl;
    std::cout << eigenT << std::endl;
    std::cout << quaternion.w() << " " << quaternion.x() << " " << quaternion.y() << " " << quaternion.z() << std::endl;
    std::cout << pose.second << std::endl;

    std::cout << "Inliers: " << it->second[h].inliers;
    std::printf(" (Rate: %.1f\%)\n", it->second[h].getInlierRate() * 100);
    std::cout << "Refined " << it->second[h].refSteps << " times. " << std::endl;
    std::cout << "Center " << center << std::endl;
    std::cout << "Width: " << it->second[h].width_ << " Height: " << it->second[h].height_ << std::endl;
    std::cout << "---------------------------------------------------" << std::endl;
    std::cout << roi << std::endl;
    */

    outputs.push_back(roi);

    if (is_train)
    {
      // add jittering rois
      float x1 = roi(2);
      float y1 = roi(3);
      float x2 = roi(4);
      float y2 = roi(5);
      float ww = x2 - x1;
      float hh = y2 - y1;

      // (-1, -1)
      roi(2) = x1 - 0.05 * ww;
      roi(3) = y1 - 0.05 * hh;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);

      // (+1, -1)
      roi(2) = x1 + 0.05 * ww;
      roi(3) = y1 - 0.05 * hh;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);

      // (-1, +1)
      roi(2) = x1 - 0.05 * ww;
      roi(3) = y1 + 0.05 * hh;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);

      // (+1, +1)
      roi(2) = x1 + 0.05 * ww;
      roi(3) = y1 + 0.05 * hh;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);

      // (0, -1)
      roi(2) = x1;
      roi(3) = y1 - 0.05 * hh;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);

      // (-1, 0)
      roi(2) = x1 - 0.05 * ww;
      roi(3) = y1;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);

      // (0, +1)
      roi(2) = x1;
      roi(3) = y1 + 0.05 * hh;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);

      // (+1, 0)
      roi(2) = x1 + 0.05 * ww;
      roi(3) = y1;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);
    }
  }
}

static double optEnergy(const std::vector<double> &pose, std::vector<double> &grad, void *data)
{
  DataForOpt* dataForOpt = (DataForOpt*) data;

  cv::Mat tvec(3, 1, CV_64F);
  cv::Mat rvec(3, 1, CV_64F);

  for(int i = 0; i < 6; i++)
  {
    if(i > 2)
      tvec.at<double>(i-3, 0) = pose[i];
    else
      rvec.at<double>(i, 0) = pose[i];
  }

  jp::cv_trans_t trans(rvec, tvec);

  // project the 3D bounding box according to the current pose
  cv::Rect bb2D = getBB2D(dataForOpt->imageWidth, dataForOpt->imageHeight, dataForOpt->bb3D, dataForOpt->camMat, trans);

  // compute IoU between boxes
  float energy = -1 * getIoU(bb2D, dataForOpt->bb2D);

  return energy;
}

double poseWithOpt(std::vector<double> & vec, DataForOpt data, int iterations)
{
  // set up optimization algorithm (gradient free)
  nlopt::opt opt(nlopt::LN_NELDERMEAD, 6);

  // set optimization bounds
  double rotRange = 180;
  rotRange *= PI / 180;
  double tRangeXY = 0.01;
  double tRangeZ = 0.01; // pose uncertainty is larger in Z direction

  std::vector<double> lb(6);
  lb[0] = vec[0]-rotRange; lb[1] = vec[1]-rotRange; lb[2] = vec[2]-rotRange;
  lb[3] = vec[3]-tRangeXY; lb[4] = vec[4]-tRangeXY; lb[5] = vec[5]-tRangeZ;
  opt.set_lower_bounds(lb);

  std::vector<double> ub(6);
  ub[0] = vec[0]+rotRange; ub[1] = vec[1]+rotRange; ub[2] = vec[2]+rotRange;
  ub[3] = vec[3]+tRangeXY; ub[4] = vec[4]+tRangeXY; ub[5] = vec[5]+tRangeZ;
  opt.set_upper_bounds(ub);

  // configure NLopt
  opt.set_min_objective(optEnergy, &data);
  opt.set_maxeval(iterations);

  // run optimization
  double energy;
  nlopt::result result = opt.optimize(vec, energy);

  // std::cout << "IoU after optimization: " << -energy << std::endl;

  return energy;
}

// compute the pose target and weight
void compute_target_weight(int height, int width, float* target, float* weight, std::vector<std::vector<cv::Point3f>> bb3Ds,
  const float* poses_gt, int num_gt, int num_classes, float fx, float fy, float px, float py, std::vector<cv::Vec<float, 13> > outputs)
{
  int num = outputs.size();
  float threshold = 0.2;

  // camera matrix
  cv::Mat_<float> camMat = cv::Mat_<float>::zeros(3, 3);
  camMat(0, 0) = fx;
  camMat(1, 1) = fy;
  camMat(2, 2) = 1.f;
  camMat(0, 2) = px;
  camMat(1, 2) = py;

  // compute the gt boxes
  std::vector<cv::Rect> bb2Ds_gt(num_gt);
  for (int i = 0; i < num_gt; i++)
  {
    Eigen::Quaternionf quaternion(poses_gt[i * 13 + 6], poses_gt[i * 13 + 7], poses_gt[i * 13 + 8], poses_gt[i * 13 + 9]);
    Eigen::Matrix3f rmatrix = quaternion.toRotationMatrix();
    cv::Mat rmat_trans = cv::Mat(3, 3, CV_32F, rmatrix.data());
    cv::Mat rmat;
    cv::transpose(rmat_trans, rmat);
    cv::Point3d tvec(poses_gt[i * 13 + 10], poses_gt[i * 13 + 11], poses_gt[i * 13 + 12]);
    jp::jp_trans_t pose(rmat, tvec);
    jp::cv_trans_t trans = jp::our2cv(pose);

    int objID = int(poses_gt[i * 13 + 1]);
    std::vector<cv::Point3f> bb3D = bb3Ds[objID-1];
    bb2Ds_gt[i] = getBB2D(width, height, bb3D, camMat, trans);
  }

  for (int i = 0; i < num; i++)
  {
    cv::Vec<float, 13> roi = outputs[i];
    int batch_id = int(roi(0));
    int class_id = int(roi(1));

    // find the gt index
    int gt_ind = -1;
    for (int j = 0; j < num_gt; j++)
    {
      int gt_batch = int(poses_gt[j * 13 + 0]);
      int gt_id = int(poses_gt[j * 13 + 1]);
      if(class_id == gt_id && batch_id == gt_batch)
      {
        gt_ind = j;
        break;
      }
    }

    if (gt_ind == -1)
      continue;

    // compute bounding box overlap
    float x1 = roi(2);
    float y1 = roi(3);
    float x2 = roi(4);
    float y2 = roi(5);
    cv::Rect bb2D(x1, y1, x2-x1, y2-y1);

    float overlap = getIoU(bb2D, bb2Ds_gt[gt_ind]);
    if (overlap < threshold)
      continue;

    target[i * 4 * num_classes + 4 * class_id + 0] = poses_gt[gt_ind * 13 + 6];
    target[i * 4 * num_classes + 4 * class_id + 1] = poses_gt[gt_ind * 13 + 7];
    target[i * 4 * num_classes + 4 * class_id + 2] = poses_gt[gt_ind * 13 + 8];
    target[i * 4 * num_classes + 4 * class_id + 3] = poses_gt[gt_ind * 13 + 9];

    weight[i * 4 * num_classes + 4 * class_id + 0] = 1;
    weight[i * 4 * num_classes + 4 * class_id + 1] = 1;
    weight[i * 4 * num_classes + 4 * class_id + 2] = 1;
    weight[i * 4 * num_classes + 4 * class_id + 3] = 1;

  }
}

ext: cc

max_issues_repo_name: SergioRAgostinho/PoseCNN

max_forks_repo_name: SergioRAgostinho/PoseCNN


text:
//
// Copyright Jason Rice 2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include <nbdl/apply_action.hpp>
#include <nbdl/variant.hpp>

#include <boost/hana/equal.hpp>
#include <boost/hana/tuple.hpp>
#include <catch.hpp>

namespace hana = boost::hana;

TEST_CASE("Apply action to hana::Searchable.", "[apply_action]")
{
  auto store = hana::make_tuple(5);

  nbdl::apply_action(store, hana::make_tuple(42));

  CHECK(hana::equal(hana::make_tuple(42), store));
}

ext: cpp

max_stars_repo_name: ricejasonf/nbdl

max_issues_repo_name: ricejasonf/nbdl

max_forks_repo_name: ricejasonf/nbdl


text:
// Copyright 2012 by BBN Technologies Corp.
// All Rights Reserved.
//
// Serif HTTP Server command-line interface

#include "Generic/common/leak_detection.h" // This must be the first #include
#include "Generic/common/ParamReader.h"
#include "Generic/common/OutputUtil.h"
#include "ICEWS/ICEWSDriver.h"
#include "ICEWS/Identifiers.h"
#include "ICEWS/Stories.h"
#include "ICEWS/EventMention.h"
#include "ICEWS/EventMentionSet.h"
#include "Generic/common/FileSessionLogger.h"
#include "Generic/driver/DocumentDriver.h"
#include "Generic/theories/Document.h"
#include "Generic/theories/DocTheory.h"
#include "Generic/results/SerifXMLResultCollector.h"
#include <boost/foreach.hpp>
#include <boost/scoped_ptr.hpp>

namespace ICEWS {

ICEWSDriver::ICEWSDriver():
		_save_final_serifxml_in_experiment_dir(false),
		_docDriver(_new DocumentDriver()),
		_sessionLogger(0)
{
	_save_final_serifxml_in_experiment_dir = ParamReader::isParamTrue("icews_save_final_serifxml");
	Symbol documentTextRegex(ParamReader::getParam(Symbol(L"icews_document_text_filter")));
	if (!documentTextRegex.is_null())
		_documentTextFilter.reset(_new boost::wregex(documentTextRegex.to_string()));
}

ICEWSDriver::~ICEWSDriver() {
	// Shut down the document driver, and *then* the session logger.
	_docDriver.reset();
	if (_sessionLogger) {
		SessionLogger::setGlobalLogger(0);
		delete _sessionLogger;
	}
}

void ICEWSDriver::processStories() {
	SerifXMLResultCollector resultCollector;

	SessionProgram preIcewsSessionProgram(false);
	preIcewsSessionProgram.setStageRange(Stage("START"), Stage("icews-actors").getPrevStage());

	SessionProgram icewsSessionProgram(false);
	icewsSessionProgram.setStageRange(Stage("icews-actors"), Stage(ParamReader::getRequiredParam("end_stage").c_str()));

	if (icewsSessionProgram.hasExperimentDir()) {
		wstring experimentDir(icewsSessionProgram.getExperimentDir());
		size_t index = experimentDir.find_last_of(LSERIF_PATH_SEP);
		OutputUtil::makeDir(experimentDir.substr(0, index).c_str());
		OutputUtil::makeDir(experimentDir.c_str());
		_sessionLogger = _new FileSessionLogger(icewsSessionProgram.getLogFile(), N_CONTEXTS, CONTEXT_NAMES);
		SessionLogger::setGlobalLogger(_sessionLogger);
	}
	SessionLogger::info("ICEWS") << "_________________________________________________\n";
	SessionLogger::info("ICEWS") << "Starting session: \"" << icewsSessionProgram.getSessionName() << "\"";
	SessionLogger::info("ICEWS") << "Parameters:";
	ParamReader::logParams();

	Stories::StoryIDRange stories = Stories::getStoryIds();
	BOOST_FOREACH(StoryId storyId, stories) {
		std::cerr << "Processing story " << storyId.getId() << std::endl;
		boost::scoped_ptr<Document> document;
		boost::scoped_ptr<DocTheory> docTheory;

		// Check if the serifxml for this story is cached.
		std::pair<Document*, DocTheory*> docPair = Stories::loadCachedSerifxmlForStory(storyId);
		if (docPair.first != 0) {
			document.reset(docPair.first);
			docTheory.reset(docPair.second);
			if (documentShouldBeSkipped(document.get())) continue;
		}
		// Otherwise, use the pre-icews doc driver to process it.
		else {
			document.reset(Stories::readDocument(storyId));
			if (documentShouldBeSkipped(document.get())) continue;
			docTheory.reset(_new DocTheory(document.get()));
			_docDriver->beginBatch(&preIcewsSessionProgram, 0, false);
			_docDriver->runOnDocTheory(docTheory.get());
			_docDriver->endBatch();
			Stories::saveCachedSerifxmlForStory(storyId, docTheory.get());
		}

		// Now use the icews doc driver to run the icews stages.
		_docDriver->beginBatch(&icewsSessionProgram, 0, false);
		_docDriver->runOnDocTheory(docTheory.get());
		_docDriver->endBatch();

		// Save the results to a file whose name is based on the story id.
		if (_save_final_serifxml_in_experiment_dir) {
			std::wostringstream outfile;
			outfile << L"icews.stories." << storyId.getId() << L".xml";
			resultCollector.loadDocTheory(docTheory.get());
			resultCollector.produceOutput(icewsSessionProgram.getOutputDir(),
				outfile.str().c_str());
		}
	}
	_docDriver->printTrace();

}

bool ICEWSDriver::documentShouldBeSkipped(Document *document) {
	const wchar_t* docText = document->getOriginalText()->toString();
	if (_documentTextFilter && !boost::regex_search(docText, *_documentTextFilter)) {
		SessionLogger::info("ICEWS") << "Skipping document \"" << document->getName()
			<< "\" because its text does not match icews_document_text_filter.";
		return true;
	}
	return false;
}

}

max_stars_repo_name: BBN-E/serif

max_issues_repo_name: BBN-E/serif

max_forks_repo_name: BBN-E/serif


text:
// Copyright (c) 2020 Bitcoin Association.
// Distributed under the Open BSV software license, see the accompanying file LICENSE.

#include <config.h>
#include <net/association.h>
#include <net/stream.h>
#include <test/test_bitcoin.h>

#include <boost/test/unit_test.hpp>

namespace
{
    CService ip(uint32_t i)
    {
        struct in_addr s;
        s.s_addr = i;
        return CService(CNetAddr(s), Params().GetDefaultPort());
    }

    void CheckInitialStreamStats(const StreamStats& stats)
    {
        BOOST_CHECK_EQUAL(stats.streamType, "GENERAL");
        BOOST_CHECK_EQUAL(stats.nLastSend, 0);
        BOOST_CHECK_EQUAL(stats.nLastRecv, 0);
        BOOST_CHECK_EQUAL(stats.nSendBytes, 0);
        BOOST_CHECK_EQUAL(stats.nSendSize, 0);
        BOOST_CHECK_EQUAL(stats.nRecvBytes, 0);
        BOOST_CHECK_EQUAL(stats.nMinuteBytesPerSec, 0);
        BOOST_CHECK_EQUAL(stats.nSpotBytesPerSec, 0);
    }
}

BOOST_FIXTURE_TEST_SUITE(TestNetAssociation, BasicTestingSetup)

// Basic stream testing only without a real network connection
BOOST_AUTO_TEST_CASE(TestBasicStream)
{
    // Create dummy CNode just to be abke to pass it to the CStream
    CAddress dummy_addr{ ip(0xa0b0c001), NODE_NONE };
    CConnman::CAsyncTaskPool asyncTaskPool { GlobalConfig::GetConfig() };
    CNodePtr pDummyNode =
        CNode::Make(
            0,
            NODE_NETWORK,
            0,
            INVALID_SOCKET,
            dummy_addr,
            0u,
            0u,
            asyncTaskPool,
            "",
            true);

    // Create a stream
    Stream stream { *pDummyNode, StreamType::GENERAL, INVALID_SOCKET };

    // Check initial state
    StreamStats stats {};
    stream.CopyStats(stats);
    CheckInitialStreamStats(stats);

    BOOST_CHECK_EQUAL(stream.GetSendQueueSize(), 0);
    AverageBandwidth abw { stream.GetAverageBandwidth() };
    BOOST_CHECK_EQUAL(abw.first, 0);
    BOOST_CHECK_EQUAL(abw.second, 0);

    // Update avg bandwidth calcs
    std::this_thread::sleep_for(std::chrono::seconds(1));
    stream.AvgBandwithCalc();
    abw = stream.GetAverageBandwidth();
    BOOST_CHECK_EQUAL(abw.first, 0);
    BOOST_CHECK_EQUAL(abw.second, 1);
}

// Basic association testing only without a real network connection
BOOST_AUTO_TEST_CASE(TestBasicAssociation)
{
    // Create dummy CNode just to be abke to pass it to the CStream
    CAddress dummy_addr{ ip(0xa0b0c001), NODE_NONE };
    CConnman::CAsyncTaskPool asyncTaskPool { GlobalConfig::GetConfig() };
    CNodePtr pDummyNode =
        CNode::Make(
            0,
            NODE_NETWORK,
            0,
            INVALID_SOCKET,
            dummy_addr,
            0u,
            0u,
            asyncTaskPool,
            "",
            true);

    // Create an association
    Association association { *pDummyNode, INVALID_SOCKET, dummy_addr };

    // Check initial state
    CAddress peerAddr { association.GetPeerAddr() };
    BOOST_CHECK_EQUAL(peerAddr.ToString(), "1.192.176.160:8333");

    AssociationStats stats {};
    association.CopyStats(stats);
    BOOST_CHECK_EQUAL(stats.streamStats.size(), 1);
    CheckInitialStreamStats(stats.streamStats[0]);
    BOOST_CHECK_EQUAL(stats.nLastSend, 0);
    BOOST_CHECK_EQUAL(stats.nLastRecv, 0);
    BOOST_CHECK_EQUAL(stats.addr.ToString(), "1.192.176.160:8333");
    BOOST_CHECK_EQUAL(stats.nAvgBandwidth, 0);
    BOOST_CHECK_EQUAL(stats.nSendBytes, 0);
    BOOST_CHECK_EQUAL(stats.nRecvBytes, 0);
    BOOST_CHECK_EQUAL(stats.nSendSize, 0);
    for(const auto& cmdTot : stats.mapSendBytesPerMsgCmd)
    {
        BOOST_CHECK_EQUAL(cmdTot.second, 0);
    }
    for(const auto& cmdTot : stats.mapRecvBytesPerMsgCmd)
    {
        BOOST_CHECK_EQUAL(cmdTot.second, 0);
    }

    BOOST_CHECK_EQUAL(association.GetTotalSendQueueSize(), 0);
    BOOST_CHECK_EQUAL(association.GetAverageBandwidth(), 0);
    AverageBandwidth abw { association.GetAverageBandwidth(StreamType::GENERAL) };
    BOOST_CHECK_EQUAL(abw.first, 0);
    BOOST_CHECK_EQUAL(abw.second, 0);

    // Update avg bandwidth calcs
    std::this_thread::sleep_for(std::chrono::seconds(1));
    association.AvgBandwithCalc();
    BOOST_CHECK_EQUAL(association.GetAverageBandwidth(), 0);
    abw = association.GetAverageBandwidth(StreamType::GENERAL);
    BOOST_CHECK_EQUAL(abw.first, 0);
    BOOST_CHECK_EQUAL(abw.second, 1);
}

BOOST_AUTO_TEST_SUITE_END();

max_stars_repo_name: z-btc/z-btc-main

max_issues_repo_name: z-btc/z-btc-main

max_forks_repo_name: z-btc/z-btc-main


text:
#include <navigation_features_3d/nav_features3d.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Core>
#include <ros/console.h>
#include <sys/time.h>
#include <numeric>
#include <string>

using namespace std;

nav3d::Features3D::Features3D() {
	tf_listener_ = new tf::TransformListener(ros::Duration(10));
	size_x_ = 5.0*2.0;  	//m
	size_y_ = 5.0*2.0;  	//m
	size_z_ = 5.0*2.0;  	//m
	//resolution_ = res;		//m/cell
	max_planning_dist_ = sqrt((size_x_*size_x_)+(size_y_*size_y_)+(size_z_*size_z_));

	setParams(std::string("navigation_features_3d"));
}

nav3d::Features3D::Features3D(std::string name, tf::TransformListener* tf, float size_x, float size_y, float size_z)
{
	tf_listener_ = tf;
	size_x_ = size_x*2.0;  	//m
	size_y_ = size_y*2.0;  	//m
	size_z_ = size_z*2.0;  	//m
	//resolution_ = res;		//m/cell
	max_planning_dist_ = sqrt((size_x_*size_x_)+(size_y_*size_y_)+(size_z_*size_z_));

	setParams(name);

}

nav3d::Features3D::Features3D(std::string name, tf::TransformListener* tf, vector<geometry_msgs::Point>* footprint, float size_x, float size_y, float size_z)
{

	tf_listener_ = tf;
	//robot_radius_ = insc_radius;
	size_x_ = size_x*2.0;  	//m
	size_y_ = size_y*2.0;  	//m
	size_z_ = size_z*2.0;  	//m
	//resolution_ = res;		//m/cell
	max_planning_dist_ = sqrt((size_x_*size_x_)+(size_y_*size_y_)+(size_z_*size_z_));

	myfootprint_ = footprint;

	setParams(name);

}

void nav3d::Features3D::setParams(std::string name)
{

	ROS_INFO("%s. SETTING PARAMETERS...", name.c_str());
	//Read the ROS params from the server
	ros::NodeHandle n(("~/" + name));

	name_ = name;

	//Dynamic reconfigure
	//dsrv_ = new dynamic_reconfigure::Server<navigation_features::nav_featuresConfig>(n); //ros::NodeHandle("~")
    //dynamic_reconfigure::Server<navigation_features::nav_featuresConfig>::CallbackType cb = boost::bind(&NavFeatures::reconfigureCB, this, _1, _2);
    //dsrv_->setCallback(cb);

    feature_set_ = 1;
	n.getParam("feature_set", feature_set_);

	//LOAD THE WEIGHTS OF THE COST FUNCTION FOR NAVIGATION
	w_.clear();
	w_.push_back(1.0); //weight for the valid feature (boolean [0,1])

	if(feature_set_ == 1)
	{
		unsigned int i = 1;
		while(i<=3)
		{
			char buf[10];
			sprintf(buf, "w%u", i);
			string st = string(buf);

			if(n.hasParam(st.c_str())){
				double wg = 0.0;
				n.getParam(st.c_str(), wg);
				w_.push_back((float)wg);
				printf("%s. weight %u= %.3f loaded\n", name_.c_str(), i, wg);
			}
			i++;
		}
	} else {

		bool ok = true;
		unsigned int i = 1;
		while(ok)
		{
			char buf[10];
			sprintf(buf, "w%u", i);
			string st = string(buf);

			if(n.hasParam(st.c_str())){
				double wg = 0.0;
				n.getParam(st.c_str(), wg);
				w_.push_back((float)wg);
				printf("%s. weight %u= %.3f loaded\n", name_.c_str(), i, wg);
			} else {
				//printf("param '%s' not found\n", st.c_str());
				ok = false;
			}
			i++;
		}
	}

	//LOAD THE WEIGHTS OF THE COST FUNCTION FOR EXPLORATION
	wexp_.clear();
	bool ok = true;
	unsigned int i = 1;
	while(ok)
	{
		char buf[10];
		sprintf(buf, "wexp%u", i);
		string st = string(buf);

		if(n.hasParam(st.c_str())){
			double wg = 0.0;
			n.getParam(st.c_str(), wg);
			wexp_.push_back((float)wg);
			printf("%s. exploration weight %u= %.3f loaded\n", name_.c_str(), i, wg);
		} else {
			ok = false;
		}
		i++;
	}

	string cloud_topic;
	n.param<string>("pointcloud_topic", cloud_topic, string("/scan360/point_cloud"));
	printf("\n %s. topic subscribed: %s \n", name_.c_str(), cloud_topic.c_str());

	double pitch_max = M_PI/2.0;
	double roll_max = M_PI/2.0;
	roughness_ = 0.03;
	n.getParam("max_pitch_inclination", pitch_max);
	n.getParam("max_roll_inclination", roll_max);
	n.getParam("max_roughness", roughness_);
	printf("%s. max pitch inclination:%.2f, max roll inclination:%.2f \n", name_.c_str(), pitch_max, roll_max);
	printf("%s. max roughness:%.2f\n", name_.c_str(), roughness_);
	min_points_allowed_ = 20;
	n.getParam("min_points_allowed", min_points_allowed_);

	//ros::NodeHandle nh("~");
	n.getParam("robot_base_frame", robot_base_frame_);
	n.getParam("robot_odom_frame", robot_odom_frame_);
	n.getParam("robot_odom_topic", robot_odom_topic_);

	nfe_exploration_ = false;
	n.getParam("nfe_exploration", nfe_exploration_);
	if(nfe_exploration_)
		printf("NFE EXPLORATION WILL BE EMPLOYED!!!\n");

	//pitch_low_ = -M_PI/2.0 - pitch_max;
	//pitch_low2_ = M_PI/2.0 - pitch_max;
	//pitch_high_ = -M_PI/2.0 + pitch_max;
	//pitch_high2_ = M_PI/2.0 + pitch_max;
	//roll_low_ = -M_PI/2.0 - roll_max;
	//roll_low2_ = M_PI/2.0 - roll_max;
	//roll_high_ = -M_PI/2.0 + roll_max;
	//roll_high2_ = M_PI/2.0 + roll_max;
	pitch_low_ = pitch_max;
	pitch_high_ = M_PI - pitch_max;
	roll_low_ = roll_max;
	roll_high_ = M_PI - roll_max;

	robot_radius_ = 0.25;
	n.getParam("robot_circuns_radius", robot_radius_);

	kdtree_ = new pcl::KdTreeFLANN<pcl::PointXYZ>();
	//pca_ =  new pcl::PCA<pcl::PointXYZ>();

	//octree_resolution_ = 128.0f;
	//pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> oc(128.0);
	//pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::Ptr oc(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>(octree_resolution_));
	//octree_ = oc;
	//octree_ = new pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>(octree_resolution_);
	//octree_ = new pcl::search::Octree<pcl::PointXYZ>(octree_resolution_);
	//octree_->setResolution(octree_resolution_);
	//octree_.setResolution(octree_resolution_);

	//goal_sub_ = nh_.subscribe("/rrt_goal", 1, &Features3D::goalCallback, this);

	cloud_sub_ = nh_.subscribe(cloud_topic, 1, &Features3D::cloudCallback, this);

	pose_sub_ = nh_.subscribe(robot_odom_topic_, 1, &Features3D::poseCallback, this);

	explore_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("rrt_explore_costs", 1);

	//pub_gaussian_markers_ = n.advertise<visualization_msgs::MarkerArray>("gaussian_markers", 5);

	//ros::NodeHandle nd("navigation_features");
	//ros::ServiceServer service = nd.advertiseService("setApproachingIT", setApproachingIT);
	//loss_srv_ = nd.advertiseService("set_use_loss_func", &features::NavFeatures::setLossService, this);
	//valid_srv_ = nd.advertiseService("is_pose_valid", &features::NavFeatures::isPoseValidService, this);
	//weights_srv_ = nd.advertiseService("setWeights", &features::NavFeatures::setWeightsService, this);
	//init_weights_srv_ = nd.advertiseService("initWeights", &features::NavFeatures::initializeWeightsService, this);
	//scenario_srv_ = nd.advertiseService("setScenario", &features::NavFeatures::setScenarioService, this);
	//features_srv_ = nd.advertiseService("getPathFeatureCount", &features::NavFeatures::getFeatureCountService, this);

	//Dynamic reconfigure
	//dsrv_ = new dynamic_reconfigure::Server<navigation_features::nav_featuresConfig>(n); //ros::NodeHandle("~")
    //dynamic_reconfigure::Server<navigation_features::nav_featuresConfig>::CallbackType cb = boost::bind(&NavFeatures::reconfigureCB, this, _1, _2);
    //dsrv_->setCallback(cb);

}

nav3d::Features3D::~Features3D() {
	delete kdtree_;
	//delete octree_;
}

//Point_cloud2 callback
void nav3d::Features3D::cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg){

	//Transform the coordinates of the pointcloud
	sensor_msgs::PointCloud2 local;
	if(!pcl_ros::transformPointCloud(robot_odom_frame_, *msg, local, *tf_listener_)) {
		ROS_WARN("%s. CloudCallback. TransformPointCloud failed!!!!!", name_.c_str());
	} else {

		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    	//pcl::fromROSMsg(*msg, *cloud);

    	//pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> oc(octree_resolution_);

		tree_mutex_.lock();
		pcl::fromROSMsg(local, *cloud);
		pcl_cloud_ = *cloud;
		//ros::WallTime t1 = ros::WallTime::now();
		kdtree_->setInputCloud(cloud);
		//ros::WallTime t2 = ros::WallTime::now();
		//pca_->setInputCloud(cloud);

		//ros::WallTime t3 = ros::WallTime::now();
		//octree_->deleteTree();
		//octree_->setInputCloud(cloud);
		//octree_->addPointsFromInputCloud();
		//ros::WallTime t4 = ros::WallTime::now();
		//double t_kdtree = (t2-t1).toSec();
		//double t_octree = (t4-t3).toSec();
		//printf("cloudCallback kdtree: %.4f, octree:%.4f\n", t_kdtree, t_octree);
		tree_mutex_.unlock();
	}
}

void nav3d::Features3D::poseCallback(const nav_msgs::OdometryConstPtr &msg)
{
	geometry_msgs::PoseStamped pose;

	pose.header = msg->header;
	pose.pose = msg->pose.pose;

	geometry_msgs::PoseStamped pose_out;
	//if(!pc_frame.empty())
	pose_out = transformPoseTo(pose, robot_odom_frame_, true);

	float d = 5.0;
	if(!robot_traj_.empty()){
		geometry_msgs::PoseStamped t = robot_traj_.back();
		float xt = t.pose.position.x;
		float yt = t.pose.position.y;
		float zt = t.pose.position.z;
		float xr = pose_out.pose.position.x;
		float yr = pose_out.pose.position.y;
		float zr = pose_out.pose.position.z;
		d = sqrt((xt-xr)*(xt-xr) + (yt-yr)*(yt-yr) + (zt-zr)*(zt-zr));
	}

	pose_mutex_.lock();
	robot_pose_ = pose_out;
	if(d > 0.03) {
		robot_traj_.push_back(pose_out);
		//printf("poseCallback. inserting x:%.2f, y:%.2f, z:%.2f, size: %u\n", pose_out.pose.position.x, pose_out.pose.position.y, pose_out.pose.position.z, (unsigned int)robot_traj_.size());
	}
	pose_mutex_.unlock();

}

bool nav3d::Features3D::poseValid(geometry_msgs::PoseStamped* s)
{
	std::vector<float> f = getFeatures(s);
	//for(unsigned int i=0; i<f.size(); i++)
	//	printf("feature[%u] = %.2f", i, f[i]);
	//printf("\n");
	if(f[0] == 1.0)
		return true;
	else
		return false;
}

std::vector<std::vector<int> > nav3d::Features3D::clusterize_leaves(std::vector<geometry_msgs::Point>* points, float radius)
{
	//Create the pcl pointcloud
	pcl::PointCloud<pcl::PointXYZ>::Ptr ps(new pcl::PointCloud<pcl::PointXYZ>);
	ps->width = points->size();
	ps->height = 1;
	ps->is_dense = true;
	for(unsigned int i=0; i<points->size(); i++)
	{
		pcl::PointXYZ p(points->at(i).x, points->at(i).y, points->at(i).z);
		ps->points.push_back(p);
	}
	//printf("Clusterize_leaves. pointcloud in size: %u\n", (unsigned int)ps->width);

	// Creating the KdTree object for the search method of the extraction
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud(ps);

	std::vector<pcl::PointIndices> cluster_indices;
	pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
	ec.setClusterTolerance(radius);
	ec.setMinClusterSize(2);
	ec.setMaxClusterSize(4);
	ec.setSearchMethod(tree);
	ec.setInputCloud(ps);
	ec.extract(cluster_indices);

	std::vector<std::vector<int> > clusters;

	if(cluster_indices.empty())
	{
		printf("%s. Clusterize_leaves. No clusters found!!!\n", name_.c_str());
		return clusters;
	}

	int j = 0;
	for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
	{
		std::vector<int> ind;
		//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
		for(std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) {
			//cloud_cluster->points.push_back(ps->points[*pit]);
			ind.push_back((int)*pit);
			//printf("nav_features3d. Cluster: %i, indice: %i \n", j, (int)*pit);
		}
		//cloud_cluster->width = cloud_cluster->points.size();
		//cloud_cluster->height = 1;
		//cloud_cluster->is_dense = true;
		//printf("nav_features3d. cluster %i filled\n", j);
		//std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size() << " data points." << std::endl;
		//std::stringstream ss;
		//ss << "cloud_cluster_" << j << ".pcd";
		//writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*

		clusters.push_back(ind);
		j++;
	}

	//printf("nav_features3d. returning %u clusters \n", (unsigned int)clusters.size());
	return clusters;

}

float nav3d::Features3D::evaluate_leaf(geometry_msgs::Point* p, float rrt_cost, std::string frame, float radius)
{
	geometry_msgs::PoseStamped ps;
	ps.header.frame_id = frame;
	ps.header.stamp = ros::Time::now();
	ps.pose.position = *p;
	ps.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
	geometry_msgs::PoseStamped st = transformPoseTo(ps, robot_odom_frame_, false);

	if(nfe_exploration_)
	{
		float max_dist = 5.0*5.0;
		float dcost = no_return_cost(&st);

		float d = (p->x*p->x + p->y*p->y + p->z*p->z);
		if (d>max_dist)
			d = max_dist;
		float nfe_cost = d/max_dist;

		float exp_cost = 0.6*nfe_cost + 0.4*dcost;
		return exp_cost;

	}

	pcl::PointXYZ sp;
	sp.x = st.pose.position.x;
	sp.y = st.pose.position.y;
	sp.z = st.pose.position.z;

	std::vector<int> pointIdxRadiusSearch;
	std::vector<float> pointRadiusSquaredDistance;

	tree_mutex_.lock();

	float num_points = kdtree_->radiusSearch(sp, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
	//printf("3DNavFeat. Evaluate_leaves. leaf %i, num_points: %.1f", i, num_points);

	if(num_points > 0.0)
	{
		//Normalize num_points
		float max_points = 10000.0;
		if(num_points > max_points) num_points = max_points;
			num_points = (num_points/max_points);

	} else
		num_points = 1.0;

	float dcost = no_return_cost(&st);

	tree_mutex_.unlock();

	float exp_cost;
	if(rrt_cost == -1.0)
		exp_cost = (wexp_[0]+wexp_[2]/2.0)*num_points + (wexp_[1]+wexp_[2]/2.0)*dcost;
	else
		exp_cost = wexp_[0]*num_points + wexp_[1]*dcost + wexp_[2]*rrt_cost;  //0.45, 0.35, 0.2

	return exp_cost;

}

std::vector<float> nav3d::Features3D::evaluate_leaves(std::vector<geometry_msgs::Point>* points, std::string frame, float radius)
{

	//publish exploration goals and costs
	visualization_msgs::MarkerArray ma;

	visualization_msgs::Marker l;
	l.header.frame_id = frame;
	l.header.stamp = ros::Time();
	l.ns = "rrt_exploration_costs";
	l.type = visualization_msgs::Marker::CYLINDER;
	l.action = visualization_msgs::Marker::ADD;
	l.scale.x = 0.1;
	l.scale.y = 0.1;
	l.scale.z = 0.03;
	l.color.r = 1.0f;
	l.color.g = 1.0f;
	l.color.b = 0.0f;
	l.color.a = 1.0;
	l.lifetime = ros::Duration();

	visualization_msgs::Marker t;
	t = l;
	t.scale.x = 0.15;
	t.scale.y = 0.15;
	t.scale.z = 0.15;
	t.color.r = 1.0f;
	t.color.g = 1.0f;
	t.color.b = 1.0f;
	t.type = visualization_msgs::Marker::TEXT_VIEW_FACING;

	std::vector<float> gain_info;

	geometry_msgs::PoseStamped ps;
	ps.header.frame_id = frame;
	ps.header.stamp = ros::Time::now();

	if(nfe_exploration_)
	{

		float max_dist = 5.0*5.0;
		for(unsigned int i=0; i<points->size(); i++)
		{
			ps.pose.position = points->at(i);
			ps.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
			geometry_msgs::PoseStamped st = transformPoseTo(ps, robot_odom_frame_, false);
			float dcost = no_return_cost(&st);

			geometry_msgs::Point p = points->at(i);
			float d = p.x*p.x + p.y*p.y + p.z*p.z;
			if (d>max_dist)
				d = max_dist;
			float nfe_cost = d/max_dist;

			float cost = 0.6*nfe_cost + 0.4*dcost;
			gain_info.push_back(cost);

			l.id = i+300;
			//l.text = std::to_string(cost);
			l.pose.position = points->at(i);
			l.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
			//printf(", id: %i, pcost: %.3f, dcost: %.3f, totalc: %.3f\n", (int)l.id, num_points, dcost, cost);
			ma.markers.push_back(l);
			t.id = i+600;
			t.pose.position = points->at(i);
			t.pose.position.z = t.pose.position.z + 0.05;
			t.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
			t.text = std::to_string(cost);
			ma.markers.push_back(t);
		}
		explore_pub_.publish(ma);
		return gain_info;
	}

	for(unsigned int i=0; i<points->size(); i++)
	{
		ps.pose.position = points->at(i);
		ps.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
		geometry_msgs::PoseStamped st = transformPoseTo(ps, robot_odom_frame_, false);

		pcl::PointXYZ sp;
		sp.x = st.pose.position.x;
		sp.y = st.pose.position.y;
		sp.z = st.pose.position.z;

		std::vector<int> pointIdxRadiusSearch;
		std::vector<float> pointRadiusSquaredDistance;

		tree_mutex_.lock();

		float num_points = kdtree_->radiusSearch(sp, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
		//printf("3DNavFeat. Evaluate_leaves. leaf %i, num_points: %.1f", i, num_points);

		if(num_points > 0.0)
		{
			//Normalize num_points
			float max_points = 10000.0;
			if(num_points > max_points) num_points = max_points;
			num_points = (num_points/max_points);

		} else
			num_points = 1.0;

		float dcost = no_return_cost(&st);

		tree_mutex_.unlock();

		float cost = 0.45*num_points + 0.35*dcost; // + 0.1*point_dist + 0.1*stddev;
		gain_info.push_back(cost);

		l.id = i+300;
		//l.text = std::to_string(cost);
		l.pose.position = points->at(i);
		l.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
		//printf(", id: %i, pcost: %.3f, dcost: %.3f, totalc: %.3f\n", (int)l.id, num_points, dcost, cost);
		ma.markers.push_back(l);
		t.id = i+600;
		t.pose.position = points->at(i);
		t.pose.position.z = t.pose.position.z + 0.05;
		t.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
		t.text = std::to_string(cost);
		ma.markers.push_back(t);

	}

	explore_pub_.publish(ma);
	return gain_info;
}

float nav3d::Features3D::no_return_cost(geometry_msgs::PoseStamped* p)
{
	pose_mutex_.lock();
	std::vector<geometry_msgs::PoseStamped> traj = robot_traj_;
	pose_mutex_.unlock();

	float xp = p->pose.position.x;
	float yp = p->pose.position.y;
	float zp = p->pose.position.z;

	float max_dist = 2.5;
	float cost = 0.0;
	float dmin = 1000.0;
	for(unsigned int i=0; i<traj.size(); i++)
	{
		float x = traj[i].pose.position.x;
		float y = traj[i].pose.position.y;
		float z = traj[i].pose.position.z;
		float dist = sqrt((xp-x)*(xp-x) + (yp-y)*(yp-y) + (zp-z)*(zp-z));
		if(dist < dmin)
			dmin = dist;
	}
	if(dmin > max_dist)
		dmin = max_dist;

	//The cost decreases linearly from 1 to 0 in a distance of 2 meters
	cost = ((max_dist-dmin)/max_dist);

	return cost;

}

bool nav3d::Features3D::pose3dValid(geometry_msgs::PoseStamped* s)
{

	geometry_msgs::PoseStamped st = transformPoseTo(*s, robot_odom_frame_, false); //false

	pcl::PointXYZ sp;
	sp.x = st.pose.position.x;
	sp.y = st.pose.position.y;
	//We have to calculate the approximate height z based on the neighbors
	sp.z = st.pose.position.z;
	//printf("3dnav_features. pose3dValid point x:%.2f, y:%.2f, z:%.2f\n", sp.x, sp.y, sp.z);

	//pcl::PointCloud<pcl::PointXYZ>::Ptr patch(new pcl::PointCloud<pcl::PointXYZ>);

	std::vector<int> pointIdxRadiusSearch;
  	std::vector<float> pointRadiusSquaredDistance;

	tree_mutex_.lock();

	//std::vector<int> pointIdxKSearch(10);
  	//std::vector<float> pointKSquaredDistance(10);
	//int found_k = kdtree_->nearestKSearch (sp, 10, pointIdxKSearch, pointKSquaredDistance);
	//printf("3denav_features. pose3dValid. kdtree ksearch found %i neighbors\n", found_k);
	//for(unsigned int j=0; j<found_k; j++)
	//	printf("point k %u, dist: %.2f\n", j, pointKSquaredDistance[j]);

	int found = kdtree_->radiusSearch(sp, (robot_radius_*1.0), pointIdxRadiusSearch, pointRadiusSquaredDistance);

	//int found2 = octree_->radiusSearch(sp, (robot_radius_*1.0), pointIdxRadiusSearch, pointRadiusSquaredDistance);

	float new_height = 0.0;
	if ( found > 0 )
	{
		//printf("3dnav_features. pose3dValid. kdtree radiussearch found %i neighbors in the radius %.2f\n", found, (robot_radius_*1.5));
		tree_mutex_.unlock();

		if(pointIdxRadiusSearch.size() < min_points_allowed_) {
			printf("%s. pose3dValid. %u points found. Minimum: %i\n", name_.c_str(), (unsigned int)pointIdxRadiusSearch.size(), min_points_allowed_);
			return false;
		}

		// Generate pointcloud data
  		//patch->width = (int)pointIdxRadiusSearch.size();
  		//patch->height = 1;
  		//patch->points.resize(patch->width * patch->height);

		int cont = 0;
		for (size_t i=0; i < pointIdxRadiusSearch.size(); ++i)
		{
			//printf("3dnav_features. pose3dValid. radiusSquare[%u] = %.2f\t", (unsigned int)i, pointRadiusSquaredDistance[i]);
			if((sqrt(pointRadiusSquaredDistance[i])) <= 0.25)
			{
				cont++;
				new_height += pcl_cloud_.points[ pointIdxRadiusSearch[i] ].z;
			}
			//patch->points[i] = pcl_cloud_.points[ pointIdxRadiusSearch[i] ];
		}
		if(cont > 0) {
			new_height = new_height/(float)cont;
			//s->pose.position.z = new_height;
		}

	} else {
		tree_mutex_.unlock();
		printf("%s. pose3dValid. kdtree found %i neighbors in the radius %.2f\n", name_.c_str(), found, robot_radius_);
		return false;
	}

	//pca_->setInputCloud(patch);
	//pca_->setIndices(const IndicesPtr &indices)
	//pca_->setIndices (const IndicesConstPtr &indices)
	//typedef boost::shared_ptr<std::vector<int> > pcl::IndicesPtr

	//pcl::IndicesPtr indices(new std::vector<int>);
	pcl::IndicesPtr indices(new std::vector<int>(pointIdxRadiusSearch));
	//printf("3dnav_features. pose3dValid. pind[0]:%i, pind[1]:%i, pind[2]:%i\n", pointIdxRadiusSearch[0], pointIdxRadiusSearch[1], pointIdxRadiusSearch[2]);
	//printf("3dnav_features. pose3dValid. ptr[0]:%i, ptr[1]:%i, ptr[2]:%i\n", indices->at(0), indices->at(1), indices->at(2));
	tree_mutex_.lock();
	//pca_->setIndices(indices);

	Eigen::Vector4f mean;
	Eigen::Vector3f evals;
	Eigen::Matrix3f evecs;

	try{
		//mean = pca_->getMean();
		//evals = pca_->getEigenValues();
		//evecs = pca_->getEigenVectors();

		mean = Eigen::Vector4f::Zero ();
		pcl::compute3DCentroid (pcl_cloud_, *indices, mean);   //input ->const pcl::PointCloud< PointT > &cloud
		// Compute demeanished cloud
		Eigen::MatrixXf cloud_demean;
		pcl::demeanPointCloud (pcl_cloud_, *indices, mean, cloud_demean);
		assert (cloud_demean.cols () == int (indices->size ()));
		// Compute the product cloud_demean * cloud_demean^T
		Eigen::Matrix3f alpha = static_cast<Eigen::Matrix3f> (cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ());

		// Compute eigen vectors and values
		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
		// Organize eigenvectors and eigenvalues in ascendent order
		for (int i = 0; i < 3; ++i)
		{
			evals[i] = evd.eigenvalues () [2-i];
			evecs.col(i) = evd.eigenvectors ().col (2-i);
		}

		tree_mutex_.unlock();
		//printf("3dnav_features. pose3dValid. mean x:%.2f, y:%.2f, z:%.2f\n", mean[0], mean[1], mean[2]);

	} catch(pcl::InitFailedException e)  {
		ROS_WARN("%s. pose3dValid. Calculation of mean, eigenvectors and eigenvalues failed!", name_.c_str());
		tree_mutex_.unlock();
		return false;
	}

	Eigen::Quaternion<float> q(evecs);

	auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2); //0->yaw, 1->pitch, 2->roll
	double yaw = euler[0];
	double pitch = euler[1];
	double roll = euler[2];

	//Identify the lowest eigenvalue
	//TODO: the eigen values are ordered.
	//The smallest is the last one. CHECK THIS!

	//Normal vector
	float nx = (float)evecs(0,2);
	float ny = (float)evecs(1,2);
	float nz = (float)evecs(2,2);

	pose_mutex_.lock();
	geometry_msgs::PoseStamped rpose = robot_pose_;
	pose_mutex_.unlock();

	//Check the direction of the normal
	// correct if n . (pv - Pi) > 0
	// n  -> evec with the lowest eigenvalue (= normal vector)
	// pv -> (0,0,0) Point of view
	// pi -> mean
	// See if we need to flip any plane normals
	/*
	// Dot product between the (viewpoint - point) and the plane normal
	float cos_theta = ((rpose.pose.position.x-mean[0]) * nx + (rpose.pose.position.y-mean[1]) * ny + (rpose.pose.position.z-mean[2]) * nz);
	// Flip the plane normal
	if (cos_theta < 0)
	{
		pcl::PointXYZ p;
		p.x = mean[0];
		p.y = mean[1];
		p.z = mean[2];
		pcl::flipNormalTowardsViewpoint(p, rpose.pose.position.x, rpose.pose.position.y, rpose.pose.position.z, nx, ny, nz);

		Eigen::Vector3d norm(nx, ny, nz);
		norm = norm.normalized();
		Eigen::Vector3d norm2;
		Eigen::Vector3d norm3;
		if ((fabs((double)norm(0)) > 0.001) || (fabs((double)norm(1)) > 0.001)) {
    		norm2 << -norm(1), norm(0), 0;
		} else {
    		norm2 << 0, norm(2), -norm(1);
		}
		norm2.normalize();
		norm3 = norm.cross(norm2);

		Eigen::Matrix3d R;  // Rotation matrix defining orientation
		R.col(0) = norm;
		R.col(1) = norm2;
		R.col(2) = norm3;

		Eigen::Quaternion<double> q(R);
		tf::Quaternion qu(q.x(), q.y(), q.z(), q.w());
		qu.normalize();
		tf::Matrix3x3 m(qu);
      	m.getRPY(roll, pitch, yaw);
	}
	*/
	/*
	//float curvature = fabsf(evals(0) / (evals(0) + evals(1) + evals(2)) );

	float sum = 0;
	for (size_t i = 0; i < patch->size(); ++i)
	{
		pcl::PointXYZ p = patch->at(i);   //(i,0);
		float s = (p.x - mean[0]) + (p.y - mean[1]);
		float sq = s * s;
		sum += sq;
	}
	//float me = sum / patch->size();
	float variance = sum / (patch->size () - 1);
	float stddev = sqrt (variance);
	*/

	//if(pitch > pitch_high_ || pitch < pitch_low_ || yaw > roll_high_ || yaw < roll_low_ || fabs(evals(2)) > roughness_) {
	//if(fabs(pitch) < (M_PI/4.0) || fabs(pitch) > (3.0*M_PI/4.0) || fabs(yaw) < (M_PI/4.0) || fabs(yaw) > (3.0*M_PI/4.0) || fabs(evals(2)) > roughness_) {
	if((fabs(pitch) > pitch_low_ && fabs(pitch) < pitch_high_) || /*(fabs(yaw) > roll_low_ && fabs(yaw) < roll_high_) ||*/ fabs(evals(2)) > roughness_) {
	//if( fabs(evals(2)) > roughness_) {
		//if(name_ == std::string("local_features_3d"))
			//printf("%s. pose3d INVALID. p:%.2f, y:%.2f, r:%.2f, upper:%.2f, lower:%.2f\n", name_.c_str(), fabs(pitch), fabs(yaw), fabs(roll), pitch_high_, pitch_low_);
		return false;
	} else {
		//if(name_ == std::string("local_features_3d"))
			//printf("%s. pose3d VALID. p:%.2f, y:%.2f, r:%.2f, upper:%.2f, lower:%.2f\n", name_.c_str(), fabs(pitch), fabs(yaw), fabs(roll), pitch_high_, pitch_low_);

		// mean[2] = height z in odom coordinates. I need to transform it to base frame and update this value in pose s.
		//geometry_msgs::PoseStamped samplepose;
		//meanpose.header.frame_id = robot_odom_frame_; //rpose.header.frame_id;
		//meanpose.header.stamp = ros::Time::now();
		//meanpose.pose.position.x = st.pose.position.x; //mean[0];
		//meanpose.pose.position.y = st.pose.position.y; //mean[1];
		st.header.stamp = ros::Time::now();
		if(new_height == 0.0)
			st.pose.position.z = mean[2];
		else
			st.pose.position.z = new_height; //mean[2];
		st.pose.orientation = rpose.pose.orientation;
		geometry_msgs::PoseStamped sbase = transformPoseTo(st, s->header.frame_id, false); //robot_base_frame_
		s->pose.position.z = sbase.pose.position.z;

		return true;
	}

}

/**
* TODO TRY TO NORMALIZE THE VALUE OF THE FEATURES!!!
*/
std::vector<float> nav3d::Features3D::getFeatures(geometry_msgs::PoseStamped* s)
{
	std::vector<float> features;

	if(feature_set_ == 1)
		features.assign(4, 0.0);  // features[4] = {valid, pitch, roll, roughness}
	else
		features.assign(8, 0.0); //features[8] = {valid, pitch, roll, roughness, point_dist, stddev, num_points, goal_dist} //curvature

	geometry_msgs::PoseStamped st = transformPoseTo(*s, robot_odom_frame_, false);

	pcl::PointXYZ sp;
	sp.x = st.pose.position.x;
	sp.y = st.pose.position.y;
	//We have to calculate the approximate height z based on the neighbors
	sp.z = st.pose.position.z;

	//pcl::PointCloud<pcl::PointXYZ>::Ptr patch(new pcl::PointCloud<pcl::PointXYZ>);

	std::vector<int> pointIdxRadiusSearch;
  	std::vector<float> pointRadiusSquaredDistance;

	tree_mutex_.lock();

	//std::vector<int> pointIdxKSearch(10);
  	//std::vector<float> pointKSquaredDistance(10);
	//int found_k = kdtree_->nearestKSearch (sp, 10, pointIdxKSearch, pointKSquaredDistance);
	//printf("3denav_features. pose3dValid. kdtree ksearch found %i neighbors\n", found_k);
	//for(unsigned int j=0; j<found_k; j++)
	//	printf("point k %u, dist: %.2f\n", j, pointKSquaredDistance[j]);

	int found = kdtree_->radiusSearch(sp, (robot_radius_*1.0), pointIdxRadiusSearch, pointRadiusSquaredDistance);
	float num_points = (float)found;
	if ( found > 0 )
	{
		//printf("3dnav_features. pose3dValid. kdtree radiussearch found %i neighbors in the radius %.2f\n", found, (robot_radius_*1.5));
		tree_mutex_.unlock();

		if(pointIdxRadiusSearch.size() < min_points_allowed_) {
			//printf("3dnav_features. pose3dValid. indices less than 3\n");
			return features;
		}

		// Generate pointcloud data
  		//patch->width = (int)pointIdxRadiusSearch.size();
  		//patch->height = 1;
  		//patch->points.resize(patch->width * patch->height);

		float new_height = 0.0;
		int cont = 0;
		float sum = 0;
		for (size_t i=0; i < pointIdxRadiusSearch.size(); ++i)
		{
			//printf("3dnav_features. pose3dValid. radiusSquare[%u] = %.2f\t", (unsigned int)i, pointRadiusSquaredDistance[i]);
			if((sqrt(pointRadiusSquaredDistance[i])) <= 0.25)
			{
				cont++;
				new_height += pcl_cloud_.points[ pointIdxRadiusSearch[i] ].z;
			}
			//patch->points[i] = pcl_cloud_.points[ pointIdxRadiusSearch[i] ];

		}
		if(cont > 0) {
			new_height = new_height/(float)cont;
			s->pose.position.z = new_height;
		}

	} else {
		tree_mutex_.unlock();
		printf("%s. getFeatures. kdtree radiussearch found %i neighbors in the radius %.2f\n", name_.c_str(), found, robot_radius_);
		return features;
	}

	//pca_->setInputCloud(patch);
	//pca_->setIndices(const IndicesPtr &indices)
	//pca_->setIndices (const IndicesConstPtr &indices)
	//typedef boost::shared_ptr<std::vector<int> > pcl::IndicesPtr

	//pcl::IndicesPtr indices(new std::vector<int>);
	pcl::IndicesPtr indices(new std::vector<int>(pointIdxRadiusSearch));
	tree_mutex_.lock();
	//pca_->setIndices(indices);

	Eigen::Vector4f mean;
	Eigen::Vector3f evals;
	Eigen::Matrix3f evecs;

	try{
		//mean = pca_->getMean();
		//evals = pca_->getEigenValues();
		//evecs = pca_->getEigenVectors();
		mean = Eigen::Vector4f::Zero ();
		pcl::compute3DCentroid (pcl_cloud_, *indices, mean);   //input ->const pcl::PointCloud< PointT > &cloud
		// Compute demeanished cloud
		Eigen::MatrixXf cloud_demean;
		pcl::demeanPointCloud (pcl_cloud_, *indices, mean, cloud_demean);
		assert (cloud_demean.cols () == int (indices->size ()));
		// Compute the product cloud_demean * cloud_demean^T
		Eigen::Matrix3f alpha = static_cast<Eigen::Matrix3f> (cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ());

		// Compute eigen vectors and values
		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
		// Organize eigenvectors and eigenvalues in ascendent order
		for (int i = 0; i < 3; ++i)
		{
			evals[i] = evd.eigenvalues () [2-i];
			evecs.col(i) = evd.eigenvectors ().col (2-i);
		}
		tree_mutex_.unlock();

	} catch(pcl::InitFailedException e)  {
		ROS_WARN("%s. getFeatures. PCA failed!", name_.c_str());
		tree_mutex_.unlock();
		return features;
	}

	float sum = 0;
	float max_d = 0;;
	for (size_t i=0; i < pointIdxRadiusSearch.size(); ++i)
	{
		float s = (pcl_cloud_.points[ pointIdxRadiusSearch[i] ].x - mean[0]) + (pcl_cloud_.points[ pointIdxRadiusSearch[i] ].y - mean[1]);
		float sq = s * s;
		sum += sq;

		//For normalization
		float n = robot_radius_ + robot_radius_;
		float sqn = n * n;
		max_d += sqn;
	}
	//float me = sum / pointIdxRadiusSearch.size();
	float variance = sum / (pointIdxRadiusSearch.size() - 1);
	float stddev = sqrt (variance);

	//for normalization
	float varn = max_d /  (pointIdxRadiusSearch.size() - 1);
	float max_stddev = sqrt(varn);

	//we can check how far is the centroid from the point sp
	float dx = sp.x - mean[0];
	float dy = sp.y - mean[1];
	float dz = sp.z - mean[2];
	float point_dist = sqrt(dx*dx + dy*dy + dz*dz);

	Eigen::Quaternion<float> q(evecs);

	auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2); //0->yaw, 1->pitch, 2->roll
	double yaw = euler[0];
	double pitch = euler[1];
	double roll = euler[2];

	//Identify the lowest eigenvalue
	//TODO: the eigen values are ordered.
	//The smallest is the last one. CHECK THIS!

	//Normal vector
	float nx = (float)evecs(0,2);
	float ny = (float)evecs(1,2);
	float nz = (float)evecs(2,2);

	pose_mutex_.lock();
	geometry_msgs::PoseStamped rpose = robot_pose_;
	pose_mutex_.unlock();

	//Check the direction of the normal
	// correct if n . (pv - Pi) > 0
	// n  -> evec with the lowest eigenvalue (= normal vector)
	// pv -> (0,0,0) Point of view
	// pi -> mean
	// See if we need to flip any plane normals

	// Dot product between the (viewpoint - point) and the plane normal
	/*float cos_theta = ((rpose.pose.position.x-mean[0]) * nx + (rpose.pose.position.y-mean[1]) * ny + (rpose.pose.position.z-mean[2]) * nz);
	// Flip the plane normal
	if (cos_theta < 0)
	{
		pcl::PointXYZ p;
		p.x = mean[0];
		p.y = mean[1];
		p.z = mean[2];
		pcl::flipNormalTowardsViewpoint(p, rpose.pose.position.x, rpose.pose.position.y, rpose.pose.position.z, nx, ny, nz);

		Eigen::Vector3d norm(nx, ny, nz);
		norm = norm.normalized();
		Eigen::Vector3d norm2;
		Eigen::Vector3d norm3;
		if ((fabs((double)norm(0)) > 0.001) || (fabs((double)norm(1)) > 0.001)) {
    		norm2 << -norm(1), norm(0), 0;
		} else {
    		norm2 << 0, norm(2), -norm(1);
		}
		norm2.normalize();
		norm3 = norm.cross(norm2);

		Eigen::Matrix3d R;  // Rotation matrix defining orientation
		R.col(0) = norm;
		R.col(1) = norm2;
		R.col(2) = norm3;

		Eigen::Quaternion<double> q(R);
		tf::Quaternion qu(q.x(), q.y(), q.z(), q.w());
		qu.normalize();
		tf::Matrix3x3 m(qu);
      	m.getRPY(roll, pitch, yaw);
	} */

	float curvature = fabsf(evals(0) / (evals(0) + evals(1) + evals(2)) );

	//Normalize pitch and yaw
	//if(fabs(pitch) > M_PI/2.0)
	//	pitch = M_PI - fabs(pitch);
	//pitch = fabs(pitch)/M_PI;
	//if(fabs(yaw) > M_PI/2.0)
	//	yaw = M_PI - yaw;
	//yaw = yaw/M_PI;

	float zero = M_PI/2.0;
	if(fabs(pitch) > zero)
		pitch = M_PI - fabs(pitch);
	pitch = fabs(pitch)/zero;
	if(fabs(yaw) > zero)
		yaw = M_PI - fabs(yaw);
	yaw = fabs(yaw)/zero;

	//Normalize roughness
	//Maximum value of roughness?????
	float max_rough = 1.5; //3.0;
	float rough = fabs(evals(2));
	if (rough > max_rough) rough = max_rough;
	rough = rough/max_rough;

	//normalize point dist. max dist = robot_radius_
	point_dist = point_dist / robot_radius_;

	//Normalize stddev
	stddev = 1.0 - (stddev / max_stddev);

	//Normalize num_points
	float area_circ = M_PI*(robot_radius_*robot_radius_);
	float max_points = area_circ * 100.0; //1 point per centimeter = 100p/m  //30.0;
	if(num_points > max_points) num_points = max_points;
	num_points = 1.0 - (num_points/max_points);

	float dist_cost = goalDistFeature(s);

	//Things to evaluate the position:
	// - Inclination (pitch and roll)
	// - roughness (lowest eigenvalue)
	// - distance between the mean and the point evaluated
	// - curvature of the plain
	// - dispersion in the sfere, stddev in xy? High stddev is better
	// - TODO: add a point density feature in the area

	//features[8] = {valid, pitch, roll, roughness, point_dist, curvature, stddev, num_points}
	features[0] = 1.0;
	features[1] = pitch;
	features[2] = yaw; //the frame is turned, so the roll is the yaw now
	features[3] = rough;
	if(feature_set_ == 1)
		return features;

	features[4] = point_dist;
	//features[5] = curvature;    //not normalized
	features[5] = stddev;
	features[6] = num_points; // point_density; ????
	features[7] = dist_cost;
	return features;

}

/*
 void nav3d::Features3D::reconfigureCB(navigation_features::nav_featuresConfig &config, uint32_t level){

    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
	//configuration_mutex_.lock();

    upo_featureset_ = config.upo_featureset;
	//use_global_map_ = config.use_global_map;
	sigmas_[0] = config.stddev_person_front;
	sigmas_[1] = config.stddev_person_aside;
	sigmas_[2] = sigmas_[1];
	sigmas_[3] = sigmas_[1];
	sigmas_[4] = config.stddev_person_right;
	sigmas_[5] = sigmas_[4]/2.5;
	grouping_ = config.enable_grouping;
	stddev_group_ = config.stddev_group;
	grouping_distance_ = config.grouping_distance;
	it_id_ = config.interaction_target_id;
	it_remove_gauss_ = config.it_remove_gaussian;
	//printf("upo_featureset. it_id:%i\n", it_id_);

	//configuration_mutex_.unlock();
}*/

//Service
/*bool nav3d::Features3D::setWeightsService(navigation_features::SetWeights::Request  &req, navigation_features::SetWeights::Response &res)
{
	setWeights(req.weights);
	return true;
}
*/

//Service
/*bool nav3d::Features3D::isPoseValidService(navigation_features::PoseValid::Request &req, navigation_features::PoseValid::Response &res)
{
	geometry_msgs::PoseStamped p = req.pose;
	res.ok = poseValid(&p);
}*/

//Service
/*bool nav3d::Features3D::setScenarioService(navigation_features::SetScenario::Request &req, navigation_features::SetScenario::Response &res)
{
	setScenario(req.obstacles, req.people, req.goal);

	return true;
}*/

//Service
/*bool nav3d::Features3D::getFeatureCountService(navigation_features::GetFeatureCount::Request &req, navigation_features::GetFeatureCount::Response &res)
{
	res.fc = getPathFeatureCount(&req.path);
	return true;
}*/

//Service
/*bool nav3d::Features3D::initializeWeightsService(navigation_features::InitWeights::Request &req, navigation_features::InitWeights::Response &res)
{
	vector<float> w;
	if(req.random) {
		srand(time(NULL));
		for(int i=0; i<w_.size(); i++){
			float v = rand() % 10 + 2;
			w.push_back(v);
		}
	} else {
		w.assign((int)w_.size(), 0.0);
	}

	//Normalize weights
	if(req.normalize && req.random) {
		float total = accumulate(w.begin(), w.end(), 0);
		for(unsigned int i=0; i<w.size(); i++)
			w[i] = w[i]/total;
	}
	w_ = w;
	res.weights = w;
	return true;
}*/

/*void nav3d::Features3D::setScenario(sensor_msgs::PointCloud2 obs, geometry_msgs::PoseStamped goal)
{
	setGoal(goal);
	//setPeople(people);
	setObstacles(obs);
}*/

/*
void nav3d::Features3D::setObstacles(sensor_msgs::PointCloud2 obs)
{
	sensor_msgs::PointCloud2 lcloud;
	obs.header.stamp = ros::Time();
	try{
		if(use_global_map_) {
			if(!pcl_ros::transformPointCloud("/map", obs, lcloud, *tf_listener_))
				ROS_WARN("TransformPointCloud failed!!!!!");
		} else {
			if(!pcl_ros::transformPointCloud("/base_link", obs, lcloud, *tf_listener_))
				ROS_WARN("TransformPointCloud failed!!!!!");
		}

	} catch (tf::TransformException ex){
		ROS_WARN("NAV FEATURES. pcCallback. TransformException: %s", ex.what());
	}

	cloudMutex_.lock();
	cloud_ = lcloud;
	cloudMutex_.unlock();
}
*/

/*
void nav3d::Features3D::goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	//printf("NavFeatures. Receiving goal!\n");
	setGoal(*msg);
}*/

void nav3d::Features3D::setGoal(geometry_msgs::PoseStamped g) {

	//boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
	//printf("NavFeatures. Receiving goal point!\n");
	goal_ = g;
}

float nav3d::Features3D::goalDistFeature(geometry_msgs::PoseStamped* s)  {

	//if we don't have a goal yet - Exploration
	if(goal_.header.frame_id.empty()){
			//printf("features3d. goalDistFeature zero. goal empty\n");
			return (float) 0.0;
	}

	//if the frame_id are different, transform s to goal frame
	geometry_msgs::PoseStamped p = transformPoseTo(*s, goal_.header.frame_id, false);
	float dx = goal_.pose.position.x - p.pose.position.x;
	float dy = goal_.pose.position.y - p.pose.position.y;
	float dz = goal_.pose.position.z - p.pose.position.z;
	float dist = (float)(sqrt(dx*dx + dy*dy + dz*dz)/max_planning_dist_);
	if(dist > 1.0) {
		//printf("NavFeatures. goal dist feature %.2f > 1.0. d: %.2f, max_dist: %.2f\n", dist, (float)sqrt(dx*dx + dy*dy), max_planning_dist_);
		dist = 1.0;
	}
	return dist;
}

void nav3d::Features3D::setWeights(vector<float> we) {
	printf("%s. Setting weights: \n", name_.c_str());
	w_.clear();
	for(unsigned int i=0; i<we.size(); i++)
	{
		//if(we[i] != 0.0) {
			w_.push_back(we[i]);
			//printf("we%u: %.3f, w_%u: %.3f\n", (i+1), we[i], (i+1), w_[i]);
		//}
	}

	for(unsigned int i=0; i<w_.size(); i++)
	{
		printf("w_%u: %.3f\n", (i+1), w_[i]);
	}

}

float nav3d::Features3D::getCost(geometry_msgs::PoseStamped* s)
{
	//boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);

	//configuration_mutex_.lock();

	vector<float> features = getFeatures(s);

	if(features[0] == 0.0) {
		printf("%s. getCost. Returning maximum cost\n", name_.c_str());
		return (1.0);
	}
	float cost = 0.0;
	for(unsigned int i=1; i<w_.size(); i++)
	{
		cost += (w_[i] * features[i]);
	}

	//printf("3dnav_features. get Cost: %.3f\n", cost);
	return cost;
}

geometry_msgs::PoseStamped nav3d::Features3D::transformPoseTo(geometry_msgs::PoseStamped pose_in, string frame_out, bool usetime)
{
	geometry_msgs::PoseStamped in = pose_in;
	if(!usetime)
		in.header.stamp = ros::Time();

	geometry_msgs::PoseStamped pose_out;

	geometry_msgs::Quaternion q = in.pose.orientation;
	if(!isQuaternionValid(q))
	{
		ROS_WARN("%s. transformPoseTo. Quaternion no valid. Creating new quaternion with yaw=0.0", name_.c_str());
		in.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
	}
	try {
		tf_listener_->transformPose(frame_out.c_str(), in, pose_out);
	}catch (tf::TransformException ex){
		ROS_WARN("%s. TransformException in method transformPoseTo. TargetFrame: %s : %s", name_.c_str(), frame_out.c_str(), ex.what());
	}
	//printf("Tranform pose. frame_in: %s, x:%.2f, y:%.2f, frame_out: %s, x:%.2f, y:%.2f\n", in.header.frame_id.c_str(), in.pose.position.x, in.pose.position.y, frame_out.c_str(), pose_out.pose.position.x, pose_out.pose.position.y);
	return pose_out;
}

bool nav3d::Features3D::isQuaternionValid(const geometry_msgs::Quaternion q){
    //first we need to check if the quaternion has nan's or infs
    if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){
		ROS_ERROR("%s. Quaternion has infs!!!!", name_.c_str());
		return false;
    }
    if(std::isnan(q.x) || std::isnan(q.y) || std::isnan(q.z) || std::isnan(q.w)) {
		ROS_ERROR("%s. Quaternion has nans !!!", name_.c_str());
		return false;
	}

	if(std::fabs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1) > 0.01) {
		ROS_ERROR("%s. Quaternion malformed, magnitude: %.3f should be 1.0", name_.c_str(), (q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w));
		return false;
	}

    tf::Quaternion tf_q(q.x, q.y, q.z, q.w);

    //next, we need to check if the length of the quaternion is close to zero
    if(tf_q.length2() < 1e-6){
      ROS_ERROR("%s. Quaternion has length close to zero... discarding.", name_.c_str());
      return false;
    }

	/*
    //next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
    tf_q.normalize();

    tf::Vector3 up(0, 0, 1);

    double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));

    if(fabs(dot - 1) > 1e-3){
      ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");
      return false;
    }*/

    return true;
}

float nav3d::Features3D::normalizeAngle(float val, float min, float max) {

	float norm = 0.0;
	if (val >= min)
		norm = min + fmod((val - min), (max-min));
	else
		norm = max - fmod((min - val), (max-min));

    return norm;
}