/*
 * Localizer.cpp
 *
 *  Created on: Jul 23, 2018
 *      Author: sujiwo
 */

#include <Eigen/Eigen>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include "KeyFrame.h"
#include "Localizer.h"
#include "MapPoint.h"
#include "Optimizer.h"
#include "utilities.h"

#include <omp.h>

const int OrbDescriptorDistanceUpperThreshold = 100;
const int OrbDescriptorDistanceLowerThreshold = 50;
const int OrbDescriptorHistogramLength = 30;

using Eigen::Vector2d;
using Eigen::Vector3d;

Localizer::Localizer(VMap *parentMap, bool emptyMask) :
	sourceMap(parentMap),
	featureDetector(parentMap->getFeatureDetector()),
	imgDb(parentMap->getImageDB())
{
	if (emptyMask==false)
		setMask(sourceMap->getMask());
	else
		setMask(cv::Mat());

	lastGoodFrame = nullptr;
}

Localizer::~Localizer()
{}

void
Localizer::setCameraParameterFromId (int cameraId)
{
	localizerCamera = sourceMap->getCameraParameter(cameraId);
}

void
Localizer::debug_KF_F_Matching (const KeyFrame &keyframe, const Frame &frame, const vector<pair<mpid,kpid>> &mapPtMatchPairs)
{
	const cv::Scalar
		colorGreen(0, 255, 0),
		colorRed(0, 0, 255);

	cv::Mat img = frame.getImage().clone();
	cv::cvtColor(img, img, CV_GRAY2BGR);
	string dpath = "/tmp/frame-" + to_string(keyframe.getSourceItemId()) + ".png";

	for (auto &mpair: mapPtMatchPairs) {
		const kpid _kp = sourceMap->getKeyPointId(keyframe.getId(), mpair.first);
		const cv::KeyPoint k_in_kf = keyframe.keypoint(_kp);
		const cv::KeyPoint k_in_f = frame.keypoint(mpair.second);
		cv::line(img, k_in_kf.pt, k_in_f.pt, colorGreen, 1);
		cv::circle(img, k_in_kf.pt, 2, colorRed);
	}

	cv::imwrite(dpath, img);
}

bool
Localizer::detect (cv::Mat &frmImg, kfid &srcMapKfId, Pose &computedPose)
{
	// Should only be set during debugging session
	const bool debugMatching = false;
	ptime t1;

	cv::Mat rzImg;
	if (frmImg.cols != localizerCamera.width) {
		float ratio = float(localizerCamera.width) / float(frmImg.cols);
		cv::resize(frmImg, rzImg, cv::Size(), ratio, ratio, cv::INTER_CUBIC);
	}
	else
		rzImg = frmImg;

	Frame frame (rzImg, this);
	frame.sourceMap = sourceMap;
	frame.cameraParam = this->localizerCamera;
	frame.computeBoW(*imgDb);

	auto placeCandidates = imgDb->findCandidates(frame);

	// for debugging
	vector<dataItemId> srcInfo(placeCandidates.size());
	for (int i=0; i<placeCandidates.size(); ++i) {
		srcInfo[i] = sourceMap->keyframe(placeCandidates[i])->getSourceItemId();
	}

	size_t bestKfScore=0;
	vector<bool> isValidKfs (placeCandidates.size(), false);
	bool gotValidPose = false;
	Pose currentFramePose;

	int numCpu = omp_get_num_procs();

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

		const KeyFrame &kf = *sourceMap->keyframe(placeCandidates[i]);
		/*
		 * Some notes on this vector:
		 * This is a pair of mappoint ID to its reflection (as keypoint) in
		 * currently investigated frame
		 */
		vector<pair<mpid,kpid>> mapPointMatches;

		t1 = getCurrentTime();
		/*
		 * Notes: this routine eats up the most significant runtime inside place detection.
		 * Solution:
		 * 1. Parallelization
		 * 2. ?
		 */
		int numMatches = SearchBoW (kf, frame, mapPointMatches);
		debugMsg("1: " + to_string(double((getCurrentTime() - t1).total_microseconds()) / 1e6));

		if (numMatches >= 15) {
			isValidKfs[i] = true;

			if (debugMatching==true)
				debug_KF_F_Matching(kf, frame, mapPointMatches);

			// store index of inlier pairs here
			vector<int> inliers;

			t1 = getCurrentTime();
			solvePose(kf, frame, mapPointMatches, currentFramePose, &inliers);
			debugMsg("2: " + to_string(double((getCurrentTime() - t1).total_microseconds()) / 1e6));

			if (debugMatching==true)
				debug_KF_F_Matching(kf, frame, mapPointMatches);

			// Store the inliers
			for (auto iidx: inliers) {
				auto p = mapPointMatches.at(iidx);
				frame.vfMapPoints[p.first] = p.second;
			}

			t1 = getCurrentTime();
			int numInliers = optimize_pose(frame, currentFramePose, sourceMap);
			debugMsg("3: " + to_string(double((getCurrentTime() - t1).total_microseconds()) / 1e6));

			if (numInliers >= numMatches/2 and gotValidPose==false)
				gotValidPose = true;

			srcMapKfId = kf.getId();
			continue;
		}
	}

	if (gotValidPose==true) {
		computedPose = currentFramePose;
	}

	return gotValidPose;
}

/*
 * Match the map points in kf to keypoints in frame
 */
int
Localizer::SearchBoW (const KeyFrame &kf, const Frame &frame, vector<pair<mpid,kpid>> &mapPtMatchPairs, const float matchNNRatio) const
{
	auto mapPtsInKF = sourceMap->allMapPointsAtKeyFrame(kf.getId());
	auto KeyFrameFeatureVec = imgDb->getFeatureVectorFromKeyFrame(kf.getId());
	auto keyPtMapPoint = reverseMap(mapPtsInKF);
	int matches = 0;

	set<mpid> vpMapPts;
	set<kpid> vFrameKp;
	mapPtMatchPairs.clear();

	auto KFit = KeyFrameFeatureVec.begin();
	auto KFend = KeyFrameFeatureVec.end();
	auto Fit = frame.getFeatureVector().begin();
	auto Fend = frame.getFeatureVector().end();

	while (KFit!=KFend and Fit!=Fend) {

		if (KFit->first == Fit->first) {

			const auto vIndicesKF = KFit->second;
			const auto vIndicesF = Fit->second;
			int vInxSz = vIndicesKF.size();

			for (uint32_t iKF=0; iKF<vIndicesKF.size(); iKF++) {

				auto realKpIdxKF = vIndicesKF[iKF];
				mpid nMpId;
				try {
					nMpId = keyPtMapPoint.at(realKpIdxKF);
				} catch (out_of_range &e) {
					continue;
				}

				const cv::Mat &descKf = kf.descriptor(realKpIdxKF);

				int bestDist1 = 256;
				int bestIdxF = -1;
				int bestDist2 = 256;

				kpid realKpIdxF;
				for (uint32_t iF=0; iF<vIndicesF.size(); ++iF) {
					realKpIdxF = vIndicesF[iF];

					if (vpMapPts.find(nMpId) != vpMapPts.end())
						continue;

					const cv::Mat descF = frame.descriptor(realKpIdxF);
					int descDist = ORBDescriptorDistance(descKf, descF);

					if (descDist < bestDist1) {
						bestDist2 = bestDist1;
						bestDist1 = descDist;
						bestIdxF = realKpIdxF;
					} else if (descDist<bestDist2) {
						bestDist2 = descDist;
					}
				}

				if (bestDist1 <= OrbDescriptorDistanceLowerThreshold) {
					if (float(bestDist1) <  matchNNRatio*float(bestDist2)) {
						vpMapPts.insert(nMpId);
						matches++;
						const cv::KeyPoint &kp = kf.keypoint(realKpIdxKF);
						mapPtMatchPairs.push_back (make_pair(nMpId, bestIdxF));

						// Check orientation
						// XXX: We skip orientation check
//						float rot = kp.angle - frame.keypoints(bestIdxF);
//						if (rot<0.0)
//							rot += 360.f;
					}
				}
			}

			KFit ++;
			Fit ++;
		}

		else if (KFit->first < Fit->first) {
			KFit = KeyFrameFeatureVec.lower_bound(Fit->first);
		}

		else {
			Fit = frame.getFeatureVector().lower_bound(KFit->first);
		}
	}

	return matches;
}

bool
Localizer::solvePose (
	const KeyFrame &kf,
	const Frame &fr,
	const vector<pair<mpid,kpid>> &mapPtMatchPairs,
	Pose& frpose,
	vector<int> *inliers)
const
{
	Eigen::Matrix4d eKfExt = kf.externalParamMatrix4();

	Eigen::Matrix3d eKfRotMat = eKfExt.block<3,3>(0,0);
	cv::Mat cKfRotMat, cRVec;
	cv::eigen2cv(eKfRotMat, cKfRotMat);
	cv::Rodrigues(cKfRotMat, cRVec);

	Eigen::Vector3d eKfTransVec = eKfExt.block<3,1>(0,3);
	cv::Mat cKfTransVec;
	cv::eigen2cv(eKfTransVec, cKfTransVec);

	cv::Mat
		objectPoints (mapPtMatchPairs.size(), 3, CV_32F),
		imagePoints (mapPtMatchPairs.size(), 2, CV_32F);
	for (int r=0; r<mapPtMatchPairs.size(); ++r) {
		objectPoints.at<float>(r, 0) = sourceMap->mappoint(mapPtMatchPairs[r].first)->X();
		objectPoints.at<float>(r, 1) = sourceMap->mappoint(mapPtMatchPairs[r].first)->Y();
		objectPoints.at<float>(r, 2) = sourceMap->mappoint(mapPtMatchPairs[r].first)->Z();
		imagePoints.at<float>(r, 0) = fr.keypoint(mapPtMatchPairs[r].second).pt.x;
		imagePoints.at<float>(r, 1) = fr.keypoint(mapPtMatchPairs[r].second).pt.y;
	}

	cv::Mat cameraMatrix = sourceMap->getCameraParameter(0).toCvMat();

	cv::Mat inlierIdx;

	bool hasSolution = cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, cv::Mat(), cRVec, cKfTransVec, true, 100, 4.0, 0.99, inlierIdx, cv::SOLVEPNP_EPNP);
	if (hasSolution==false)
		return false;

	cv::Rodrigues(cRVec, cKfRotMat);
	cv::cv2eigen(cKfRotMat, eKfRotMat);
	Eigen::Quaterniond Q;
	Q = eKfRotMat;
	cv::cv2eigen(cKfTransVec, eKfTransVec);

	frpose = Pose::from_Pos_Quat(eKfTransVec, Q).inverse();

	if (inliers != nullptr) {
		inliers->resize(inlierIdx.rows);
		for (int i=0; i<inlierIdx.rows; ++i) {
			inliers->at(i) = inlierIdx.at<int>(i,0);
		}
	}

	return true;
}

bool
Localizer::detect_mt (cv::Mat &frmImg, kfid &srcMapKfId, Pose &computedPose)
{
	// Should only be set during debugging session
	const bool debugMatching = false;
	ptime t1;

	cv::Mat rzImg;
	if (frmImg.cols != localizerCamera.width) {
		float ratio = float(localizerCamera.width) / float(frmImg.cols);
		cv::resize(frmImg, rzImg, cv::Size(), ratio, ratio, cv::INTER_CUBIC);
	}
	else
		rzImg = frmImg;

	auto frame = createFrame(rzImg);

	auto placeCandidates = imgDb->findCandidates(*frame);

	// for debugging
	vector<dataItemId> srcInfo(placeCandidates.size());
	for (int i=0; i<placeCandidates.size(); ++i) {
		srcInfo[i] = sourceMap->keyframe(placeCandidates[i])->getSourceItemId();
	}

	size_t bestKfScore=0;
	vector<bool> isValidKfs (placeCandidates.size(), false);
	bool gotValidPose = false;
	Pose currentFramePose;

	kfid bestKfMatch;
	int bestNumValidMatches=-1;

	const int numCpu = omp_get_num_procs();
	const int numRepeatSearch = (placeCandidates.size() + numCpu - 1) / numCpu;
	vector<int> numValidMatches (numCpu, 0);
	vector < vector <pair <mpid,kpid> > > mapPointMatches (numCpu);

	for (int r=0; r<numRepeatSearch; r++) {

		int numProc;
		if (r<numRepeatSearch-1)
			numProc = numCpu;
		else
			numProc = placeCandidates.size() % numCpu;

		// Parallelizable
#pragma omp parallel for
		for (int c=0; c<numProc; c++) {

			int p = r*numCpu + c;
			const KeyFrame &kf = *sourceMap->keyframe(placeCandidates[p]);
			mapPointMatches[c].clear();
			numValidMatches[c] = SearchBoW(kf, *frame, mapPointMatches[c]);

		}

		// Join. (only using single thread here)
		vector<int> inliers;
		for (int c=0; c<numProc; c++) {

			if (numValidMatches[c] > bestNumValidMatches) {
				bestNumValidMatches = numValidMatches[c];
				bestKfMatch = placeCandidates[r*numCpu + c];
			}

			if (numValidMatches[c] >= 15) {

				int p = r*numCpu + c;
				const KeyFrame &kf = *sourceMap->keyframe(placeCandidates[p]);

				inliers.clear();
				solvePose(kf, *frame, mapPointMatches[c], currentFramePose, &inliers);

				// Store the inliers
				for (auto iidx: inliers) {
					const auto &match = mapPointMatches[c].at(iidx);
					frame->vfMapPoints[match.first] = match.second;
				}

				int numInliers = optimize_pose(*frame, currentFramePose, sourceMap);
				if (numInliers >= numValidMatches[c]/2 and gotValidPose==false) {
					gotValidPose = true;
					srcMapKfId = kf.getId();
				}

				// XXX: Should we break off when pose is found ?

				continue;
			}
		}

		if (gotValidPose)
			break;

		continue;
	}

	if (gotValidPose) {
		computedPose = currentFramePose;
		lastGoodFrame = frame;
	}
	else {
		lastGoodFrame = nullptr;
	}
	return gotValidPose;
}

shared_ptr<Frame>
Localizer::createFrame (cv::Mat &imgSrc) const
{
	shared_ptr<Frame> newFrame (new Frame(imgSrc, this, &this->localizerCamera));
	newFrame->sourceMap = sourceMap;
	newFrame->computeBoW(*imgDb);

	return newFrame;
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

#include <scitbx/line_search/more_thuente_1994.h>

namespace scitbx { namespace line_search { namespace boost_python {

namespace {

  struct more_thuente_1994_wrappers
  {
    typedef more_thuente_1994<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("line_search_more_thuente_1994")
        .def_readwrite("xtol", &w_t::xtol)
        .def_readwrite("ftol", &w_t::ftol)
        .def_readwrite("gtol", &w_t::gtol)
        .def_readwrite("stpmin", &w_t::stpmin)
        .def_readwrite("stpmax", &w_t::stpmax)
        .def_readwrite("maxfev", &w_t::maxfev)
        .def_readonly("info_code", &w_t::info_code)
        .def_readonly("info_meaning", &w_t::info_meaning)
        .def_readonly("stp", &w_t::stp)
        .def_readonly("nfev", &w_t::nfev)
        .def("start", &w_t::start, (
          arg("x"),
          arg("functional"),
          arg("gradients"),
          arg("search_direction"),
          arg("initial_estimate_of_satisfactory_step_length")))
        .def("next", &w_t::next, (
          arg("x"),
          arg("functional"),
          arg("gradients")))
      ;
    }
  };

  void
  wrap()
  {
    more_thuente_1994_wrappers::wrap();
  }

}}} // namespace line_search::boost_python::<anonymous>

namespace math { namespace boost_python {

  void wrap_line_search()
  {
    line_search::boost_python::wrap();
  }

}}} // namespace scitbx::math::boost_python


//===----------------------------------------------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//  Adaptation to Boost of the libcxx
//  Copyright 2010 Vicente J. Botet Escriba
//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

#include <boost/chrono/duration.hpp>
#include <boost/detail/lightweight_test.hpp>

#include <libs/chrono/test/rep.h>
#include <iostream>

template <class D>
void
check_default()
{
    D d=D();
  //std::cout << d.count() << std::endl;
  //std::cout << typename D::rep() <<  std::endl;
    BOOST_TEST(d.count() == typename D::rep());
}

template <class D, class R>
void
check_from_rep(R r)
{
    D d(r);
    BOOST_TEST(d.count() == r);
}

int main()
{
    // exact conversions allowed for integral reps
    {
        boost::chrono::milliseconds ms(1);
        boost::chrono::microseconds us = ms;
        BOOST_TEST(us.count() == 1000);
    }
    // inexact conversions allowed for floating point reps
    {
        boost::chrono::duration<double, boost::micro> us(1);
        boost::chrono::duration<double, boost::milli> ms = us;
        BOOST_TEST(ms.count() == 1./1000);
    }
    // Convert int to float
    {
        boost::chrono::duration<int> i(3);
        boost::chrono::duration<int> d = i;
        BOOST_TEST(d.count() == 3);
    }
    // default constructor
    {
        check_default<boost::chrono::duration<Rep> >();
    }
    // constructor from rep
    {
        check_from_rep<boost::chrono::duration<int> >(5);
        check_from_rep<boost::chrono::duration<int, boost::ratio<3, 2> > >(5);
        check_from_rep<boost::chrono::duration<Rep, boost::ratio<3, 2> > >(Rep(3));
        check_from_rep<boost::chrono::duration<double, boost::ratio<2, 3> > >(5.5);
    }
    // constructor from other rep
    {
        boost::chrono::duration<double> d(5);
        BOOST_TEST(d.count() == 5);
        return boost::report_errors();
    }

    return boost::report_errors();
}


#include "drake/multibody/inverse_kinematics/minimum_distance_constraint.h"

#include <limits>
#include <vector>

#include <Eigen/Dense>

#include "drake/multibody/inverse_kinematics/distance_constraint_utilities.h"
#include "drake/multibody/inverse_kinematics/kinematic_constraint_utilities.h"

namespace drake {
namespace multibody {
using internal::RefFromPtrOrThrow;

template <typename T, typename S>
VectorX<S> Distances(const MultibodyPlant<T>& plant,
                     systems::Context<T>* context,
                     const Eigen::Ref<const VectorX<S>>& q,
                     double influence_distance) {
  internal::UpdateContextConfiguration(context, plant, q);
  const auto& query_port = plant.get_geometry_query_input_port();
  if (!query_port.HasValue(*context)) {
    throw std::invalid_argument(
        "MinimumDistanceConstraint: Cannot get a valid geometry::QueryObject. "
        "Either the plant geometry_query_input_port() is not properly "
        "connected to the SceneGraph's output port, or the plant_context_ is "
        "incorrect. Please refer to AddMultibodyPlantSceneGraph on connecting "
        "MultibodyPlant to SceneGraph.");
  }
  const auto& query_object =
      query_port.template Eval<geometry::QueryObject<T>>(*context);

  const std::vector<geometry::SignedDistancePair<T>> signed_distance_pairs =
      query_object.ComputeSignedDistancePairwiseClosestPoints(
          influence_distance);
  VectorX<S> distances(signed_distance_pairs.size());
  int distance_count{0};
  for (const auto& signed_distance_pair : signed_distance_pairs) {
    if (signed_distance_pair.distance < influence_distance) {
      const geometry::SceneGraphInspector<T>& inspector =
          query_object.inspector();
      const geometry::FrameId frame_A_id =
          inspector.GetFrameId(signed_distance_pair.id_A);
      const geometry::FrameId frame_B_id =
          inspector.GetFrameId(signed_distance_pair.id_B);
      const Frame<T>& frameA =
          plant.GetBodyFromFrameId(frame_A_id)->body_frame();
      const Frame<T>& frameB =
          plant.GetBodyFromFrameId(frame_B_id)->body_frame();
      internal::CalcDistanceDerivatives(
          plant, *context, frameA, frameB,
          // GetPoseInFrame() returns RigidTransform<double> -- we can't
          // multiply across heterogeneous scalar types; so we cast the double
          // to T.
          inspector.GetPoseInFrame(signed_distance_pair.id_A)
                  .template cast<T>() *
              signed_distance_pair.p_ACa,
          signed_distance_pair.distance, signed_distance_pair.nhat_BA_W, q,
          &distances(distance_count++));
    }
  }
  distances.resize(distance_count);
  return distances;
}

template <typename T>
void MinimumDistanceConstraint::Initialize(
    const MultibodyPlant<T>& plant, systems::Context<T>* plant_context,
    double minimum_distance, double influence_distance_offset,
    MinimumDistancePenaltyFunction penalty_function) {
  CheckPlantIsConnectedToSceneGraph(plant, *plant_context);
  if (!std::isfinite(influence_distance_offset)) {
    throw std::invalid_argument(
        "MinimumDistanceConstraint: influence_distance_offset must be finite.");
  }
  if (influence_distance_offset <= 0) {
    throw std::invalid_argument(
        "MinimumDistanceConstraint: influence_distance_offset must be "
        "positive.");
  }
  const auto& query_port = plant.get_geometry_query_input_port();
  // Maximum number of SignedDistancePairs returned by calls to
  // ComputeSignedDistancePairwiseClosestPoints().
  const int num_collision_candidates =
      query_port.template Eval<geometry::QueryObject<T>>(*plant_context)
          .inspector()
          .GetCollisionCandidates()
          .size();
  minimum_value_constraint_ = std::make_unique<solvers::MinimumValueConstraint>(
      this->num_vars(), minimum_distance, influence_distance_offset,
      num_collision_candidates,
      [&plant, plant_context](const auto& x, double influence_distance) {
        return Distances<T, AutoDiffXd>(plant, plant_context, x,
                                        influence_distance);
      },
      [&plant, plant_context](const auto& x, double influence_distance) {
        return Distances<T, double>(plant, plant_context, x,
                                    influence_distance);
      });
  this->set_bounds(minimum_value_constraint_->lower_bound(),
                   minimum_value_constraint_->upper_bound());
  if (penalty_function) {
    minimum_value_constraint_->set_penalty_function(penalty_function);
  }
}

MinimumDistanceConstraint::MinimumDistanceConstraint(
    const multibody::MultibodyPlant<double>* const plant,
    double minimum_distance, systems::Context<double>* plant_context,
    MinimumDistancePenaltyFunction penalty_function,
    double influence_distance_offset)
    : solvers::Constraint(1, RefFromPtrOrThrow(plant).num_positions(),
                          Vector1d(0), Vector1d(0)),
      plant_double_{plant},
      plant_context_double_{plant_context},
      plant_autodiff_{nullptr},
      plant_context_autodiff_{nullptr} {
  Initialize(*plant_double_, plant_context_double_, minimum_distance,
             influence_distance_offset, penalty_function);
}

MinimumDistanceConstraint::MinimumDistanceConstraint(
    const multibody::MultibodyPlant<AutoDiffXd>* const plant,
    double minimum_distance, systems::Context<AutoDiffXd>* plant_context,
    MinimumDistancePenaltyFunction penalty_function,
    double influence_distance_offset)
    : solvers::Constraint(1, RefFromPtrOrThrow(plant).num_positions(),
                          Vector1d(0), Vector1d(0)),
      plant_double_{nullptr},
      plant_context_double_{nullptr},
      plant_autodiff_{plant},
      plant_context_autodiff_{plant_context} {
  Initialize(*plant_autodiff_, plant_context_autodiff_, minimum_distance,
             influence_distance_offset, penalty_function);
}

template <typename T>
void MinimumDistanceConstraint::DoEvalGeneric(
    const Eigen::Ref<const VectorX<T>>& x, VectorX<T>* y) const {
  minimum_value_constraint_->Eval(x, y);
}

void MinimumDistanceConstraint::DoEval(
    const Eigen::Ref<const Eigen::VectorXd>& x, Eigen::VectorXd* y) const {
  DoEvalGeneric(x, y);
}

void MinimumDistanceConstraint::DoEval(const Eigen::Ref<const AutoDiffVecXd>& x,
                                       AutoDiffVecXd* y) const {
  DoEvalGeneric(x, y);
}
}  // namespace multibody
}  // namespace drake


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSE2_DIVS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSE2_DIVS_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/toolbox/arithmetic/functions/divs.hpp>
#include <boost/simd/include/functions/simd/group.hpp>
#include <boost/simd/include/functions/simd/split.hpp>
#include <boost/dispatch/meta/upgrade.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divs_, boost::simd::tag::sse2_, (A0)
                            , ((simd_<ints8_<A0>,boost::simd::tag::sse_>))
                              ((simd_<ints8_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::upgrade<A0>::type ivtype;
      ivtype a0l, a0h, a1l, a1h;
      split(a0, a0l, a0h);
      split(a1, a1l, a1h);
      return group( boost::simd::divs(a0l, a1l)
                  , boost::simd::divs(a0h, a1h)
                  );
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divs_, boost::simd::tag::sse2_, (A0)
                            , ((simd_<ints16_<A0>,boost::simd::tag::sse_>))
                              ((simd_<ints16_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::upgrade<A0>::type ivtype;
      ivtype a0l, a0h, a1l, a1h;
      split(a0, a0l, a0h);
      split(a1, a1l, a1h);
      return group( boost::simd::divs(a0l, a1l)
                  , boost::simd::divs(a0h, a1h)
                  );
    }
  };
} } }

#endif
#endif


/**
 *  @file
 *  @copyright defined in eos/LICENSE.txt
 */
#pragma once
#include <vector>
#include <array>
#include <string>
#include <functional>

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/algorithm/string/trim.hpp>

#include <fc/variant.hpp>
#include <fc/crypto/base64.hpp>
#include <fc/crypto/hex.hpp>
#include <fc/crypto/elliptic.hpp>
#include <fc/crypto/sha256.hpp>
#include <fc/io/datastream.hpp>
#include <fc/time.hpp>
#include <fc/fixed_string.hpp>
#include <fc/string.hpp>

#include <fc/reflect/reflect.hpp>

#define N(X) eos::types::string_to_name(#X)

namespace eos { namespace types {
   using namespace boost::multiprecision;

   template<typename... T>
   using Vector       = std::vector<T...>;

   template<typename... T>
   using Array        = std::array<T...>;

   using String        = std::string;
   using Time          = fc::time_point_sec;
   using Signature     = fc::ecc::compact_signature;
   using Checksum      = fc::sha256;
   using FieldName     = fc::fixed_string<>;
   using FixedString32 = fc::fixed_string<fc::array<uint64_t,4>>;// std::tuple<uint64_t,uint64_t,uint64_t,uint64_t>>;
   using FixedString16 = fc::fixed_string<>;
   using TypeName      = FixedString32;;
   using Bytes         = Vector<char>;

   template<size_t Size>
   using UInt = number<cpp_int_backend<Size, Size, unsigned_magnitude, unchecked, void> >;
   template<size_t Size>
   using Int = number<cpp_int_backend<Size, Size, signed_magnitude, unchecked, void> >;

   using UInt8     = UInt<8>;
   using UInt16    = UInt<16>;
   using UInt32    = UInt<32>;
   using UInt64    = UInt<64>;
   using UInt128   = boost::multiprecision::uint128_t;
   using UInt256   = boost::multiprecision::uint256_t;
   using Int8      = int8_t;//Int<8>;  these types are different sizes than native...
   using Int16     = int16_t; //Int<16>;
   using Int32     = int32_t; //Int<32>;
   using Int64     = int64_t; //Int<64>;
   using Int128    = boost::multiprecision::int128_t;
   using Int256    = boost::multiprecision::int256_t;
   using uint128_t = unsigned __int128; /// native clang/gcc 128 intrinisic

   static constexpr char char_to_symbol( char c ) {
      if( c >= 'a' && c <= 'z' )
         return (c - 'a') + 6;
      if( c >= '1' && c <= '5' )
         return (c - '1') + 1;
      return 0;
   }

   static constexpr uint64_t string_to_name( const char* str ) {

      uint32_t len = 0;
      while( str[len] ) ++len;

      uint64_t value = 0;

      for( uint32_t i = 0; i <= 12; ++i ) {
         uint64_t c = 0;
         if( i < len && i <= 12 ) c = char_to_symbol( str[i] );

         if( i < 12 ) {
            c &= 0x1f;
            c <<= 64-5*(i+1);
         }
         else {
            c &= 0x0f;
         }

         value |= c;
      }

      return value;
   }

   struct Name {
      uint64_t value = 0;
      bool valid()const { return true; }
      bool empty()const { return 0 == value; }
      bool good()const  { return !empty() && valid();  }

      Name( const char* str )   { set(str);           }
      Name( const String& str ) { set( str.c_str() ); }

      void set( const char* str ) {
      try {
         const auto len = strnlen(str,14);
         FC_ASSERT( len <= 13 );
         value = string_to_name(str);
         FC_ASSERT( toString() == String(str), "name not properly normalized", ("name",String(str))("normalized",toString())  );
      }FC_CAPTURE_AND_RETHROW( (str) ) }

      Name( uint64_t v = 0 ):value(v){}

      explicit operator String()const {
        static const char* charmap = ".12345abcdefghijklmnopqrstuvwxyz";

         String str(13,'.');

         uint64_t tmp = value;
         for( uint32_t i = 0; i <= 12; ++i ) {
            char c = charmap[tmp & (i == 0 ? 0x0f : 0x1f)];
            str[12-i] = c;
            tmp >>= (i == 0 ? 4 : 5);
         }

         boost::algorithm::trim_right_if( str, []( char c ){ return c == '.'; } );
         return str;
      }
      String toString() const { return String(*this); }

      Name& operator=( uint64_t v ) {
         value = v;
         return *this;
      }

      Name& operator=( const String& n ) {
         value = Name(n).value;
         return *this;
      }
      Name& operator=( const char* n ) {
         value = Name(n).value;
         return *this;
      }

      template<typename Stream>
      friend Stream& operator << ( Stream& out, const Name& n ) {
         return out << String(n);
      }

      friend bool operator < ( const Name& a, const Name& b ) { return a.value < b.value; }
      friend bool operator <= ( const Name& a, const Name& b ) { return a.value <= b.value; }
      friend bool operator > ( const Name& a, const Name& b ) { return a.value > b.value; }
      friend bool operator >=( const Name& a, const Name& b ) { return a.value >= b.value; }
      friend bool operator == ( const Name& a, const Name& b ) { return a.value == b.value; }
      friend bool operator != ( const Name& a, const Name& b ) { return a.value != b.value; }

      operator bool()const            { return value; }
      operator uint64_t()const        { return value; }
   };

   struct Field {
      FieldName name;
      TypeName  type;

      bool operator==(const Field& other) const;
   };

   struct Struct {
      TypeName        name;
      TypeName        base;
      Vector<Field>   fields;

      bool operator==(const Struct& other) const;
   };

   using Fields = Vector<Field>;

   template<typename T>
   struct GetStruct{};

    template<> struct GetStruct<Field> {
        static const Struct& type() {
           static Struct result = { "Field ", "", {
                {"name", "FieldName"},
                {"type", "TypeName"}
              }
           };
           return result;
         }
    };
    template<> struct GetStruct<Struct> {
        static const Struct& type() {
           static Struct result = { "Struct ", "", {
                {"name", "TypeName"},
                {"base", "TypeName"},
                {"fields", "Field[]"}
              }
           };
           return result;
         }
    };

   /// TODO: make sure this works with FC raw
   template<typename Stream, typename Number>
   void fromBinary(Stream& st, boost::multiprecision::number<Number>& value) {
      unsigned char data[(std::numeric_limits<decltype(value)>::digits+1)/8];
      st.read((char*)data, sizeof(data));
      boost::multiprecision::import_bits(value, data, data + sizeof(data), 1);
   }
   template<typename Stream, typename Number>
   void toBinary(Stream& st, const boost::multiprecision::number<Number>& value) {
      unsigned char data[(std::numeric_limits<decltype(value)>::digits+1)/8];
      boost::multiprecision::export_bits(value, data, 1);
      st.write((const char*)data, sizeof(data));
   }

}} // namespace eos::types

namespace fc {
  void to_variant(const eos::types::Name& c, fc::variant& v);
  void from_variant(const fc::variant& v, eos::types::Name& check);
  void to_variant(const std::vector<eos::types::Field>& c, fc::variant& v);
  void from_variant(const fc::variant& v, std::vector<eos::types::Field>& check);
  void to_variant(const std::map<std::string,eos::types::Struct>& c, fc::variant& v);
  void from_variant(const fc::variant& v, std::map<std::string,eos::types::Struct>& check);
}

FC_REFLECT(eos::types::Name, (value))
FC_REFLECT(eos::types::Field, (name)(type))
FC_REFLECT(eos::types::Struct, (name)(base)(fields))


/////////////////////////////////////////////////////////////////////////////////
// distribution::common::distributions::reference::random.hpp                  //
//                                                                             //
//  (C) Copyright 2009 Erwann Rogard                                           //
//  Use, modification and distribution are subject to the                      //
//  Boost Software License, Version 1.0. (See accompanying file                //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)           //
/////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_COMMON_DISTRIBUTIONS_REFERENCE_RANDOM_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_COMMON_DISTRIBUTIONS_REFERENCE_RANDOM_HPP_ER_2009
#include <boost/math/distributions/exponential.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <boost/statistics/detail/distribution_common/meta/random/distribution.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace meta{

    template<typename D>
    struct random_distribution< reference_wrapper<D> >
    {
        typedef typename reference_wrapper<D>::distribution_type dist_;
        typedef random_distribution<dist_> inner_;
        typedef typename inner_::type type;

        static type call(const reference_wrapper<D>& d){
        	return inner_::call(d.distribution());
        }
    };

}// meta
}// distribution
}// detail
}// statistics
}// boost

#endif


//
// Created by nvidia on 6/22/18.
//

//
// Created by nvidia on 6/22/18.
//

#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/ximgproc.hpp>
#include <algorithm> // for copy
#include <iterator> // for ostream_iterator
#include <boost/graph/graph_utility.hpp>
#include "../lib/SpVec.hpp"
#include "../lib/superpixel_seg.hpp"
#include "../lib/distance.hpp"
#include "../lib/FloodFillSsdSuperpixelAlgorithm.hpp"
#include "../lib/RTreesClassifier.hpp"
#include "../lib/util.hpp"

using namespace cv;
using namespace std;
using namespace cv::ximgproc;

using namespace cove;

inline TermCriteria TC(int iters, double eps)
{
    return TermCriteria(TermCriteria::MAX_ITER + (eps > 0 ? TermCriteria::EPS : 0), iters, eps);
}

int main(int argc, char** argv) {
    int idx = 1;
#ifdef DEBUG
    namedWindow("params", 1);
#endif
    ALGORITHM_PARAM(blurKernel, 3, 10)
    ALGORITHM_PARAM(iterations, 10, 20)
    ALGORITHM_PARAM(blurDev, 0, 100)
    ALGORITHM_PARAM(regionSize, 7, 50)
    ALGORITHM_PARAM(ratio, 70, 200)
    ALGORITHM_PARAM(cannyLow, 70, 150)
    ALGORITHM_PARAM(ssdEpsilonSlider, 10, 250)

    int frameno=0;
    Mat frame;
    Mat label;

    FloodFillSsdSuperpixelAlgorithm algorithm(80, 60, regionSize, static_cast<float>(ssdEpsilonSlider) / 10, blurKernel, blurDev, iterations,
                                              static_cast<float>(ratio) / 1000, cannyLow, cannyLow*2.5);

    vector<double> ius;

    Mat samples = Mat::zeros(0, 10, CV_32F);
    Mat responses = Mat::zeros(0, 1, CV_32S);

    for (int idx = 1; idx < argc*0.8; idx++) {
        clock_t start = CLOCK();

#ifdef DEBUG
        if (blurKernel % 2 == 0) {
            blurKernel++;
        }
#endif

        string path(argv[idx]);
        string filename = util::getFileName(path, true);
        string labelPath = "img/labels/" + filename;
        frame = imread(path);
        label = imread(labelPath);
//        SHOW_ALWAYS("frame", frame)
//        SHOW_ALWAYS("label", label)
        if (frame.empty() || label.empty()) {
            cout << "frame empty" << endl;
            break;
        }

        Mat floorMask;
        Mat resizedLabel;
        algorithm.Run(frame, floorMask, resizedLabel);
        resize(label, resizedLabel, resizedLabel.size(), 0, 0, INTER_NEAREST);
//        SHOW_ALWAYS("resize", resizedLabel)
//        SHOW_ALWAYS("calced floor", floorMask)
        Mat carpet;
        Mat hardfloor;
        Mat lab;
        Mat sizedFrame;
        inRange(resizedLabel, Scalar(0, 0, 255), Scalar(0, 0, 255), carpet);
        inRange(resizedLabel, Scalar(0, 255, 0), Scalar(0, 255, 0), hardfloor);

        auto super = algorithm.getSuperPixels(frame, lab, sizedFrame);
        cvtColor(sizedFrame, sizedFrame, COLOR_BGR2Lab);
        super->iterate(iterations);
        super->enforceLabelConnectivity(15);
        Mat spxLabels;
        super->getLabels(spxLabels);
        for (int i = 0; i < super->getNumberOfSuperpixels(); i++) {
            Mat mask;
            inRange(spxLabels, Scalar(i), Scalar(i), mask);
            // classify on label
            int label;
            Mat temp;
            bitwise_and(carpet, mask, temp);
            if (countNonZero(temp) > 7 * 7 / 2) {
                // is carpet
                label = 1;
                goto add_data;
            }
            bitwise_and(hardfloor, mask, temp);
            if (countNonZero(temp) > 7 * 7 / 2) {
                // is hardfloor
                label = 2;
                goto add_data;
            }
            // is bg
            label = 3;
            goto add_data;

            add_data:
            samples.push_back(RTreesClassifier::getData(mask, sizedFrame));;

            Mat thisResponse = Mat::zeros(1, 1, CV_32S);
            thisResponse.at<int>(0, 0) = label;
            responses.push_back(thisResponse);

        }

        // =========== time fps and exit ===========
        double dur = CLOCK() - start;
        printf("avg time per frame %f ms. fps %f. frameno = %d\n", avgdur(dur), avgfps(), frameno++);
        cout << argv[idx] << endl;

    }
    auto data = ml::TrainData::create(samples, ml::ROW_SAMPLE, responses);

    auto rtrees = ml::RTrees::create();
    rtrees->setMaxDepth(20);
    rtrees->setMinSampleCount(10);
    rtrees->setRegressionAccuracy(0);
    rtrees->setUseSurrogates(false);
    rtrees->setMaxCategories(15);
    rtrees->setPriors(Mat());
    rtrees->setCalculateVarImportance(true);
    rtrees->setActiveVarCount(0);
    rtrees->setTermCriteria(TC(100, 0.01f));
    rtrees->train(data);
    rtrees->save("RTREES");

    return 0;
}


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#include <boost/python.hpp>

#include <ndhist/stats/median.hpp>

namespace bp = boost::python;

namespace ndhist {

namespace stats {

void register_median()
{
    bp::def("median"
      , &py::median
      , ( bp::arg("hist")
        , bp::arg("axis")=bp::object()
        )
      , "Calculates the median value along the given axis of the given ndhist\n"
        "object.                                                             \n"
        "It generates a projection along the given axis and then calculates  \n"
        "the median value. The median value is defined as the axis value     \n"
        "where half of the sum of the weights are below and above that value.\n"
        "If ``None`` is given as axis argument (the default), the median     \n"
        "for all individual axes of the ndhist object is calculated and      \n"
        "returned as a tuple. But if the dimensionality of the histogram is  \n"
        "1, a scalar value is returned.                                      \n"
        "\n"
        ".. note:: This function is only defined for ndhist objects with POD \n"
        "          type axis values AND POD type weight values.              \n"
    );
}

}// namespace stats
}// namespace ndhist


#include <vector>
#include <iostream>
#include <Eigen/Core>
#include <pcl/common/time.h>
#include "pcl/point_types.h"
#include "pcl/point_cloud.h"
#include <pcl/correspondence.h>
#include "pcl/io/pcd_io.h"
#include <pcl/search/kdtree.h>
#include "pcl/kdtree/kdtree_flann.h"
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/transforms.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/registration/transformation_estimation_svd_scale.h>
#include <pcl/features/shot_omp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/registration/icp.h>
#include <pcl/features/board.h>
#include <pcl/filters/uniform_sampling.h>
#include <pcl/recognition/cg/hough_3d.h>
#include <pcl/recognition/cg/geometric_consistency.h>
#include <pcl/console/parse.h>
#include <pcl/recognition/hv/hv_go.h>

float cg_size_ (15.0f);
float cg_thresh_ (2.0f);
int icp_max_iter_ (5);
float icp_corr_distance_ (0.01f);
float hv_clutter_reg_ (5.0f);
float hv_inlier_th_ (0.01f);
float hv_occlusion_th_ (0.02f);
float hv_rad_clutter_ (0.05f);
float hv_regularizer_ (3.0f);
float hv_rad_normals_ (0.08);
bool hv_detect_clutter_ (true);

std::string model_filename_;
std::string scene_filename_;

void
showHelp (char *filename)
{
  std::cout << std::endl;
  std::cout << "***************************************************************************" << std::endl;
  std::cout << "*                                                                         *" << std::endl;
  std::cout << "*             Correspondence Grouping Tutorial - Usage Guide              *" << std::endl;
  std::cout << "*                                                                         *" << std::endl;
  std::cout << "***************************************************************************" << std::endl << std::endl;
  std::cout << "Usage: " << filename << " model_filename.pcd scene_filename.pcd [Options]" << std::endl << std::endl;
  std::cout << "Options:" << std::endl;
  std::cout << "     --cg_size val:          Cluster size (default "<< cg_size_ << ")"  << std::endl;
  std::cout << "     --cg_thresh val:        Clustering threshold (default " << cg_thresh_ << ")" << std::endl;
  std::cout << "     --icp_max_iter val:          ICP max iterations number (default " << icp_max_iter_ << ")" << std::endl;
  std::cout << "     --icp_corr_distance val:     ICP correspondence distance (default " << icp_corr_distance_ << ")" << std::endl << std::endl;
  std::cout << "     --hv_clutter_reg val:        Clutter Regularizer (default " << hv_clutter_reg_ << ")" << std::endl;
  std::cout << "     --hv_inlier_th val:          Inlier threshold (default " << hv_inlier_th_ << ")" << std::endl;
  std::cout << "     --hv_occlusion_th val:       Occlusion threshold (default " << hv_occlusion_th_ << ")" << std::endl;
  std::cout << "     --hv_rad_clutter val:        Clutter radius (default " << hv_rad_clutter_ << ")" << std::endl;
  std::cout << "     --hv_regularizer val:        Regularizer value (default " << hv_regularizer_ << ")" << std::endl;
  std::cout << "     --hv_rad_normals val:        Normals radius (default " << hv_rad_normals_ << ")" << std::endl;
  std::cout << "     --hv_detect_clutter val:     TRUE if clutter detect enabled (default " << hv_detect_clutter_ << ")" << std::endl << std::endl;
}

void
downsample (pcl::PointCloud<pcl::PointXYZ>::Ptr &points, float leaf_size,
            pcl::PointCloud<pcl::PointXYZ>::Ptr &downsampled_out)
{
  pcl::VoxelGrid<pcl::PointXYZ> vox_grid;
  vox_grid.setLeafSize (leaf_size, leaf_size, leaf_size);
  vox_grid.setInputCloud (points);
  vox_grid.filter (*downsampled_out);
}

void
compute_surface_normals (pcl::PointCloud<pcl::PointXYZ>::Ptr &points, float normal_radius,
                         pcl::PointCloud<pcl::Normal>::Ptr &normals_out)
{
  pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> norm_est;

  // Use a FLANN-based KdTree to perform neighborhood searches
  //norm_est.setSearchMethod (pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr (new pcl::KdTreeFLANN<pcl::PointXYZ>));
  norm_est.setSearchMethod (pcl::search::KdTree<pcl::PointXYZ>::Ptr (new pcl::search::KdTree<pcl::PointXYZ>));

  // Specify the size of the local neighborhood to use when computing the surface normals
  norm_est.setRadiusSearch (normal_radius);

  // Set the input points
  norm_est.setInputCloud (points);

  // Estimate the surface normals and store the result in "normals_out"
  norm_est.compute (*normals_out);
}

void
compute_SHOT_features (pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
                       pcl::PointCloud<pcl::PointXYZ>::Ptr &points,
                       pcl::PointCloud<pcl::Normal>::Ptr &normals,
                       float feature_radius,
                       pcl::PointCloud<pcl::SHOT352>::Ptr &descriptors_out)
{

  // Create a PFHEstimation object
  pcl::SHOTEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::SHOT352, pcl::ReferenceFrame> shot_est;

  // Set it to use a FLANN-based KdTree to perform its neighborhood searches
  shot_est.setSearchMethod (pcl::search::KdTree<pcl::PointXYZ>::Ptr (new pcl::search::KdTree<pcl::PointXYZ>));

  // Specify the radius of the PFH feature
  shot_est.setRadiusSearch (feature_radius);

  shot_est.setSearchSurface (cloud);

  // Set the input points and surface normals
  shot_est.setInputCloud (points);
  shot_est.setInputNormals (normals);
  std::cout << "start compute SHOT features descriptors" << std::endl;
  // Compute the features
  shot_est.compute (*descriptors_out);
  std::cout << "Compute SHOT descriptors successfully, the number of descriptors is " << descriptors_out->size() << std::endl;
}

void estimation_feature_correspondences_SHOT(pcl::PointCloud<pcl::SHOT352>::Ptr &source_descriptors,
                              pcl::PointCloud<pcl::SHOT352>::Ptr &target_descriptors, pcl::CorrespondencesPtr &correspondences_all)
{
  PCL_INFO("Finding correspondences...\n");
  pcl::registration::CorrespondenceEstimation<pcl::SHOT352, pcl::SHOT352> ce;
  ce.setInputSource (source_descriptors);
  ce.setInputTarget (target_descriptors);
  ce.determineReciprocalCorrespondences (*correspondences_all);
  PCL_INFO("%d correspondences found\n", correspondences_all->size ());
}

void
find_feature_SHOT_correspondences (pcl::PointCloud<pcl::SHOT352>::Ptr &model_descriptors,
                                   pcl::PointCloud<pcl::SHOT352>::Ptr &scene_descriptors,
                                   pcl::CorrespondencesPtr &model_scene_corrs)
{

  pcl::KdTreeFLANN<pcl::SHOT352> match_search;
  match_search.setInputCloud (model_descriptors);

  //  For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector.
  for (size_t i = 0; i < scene_descriptors->size (); ++i)
  {
    std::vector<int> neigh_indices (1);
    std::vector<float> neigh_sqr_dists (1);
    if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs
    {
      continue;
    }
    int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
    if(found_neighs == 1 && neigh_sqr_dists[0] < 0.20f) //  add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
    {
      pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
      model_scene_corrs->push_back (corr);
    }
  }
  std::cout << "SHOT Correspondences found: " << model_scene_corrs->size () << std::endl;

}

void rejection_correspondences(pcl::PointCloud<pcl::PointXYZ>::Ptr &points1, pcl::PointCloud<pcl::PointXYZ>::Ptr &points2,
                               pcl::CorrespondencesPtr &correspondences_all, pcl::CorrespondencesPtr &correspondences_inliers)
{

  pcl::CorrespondencesPtr correspondences_x (new pcl::Correspondences);
  pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZ> sac;
  sac.setInputSource (points1);
  sac.setInputTarget (points2);
  sac.setInlierThreshold (0.1);
  sac.setMaximumIterations (1000);
  sac.setInputCorrespondences (correspondences_all);
  sac.getCorrespondences (*correspondences_inliers);

  PCL_INFO("%d inliers correspondences\n", correspondences_inliers->size ());
/*
  for (size_t i=0; i < correspondences_x -> size (); i++){
    if (correspondences_x ->at(i).distance < 50.0f) {
      correspondences_inliers->push_back(correspondences_x->at(i));
    }
  }

  PCL_INFO("%d inliers correspondences\n", correspondences_inliers->size ());
  for(size_t i =0; i< correspondences_inliers->size ();i++){
    std::cerr<<"the linliers correspondences"<< correspondences_inliers->at(i)<<std::endl;
  }
*/
}

void rf_calculation  (pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
                      pcl::PointCloud<pcl::PointXYZ>::Ptr &points,
                      pcl::PointCloud<pcl::Normal>::Ptr &normals,
                      pcl::PointCloud<pcl::ReferenceFrame>::Ptr &reference_frame)
{
  pcl::BOARDLocalReferenceFrameEstimation<pcl::PointXYZ, pcl::Normal, pcl::ReferenceFrame> rf_est;
  rf_est.setFindHoles (true);
  rf_est.setRadiusSearch (0.05f);

  rf_est.setInputCloud (points);
  rf_est.setInputNormals (normals);
  rf_est.setSearchSurface (cloud);
  rf_est.compute (*reference_frame);
}

void Hough_groupping  (float &cg_size,
                       float &cg_thresh,
                       pcl::PointCloud<pcl::PointXYZ>::Ptr &model,
                       pcl::PointCloud<pcl::PointXYZ>::Ptr &scene,
                       pcl::PointCloud<pcl::PointXYZ>::Ptr &model_keypoints,
                       pcl::PointCloud<pcl::PointXYZ>::Ptr &scene_keypoints,
                       pcl::PointCloud<pcl::ReferenceFrame>::Ptr &model_rf,
                       pcl::PointCloud<pcl::ReferenceFrame>::Ptr &scene_rf,
                       pcl::CorrespondencesPtr &model_scene_corrs,
                       pcl::CorrespondencesPtr & group_corrs,
                       Eigen::Matrix4f & transforms_hough)
{

  std::cout << "cg_size: " << cg_size << "  cg_thresh: " << cg_thresh << std::endl;

    //  Clustering
  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
  std::vector<pcl::Correspondences> clustered_corrs;

  pcl::Hough3DGrouping<pcl::PointXYZ, pcl::PointXYZ, pcl::ReferenceFrame, pcl::ReferenceFrame> clusterer;
  clusterer.setHoughBinSize (cg_size);//25
  clusterer.setHoughThreshold (cg_thresh);//15
  clusterer.setUseInterpolation (true);
  clusterer.setUseDistanceWeight (false);

  clusterer.setInputCloud (model_keypoints);
  clusterer.setInputRf (model_rf);
  clusterer.setSceneCloud (scene_keypoints);
  clusterer.setSceneRf (scene_rf);
  clusterer.setModelSceneCorrespondences (model_scene_corrs);

    //clusterer.cluster (clustered_corrs);
  clusterer.recognize (transformations, clustered_corrs);
  std::cout << "Model instances found: " << transformations.size () << std::endl;

  int num_cor=0;
  int no_max=0;

  for (size_t i = 0; i < transformations.size(); i++)
  {
    std::cout << "Instance " << (i + 1) << ":" << std::endl;
    std::cout << "\tHas " << clustered_corrs[i].size() << " correspondences." << std::endl << std::endl;

    if(num_cor <= clustered_corrs[i].size ())
    {
      num_cor = clustered_corrs[i].size ();
      no_max = i;
    }

    Eigen::Matrix3f rotation = transformations[i].block<3, 3>(0, 0);
    Eigen::Vector3f translation = transformations[i].block<3, 1>(0, 3);
    printf("\t\t    | %6.3f %6.3f %6.3f | \n", rotation(0, 0), rotation(0, 1), rotation(0, 2));
    printf("\t\tR = | %6.3f %6.3f %6.3f | \n", rotation(1, 0), rotation(1, 1), rotation(1, 2));
    printf("\t\t    | %6.3f %6.3f %6.3f | \n", rotation(2, 0), rotation(2, 1), rotation(2, 2));
    std::cout << std::endl;
    printf("\t\tt = < %0.3f, %0.3f, %0.3f >\n", translation(0), translation(1), translation(2));

    std::cout << "\t We choice this one :  Index "<< no_max<<" has" << clustered_corrs[no_max].size() << " correspondences." << std::endl << std::endl;

    Eigen::Matrix4f transforms_hough_;
    pcl::CorrespondencesPtr group_corrs_ (new pcl::Correspondences);

    transforms_hough_ = transformations[i];//.block<4,4>(0, 0);
    for (size_t j = 0; j < clustered_corrs[i].size (); ++j)
      group_corrs_->push_back(clustered_corrs[i][j]);

    pcl::PointCloud<pcl::PointXYZ>::Ptr model_keypoints_trans (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud  (*model_keypoints, *model_keypoints_trans, transforms_hough_);
    // Show grouping
    pcl::visualization::PCLVisualizer visu("every grouping");
    visu.addPointCloud (model_keypoints_trans, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> (model_keypoints_trans, 255.0, 0.0, 0.0), "model");
    visu.addPointCloud (scene_keypoints, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> (scene_keypoints, 255.0, 255.0, 255.0), "scene_keypoints");
    visu.spin ();

  }
  transforms_hough = transformations[no_max];//.block<4,4>(0, 0);
  for (size_t j = 0; j < clustered_corrs[no_max].size (); ++j)
    group_corrs->push_back(clustered_corrs[no_max][j]);

  std::vector<pcl::PointCloud<pcl::PointXYZ>::ConstPtr> instances;
  for (size_t i = 0; i < transformations.size(); ++i)
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr rotated_model (new pcl::PointCloud<pcl::PointXYZ> ());
    pcl::transformPointCloud (*model, *rotated_model, transformations[i]);
    instances.push_back (rotated_model);
  }

  /**
   * ICP
   */
  std::vector<pcl::PointCloud<pcl::PointXYZ>::ConstPtr> registered_instances;
  if (true)
  {
    cout << "--- ICP ---------" << endl;

    for (size_t i = 0; i < transformations.size (); ++i)
    {
      pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
      icp.setMaximumIterations (icp_max_iter_);
      icp.setMaxCorrespondenceDistance (icp_corr_distance_);
      icp.setInputTarget (scene);
      icp.setInputSource (instances[i]);
      pcl::PointCloud<pcl::PointXYZ>::Ptr registered (new pcl::PointCloud<pcl::PointXYZ>);
      icp.align (*registered);
      registered_instances.push_back (registered);
      cout << "Instance " << i << " ";
      if (icp.hasConverged ())
      {
        cout << "Aligned!" << endl;
      }
      else
      {
        cout << "Not Aligned!" << endl;
      }
    }

    cout << "-----------------" << endl << endl;
  }
/***

   // Hypothesis Verification

  cout << "--- Hypotheses Verification ---" << endl;
  std::vector<bool> hypotheses_mask;  // Mask Vector to identify positive hypotheses

  pcl::GlobalHypothesesVerification<pcl::PointXYZ, pcl::PointXYZ> GoHv;

  GoHv.setSceneCloud (scene);  // Scene Cloud
  GoHv.addModels (registered_instances, true);  //Models to verify
  GoHv.addCompleteModels(registered_instances);

  GoHv.setInlierThreshold (hv_inlier_th_);
  GoHv.setOcclusionThreshold (hv_occlusion_th_);
  GoHv.setRegularizer (hv_regularizer_);
  GoHv.setRadiusClutter (hv_rad_clutter_);
  GoHv.setClutterRegularizer (hv_clutter_reg_);
  GoHv.setDetectClutter (hv_detect_clutter_);
  GoHv.setRadiusNormals (hv_rad_normals_);

  GoHv.verify ();
  GoHv.getMask (hypotheses_mask);  // i-element TRUE if hvModels[i] verifies hypotheses

  for (int i = 0; i < hypotheses_mask.size (); i++)
  {
    if (hypotheses_mask[i])
    {
      cout << "Instance " << i << " is GOOD! <---" << endl;
    }
    else
    {
      cout << "Instance " << i << " is bad!" << endl;
    }
  }
  cout << "-------------------------------" << endl;
*/

}

void geometric_consistency (float &cg_size,
                            float &cg_thresh,
                            pcl::PointCloud<pcl::PointXYZ>::Ptr &model_keypoints,
                            pcl::PointCloud<pcl::PointXYZ>::Ptr &scene_keypoints,
                            pcl::CorrespondencesPtr &model_scene_corrs,
                            pcl::CorrespondencesPtr & group_corrs,
                            Eigen::Matrix4f & transforms_geo)

{

  std::cout << "cg_size: " << cg_size << "cg_thresh: " << cg_thresh << std::endl;
  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
  std::vector<pcl::Correspondences> clustered_corrs;

  pcl::GeometricConsistencyGrouping<pcl::PointXYZ, pcl::PointXYZ> gc_clusterer;
  gc_clusterer.setGCSize (cg_size);
  gc_clusterer.setGCThreshold (cg_thresh);

  gc_clusterer.setInputCloud (model_keypoints);
  gc_clusterer.setSceneCloud (scene_keypoints);
  gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);

  //gc_clusterer.cluster (clustered_corrs);
  gc_clusterer.recognize (transformations, clustered_corrs);

  int num_cor=0;
  int no_max=0;

  std::cout << "Model instances found: " << transformations.size () << std::endl;
  for (size_t i = 0; i < transformations.size(); i++)
  {

    if(num_cor <= clustered_corrs[i].size ()) {
      num_cor = clustered_corrs[i].size ();
      no_max = i;
    }

    std::cout << "Instance " << (i + 1) << ":" << std::endl;
    std::cout << "\tHas " << clustered_corrs[i].size() << " correspondences." << std::endl << std::endl;

    Eigen::Matrix3f rotation = transformations[i].block<3, 3>(0, 0);
    Eigen::Vector3f translation = transformations[i].block<3, 1>(0, 3);
    printf("\t\t    | %6.3f %6.3f %6.3f | \n", rotation(0, 0), rotation(0, 1), rotation(0, 2));
    printf("\t\tR = | %6.3f %6.3f %6.3f | \n", rotation(1, 0), rotation(1, 1), rotation(1, 2));
    printf("\t\t    | %6.3f %6.3f %6.3f | \n", rotation(2, 0), rotation(2, 1), rotation(2, 2));
    std::cout << std::endl;
    printf("\t\tt = < %0.3f, %0.3f, %0.3f >\n", translation(0), translation(1), translation(2));
    std::cout << "\t We choice this one :  Index "<< no_max<<" has" << clustered_corrs[no_max].size() << " correspondences." << std::endl << std::endl;

    Eigen::Matrix4f transforms_geo_;
    pcl::CorrespondencesPtr group_corrs_ (new pcl::Correspondences);

    transforms_geo_ = transformations[i].block<4,4>(0, 0);
    for (size_t j = 0; j < clustered_corrs[i].size (); ++j)
      group_corrs_->push_back(clustered_corrs[i][j]);

    pcl::PointCloud<pcl::PointXYZ>::Ptr model_keypoints_trans (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud  (*model_keypoints, *model_keypoints_trans, transforms_geo_);
    // Show grouping
    pcl::visualization::PCLVisualizer visu("every grouping");
    visu.addPointCloud (model_keypoints_trans, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> (model_keypoints_trans, 255.0, 0.0, 0.0), "model");
    visu.addPointCloud (scene_keypoints, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> (scene_keypoints, 255.0, 255.0, 255.0), "scene_keypoints");
    visu.spin ();

    transforms_geo = transformations[no_max].block<4,4>(0, 0);
    for (size_t j = 0; j < clustered_corrs[no_max].size (); ++j)
      group_corrs->push_back(clustered_corrs[no_max][j]);
  }
/*
  int num_cor=0;
  int no_max=0;

  for (size_t i = 0; i < transformations.size(); i++)
  {
    std::cout << "Instance " << (i + 1) << ":" << std::endl;
    std::cout << "\tHas " << clustered_corrs[i].size() << " correspondences." << std::endl << std::endl;

    if(num_cor <= clustered_corrs[i].size ()) {
      num_cor = clustered_corrs[i].size ();
      no_max = i;
    }

    Eigen::Matrix3f rotation = transformations[i].block<3, 3>(0, 0);
    Eigen::Vector3f translation = transformations[i].block<3, 1>(0, 3);
    printf("\t\t    | %6.3f %6.3f %6.3f | \n", rotation(0, 0), rotation(0, 1), rotation(0, 2));
    printf("\t\tR = | %6.3f %6.3f %6.3f | \n", rotation(1, 0), rotation(1, 1), rotation(1, 2));
    printf("\t\t    | %6.3f %6.3f %6.3f | \n", rotation(2, 0), rotation(2, 1), rotation(2, 2));
    std::cout << std::endl;
    printf("\t\tt = < %0.3f, %0.3f, %0.3f >\n", translation(0), translation(1), translation(2));
  }

  std::cout << "\t We choice this one :  Index "<< no_max<<" has" << clustered_corrs[no_max].size() << " correspondences." << std::endl << std::endl;

  transforms_geo = transformations[no_max].block<4,4>(0, 0);
  for (size_t j = 0; j < clustered_corrs[no_max].size (); ++j)
    group_corrs->push_back(clustered_corrs[no_max][j]);

*/

}

void translation_only (pcl::PointCloud<pcl::PointXYZ>::Ptr &points1,
                       pcl::PointCloud<pcl::PointXYZ>::Ptr &points2,
                       pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud1,
                       pcl::PointCloud<pcl::PointXYZ>::Ptr &points1_trans,
                       pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud1_trans)
{

//  Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
  Eigen::Vector4f centroid1;
  pcl::compute3DCentroid (*points1, centroid1);
  Eigen::Vector4f centroid2;
  pcl::compute3DCentroid (*points2, centroid2);
  Eigen::Vector4f translation_ = centroid2 - centroid1;

  Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();

  // Define a translation of 2.5 meters on the x axis.
  transform_2.translation() << translation_[0], translation_[1],translation_[2];

  pcl::transformPointCloud (*points1, *points1_trans, transform_2);
  pcl::transformPointCloud (*cloud1, *cloud1_trans, transform_2);

}

void calculate_transformation (pcl::PointCloud<pcl::PointXYZ>::Ptr &points1,
                               pcl::PointCloud<pcl::PointXYZ>::Ptr &points2,
                               pcl::CorrespondencesPtr &correspondences_inliers,
                               pcl::PointCloud<pcl::PointXYZ>::Ptr &points1_trans)
{
/*
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);

  for (size_t i = 0; i < correspondences_inliers->size (); ++i)
  {
     cloud_in->push_back(points1-> points[correspondences_inliers->at(i).index_query]);
     cloud_out->push_back(points1-> points[correspondences_inliers->at(i).index_query]);
  }

  pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ> trans_SVD;
  pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ>::Matrix4 transformation2;
  trans_SVD.estimateRigidTransformation (*cloud_in,*cloud_out,transformation2);
*/

  pcl::registration::TransformationEstimationSVDScale<pcl::PointXYZ,pcl::PointXYZ> trans_SVD;
  pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ>::Matrix4 transformation2;
  trans_SVD.estimateRigidTransformation (*points1,*points2, *correspondences_inliers, transformation2);

  std::cout << "The Estimated Rotation and translation matrices (using getTransformation function) are : \n" << std::endl;
  printf ("\n");
  printf ("    | %6.3f %6.3f %6.3f %6.3f| \n", transformation2 (0,0), transformation2 (0,1), transformation2 (0,2), transformation2 (0,3));
  printf ("R = | %6.3f %6.3f %6.3f %6.3f| \n", transformation2 (1,0), transformation2 (1,1), transformation2 (1,2), transformation2 (1,3));
  printf ("    | %6.3f %6.3f %6.3f %6.3f| \n", transformation2 (2,0), transformation2 (2,1), transformation2 (2,2), transformation2 (2,3));
  printf ("    | %6.3f %6.3f %6.3f %6.3f| \n", transformation2 (3,0), transformation2 (3,1), transformation2 (3,2), transformation2 (3,3));
  printf ("\n");
  printf ("t = < %0.3f, %0.3f, %0.3f >\n", transformation2 (0,3), transformation2 (1,3), transformation2 (2,3));
  PCL_INFO("start transform\n");
  pcl::transformPointCloud (*points1, *points1_trans, transformation2);

}

void alignement (pcl::PointCloud<pcl::PointXYZ>::Ptr &points1,
                 pcl::PointCloud<pcl::PointXYZ>::Ptr &points2,
                 pcl::PointCloud<pcl::SHOT352>::Ptr &points1_descriptors,
                 pcl::PointCloud<pcl::SHOT352>::Ptr &points2_descriptors,
                 pcl::PointCloud<pcl::PointXYZ>::Ptr &object_aligned )
{
  const float leaf = 1.6f;
  pcl::console::print_highlight ("Starting alignment...\n");
  pcl::SampleConsensusPrerejective<pcl::PointXYZ, pcl::PointXYZ, pcl::SHOT352> align;
  align.setInputSource (points1);
  align.setSourceFeatures (points1_descriptors);
  align.setInputTarget (points2);
  align.setTargetFeatures (points2_descriptors);
  align.setMaximumIterations (50000); // Number of RANSAC iterations
  align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose
  align.setCorrespondenceRandomness (5); // Number of nearest features to use
  align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold
  align.setMaxCorrespondenceDistance (2.5f * leaf); // Inlier threshold
  align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis
  {
    pcl::ScopeTime t("Alignment");
    align.align (*object_aligned);
  }

  if (align.hasConverged ())
  {
    // Print results
    printf ("\n");
    Eigen::Matrix4f transformation = align.getFinalTransformation ();
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
    pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
    pcl::console::print_info ("\n");
    pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
    pcl::console::print_info ("\n");
    pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), points1->size ());

    // Show alignment
    pcl::visualization::PCLVisualizer visu("Alignment");
    visu.addPointCloud (points2, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> (points2, 0.0, 255.0, 0.0), "model");
    visu.addPointCloud (object_aligned, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
    visu.spin ();
  }
  else
  {
    pcl::console::print_error ("Alignment failed!\n");
  }

}

pcl::PointCloud<pcl::PointXYZ>::Ptr runICP(pcl::PointCloud<pcl::PointXYZ>::Ptr irCloud,
                                           pcl::PointCloud<pcl::PointXYZ>::Ptr eoCloud,
                                           Eigen::Matrix4f& transform) {

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

  // Set the input source and target
  icp.setInputSource(irCloud);
  icp.setInputTarget(eoCloud);

  // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
//  icp.setMaxCorrespondenceDistance (0.1);
  // Set the maximum number of iterations (criterion 1)
  icp.setMaximumIterations (5000);
  // Set the transformation epsilon (criterion 2)
//  icp.setTransformationEpsilon (1e-8);
  // Set the euclidean distance difference epsilon (criterion 3)
//  icp.setEuclideanFitnessEpsilon (1);

  // Perform the alignment
  pcl::PointCloud<pcl::PointXYZ>::Ptr registered(new pcl::PointCloud<pcl::PointXYZ>());
  icp.align (*registered);
  cout<<"ICP has converged:"<<icp.hasConverged()<<" score: "<<icp.getFitnessScore()<<endl;

  // Obtain the transformation that aligned cloud_source to cloud_source_registered
  //Eigen::Matrix4f transformation = icp.getFinalTransformation ();
  transform = icp.getFinalTransformation ();

  //report transform and error
  return registered;
}

void visualize_correspondences (const pcl::PointCloud<pcl::PointXYZ>::Ptr points1,
                                const pcl::PointCloud<pcl::PointXYZ>::Ptr points2,
                                const pcl::CorrespondencesPtr &correspondences_all)
{
  // We want to visualize two clouds side-by-side, so do to this, we'll make copies of the clouds and transform them
  // by shifting one to the left and the other to the right.  Then we'll draw lines between the corresponding points

  // Create some new point clouds to hold our transformed data
  pcl::PointCloud<pcl::PointXYZ>::Ptr points_left (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr points_right (new pcl::PointCloud<pcl::PointXYZ>);

  // Shift the first clouds' points to the left
  //const Eigen::Vector3f translate (0.0, 0.0, 0.3);
  const Eigen::Vector3f translate (0.5, 0.0, 0.0);
  const Eigen::Quaternionf no_rotation (0, 0, 0, 0);

  pcl::transformPointCloud (*points1, *points_left, -translate, no_rotation);

  // Shift the second clouds' points to the right
  pcl::transformPointCloud (*points2, *points_right, translate, no_rotation);

  // Add the clouds to the vizualizer
  pcl::visualization::PCLVisualizer viz;
  viz.addPointCloud (points_left, "points_left");
  viz.addPointCloud (points_right, "points_right");

  // Draw lines between the best corresponding points
  PCL_INFO("%d correspondences found\n", correspondences_all->size ());

  for (size_t i = 0; i < correspondences_all->size (); ++i)
  {

    // Get the pair of points
    const pcl::PointXYZ & p_left = points_left-> points[correspondences_all->at(i).index_query];
    const pcl::PointXYZ & p_right = points_right->points[correspondences_all->at(i).index_match];

    // Generate a random (bright) color
    double r = (rand() % 100);
    double g = (rand() % 100);
    double b = (rand() % 100);
    double max_channel = std::max (r, std::max (g, b));
    r /= max_channel;
    g /= max_channel;
    b /= max_channel;

    // Generate a unique string for each line
    std::stringstream ss ("line");
    ss << i;
    // Draw the line
//    if (i % 7 == 0){
      viz.addLine<pcl::PointXYZ, pcl::PointXYZ> (p_left, p_right, r, g, b, ss.str ());
//    }
  }

  // Give control over to the visualizer
  viz.spin ();
}

void visualize_transformation(const pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud,
                              const pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud)
{

  printf(  "\nPoint cloud colors :  white  = original point cloud\n"
      "                        red  = transformed point cloud\n");

  pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");

   // Define R,G,B colors for the point cloud
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255);
  // We add the point cloud to the viewer and pass the color handler
  viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");

  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
  viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");

  viewer.addCoordinateSystem (1.0, "cloud", 0);
  viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
  //viewer.setPosition(800, 400); // Setting visualiser window position

  while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
    viewer.spinOnce ();
  }

}

void
parseCommandLine (int argc, char *argv[])
{
  //Show help
  if (pcl::console::find_switch (argc, argv, "-h"))
  {
    showHelp (argv[0]);
    exit (0);
  }

  //Model & scene filenames
  std::vector<int> filenames;
  filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
  if (filenames.size () != 2)
  {
    std::cout << "Filenames missing.\n";
    showHelp (argv[0]);
    exit (-1);
  }

  model_filename_ = argv[filenames[0]];
  scene_filename_ = argv[filenames[1]];

  //General parameters
  pcl::console::parse_argument (argc, argv, "--cg_size", cg_size_);
  pcl::console::parse_argument (argc, argv, "--cg_thresh", cg_thresh_);
  pcl::console::parse_argument (argc, argv, "--icp_max_iter", icp_max_iter_);
  pcl::console::parse_argument (argc, argv, "--icp_corr_distance", icp_corr_distance_);
  pcl::console::parse_argument (argc, argv, "--hv_clutter_reg", hv_clutter_reg_);
  pcl::console::parse_argument (argc, argv, "--hv_inlier_th", hv_inlier_th_);
  pcl::console::parse_argument (argc, argv, "--hv_occlusion_th", hv_occlusion_th_);
  pcl::console::parse_argument (argc, argv, "--hv_rad_clutter", hv_rad_clutter_);
  pcl::console::parse_argument (argc, argv, "--hv_regularizer", hv_regularizer_);
  pcl::console::parse_argument (argc, argv, "--hv_rad_normals", hv_rad_normals_);
  pcl::console::parse_argument (argc, argv, "--hv_detect_clutter", hv_detect_clutter_);
}

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

  parseCommandLine (argc, argv);
  static double first = pcl::getTime();
  // Create some new point clouds to hold our data
  pcl::PointCloud<pcl::PointXYZ>::Ptr points1 (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled1 (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::Normal>::Ptr normals_all1 (new pcl::PointCloud<pcl::Normal>);
  pcl::PointCloud<pcl::Normal>::Ptr normals1 (new pcl::PointCloud<pcl::Normal>);
  pcl::PointCloud<pcl::SHOT352>::Ptr descriptors_shot1 (new pcl::PointCloud<pcl::SHOT352>);

  pcl::PointCloud<pcl::PointXYZ>::Ptr points2 (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled2 (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::Normal>::Ptr normals_all2 (new pcl::PointCloud<pcl::Normal>);
  pcl::PointCloud<pcl::Normal>::Ptr normals2 (new pcl::PointCloud<pcl::Normal>);
  pcl::PointCloud<pcl::SHOT352>::Ptr descriptors_shot2 (new pcl::PointCloud<pcl::SHOT352>);

  pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled1_trans (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr object_aligned (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled2_trans (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::Normal>::Ptr normals1_trans (new pcl::PointCloud<pcl::Normal>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled2_move (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr points2_move (new pcl::PointCloud<pcl::PointXYZ>);

  pcl::CorrespondencesPtr correspondences_all (new pcl::Correspondences);
  pcl::CorrespondencesPtr correspondences_inliers (new pcl::Correspondences);

  pcl::CorrespondencesPtr correspondences_hough (new pcl::Correspondences);
  pcl::CorrespondencesPtr correspondences_inliers_ (new pcl::Correspondences);

  pcl::PointCloud<pcl::ReferenceFrame>::Ptr model_rf (new pcl::PointCloud<pcl::ReferenceFrame> ());
  pcl::PointCloud<pcl::ReferenceFrame>::Ptr scene_rf (new pcl::PointCloud<pcl::ReferenceFrame> ());
  Eigen::Matrix4f group_transform;

  //  Load clouds
  if (pcl::io::loadPCDFile (model_filename_, *points1) < 0)
  {
    std::cout << "Error loading model cloud." << std::endl;
    showHelp (argv[0]);
    return (-1);
  }
  if (pcl::io::loadPCDFile (scene_filename_, *points2) < 0)
  {
    std::cout << "Error loading scene cloud." << std::endl;
    showHelp (argv[0]);
    return (-1);
  }

  std::cout << "load  " << model_filename_
            << "and " << scene_filename_ << "successfully" << std::endl;

  // Downsample the cloud
  const float voxel_grid_leaf_size1 = 6.0;
  const float voxel_grid_leaf_size2 = 4.0;
  downsample (points1, voxel_grid_leaf_size1, downsampled1);
  downsample (points2, voxel_grid_leaf_size2, downsampled2);

  std::cout << "downsample points1 :" << downsampled1->size () << "/ "<<points1->size () <<"total points." << std::endl;
  std::cout << "downsample points2 :" << downsampled2->size () << "/ "<<points2->size () <<"total points." << std::endl;

  translation_only(downsampled2, downsampled1, points2, downsampled2_move, points2_move);

  // Compute surface normals
  const float normal_radius1 = 7.0;
  const float normal_radius2 = 5.0;
//  compute_surface_normals (downsampled1, normal_radius1, normals1);
//  compute_surface_normals (downsampled2, normal_radius2, normals2);
  compute_surface_normals (points1, normal_radius1, normals1);
  compute_surface_normals (points2_move, normal_radius2, normals2);

  // Compute SHOT features
  const float feature_radius1 = 7.0;
  const float feature_radius2 = 5.0;

  std::cout << "get ready !!!!!" << std::endl;

  compute_SHOT_features (points1, downsampled1, normals1, feature_radius1, descriptors_shot1);
  compute_SHOT_features (points2_move, downsampled2_move, normals2, feature_radius2, descriptors_shot2);

  std::cout << "compute feutures successfully" << std::endl;

//  find_feature_SHOT_correspondences (descriptors_shot1, descriptors_shot2, correspondences_all);
  find_feature_SHOT_correspondences (descriptors_shot2, descriptors_shot1, correspondences_all);

  rf_calculation(points1, downsampled1, normals1, scene_rf);
  rf_calculation(points2_move, downsampled2_move, normals2, model_rf);

  Hough_groupping(cg_size_, cg_thresh_, points2_move, points1, downsampled2_move, downsampled1, model_rf, scene_rf, correspondences_all, correspondences_hough, group_transform);
//  geometric_consistency(cg_size_, cg_thresh_, downsampled2, downsampled1, correspondences_all, correspondences_hough, group_transform);

//  estimation_feature_correspondences_SHOT(descriptors_shot1, descriptors_shot2, correspondences_all);
//  rejection_correspondences(downsampled1, downsampled2, correspondences_all, correspondences_inliers);

//  std::cout << "find correspondences successfully" << std::endl;

  // Visualize the two point clouds and their feature correspondences
//  visualize_correspondences (downsampled1, downsampled2, correspondences_inliers);
  static double last = pcl::getTime();
  std::cerr << "Using time : " << (last-first) << std::endl;
  visualize_correspondences (downsampled2_move, downsampled1, correspondences_hough);

//  calculate_transformation (downsampled2_move, downsampled1, correspondences_hough, downsampled2_trans);

  pcl::transformPointCloud (*downsampled2_move, *downsampled2_trans, group_transform);

//  calculate_transformation (downsampled1, downsampled2, correspondences_inliers, downsampled1_trans);

  // Visualize the superposition of point clouds and the another transformed point clouds
  visualize_transformation (downsampled1, downsampled2_trans);
/*
  pcl::PointCloud<pcl::PointXYZ>::Ptr finalpoints (new pcl::PointCloud<pcl::PointXYZ>);
  Eigen::Matrix4f transform;
  finalpoints = runICP(downsampled2_trans, downsampled1, transform);

  visualize_transformation (downsampled1, finalpoints);
*/

  return (0);
}


/*===========================================================================*\

author: Matthias W. Smith
email:  mwsmith2@uw.edu
date:   2014/12/02

notes: Tests the techniques used to estimate the fit errors on the
      frequency extraction.

usage:

./error_estimator_test [<output_file>]

The parameters in brackets are optional.  The default output is
error_estimator_data.csv.

\*===========================================================================*/

//--- std includes ------------ ----------------------------------------------//
#include <iostream>
#include <fstream>
#include <vector>
#include <random>
using std::cout;
using std::endl;
using std::string;

//--- other includes --------------------------------------------------------//
#include <boost/filesystem.hpp>

//--- project includes ------------------------------------------------------//
#include "fid.h"

using namespace fid;

int main(int argc, char **argv)
{
  // Some necessary parameters
  int rounds = 10;

  std::vector<double> wf;
  std::vector<double> tm = time_vector();

  // Ouput filestream.
  std::ofstream out;
  out.precision(12);

  // now get optional output file
  if (argc > 1) {

    out.open(argv[1]);

  } else {

    out.open("data/error_estimator_data.csv");
  }

  // Create random number engine/distribution.
  std::default_random_engine gen;
  std::uniform_real_distribution<double> rand_flat_dist(35.0, 40.0);

  FidFactory ff;
  ff.SetSignalToNoise(100 * 100);

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

    // Make ideal Fid waveform
    ff.SetFidFreq(rand_flat_dist(gen));
    ff.IdealFid(wf, tm);

    Fid myfid(wf, tm);
    out << ff.freq() << "," << 0.0 << ",";
    myfid.WriteMethodCsv(out);
  }

  out.close();

  return 0;
}


//  (C) Copyright John Maddock 2000.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org for most recent version including documentation.

#include <climits>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>

template <class UnsignedInt>
class myclass
{
private:
   BOOST_STATIC_ASSERT(sizeof(UnsignedInt) * CHAR_BIT >= 16);
   BOOST_STATIC_ASSERT(std::numeric_limits<UnsignedInt>::is_specialized
                        && std::numeric_limits<UnsignedInt>::is_integer
                        && !std::numeric_limits<UnsignedInt>::is_signed);
public:
   /* details here */
};

myclass<int>           m2; // this should fail
myclass<unsigned char> m3; // and so should this

int main()
{
   return 0;
}


#include <iostream>
#include <cassert>
#include <boost/core/demangle.hpp>

#include <boost/safe_numerics/safe_integer.hpp>
#include <boost/safe_numerics/safe_integer_range.hpp>
#include <boost/safe_numerics/automatic.hpp>
#include <boost/safe_numerics/utility.hpp>

int test_log(){
    using namespace boost::safe_numerics::utility;
    assert(ilog2(127u) == 6);
    assert(ilog2(128u) == 7);
    assert(ilog2(129u) == 7);
    assert(ilog2(255u) == 7);
    assert(ilog2(256u) == 8);

    assert(ilog2(127) == 6);
    assert(ilog2(128) == 7);
    assert(ilog2(129) == 7);
    assert(ilog2(255) == 7);
    assert(ilog2(256) == 8);
    return 0;
}

template<class T>
void print_argument_type(const T & t){
    const std::type_info & ti = typeid(T);
    std::cout
        << boost::core::demangle(ti.name()) << ' ' << t << std::endl;
}

template<typename T, typename U>
int test_auto(const T & t, const U & u){
    using namespace boost::safe_numerics;

    try{
        safe<T, automatic>(t) + u;
    }
    catch(std::exception e){
        safe<T, automatic>(t) + u;
    }

    try{
        t + safe<U, automatic>(u);
    }
    catch(std::exception e){
        t + safe<U, automatic>(u);
    }
    return 0;
}

int test_auto_result(){
    using namespace boost::safe_numerics;
    automatic::addition_result<
        safe<std::int8_t, automatic>,
        safe<std::uint16_t, automatic>
    >::type r1;
    print_argument_type(r1);

    automatic::addition_result<
        safe_signed_range<-3, 8, automatic>,
        safe_signed_range<-4, 9, automatic>
    >::type r2;
    print_argument_type(r2);
    return 0;
}

int test_compare_result(){
    using namespace boost::safe_numerics;

    automatic::comparison_result<
        safe<std::int16_t, automatic>,
        safe<std::int16_t, automatic>
    >::type r1;
    print_argument_type(r1);

    return 0;
}

int main(){
    using namespace boost::safe_numerics;

    test_log();
    test_auto<std::int8_t, std::int8_t>(1, -128);
    test_auto_result();
    test_compare_result();
    return 0;
}


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::model::meta::include.hpp                            //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_SURVIVAL_MODEL_META_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_MODEL_META_INCLUDE_HPP_ER_2009

#include <boost/statistics/survival/model/meta/data.hpp>
#include <boost/statistics/survival/model/meta/response.hpp>
#include <boost/statistics/survival/model/meta/model_data.hpp>

#endif


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file knapsack_unbounded_fptas.hpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-10-01
 */
#ifndef PAAL_KNAPSACK_UNBOUNDED_FPTAS_HPP
#define PAAL_KNAPSACK_UNBOUNDED_FPTAS_HPP

#include "paal/dynamic/knapsack_unbounded.hpp"
#include "paal/dynamic/knapsack/get_bound.hpp"
#include "paal/dynamic/knapsack/knapsack_fptas_common.hpp"

#include <boost/function_output_iterator.hpp>

namespace paal {

template <typename OutputIterator, typename Objects,
          typename ObjectSizeFunctor, typename ObjectValueFunctor>
typename detail::knapsack_base<Objects, ObjectSizeFunctor,
                               ObjectValueFunctor>::return_type
knapsack_unbounded_on_value_fptas(
    double epsilon, Objects && objects,
    detail::FunctorOnRangePValue<ObjectSizeFunctor, Objects>
        capacity, // capacity is of size type
    OutputIterator out, ObjectSizeFunctor size, ObjectValueFunctor value) {
    return detail::knapsack_general_on_value_fptas_retrieve(
        epsilon, detail::make_knapsack_data(std::forward<Objects>(objects), capacity, size, value, out),
        detail::unbounded_tag{});
}

template <typename OutputIterator, typename Objects,
          typename ObjectSizeFunctor, typename ObjectValueFunctor>
typename detail::knapsack_base<Objects, ObjectSizeFunctor,
                               ObjectValueFunctor>::return_type
knapsack_unbounded_on_size_fptas(
    double epsilon, Objects && objects,
    detail::FunctorOnRangePValue<ObjectSizeFunctor, Objects>
        capacity, // capacity is of size type
    OutputIterator out, ObjectSizeFunctor size, ObjectValueFunctor value) {
    return detail::knapsack_general_on_size_fptas_retrieve(
        epsilon, detail::make_knapsack_data(std::forward<Objects>(objects), capacity, size, value, out),
        detail::unbounded_tag{});
}

} //! paal

#endif // PAAL_KNAPSACK_UNBOUNDED_FPTAS_HPP


#include <boost/geometry/algorithms/relate.hpp>


#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/program_options.hpp>
#include "picpac-cv.h"

using namespace std;
using namespace picpac;
namespace ba = boost::accumulators;

class Context {

public:
    struct Config {
        string path;
        int size;
        Config (): size(240){
        }
    };
private:
    Config config;
    ba::accumulator_set<double, ba::stats<ba::tag::mean, ba::tag::min, ba::tag::max, ba::tag::variance > > acc;
    ImageEncoder encoder;
    FileWriter db;
public:
    Context (Config const &c): config(c), encoder(".jpg"), db(config.path)
    {
    }
    ~Context () {
    }

    void add (Record const &rec) {
        cv::Mat image = decode_buffer(rec.field(0), -1);
        if (image.cols <= config.size
                && image.rows <= config.size) {
            // downsize
            db.append(rec);
            return;
        }
        float r = std::min(1.0 * config.size / image.cols,
                           1.0 * config.size / image.rows);
        cv::resize(image, image, cv::Size(), r, r);
        string f0;
        encoder.encode(image, &f0);
        if (rec.meta().width < 2) {
            Record recx(0, f0);
            recx.meta().copy(rec.meta());
            db.append(recx);
        }
        else if (rec.meta().width == 2) {
            Record recx(0, f0, rec.field_string(1));
            recx.meta().copy(rec.meta());
            db.append(recx);
        }
    }
};

int main(int argc, char const* argv[]) {
    Context::Config config;
    fs::path input_path;

    namespace po = boost::program_options;
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "produce help message.")
        ("input", po::value(&input_path), "")
        ("output", po::value(&config.path), "")
        ("size", po::value(&config.size), "")
        ;

    po::positional_options_description p;
    p.add("input", 1);
    p.add("output", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).
                     options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help") || input_path.empty() || config.path.empty()) {
        cout << "Usage:" << endl;
        cout << "\tpicpac-stat ... <db>" << endl;
        cout << desc;
        cout << endl;
        return 0;
    }

    Context ctx(config);
    picpac::IndexedFileReader db(input_path);
    db.loop(std::bind(&Context::add, &ctx, placeholders::_1));
    return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_BITWISE_FUNCTIONS_TWOPOWER_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_TWOPOWER_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief  twopower generic tag

      Represents the twopower function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct twopower_ : ext::elementwise_<twopower_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<twopower_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_twopower_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::twopower_, Site> dispatching_twopower_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::twopower_, Site>();
    }
    template<class... Args>
    struct impl_twopower_;
  }

  /*!
    Returns \f$2^n\f$ (0 if n is less than zero)

    @par semantic:
    For any given value n  of integral type @c I:

    @code
    T r = twopower(n);
    @endcode

    code is similar to:

    @code
    T r = 1 << n;
    @endcode

    @par Note:

    This function is not defined for floating entries

    @see  @funcref{ilog2}
    @param  a0

    @return      a value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::twopower_, twopower, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::twopower_, iexp2, 1)
} }
#endif


/*
 * Copyright (C) 2016 - Garvan Institute of Medical Research
 *
 *  Ted Wong, Bioinformatic Software Engineer at Garvan Institute.
 */

#ifndef R_FOLD_HPP
#define R_FOLD_HPP

#include "data/dtest.hpp"
#include <boost/format.hpp>
#include "stats/analyzer.hpp"

// Defined in resources.cpp
extern Anaquin::Scripts PlotTLODR();

// Defined in resources.cpp
extern Anaquin::Scripts PlotTROC();

// Defined in resources.cpp
extern Anaquin::Scripts PlotTMA();

// Defined in resources.cpp
extern Anaquin::FileName LadRef();

// Defined in resources.cpp
extern Anaquin::FileName GTFRef();

namespace Anaquin
{
    struct RFold : public Analyzer
    {
        enum class Metrics
        {
            Gene,
            Isoform,
        };

        enum class Format
        {
            DESeq2,
            edgeR,
            Sleuth,
            Cuffdiff,
            Anaquin
        };

        struct Options : public AnalyzerOptions
        {
            Options() {}
            Format format;
            Metrics metrs;
        };

        struct Stats : public SequinStats, public MappingStats, public AnalyzerStats, public HistStats
        {
            struct Data
            {
                // Expcted log-fold ratio
                Concent exp;

                // Measured log-fold ratio
                Concent obs;

                Measured samp1 = NAN;
                Measured samp2 = NAN;

                // Standard deviation
                double se = NAN;

                // Base mean
                double mean = NAN;

                Probability p = NAN;
                Probability q = NAN;
            };

            std::map<SequinID, Data> data;

            /*
             * Optional inputs
             */

            // Normalized average counts for the replicates
            std::vector<double> means;

            // Log-fold ratios standard deviation
            std::vector<double> ses;
        };

        static Scripts generateRFold(const RFold::Stats &, const FileName &, const RFold::Options &);
        static Scripts generateRROC(const RFold::Stats &, const RFold::Options &);
        static Scripts generateRLODR(const RFold::Stats &, const RFold::Options &);

        static Scripts generateCSV(const RFold::Stats &, const RFold::Options &);
        static Scripts generateSummary(const FileName &,
                                       const RFold::Stats &,
                                       const RFold::Options &,
                                       const Units &);

        static void writeRFold(const FileName &, const RFold::Stats &, const RFold::Options &);
        static void writeRROC(const FileName &, const RFold::Stats &, const RFold::Options &);
        static void writeRLODR(const FileName &, const RFold::Stats &, const RFold::Options &);

        static void writeCSV(const FileName &, const RFold::Stats &, const RFold::Options &);
        static void writeSummary(const FileName &,
                                 const FileName &,
                                 const RFold::Stats &,
                                 const RFold::Options &,
                                 const Units &);

        /*
         * Classify and construct a vector of TP/FP/TN/FN, given the q-values and expected
         * fold-changes. The threshold for the TP classification is also required.
         */

        static std::vector<std::string> classify(const std::vector<double> &,
                                                 const std::vector<double> &,
                                                 double qCut,
                                                 double foldCut);

        static Stats analyze(const FileName &, const Options &o);
        static void  report (const FileName &, const Options &o = Options());
    };
}

#endif


/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2011 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_EXPRESSION_NODE_TYPES_HPP
#define MAPNIK_EXPRESSION_NODE_TYPES_HPP

// mapnik
//#include <mapnik/value.hpp>
//#include <mapnik/attribute.hpp>

// boost
#include <boost/variant/variant_fwd.hpp>

namespace boost { template <typename T> class recursive_wrapper; }

namespace mapnik
{

struct attribute;
struct geometry_type_attribute;
namespace value_adl_barrier {
  class value;
}
using value_adl_barrier::value;

namespace tags  {
struct negate
{
    static const char* str()
    {
        return "-";
    }
};

struct plus
{
    static const char* str()
    {
        return "+";
    }
};

struct minus
{
    static const char* str()
    {
        return "-";
    }
};

struct mult
{
    static const char* str()
    {
        return "*";
    }
};

struct div
{
    static const char* str()
    {
        return "/";
    }
};

struct  mod
{
    static const char* str()
    {
        return "%";
    }
};

struct less
{
    static const char* str()
    {
        return "<";
    }
};

struct  less_equal
{
    static const char* str()
    {
        return "<=";
    }
};

struct greater
{
    static const char* str()
    {
        return ">";
    }
};

struct greater_equal
{
    static const char* str()
    {
        return ">=";
    }
};

struct equal_to
{
    static const char* str()
    {
        return "=";
    }
};

struct not_equal_to
{
    static const char* str()
    {
        return "!=";
    }
};

struct logical_not
{
    static const char* str()
    {
        return "not ";
    }
};

struct logical_and
{
    static const char* str()
    {
        return " and ";
    }
};

struct logical_or
{
    static const char* str()
    {
        return " or ";
    }
};

} // end operation tags

template <typename Tag> struct binary_node;
template <typename Tag> struct unary_node;
struct regex_match_node;
struct regex_replace_node;

typedef mapnik::value value_type;

typedef boost::variant <
value_type,
attribute,
geometry_type_attribute,
boost::recursive_wrapper<unary_node<tags::negate> >,
boost::recursive_wrapper<binary_node<tags::plus> >,
boost::recursive_wrapper<binary_node<tags::minus> >,
boost::recursive_wrapper<binary_node<tags::mult> >,
boost::recursive_wrapper<binary_node<tags::div> >,
boost::recursive_wrapper<binary_node<tags::mod> >,
boost::recursive_wrapper<binary_node<tags::less> >,
boost::recursive_wrapper<binary_node<tags::less_equal> >,
boost::recursive_wrapper<binary_node<tags::greater> >,
boost::recursive_wrapper<binary_node<tags::greater_equal> >,
boost::recursive_wrapper<binary_node<tags::equal_to> >,
boost::recursive_wrapper<binary_node<tags::not_equal_to> >,
boost::recursive_wrapper<unary_node<tags::logical_not> >,
boost::recursive_wrapper<binary_node<tags::logical_and> >,
boost::recursive_wrapper<binary_node<tags::logical_or> >,
boost::recursive_wrapper<regex_match_node>,
boost::recursive_wrapper<regex_replace_node>
> expr_node;

}

#endif //MAPNIK_EXPRESSION_NODE_HPP