#ifndef BITMASK_H
#define BITMASK_H

#include <chrono>
#include <unordered_map>
#include <map>
#include <vector>
#include <iostream>
#include <sstream>
#include <gmp.h>
#include <boost/dynamic_bitset.hpp>
#include <simdpp/simd.h>
#include <tbb/scalable_allocator.h>

#include "integrity_violation.hpp"

using bitblock = uint64_t; // Type used to store bits
using rangeblock = uint16_t; // Type used to chunk the binary bits into  precomputable sequences
using codeblock = char; // Type used to store run-length codes for each precomputable sequence

using dynamic_bitset = boost::dynamic_bitset< unsigned long long >;
/// We define a function module as a collection of functions
/// This declaration acts as both a function module and a container class
/// The static class methods implements a function module providing operations on arrays of type bitblock, which can be allocated on the stack
/// The non-static class methods implements a heap-allocated equivalence, which supports the same methods
/// @note: Many of the binary operations assume that operands have the same length
class Bitmask {
public:
    static const bitblock ranges_per_code; // Number of ranges that can be encoded using a single codeblock
    static const bitblock bits_per_range; // Number of bits in a single rangeblock
    static const bitblock bits_per_block; // Number of bits in a single bitblock

    // @param blocks: the blocks containing bits
    // @param size: the number of bits which are represented in blocks
    // @modifies blocks will be set to have all bits be 1
    static void ones(bitblock * blocks, unsigned int size);

    // @param blocks: the blocks containing bits
    // @param size: the number of bits which are represented in blocks
    // @modifies blocks will be set to have all bits be 0
    static void zeros(bitblock * blocks, unsigned int size);

    // @param blocks: the blocks containing bits to copy
    // @param other_blocks: the blocks which will be overwritten by the copy
    // @param size: the number of bits which are represented in blocks
    // @modifies other_blocks will be overwritten with the contents of blocks
    static void copy(bitblock * blocks, bitblock * other_blocks, unsigned int size);

    // @param size: the number of bits to be represented
    // @param number_of_blocks: pointer to store the number of blocks required to represent these bits
    // @param block_offset: pointer to store the number of bits actually used in the last block
    // @modifies number_of_bits: initialized with the number of blocks required to represent these bits
    // @modifies block_offset: initialized with the number of bits actually used in the last block
    static void block_layout(unsigned int size, unsigned int * number_of_blocks, unsigned int * block_offset);

    // @param blocks: the blocks to sanitize / normalize
    // @param number of blocks: the number of blocks present in blocks
    // @param offset: the number of bits present in the final block
    // @modifies blocks: the final block will have all unsused bits cleared
    static void clean(bitblock * blocks, unsigned int number_of_blocks, unsigned int offset);

    // @param blocks: the blocks from which to count the bits which are set to 1
    // @param size: the number of bits which are represented in blocks
    // @returns the number of represented bits which are set to 1
    static unsigned int count(bitblock * blocks, unsigned int size);

    // @param blocks: the blocks from which to compute the number of contiguous ranges of bits which are set to 1
    // @param size: the number of bits which are represented in blocks
    // @returns the estimated number of contiguous ranges of bits which are set to 1
    // @note the this was originally an approximate method. I later altered this to an exact method :D
    static unsigned int words(bitblock * blocks, unsigned int size);

    // @param blocks: blocks which act as a vector of bits for a bit-wise logical and
    // @param other_blocks: blocks which act as a vector of bits for a bit-wise logical and
    // @param size: the number of bits which are represented in blocks and other_blocks
    // @param flip: whether or not to interpret the bits of blocks as flipped before applying the bit-wise operation
    // @modifies other_blocks: will be overwritten with the result
    static void bit_and(bitblock * blocks, bitblock * other_blocks, unsigned int size, bool flip = false);

    // @param blocks: blocks which act as a vector of bits for a bit-wise logical or
    // @param other_blocks: blocks which act as a vector of bits for a bit-wise logical or
    // @param size: the number of bits which are represented in blocks and other_blocks
    // @param flip: whether or not to interpret the bits of blocks as flipped before applying the bit-wise operation
    // @modifies other_blocks: will be overwritten with the result
    static void bit_or(bitblock * blocks, bitblock * other_blocks, unsigned int size, bool flip = false);

    // @param blocks: blocks which act as a vector of bits for a bit-wise logical xor
    // @param other_blocks: blocks which act as a vector of bits for a bit-wise logical xor
    // @param size: the number of bits which are represented in blocks and other_blocks
    // @param flip: whether or not to interpret the bits of blocks as flipped before applying the bit-wise operation
    // @modifies other_blocks: will be overwritten with the result
    static void bit_xor(bitblock * blocks, bitblock * other_blocks, unsigned int size, bool flip = false);

    // @param blocks: blocks which act as a vector of bits for comparison
    // @param other_blocks: blocks which act as a vector of bits for comparison
    // @param size: the number of bits which are represented in blocks and other_blocks
    // @param flip: whether or not to interpret the bits of blocks as flipped before applying the bit-wise operation
    // @returns whether blocks is bit-wise equal to other_blocks
    static bool equals(bitblock * const blocks, bitblock * const other_blocks, unsigned int size, bool flip = false);

    // @param left: blocks which act as a vector of bits for comparison
    // @param right: blocks which act as a vector of bits for comparison
    // @param size: the number of bits which are represented in blocks and other_blocks
    // @returns 0 if left == right, -1 if left < right, 1 if left > right
    // @note this comparison uses bit 0 as the least significant bit (little endian)
    static int compare(bitblock * const left, bitblock * const right, unsigned int size);

    // @param left: blocks which will be compared with right
    // @param right: blocks which will be compared with left
    // @param size: the number of blocks to use during comparison
    // @returns whether left is less (or greater) than right
    // @note this comparison uses bit 0 as the least significant bit (little endian)
    static bool less_than(bitblock * const left, bitblock * const right, unsigned int size);
    static bool greater_than(bitblock * const left, bitblock * const right, unsigned int size);

    // @param blocks: blocks which contain bits to be hashed
    // @param size: the number of bits which are represented in blocks
    // @returns a hash value of blocks
    static size_t hash(bitblock * blocks, unsigned int size);

    // @param blocks: blocks which contain bits to be read
    // @param size: the number of bits which are represented in blocks
    // @param index: index of the bit to be read
    // @returns the bit at the provided index represented as either 1 or 0
    static unsigned int get(bitblock * blocks, unsigned int size, unsigned int index);

    // @param blocks: blocks which contain bits to be written
    // @param size: the number of bits which are represented in blocks
    // @param index: index of the bit to be written
    // @param value: value to be written to at indexed position
    // @modifies blocks: blocks will be modified at the specified index with the specified value
    static void set(bitblock * blocks, unsigned int size, unsigned int index, bool value = 1);

    // @param blocks: blocks of bits to scan over
    // @param size: the number of bits which are represented in blocks
    // @param start: the starting index (inclusive) to scan for a bit that matches 'value'
    // @returns: the index of the first bit that matches value
    // @note: scan will search from 'start' towards more significant bits
    // @note: rscan will search from 'start' towards less significant bits
    static int scan(bitblock * blocks, int size, int start, bool value = 1);
    static int rscan(bitblock * blocks, int size, int start, bool value = 1);

    // @param blocks: blocks which will be converted to string format
    // @param size: the number of bits expected to convert
    // @param reverse: whether to reverse the printed bit order
    // @returns a string containing the bits in order of least significant to most significant
    static std::string to_string(bitblock * blocks, unsigned int size, bool flip = false);

    static bool integrity_check; // Flag that indicates whether null pointer checks are performed on object instances
    static bool precomputed; // Flag that indicates whether look-up tables are completed
    static std::vector< std::vector< codeblock > > ranges; // Precomputed run-length codes in rangeblock-sized sequences
    static std::vector< size_t > hashes; // Precomputed hashes in rangeblock-sized sequences
    static std::vector< char > counts; // Precomputed population counts in rangeblock-sized sequences

    static void precompute(); // Perform the one-time precomputation for rangeblock-sized sequences
    static void benchmark(unsigned int size); // Run a benchmark for a bitmask of the given size

    Bitmask();
    // Construction from a single fill-value and size
    Bitmask(unsigned int size, bool fill = false, bitblock * local_buffer=NULL);
    // Construction by copying from a stack-based bitblock array
    Bitmask(bitblock * blocks, unsigned int block_count, bitblock * local_buffer=NULL);
    // Construction by copying from a dynamic_bitset
    Bitmask(dynamic_bitset const & source, bitblock * local_buffer=NULL);
    // Construction by copying from another instance
    Bitmask(Bitmask const & source, bitblock * local_buffer=NULL);

    ~Bitmask();

    // Initialize attributes and allocate memory (if needed) for this instance
    void initialize(unsigned int size, bitblock * local_buffer=NULL);

    // @param dest_blocks: an array of bitblocks to store bits on
    // @modifies dest_blocks: the contents of dest_blocks will be overwritten with a copy of the bits stored in this bitmask
    void copy_to(bitblock * dest_blocks) const;

    // @param dest_blocks: an array of bitblocks to copy bits from
    // @modifies content: the instance member content will be overwritten with _size blocks of src_blocks
    void copy_from(bitblock * src_blocks);
    // Aliases to copy_from
    Bitmask & operator=(Bitmask const & other);

    // @returns a pointer to the blocks of this instance
    bitblock * data() const;

    // @returns the number of bits represented by this instance
    unsigned int size() const;

    // @returns the number of bits representable by this instance
    unsigned int capacity() const;

    // @returns the number of bits set to 1
    unsigned int count() const;

    // @returns estimates the number of contiguous ranges of 1's
    unsigned int words() const;

    // @param start: the bit index to start scanning from
    // @param value the value of the bit to scan for
    // @returns the index of the first bit that matches value, starting at (inclusive) the index 'start'.
    // @note scan returns size if scan doesn't find any matches, -1 if rscan doesn't find any matches
    // @note the scan implementation searches from start to most-significant bit
    // @note the rscan implementation searches from start to least-significant bit
    int scan(int start, bool value) const;
    int rscan(int start, bool value) const;

    // @param value the value of the bit to scan for
    // @param begin: the bit index to start scanning from
    // @modifies begin: the index of the first bit in the scan that matches value
    // @modifies end: the index of the first bit that doesn't match the value following the bit matched at 'begin'
    // @return true if a range is found
    // @note these implementations are for finding contiguous ranges of bits matching a specific value
    bool scan_range(bool value, int & begin, int & end) const;
    bool rscan_range(bool value, int & begin, int & end) const;

    // @returns true if and only if all bits are set to 0
    bool empty() const;

    // @returns true if and only if all bits are set to 1
    bool full() const;

    // @param blocks: the second operand vector of blocks of bits
    // @param flip: whether or not to treat the bits of this instance as flipped before applying the operation
    // @modifies blocks: blocks will be overwritten with the resulting bits
    void bit_and(bitblock * blocks, bool flip = false) const;
    void bit_xor(bitblock * blocks, bool flip = false) const;
    void bit_or(bitblock * blocks, bool flip = false) const;
    void bit_and(Bitmask const & other, bool flip = false) const;
    void bit_xor(Bitmask const & other, bool flip = false) const;
    void bit_or(Bitmask const & other, bool flip = false) const;

    // @modifes: sets every bit to zero
    void clear();

    // @modifes: sets every bit to one
    void fill();

    // @param blocks: an array of bitblocks containing bits to be compared
    // @returns whether the bits match up to the size of this instance
    bool operator==(bitblock * blocks) const;

    // @param other: a bitmask containing an array of bitblocks containing bits to be compared
    // @returns whether the bits match up to the size of this instance
    bool operator==(Bitmask const & other) const;

    // @param other: another bitmask instance for comparison
    // @returns whether the other bitmask is considered lesser
    // @note the 0th bit is considered most significant when making this comparison
    bool operator<(Bitmask const & other) const;

    // @param other: another bitmask instance for comparison
    // @returns whether the other bitmask is considered lesser
    // @note the 0th bit is considered most significant when making this comparison
    bool operator>(Bitmask const & other) const;

    // Additional derived relational operators
    bool operator!=(Bitmask const & other) const;
    bool operator<=(Bitmask const & other) const;
    bool operator>=(Bitmask const & other) const;

    // @returns the hash value of this bitmask
    size_t hash(bool bitwise = true) const;

    // @param index: an index specifying which bit to read
    // @returns: the accessed bit in the form of an integer
    unsigned int get(unsigned int index) const;
    // @note: aliases to get
    unsigned int operator[](unsigned int index) const;

    // @param index: an index specifying which bit to modify
    // @param value: a boolean value specifying whether to set the bit to 1 or 0
    void set(unsigned int index, bool value = true);

    // @param reverse: reverses the sequence so that the 0th bit is on the right, as opposed to on the left (little endian)
    // @returns a string representing the bit sequence with each bit represented as the character '1' or '0'
    // @note default reverses the sequence so that bit-0 is the left most in the string
    std::string to_string(bool reverse = false) const;

    // @returns true if the content of the object passes the null pointer check
    bool valid() const;
    // @throws an exception if this object contains a null pointer
    void validate() const;

    // @requires the new_size must be less that _max_blocks * bits_per_block. (i.e. the maximum capacity set during construction)
    // @modifies _size: modified to the new size
    // @modifies _used_blocks: modified to the new number of used blocks
    // @modifies _offset: modified to the new number of bits used in the last used block
    // @note: resizing does not affect memory usage. allocated memory remain constant until this instance is deconstructed
    void resize(unsigned int _new_size); // The number of bits actually being used (excludes leading zeros in final block)

private:
    static tbb::scalable_allocator<bitblock> allocator; // Allocator used to managing memory
    bitblock * content = NULL; // A pointer the blocks containing the bits
    unsigned int _size = 0; // The number of bits actually being used (excludes leading zeros in final block)
    unsigned int _offset = 0; // The number of bits used in the last used block by this object instance
    unsigned int _used_blocks = 0; // The number of blocks currently used by this object instance
    unsigned int _max_blocks = 0; // The number of blocks occupied by this object instance

    // If shallow is true, then the destructor is not responsible for deallocating the memory pointed to by content
    bool shallow = false;
};

// Explicit template specialization
namespace std {
    template <>
    struct hash< Bitmask > {
        std::size_t operator()(Bitmask const & bitmask) const { return bitmask.hash(); }
    };

    template <>
    struct less< Bitmask > {
        bool operator()(Bitmask const & left, Bitmask const & right) const { return left < right; }
    };

    template <>
    struct equal_to< Bitmask > {
        bool operator()(Bitmask const & left, Bitmask const & right) const { return left == right; }
    };
}

#endif


#include "inexacttoexact.hpp"
#include "ast.hpp"
#include "parsers.hpp"
#include "types.hpp"
#include <boost/variant.hpp>
#include <stdexcept>
namespace HT
{
    void inexactToExact(PASTNode astnode, ParsersHelper& ph)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()!=2)
          throw std::runtime_error("Inexact->Exact can only have one parameter");
        auto & secondCh = *astnode->ch.rbegin();
        ph.parse(secondCh);
        if (secondCh->token.tokenType != Complex)
          throw std::runtime_error("The argument of Inexact->exact must be Complex");
        auto cast = boost::get<ComplexType>(secondCh->token.info);

        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        astnode->token.info = ComplexType(cast.toexact());
        astnode->remove();
    }

}


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 * All rights reserved.
 *
 * See LICENSE file in the root of the mrob library.
 *
 *
 * plane.hpp
 *
 *  Created on: Feb 1, 2019
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#ifndef PLANE_HPP_
#define PLANE_HPP_

#include <vector>
#include <memory>
#include "mrob/SE3.hpp"
#include <Eigen/StdVector>

namespace mrob{

/**
 * class Plane stores a vector of point clouds from different observations
 * and  provides the gradients with respect to those poses.
 *
 */
class Plane{
  public:
    Plane(uint_t timeLength);
    ~Plane();

    void reserve(uint_t d, uint_t t);
    Mat41 get_plane(void) {return planeEstimation_;};
    /**
     * This function is intended to add a point that belongs to the plane at time t
     * Internally, we will save a copy of them
     */
    void push_back_point(Mat31 &point, uint_t t);
    std::vector<Mat31>& get_points(uint_t t);
    uint_t get_number_points(uint_t t) {return allPlanePoints_[t].size();};
    uint_t get_total_number_points() {return numberPoints_;};
    void clear_points();
    void set_trajectory(const std::shared_ptr<const std::vector<SE3>> &trajectory) {trajectory_ = trajectory;};

    /**
     * Estimates the plane parameters: v = [n', d]'_{4x1}, where v is unitary, (due to the SVD solution)
     * although for standard plane estimation we could enforce unitary on the normal vector n.
     */
    double estimate_plane();
    /**
     * Estimates Incremetally the plane parameters: v = [n', d]'_{4x1}, where v is unitary, (due to the SVD solution)
     * although for standard plane estimation we could enforce unitary on the normal vector n.
     *
     * The difference with the previous estimate_plane() is that we update the matrix Q for the give time
     * stamp and recalculate the solution, on constant time O(1)
     */
    double estimate_plane_incrementally(uint_t t);
    /**
     * get error: returns the error as the min eigenvalue
     */
    double get_error() const {return lambda_;};
    /**
     * get error incremental: returns the error as the min eigenvalue only updating the
     * value of Q_t, at time step t. Nothing insie gets updated
     */
    double get_error_incremental(uint_t t) const;

    /**
     *  calculates the matrix S = sum(p*p'), where p = [x,y,z,1]
     * for all planes, as an aggregation of the outer product of all
     * homogeneous points
     * If reset = true, clears all information and recalculates S
     * If reset = false (default) only calculates S if there is no calculation yet
     */
    void calculate_all_matrices_S(bool reset=false);
    /**
     * get mean point calculates the mean of the pointcloud observed at time t,
     * given that S = sum p * p' =  sum ([x2 xy xz x
     *                                    yx y2 yz y
     *                                    zx zy z2 z
     *                                    x   y  z 1]
     * ser we just calcualte S and return
     */
    Mat31 get_mean_point(uint_t t);
    /**
     *  calculates the matrix Qi = 1^T_i * Si * (1^T_i)^transp
     *  for all planes. Since this is an iterative process on T's,
     *  we separate the calculation of the S matrix,
     *  and the Q matrix which rotates S
     */
    void calculate_all_matrices_Q();
    /**
     * calculate gradient at time t, returns the Jacobian over the EF from SVD of Q  = U D V'
     * as d lamda = v' * dQ * v
     *
     */
    Mat61 calculate_gradient(uint_t t);

    /**
     * calculates the Hessian of the eigen factor, as described in the paper.
     * It requires to FIRST calculate gradient.
     * Output is an upper traingular view of the symetric hessian matrix
     */
    Mat6 calculate_hessian(uint_t t);

    void reset();
    void print() const;

  protected:
    // max index of time, or trajectory length
    uint_t timeLength_;
    //std::vector<uint_t> timeIndex_;// not used now, but it would be useful for non-consecutive obs

    // Overparametrized plane, as a transformation in SE3 TODO needed?
    //SE3 plane_;
    Mat41 planeEstimation_;
    double lambda_;
    bool isPlaneEstimated_;// XXX used or deprecated?

    // subset of pointcloud for the given plane
    std::vector< std::vector<Mat31> > allPlanePoints_;
    uint_t numberPoints_;

    // Transformations. We shared it across all planes that need it.
    std::shared_ptr<const std::vector<SE3>> trajectory_;

    // gradient calculation Q
    std::vector<Mat4> matrixS_, matrixQ_;
    Mat4 accumulatedQ_;//Q matrix of accumulated values for the incremental update of the error.

    // Store last gradients calculated (for Hessian)
    std::vector<Mat4> gradQ_, lieGenerativeMatrices_;//TODO move this to a common library utility, replicating this every time is terrible.

  public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}

#endif /* PLANE_HPP_ */


#pragma once

#include <boost/qvm/all.hpp>
#include <boost/array.hpp>

namespace boost
{
	namespace qvm
	{
		template< class _Scalar, std::size_t _NumDim >
		struct v_traits< boost::array< _Scalar, _NumDim > >
		{
			typedef boost::array< _Scalar, _NumDim > vector_type;
			static int const dim = _NumDim;
			typedef _Scalar scalar_type;

			template <int I> static inline const scalar_type &r( const vector_type &v ) { return v[I]; }
			template <int I> static inline scalar_type &w( vector_type &v ) { return v[I]; }

			static inline const scalar_type &ir( int i, const vector_type &v ) { return v[i]; }
			static inline scalar_type &iw( int i, vector_type &v ){	return v[i]; }
		};

		template< class _Scalar >
		struct q_traits< boost::array< _Scalar, 4 > >{
			typedef boost::array< _Scalar, 4 > quaternion_type;
			typedef _Scalar scalar_type;
			template <int I> static inline const scalar_type &r(const quaternion_type &q){ return q[I]; }
			template <int I> static inline scalar_type &w(quaternion_type &q){ return q[I]; }
		};
	}
}


#include "ros/ros.h"

#include <iostream>
#include <string>
#include <vector>

#include "task.h"
#include "scheduler.h"

#include <boost/numeric/ublas/matrix.hpp>

using namespace std;
namespace bn = boost::numeric::ublas;

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

  string s1("ChargingPoint");
  string s2("Lobby");

  int task_count = 10;

  double one_hour = 60 * 60 * 60;
  double task_duration = one_hour / 2.0;
  double nav_time = 300;
  double window_start = 0;
  double window_end = window_start + ((task_duration + nav_time) * task_count * task_count * task_count);

  vector<Task*> tasks;

  //for (int i=0; i< task_count; i++)
  //{
   // tasks.push_back(new Task(1, window_start, window_end, task_duration, s2, s2));
    //tasks.push_back(new Task(2, window_start, window_end, task_duration, s1, s1));
   // tasks.push_back(new Task(3, window_start, window_end, task_duration, s2, s2));
  //}

   tasks.push_back(new Task(1, 0, 8, 2, "s1", "s1"));
   tasks.push_back(new Task(2, 2, 8, 2, "s2", "s2"));
   tasks.push_back(new Task(3, 0, 7, 1, "s3", "s3"));

  double ** duration_array = toArray(createUniformDurationMatrix(&tasks, 1.0));

  Scheduler scheduler(&tasks, duration_array, 1.0);
  bool worked = scheduler.solve(4, "");

  if(worked)  {
    cout<< "Schedule found" << worked << "\n";
    for(auto & tp : tasks) {
      cout<<"task "<<tp->getID()<<endl;
      cout<< "start time "<<tp->getExecTime()<<endl;

    }
  }
  else {
    cout<< "No schedule found"<< endl;
  }

  return 0;

}


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014, 2015.
// Modifications copyright (c) 2014-2015 Oracle and/or its affiliates.

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

// Use, modification and distribution is 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_GEOMETRY_TEST_CONVEX_HULL_HPP
#define BOOST_GEOMETRY_TEST_CONVEX_HULL_HPP

#include <boost/variant/variant.hpp>

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/convex_hull.hpp>
#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/is_empty.hpp>
#include <boost/geometry/algorithms/num_points.hpp>
#include <boost/geometry/algorithms/perimeter.hpp>

#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/strategies/area/services.hpp>
#include <boost/geometry/strategies/spherical/side_by_cross_track.hpp>

#include <boost/geometry/strategy/cartesian/side_robust.hpp>
#include <boost/geometry/strategy/cartesian/side_non_robust.hpp>
#include <boost/geometry/strategy/cartesian/precise_area.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>

#include <boost/geometry/geometries/polygon.hpp>

struct robust_cartesian : boost::geometry::strategies::detail::cartesian_base
{
    static auto side()
    {
        return boost::geometry::strategy::side::side_robust<>();
    }
};

struct non_robust_cartesian_fast : boost::geometry::strategies::detail::cartesian_base
{
    static auto side()
    {
        return boost::geometry::strategy::side::side_non_robust<>();
    }
};

struct non_robust_cartesian_sbt : boost::geometry::strategies::detail::cartesian_base
{
    static auto side()
    {
        return boost::geometry::strategy::side::side_by_triangle<>();
    }
};

template <typename CalculationType = void>
struct sphrerical_side_by_cross_track : boost::geometry::strategies::detail::spherical_base<void>
{
    static auto side()
    {
        return boost::geometry::strategy::side::side_by_cross_track<>();
    }
};

struct precise_cartesian : boost::geometry::strategies::detail::cartesian_base
{
    template <typename Geometry>
    static auto area(Geometry const&)
    {
        return boost::geometry::strategy::area::precise_cartesian<>();
    }
};

template <typename Geometry, typename Hull, typename AreaStrategy>
void check_convex_hull(Geometry const& geometry, Hull const& hull,
                      std::size_t /*size_original*/, std::size_t size_hull,
                      double expected_area, double expected_perimeter,
                      bool reverse, AreaStrategy area_strategy)
{
    std::size_t n = bg::num_points(hull);

    BOOST_CHECK_MESSAGE(n == size_hull,
        "convex hull: " << bg::wkt(geometry)
        << " -> " << bg::wkt(hull)
        << " type "
        << (typeid(typename bg::coordinate_type<Hull>::type).name())
        << " -> Expected: " << size_hull
        << " detected: " << n);

    // We omit this check as it is not important for the hull algorithm
    // BOOST_CHECK(bg::num_points(geometry) == size_original);

    auto ah = bg::area(hull, area_strategy);

    if (reverse)
    {
        ah = -ah;
    }

    BOOST_CHECK_CLOSE(ah, expected_area, 0.001);

    if ( expected_perimeter >= 0 )
    {
        auto ph = bg::perimeter(hull);

        BOOST_CHECK_CLOSE(ph, expected_perimeter, 0.001);
    }
}

namespace resolve_variant {

struct closure_visitor : public boost::static_visitor<bg::closure_selector>
{
    template <typename Geometry>
    bg::closure_selector operator()(Geometry const&) const
    {
        return bg::closure<Geometry>::value;
    }
};

template <typename Geometry>
inline bg::closure_selector get_closure(Geometry const&)
{
    return bg::closure<Geometry>::value;
}

template <BOOST_VARIANT_ENUM_PARAMS(typename T)>
inline bg::closure_selector get_closure(boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> const& v)
{
    return boost::apply_visitor(closure_visitor(), v);
}

} // namespace resolve_variant

template
<
    typename Hull,
    typename Strategy,
    typename AreaStrategy
>
struct test_convex_hull
{
    template <typename Geometry>
    static void apply(Geometry const& geometry,
                      std::size_t size_original,
                      std::size_t size_hull_closed,
                      double expected_area,
                      double expected_perimeter,
                      bool reverse)
    {
        bool const is_original_closed = resolve_variant::get_closure(geometry) != bg::open;
        static bool const is_hull_closed = bg::closure<Hull>::value != bg::open;

        // convex_hull_insert() uses the original Geometry as a source of the info
        // about the order and closure
        std::size_t const size_hull_from_orig = is_original_closed ?
                    size_hull_closed : size_hull_closed - 1;
        std::size_t const size_hull = is_hull_closed ? size_hull_closed :
                                                       size_hull_closed - 1;

        Hull hull;

        // Test version with strategy
        bg::clear(hull);
        bg::convex_hull(geometry, hull.outer(), Strategy());
        check_convex_hull(geometry, hull, size_original, size_hull,
            expected_area, expected_perimeter, false, AreaStrategy());

        // Test version with output iterator and strategy
        bg::clear(hull);
        bg::detail::convex_hull::convex_hull_insert(
            geometry,
            std::back_inserter(hull.outer()), Strategy());
        check_convex_hull(geometry, hull, size_original, size_hull_from_orig,
            expected_area, expected_perimeter, reverse, AreaStrategy());

    }
};

template
<
    typename Hull,
    typename AreaStrategy
>
struct test_convex_hull<Hull, robust_cartesian, AreaStrategy>
{
    template <typename Geometry>
    static void apply(Geometry const& geometry,
                      std::size_t size_original,
                      std::size_t size_hull_closed,
                      double expected_area,
                      double expected_perimeter,
                      bool reverse)
    {
        bool const is_original_closed = resolve_variant::get_closure(geometry) != bg::open;
        static bool const is_hull_closed = bg::closure<Hull>::value != bg::open;

        // convex_hull_insert() uses the original Geometry as a source of the info
        // about the order and closure
        std::size_t const size_hull_from_orig = is_original_closed ?
                    size_hull_closed : size_hull_closed - 1;
        std::size_t const size_hull = is_hull_closed ? size_hull_closed :
                                                       size_hull_closed - 1;

        Hull hull;

        // Test version with output iterator
        bg::detail::convex_hull::convex_hull_insert(geometry,
                                                    std::back_inserter(hull.outer()));
        check_convex_hull(geometry, hull, size_original, size_hull_from_orig,
                          expected_area, expected_perimeter, reverse,
                          AreaStrategy());

        // Test version with ring as output
        bg::clear(hull);
        bg::convex_hull(geometry, hull.outer());
        check_convex_hull(geometry, hull, size_original, size_hull, expected_area,
                          expected_perimeter, false, AreaStrategy());

        // Test version with polygon as output
        bg::clear(hull);
        bg::convex_hull(geometry, hull);
        check_convex_hull(geometry, hull, size_original, size_hull, expected_area,
                          expected_perimeter, false, AreaStrategy());

        // Test version with strategy
        bg::clear(hull);
        bg::convex_hull(geometry, hull.outer(), robust_cartesian());
        check_convex_hull(geometry, hull, size_original, size_hull, expected_area,
                          expected_perimeter, false, AreaStrategy());

        // Test version with output iterator and strategy
        bg::clear(hull);
        bg::detail::convex_hull::convex_hull_insert(geometry,
                                                    std::back_inserter(hull.outer()), robust_cartesian());
        check_convex_hull(geometry, hull, size_original, size_hull_from_orig,
                          expected_area, expected_perimeter, reverse, AreaStrategy());

    }
};

template
<
    typename Geometry,
    typename Strategy,
    typename AreaStrategy,
    bool Clockwise,
    bool Closed
>
void test_geometry_order(std::string const& wkt,
                         std::size_t size_original,
                         std::size_t size_hull_closed,
                         double expected_area,
                         double expected_perimeter = -1.0)
{
    typedef bg::model::polygon
        <
            typename bg::point_type<Geometry>::type,
            Clockwise,
            Closed
        > hull_type;

    Geometry geometry;
    bg::read_wkt(wkt, geometry);

    test_convex_hull<hull_type, Strategy, AreaStrategy>::apply(geometry, size_original,
        size_hull_closed, expected_area, expected_perimeter, !Clockwise);

    boost::variant<Geometry> v(geometry);
    test_convex_hull<hull_type, Strategy, AreaStrategy>::apply(v, size_original,
        size_hull_closed, expected_area, expected_perimeter, !Clockwise);
}

template
<
    typename Geometry,
    typename Strategy,
    typename AreaStrategy = typename bg::strategies::area::services::default_strategy
        <
            Geometry
        >::type
>
void test_geometry(std::string const& wkt,
                   std::size_t size_original,
                   std::size_t size_hull_closed,
                   double expected_area,
                   double expected_perimeter = -1.0)
{
    test_geometry_order<Geometry, Strategy, AreaStrategy, true, true>(
        wkt, size_original, size_hull_closed, expected_area, expected_perimeter);
    test_geometry_order<Geometry, Strategy, AreaStrategy, false, true>(
        wkt, size_original, size_hull_closed, expected_area, expected_perimeter);
    test_geometry_order<Geometry, Strategy, AreaStrategy, true, false>(
        wkt, size_original, size_hull_closed, expected_area, expected_perimeter);
    test_geometry_order<Geometry, Strategy, AreaStrategy, false, false>(
        wkt, size_original, size_hull_closed, expected_area, expected_perimeter);
}

template <class SGeometry, class GGeometry>
void test_geometry_sph_geo(std::string const& wkt,
                           std::size_t size_original,
                           std::size_t size_hull_closed,
                           double spherical_expected_area,
                           double geographic_expected_area)
{
    typedef boost::geometry::strategies::convex_hull::spherical<> SphericalStrategy;
    test_geometry<SGeometry, SphericalStrategy>(wkt, size_original,
        size_hull_closed, spherical_expected_area);

    typedef boost::geometry::strategies::convex_hull::geographic<> GeoStrategy;
    test_geometry<GGeometry, GeoStrategy>(wkt, size_original,
        size_hull_closed, geographic_expected_area);
}

template <typename Geometry>
void test_empty_input()
{
    Geometry geometry;
    bg::model::polygon
        <
            typename bg::point_type<Geometry>::type
        > hull;

    bg::convex_hull(geometry, hull);
    BOOST_CHECK_MESSAGE(bg::is_empty(hull), "Output convex hull should be empty" );
}

#endif


#include <frovedis.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;
using namespace std;

int sum(int a, int b){return a + b;}

BOOST_AUTO_TEST_CASE( frovedis_test )
{
  int argc = 1;
  char** argv = NULL;
  use_frovedis use(argc, argv);

  // filling sample input vector
  std::vector<std::pair<int,int>> v;
  v.push_back(std::make_pair(1,10));
  v.push_back(std::make_pair(4,10));
  v.push_back(std::make_pair(3,10));
  v.push_back(std::make_pair(2,10));
  v.push_back(std::make_pair(3,20));
  v.push_back(std::make_pair(2,20));
  v.push_back(std::make_pair(4,20));
  v.push_back(std::make_pair(1,20));

  // filling expected output vector
  std::vector<std::pair<int,int>> ref_out;
  ref_out.push_back(std::make_pair(1,30));
  ref_out.push_back(std::make_pair(2,30));
  ref_out.push_back(std::make_pair(3,30));
  ref_out.push_back(std::make_pair(4,30));

  // testing reduce_by_key
  auto d1 = frovedis::make_dvector_scatter(v);
  auto d2 = d1.reduce_by_key<int,int>(sum);
  auto r = d2.as_dvector().sort().gather();

  BOOST_CHECK(r == ref_out);
}


//  endian/example/conversion_use_case.cpp

//  Copyright Beman Dawes 2014

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

//  This program reads a binary file of fixed length records, with a format defined
//  in a header file supplied by a third-party. The records inserted into a vector,
//  and the vector is sorted. The sorted records are then written to an output file.

//  Full I/O error testing omitted for brevity, So don't try this at home.

#include "third_party_format.hpp"
#include <boost/endian/conversion.hpp>
#include <vector>
#include <fstream>
#include <algorithm>
#include <iostream>

using third_party::record;

int main()
{
  std::ifstream in("data.bin", std::ios::binary);
  if (!in) { std::cout << "Could not open data.bin\n"; return 1; }

  std::ofstream out("sorted-data.bin", std::ios::binary);
  if (!out) { std::cout << "Could not open sorted-data.bin\n"; return 1; }

  record rec;
  std::vector<record> recs;

  while (!in.eof())  // read each record
  {
    in.read((char*)&rec, sizeof(rec));
    rec.balance = boost::endian::big_to_native(rec.balance);  // reverse if needed
    recs.push_back(rec);
  }

  std::sort(recs.begin(), recs.end(),  // decending sort by balance
    [](const record& lhs, const record& rhs) -> bool
      { return lhs.balance > rhs.balance; });

  for (auto &out_rec : recs)  // write each record
  {
    out_rec.balance = boost::endian::native_to_big(out_rec.balance);  // reverse if needed
    out.write((const char*)&out_rec, sizeof(out_rec));
  }

}


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::data::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_DATA_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_DATA_INCLUDE_HPP_ER_2009

#include <boost/statistics/survival/constant.hpp>
#include <boost/statistics/survival/data/algorithm/include.hpp>
#include <boost/statistics/survival/data/data/include.hpp>
#include <boost/statistics/survival/data/meta/include.hpp>
#include <boost/statistics/survival/data/random/include.hpp>

#endif


#define BOOST_TEST_MODULE SparseLatticeSolveTestsDOUBLE

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

#include "sparse_lattice_solve_test.hpp"

BOOST_AUTO_TEST_CASE( SparseLatticeSolveTestsDOUBLE )
{

    //multwork<double>(3,3,3,3);

    //solvework<double>(10,10,10,1);

    //solvework<double>(20,20,10,20);

    solvework<double>(20,20,10,20);
    //solvework<float>(5,5,4,5);

}


#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <vector>

namespace hdi_plan {

struct Color {
	double r = 0;
	double g = 0;
	double b = 0;
	double a = 1;
};

struct VisualizationMarker {
	// from ROS visualization_msgs/marker.msg documentation (Only relevant fields)
	const static unsigned char ARROW = 0;
	const static unsigned char CUBE = 1;
	const static unsigned char SPHERE = 2;
	const static unsigned char CYLINDER = 3;
	const static unsigned char LINE_STRIP = 4;
	const static unsigned char LINE_LIST = 5;
	const static unsigned char CUBE_LIST = 6;
	const static unsigned char SPHERE_LIST = 7;
	const static unsigned char POINTS = 8;
	const static unsigned char TEXT_VIEW_FACING = 9;
	const static unsigned char MESH_RESOURCE = 10;
	const static unsigned char TRIANGLE_LIST = 11;
	const static unsigned char ADD = 0;
	const static unsigned char MODIFY = 0;
	const static unsigned char DELETE = 2;
	const static unsigned char DELETEALL = 3;
	const static unsigned char OVERWRITE = 4;   // added this option as RVIZ needs to remove the remaining markers
	int type = 0;
	int action = 0;
	int id = 0;
	std::string ns = "";
	std::string text = "";
	Eigen::Vector3d position = Eigen::Vector3d(0, 0, 0);
	Eigen::Quaterniond orientation = Eigen::Quaterniond(1, 0, 0, 0);
	Eigen::Vector3d scale = Eigen::Vector3d(0, 0, 0);
	Color color = Color();
	std::vector<Eigen::Vector3d> points;  // empty initialization
	std::vector<Color> colors;
};

struct VisualizationMarkers {
	// access
	void addMarker(const VisualizationMarker &marker);

	int getNextVisualizationId();

	const std::vector<VisualizationMarker> &getMarkers() const;

	std::vector<VisualizationMarker> &getMarkers();

protected:
	std::vector<VisualizationMarker> markers;
};

}


#include "gms_matcher.h"
#include <Eigen/Dense>
#include <iostream>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <string>

// #define USE_GPU
#ifdef USE_GPU
#include <opencv2/cudafeatures2d.hpp>
using cuda::GpuMat;
#endif

/****************************************************
 * 本程序演示了如何使用2D-2D的特征匹配估计相机运动
 * **************************************************/

void find_feature_matches(const cv::Mat &img_1, const cv::Mat &img_2,
                          std::vector<cv::KeyPoint> &keypoints_1,
                          std::vector<cv::KeyPoint> &keypoints_2,
                          std::vector<cv::DMatch> &matches);

void find_gms_matches(const cv::CommandLineParser &parser, const cv::Mat &img_1,
                      const cv::Mat &img_2,
                      std::vector<cv::KeyPoint> &keypoints_1,
                      std::vector<cv::KeyPoint> &keypoints_2,
                      std::vector<cv::DMatch> &matches);

void pose_estimation_2d2d(std::vector<cv::KeyPoint> keypoints_1,
                          std::vector<cv::KeyPoint> keypoints_2,
                          std::vector<cv::DMatch> matches, cv::Mat K,
                          cv::Mat &R, cv::Mat &t);
void get_random_tf(Eigen::Matrix4d &R);
void quat2matrix(const Eigen::Vector4d &qvec, Eigen::Matrix3d &mat);
void rvec2matrix(const Eigen::Vector3d &rvec, Eigen::Matrix3d &mat);

// 像素坐标转相机归一化坐标
cv::Point2d pixel2cam(const cv::Point2d &p, const cv::Mat &K);

int main(int argc, char **argv) {
  const char *keys =
      "{ h help        |                  | print help message  }"
      "{ l left        |                  | specify left (reference) image  }"
      "{ r right       |                  | specify right (query) image }"
      "{ camera        | 0                | specify the camera device number }"
      "{ nfeatures     | 10000            | specify the maximum number of ORB "
      "features }"
      "{ fastThreshold | 20               | specify the FAST threshold }"
      "{ drawSimple    | true             | do not draw not matched keypoints }"
      "{ withRotation  | false            | take rotation into account }"
      "{ withScale     | false            | take scale into account }";

  cv::CommandLineParser cmd(argc, argv, keys);
  if (cmd.has("help"))
	{
		std::cout << "Usage: gms_matcher [options]" << std::endl;
		std::cout << "Available options:" << std::endl;
		cmd.printMessage();
		return EXIT_SUCCESS;
	}

#ifdef USE_GPU
  int flag = cuda::getCudaEnabledDeviceCount();
  if (flag != 0) {
    cuda::setDevice(0);
    std::cout << "using GPU" << endl;
  }
#endif // USE_GPU

  cv::String imgL_path = cmd.get<cv::String>("left");
  cv::String imgR_path = cmd.get<cv::String>("right");

  if (!imgL_path.empty() && !imgR_path.empty()) {
    //-- 读取图像
    cv::Mat img_1 = cv::imread(imgR_path, cv::IMREAD_COLOR);
    cv::Mat img_2 = cv::imread(imgL_path, cv::IMREAD_COLOR);

    std::vector<cv::KeyPoint> keypoints_1, keypoints_2;
    std::vector<cv::DMatch> matches;
    find_gms_matches(cmd, img_1, img_2, keypoints_1, keypoints_2, matches);
    std::cout << "一共找到了" << matches.size() << "组匹配点" << std::endl;

    cv::Mat frameMatches;
    cv::drawMatches(img_1, keypoints_1, img_2, keypoints_2, matches,
                    frameMatches, cv::Scalar::all(-1), cv::Scalar::all(-1),
                    std::vector<char>(),
                    cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
    cv::imshow("Matches GMS", frameMatches);
    cv::waitKey(0);
  }
  return 0;
}

void get_random_tf(Eigen::Matrix4d &R) {
  Eigen::Vector3d angle = Eigen::Vector3d::Random() * 30;
  std::cout << "random angles:" << angle.transpose() << std::endl;
  angle = angle * M_PI / 180.0;
  Eigen::Vector3d tvec = Eigen::Vector3d::Random() * 0.1;
  Eigen::Matrix3d cam_rot =
      (Eigen::AngleAxisd(angle[0], Eigen::Vector3d::UnitX()) *
       Eigen::AngleAxisd(angle[1], Eigen::Vector3d::UnitY()) *
       Eigen::AngleAxisd(angle[2], Eigen::Vector3d::UnitZ()))
          .toRotationMatrix();
  R = Eigen::Matrix4d::Identity();
  R.block<3, 3>(0, 0) = cam_rot;
  R.block<3, 1>(0, 3) = tvec;
}

void quat2matrix(const Eigen::Vector4d &qvec, Eigen::Matrix3d &mat) {
  Eigen::Quaterniond quat(qvec[0], qvec[1], qvec[2], qvec[3]); // w, x, y, z
  mat = quat.normalized().toRotationMatrix();
}

void rvec2matrix(const Eigen::Vector3d &rvec, Eigen::Matrix3d &mat) {
  double rot_angle = rvec.norm();
  Eigen::Vector3d axis = rvec / rot_angle;
  Eigen::AngleAxisd angle_axis = Eigen::AngleAxisd(rot_angle, axis);
  mat = angle_axis.toRotationMatrix();
}

void find_gms_matches(const cv::CommandLineParser &parser, const cv::Mat &img_1,
                      const cv::Mat &img_2,
                      std::vector<cv::KeyPoint> &keypoints_1,
                      std::vector<cv::KeyPoint> &keypoints_2,
                      std::vector<cv::DMatch> &matches) {
  bool withRotation = parser.get<bool>("withRotation");
  bool withScale = parser.get<bool>("withScale");

  cv::Mat d1, d2;
  std::vector<cv::DMatch> matches_all;

  cv::Ptr<cv::ORB> orb = cv::ORB::create(parser.get<int>("nfeatures"));
  orb->setFastThreshold(parser.get<int>("fastThreshold"));
  cv::Rect roi1(0, 0, img_1.cols, int(0.8 * img_1.rows));
  cv::Mat mask1 = cv::Mat::zeros(img_1.size(), CV_8UC1);
  cv::Rect roi2(0, 0, img_2.cols, int(0.8 * img_2.rows));
  cv::Mat mask2 = cv::Mat::zeros(img_2.size(), CV_8UC1);
  mask1(roi1).setTo(255);
  mask2(roi2).setTo(255);
  orb->detectAndCompute(img_1, mask1, keypoints_1, d1);
  orb->detectAndCompute(img_2, mask2, keypoints_2, d2);

#ifdef USE_GPU
  cv::GpuMat gd1(d1), gd2(d2);
  cv::Ptr<cuda::DescriptorMatcher> matcher =
      cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING);
  matcher->match(gd1, gd2, matches_all);
#else
  std::cout << "using CPU" << std::endl;
  cv::BFMatcher matcher(cv::NORM_HAMMING);
  matcher.match(d1, d2, matches_all);
#endif

  // GMS filter
  cv::matchGMS(img_1.size(), img_2.size(), keypoints_1, keypoints_2,
               matches_all, matches, withRotation, withScale);
  std::cout << "matchesGMS: " << matches.size() << std::endl;
}

void find_feature_matches(const cv::Mat &img_1, const cv::Mat &img_2,
                          std::vector<cv::KeyPoint> &keypoints_1,
                          std::vector<cv::KeyPoint> &keypoints_2,
                          std::vector<cv::DMatch> &matches) {
  //-- 初始化
  cv::Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
  cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
  // use this if you are in OpenCV2
  // cv::Ptr<cv::FeatureDetector> detector = cv::FeatureDetector::create ( "ORB"
  // );
  // cv::Ptr<cv::DescriptorExtractor> descriptor =
  // cv::DescriptorExtractor::create ( "ORB" );
  cv::Ptr<cv::DescriptorMatcher> matcher =
      cv::DescriptorMatcher::create("BruteForce-Hamming");
  //-- 第一步:检测 Oriented FAST 角点位置
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  //-- 第二步:根据角点位置计算 BRIEF 描述子
  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
  std::vector<cv::DMatch> match;
  // cv::BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  //-- 第四步:匹配点对筛选
  double min_dist = 10000, max_dist = 0;

  //找出所有匹配之间的最小距离和最大距离,
  //即是最相似的和最不相似的两组点之间的距离
  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist)
      min_dist = dist;
    if (dist > max_dist)
      max_dist = dist;
  }

  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);

  //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= std::max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

cv::Point2d pixel2cam(const cv::Point2d &p, const cv::Mat &K) {
  return cv::Point2d((p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
                     (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}

void pose_estimation_2d2d(std::vector<cv::KeyPoint> keypoints_1,
                          std::vector<cv::KeyPoint> keypoints_2,
                          std::vector<cv::DMatch> matches, cv::Mat K,
                          cv::Mat &R, cv::Mat &t) {
  //-- 把匹配点转换为vector<cv::Point2f>的形式
  std::vector<cv::Point2f> points1;
  std::vector<cv::Point2f> points2;

  for (int i = 0; i < (int)matches.size(); i++) {
    points1.push_back(keypoints_1[matches[i].queryIdx].pt);
    points2.push_back(keypoints_2[matches[i].trainIdx].pt);
  }

  //-- 计算基础矩阵
  // cv::Mat fundamental_matrix;
  // fundamental_matrix = cv::findFundamentalMat(points1, points2,
  // CV_FM_8POINT);
  // std::cout << "fundamental_matrix is " << std::endl
  //           << fundamental_matrix << std::endl;

  //-- 计算本质矩阵
  cv::Point2d principal_point(K.at<double>(0, 2), K.at<double>(1, 2));
  double focal_length = K.at<double>(0, 0); //相机焦距
  cv::Mat essential_matrix;
  essential_matrix = cv::findEssentialMat(points1, points2, K);
  // essential_matrix = cv::findEssentialMat(points1, points2, focal_length,
  // principal_point);
  std::cout << "essential_matrix is " << std::endl
            << essential_matrix << std::endl;

  //-- 计算单应矩阵
  // cv::Mat homography_matrix;
  // homography_matrix = cv::findHomography(points1, points2, cv::RANSAC, 3);
  // std::cout << "homography_matrix is " << std::endl
  //           << homography_matrix << std::endl;

  //-- 从本质矩阵中恢复旋转和平移信息.
  cv::recoverPose(essential_matrix, points1, points2, K, R, t);
  // cv::recoverPose(essential_matrix, points1, points2, R, t, focal_length,
  // principal_point);
  std::cout << "R is " << std::endl << R << std::endl;
  std::cout << "t is " << std::endl << t << std::endl;
}


#include <ompl/base/spaces/DiscreteStateSpace.h>
#include <ompl/base/spaces/SO2StateSpace.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "fplus/fplus.hpp"
#include "initial.hh"

namespace planner::initial {
void make_initial_state(ob::ScopedState<cspace::CompositeSpace>& initial_state,
                        const spec::Initial& discrete_init,
                        const structures::object::ObjectSet& object_init,
                        const structures::robot::Robot& robot_init,
                        const tsl::hopscotch_map<Str, spec::DimId>& eqclass_dimensions,
                        const tsl::hopscotch_map<Str, spec::DimId>& discrete_dimensions,
                        const ob::SpaceInformationPtr& si) {
  auto robot_space =
  si->getStateSpace()->as<ob::CompoundStateSpace>()->getSubspace(cspace::ROBOT_SPACE);
  ob::ScopedState<ob::CompoundStateSpace> robot_state(robot_space);

  auto objects_space =
  si->getStateSpace()->as<ob::CompoundStateSpace>()->getSubspace(cspace::OBJECT_SPACE);
  ob::ScopedState<ob::CompoundStateSpace> objects_state(objects_space);

  auto eqclass_space =
  si->getStateSpace()->as<ob::CompoundStateSpace>()->getSubspace(cspace::EQCLASS_SPACE);
  ob::ScopedState<ob::CompoundStateSpace> eqclass_state(eqclass_space);

  auto discrete_space =
  si->getStateSpace()->as<ob::CompoundStateSpace>()->getSubspace(cspace::DISCRETE_SPACE);
  ob::ScopedState<ob::CompoundStateSpace> discrete_state(discrete_space);

  // Fill the discrete state
  for (const auto& dim : discrete_init) {
    if (fplus::map_contains(eqclass_dimensions, dim)) {
      eqclass_state->as<ob::DiscreteStateSpace::StateType>(eqclass_dimensions.at(dim))->value = 1;
    } else {
      discrete_state->as<ob::DiscreteStateSpace::StateType>(discrete_dimensions.at(dim))->value =
      1;
    }
  }

  // Fill the robot state
  if (robot_init.base_movable) {
    const auto& base_pose        = *robot_init.base_pose;
    const auto& base_translation = base_pose.translation();
    const auto& base_rotation    = Eigen::Quaterniond(base_pose.linear());
    ob::ScopedState<cspace::RobotBaseSpace> base_state(
    robot_space->as<ob::CompoundStateSpace>()->getSubspace(cspace::BASE_SPACE));
    base_state->setXYZ(base_translation(0, 0), base_translation(1, 0), base_translation(2, 0));

    const double siny_cosp =
    2.0 * (base_rotation.w() * base_rotation.z() + base_rotation.x() * base_rotation.y());
    const double cosy_cosp =
    1.0 - 2.0 * (base_rotation.y() * base_rotation.y() + base_rotation.z() * base_rotation.z());
    const double yaw          = std::atan2(siny_cosp, cosy_cosp);
    auto& base_state_rotation = base_state->rotation();
    base_state_rotation.value = yaw;

    base_state >> robot_state;
  }

  auto joint_space = robot_space->as<ob::CompoundStateSpace>()->getSubspace(cspace::JOINT_SPACE);
  ob::ScopedState<cspace::RobotJointSpace> joints_state(joint_space);
  for (const auto& joint : robot_init.controllable_joints) {
    const auto& [joint_name, _l, _u, initial_value] = joint.second;
    if (robot_space->as<ob::CompoundStateSpace>()->hasSubspace(joint_name)) {
      // Joint is continuous and has SO(2) state
      const auto& cont_joint_space =
      robot_space->as<ob::CompoundStateSpace>()->getSubspace(joint_name);
      ob::ScopedState<ob::SO2StateSpace> cont_joint_state(cont_joint_space);
      cont_joint_state->value = initial_value;
      cont_joint_state >> robot_state;
    } else if (robot_init.tree_nodes.at(joint_name)->idx) {
      // Joint is bounded and has R state
      const auto idx             = robot_init.tree_nodes.at(joint_name)->idx;
      joints_state->values[*idx] = initial_value;
    }
  }

  joints_state >> robot_state;

  // Fill the object state
  for (const auto& [name, object] : object_init) {
    const auto& pose        = object->initial_pose;
    const auto& translation = pose.getOrigin();
    const auto& rotation    = pose.getRotation();

    ob::ScopedState<cspace::ObjectSpace> object_state(
    objects_space->as<ob::CompoundStateSpace>()->getSubspace(name));
    object_state->setXYZ(translation.x(), translation.y(), translation.z());

    auto& state_rotation = object_state->rotation();
    state_rotation.x     = rotation.x();
    state_rotation.y     = rotation.y();
    state_rotation.z     = rotation.z();
    state_rotation.w     = rotation.w();

    object_state >> objects_state;
  }

  // Fill the full initial state
  objects_state >> initial_state;
  robot_state >> initial_state;
  eqclass_state >> initial_state;
  discrete_state >> initial_state;
}
}  // namespace planner::initial


#include <memory>
#include <random>

#include <Eigen/Core>
#include <aslam/common/memory.h>
#include <ceres-error-terms/common.h>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <maplab-common/gravity-provider.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

#include "map-optimization-legacy/test/6dof-test-trajectory-gen.h"

using imu_integrator::ImuIntegratorRK4;
using imu_integrator::kErrorStateSize;
using imu_integrator::kStateSize;
using imu_integrator::kStateOrientationBlockSize;
using imu_integrator::kPositionBlockSize;
using imu_integrator::kImuReadingSize;
using imu_integrator::kGyroReadingOffset;
using imu_integrator::kStatePositionOffset;
using imu_integrator::kStateOrientationOffset;

namespace map_optimization_legacy {

class ViwlsGraph : public ::testing::Test {
 protected:
  virtual void SetUp() {
    common::GravityProvider gravity_provider(
        common::locations::kAltitudeZurichMeters,
        common::locations::kLatitudeZurichDegrees);

    // No IMU noise considered.
    settings_.imu_sigmas.acc_bias_random_walk_noise_density = 0.0;
    settings_.imu_sigmas.acc_noise_density = 0.0;
    settings_.gravity_meter_by_second2 = gravity_provider.getGravityMagnitude();
    settings_.imu_sigmas.gyro_bias_random_walk_noise_density = 0.0;
    settings_.imu_sigmas.gyro_noise_density = 0.0;
    // Seed does not matter here, we don't want noise anyways.
    settings_.imu_noise_bias_seed = 1;
    settings_.landmark_seed = 10;
    settings_.landmark_variance_meter = 3.0;
    settings_.mode = test_trajectory_gen::Path::kCircular;
    settings_.num_of_landmarks = 500;
    // 8 points for path constraints.
    settings_.num_of_path_constraints = 8;
    settings_.sampling_time_second = 0.01;
  }

  void generatePath();

  void constructIntegrator() {
    integrator_ = std::shared_ptr<ImuIntegratorRK4>(
        new ImuIntegratorRK4(
            settings_.imu_sigmas.gyro_noise_density,
            settings_.imu_sigmas.gyro_bias_random_walk_noise_density,
            settings_.imu_sigmas.acc_noise_density,
            settings_.imu_sigmas.acc_bias_random_walk_noise_density,
            settings_.gravity_meter_by_second2));
  }

  void integrate() {
    integrator_->integrate(
        current_state_, debiased_imu_readings_, settings_.sampling_time_second,
        &next_state_, &next_phi_, &next_cov_);
  }

  test_trajectory_gen::PathAndLandmarkSettings settings_;

  std::shared_ptr<ImuIntegratorRK4> integrator_;
  Eigen::Matrix<double, kStateSize, 1> current_state_;
  Eigen::Matrix<double, 2 * kImuReadingSize, 1> debiased_imu_readings_;
  Eigen::Matrix<double, kStateSize, 1> next_state_;
  Eigen::Matrix<double, kErrorStateSize, kErrorStateSize> next_phi_;
  Eigen::Matrix<double, kErrorStateSize, kErrorStateSize> next_cov_;

  Eigen::Matrix3Xd true_positions_;
  Eigen::Matrix4Xd true_rotations_;
  Eigen::Matrix<double, imu_integrator::kImuReadingSize, Eigen::Dynamic>
      imu_data_;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

void ViwlsGraph::generatePath() {
  SixDofTestTrajectoryGenerator generator(settings_);

  LOG(INFO) << "Generating trajectory, may take a while...";
  generator.generate6DofPath();
  true_positions_ = generator.getTruePositions();
  true_rotations_ = generator.getTrueRotations();
  imu_data_ = generator.getImuData();
  generator.saveTrajectory("6dof_traj.csv");
  LOG(INFO) << "Number of path points: " << true_positions_.cols();
}

// This test verifies if the data generated from 6DoF trajectory generator
// (adds additional excitation to 4DoF path from mav_planning_utils)
// is properly integrated, i.e. it is matching the provided groundtruth poses.
TEST_F(ViwlsGraph, GeneratedTrajectoryVisualBundleAdj) {
  generatePath();
  constructIntegrator();

  current_state_.setZero();
  // Note that state contains B_q_G quaternion (it simplifies Jacobians).
  current_state_.block<kStateOrientationBlockSize, 1>(
      kStateOrientationOffset, 0) = true_rotations_.col(0);
  current_state_.block<kPositionBlockSize, 1>(kStatePositionOffset, 0) =
      true_positions_.col(0);

  for (int i = 0; i < (imu_data_.cols() - 1); ++i) {
    debiased_imu_readings_.block<kImuReadingSize, 1>(0, 0) = imu_data_.col(i);
    debiased_imu_readings_.block<kImuReadingSize, 1>(kImuReadingSize, 0) =
        imu_data_.col(i + 1);

    integrate();
    current_state_ = next_state_;

    Eigen::Vector3d position = current_state_.block<3, 1>(13, 0);
    Eigen::Quaterniond B_q_G_curr(current_state_.block<4, 1>(0, 0).data());
    Eigen::Quaterniond B_q_G_gt(true_rotations_.col(i + 1).data());
    EXPECT_NEAR_EIGEN(position, true_positions_.col(i + 1), 1e-5);
    EXPECT_NEAR_KINDR_QUATERNION(
        pose::Quaternion(B_q_G_curr), pose::Quaternion(B_q_G_gt), 1e-5);
  }
}

}  // namespace map_optimization_legacy

MAPLAB_UNITTEST_ENTRYPOINT


/*
Copyright (c) 2013, Sibt ul Hussain <sibt.ul.hussain at gmail dot com>
All rights reserved.
Released under BSD License
-------------------------
For license terms please see license.lic
*/
#include "../features/includeHeaders.h"
#include "../util/processImage.h"
#include "../features/customValidators.h"
#include <boost/program_options.hpp>
namespace po = boost::program_options;
#include <iterator>

UINT Features::xcell = 8, Features::ycell = 8, Features::norient = 36,
		Features::foffset = 8, Features::cellsize = 8;
//TODO: Remove it
//#include "../util/boosting.h"
//UINT DecisionStumps::dimfeature = 0;

void WriteFeatures(const string & fname, const UINT & xbmax, const UINT & ybmax,
		vector<REAL> &feat) {
	ofstream ofile(fname.c_str(), ios::out | ios::binary);
	if (!ofile) {
		cout << " Couldn't open the file" << fname << endl;
		exit(EXIT_FAILURE);
	}
	UINT version = 001, rsize = feat.size() / ybmax;
	ofile.write((char*) &version, sizeof(UINT));
	ofile.write((char*) &ybmax, sizeof(UINT));
	ofile.write((char*) &rsize, sizeof(UINT));
	REAL *featptr = &feat[0];
	ofile.write((char*) featptr, sizeof(REAL) * feat.size());
	ofile.close();
}

class Process {
public:
	Process(po::variables_map &obj, LBPParams &lbpparam, PatchParams &pparam,
			CBParams &cbparams) {
		fobj = NULL;
		pim = NULL;
		ProcessInput(obj, lbpparam, pparam, cbparams);
	}
	void
	ProcessInput(po::variables_map&, LBPParams &, PatchParams &, CBParams&);
	void DumpFeatures(const string &lstfile, const string &offile, UINT width,
			UINT height);
	void DumpGaborLQPFeatures(const string &lstfile, UINT nglevels, UINT width,
			UINT height);
	void DumpPartFeatures(const string &lstfile, const string &feattype,
			int xcellsize, int ycellsize, int sx, int sy, int ex, int ey);

	void InitializeImageProcessor(po::variables_map&);
	void ParseLocalPatternParams(po::variables_map&, LBPParams&);
	void ParsePatchParams(po::variables_map&, LBPParams&, CBParams&);
	string InitializeLocalPatternFeatures(const FeatureTypes &ftype,
			LBPParams &lbpparam, UINT width, UINT height, UINT nplevels);
	~Process() {
		delete fobj;
		delete pim;
	}
private:
	Features *fobj;
	ProcessImage *pim;
	string dir;
};
void Process::InitializeImageProcessor(po::variables_map&obj)
// initialize different Image Processing Parameters...
		{
	//§§§§§§§§§§§§§§§§§§§§§§§    Process Image Parameter       ////////////////////:::
	Channel ch = obj["Color-Channel"].as<Channel>(); // which color channel to use..
	REAL gnorm = obj["Gamma-Normalization"].as<REAL>();
	REAL alpha = obj["Cont-Alpha"].as<REAL>();
	REAL tou = (REAL) obj["Cont-Tou"].as<REAL>() / 255;
	REAL k1 = obj["DoG-K1"].as<REAL>();
	REAL k2 = obj["DoG-K2"].as<REAL>();
	bool dog = obj["DoG"].as<bool>();
	bool coneq = obj["Cont-Equal"].as<bool>();
	pim = new ProcessImage(gnorm, dog, k1, k2, coneq, tou, alpha, ch);
}

void Process::DumpFeatures(const string &lstfile, const string &feattype,
		UINT width, UINT height) {
	vector<REAL> feat(fobj->GetDim(width, height));
	UINT xbmax = (UINT) (round((REAL) width / (REAL) fobj->cellsize)), ybmax =
			(UINT) (round((REAL) height / (REAL) fobj->cellsize)), wcount = 0;
	cout << " \n Feature Size for Window " << width << " X " << height << " ="
			<< feat.size() << endl << " Xbmax=" << xbmax << " , Ybmax= "
			<< ybmax << " , Cell Size " << fobj->cellsize << endl;

	vector<string> lstimages;
	ParseListFile(lstfile, lstimages);
	Image im;
	stringstream ss;
	for (UINT i = 0; i < lstimages.size(); ++i) {
		im.read(lstimages[i]);
		cout << "\n Processing Image " << lstimages[i] << flush << endl;
		fobj->InitalizeMaps(im, NoPyramid);
		fobj->GetFeatures(0, 0, 0, width, height, feat);
		ss.str("");
		ss << SplitFilenameFromDir(lstimages[i]) << "-" << feattype;
		WriteFeatures(ss.str(), xbmax, ybmax, feat);
		cout << " Number = " << wcount << " / " << lstimages.size() * 2;
		++wcount;
		// Flip the image
		im.flop();
		fobj->InitalizeMaps(im, NoPyramid);
		fobj->GetFeatures(0, 0, 0, width, height, feat);
		ss.str("");
		ss << SplitFilenameFromDir(lstimages[i]) << "-" << feattype << "-f";
		WriteFeatures(ss.str(), xbmax, ybmax, feat);
		cout << " Number = " << wcount << " / " << lstimages.size() * 2;
		++wcount;
	}
}
void Process::DumpGaborLQPFeatures(const string &lstfile, UINT nglevels,
		UINT width, UINT height) {
	vector<REAL> feat(fobj->GetDim(width, height));
	UINT xbmax = (UINT) (round((REAL) width / (REAL) fobj->cellsize)), ybmax =
			(UINT) (round((REAL) height / (REAL) fobj->cellsize)), wcount = 0;
	cout << " \n Feature Size for Window " << width << " X " << height << " ="
			<< feat.size() << endl << " Xbmax=" << xbmax << " , Ybmax= "
			<< ybmax << " , Cell Size " << fobj->cellsize << endl;

	vector<string> lstimages;
	ParseListFile(lstfile, lstimages);
	stringstream ss;

	if (lstimages.size() % (nglevels * 2 + 1) != 0) {
		cout
				<< "\n Error! Number of images in the list file is not factor of (Number of Gabor-LQP Levels)"
				<< endl;
		exit(EXIT_FAILURE);
	}
	vector<Image> image(nglevels * 2 + 1), fimage(nglevels * 2 + 1);
	string feattype = "glqp";
	for (UINT i = nglevels; i < lstimages.size(); i += (nglevels * 2 + 1)) {
		cout << " Processing Image # = " << (i / nglevels + 1) << " "
				<< lstimages[i] << endl;
		for (UINT j = i - nglevels, count = 0; j <= i + nglevels;
				++j, ++count) {
			image[count].read(lstimages[j]);
			fimage[count] = image[count];
			fimage[count].flop();
		}

		fobj->InitalizeMaps(image);
		fobj->GetFeatures(0, 0, 0, width, height, feat);
		ss.str("");
		ss << SplitFilenameFromDir(lstimages[i]) << "-" << feattype;
		WriteFeatures(ss.str(), xbmax, ybmax, feat);
		cout << " Number = " << wcount << " / " << lstimages.size() * 2;
		++wcount;

		fobj->InitalizeMaps(fimage);
		fobj->GetFeatures(0, 0, 0, width, height, feat);
		ss.str("");
		ss << SplitFilenameFromDir(lstimages[i]) << "-" << feattype << "-f";
		WriteFeatures(ss.str(), xbmax, ybmax, feat);
		cout << " Number = " << wcount << " / " << lstimages.size() * 2;
		++wcount;
	}
}
void Process::DumpPartFeatures(const string &lstfile, const string &feattype,
		int xcellsize, int ycellsize, int sx, int sy, int ex, int ey) {
	int width = ex - sx + 1, height = ey - sy + 1;
	vector<REAL> feat(fobj->GetDim(width, height));
	UINT xbmax = (UINT) (round((REAL) width / (REAL) xcellsize)), ybmax =
			(UINT) (round((REAL) height / (REAL) ycellsize)), wcount = 0;
	cout << " \n Feature Size for Window " << width << " X " << height << " ="
			<< feat.size() << endl << " Xbmax=" << xbmax << " , Ybmax= "
			<< ybmax << " , xCell Size " << xcellsize << " , yCell Size "
			<< ycellsize << endl;

	vector<string> lstimages;
	ParseListFile(lstfile, lstimages);
	Image im;
	stringstream ss;
	for (UINT i = 0; i < lstimages.size(); ++i) {
		im.read(lstimages[i]);
		cout << "\n Processing Image " << lstimages[i] << flush << endl;
		fobj->ComputeTextureFeatures(im, xcellsize, ycellsize, sx, sy, ex, ey,
				feat);
		ss.str("");
		ss << SplitFilenameFromDir(lstimages[i]) << "-" << feattype;
		WriteFeatures(ss.str(), xbmax, ybmax, feat);
		cout << " Number = " << wcount << " / " << lstimages.size() * 2;
		++wcount;
		// Flip the image
		im.flop();
		int tsx = im.columns() - ex, tex = im.columns() - sx + 1;
		fobj->ComputeTextureFeatures(im, xcellsize, ycellsize, tsx, sy, tex, ey,
				feat);
		ss.str("");
		ss << SplitFilenameFromDir(lstimages[i]) << "-" << feattype << "-f";
		WriteFeatures(ss.str(), xbmax, ybmax, feat);
		cout << " Number = " << wcount << " / " << lstimages.size() * 2;
		++wcount;
	}
}

string Process::InitializeLocalPatternFeatures(const FeatureTypes &ftype,
		LBPParams &lbpparam, UINT width, UINT height, UINT nplevels) {

	string feattype;
#ifndef WINDOW_CLASSIFIER
	/*For different Colour Channels, and Gray Scale with L1, L1Sqrt, L2, Normalization...*/
	/*There are two different types of implementations (i.e With Cell & Without Cell Suffix)
	 1. Cell Suffix: Features are computed for the complete image at Once
	 (image level) and features can be extracted with step size=single cell.
	 Note that number of pixels = single cell size are ignored around the four sides to avoid the boundary artifacts.
	 2. Without Cell Suffix: Features are computed at a given window (window level) thus can be computed with any step size.
	 */
	switch (ftype) {
		case FT_LBP:
		feattype = "LBP";
		fobj = new LBPFeaturesRGBBlock(lbpparam, width, height, nplevels,
				*pim);
		break;
		case FT_LTP:
		feattype = "LTP";
		fobj = new OnlyLTPFeaturesRGBCell(lbpparam, width, height,
				nplevels, *pim);
		break;
		case FT_LBPLTP:
		feattype = "LBPLTP";
		fobj = new LTPFeaturesRGBCell(lbpparam, width, height, nplevels,
				*pim);
		break;
	}
#else
	/*For Backward Compatibility, Works at window levels*/
	switch (ftype) {
	case FT_LBP:
		feattype = "LBP";
		fobj = new LBPFeatures(lbpparam, width, height, nplevels, *pim);
		break;
	case FT_LTP:
		feattype = "LTP";
		fobj = new OnlyLTPFeatures(lbpparam, width, height, nplevels, *pim);
		break;
	case FT_LBPLTP:
		feattype = "LBPLTP";
		fobj = new LTPFeatures(lbpparam, width, height, nplevels, *pim);
		break;
	}
#endif
	return feattype;
}
void Process::ProcessInput(po::variables_map &obj, LBPParams &lbpparam,
		PatchParams &pparam, CBParams &cbparam) {
	string feattype;
	UINT width, height;
	InitializeImageProcessor(obj); // initialize Image Processor...

	pparam.cbparams = &cbparam;
	pparam.ncodelevels = pparam.petype;

	width = obj["Win-Width"].as<int>(); // Reference Width & Height of images or image windows
	height = obj["Win-Height"].as<int>();

	string tfile;
	if (obj.count("TrainingFile"))

		tfile = obj["TrainingFile"].as<string>();
	else
		tfile = cbparam.vimgfname;
	//	UINT xstride = lbpparam.cellsize, ystride = xstride;
	UINT nplevels = 1; // obj["Number-Levels"].as<int> (); // number of pyramid levels

	// Parse Miscelleneous parameters
	FeatureTypes ftype = obj["FeatureType"].as<FeatureTypes>();
	int nglevels = obj["GaborLQPLevels"].as<int>();

	/* Region window coordinates used to learn Region level codebook (these coordinates can be used to compute part-level features
	 * or features for a given region)
	 */
	int sx, sy, ex, ey, xcellsize, ycellsize;
	sx = obj["WinX1"].as<int>(); /* */
	sy = obj["WinY1"].as<int>();
	ex = obj["WinX2"].as<int>();
	ey = obj["WinY2"].as<int>();
	xcellsize = lbpparam.cellsize;
	ycellsize = lbpparam.cellsize;

	cbparam.sx = sx;
	cbparam.sy = sy;
	cbparam.ex = ex;
	cbparam.ey = ey;

	/* Normalize the tolerance Values to be in the range [0,1] */
	lbpparam.tolerance = lbpparam.tolerance / 255.0;
	lbpparam.lbptolerance = lbpparam.lbptolerance / 255.0;

	if (obj.count("ToleranceLevels")) {
		pparam.tolval = obj["ToleranceLevels"].as<vector<REAL> >();
		for (vector<REAL>::iterator iter = pparam.tolval.begin();
				iter != pparam.tolval.end(); ++iter)
			*iter = *iter / 255;

	} else {
		pparam.tolval.resize(1, 0);
		pparam.tolval[0] = lbpparam.tolerance;
	}

	/* Now Compute the features */
	switch (ftype) {
	case FT_LBP:
	case FT_LTP:
	case FT_LBPLTP:
		feattype = InitializeLocalPatternFeatures(ftype, lbpparam, width,
				height, nplevels);
		break;

	case FT_GaborLQP:
	case FT_LQPSoft:
	case FT_SeparateGaborLQP:
	case FT_EncodedLTP: // Generate Dictionary of the features...
	case FT_EncodedLBP:
	case FT_EncodedLBPPatch:
	case FT_EncodedLTPPatch:
	case FT_EncodedGenericPatch:
	case FT_EncodedPatchPlusOther:
	case FT_EncodedRFPatchFeatures:
	case FT_MultipleEncodedGenericPatchFeatures:
		// always uses  non-symmetric, current implementation does not learn symmetric vocabularies
		feattype = "LQP";
		EncodedPatchFeatures *tobj;
		switch (ftype) {
		case FT_EncodedGenericPatch:
		case FT_LQPSoft:
		case FT_SeparateGaborLQP:
		case FT_GaborLQP:
			switch (cbparam.cbtype) {
			case CBT_CellQuantizeAll:
				fobj = new MultipleCBQuantizeLQPFeatures(lbpparam, width,
						height, nplevels, *pim, pparam);
				break;
			case CBT_PosCrop:
			case CBT_PosPyramid:
			case CBT_Complete:
			case CBT_NegativePosDiscriminant:
			default:
				if (ftype == FT_LQPSoft)
					fobj = new SoftLQPFeatures(lbpparam, width, height,
							nplevels, *pim, pparam);
				else if (ftype == FT_GaborLQP)
					fobj = new GaborLQPFeatures(lbpparam, width, height,
							nplevels, *pim, pparam, nglevels);
				else if (ftype == FT_SeparateGaborLQP) {
					cout << " \n Featurey TYpe = FT_SeparateGaborLQP ";
					fobj = new SeparateGaborLQPFeatures(lbpparam, width, height,
							nplevels, *pim, pparam, nglevels);
				} else
					fobj = new EncodedPatchFeatures(lbpparam, width, height,
							nplevels, *pim, pparam);
				break;
			}
			break;
		}
		fobj->GenerateCodeBook(); //
		break;
	default:
		cout
				<< " \n UnRecognized Feature Type: Please Select a Valid Feature Type"
				<< endl;
		exit(EXIT_FAILURE);
	}
	if (obj.count("OnlyGenerateCodeBook")) {
		cout << "\n Program was called only for the generation of codebooks"
				<< ", It's finished so Quittttttttttting..... " << endl;
		exit(EXIT_SUCCESS);
	}
	if (ftype == FT_GaborLQP || ftype == FT_SeparateGaborLQP)
		DumpGaborLQPFeatures(tfile, nglevels, width, height);
	else if (cbparam.sx >= 0 && cbparam.sy >= 0)
		DumpPartFeatures(tfile, feattype, xcellsize, ycellsize, sx, sy, ex, ey);
	else
		DumpFeatures(tfile, feattype, width, height);
}
void AddLBPOptions(po::options_description &lbp_options, LBPParams &lbpparam) {

	//	lbpparam.gridtype = (GridType) obj["LBPGridType"].as<int> ();
	//	lbpparam.addfeat = (obj["Add-RGBLBP"].as<int> () == 1 ? true : false);
	//	lbpparam.cellsize = obj["Cell-Size"].as<int> ();
	//	lbpparam.radius = obj["Radius"].as<int> ();
	//	lbpparam.npoints = obj["Number-Points"].as<int> ();
	//	lbpparam.nflevels = obj["Feature-Levels"].as<int> ();
	//	lbpparam.stride = obj["LBP-Stride"].as<int> ();
	//	lbpparam.nbins = obj["LBP-NBins"].as<int> ();
	//
	//	lbpparam.tolerance = ((REAL) obj["LTP-Tolerance"].as<int> ()) / 255.0;
	//	lbpparam.lbptolerance = ((REAL) obj["LBP-Tolerance"].as<int> ()) / 255.0;
	//
	//	lbpparam.normfeatsep = (obj["Norm-Sep"].as<int> () == 1 ? true : false);
	//	lbpparam.featnorm = (NORM) obj["Normalization"].as<int> ();
	//	//	lbpparam.folded = obj.count("Fold-Features"); // obselete ...
	//	lbpparam.nthrlevels = obj["LTPLevels"].as<int> ();
	//	lbpparam.extent = obj["Extent"].as<int> ();
	//	lbpparam.hmethod = (HistogramMethod) obj["HistogramMethod"].as<int> ();
	//	lbpparam.useblocknorm = obj.count("BlockNorm");
	//	lbpparam.usemeanval = obj.count("UseMeanCenVal");
	//	lbpparam.usegradmag = obj.count("UseGradMag");
	lbp_options.add_options()("LBPGridType",
			po::value<GridType>(&lbpparam.gridtype)->default_value(Circular),
			"0. Circular\n 2. Rectangular")("Add-RGBLBP",
			po::value<bool>(&lbpparam.addfeat)->default_value(1),
			"Merge RGB Channels \n0. Concat RGB-Channels Hist\n1. Add RGB-Channels Hist  ")(
			"Norm-Sep",
			po::value<bool>(&lbpparam.normfeatsep)->default_value(0),
			"For Above Option 1: \n0. Add RGB-channels & then Normalize Once \n1. Normalize Each RGB channel & then add")/*
	 // Not used
	 ("Skip",
	 po::value<int>()->default_value(0),
	 "Skip 'Skip' Number of Pixels on Borders")*/("LBP-NBins",
			po::value<UINT>(&lbpparam.nbins)->default_value(59),
			"Number of LBPBins can be either 256 or 59")("Cell-Size",
			po::value<UINT>(&lbpparam.cellsize)->default_value(10),
			"LBP Feature: Cell size")("Radius",
			po::value<UINT>(&lbpparam.radius)->default_value(1),
			"Radius of LBP Features")("Number-Points",
			po::value<UINT>(&lbpparam.npoints)->default_value(8),
			"Number of Points around each point")("Feature-Levels",
			po::value<UINT>(&lbpparam.nflevels)->default_value(1),
			"Number of Feature Levels(default = 1)")("LTP-Tolerance",
			po::value<float>(&lbpparam.tolerance)->default_value(5.0),
			"+,- Threshold for the LTP Features (in the range [0,255])")(
			"LBP-Tolerance",
			po::value<float>(&lbpparam.lbptolerance)->default_value(0.0),
			"Threshold used during some types of LBP Features...")(
			"Normalization",
			po::value<NORM>(&lbpparam.featnorm)->default_value(L1sqrt),
			"Histogram Normalization :\n 0. L1 Norm  \n 1. L1Sqrt Norm"
					" \n 2. L2 Norm \n 3. LOG2(some cases) \n 6. Average(of RGB Channels for Texture Case)")(
			"LBP-Stride", po::value<UINT>(&lbpparam.stride)->default_value(16),
			"LBP Cell Stride-Size in Pixels")("BlockNorm",
			po::bool_switch(&lbpparam.useblocknorm),
			"Use Neigbhour block normalization for LBP Same as HOG(by default false)")(
			"UseMeanCenVal", po::bool_switch(&lbpparam.usemeanval),
			"Use Mean of Square box radius X radius instead of center pixel value(by default false)")(
			"UseGradMag", po::bool_switch(&lbpparam.usegradmag),
			"Use Gradient as voting for the LBP/LTP histogram  bins(by default false)")(
			"LTPLevels",
			po::value<UINT>(&lbpparam.nthrlevels)->default_value(1),
			"Number of LTP Level's(Threshold is used as base)")(
			"HistogramMethod",
			po::value<HistogramMethod>(&lbpparam.hmethod)->default_value(
					HM_Bilinear), "Binning Method for Histograms "
					"\n 0. Discrete Binning "
					"\n 1. Bilinear Interpolation")("Extent",
			po::value<UINT>(&lbpparam.extent)->default_value(16),
			"Cropped Image Border");

}
void AddMiscOptions(po::options_description & misc_options) {
	misc_options.add_options()("FeatureType",
			po::value<FeatureTypes>()->default_value(FT_EncodedGenericPatch),
			"Features Type :\n"
					"1. LBP\n"
					"2. LTP\n"
					"3. LBPLTP\n"
					"14.LQP (EncodedPatchFeatures -- covers all the encoded features)\n"
					"22.GaborLQP -- Multiple Orientation-Scale Co-occurence LQP\n"
					"23.SeparateGaborLQP -- Combines LQP Computed over multiple orientations & scales at codebook level")(
			"Win-Width", po::value<int>()->default_value(64), "Window Width")(
			"Win-Height", po::value<int>()->default_value(128), "Window Height")/*(
	 "X-Stride", po::value<int>()->default_value(8),
	 "X Stride in Pixels (Must be a multiple of cell size)")("Y-Stride",
	 po::value<int>()->default_value(8), "Y Stride in Pixels")*/(
			"OnlyGenerateCodeBook",
			"LQP: Only Generate CodeBook and then quit...")("GaborLQPLevels",
			po::value<int>()->default_value(1),
			"Number of Scale and Orientation to include in Gabor-LQP")
	///Window Specification for learning codebooks & extracting features from a given region of the training images...
	("WinX1", po::value<int>()->default_value(-1),
			"Xcoord of window top left coordinate\n (for learning codebooks & extracting features from only the given region)")(
			"WinY1", po::value<int>()->default_value(-1),
			"Ycoord of window top left coordinate")("WinX2",
			po::value<int>()->default_value(-1),
			"Ycoord of window bottom right coordinate")("WinY2",
			po::value<int>()->default_value(-1),
			"Ycoord of window bottom right coordinate")("TrainingFile",
			po::value<string>(),
			"File contains list of training images for which features need to be computed")/*(
			 "Number-Levels", po::value<int>()->default_value(10),
			 "Number of Levels in each Pyramid Octave")("Pos-Train-File",
			 po::value<string>(), "File containing list of positive images")(
			 "Neg-Train-File", po::value<string>(),
			 "File containing list of negative images")

			 ///##################### PATCH PLUS FEATURE TYPE
			 ("PatchPlusFeatureType", po::value<int>()->default_value(0),
			 "Patch Plus Feature Type, One of FeatureType")("MEPF-ConfigFile",
			 po::value<string>()->default_value(""),
			 "MultipleEncodedPatchFeatures Configuration File")("NumberCircles",
			 po::value<int>()->default_value(1),
			 "Number of Concentric Circles for Computation of LBP/LTP")
			 //++++++++++++++++++++++++ Combined Features++++++++++++++++++++++++
			 ("Combined-Feature-Type,F", po::value<vector<int> >(),
			 "Specify Multiple features to have combined features from the Feature-Type")(
			 "Feature-ConfigFile", po::value<string>(),
			 "Feature Configuration File To Specify Various Combinations")*/;

}
void AddCBOptions(po::options_description & cb_options, CBParams &cbparam) {
	//	CBParams cbparam;

	//	cbparam.cbtype = (CBType) obj["CodeBook-Type"].as<int> ();
	//	cbparam.codebooksize = obj["CodeBookSize"].as<int> ();
	//	cbparam.nwinsampled = obj["CodeBook-NegSampled"].as<int> ();
	//	cbparam.nclusrounds = obj["ClusteringRounds"].as<int> ();
	//	cbparam.inituniform = false; // argument not used..
	//	cbparam.pmethod = (PoolingMethod) obj["CodeBook-Pooling"].as<int> ();
	//	cbparam.softq = (QuantizationType) obj["SoftQuantize"].as<int> ();
	//	cbparam.meanp = obj["MeanDivisor"].as<REAL> ();
	//	cbparam.cboffset = obj["ReadCodeBook"].as<string> ();
	//	cbparam.sx = sx;
	//	cbparam.ex = ex;
	//	cbparam.sy = sy;
	//	cbparam.ey = ey;

	// CodeBook Parameters
	cb_options.add_options()("CodeBookSize",
			po::value<UINT>(&cbparam.codebooksize)->default_value(100),
			"Code Book Size (Number of visual words) for Mapping the features")(
			"CodeBook-Type",
			po::value<CBType>(&cbparam.cbtype)->default_value(CBT_Complete),
			"Different CodeBook Types "
					"0. Complete - use complete training set to generate CB\n"
					"1. PosCrop - CB from positive crop images\n"
					"2. PosCropNegSampled - quantize against both poscrop and -ve CBs\n"
					"3. PosPyramid\n - quantize against scale-space positive images CBs\n"
					"4. PosPyramidNegSampled - quantize against both scale-space positive images and -ve CBs \n"
					"5. ConstantPosCellNeg - quantize each cell against all +ve and -ve CBss\n"
					"6. ConstantPosCellNegSingleQuantization - Window level histogram of cell CBss (obtained via option ConstantPosCellNeg.)\n"
					"7. PosCellNeg - Each Cell is mapped only against its own CB and -ve class CB; negatives cells are accumulated at window level \n"
					"8. PosCellNegSep - Same as PosCellNeg except negatives are not accumulated at windows levels\n"
					"9. Negative - quantize only against negative class codebook\n"
					"10.NegativeThenPosCrop - first initialize from negative class then purify with pos class\n"
					"11.MultiClass \n"
					"12.Discriminative- CodeBook is constructed from both positive and negative classes with an extra bin for codecount_pos/codecount_neg ratio\n"
					"13. A Separate CodeBook for each cell and each cell is quantized against all cells")(
			"CodeBook-DMetric", po::value<int>()->default_value(0),
			"Distance to Compute Code Book \n 0.L2 1.Hamming or L1")(
			"ClusteringRounds",
			po::value<UINT>(&cbparam.nclusrounds)->default_value(3),
			"Number of KMeans Clustering Rounds for generation of CodeBook")(
			"SoftQuantize",
			po::value<QuantizationType>(&cbparam.softq)->default_value(QT_VQ),
			"Different Vector Quantization Methods"
					"\n 0. VQ -- Normal KMeans"
					"\n 1. Mean "
					"\n 2. LLC Mapping"
					"\n 3. Discriminant")("CodeBook-Pooling",
			po::value<PoolingMethod>(&cbparam.pmethod)->default_value(PM_Sum),
			"Pooling Methods for codes"
					"\n 0. Sum Pooling"
					"\n 1. Max Pooling")("CodeBook-NegSampled",
			po::value<UINT>(&cbparam.nwinsampled)->default_value(40),
			"Number of -ve windows to Sampled For Negative CodeBook(0=Complete Negative Set)")(
			"MeanDivisor", po::value<REAL>(&cbparam.meanp)->default_value(1),
			"Divisor(md) of mean distance to use as distance limiting threshold ie max(0,mu/md-d_i)")(
			"ReadCodeBook",
			po::value<string>(&cbparam.cboffset)->default_value(""),
			"post-fix flag for file containing the "
					"\n LQP lookup table (if specified codebook is read from the given file otherwise no)")(
			"Validation-File",
			po::value<string>(&cbparam.vimgfname)->default_value(""),
			"Validation File Used for CodeBook Generation");
	;
}
void AddLQPOptions(po::options_description &lqp_options, PatchParams & pparam) {
	//	pparam.size = obj["Patch-PatchSize"].as<int> ();
	//	pparam.cpatchsize = obj["Patch-CircPatchSize"].as<int> ();
	//	pparam.stride = obj["Patch-PatchStride"].as<int> ();
	//	pparam.type = (PatchType) obj["PatchType"].as<int> ();
	//	pparam.pstep = obj["Patch-PixelStride"].as<int> ();
	//	pparam.ncodelevels = obj["Patch-CodingType"].as<int> ();
	//	pparam.pcount = obj["Patch-PruneCount"].as<int> ();
	//	pparam.petype = (PatchEncodingType) obj["Patch-CodingType"].as<int> ();
	//	if (obj.count("ToleranceLevels")) {
	//		pparam.tolval = obj["ToleranceLevels"].as<vector<REAL> > ();
	//		for (UINT i = 0; i < pparam.tolval.size(); ++i)
	//			pparam.tolval[i] /= 255;
	//	} else {
	//		pparam.tolval.resize(1, 0);
	//		pparam.tolval[0] = ((REAL) obj["LTP-Tolerance"].as<int> ()) / 255.0;
	//	}
	//	pparam.cbparams = &cbparam;

	// Image Processor
	lqp_options.add_options() // Image Processor
	// Patch Parameters
	("PatchType",
			po::value<PatchType>(&pparam.type)->default_value(PT_Circular),
			"Patch Type \n "
			//PT_HStrip, PT_VStrip, PT_Circular, PT_Diag, PT_ADiag
					"0. Horizontal Strip\n"
					"1. Vertical Strip\n"
					"2. Circular/Rect Patch\n"
					"3. Diagonal Strip\n"
					"4. ADiagonal Strip\n"
					"5. Hor+Vert Strip\n"
					"6. Diag+ADiag Strip\n"
					"7. Hor+Vert+Circ Strip\n"
					"8. Diag+ADiag+Circ Strip\n"
					"9. Hor+Vert+Diag+ADiag Strip\n"
					"10. Hor+Vert+Diag+ADiag Neighbour/comparsion Strip\n"
					"11. Haar Pyramid With 4x4 Neighbourhood\n"
					"12. Circular Shallow Only Outer Ring is considered\n"
					"13. Circular+Angular+ Radial difference among the pixels\n")(
			"Patch-PatchSize", po::value<UINT>(&pparam.size)->default_value(5),
			"Patch size (diameter) for Patch Features")("Patch-CodingType",
			po::value<PatchEncodingType>(&pparam.petype)->default_value(
					PET_LTP), "Patch Coding Type\n"
					"0. PET_LBP\n"
					"1. PET_LTP\n"
					"2. PET_CSLBP\n"
					"3. PET_CSLTP\n"
					"4. PET_SplitLTP\n")("Patch-CircPatchSize",
			po::value<UINT>(&pparam.cpatchsize)->default_value(0),
			"Circular Patch size works only with patch type 7&8")(
			"Patch-PatchStride",
			po::value<UINT>(&pparam.stride)->default_value(1),
			"Distance Between LQP Patches(2 means skip every 2nd Pixel)")(
			"Patch-PixelStride",
			po::value<UINT>(&pparam.pstep)->default_value(1),
			"Pixel Stride inside the patch")("ToleranceLevels",
			po::value<vector<REAL> >(&pparam.tolval)/*->default_value(
			 vector<REAL>(1, 5.0), "default")*/,
			" Tolerance"
					"Values for MultiLevelLQPs i.e Quinary (Input=Vector) in the range [0,255]")(
			"Patch-PruneCount",
			po::value<UINT>(&pparam.pcount)->default_value(0),
			"Prune the codes having count < PruneCount")

	("CodeBook-HistogramNormalization",
			po::value<EncodedHistogramNormType>(&pparam.hnormtype)->default_value(
					EHNT_Global),
			"CodeBook Histogram Normalization For Cell Based CodeBooks \n "
					"0. EHNT_Local\n 1. EHNT_Global\n2. EHNT_Both");

}
void AddImageProcessOptions(po::options_description &im_options
/*,ProcessImage *pim*/) {

	// Image Processor
	im_options.add_options()("Color-Channel",
			po::value<Channel>()->default_value(RGB),
			"Image Color Channel to use "
					"0. Red,\n"
					"1. Green,\n"
					"2. Blue,\n"
					"3. Gray,\n"
					"4. RGB,\n"
					"5. O1,\n"
					"6. O2,\n"
					"7. O3,\n"
					"8. HSL,\n"
					"9. LAB,\n"
					"10. WandellOCS,\n"
					"11. SandeOCS,\n"
					"12. ABSGradientMag\n")("Gamma-Normalization",
			po::value<REAL>()->default_value(1),
			"Perform Gamma Normalization (for LBP & LTP)")("DoG",
			po::bool_switch(), "Apply DoG Filter")("DoG-K1",
			po::value<REAL>()->default_value(1),
			"DoG filter Inner kernel StDev")("DoG-K2",
			po::value<REAL>()->default_value(3),
			"DoG filter outer kernel StDev")("Cont-Equal", po::bool_switch(),
			"Perform Contrast Equalization")("Cont-Alpha",
			po::value<REAL>()->default_value(0.1),
			"Contrast Equalization:Alpha value")("Cont-Tou",
			po::value<REAL>()->default_value(10),
			"Contrast Equalization:Threshold value in the range [0,255]");
}
void AddImageProcessOptions(po::options_description &im_options,
		ProcessImage *pim) {

	//	Channel ch = (Channel) obj["Color-Channel"].as<int> (); // which color channel to use..
	//	REAL gnorm = obj["Gamma-Normalization"].as<REAL> ();
	//	REAL alpha = obj["Cont-Alpha"].as<REAL> ();
	//	REAL tou = (REAL) obj["Cont-Tou"].as<int> () / 255;
	//	REAL k1 = obj["DoG-K1"].as<REAL> ();
	//	REAL k2 = obj["DoG-K2"].as<REAL> ();
	//	bool dog = obj.count("DoG");
	//	bool coneq = obj.count("Cont-Equal");

	Channel ch;
	bool dog, coneq;
	REAL gnorm, alpha, tou, k1, k2;
	// Image Processor
	im_options.add_options()("Color-Channel",
			po::value<Channel>(&ch)->default_value(RGB),
			"Image Color Channel to use "
					"0. Red,\n"
					"1. Green,\n"
					"2. Blue,\n"
					"3. Gray,\n"
					"4. RGB,\n")("Gamma-Normalization",
			po::value<REAL>(&gnorm)->default_value(1),
			"Perform Gamma Normalization (for LBP & LTP)")("DoG",
			po::bool_switch(&dog), "Apply DoG Filter")("DoG-K1",
			po::value<REAL>(&k1)->default_value(1),
			"DoG filter Inner kernel StDev")("DoG-K2",
			po::value<REAL>(&k2)->default_value(3),
			"DoG filter outer kernel StDev")("Cont-Equal",
			po::bool_switch(&coneq), "Perform Contrast Equalization")(
			"Cont-Alpha", po::value<REAL>(&alpha)->default_value(0.1),
			"Contrast Equalization:Alpha value")("Cont-Tou",
			po::value<REAL>(&tou)->default_value(10),
			"Contrast Equalization:Threshold value");

	pim = new ProcessImage(gnorm, dog, k1, k2, coneq, tou, alpha, ch);

}

int main(int ac, char* av[]) {

	// Set Program options
	po::options_description generic("Generic Options");
	generic.add_options()("help", "Produce help message");

	po::options_description input_options("Input/Output/Miscellaneous Options");
	po::options_description im_options("Image Processor (IP) Options");
	po::options_description lbp_options("Local Patterns (LBP/LTP) Options");
	po::options_description lqp_options("Local Patterns (LQP) Options");
	po::options_description cb_options("LQP CodeBook Options");

	LBPParams lbpparam;
	PatchParams lqpparam;
	CBParams cbparam;

	AddMiscOptions(input_options);
	AddImageProcessOptions(im_options);
	AddLBPOptions(lbp_options, lbpparam);
	AddLQPOptions(lqp_options, lqpparam);
	AddCBOptions(cb_options, cbparam);
	po::options_description all_options;
	all_options.add(generic).add(input_options).add(im_options).add(lbp_options).add(
			lqp_options).add(cb_options);
	//	po::variables_map vm;
	//	po::store(po::command_line_parser(argc, argv).options(all_options).run(),
	//			vm);
	//	po::notify(vm);

	try {
		po::variables_map vm;
		po::store(
				po::parse_command_line(ac, av, all_options,
						(po::command_line_style::default_style
								^ po::command_line_style::allow_guessing)), vm);

		po::notify(vm);

		if (vm.count("help")) {
			cout << all_options << "\n";
			return 1;
		} else
			Process pobj(vm, lbpparam, lqpparam, cbparam);
	} catch (exception& e) {
		cerr << "error: " << e.what() << "\n";
		return 1;
	} catch (...) {
		cerr << "Exception of unknown type!\n";
	}
	return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IS_NEQ_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_NEQ_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-operator
    Function object implementing is_neq capabilities

    Returns @ref True if x and y are not equal else @ref False.
    Infix notation can be used with operator '!='.

    This is a convenient alias of @ref is_not_equal
  **/
  const boost::dispatch::functor<tag::is_neq_> is_neq = {};
} }
#endif

#include <boost/simd/function/scalar/is_not_equal.hpp>
#include <boost/simd/function/simd/is_neq.hpp>

#endif