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

// Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.

// Copyright (c) 2014-2018, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_is_valid_geo
#endif

#include <limits>
#include <iostream>

#include <boost/test/included/unit_test.hpp>

#include "test_is_valid.hpp"

#include <boost/geometry/core/coordinate_type.hpp>

#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/algorithms/reverse.hpp>

BOOST_AUTO_TEST_CASE( test_is_valid_geo_polygon )
{
    typedef bg::model::point<double, 2, bg::cs::geographic<bg::degree> > pt;
    typedef bg::model::polygon<pt, false> G;

    typedef validity_tester_geo_areal<false> tester;
    typedef test_valid<tester, G> test;

    test::apply("p01", "POLYGON((-1  -1, 1  -1, 1  1, -1  1, -1  -1),(-0.5 -0.5, -0.5 0.5, 0.0 0.0, -0.5 -0.5),(0.0 0.0, 0.5 0.5, 0.5 -0.5, 0.0 0.0))", true);
}

template <typename Poly, typename Spheroid>
void test_valid_s(std::string const& wkt,
                  Spheroid const& sph,
                  bool expected_result)
{
    bg::strategy::intersection::geographic_segments<> is(sph);
    bg::strategy::area::geographic<> as(sph);

    Poly p;
    bg::read_wkt(wkt, p);
    bg::correct(p, as);

    BOOST_CHECK(bg::is_valid(p, is) == expected_result);
}

BOOST_AUTO_TEST_CASE( test_is_valid_epsg4053_polygon )
{
    typedef bg::model::point<double, 2, bg::cs::geographic<bg::degree> > pt;
    typedef bg::model::polygon<pt, false> po;

    bg::srs::spheroid<double> epsg4053(6371228, 6371228);
    bg::srs::spheroid<double> wgs84;

    // NOTE: the orientation is different in these CSes,
    // in one of them the polygon is corrected before passing it into is_valid()

    test_valid_s<po>("POLYGON((-148 -68,178 -74,76 0,-148 -68))", wgs84, true);
    test_valid_s<po>("POLYGON((-148 -68,178 -74,76 0,-148 -68))", epsg4053, true);

    test_valid_s<po>("POLYGON((-152 -54,-56 43,142 -52,-152 -54))", wgs84, true);
    test_valid_s<po>("POLYGON((-152 -54,-56 43,142 -52,-152 -54))", epsg4053, true);

    return;
}


#ifndef OPTIONS_HPP
#define OPTIONS_HPP

#include <memory>
#include <vector>

#include <Eigen/Dense>

using namespace Eigen;

class Options{

public:
	Options(){}

	~Options(){}

	//save file name
	std::string save_file;

	//lattice spacing
	int d = 1;

	//hopping
	float t = 1;

	//on-site energy
	float U = -3*t;

	//dimensionality
	int bands = 3;
	int dim = 2;

	//chemical potential
	float mu_up= 1;
	float mu_down = 1;

	//range of calculated k values
	float k_min = 0;
	float k_max = 2*3.14;

	//number of temperatures
	int n_T = 1;

	//min and max chemical potential and number of points
	float mu_min = -3;
	float mu_max = 3;
	int n_mu = 1;

	//do you want to plot 1 yes, 0 no
	int plotting = 1;

	//minimization options
	int max_iter = 300;
	float xDelta = 0.005;
	float fDelta = 0.005;
	float condition = 0.0005;
	float delta0 = 0.001;

	//lattice basis vectors
	MatrixXf basis;

	//matrix containing coupling between A -> B
	MatrixXf t_matrix;

	//contains all vector pointing from A -> B
	std::vector<std::shared_ptr<MatrixXf>> R;

};

#endif


#include "../../Utils/cmRootSumOfSquareImageFilter.h"
#include <omp.h>

#include <boost/make_shared.hpp>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <boost/locale/encoding_utf.hpp>
using boost::locale::conv::utf_to_utf;

#include "itkVectorImage.h"

#include "itkComplexToModulusImageFilter.h"

#include "../../Utils/cmISMRMRDToITKImageFilter.h"

#include "../../Utils/cmiFFTPhasedArrayFilter.h"

#include "itkExtractImageFilter.h"

#include "itkTileImageFilter.h"

#include "itkImageFileWriter.h"

#include "../../Utils/cm.h"

#include "../../Utils/ImageUtils.h"

int main(int argc, char *argv[]) {

	/** Signal file name. */

	std::string IN;
	/** Output file name. */
	std::string ON;
	/** Output file name. */
	std::string FON;

	/** */
	bool AVG =false;
	bool ROS = false;

	/** The options */
	po::options_description desc("Allowed options");
	desc.add_options()("help,h", "Produce HELP message")
																									("IN,s",po::value<std::string>(&IN), "Signal File Name H5")
																									("FON,f", po::value<std::string>(&FON), " Output File Name not sliced")
																									("AVG,a", po::value<bool>(&AVG), " Averaged Kspace")
																									("ROS,r", po::value<bool>(&ROS), " Remove OS")
																									;

	po::options_description myerror("Read an ISMRMRD file and calculate the Root Sum of Squares");

	po::variables_map vm;

	if (argc < 2) {
		std::cout << myerror << "\n";
		return 1;
	}
	try {
		po::store(po::parse_command_line(argc, argv, desc), vm);
		po::notify(vm);

		if (vm.count("help")) {
			std::cout << desc << "\n";
			return 1;
		}

	}

	catch (po::error& e) {
		std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
		std::cerr << desc << std::endl;
		return -1;

	}

	using PT = float;
	using PixelType = vcl_complex< PT >;
	const unsigned int      	Dimension = 3;

	//					using PixelType = float;

	typedef itk::VectorImage<PixelType, Dimension >  VectorImageType;
	typedef itk::Image<PixelType, Dimension >    ScalarImageType;
	typedef itk::Image<PT, Dimension >    ResultsImageType;

	typedef itk::VectorImage<PixelType, Dimension-1 >  VectorSliceImageType;
	typedef itk::Image<PixelType, Dimension-1 >    ScalarSliceImageType;

	typedef cm::ISMRMRDToITKImageFilter<VectorImageType> RF;
	RF::Pointer r=RF::New();
	r->SetIsmrmrdFileName(IN);
	r->SetremoveFrequencyOversampling(ROS);
	r->SetaverageImageThroughAverages(AVG);

	r->Update();

//			typedef cm::iFFTPhasedArrayFilter<VectorImageType> TEST;
//			TEST::Pointer coil= TEST::New();

//			coil->SetInput(r->GetOutput());
//			coil->SetKSpaceDimension(r->GetKSpaceDimension());
//			coil->Update();

//			writeImage<VectorImageType>(coil->GetOutput(),"coil.mha");

	if(vm.count("FON"))
	{

		typedef cm::RootSumOfSquareImageFilter<VectorImageType,ScalarImageType> P2;
		P2::Pointer pp2= P2::New();

		pp2->SetInput(r->GetOutput());
		pp2->SetKSpaceDimension(r->GetKSpaceDimension());
		pp2->Update();

		typedef itk::ComplexToModulusImageFilter<ScalarImageType,ResultsImageType> MFilterType;
		MFilterType::Pointer mfilter = MFilterType::New();
		mfilter->SetInput(pp2->GetOutput());
		mfilter->Update();

		std::cout<<r->getNumberOfRepetition()<<std::endl;
			std::cout<<r->getNumberOfCoils()<<std::endl;
			std::cout<<r->getRelativeReceiverBandwidth()<<std::endl;
			std::cout<<r->getFieldStrength()<<std::endl;
			std::cout<<r->getNumberOfSlice()<<std::endl;
			std::cout<<r->getTrajectory()<<std::endl;
			std::cout<<r->isAccelerated()<<std::endl;

			std::cout<<r->getAcceleration1()<<std::endl;
			std::cout<<r->getAcceleration2()<<std::endl;
			std::cout<<r->getWaveForm()<<std::endl;
			std::cout<<r->getKSpaceDimensionString()<<std::endl;
			std::cout<<r->getEncodedSize()<<std::endl;

			std::cout<<r->getEncodedResolution()<<std::endl;
			std::cout<<r->getReconResolution()<<std::endl;

		typedef itk::ImageFileWriter<ResultsImageType>  W3Type;
		typename W3Type::Pointer w3 = W3Type::New();
		w3->SetInput(mfilter->GetOutput());
		w3->SetFileName(FON);
		w3->Update();

	}

	return EXIT_SUCCESS;
}


///////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2002 - 2011.
//  Copyright 2011 John Maddock. 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_
//
// This work is based on an earlier work:
// "Algorithm 910: A Portable C++ Multiple-Precision System for Special-Function Calculations",
// in ACM TOMS, {VOL 37, ISSUE 4, (February 2011)} (C) ACM, 2011. http://doi.acm.org/10.1145/1916461.1916469

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error

#if !defined(TEST_MPF_50) && !defined(TEST_BACKEND) && !defined(TEST_CPP_DEC_FLOAT) && !defined(TEST_MPFR_50)
#  define TEST_MPF_50
#  define TEST_MPFR_50
#  define TEST_CPP_DEC_FLOAT

#ifdef _MSC_VER
#pragma message("CAUTION!!: No backend type specified so testing everything.... this will take some time!!")
#endif
#ifdef __GNUC__
#pragma warning "CAUTION!!: No backend type specified so testing everything.... this will take some time!!"
#endif

#endif

#if defined(TEST_MPF_50)
#include <boost/multiprecision/gmp.hpp>
#endif
#if defined(TEST_MPFR_50)
#include <boost/multiprecision/mpfr.hpp>
#endif
#ifdef TEST_BACKEND
#include <boost/multiprecision/concepts/mp_number_archetypes.hpp>
#endif
#ifdef TEST_CPP_DEC_FLOAT
#include <boost/multiprecision/cpp_dec_float.hpp>
#endif

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/sqrt1pm1.hpp>
#include <boost/math/special_functions/powm1.hpp>

#include "table_type.hpp"

#include "libs/math/test/powm1_sqrtp1m1_test.hpp"

//
// DESCRIPTION:
// ~~~~~~~~~~~~
//
// This file tests the functions log1p and expm1.  The accuracy tests
// use values generated with NTL::RR at 1000-bit precision
// and our generic versions of these functions.
//
// Note that when this file is first run on a new platform many of
// these tests will fail: the default accuracy is 1 epsilon which
// is too tight for most platforms.  In this situation you will
// need to cast a human eye over the error rates reported and make
// a judgement as to whether they are acceptable.  Either way please
// report the results to the Boost mailing list.  Acceptable rates of
// error are marked up below as a series of regular expressions that
// identify the compiler/stdlib/platform/data-type/test-data/test-function
// along with the maximum expected peek and RMS mean errors for that
// test.
//

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //

   //
   // Catch all cases come last:
   //
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      ".*mpfr_float_backend<18>.*",  // test type(s)
      ".*",                          // test data group
      ".*",                          // test function
      300,                           // Max Peek error
      50);                           // Max mean error
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      ".*",                          // test type(s)
      ".*",                          // test data group
      ".*",                          // test function
      15,                             // Max Peek error
      5);                            // Max mean error

   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

BOOST_AUTO_TEST_CASE( test_main )
{
   using namespace boost::multiprecision;
   expected_results();
   //
   // We test two different precisions:
   // 18 decimal digits triggers Boost.Math's 64-bit long double support.
   // 30 decimal digits triggers Boost.Math's 128-bit long double support.
   // 35 decimal digits triggers true arbitrary precision support.
   //
#ifdef TEST_MPF_50
   // We have accuracy issues with gmp_float<18> - maybe in std::exp??  Disabled for now.
   //test_powm1_sqrtp1m1(number<gmp_float<18> >(), "number<gmp_float<18> >");
   test_powm1_sqrtp1m1(number<gmp_float<30> >(), "number<gmp_float<30> >");
   test_powm1_sqrtp1m1(number<gmp_float<35> >(), "number<gmp_float<35> >");
   // there should be at least one test with expression templates off:
   test_powm1_sqrtp1m1(number<gmp_float<35>, et_off>(), "number<gmp_float<35>, et_off>");
#endif
#ifdef TEST_MPFR_50
   test_powm1_sqrtp1m1(number<mpfr_float_backend<18> >(), "number<mpfr_float_backend<18> >");
   test_powm1_sqrtp1m1(number<mpfr_float_backend<30> >(), "number<mpfr_float_backend<30> >");
   test_powm1_sqrtp1m1(number<mpfr_float_backend<35> >(), "number<mpfr_float_backend<35> >");
#endif
#ifdef TEST_CPP_DEC_FLOAT
   test_powm1_sqrtp1m1(number<cpp_dec_float<18> >(), "number<cpp_dec_float<18> >");
   test_powm1_sqrtp1m1(number<cpp_dec_float<30> >(), "number<cpp_dec_float<30> >");
   test_powm1_sqrtp1m1(number<cpp_dec_float<35, long long, std::allocator<void> > >(), "number<cpp_dec_float<35, long long, std::allocator<void> > >");
#endif
}


#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

//Publish Messages
#include <opt_msgs/RoiRect.h>
#include <opt_msgs/Rois.h>
#include <std_msgs/String.h>
#include <sensor_msgs/CameraInfo.h>
#include <opt_msgs/Detection.h>
#include <opt_msgs/DetectionArray.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>

#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <vector>
#include <image_transport/image_transport.h>
#include <dynamic_reconfigure/server.h>
#include <yolo_detector/open_ptrack_yoloConfig.h>

extern  "C"{
#include "network.h"
#include "image.h"
#include "run_yolo.h"
#include "parser.h"

#include "network.h"
#include "detection_layer.h"
#include "region_layer.h"
#include "cost_layer.h"
#include "utils.h"
#include "parser.h"
#include "box.h"
#include "image.h"
#include "demo.h"
#include "option_list.h"
}

#include "opencv2/highgui/highgui_c.h"
#include "opencv2/imgproc/imgproc_c.h"

#include <Eigen/Eigen>

#include <open_ptrack/opt_utils/conversions.h>

using namespace sensor_msgs;
using namespace message_filters;
using namespace opt_msgs;

typedef sensor_msgs::Image Image;

namespace enc = sensor_msgs::image_encodings;

network net;
char **names;
image **alphabet;
box *boxes_y;
float **probs;

float thresh;
float hier_thresh;
float median_factor;

image_transport::Publisher pub;
ros::Publisher detection_pub;

open_ptrack::opt_utils::Conversions converter;

Eigen::Matrix3f intrinsics_matrix;
bool camera_info_available_flag = false;

double _cx;
double _cy;

double _constant_x;
double _constant_y;

std::string encoding;
float mm_factor;

void camera_info_cb (const CameraInfo::ConstPtr & msg)
{
	intrinsics_matrix << msg->K[0], 0, msg->K[2], 0, msg->K[4], msg->K[5], 0, 0, 1;

	_cx = msg->K[2];
	_cy = msg->K[5];

	_constant_x =  1.0f / msg->K[0];
	_constant_y = 1.0f /  msg->K[4];

	camera_info_available_flag = true;
}

void dynamic_callback(yolo_detector::open_ptrack_yoloConfig &config, uint32_t level)
{
	std::cout << "Adjusting Parameters" <<  std::endl;
	thresh = (float)config.Thresh;
	hier_thresh = (float)config.Hier_Thresh;
	median_factor = (float)config.Median_Factor;
}

image ipl_to_image(IplImage* src)
{
    unsigned char *data = (unsigned char *)src->imageData;
    int h = src->height;
    int w = src->width;
    int c = src->nChannels;
    int step = src->widthStep;
    image out = make_image(w, h, c);
    int i, j, k, count=0;;

    for(k= 0; k < c; ++k){
        for(i = 0; i < h; ++i){
            for(j = 0; j < w; ++j){
                out.data[count++] = data[i*step + j*c + k]/255.;
            }
        }
    }
    return out;
}

image convert_image(cv_bridge::CvImageConstPtr cv_ptr_rgb, int w, int h)
{
	IplImage iplimg = cv_ptr_rgb->image;
	image out = ipl_to_image(&iplimg);
    rgbgr_image(out);

    if((h && w) && (h != out.h || w != out.w))
    {
        image resized = resize_image(out, w, h);
        free_image(out);
        out = resized;
    }

    return out;
}

float median(const cv::Mat& Input)
{

  std::vector<float> array;

  if (Input.isContinuous())
  {
    array.assign(Input.datastart, Input.dataend);

  }
  else
  {
    for (int i = 0; i < Input.rows; ++i)
    {
      array.insert(array.end(), Input.ptr<float>(i), Input.ptr<float>(i)+Input. cols);
    }
  }

  std::nth_element(array.begin() , array.begin() + array.size() * 0.5, array.end());

  return array[array.size() * 0.5];
}

void callback(const Image::ConstPtr& rgb_image,
         const Image::ConstPtr& depth_image)
{
	//std::cout<<"Sync Started"<<std::endl;
    cv_bridge::CvImageConstPtr cv_ptr_rgb = cv_bridge::toCvShare(rgb_image,
                                                                   enc::BGR8);
    if((pub.getNumSubscribers() > 0 || detection_pub.getNumSubscribers()) && camera_info_available_flag)
    {
		//std::cout<<"Run Yolo"<<std::endl;
		//ros::Time begin = ros::Time::now();
		image im = convert_image(cv_ptr_rgb,0,0);

		//std::cout << "START CREATE BOX INFO" << std::endl;
		boxInfo* boxes = (boxInfo*)calloc(1, sizeof(boxInfo));
		boxes->num = 200;
		boxes->boxes = (adjBox*)calloc(200, sizeof(adjBox));

		//std::cout << "ENTER C CODE" << std::endl;
		run_yolo_detection(im, net, boxes_y, probs, thresh,  hier_thresh, names, boxes);

		//printf( "Num of people here = %d\n", boxes->num);
		//double duration = ros::Time::now().toSec() - begin.toSec();

		//std::cout << "Time Duration: " << duration<< std::endl;

    	//Get Depth Image
		cv::Mat _depth_image;
		cv_bridge::CvImage::Ptr cv_ptr_depth;
		try
		{
		    cv_ptr_depth = cv_bridge::toCvCopy(depth_image, encoding); //, sensor_msgs::image_encodings::TYPE_32FC1);
		}
		catch (cv_bridge::Exception& e)
		{
		    ROS_ERROR("cv_bridge exception: %s", e.what());
		    return;
		}

		_depth_image = cv_ptr_depth->image;

		DetectionArray::Ptr detection_array_msg(new DetectionArray);
		detection_array_msg->header = rgb_image->header;

		for(int i = 0; i < 3; i++)
		{
			for(int j = 0; j < 3; j++)
			{
				detection_array_msg->intrinsic_matrix.push_back(intrinsics_matrix(i, j));
			}
		}

		cv::Mat image = cv_ptr_rgb->image;

		detection_array_msg->confidence_type = std::string("yolo");
		detection_array_msg->image_type = std::string("rgb");

		int i;
		for(i = 0; i < boxes->num; i++)
		{
			int medianX = boxes->boxes[i].x + (boxes->boxes[i].w / 2);
			int medianY = boxes->boxes[i].y + (boxes->boxes[i].h / 2);

			int newX = medianX - (median_factor * (medianX - boxes->boxes[i].x));
			int newY = medianY - (median_factor * (medianY - boxes->boxes[i].y));
			int newWidth = 2 * (median_factor * (medianX - boxes->boxes[i].x));
			int newHeight = 2 * (median_factor * (medianY - boxes->boxes[i].y));

			cv::Rect rect(newX, newY, newWidth, newHeight);
			float medianDepth = median(_depth_image(rect)) / mm_factor;
			//float medianDepth = _depth_image.at<float>(medianY, medianX) / 1000.0f;

			std::stringstream ss;
			ss << medianDepth << " " << mm_factor;

			if(pub.getNumSubscribers() > 0)
			{
				cv::rectangle(image, cv::Point( newX, newY ), cv::Point( newX+ newWidth, newY+ newHeight), cv::Scalar( 0, 255, 0 ), 4);
				cv::rectangle(image, cv::Point( boxes->boxes[i].x, boxes->boxes[i].y ),
									 cv::Point( boxes->boxes[i].x+ boxes->boxes[i].w, boxes->boxes[i].y+ boxes->boxes[i].h), cv::Scalar( 255, 0, 255 ), 10);
				//cv::putText(image, ss.str(), cv::Point(30,30), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(200,200,250), 1, CV_AA);
			}

			float mx =  (medianX - _cx) * medianDepth * _constant_x;
			float my = (medianY - _cy) * medianDepth * _constant_y;

			if(std::isfinite(medianDepth) && std::isfinite(mx) && std::isfinite(my))
			{
				Detection detection_msg;

				detection_msg.box_3D.p1.x = mx;
				detection_msg.box_3D.p1.y = my;
				detection_msg.box_3D.p1.z = medianDepth;

				detection_msg.box_3D.p2.x = mx;
				detection_msg.box_3D.p2.y = my;
				detection_msg.box_3D.p2.z = medianDepth;

				detection_msg.box_2D.x = medianX;
				detection_msg.box_2D.y = medianY;
				detection_msg.box_2D.width = 0;
				detection_msg.box_2D.height = 0;
				detection_msg.height = 0;
				detection_msg.confidence = boxes->boxes[i].prob;
				detection_msg.distance = medianDepth;

				detection_msg.centroid.x = mx;
				detection_msg.centroid.y = my;
				detection_msg.centroid.z = medianDepth;

				detection_msg.top.x = 0;
				detection_msg.top.y = 0;
				detection_msg.top.z = 0;

				detection_msg.bottom.x = 0;
				detection_msg.bottom.y = 0;
				detection_msg.bottom.z = 0;

				detection_array_msg->detections.push_back(detection_msg);
			}
		}

		if(pub.getNumSubscribers() > 0)
		{

			sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
			pub.publish(msg);
    	}

		detection_pub.publish(detection_array_msg);
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "yolo_based_people_detector");
    ros::NodeHandle nh("~");

	std::string depth_image_topic;
	nh.param("depth_image_topic", depth_image_topic, std::string("/camera/depth_registered/points"));
	std::string rgb_image_topic;
	nh.param("rgb_image_topic", rgb_image_topic, std::string("/camera/depth_registered/points"));
	std::string output_topic;
	nh.param("output_topic", output_topic, std::string("/ground_based_people_detector/detections"));
	std::string camera_info_topic;
	nh.param("camera_info_topic", camera_info_topic, std::string("/camera/rgb/camera_info"));

	std::string encoding_param;
	nh.param("encoding_type", encoding_param, std::string("16UC1"));

	int in_mm;
	nh.param("in_mm", in_mm, 0);

	if(encoding_param.compare(std::string("16UC1")) == 0)
	{
		encoding = sensor_msgs::image_encodings::TYPE_16UC1;
	}
	else if(encoding_param.compare(std::string("32FC1")) == 0)
	{
		encoding = sensor_msgs::image_encodings::TYPE_32FC1;
	}
	else
	{
		encoding = sensor_msgs::image_encodings::TYPE_16UC1;
	}

	if(in_mm)
	{
		mm_factor = 1000.0f;
	}
	else
	{
		mm_factor = 1.0f;
	}

	double thresh_;
	double hier_thresh_;
	median_factor = 0.1;
	camera_info_available_flag = false;

	nh.param("thresh", thresh_, 0.25);
	nh.param("heir_thresh", hier_thresh_, 0.5);

	thresh = (float)thresh_;
	hier_thresh = (float)hier_thresh_;

	std::string datacfg;
	nh.param("data_cfg", datacfg, std::string("cfg/coco.data"));
	std::string cfgfile;
	nh.param("yolo_cfg", cfgfile, std::string("cfg/yolo.cfg"));
	std::string weightfile;
	nh.param("weight_file", weightfile, std::string("yolo.weights"));

	std::string root_str;
	nh.param("root", root_str, std::string("home"));

    net = parse_network_cfg( (char*)cfgfile.c_str() );
	char *arr = (char*)((void*) &(net.layers[0]));

	printf("exit");
    //printf( "detect layer  w = %d h = %d n = %d max = %d\n",  ((layer)arr[(net.n - 1)*sizeof_layer()]).w, net.layers[net.n-1].h, net.layers[net.n-1].n, net.layers[net.n-1].w*net.layers[net.n-1].h*net.layers[net.n-1].n );
	//printf( "detect layer  w = %d h = %d n = %d max = %d\n",  layers.w, layers.h, layers.n, layers.w*layers.h*layers.n );
    load_weights( &net, (char*)weightfile.c_str() );

    set_batch_network( &net, 1 );
	srand(2222222);

	boxes_y = init_boxes(net);
	probs = init_probs(net);

	list *options = read_data_cfg((char*)datacfg.c_str() );
    char *name_list = option_find_str(options, "names", "data/names.list");

    std::string name_list_str(name_list);
    name_list_str = root_str + "/" + name_list_str;

    std::string data_list_str = root_str + "/data";
    std::cout<<data_list_str<<std::endl;

    names = get_labels((char*)name_list_str.c_str());
    image **alphabet = load_alphabet_((char*)data_list_str.c_str());

	std::cout<<"YOLO Set UP"<<std::endl;

	image_transport::ImageTransport it(nh);
    pub = it.advertise("yolo_detector/image", 1);
    detection_pub= nh.advertise<DetectionArray>(output_topic, 3);

    dynamic_reconfigure::Server<yolo_detector::open_ptrack_yoloConfig> server;
    dynamic_reconfigure::Server<yolo_detector::open_ptrack_yoloConfig>::CallbackType f;

    f = boost::bind(&dynamic_callback, _1, _2);
    server.setCallback(f);

    message_filters::Subscriber<Image> rgb_image_sub(nh, rgb_image_topic, 1);
    message_filters::Subscriber<Image> depth_image_sub(nh, depth_image_topic, 1);

    ros::Subscriber camera_info_sub = nh.subscribe(camera_info_topic, 1, camera_info_cb);

    typedef sync_policies::ApproximateTime<Image, Image> Sync1Policy;

    Synchronizer<Sync1Policy> sync(Sync1Policy(10), rgb_image_sub, depth_image_sub);
    sync.registerCallback(boost::bind(&callback, _1, _2));

    ros::spin();

    return 0;
}


#include <boost/test/unit_test.hpp>

#include <numeric>
#include <cstring>
#include <vector>
#include <deque>
#include <tuple>

#include <cereal/cereal.hpp>
#include <cereal/types/vector.hpp>
#include <cereal/archives/xml.hpp>
#include <fstream>

#include "rfr/data_containers/default_data_container.hpp"
#include "rfr/splits/binary_split_one_feature_rss_loss.hpp"
#include "rfr/nodes/temporary_node.hpp"
#include "rfr/nodes/k_ary_node.hpp"

typedef double num_t;
typedef double response_t;
typedef unsigned int index_t;
typedef std::default_random_engine rng_type;
typedef rfr::splits::data_info_t<num_t, num_t, index_t> info_t;

typedef rfr::data_containers::default_container<num_t, response_t, index_t> data_container_type;

typedef rfr::splits::binary_split_one_feature_rss_loss<num_t, response_t, index_t, rng_type> split_type;
typedef rfr::nodes::temporary_node<num_t, response_t, index_t> tmp_node_type;

typedef rfr::nodes::k_ary_node_minimal<2, split_type, num_t, response_t, index_t, rng_type> minimal_node_type;
typedef rfr::nodes::k_ary_node_full<2, split_type, num_t, response_t, index_t, rng_type> full_node_type;

data_container_type load_toy_data(){
	data_container_type data(2);

    std::string feature_file, response_file;

    feature_file  = std::string(boost::unit_test::framework::master_test_suite().argv[1]) + "toy_data_set_features.csv";
    response_file = std::string(boost::unit_test::framework::master_test_suite().argv[1]) + "toy_data_set_responses.csv";

    data.import_csv_files(feature_file, response_file);

	data.set_type_of_feature(1,10);
    return(data);
}

template <typename node_type>
void test_make_internal_node_and_make_leaf_node(){
	auto data = load_toy_data();
	data.set_type_of_feature(1,0);
	std::vector<info_t > data_info(data.num_data_points());
	BOOST_REQUIRE_EQUAL(data.num_data_points(), 100);

	for (auto i=0u; i<data.num_data_points(); ++i){
		data_info[i].index=i;
		data_info[i].response = data.response(i);
		data_info[i].prediction_value = data.response(i);
		data_info[i].weight = 1;
	}

	rng_type rng;

	// create an empty node
	std::vector<node_type> nodes;
	nodes.emplace_back();
	BOOST_REQUIRE_EQUAL(nodes.size(), 1);

	// setup a temporary node
	std::deque<tmp_node_type> tmp_nodes;
	tmp_node_type tmp_node2(0, 0, 0, data_info.begin(), data_info.end());
	tmp_nodes.push_back(tmp_node2);

	std::vector<index_t> features_to_try({0,1});

	// actually split the data and remove the tmp_node
	nodes[0].make_internal_node(tmp_nodes.front(), data, features_to_try, nodes.size(), tmp_nodes, 1, 1, rng);
	tmp_nodes.pop_front();

	// turn the first child into a leaf
	nodes.emplace_back();
	BOOST_REQUIRE_CLOSE(tmp_nodes.front().total_weight(), 60, 1e-4);
	nodes[1].make_leaf_node(tmp_nodes[0], data);
	tmp_nodes.pop_front();

	// turn the second child into a leaf
	nodes.emplace_back();
	BOOST_REQUIRE_EQUAL(nodes.size(), 3);
	BOOST_REQUIRE_CLOSE(tmp_nodes.front().total_weight(), 40, 1e-4);
	nodes[2].make_leaf_node(tmp_nodes[0], data);
	tmp_nodes.pop_front();

	// check is_leaf
	BOOST_REQUIRE(!nodes[0].is_a_leaf());
	BOOST_REQUIRE( nodes[1].is_a_leaf());
	BOOST_REQUIRE( nodes[2].is_a_leaf());

	//check children
	auto children = nodes[0].get_children();
	BOOST_REQUIRE_EQUAL(children[0],1);
	BOOST_REQUIRE_EQUAL(children[1],2);

	children = nodes[1].get_children();
	BOOST_REQUIRE_EQUAL(children[0],0);
	BOOST_REQUIRE_EQUAL(children[1],0);

	//check split_fraction
	auto sf = nodes[0].get_split_fractions();
	BOOST_REQUIRE_CLOSE(sf[0], 0.6,1e-6);
	BOOST_REQUIRE_CLOSE(sf[1], 0.4,1e-6);

	sf = nodes[1].get_split_fractions();
	BOOST_REQUIRE(std::isnan(sf[0]));
	BOOST_REQUIRE(std::isnan(sf[1]));

	// check calculation of mean and variance
	// first an internal node
	auto info0 = nodes[0].leaf_statistic();
	BOOST_CHECK(std::isnan(info0.mean()));
	BOOST_CHECK(std::isnan(info0.variance_population()));
	BOOST_REQUIRE_EQUAL(info0.sum_of_weights(), 0);
	BOOST_REQUIRE(!(nodes[0].is_a_leaf()));

	BOOST_REQUIRE_EQUAL(nodes[0].get_split_fraction(0), 0.6);
	BOOST_REQUIRE_EQUAL(nodes[0].get_split_fraction(1), 0.4);

	// now the two leaves:
	auto info1 = nodes[1].leaf_statistic();
	BOOST_CHECK_CLOSE(info1.mean(), ((num_t) 5)/3, 1e-10);
	BOOST_CHECK_CLOSE(info1.variance_unbiased_frequency(), ((num_t) 40)/177, 1e-10);
	BOOST_REQUIRE_EQUAL(info1.sum_of_weights(), 60);
	BOOST_REQUIRE(nodes[1].is_a_leaf());

	auto info2 = nodes[2].leaf_statistic();
	BOOST_CHECK_CLOSE(info2.mean(), ((num_t) 7)/2, 1e-10);
	BOOST_CHECK_CLOSE(info2.variance_unbiased_frequency(), ((num_t) 10)/39, 1e-10);
	BOOST_REQUIRE_EQUAL(info2.sum_of_weights(), 40);
	BOOST_REQUIRE(nodes[2].is_a_leaf());

	// add and remove an additional sample to a node and check that this works
	nodes[2].push_response_value(1,1);
	nodes[2].pop_response_value(1,1);
	info2 = nodes[2].leaf_statistic();
	BOOST_CHECK_CLOSE(info2.mean(), ((num_t) 7)/2, 1e-10);
	BOOST_CHECK_CLOSE(info2.variance_unbiased_frequency(), ((num_t) 10)/39, 1e-10);
	BOOST_REQUIRE_EQUAL(info2.sum_of_weights(), 40);
	BOOST_REQUIRE(nodes[2].is_a_leaf());

	// test serializability
	{
		std::ofstream ofs("/tmp/rfr_test_binary_nodes.xml");
		cereal::XMLOutputArchive oarchive(ofs);
		oarchive(nodes);
	}

	std::vector<node_type> nodes2;
	{
		std::ifstream ifs("/tmp/rfr_test_binary_nodes.xml");
		cereal::XMLInputArchive iarchive(ifs);
		iarchive(nodes2);
	}

	// just for the coverage :)
	for (auto &n: nodes)
		n.print_info();

	// check the predictions of the leaf nodes (skip root note!)
	for (auto i=1u; i< nodes.size(); ++i){
		auto stat1 = nodes[i].leaf_statistic();
		auto stat2 = nodes2[i].leaf_statistic();

		BOOST_REQUIRE_EQUAL(stat1.mean(), stat2.mean());
		BOOST_REQUIRE(stat1.numerically_equal(stat2, 1e-6));

	}
}

template <typename node_type>
void test_make_internal_node_and_make_leaf_node_differing_values(){
	auto data = load_toy_data();
	data.set_type_of_feature(1,0);
	std::vector<info_t > data_info(data.num_data_points());
	BOOST_REQUIRE_EQUAL(data.num_data_points(), 100);

	for (auto i=0u; i<data.num_data_points(); ++i){
		data_info[i].index=i;
		data_info[i].response = data.response(i);
		data_info[i].prediction_value = data.response(i)*10;
		data_info[i].weight = 1;
	}

	rng_type rng;

	// create an empty node
	std::vector<node_type> nodes;
	nodes.emplace_back();
	BOOST_REQUIRE_EQUAL(nodes.size(), 1);

	// setup a temporary node
	std::deque<tmp_node_type> tmp_nodes;
	tmp_node_type tmp_node2(0, 0, 0, data_info.begin(), data_info.end());
	tmp_nodes.push_back(tmp_node2);

	std::vector<index_t> features_to_try({0,1});

	// actually split the data and remove the tmp_node
	nodes[0].make_internal_node(tmp_nodes.front(), data, features_to_try, nodes.size(), tmp_nodes, 1, 1, rng);
	tmp_nodes.pop_front();

	// turn the first child into a leaf
	nodes.emplace_back();
	BOOST_REQUIRE_CLOSE(tmp_nodes.front().total_weight(), 60, 1e-4);
	nodes[1].make_leaf_node(tmp_nodes[0], data);
	tmp_nodes.pop_front();

	// turn the second child into a leaf
	nodes.emplace_back();
	BOOST_REQUIRE_EQUAL(nodes.size(), 3);
	BOOST_REQUIRE_CLOSE(tmp_nodes.front().total_weight(), 40, 1e-4);
	nodes[2].make_leaf_node(tmp_nodes[0], data);
	tmp_nodes.pop_front();

	// check is_leaf
	BOOST_REQUIRE(!nodes[0].is_a_leaf());
	BOOST_REQUIRE( nodes[1].is_a_leaf());
	BOOST_REQUIRE( nodes[2].is_a_leaf());

	//check children
	auto children = nodes[0].get_children();
	BOOST_REQUIRE_EQUAL(children[0],1);
	BOOST_REQUIRE_EQUAL(children[1],2);

	children = nodes[1].get_children();
	BOOST_REQUIRE_EQUAL(children[0],0);
	BOOST_REQUIRE_EQUAL(children[1],0);

	//check split_fraction
	auto sf = nodes[0].get_split_fractions();
	BOOST_REQUIRE_CLOSE(sf[0], 0.6,1e-6);
	BOOST_REQUIRE_CLOSE(sf[1], 0.4,1e-6);

	sf = nodes[1].get_split_fractions();
	BOOST_REQUIRE(std::isnan(sf[0]));
	BOOST_REQUIRE(std::isnan(sf[1]));

	// check calculation of mean and variance
	// first an internal node
	auto info0 = nodes[0].leaf_statistic();
	BOOST_CHECK(std::isnan(info0.mean()));
	BOOST_CHECK(std::isnan(info0.variance_population()));
	BOOST_REQUIRE_EQUAL(info0.sum_of_weights(), 0);
	BOOST_REQUIRE(!(nodes[0].is_a_leaf()));

	BOOST_REQUIRE_EQUAL(nodes[0].get_split_fraction(0), 0.6);
	BOOST_REQUIRE_EQUAL(nodes[0].get_split_fraction(1), 0.4);

	// now the two leaves:
	auto info1 = nodes[1].leaf_statistic();
	BOOST_CHECK_CLOSE(info1.mean(), ((num_t) 5)/3 * 10, 1e-10);
	BOOST_CHECK_CLOSE(info1.variance_unbiased_frequency(), ((num_t) 40)/177 * 100, 1e-10);
	BOOST_REQUIRE_EQUAL(info1.sum_of_weights(), 60);
	BOOST_REQUIRE(nodes[1].is_a_leaf());

	auto info2 = nodes[2].leaf_statistic();
	BOOST_CHECK_CLOSE(info2.mean(), ((num_t) 7)/2 * 10, 1e-10);
	BOOST_CHECK_CLOSE(info2.variance_unbiased_frequency(), ((num_t) 10)/39 * 100, 1e-10);
	BOOST_REQUIRE_EQUAL(info2.sum_of_weights(), 40);
	BOOST_REQUIRE(nodes[2].is_a_leaf());

	// add and remove an additional sample to a node and check that this works
	nodes[2].push_response_value(1,1);
	nodes[2].pop_response_value(1,1);
	info2 = nodes[2].leaf_statistic();
	BOOST_CHECK_CLOSE(info2.mean(), ((num_t) 7)/2 * 10, 1e-10);
	BOOST_CHECK_CLOSE(info2.variance_unbiased_frequency(), ((num_t) 10)/39 * 100, 1e-10);
	BOOST_REQUIRE_EQUAL(info2.sum_of_weights(), 40);
	BOOST_REQUIRE(nodes[2].is_a_leaf());

	// test serializability
	{
		std::ofstream ofs("/tmp/rfr_test_binary_nodes.xml");
		cereal::XMLOutputArchive oarchive(ofs);
		oarchive(nodes);
	}

	std::vector<node_type> nodes2;
	{
		std::ifstream ifs("/tmp/rfr_test_binary_nodes.xml");
		cereal::XMLInputArchive iarchive(ifs);
		iarchive(nodes2);
	}

	// just for the coverage :)
	for (auto &n: nodes)
		n.print_info();

	// check the predictions of the leaf nodes (skip root note!)
	for (auto i=1u; i< nodes.size(); ++i){
		auto stat1 = nodes[i].leaf_statistic();
		auto stat2 = nodes2[i].leaf_statistic();

		BOOST_REQUIRE_EQUAL(stat1.mean(), stat2.mean());
		BOOST_REQUIRE(stat1.numerically_equal(stat2, 1e-6));

	}
}

BOOST_AUTO_TEST_CASE( minimal_node_tests ){
	test_make_internal_node_and_make_leaf_node<minimal_node_type>();
	test_make_internal_node_and_make_leaf_node_differing_values<minimal_node_type>();
}

BOOST_AUTO_TEST_CASE( full_node_tests ){
	test_make_internal_node_and_make_leaf_node<full_node_type>();
	test_make_internal_node_and_make_leaf_node_differing_values<full_node_type>();
}


#pragma once

#include <boost/multiprecision/cpp_int.hpp>
#include <type_traits>
#include <string>
#include <fstream>
#include <array>
#include <cstdio>
#include "utils.hpp"

namespace mp = boost::multiprecision;

namespace rsa {

    template<typename Stream, template<typename> typename... Operations>
    class stream_handler : public Operations<stream_handler<Stream, Operations...>>... {

    public:

        stream_handler() = delete;
        stream_handler(const stream_handler&) = delete;
        stream_handler(stream_handler&&) = delete;

        explicit stream_handler(Stream& stream) : _stream(stream),  Operations<stream_handler<Stream, Operations...>>(this)... {}

        auto& get_stream() {
            return _stream;
        }

    private:

        Stream& _stream;

    };

    template<typename OwnerT>
    class operation {

    public:

        explicit operation(OwnerT* owner) : _owner(owner) {}

    protected:

        OwnerT* _owner;

    };

    template<typename OwnerT>
    class read_bytes_operation : private operation<OwnerT> {

    public:

        explicit read_bytes_operation(OwnerT* owner) : operation<OwnerT>(owner) {}

        template<unsigned int N>
        auto read_bytes() {
            static unsigned int n;
            return read_bytes(n);
        }

        template<unsigned int N>
        auto read_bytes(unsigned int& n) {
            std::array<char, N> result;
            this->_owner->get_stream().read(result.data(), N);
            n = this->_owner->get_stream().gcount();
            return result;
        }

    };

    template<typename OwnerT>
    class read_number_operation {

    public:

        explicit read_number_operation(OwnerT* owner) : _read_bytes(owner) {}

        template<unsigned int N>
        auto read_number() {
            unsigned int n;
            const auto bytes = _read_bytes.template read_bytes<N / 8>(n);
            return num_utils<N>::bytes_to_number(bytes.begin(), bytes.begin() + n);
        }

    private:

        read_bytes_operation<OwnerT> _read_bytes;

    };

    template<typename OwnerT>
    class read_number_padding_operation {

    public:

        explicit read_number_padding_operation(OwnerT* owner) : _read_bytes(owner) {}

        template<unsigned int N>
        auto read_number_padding(unsigned int& n) {
            auto bytes = _read_bytes.template read_bytes<N / 8>(n);
            for (unsigned int i = 0; i < N / 8 - n; ++i)
                bytes[n + i] = static_cast<char>(N / 8 - n);
            return num_utils<N>::bytes_to_number(bytes.begin(), bytes.end());
        }

        template<unsigned int N>
        auto read_number_padding() {
            static unsigned int n;
            return read_number_padding(n);
        }

    private:

        read_bytes_operation<OwnerT> _read_bytes;

    };

    template<typename OwnerT>
    class write_bytes_operation : private operation<OwnerT> {

    public:

        explicit write_bytes_operation(OwnerT* owner) : operation<OwnerT>(owner) {}

        template<unsigned int N>
        void write_bytes(const std::array<char, N>& bytes) {
            this->_owner->get_stream().write(bytes.data(), N);
        }

    };

    template<typename OwnerT>
    class write_number_operation {

    public:

        explicit write_number_operation(OwnerT* owner) : _write_bytes(owner) {}

        template<unsigned int N>
        void write_number(const typename num_utils<N>::number number) {
            _write_bytes.template write_bytes<N / 8>(num_utils<N>::number_to_bytes(number));
        }

    private:

        write_bytes_operation<OwnerT> _write_bytes;

    };

    template<typename OwnerT>
    class write_number_nlz_operation : operation<OwnerT> {

    public:

        explicit write_number_nlz_operation(OwnerT* owner) : operation<OwnerT>(owner) {}

        template<unsigned int N>
        void write_number_nlz(const typename num_utils<N>::number& number) {
            const auto msb = mp::msb(number);
            const auto n = msb % 8 == 0 ? msb / 8 : msb / 8 + 1;
            const auto bytes = num_utils<N>::number_to_bytes(number);
            this->_owner->get_stream().write(bytes.data() + N - n, n);
        }

    };

    template<typename OwnerT>
    class eof_operation : operation<OwnerT> {

    public:

        explicit eof_operation(OwnerT* owner) : operation<OwnerT>(owner) {}

        bool eof() {
            return this->_owner->get_stream().peek() == EOF;
        }

    };

    template<template<typename> typename... Operations>
    using ifstream_handler = stream_handler<std::ifstream, Operations...>;

    template<template<typename> typename... Operations>
    using ofstream_handler = stream_handler<std::ofstream, Operations...>;

    template<typename Handler, template<typename> typename Operation>
    struct has_operation {

        static constexpr bool value = std::is_base_of_v<Operation<Handler>, Handler>;

    };

    template<typename Handler, template<typename> typename Operation>
    inline constexpr bool has_operation_v = has_operation<Handler, Operation>::value;

};// namespace rsa


#include <limits>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include "pcl/point_types.h"
#include "pcl/point_cloud.h"
#include "pcl/io/pcd_io.h"
#include "pcl/kdtree/kdtree_flann.h"
#include "pcl/filters/passthrough.h"
#include "pcl/filters/voxel_grid.h"
#include "pcl/features/normal_3d.h"
#include "pcl/features/fpfh.h"
#include "pcl/registration/ia_ransac.h"
#include "pcl/registration/icp.h"
#include <pcl/surface/mls.h>

#include <NDTMatcherF2F.hh>
#include <PointCloudUtils.hh>

class FeatureCloud
{
public:
    // A bit of shorthand
    typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
    typedef pcl::PointCloud<pcl::Normal> SurfaceNormals;
    typedef pcl::PointCloud<pcl::FPFHSignature33> LocalFeatures;
    //typedef pcl::KdTreeFLANN<pcl::PointXYZ> SearchMethod;

    FeatureCloud () :
        //search_method_xyz_ (new SearchMethod),
        normal_radius_ (0.05),
        feature_radius_ (0.05)
    {
    }

    ~FeatureCloud () {}

    // Process the given cloud
    void
    setInputCloud (PointCloud::Ptr xyz)
    {
        xyz_ = xyz;
        processInput ();
    }

    // Load and process the cloud in the given PCD file
    void
    loadInputCloud (const std::string &pcd_file)
    {
        xyz_ = PointCloud::Ptr (new PointCloud);
        pcl::io::loadPCDFile (pcd_file, *xyz_);
        processInput ();
    }

    // Get a pointer to the cloud 3D points
    PointCloud::Ptr
    getPointCloud () const
    {
        return (xyz_);
    }

    // Get a pointer to the cloud of 3D surface normals
    SurfaceNormals::Ptr
    getSurfaceNormals () const
    {
        return (normals_);
    }

    // Get a pointer to the cloud of feature descriptors
    LocalFeatures::Ptr
    getLocalFeatures () const
    {
        return (features_);
    }

protected:
    // Compute the surface normals and local features
    void
    processInput ()
    {
        computeSurfaceNormals ();
        computeLocalFeatures ();
    }

    // Compute the surface normals
    void
    computeSurfaceNormals ()
    {
        normals_ = SurfaceNormals::Ptr (new SurfaceNormals);

        pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> norm_est;
        norm_est.setInputCloud (xyz_);
        norm_est.setSearchMethod (search_method_xyz_);
        norm_est.setRadiusSearch (normal_radius_);
        norm_est.compute (*normals_);
    }

    // Compute the local feature descriptors
    void
    computeLocalFeatures ()
    {
        features_ = LocalFeatures::Ptr (new LocalFeatures);

        pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
        fpfh_est.setInputCloud (xyz_);
        fpfh_est.setInputNormals (normals_);
        fpfh_est.setSearchMethod (search_method_xyz_);
        fpfh_est.setRadiusSearch (feature_radius_);
        fpfh_est.compute (*features_);
    }

private:
    // Point cloud data
    PointCloud::Ptr xyz_;
    SurfaceNormals::Ptr normals_;
    LocalFeatures::Ptr features_;
    //SearchMethod::Ptr search_method_xyz_;
    pcl::Feature<pcl::PointXYZ, pcl::Normal>::KdTreePtr search_method_xyz_;

    // Parameters
    float normal_radius_;
    float feature_radius_;
};

class TemplateRegistration
{
public:

    // A struct for storing alignment results

    typedef Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Result;

    TemplateRegistration () :
        min_sample_distance_ (0.05),
        max_correspondence_distance_ (0.01*0.01),
        nr_iterations_ (500)
    {
        // Intialize the parameters in the Sample Consensus Intial Alignment (SAC-IA) algorithm
        sac_ia_.setMinSampleDistance (min_sample_distance_);
        sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);
        sac_ia_.setMaximumIterations (nr_iterations_);
        sac_ia_.setNumberOfSamples (10);
    }

    ~TemplateRegistration () {}

    // Set the given cloud as the target to which the templates will be aligned
    void
    setTargetCloud (FeatureCloud &target_cloud)
    {
        target_ = target_cloud;
        sac_ia_.setInputTarget (target_cloud.getPointCloud ());
        sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());
    }

    // Align the moving cloud to the target specified by setTargetCloud ()
    void
    align (FeatureCloud &moving_cloud, TemplateRegistration::Result &result)
    {
        sac_ia_.setInputCloud (moving_cloud.getPointCloud ());
        sac_ia_.setSourceFeatures (moving_cloud.getLocalFeatures ());

        pcl::PointCloud<pcl::PointXYZ> registration_output;
        sac_ia_.align (registration_output);

        result = sac_ia_.getFinalTransformation ().cast<double>();
    }

private:
    // A list of template clouds and the target to which they will be aligned
    FeatureCloud target_;

    // The Sample Consensus Initial Alignment (SAC-IA) registration routine and its parameters
    pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia_;
    float min_sample_distance_;
    float max_correspondence_distance_;
    float nr_iterations_;
};

bool matchICP(pcl::PointCloud<pcl::PointXYZ> &fixed,  pcl::PointCloud<pcl::PointXYZ> &moving,
              Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &Tout)
{

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::ConstPtr f (new pcl::PointCloud<pcl::PointXYZ>(fixed) );
    pcl::PointCloud<pcl::PointXYZ>::ConstPtr m (new pcl::PointCloud<pcl::PointXYZ>(moving) );

    pcl::VoxelGrid<pcl::PointXYZ> gr1,gr2;
    gr1.setLeafSize(0.1,0.1,0.1);
    gr2.setLeafSize(0.1,0.1,0.1);

    gr1.setInputCloud(m);
    gr2.setInputCloud(f);

    cloud_in->height = 1;
    cloud_in->width = cloud_in->points.size();
    cloud_out->height = 1;
    cloud_out->width = cloud_out->points.size();
    cloud_in->is_dense = false;
    cloud_out->is_dense = false;

    gr1.filter(*cloud_in);
    gr2.filter(*cloud_out);
    //*cloud_in = moving;
    //*cloud_out= fixed;

    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

    icp.setMaximumIterations(10000);
    std::cout<<"max itr are "<<icp.getMaximumIterations()<<std::endl;
    icp.setInputCloud(cloud_in);
    icp.setInputTarget(cloud_out);

    icp.setRANSACOutlierRejectionThreshold (2);
    icp.setMaxCorrespondenceDistance(10);
    icp.setTransformationEpsilon(0.00001);
//    cout<<"ransac outlier thersh   : "<<icp.getRANSACOutlierRejectionThreshold ()<<endl;
//    cout<<"correspondance max dist : "<<icp.getMaxCorrespondenceDistance() << endl;
//    cout<<"epsilon : "<<icp.getTransformationEpsilon() << endl;
    pcl::PointCloud<pcl::PointXYZ> Final;
    icp.align(Final);

//    std::cout << "has converged:" << icp.hasConverged() << " score: " <<
//	icp.getFitnessScore() << std::endl;
//    std::cout << icp.getFinalTransformation() << std::endl;

    //Eigen::Transform<float,3,Eigen::Affine,Eigen::ColMajor> tTemp;
    Tout = (icp.getFinalTransformation()).cast<double>();

    /*    char fname[50];
        snprintf(fname,49,"/home/tsv/ndt_tmp/c2_offset.wrl");
        FILE *fout = fopen(fname,"w");
        fprintf(fout,"#VRML V2.0 utf8\n");
        lslgeneric::writeToVRML(fout,*cloud_out,Eigen::Vector3d(0,1,0));
        lslgeneric::writeToVRML(fout,Final,Eigen::Vector3d(1,0,0));
        lslgeneric::writeToVRML(fout,*cloud_in,Eigen::Vector3d(1,1,1));
        fclose(fout);
    */
    return icp.hasConverged();

}

// Align two point clouds based on the features
int
main (int argc, char **argv)
{
    if (argc < 3)
    {
        printf ("No targets given!\n");
        return (-1);
    }

    struct timeval tv_start, tv_end1, tv_end2;
    TemplateRegistration::Result ToutFPFH, ToutICP, ToutNDT, Tout;

    // Load the target cloud PCD file
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudM (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudF (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ> Final, Final0, Final1;

    *cloudM = lslgeneric::readVRML(argv[1]);
    *cloudF = lslgeneric::readVRML(argv[2]);

    double __res[] = {0.2, 0.4, 1, 2};
    std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
    lslgeneric::NDTMatcherF2F matcherF2F(false, false, false, resolutions);
    bool ret = matcherF2F.match(*cloudF,*cloudM,ToutNDT);
    Final1 = lslgeneric::transformPointCloud(ToutNDT,*cloudM);

    /* char f[50];
     snprintf(f,49,"/home/tsv/ndt_tmp/c2_offset.wrl");
     FILE *fo = fopen(f,"w");
     fprintf(fo,"#VRML V2.0 utf8\n");
     lslgeneric::writeToVRML(fo,*cloudM,Eigen::Vector3d(1,0,0));
     lslgeneric::writeToVRML(fo,Final1,Eigen::Vector3d(0,1,1));
     lslgeneric::writeToVRML(fo,*cloudF,Eigen::Vector3d(1,1,1));
     fclose(fo);
     return (0);
     */
    //start timing
    gettimeofday(&tv_start,NULL);

    pcl::VoxelGrid<pcl::PointXYZ> gr1,gr2;
    gr1.setLeafSize(0.05,0.05,0.05);
    gr2.setLeafSize(0.05,0.05,0.05);

    gr1.setInputCloud(cloudM);
    gr2.setInputCloud(cloudF);

    gr1.filter(*cloudM);
    gr2.filter(*cloudF);

    cloudM->height = 1;
    cloudM->width = cloudM->points.size();
    cloudF->height = 1;
    cloudF->width = cloudF->points.size();
    cloudM->is_dense = false;
    cloudF->is_dense = false;

    // Assign to the target FeatureCloud
    FeatureCloud target_cloud, moving_cloud;
    target_cloud.setInputCloud (cloudF);
    moving_cloud.setInputCloud (cloudM);

    TemplateRegistration templateReg;
    templateReg.setTargetCloud (target_cloud);

    // Find the best template alignment
    templateReg.align(moving_cloud,ToutFPFH);
    //stop timing1
    gettimeofday(&tv_end1,NULL);

    std::cout<<"ToutFPFH: "<<ToutFPFH.translation().transpose()<<"\n"<<ToutFPFH.rotation()<<std::endl;
    Final0 = lslgeneric::transformPointCloud(ToutFPFH,*cloudM);

    bool converged = matchICP(*cloudF,Final0,ToutICP);
    Final = lslgeneric::transformPointCloud(ToutICP,Final0);
    //stop timing2
    gettimeofday(&tv_end2,NULL);

    Tout = ToutFPFH*(ToutNDT.inverse());
    std::cout<<"FPFH\n";
    std::cout<<"E translation "<<Tout.translation().transpose()
             <<" (norm) "<<Tout.translation().norm()<<std::endl;
    std::cout<<"E rotation "<<Tout.rotation().eulerAngles(0,1,2).transpose()
             <<" (norm) "<<Tout.rotation().eulerAngles(0,1,2).norm()<<std::endl;
    std::cout<<" TIME: "<<
             (tv_end1.tv_sec-tv_start.tv_sec)*1000.+(tv_end1.tv_usec-tv_start.tv_usec)/1000.<<std::endl;

    Tout = ToutICP*ToutFPFH*(ToutNDT.inverse());
    std::cout<<"FPFH+ICP\n";
    std::cout<<"E translation "<<Tout.translation().transpose()
             <<" (norm) "<<Tout.translation().norm()<<std::endl;
    std::cout<<"E rotation "<<Tout.rotation().eulerAngles(0,1,2).transpose()
             <<" (norm) "<<Tout.rotation().eulerAngles(0,1,2).norm()<<std::endl;
    std::cout<<" TIME: "<<
             (tv_end2.tv_sec-tv_start.tv_sec)*1000.+(tv_end2.tv_usec-tv_start.tv_usec)/1000.<<std::endl;

    char fname[50];
    snprintf(fname,49,"/home/tsv/ndt_tmp/c2_offset.wrl");
    FILE *fout = fopen(fname,"w");
    fprintf(fout,"#VRML V2.0 utf8\n");
    lslgeneric::writeToVRML(fout,*cloudM,Eigen::Vector3d(1,0,0));
    lslgeneric::writeToVRML(fout,Final0,Eigen::Vector3d(0,0,1));
    lslgeneric::writeToVRML(fout,Final1,Eigen::Vector3d(0,1,1));
    lslgeneric::writeToVRML(fout,Final,Eigen::Vector3d(0,1,0));
    lslgeneric::writeToVRML(fout,*cloudF,Eigen::Vector3d(1,1,1));
    fclose(fout);

    return (0);
}


#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/project_inliers.h>

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <visualization_msgs/Marker.h>

#include <pcl_tutorial/compute_volume_nodeConfig.h>
#include <dynamic_reconfigure/server.h>
 #include "std_msgs/String.h"

 #include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <boost/bind.hpp>

using namespace message_filters;

 typedef sync_policies::ApproximateTime<sensor_msgs::PointCloud2, std_msgs::String> MySyncPolicy;

class ComputeVolumeNode
{
public:
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  ComputeVolumeNode()
  {

    bool ok2;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_final(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_linii(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_proiectii(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());

    pcl::PointCloud<pcl::PointXYZ> all_lines[4][4];
    pcl::PointCloud<pcl::PointXYZ>::Ptr all_planes[4];
    pcl::PointCloud<pcl::PointXYZ>::Ptr all_projected_lines[4][4];
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals;

    float Coeficients[3][4];

    float Volum = 1;

    pub1_ = nh_.advertise<sensor_msgs::PointCloud2>("/output_plan", 1);
    pub2_ = nh_.advertise<sensor_msgs::PointCloud2>("/output_proiectii", 1);
    pub3_ = nh_.advertise<sensor_msgs::PointCloud2>("/output_linii", 1);

    message_filters::Subscriber<sensor_msgs::PointCloud2> sub_(nh_, "/pf_out", 1000);
    message_filters::Subscriber<std_msgs::String> sub_float(nh_, "/Dimensions", 1000);

     Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), sub_, sub_float);

     sync.registerCallback(boost::bind(&ComputeVolumeNode::cloudCallback, _1, _2));

    //sub_ = nh_.subscribe("/pf_out", 1, &ComputeVolumeNode::cloudCallback, this);

    config_server_.setCallback(boost::bind(&ComputeVolumeNode::dynReconfCallback, this, _1, _2));

    vis_pub = nh_.advertise<visualization_msgs::Marker>("/Volum_final", 0);
    vis2_pub = nh_.advertise<visualization_msgs::Marker>("/Nr_of_planes", 0);
    vis3_pub = nh_.advertise<visualization_msgs::Marker>("/1_plane_cases", 0);
    vis4_pub = nh_.advertise<visualization_msgs::Marker>("/camera_type", 0);
  }

  ~ComputeVolumeNode() {}

  void planar_segmenting_single_time(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                                     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_segmented,
                                     pcl::ModelCoefficients::Ptr coefficients)
  {
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

    pcl::PointCloud<pcl::PointXYZ>::Ptr outliers(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr outliers_segmented(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::SACSegmentation<pcl::PointXYZ> seg;

    // Optional
    seg.setOptimizeCoefficients(true);
    // Mandatory
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.01);

    // Segment dominant plane
    seg.setInputCloud(cloud);
    seg.segment(*inliers, *coefficients);
    pcl::copyPointCloud<pcl::PointXYZ>(*cloud, *inliers, *cloud_segmented);

    if (inliers->indices.size() == 0)
    {

      PCL_ERROR("Could not estimate a planar model for the given dataset.");
      //ok2 = 0;
    }
  }

  void third_perpendicular_plane(float Coeficients[3][4],
                                 int i,
                                 int j,
                                 float x0,
                                 float y0,
                                 float z0)
  {

    float a1 = Coeficients[i - 1][0];
    float b1 = Coeficients[i - 1][1];
    float c1 = Coeficients[i - 1][2];

    float a2 = Coeficients[j - 1][0];
    float b2 = Coeficients[j - 1][1];
    float c2 = Coeficients[j - 1][2];

    float a3 = b1 * c2 - c1 * b2;
    float b3 = c1 * a2 - a1 * c2;
    float c3 = a1 * b2 - a2 * b1;

    float d3 = a3 * x0 + b3 * y0 + c3 * z0;

    Coeficients[3 - (i - 1) - (j - 1)][0] = a3;
    Coeficients[3 - (i - 1) - (j - 1)][1] = b3;
    Coeficients[3 - (i - 1) - (j - 1)][2] = c3;
    Coeficients[3 - (i - 1) - (j - 1)][3] = -d3;
  }

  void euclidean_segmenting(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f,
                            bool &ok2)
  {

    // Create the filtering object: downsample the dataset using a leaf size of 1cm
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    pcl::SACSegmentation<pcl::PointXYZ> seg;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());

    vg.setInputCloud(cloud);
    vg.setLeafSize(0.01f, 0.01f, 0.01f);
    vg.filter(*cloud_filtered);
    // std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; //*

    // Create the segmentation object for the planar model and set all the parameters

    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(100);
    seg.setDistanceThreshold(0.02);

    int i = 0, nr_points = (int)cloud_filtered->points.size();

    // Segment the largest planar component from the remaining cloud
    seg.setInputCloud(cloud_filtered);
    seg.segment(*inliers, *coefficients);
    if (inliers->indices.size() == 0)
    {
      std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
      //////
      ///////////
      //////////
      // TREBUIE FUNCTIE DE OPRIRE
      /////////////
    }

    // Extract the planar inliers from the input cloud
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud(cloud_filtered);
    extract.setIndices(inliers);
    extract.setNegative(false);

    // Write the planar inliers to disk
    extract.filter(*cloud_plane);
    //std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;

    // Remove the planar inliers, extract the rest
    extract.setNegative(true);
    extract.filter(*cloud_f);
    *cloud_filtered = *cloud_f; //     HERE IS THE CLOUD FILTERED EXTRACTED

    if (cloud_filtered->size() != 0)
    {
      // 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(cloud_filtered);

      std::vector<pcl::PointIndices> cluster_indices;
      pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
      ec.setClusterTolerance(0.02); // 2cm
      ec.setMinClusterSize(100);
      ec.setMaxClusterSize(25000);
      ec.setSearchMethod(tree);
      ec.setInputCloud(cloud_filtered);
      ec.extract(cluster_indices);

      for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
      {
        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(cloud_filtered->points[*pit]); //*
        cloud_cluster->width = cloud_cluster->points.size();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;

        cloud = cloud_cluster;
        ok2 = 1;
      }

      ////////////////////////////////////
    }
    else
    {
      ok2 = 0;
    }
  }

  void compute_length_line(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                           float &distanta,
                           float &coordonate_punct_minim_x,
                           float &coordonate_punct_minim_y,
                           float &coordonate_punct_minim_z,
                           float &coordonate_punct_maxim_x,
                           float &coordonate_punct_maxim_y,
                           float &coordonate_punct_maxim_z)
  {

    float minim_x = cloud->points[0].x;
    int index_min_x = 0;

    float minim_y = cloud->points[0].y;
    int index_min_y = 0;

    float minim_z = cloud->points[0].z;
    int index_min_z = 0;

    float maxim_x = cloud->points[0].x;
    int index_max_x = 0;

    float maxim_y = cloud->points[0].y;
    int index_max_y = 0;

    float maxim_z = cloud->points[0].z;
    int index_max_z = 0;

    for (int nIndex = 0; nIndex < cloud->points.size(); nIndex++)
    {
      if (minim_x > cloud->points[nIndex].x)
      {
        minim_x = cloud->points[nIndex].x;
        index_min_x = nIndex;
      }

      if (minim_y > cloud->points[nIndex].y)
      {
        minim_y = cloud->points[nIndex].y;
        index_min_y = nIndex;
      }

      if (minim_z > cloud->points[nIndex].z)
      {
        minim_z = cloud->points[nIndex].z;
        index_min_z = nIndex;
      }

      if (maxim_x < cloud->points[nIndex].x)
      {
        maxim_x = cloud->points[nIndex].x;
        index_max_x = nIndex;
      }

      if (maxim_y < cloud->points[nIndex].y)
      {
        maxim_y = cloud->points[nIndex].y;
        index_max_y = nIndex;
      }

      if (maxim_z < cloud->points[nIndex].z)
      {
        maxim_z = cloud->points[nIndex].z;
        index_max_z = nIndex;
      }
    }

    float Sortare[3];

    Sortare[0] = abs(maxim_x - minim_x);
    Sortare[1] = abs(maxim_y - minim_y);
    Sortare[2] = abs(maxim_z - minim_z);

    float maximum = Sortare[0];

    float Puncte[2][3];

    int t = 0;

    Puncte[0][0] = index_min_x;
    Puncte[1][0] = index_max_x;
    Puncte[0][1] = index_min_y;
    Puncte[1][1] = index_max_y;
    Puncte[0][2] = index_min_z;
    Puncte[1][2] = index_max_z;

    for (int q = 0; q < 3; q++)
    {
      if (maximum < Sortare[q])
      {
        maximum = Sortare[q];
        t = q;
      }
    }

    int pozitie_min = Puncte[0][t];
    int pozitie_max = Puncte[1][t];

    float distanta_x = (cloud->points[pozitie_max].x - cloud->points[pozitie_min].x);

    distanta_x = distanta_x * distanta_x;

    float distanta_y = (cloud->points[pozitie_max].y - cloud->points[pozitie_min].y);

    distanta_y = distanta_y * distanta_y;

    float distanta_z = (cloud->points[pozitie_max].z - cloud->points[pozitie_min].z);

    distanta_z = distanta_z * distanta_z;

    distanta = distanta_x + distanta_y + distanta_z;

    distanta = sqrt(distanta_x + distanta_y + distanta_z);

    coordonate_punct_minim_x = cloud->points[pozitie_min].x;
    coordonate_punct_minim_y = cloud->points[pozitie_min].y;
    coordonate_punct_minim_z = cloud->points[pozitie_min].z;

    coordonate_punct_maxim_x = cloud->points[pozitie_max].x;
    coordonate_punct_maxim_y = cloud->points[pozitie_max].y;
    coordonate_punct_maxim_z = cloud->points[pozitie_max].z;
  }

  void check_perpendicular(pcl::ModelCoefficients::Ptr plane_1,
                           pcl::ModelCoefficients::Ptr plane_2,
                           double perpendicular_threshold,
                           bool &perp_ok)

  {
    perp_ok = 0;

    float a1 = plane_1->values[0];
    float b1 = plane_1->values[1];
    float c1 = plane_1->values[2];

    float a2 = plane_2->values[0];
    float b2 = plane_2->values[1];
    float c2 = plane_2->values[2];

    float aux = abs(a1 * a2 + b1 * b2 + c1 * c2);

    if (abs(a1 * a2 + b1 * b2 + c1 * c2) < perpendicular_threshold)
    {
      //std::cout << "Perpendicular" << '\n';
      perp_ok = 1;
    }
    else
    {
     // std::cout << "Nu e Perpendicular" << '\n';
    }

    std::cout << aux << '\n';
  }

  void add_normals(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_final,
                   pcl::PointCloud<pcl::Normal>::Ptr cloud_normals)
  {
   std::cout<<cloud_final->size()<<'\n';

     // Create the normal estimation class, and pass the input dataset to it
      pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
     ne.setInputCloud (cloud_final);

  // Create an empty kdtree representation, and pass it to the normal estimation object.
  // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  ne.setSearchMethod (tree);

  // Use all neighbors in a sphere of radius 3cm
  ne.setRadiusSearch (0.03);

  // Compute the features
  ne.compute (*cloud_normals);

  std::cout<<cloud_normals->size()<<'\n';

  // cloud_normals->size () should have the same size as the input cloud->size ()*

  }

  void check_passthrough_limits(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_final,
                                float threshold_x,
                                float threshold_y,
                                float threshold_z,
                                int minimum_nr_points)
  {
    int nr_puncte[3][2];

    for(int i=0;i<3;i++){
      for(int j=0;j<2;j++)
      {
        nr_puncte[i][j]=0;
      }
    }

      for (int nIndex = 0; nIndex < cloud_final->points.size(); nIndex++)
      {

      }

  }

  void check_parallel(pcl::ModelCoefficients::Ptr plane_1,
                      pcl::ModelCoefficients::Ptr plane_2,
                      double parallel_threshold,
                      bool &paral_ok)

  {
    paral_ok = 0;

    float a1 = plane_1->values[0];
    float b1 = plane_1->values[1];
    float c1 = plane_1->values[2];

    float a2 = plane_2->values[0];
    float b2 = plane_2->values[1];
    float c2 = plane_2->values[2];

    if ((abs(a1 / a2 - b1 / b2) < parallel_threshold) && (abs(b1 / b2 - c1 / c2) < parallel_threshold) && (abs(a1 / a2 - c1 / c2) < parallel_threshold))
    {
      std::cout << "Paralel" << '\n';
      paral_ok = 1;
    }
  }

  void project_plane_2_plane_single(pcl::PointCloud<pcl::PointXYZ>::Ptr plane,
                                    float Coeficients[3][4],
                                    int j,
                                    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected)
  {

    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
    coefficients->values.resize(4);

    coefficients->values[0] = Coeficients[j - 1][0];
    coefficients->values[1] = Coeficients[j - 1][1];
    coefficients->values[2] = Coeficients[j - 1][2];
    coefficients->values[3] = Coeficients[j - 1][3];

    pcl::ProjectInliers<pcl::PointXYZ> proj;
    proj.setModelType(pcl::SACMODEL_PLANE);
    proj.setInputCloud(plane);
    proj.setModelCoefficients(coefficients);
    proj.filter(*cloud_projected);
  }

  void planar_segmenting(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                         float Coeficients[3][4],
                         pcl::PointCloud<pcl::PointXYZ>::Ptr all_planes[4],
                         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_final,
                         int t,
                         bool &ok2)
  {
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_segmented(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr outliers(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr outliers_segmented(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::SACSegmentation<pcl::PointXYZ> seg;

    // Optional
    seg.setOptimizeCoefficients(true);
    // Mandatory
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.01);

    // Segment dominant plane
    seg.setInputCloud(cloud);
    seg.segment(*inliers, *coefficients);
    pcl::copyPointCloud<pcl::PointXYZ>(*cloud, *inliers, *cloud_segmented);

    if (inliers->indices.size() == 0)
    {

      PCL_ERROR("Could not estimate a planar model for the given dataset.");
      ok2 = 0;
    }

    else
    {

      Coeficients[t - 1][0] = coefficients->values[0];
      Coeficients[t - 1][1] = coefficients->values[1];
      Coeficients[t - 1][2] = coefficients->values[2];
      Coeficients[t - 1][3] = coefficients->values[3];

      *cloud_final += *cloud_segmented;

      all_planes[t] = cloud_segmented;

      ok2 = 1;
    }
  }

  void create_lines(float Coeficients[3][4],
                    pcl::PointCloud<pcl::PointXYZ>::Ptr all_planes[4],
                    pcl::PointCloud<pcl::PointXYZ> all_lines[4][4],
                    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_linii,
                    bool &ok2,
                    bool &ok_lines)
  {

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    std::string str;
    std::string str2;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);

    int i, j;

    if (ok2 != 0)
    {

      for (i = 1; i < 4; i++)
      {

        cloud = all_planes[i];

        // Create a set of planar coefficients with X=Y=0,Z=1
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
        coefficients->values.resize(4);

        for (j = 1; j < 4; j++)
        {
          if (j != i)
          {
            /*
        std::cout << "\n";
        std::cout << "plan " << i << "\n";
         */
            coefficients->values[0] = Coeficients[j - 1][0];
            coefficients->values[1] = Coeficients[j - 1][1];
            coefficients->values[2] = Coeficients[j - 1][2];
            coefficients->values[3] = Coeficients[j - 1][3];

            /*
        std::cout << "Projecting plane " << i << " to plane " << j << "\n";
        std::cout << "Saving line " << i << "_" << j << "\n";
         */
            // Create the filtering object
            pcl::ProjectInliers<pcl::PointXYZ> proj;
            proj.setModelType(pcl::SACMODEL_PLANE);
            proj.setInputCloud(cloud);
            proj.setModelCoefficients(coefficients);
            proj.filter(*cloud_projected);

            //PCL_INFO("Saving the projected Pointcloud \n");

            all_lines[i][j] = *cloud_projected;

            *cloud_linii = *cloud_linii + *cloud_projected;

            if (all_lines[i][j].width < 10)
            {
              ok_lines = 0;
            }
          }
        }
      }
    }
    else
    {
      std::cout << "Cannot segment"
                << "\n";
    }
  }

  void project_line_2_plane(float Coeficients[3][4],
                            pcl::PointCloud<pcl::PointXYZ>::Ptr all_planes[4],
                            pcl::PointCloud<pcl::PointXYZ> all_lines[4][4],
                            pcl::PointCloud<pcl::PointXYZ>::Ptr all_projected_lines[4][4],
                            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_proiectii,
                            bool &ok_lines)
  {

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    int i, j;

    int aux;

    for (i = 1; i < 3; i++)
    {
      for (j = i; j < 4; j++)
      {
      }
    }

    for (i = 1; i < 3; i++)
    {
      for (j = i; j < 4; j++)
      {

        if (i != j)
        {

          aux = (6 - i - j);

          // Create a set of planar coefficients with X=Y=0,Z=1
          pcl::ModelCoefficients::Ptr coefficients2(new pcl::ModelCoefficients());
          coefficients2->values.resize(4);

          coefficients2->values[0] = Coeficients[aux - 1][0];
          coefficients2->values[1] = Coeficients[aux - 1][1];
          coefficients2->values[2] = Coeficients[aux - 1][2];
          coefficients2->values[3] = Coeficients[aux - 1][3];

          int aux2;
          pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_c(new pcl::PointCloud<pcl::PointXYZ>);

          for (int z = 1; z < 3; z++)
          {

            aux2 = i;
            i = j;
            j = aux2;

            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);

            *cloud = all_lines[i][j];

            // Create the filtering object
            pcl::ProjectInliers<pcl::PointXYZ> proj;
            proj.setModelType(pcl::SACMODEL_PLANE);
            proj.setInputCloud(cloud);
            proj.setModelCoefficients(coefficients2);
            proj.filter(*cloud_projected);

            *cloud_c += *cloud;
            *cloud_c += *cloud_projected;

            *cloud_proiectii += *cloud_c;
          }

          all_projected_lines[i][j] = cloud_c;

          if (all_projected_lines[i][j]->width < 10)
          {
            ok_lines = 0;
          }
        }
      }
    }
  }

  void compute_volume(pcl::PointCloud<pcl::PointXYZ>::Ptr all_projected_lines[4][4],
                      float &Volum)
  {

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    float muchii[3];

    int i, j;

    for (i = 1; i < 3; i++)
    {
      for (j = 1; j < 4; j++)
      {

        if (i < j)
        {

          cloud = all_projected_lines[i][j];

          float coordonate_punct_minim_x;
          float coordonate_punct_minim_y;
          float coordonate_punct_minim_z;
          float coordonate_punct_maxim_x;
          float coordonate_punct_maxim_y;
          float coordonate_punct_maxim_z;

          float distanta;

          compute_length_line(cloud, distanta,
                              coordonate_punct_minim_x,
                              coordonate_punct_minim_y,
                              coordonate_punct_minim_z,
                              coordonate_punct_maxim_x,
                              coordonate_punct_maxim_y,
                              coordonate_punct_maxim_z);

          Volum = Volum * distanta;
        }
      }
    }

    std::cout << "Volum final " << Volum << " m^3"
              << "\n";
  }

  void compute_volume_2_planes(float Coeficients[3][4],
                               pcl::PointCloud<pcl::PointXYZ>::Ptr all_planes[4],
                               pcl::PointCloud<pcl::PointXYZ> all_lines[4][4],
                               pcl::PointCloud<pcl::PointXYZ>::Ptr all_projected_lines[4][4],
                               pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_proiectii,
                               pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_linii,
                               bool &ok_lines,
                               float &Volum)
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr projection(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::PointCloud<pcl::PointXYZ>::Ptr plane(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::PointCloud<pcl::PointXYZ>::Ptr line(new pcl::PointCloud<pcl::PointXYZ>);

    plane = all_planes[1];

    project_plane_2_plane_single(plane, Coeficients, 2, projection);

    *line += *projection;

    *cloud_linii += *projection;

    all_lines[1][2] = *projection;

    plane = all_planes[2];

    project_plane_2_plane_single(plane, Coeficients, 1, projection);

    *line += *projection;

    *cloud_linii += *projection;

    all_lines[2][1] = *projection;

    float coordonate_punct_minim_x;
    float coordonate_punct_minim_y;
    float coordonate_punct_minim_z;
    float coordonate_punct_maxim_x;
    float coordonate_punct_maxim_y;
    float coordonate_punct_maxim_z;

    float distanta;

    if (line->width > 10)
    {
      compute_length_line(line,
                          distanta,
                          coordonate_punct_minim_x,
                          coordonate_punct_minim_y,
                          coordonate_punct_minim_z,
                          coordonate_punct_maxim_x,
                          coordonate_punct_maxim_y,
                          coordonate_punct_maxim_z);

      third_perpendicular_plane(Coeficients, 1, 2, coordonate_punct_maxim_x, coordonate_punct_maxim_y, coordonate_punct_maxim_z);

      plane = all_planes[1];

      project_plane_2_plane_single(plane, Coeficients, 3, projection);

      all_lines[1][3] = *projection;
      all_lines[3][1] = *projection;

      *cloud_linii += *projection;

      plane = all_planes[2];

      project_plane_2_plane_single(plane, Coeficients, 3, projection);

      all_lines[2][3] = *projection;
      all_lines[3][2] = *projection;

      *cloud_linii += *projection;

      project_line_2_plane(Coeficients, all_planes, all_lines, all_projected_lines, cloud_proiectii, ok_lines);

      if (ok_lines)
      {
        compute_volume(all_projected_lines, Volum);
      }
    }
    else
    {
      std::cout << "Cannot create line" << '\n';
    }
  }

  void compute_all(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_floor,
                   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_final,
                   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_proiectii,
                   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_linii,
                   float &Volum, int &p, bool &perp_ok, bool &paral_ok)
  {

    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);

    float Coeficients[3][4];

    pcl::PointCloud<pcl::PointXYZ> all_lines[4][4];
    pcl::PointCloud<pcl::PointXYZ>::Ptr all_planes[4];
    pcl::PointCloud<pcl::PointXYZ>::Ptr all_projected_lines[4][4];

    pcl::ModelCoefficients::Ptr coefficients_floor(new pcl::ModelCoefficients);

    Eigen::Vector3f normal_floor;

    bool ok = 1;

    bool ok2 = 1;

    bool ok_lines = 1;

    pcl::PCDWriter writer;

    if (cloud->size() < nivel_initial)
    {
      ok = 0;
      p = 0; // NO PLANE
    }
    else
    {
      planar_segmenting_single_time(cloud, cloud_floor, coefficients_floor);

      normal_floor << coefficients_floor->values[0], coefficients_floor->values[1], coefficients_floor->values[2];
      if (coefficients_floor->values[3] < 0)
      {
        normal_floor *= -1;
      }
      p = 1;
    }

    for (int t = 1; (t < 3) && ok; t++)
    {

      euclidean_segmenting(cloud, cloud_f, ok2);

      if (cloud_f->size() < nivel_initial / dividing_number)
      {
        ok = 0; //No more planes to cut out
      }

      if (ok2 != 0)
      {
        planar_segmenting(cloud_f, Coeficients, all_planes, cloud_final, t, ok2);

        cloud = cloud_f; // Cloud is now the extracted pointcloud

        if (cloud->size() < nivel_initial / dividing_number)
        {
          ok = 0;
          if (t == 1)
          {
            p = 1; // Only the Ground Plane
          }
        }

        p = t + 1;
      }
    }

    if (p == 2)
    {

      pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients);
      coefficients_plane->values.resize(4);

      coefficients_plane->values[0] = Coeficients[0][0];
      coefficients_plane->values[1] = Coeficients[0][1];
      coefficients_plane->values[2] = Coeficients[0][2];
      coefficients_plane->values[3] = Coeficients[0][3];

      check_parallel(coefficients_floor,
                     coefficients_plane,
                     parallel_threshold,
                     paral_ok);

      check_perpendicular(coefficients_floor,
                          coefficients_plane,
                          parallel_threshold,
                          perp_ok);
    }

    if (p == 3)
    {

      std::cout << "2 Planuri" << '\n';

      bool is_perp = 0;
      /*
        pcl::ModelCoefficients::Ptr plane_1(new pcl::ModelCoefficients);
        plane_1->values.resize(4);

        plane_1->values[0]=Coeficients[0][0];
        plane_1->values[1]=Coeficients[0][1];
        plane_1->values[2]=Coeficients[0][2];
        plane_1->values[3]=Coeficients[0][3];

        pcl::ModelCoefficients::Ptr plane_2(new pcl::ModelCoefficients);
        plane_2->values.resize(4);

        plane_2->values[0]=Coeficients[0][0];
        plane_2->values[1]=Coeficients[0][1];
        plane_2->values[2]=Coeficients[0][2];
        plane_2->values[3]=Coeficients[0][3];

        check_perpendicular(plane_1,plane_2,perpendicular_threshold,is_perp);

        */
      /*

        if(is_perp)
        {
            compute_volume_2_planes(Coeficients,
                            all_planes,
                            all_lines,
                            all_projected_lines,
                            cloud_proiectii,
                            cloud_linii,
                            ok_lines,
                            Volum);
        }
        */

      compute_volume_2_planes(Coeficients,
                              all_planes,
                              all_lines,
                              all_projected_lines,
                              cloud_proiectii,
                              cloud_linii,
                              ok_lines,
                              Volum);

    add_normals(cloud_final,cloud_normals);

    }

    if (p == 4)
    {
      std::cout << "3 Planuri" << '\n';

      create_lines(Coeficients, all_planes, all_lines, cloud_linii, ok2, ok_lines);

      /*compute_volume_2_planes(Coeficients,
                            all_planes,
                            all_lines,
                            all_projected_lines,
                            cloud_proiectii,
                            cloud_linii,
                            ok_lines,
                            Volum);*/

      project_line_2_plane(Coeficients, all_planes, all_lines, all_projected_lines, cloud_proiectii, ok_lines);

      if (ok_lines)
      {
        compute_volume(all_projected_lines, Volum);
      }
    }
  }

  void
  dynReconfCallback(pcl_tutorial::compute_volume_nodeConfig &config, uint32_t level)
  {
    selection_camera = config.selection_camera;

    nivel_initial = config.nr_points_initial;
    dividing_number = config.dividing_number;
    perpendicular_threshold = config.perpendicular_threshold;
    parallel_threshold = config.parallel_threshold;

    threshold_x = config.threshold_x;
    threshold_y = config.threshold_y;
    threshold_z = config.threshold_z;
    minimum_nr_points = (int)config.minimum_nr_points;

     z_lower_limit=config.z_lower_limit;
     z_upper_limit=config.z_upper_limit;
     x_lower_limit=config.x_lower_limit;
     x_upper_limit=config.x_upper_limit;
     y_lower_limit=config.y_lower_limit;
     y_upper_limit=config.y_upper_limit;

     passthrough_limits[0]=config.x_lower_limit;
     passthrough_limits[1]=config.x_upper_limit;
     passthrough_limits[2]=config.y_lower_limit;
     passthrough_limits[3]=config.y_upper_limit;
     passthrough_limits[4]=config.z_lower_limit;
     passthrough_limits[5]=config.z_upper_limit;
  }

  void
  set_marker(visualization_msgs::Marker &marker)
  {
    marker.header.stamp = ros::Time::now();
    marker.pose.position.x = 1;
    marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    marker.action = visualization_msgs::Marker::ADD;

    marker.pose.position.y = 1;
    marker.pose.position.z = 1;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;
    marker.scale.x = 1;
    marker.scale.y = 0.1;
    marker.scale.z = 0.1;
    marker.color.a = 1.0; // Don't forget to set the alpha! Otherwise it is invisible
    marker.color.r = 0.0;
    marker.color.g = 1.0;
    marker.color.b = 0.0;
    marker.lifetime = ros::Duration();
  }

  void
  cloudCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, const std_msgs::StringPtr &msg)
  {

    float Volum = 1;
    int p = 0;

    bool paral_ok = 0;
    bool perp_ok = 0;
    ;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_final(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_proiectii(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_linii(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_floor(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ> cloud_Test;
    pcl::fromROSMsg(*cloud_msg, cloud_Test);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plan_1(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plan_2(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPTR(new pcl::PointCloud<pcl::PointXYZ>);
    *cloudPTR = cloud_Test;

    compute_all(cloudPTR, cloud_floor, cloud_final, cloud_proiectii, cloud_linii, Volum, p, perp_ok, paral_ok);

    sensor_msgs::PointCloud2 tempROSMsg;
    sensor_msgs::PointCloud2 tempROSMsg2;
    sensor_msgs::PointCloud2 tempROSMsg3;

    pcl::toROSMsg(*cloud_final, tempROSMsg);
    pcl::toROSMsg(*cloud_proiectii, tempROSMsg2);
    pcl::toROSMsg(*cloud_linii, tempROSMsg3);

    //Camera_type
    ////////////////////////////////////////
    std::stringstream ss_camera;
    std::stringstream header_camera;

    if (selection_camera < 1.5)
    {
      ss_camera << "Openni";
      header_camera << "camera_depth_optical_frame";
    }
    else
    {
      ss_camera << "Pico";
      header_camera << "pico_zense_depth_frame";
    }

    visualization_msgs::Marker marker_camera;
    marker_camera.header.frame_id = header_camera.str();
    marker_camera.text = ss_camera.str();
    set_marker(marker_camera);

    tempROSMsg.header.frame_id = header_camera.str();
    tempROSMsg2.header.frame_id = header_camera.str();
    tempROSMsg3.header.frame_id = header_camera.str();

    //Message Marker Volume
    ////////////////////////////////////////
    std::stringstream ss;

    ss << "Volumul este " << Volum << " m3";

    visualization_msgs::Marker marker;
    marker.header.frame_id = header_camera.str();
    marker.text = ss.str();
    set_marker(marker);

    //Message Marker Volume
    ////////////////////////////////////////
    std::stringstream ss2;

    switch (p)
    {
    case 0:
      ss2 << "No plane detected"
          << "\n";
      break;
    case 1:
      ss2 << "Only ground plane detected"
          << "\n";
      break;
    case 2:
      ss2 << "Ground plane and 1 plane detected"
          << "\n";
      break;
    case 3:
      ss2 << "Ground plane and 2 planes detected"
          << "\n";
      break;
    case 4:
      ss2 << "Ground plane and 3 planes detected"
          << "\n";
      break;

      for(int i=0;i<5;i++){
       std::cout<<passthrough_limits[i]<<" ";
          }
          std::cout<<'\n';
    }

    visualization_msgs::Marker marker2;
    marker2.header.frame_id = header_camera.str();
    marker2.text = ss2.str();
    set_marker(marker2);

    //////////////////////
    std::stringstream ss3;

    ss3 << "Perpendicular " << perp_ok << " si Paralel " << paral_ok;

    /*
    if (perp_ok && paral_ok)
    {
          ss3<<"Planul e paralel si perpendicular cu podeaua"
    }
    else
    {
      if (perp_ok)
        {
          ss3<<"Planul e perpendicular cu podeaua"
        }
      else
        {
          if (paral_ok)
          {
            ss3<<"Planul e paralel cu podeaua"
          }
          else
          {
            ss3<<"Planul nu are legatura cu podeaua"
          }
        }
    }

    */

    visualization_msgs::Marker marker3;
    marker3.header.frame_id = header_camera.str();
    marker3.text = ss3.str();
    set_marker(marker3);

    ///////////////

    //Publish the data

    pub1_.publish(tempROSMsg);
    pub2_.publish(tempROSMsg2);
    pub3_.publish(tempROSMsg3);

    vis_pub.publish(marker);
    vis2_pub.publish(marker2);
    vis3_pub.publish(marker3);
    vis4_pub.publish(marker_camera);

    cloud_final->clear();
    cloud_linii->clear();
    cloud_proiectii->clear();

    Volum = 1;
  }

private:
  bool ok2;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_final;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_linii;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_proiectii;

  pcl::SACSegmentation<pcl::PointXYZ> seg;
  pcl::PointIndices::Ptr inliers;
  pcl::ModelCoefficients::Ptr coefficients;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane;

  pcl::PointCloud<pcl::PointXYZ> all_lines[4][4];
  pcl::PointCloud<pcl::PointXYZ>::Ptr all_planes[4];
  pcl::PointCloud<pcl::PointXYZ>::Ptr all_projected_lines[4][4];

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_floor;
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals;

  float Coeficients[3][4];

  float Volum;

  bool paral_ok;
  bool perp_ok;

  dynamic_reconfigure::Server<pcl_tutorial::compute_volume_nodeConfig> config_server_;

  double nivel_initial;
  double dividing_number;
  double perpendicular_threshold;
  double parallel_threshold;

  double threshold_x;
  double threshold_y;
  double threshold_z;
  int minimum_nr_points;

  float z_lower_limit;
  float z_upper_limit;
  float y_lower_limit;
  float y_upper_limit;
  float x_lower_limit;
  float x_upper_limit;

  float passthrough_limits[6];

  double selection_camera;

  ros::NodeHandle nh_;
 // ros::Subscriber sub_;

  //ros::Subscriber sub_floats;
  ros::Publisher pub1_;
  ros::Publisher pub2_;
  ros::Publisher pub3_;

  ros::Publisher vis_pub;
  ros::Publisher vis2_pub;
  ros::Publisher vis3_pub;
  ros::Publisher vis4_pub;
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "compute_volume_node");

  ComputeVolumeNode vf;

  ros::spin();
}


#define BOOST_TEST_MODULE "test_mbasin_attractive_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/forcefield/local/GoContactPotential.hpp>
#include <mjolnir/forcefield/MultipleBasin/MBasinAttractivePotential.hpp>
#include <iomanip>

BOOST_AUTO_TEST_CASE(MBasinAttractive)
{
    using real_type = double;
    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;

    const real_type e  = 1.0;
    const real_type r0 = 5.0;

    mjolnir::GoContactPotential<real_type>        ref(e, r0);
    mjolnir::MBasinAttractivePotential<real_type> pot(e, r0);

    {
        const real_type x_min = 0.0;
        const real_type x_max = r0 * std::sqrt(5.0 / 6.0);
        const real_type dx = (x_max - x_min) / N;
        for(std::size_t i=0; i<N; ++i)
        {
            const real_type x = x_min + i * dx;
            const real_type p = pot.potential(x);
            const real_type d = pot.derivative(x);

            BOOST_TEST(p == 0.0, boost::test_tools::tolerance(h));
            BOOST_TEST(d == 0.0, boost::test_tools::tolerance(h));
        }
    }
    {
        const real_type x_min = r0 * std::sqrt(5.0 / 6.0);
        const real_type x_max = pot.cutoff();
        const real_type dx = (x_max - x_min) / N;
        for(std::size_t i=1; i<N; ++i)
        {
            const real_type x    = x_min + i * dx;
            const real_type p1 = pot.potential(x);
            const real_type p2 = ref.potential(x);
            const real_type d1 = pot.derivative(x);
            const real_type d2 = ref.derivative(x);

            BOOST_TEST(p1 == p2, boost::test_tools::tolerance(h));
            BOOST_TEST(d1 == d2, boost::test_tools::tolerance(h));
        }
    }
}


#ifndef STATS_HPP
#define STATS_HPP

#include "cli_args.hpp"
#include "connection.hpp"
#include "event.hpp"
#include "graph.hpp"
#include "module.hpp"
#include "routing.hpp"
#include "sim.hpp"
#include "traffic.hpp"

#include <chrono>
#include <map>
#include <optional>
#include <vector>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

namespace ba = boost::accumulators;

class client;

class stats: public module<sim>
{
  // The singleton of the class.
  static stats *singleton;

  // The traffic of the run.
  const traffic &m_tra;

  // The arguments of the run.
  const cli_args &m_args;

  // The number of instantaneous measurements of the network state.
  const int nom = 100;

  // The time difference for taking the instantaneous measurements.
  const sim::time_type m_dt;

  // The accumulator type with double values.
  typedef ba::accumulator_set<double, ba::features<ba::tag::count,
                                                   ba::tag::mean,
                                                   ba::tag::max>> dbl_acc;

  typedef ba::accumulator_set<long, ba::features<ba::tag::sum>> sum_acc;

  // The utilization.
  dbl_acc m_utilization;

  // The blocking probability.
  std::map<routing::rt_t, dbl_acc> m_bp;
  // The blocked bitrate.
  std::map<routing::rt_t, sum_acc> m_bb;
  // The requested bitrate.
  std::map<routing::rt_t, sum_acc> m_rb;

  // The cost (CU * length) of the established protected connection.
  std::map<routing::rt_t, dbl_acc> m_cstec;

  // The time taken by a search.
  std::map<routing::rt_t, dbl_acc> m_t;

  // The number of searches.
  std::map<routing::rt_t, unsigned> m_searches;

  // The memory usage.
  std::map<routing::rt_t, dbl_acc> m_mmwus;
  std::map<routing::rt_t, dbl_acc> m_mpqcs;
  std::map<routing::rt_t, dbl_acc> m_mscs;
  std::map<routing::rt_t, dbl_acc> m_mqcs;

  // The number of connections served.
  dbl_acc m_conns;
  // The capacity served.
  dbl_acc m_capser;
  // The number of fragments.
  dbl_acc m_frags;

public:
  stats(const cli_args &, const traffic &);

  ~stats();

  static stats &
  get();

  void
  schedule(const double t);

  void
  operator()(const double t);

  // Report the algorithm performance.
  void
  algo_perf(const routing::rt_t rt, const double dt,
            const int &ncu,
            const std::pair<std::array<unsigned long, 4>,
                            std::optional<cupp>> &p);

private:
  // Calculate the average number of fragments on a link.
  double
  calculate_frags();
};

#endif