/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_MATH_ROTATION3D_HPP
#define RW_MATH_ROTATION3D_HPP

/**
 * @file Rotation3D.hpp
 */
#if !defined(SWIG)
#include <rw/common/Serializable.hpp>
#include <rw/core/macros.hpp>
#include <rw/math/Vector3D.hpp>

#include <Eigen/Core>
#include <limits>
#endif

namespace rw { namespace math {

    template< class T > class Rotation3DVector;
    /** @addtogroup math */
    /* @{*/

#if !defined(SWIGJAVA)
    /**
     * @brief A 3x3 rotation matrix \f$ \mathbf{R}\in SO(3) \f$
     *
     * @f$
     *  \mathbf{R}=
     *  \left[
     *  \begin{array}{ccc}
     *  {}^A\hat{X}_B & {}^A\hat{Y}_B & {}^A\hat{Z}_B
     *  \end{array}
     *  \right]
     *  =
     *  \left[
     *  \begin{array}{ccc}
     *  r_{11} & r_{12} & r_{13} \\
     *  r_{21} & r_{22} & r_{23} \\
     *  r_{31} & r_{32} & r_{33}
     *  \end{array}
     *  \right]
     * @f$
     */

#endif
    template< class T = double > class Rotation3D
    {
      public:
        //! Value type.
        typedef T value_type;

        //! @brief The type of the internal Eigen matrix implementation.
        typedef Eigen::Matrix< T, 3, 3 > EigenMatrix3x3;

        /**
           @brief A rotation matrix with uninitialized storage.
         */
        Rotation3D ()
        {
            _m (0, 0) = 1;
            _m (0, 1) = 0;
            _m (0, 2) = 0;
            _m (1, 0) = 0;
            _m (1, 1) = 1;
            _m (1, 2) = 0;
            _m (2, 0) = 0;
            _m (2, 1) = 0;
            _m (2, 2) = 1;
        }

#if !defined(SWIGJAVA)
        /**
         * @brief Constructs an initialized 3x3 rotation matrix
         *
         * @param r11 \f$ r_{11} \f$
         * @param r12 \f$ r_{12} \f$
         * @param r13 \f$ r_{13} \f$
         * @param r21 \f$ r_{21} \f$
         * @param r22 \f$ r_{22} \f$
         * @param r23 \f$ r_{23} \f$
         * @param r31 \f$ r_{31} \f$
         * @param r32 \f$ r_{32} \f$
         * @param r33 \f$ r_{33} \f$
         *
         * @f$
         *  \mathbf{R} =
         *  \left[
         *  \begin{array}{ccc}
         *  r_{11} & r_{12} & r_{13} \\
         *  r_{21} & r_{22} & r_{23} \\
         *  r_{31} & r_{32} & r_{33}
         *  \end{array}
         *  \right]
         * @f$
         */

#endif
        Rotation3D (T r11, T r12, T r13, T r21, T r22, T r23, T r31, T r32, T r33)
        {
            _m (0, 0) = r11;
            _m (0, 1) = r12;
            _m (0, 2) = r13;
            _m (1, 0) = r21;
            _m (1, 1) = r22;
            _m (1, 2) = r23;
            _m (2, 0) = r31;
            _m (2, 1) = r32;
            _m (2, 2) = r33;
        }

#if !defined(SWIGJAVA)
        /**
         * @brief Constructs an initialized 3x3 rotation matrix
         * @f$ \robabx{a}{b}{\mathbf{R}} =
         * \left[
         *  \begin{array}{ccc}
         *   \robabx{a}{b}{\mathbf{i}} & \robabx{a}{b}{\mathbf{j}} & \robabx{a}{b}{\mathbf{k}}
         *  \end{array}
         * \right]
         * @f$
         *
         * @param i @f$ \robabx{a}{b}{\mathbf{i}} @f$
         * @param j @f$ \robabx{a}{b}{\mathbf{j}} @f$
         * @param k @f$ \robabx{a}{b}{\mathbf{k}} @f$
         */

#endif
        Rotation3D (const rw::math::Vector3D< T >& i, const rw::math::Vector3D< T >& j,
                    const rw::math::Vector3D< T >& k)
        {
            _m (0, 0) = i[0];
            _m (0, 1) = j[0];
            _m (0, 2) = k[0];
            _m (1, 0) = i[1];
            _m (1, 1) = j[1];
            _m (1, 2) = k[1];
            _m (2, 0) = i[2];
            _m (2, 1) = j[2];
            _m (2, 2) = k[2];
        }

#if !defined(SWIGJAVA)
        /**
         * @brief Initialize Rotation3D from other rotation types
         * @param rotVec [in] rotation type such as \b EAA, \b RPY, or \b Quaternion
         */
        explicit Rotation3D (const Rotation3DVector< T >& rotVec);

        /**
         * @brief Constructs a 3x3 rotation matrix set to identity
         * @return a 3x3 identity rotation matrix
         *
         * @f$
         * \mathbf{R} =
         * \left[
         * \begin{array}{ccc}
         * 1 & 0 & 0 \\
         * 0 & 1 & 0 \\
         * 0 & 0 & 1
         * \end{array}
         * \right]
         * @f$
         */

#endif
        static const Rotation3D& identity ();

        /**
         * @brief Normalizes the rotation matrix to satisfy SO(3).
         *
         * Makes a normalization of the rotation matrix such that the columns
         * are normalized and othogonal s.t. it belongs to SO(3).
         */
        void normalize ();
#if !defined(SWIG)
        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        inline T& operator() (size_t row, size_t column) { return _m (row, column); }

        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        inline const T& operator() (size_t row, size_t column) const { return _m (row, column); }
#else
        MATRIXOPERATOR (T);
#endif
        /**
         * @brief Returns the i'th row of the rotation matrix
         * @param i [in] Index of the row to return. Only valid indices are 0, 1 and 2.
         */
        const rw::math::Vector3D< T > getRow (size_t i) const
        {
            RW_ASSERT (i < 3);
            return rw::math::Vector3D< T > (_m (i, 0), _m (i, 1), _m (i, 2));
        }

        /**
         * @brief Returns the i'th column of the rotation matrix
         * @param i [in] Index of the column to return. Only valid indices are 0, 1 and 2.
         */
        const rw::math::Vector3D< T > getCol (size_t i) const
        {
            RW_ASSERT (i < 3);
            return rw::math::Vector3D< T > (_m (0, i), _m (1, i), _m (2, i));
        }

        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true only if all elements are equal.
         *
         * @param rhs [in] Rotation to compare with
         * @return True if equal.
         */
        bool operator== (const Rotation3D< T >& rhs) const
        {
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    if (!(_m (i, j) == rhs (i, j)))
                        return false;
            return true;
        }

        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true if any of the elements are different.
         *
         * @param rhs [in] Rotation to compare with
         * @return True if not equal.
         */
        bool operator!= (const Rotation3D< T >& rhs) const { return !(*this == rhs); }

        /**
         * @brief Compares rotations with a given precision
         *
         * Performs an element wise comparison. Two elements are considered equal if the difference
         * are less than \b precision.
         *
         * @param rot [in] Rotation to compare with
         * @param precision [in] The precision to use for testing
         * @return True if all elements are less than \b precision apart.
         */
        bool equal (const Rotation3D< T >& rot,
                    const T precision = std::numeric_limits< T >::epsilon ()) const
        {
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    if (fabs (_m (i, j) - rot (i, j)) > precision)
                        return false;
            return true;
        }

        /**
         * @brief Verify that this rotation is a proper rotation
         *
         * @return True if this rotation is considered a proper rotation
         */
        bool isProperRotation () const;

        /**
         * @brief Verify that this rotation is a proper rotation
         *
         * @return True if this rotation is considered a proper rotation
         */
        bool isProperRotation (T precision) const;

        /**
         * @brief Returns a Eigen 3x3 matrix @f$ \mathbf{M}\in SO(3)
         * @f$ that represents this rotation
         *
         * @return @f$ \mathbf{M}\in SO(3) @f$
         */
        const EigenMatrix3x3& e () const { return _m; };

        /**
         * @brief Returns a Eigen 3x3 matrix @f$ \mathbf{M}\in SO(3)
         * @f$ that represents this rotation
         *
         * @return @f$ \mathbf{M}\in SO(3) @f$
         */
        EigenMatrix3x3& e () { return _m; };

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{R}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @param bRc [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @return \f$ \robabx{a}{c}{\mathbf{R}} \f$
         */
        inline const Rotation3D operator* (const Rotation3D& bRc) const
        {
            return multiply (*this, bRc);
        }

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{R}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @param rhs [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @return \f$ \robabx{a}{c}{\mathbf{R}} \f$
         */
        template< class R >
        inline const Rotation3D operator* (const Eigen::MatrixBase< R >& rhs) const
        {
            return Rotation3D< T > (this->e () * rhs);
        }

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{R}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @param lhs [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         * @param rhs [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         * @return \f$ \robabx{a}{c}{\mathbf{R}} \f$
         */
        template< class R >
        friend inline Rotation3D operator* (const Eigen::MatrixBase< R >& lhs,
                                            const Rotation3D< T >& rhs)
        {
            return Rotation3D< T > (lhs * rhs.e ());
        }

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{v}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{v}} \f$
         *
         * @param bVc [in] \f$ \robabx{b}{c}{\mathbf{v}} \f$
         * @return \f$ \robabx{a}{c}{\mathbf{v}} \f$
         */
        inline const rw::math::Vector3D< T > operator* (const rw::math::Vector3D< T >& bVc) const
        {
            return multiply (*this, bVc);
        }

        /**
         * @brief Construct a rotation matrix from a 3x3 Eigen matrix
         * It is the responsibility of the user that 3x3 matrix is indeed a
           rotation matrix.
         */
        template< class R > explicit Rotation3D (const EigenMatrix3x3& r)
        {
            _m (0, 0) = r (0, 0);
            _m (0, 1) = r (0, 1);
            _m (0, 2) = r (0, 2);
            _m (1, 0) = r (1, 0);
            _m (1, 1) = r (1, 1);
            _m (1, 2) = r (1, 2);
            _m (2, 0) = r (2, 0);
            _m (2, 1) = r (2, 1);
            _m (2, 2) = r (2, 2);
        }

        /**
         * @brief Construct a rotation matrix from a 3x3 Eigen matrix
         * It is the responsibility of the user that 3x3 matrix is indeed a
           rotation matrix.
        */
        template< class R > explicit Rotation3D (const Eigen::MatrixBase< R >& m)
        {
            RW_ASSERT (m.cols () == 3);
            RW_ASSERT (m.rows () == 3);
            _m (0, 0) = T (m.row (0) (0));
            _m (0, 1) = T (m.row (0) (1));
            _m (0, 2) = T (m.row (0) (2));
            _m (1, 0) = T (m.row (1) (0));
            _m (1, 1) = T (m.row (1) (1));
            _m (1, 2) = T (m.row (1) (2));
            _m (2, 0) = T (m.row (2) (0));
            _m (2, 1) = T (m.row (2) (1));
            _m (2, 2) = T (m.row (2) (2));
        }

        /**
         * @brief Creates a skew symmetric matrix from a Vector3D. Also
         * known as the cross product matrix of v.
         *
         * @relates Rotation3D
         *
         * @param v [in] vector to create Skew matrix from
         */
        static Rotation3D< T > skew (const rw::math::Vector3D< T >& v)
        {
            return Rotation3D< T > (0, -v (2), v (1), v (2), 0, -v (0), -v (1), v (0), 0);
        }

        // Faster-than-boost matrix multiplications below.

        /**
         *  @brief Write to \b result the product \b a * \b b.
         */
        static void multiply (const Rotation3D< T >& a, const Rotation3D< T >& b,
                              Rotation3D< T >& result);

        /**
         *  @brief Write to \b result the product \b a * \b b.
         */
        static void multiply (const Rotation3D< T >& a, const rw::math::Vector3D< T >& b,
                              rw::math::Vector3D< T >& result);

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{R}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @param aRb [in] \f$ \robabx{a}{b}{\mathbf{R}} \f$
         *
         * @param bRc [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @return \f$ \robabx{a}{c}{\mathbf{R}} \f$
         */
        static const Rotation3D< T > multiply (const Rotation3D< T >& aRb,
                                               const Rotation3D< T >& bRc);

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{v}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{v}} \f$
         *
         * @param aRb [in] \f$ \robabx{a}{b}{\mathbf{R}} \f$
         * @param bVc [in] \f$ \robabx{b}{c}{\mathbf{v}} \f$
         * @return \f$ \robabx{a}{c}{\mathbf{v}} \f$
         */
        static const rw::math::Vector3D< T > multiply (const Rotation3D< T >& aRb,
                                                       const rw::math::Vector3D< T >& bVc);
#if !defined(SWIGJAVA)
        /**
         * @brief Calculate the inverse.
         * @note This function changes the object that it is invoked on, but this is about x5 faster
         * than rot = inverse( rot )
         * @see rw::math::inverse(const rw::math::Rotation3D< T > &) for the (slower) version that does not change the
         * rotation object itself.
         * @return the inverse rotation.
         */
         #endif
        inline Rotation3D< T >& inverse ()
        {
            T tmpVal  = _m (0, 1);
            _m (0, 1) = _m (1, 0);
            _m (1, 0) = tmpVal;

            tmpVal    = _m (0, 2);
            _m (0, 2) = _m (2, 0);
            _m (2, 0) = tmpVal;

            tmpVal    = _m (1, 2);
            _m (1, 2) = _m (2, 1);
            _m (2, 1) = tmpVal;
            return *this;
        }

        /**
         * @brief Calculate the inverse.
         * @param copy [in] if coopy is false, This function changes the object that it is invoked
         * on, but this is about x5 faster than rot = inverse( rot ). else it changes the object,
         * making it a bit slower.
         * @return the inverse rotation.
         */
        Rotation3D< T > inverse (bool copy);

        /**
         * @brief Calculate the inverse. of a const Rotation3D. For this function copy is always
         * true
         * @param copy [in] always true
         * @return the inverse rotation.
         */
        Rotation3D< T > inverse (bool copy) const;

        T tr () const { return (*this) (0, 0) + (*this) (1, 1) + (*this) (2, 2); }

#if defined(SWIG)
        TOSTRING ();
#endif

      private:
        EigenMatrix3x3 _m;
    };

    /**
     * @brief Casts Rotation3D<T> to Rotation3D<Q>
     *
     * @relates Rotation3D
     *
     * @param rot [in] Rotation3D with type T
     * @return Rotation3D with type Q
     */
    template< class Q, class T > const Rotation3D< Q > cast (const Rotation3D< T >& rot)
    {
        Rotation3D< Q > res;
        for (size_t i = 0; i < 3; i++)
            for (size_t j = 0; j < 3; j++)
                res (i, j) = static_cast< Q > (rot (i, j));
        return res;
    }

#if !defined(SWIGJAVA)
    /**
     * @brief Calculates the inverse @f$ \robabx{b}{a}{\mathbf{R}} =
     * \robabx{a}{b}{\mathbf{R}}^{-1} @f$ of a rotation matrix
     *
     * @relates Rotation3D
     *
     * @see Rotation3D::inverse() for a faster version that modifies the existing rotation object
     * instead of allocating a new one.
     *
     * @param aRb [in] the rotation matrix @f$ \robabx{a}{b}{\mathbf{R}} @f$
     *
     * @return the matrix inverse @f$ \robabx{b}{a}{\mathbf{R}} =
     * \robabx{a}{b}{\mathbf{R}}^{-1} @f$
     *
     * @f$ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} =
     * \robabx{a}{b}{\mathbf{R}}^T @f$
     */

#endif
    template< class T > const rw::math::Rotation3D< T > inverse (const rw::math::Rotation3D< T >& aRb)
    {
        return Rotation3D< T > (aRb (0, 0),
                                aRb (1, 0),
                                aRb (2, 0),

                                aRb (0, 1),
                                aRb (1, 1),
                                aRb (2, 1),

                                aRb (0, 2),
                                aRb (1, 2),
                                aRb (2, 2));
    }

    /**
     * @brief Writes rotation matrix to stream
     *
     * @relates Rotation3D
     *
     * @param os [in/out] output stream to use
     * @param r [in] rotation matrix to print
     * @return the updated output stream
     */
    template< class T > std::ostream& operator<< (std::ostream& os, const Rotation3D< T >& r)
    {
        return os << "Rotation3D(" << r (0, 0) << ", " << r (0, 1) << ", " << r (0, 2) << ", "
                  << r (1, 0) << ", " << r (1, 1) << ", " << r (1, 2) << ", " << r (2, 0) << ", "
                  << r (2, 1) << ", " << r (2, 2) << ")";
    }
#if !defined(SWIG)
    // Explicit template specifications.
    extern template class rw::math::Rotation3D< double >;
    extern template class rw::math::Rotation3D< float >;
#else
    SWIG_DECLARE_TEMPLATE (Rotation3Dd, rw::math::Rotation3D< double >);
    SWIG_DECLARE_TEMPLATE (Rotation3Df, rw::math::Rotation3D< float >);
#endif
    using Rotation3Dd = Rotation3D< double >;
    using Rotation3Df = Rotation3D< float >;

    /**@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Rotation3D
         */
        template<>
        void write (const rw::math::Rotation3D< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Rotation3D
         */
        template<>
        void write (const rw::math::Rotation3D< float >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Rotation3D
         */
        template<>
        void read (rw::math::Rotation3D< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Rotation3D
         */
        template<>
        void read (rw::math::Rotation3D< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

namespace boost { namespace serialization {
    /**
     * @brief Boost serialization.
     * @param archive [in] the boost archive to read from or write to.
     * @param R [in/out] the rotation matrix to read/write.
     * @param version [in] class version (currently version 0).
     * @relatedalso rw::math::Rotation3D
     */
    template< class Archive, class T >
    void serialize (Archive& archive, rw::math::Rotation3D< T >& R, const unsigned int version)
    {
        archive& R (0, 0);
        archive& R (0, 1);
        archive& R (0, 2);
        archive& R (1, 0);
        archive& R (1, 1);
        archive& R (1, 2);
        archive& R (2, 0);
        archive& R (2, 1);
        archive& R (2, 2);
    }
}}    // namespace boost::serialization

#endif    // end include guard


#define CATCH_CONFIG_MAIN  // This tells Catch to provide a main() - only do this in one cpp file

#include "catch.hpp"
#include <mumpscpp/mumpscpp.h>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <Eigen\core>
#include <shiva\Environment.h>
#include <shiva\Communicator.h>
#include <boost/numeric/ublas/io.hpp>
#include <mumpscpp/UblasCoordinateAdaptor.h>
#include <mumpscpp/UblasVectorAdaptor.h>
#include <mumpscpp/EigenVector.h>

shiva::environment mpi_env;

void assemble_unsymmetric(mumpscpp::UblasCoordinateSparseMatrix& k)
{
  const size_t matrix_size = 5;
  k.resize(matrix_size, matrix_size, false);
  k.clear();
  k.append_element(0, 0, 1.0);
  k.append_element(0, 1, 2.0);
  k.append_element(0, 2, 3.0);
  k.append_element(0, 3, -4.0);
  k.append_element(0, 4, -5.0);
  k.append_element(1, 0, 2.0);
  k.append_element(1, 1, 1.0);
  k.append_element(1, 2, 4.0);
  k.append_element(1, 3, 3.0);
  k.append_element(1, 4, -2.0);
  k.append_element(2, 0, 3.0);
  k.append_element(2, 1, 4.0);
  k.append_element(2, 2, 1.0);
  k.append_element(2, 3, 2.0);
  k.append_element(2, 4, 3.0);
  k.append_element(3, 0, 4.0);
  k.append_element(3, 1, -2.0);
  k.append_element(3, 2, 4.0);
  k.append_element(3, 3, 1.0);
  k.append_element(3, 4, 3.0);
  k.append_element(4, 0, 5.0);
  k.append_element(4, 1, 4.0);
  k.append_element(4, 2, -2.0);
  k.append_element(4, 3, 4.0);
  k.append_element(4, 4, 1.0);
  k.sort();
}

void assemble_symmetric(mumpscpp::UblasCoordinateSparseMatrix& k)
{
  // lower triangular part
  const size_t matrix_size = 3;
  k.resize(matrix_size, matrix_size, false);
  k.clear();
  k.append_element(0, 0, 2.0);
  k.append_element(1, 0, -1.0);
  k.append_element(1, 1, 2.0);
  k.append_element(2, 0, 0.0);
  k.append_element(2, 1, -1.0);
  k.append_element(2, 2, 2.0);

  k.sort();
}

bool is_close(double d1, double d2, double eps)
{
  return abs(d1 - d2) < eps;
};

TEST_CASE("Mumps unsymmetric", "[mumps]") {
  shiva::communicator world;
  const size_t matrix_size = 5;

  mumpscpp::UblasCoordinateSparseMatrix k;
  assemble_unsymmetric(k);
  mumpscpp::EigenVector f = Eigen::VectorXd::Ones(matrix_size);

  mumpscpp::Mumps<double> mumps(mumpscpp::MatrixType::unsymmetric, mumpscpp::HostParallelism::involved, world.fortran_mpi_communicator());
  mumps.set_output_level(mumpscpp::OutputLevel::error);
  mumps.setDistributedInput(k);

  mumps.analyzeFactorize();
  mumps.solve(f);

  REQUIRE(is_close(f[0], 0.1933, 0.0001));
  REQUIRE(is_close(f[1], 0.1015, 0.0001));
  REQUIRE(is_close(f[2], 0.1314, 0.0001));
  REQUIRE(is_close(f[3], -0.0212, 0.0001));
  REQUIRE(is_close(f[4], -0.0249, 0.0001));

  mumps.destroy();
}

TEST_CASE("Mumps unsymmetric schur", "[mumps]") {
  shiva::communicator world;
  const size_t matrix_size = 5;

  mumpscpp::UblasCoordinateSparseMatrix k;
  assemble_unsymmetric(k);
  mumpscpp::EigenVector f = Eigen::VectorXd::Ones(matrix_size);

  mumpscpp::Mumps<double> mumps(mumpscpp::MatrixType::unsymmetric, mumpscpp::HostParallelism::involved, world.fortran_mpi_communicator());
  mumps.set_output_level(mumpscpp::OutputLevel::none);
  mumps.setDistributedInput(k);

  auto get_schur = [&](std::vector<int>& dofs)
  {
    if (world.rank() == 0) {
      mumps.set_host_shur_complement(dofs, mumpscpp::SchurReturnType::host);
    }

    mumps.analyzeFactorize();
    auto&& schur = mumps.get_host_schur();
    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> mf(schur.data(), 2, 2);
    return mf;

  };

  std::vector<int> dofs = { 1, 2 };
  auto mf1 = get_schur(dofs);
  REQUIRE(is_close(mf1(0,0), 2.7826, 0.0001));
  REQUIRE(is_close(mf1(0,1), 18.6957, 0.0001));
  REQUIRE(is_close(mf1(1,0), -8.5652, 0.0001));
  REQUIRE(is_close(mf1(1,1), 17.6087, 0.0001));

  mumps.destroy();

}

TEST_CASE("Mumps symmetric", "[mumps]") {
  shiva::communicator world;
  const size_t matrix_size = 3;

  mumpscpp::UblasCoordinateSparseMatrix k;
  assemble_symmetric(k);
  mumpscpp::EigenVector f = Eigen::VectorXd::Zero(matrix_size);
  f[0] = 1.25;
  f[1] = -2.0;
  f[2] = 1.75;

  mumpscpp::Mumps<double> mumps(mumpscpp::MatrixType::symmetric, mumpscpp::HostParallelism::involved, world.fortran_mpi_communicator());
  mumps.set_output_level(mumpscpp::OutputLevel::error);
  mumps.setDistributedInput(k);

  mumps.analyzeFactorize();
  mumps.solve(f);
  mumps.write_problem("d:\\test.txt");

  //std::cout << f << std::endl;

  REQUIRE(is_close(f[0], 0.375, 0.0001));
  REQUIRE(is_close(f[1], -0.5, 0.0001));
  REQUIRE(is_close(f[2], 0.625, 0.0001));

  mumps.destroy();
}

TEST_CASE("Mumps symmetric schur", "[mumps]") {
  shiva::communicator world;
  const size_t matrix_size = 3;

  mumpscpp::UblasCoordinateSparseMatrix k;
  assemble_symmetric(k);
  mumpscpp::EigenVector f = Eigen::VectorXd::Ones(matrix_size);

  mumpscpp::Mumps<double> mumps(mumpscpp::MatrixType::symmetric, mumpscpp::HostParallelism::involved, world.fortran_mpi_communicator());
  mumps.set_output_level(mumpscpp::OutputLevel::none);
  mumps.setDistributedInput(k);

  auto get_schur = [&](std::vector<int>& dofs)
  {
    mumps.set_host_shur_complement(dofs, mumpscpp::SchurReturnType::slaves_lower);

    mumps.analyzeFactorize();
    auto&& schur = mumps.get_host_schur();
    return schur;

  };

  std::vector<int> dofs = { 1, 2 };
  auto mf1 = get_schur(dofs);

  //for (auto d : mf1) { std::cout << d << std::endl; }

  // only the lower triangular will be sent by mumps
  REQUIRE(is_close(mf1[0], 2.0, 0.0001));
  REQUIRE(is_close(mf1[1], -1, 0.0001));
  REQUIRE(is_close(mf1[2], 0.0, 0.0001));
  REQUIRE(is_close(mf1[3], 1.5, 0.0001));

  mumps.destroy();

}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/fold_right.hpp>
#include <boost/hana/optional.hpp>
#include <boost/hana/plus.hpp>
namespace hana = boost::hana;

static_assert(hana::fold_right(hana::nothing, 1, hana::plus) == 1, "");
static_assert(hana::fold_right(hana::just(4), 1, hana::plus) == 5, "");

int main() { }


#include <blitz/array.h>

using namespace blitz;

int main()
{
    Array<int,1> A(4), B(4), C(4);
    A = 0, 1, 2, 3;
    B = 0, 1, 0, 3;
    C = A ^ B;
    cout << C << endl;
}


// Header for this file
#include "jvector.h"
// C System-Headers
#include <termios.h> /* POSIX terminal control definitions */
#include <sys/ioctl.h>
#include <fcntl.h>   //fopen(),fclose()
#include <unistd.h>  //read(), write()
#include <stdio.h>

// C++ System headers
#include <vector>      //vector
#include <string>      //string
#include <fstream>     //iss* ofstream
#include <chrono>      // timing functions
#include <cmath>       //sqrt, abs
#include <iostream>    //cout
#include <typeinfo>    //typeid
#include <algorithm>   // transform, find, count, erase
#include <functional>  // plus/minus/multiplies
#include <utility>     //std::make_pair
#include <map>         //std::map
#include <typeinfo>    //typeid
#include <mutex> //protect against concurrent access when using (unordered) parallel for loops
#include <assert.h> //static_assert

// Boost Headers
#include <boost/algorithm/string.hpp>  //split() and is_any_of for parsing .csv files
#include <boost/lexical_cast.hpp>  //lexical cast (unsurprisingly)
#include <dirent.h>

// Miscellaneous Headers
#include <omp.h>  //OpenMP pragmas

namespace jaspl {

template class JVector<int>;
template class JVector<float>;
template class JVector<double>;
template class JVector<char>;

template <class F> JVector<F>::JVector(std::string raw_data) {
    ParseRawData(raw_data);
}

template <class F> JVector<F>::JVector(std::vector<F> vec) {
    underlying_vector = vec;
}

template <class F> JVector<F>::JVector(F* ptr, uint ptr_size) {

    underlying_vector.reserve(underlying_vector.size() + ptr_size);
    std::copy(&ptr[0], &ptr[ptr_size], std::back_inserter(underlying_vector));
}

template <class F> JVector<F>::JVector(uint size) {
    underlying_vector.reserve( size );
}

template <class F> JVector<F>::JVector(uint size, F fill_element) {
    underlying_vector.reserve( size );
    underlying_vector = std::vector<F> ( size , fill_element);
}

template <class F> JVector<F>::JVector() {}

template <class F> JVector<F>::~JVector() {
    underlying_vector.clear();
}

template <class F> bool JVector<F>::check_if_arithmetic(F input) {

    if ( !std::is_arithmetic<F>::value ) {

        std::string err_mesg = "JVector: ";
        err_mesg += "cannot initialize from non-arithmetic type ";
        err_mesg += boost::lexical_cast<std::string>(typeid(input).name());
        throw std::invalid_argument(err_mesg);

        return false;
    } else {
        return true;
    }
}

template <class F> uint JVector<F>::size() {
    return underlying_vector.size();
}

template <class F> uint JVector<F>::num_chars(std::string raw_data, char delim) {
    return std::count(raw_data.begin(), raw_data.end(), delim);
}

template <class F> void JVector<F>::ParseRawData( std::string raw_data ) {

    uint lines = num_chars(raw_data, '\n');

    std::istringstream data_stream(raw_data);

    for (uint i = 0; i < lines; i++) {
        std::string input;
        std::getline(data_stream, input);
        try {
            F val = boost::lexical_cast<F>(input);
            underlying_vector.push_back(val);
        } catch (const boost::bad_lexical_cast& err) {
            std::cerr << err.what() << std::endl;
        }
    }
}

template <class F> double JVector<F>::sum(std::vector<F>& data_list,double exponent) {

    double tot=0;

    for ( uint i = 0 ; i < data_list.size(); i ++) {
        tot += pow( data_list[i], exponent );
    }

    return tot;
}

template <class F> double JVector<F>::mean(std::vector<F>& data_list) {
    //compute mean value of data set
    double sum_x=sum(data_list,1.0);
    double n=data_list.size();
    return sum_x/n;
}

template <class F> double JVector<F>::mean() {
    return mean(underlying_vector);
}

template <class F> F JVector<F>::min() {
    F min_power= *std::min_element(underlying_vector.begin(), underlying_vector.end());
    return min_power;
}

template <class F> F JVector<F>::max() {
    F min_power= *std::max_element(underlying_vector.begin(), underlying_vector.end());
    return min_power;
}

template <class F> double JVector<F>::std_dev(std::vector<F> &data_list) {

    //compute mean value of data set
    double sum_x=sum(data_list,1.0);
    double n=data_list.size();
    double mean = sum_x/n;

    //compute variance taking into account Bessel's correction i.e. n/(n-1)
    double sum_x2=sum(data_list,2.0);
    double sigma_sqr=sum_x2/(n-1.0)-n/(n-1.0)*pow(mean,2.0);

    //return square root of variance
    return sqrt(sigma_sqr);
}

template <class F> double JVector<F>::std_dev() {
    return std_dev(underlying_vector);
}

template <class F> double JVector<F>::norm() {
    return sqrt(sum(underlying_vector,2.0));

}

template <class F> void JVector<F>::Normalize() {
    double norm_factor=sqrt(sum(underlying_vector,2.0));

    for(unsigned int i = 0; i<underlying_vector.size(); i++) {
        underlying_vector.at(i)=underlying_vector.at(i)/norm_factor;
    }
}

}


// MTL4 Test 05.cpp : Defines the entry point for the console application.
//

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace mtl;

typedef mtl::dense_vector<double> mtlVec;
mtlVec testFunc( mtlVec A,double Scale);

int main(int, char**)
{
    mtlVec u(3);
    u(0)=1.1;
    u(1)=2.2;
    u(2)=3.3;
    mtlVec myVec(3);
    myVec=testFunc(u,55.5);
    print_vector(myVec);

    return 0;
}

mtlVec testFunc(mtlVec A ,double Scale)
{
    const std::size_t testSize=size(A);
    mtlVec testVec(testSize);
    testVec=Scale*A;
    print_vector(testVec);

    return testVec;
}


//
//=======================================================================
// Copyright 2012
// Author: Alex Hagen-Zanker
//
// 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)
//=======================================================================
//
// The nearest source visitor implements the dijkstra visitor concepts and
// records for each node which is the nearest source. Only useful for a
// search with multiple sources.
//
//=======================================================================
//

#ifndef BLINK_GRAPH_DIJKSTRA_VISITOR_NEAREST_SOURCE_VISITOR_HPP
#define BLINK_GRAPH_DIJKSTRA_VISITOR_NEAREST_SOURCE_VISITOR_HPP

#include <blink/graph/property_maps/vertex_property_map_helper.hpp>
#include <boost/tuple/tuple.hpp> //tie
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/properties.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/range/algorithm/for_each.hpp>

namespace blink {

template<typename NearestsourceMap>
struct nearest_source_visitor : public boost::default_dijkstra_visitor
{
  nearest_source_visitor(const NearestsourceMap& nearest = NearestsourceMap() )
    : m_nearest_source_map(nearest)
  {}
  template<typename Graph>
  void init_map(const Graph& g)
  {
    typename boost::graph_traits<Graph>::vertex_iterator ui, ui_end;
    for (boost::tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui) {
      put(m_nearest_source_map, *ui, *ui);
    }
  }

  template<typename E, typename G>
  void edge_relaxed(const E& e, const G& g)
  {
    put(m_nearest_source_map, target(e,g), get(m_nearest_source_map, source(e,g)));
  }

  NearestsourceMap m_nearest_source_map;
};

template<typename Graph, typename NearestsourceMap>
void init_nearest_source_map(const Graph& g, NearestsourceMap nearest)
{
  typename graph_traits<Graph>::vertex_iterator ui, ui_end;
  for (tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui) {
    put(nearest, *ui, *ui);
  }
}

template<typename DijkstraState>
struct nearest_source_helper
{
  typedef typename DijkstraState::template param<boost::vertex_index_t>::type index_type;
  typedef typename DijkstraState::graph_type graph_type;

  typedef typename boost::graph_traits<graph_type>::vertex_descriptor vertex_descriptor;
  typedef typename vertex_property_map_helper<vertex_descriptor,graph_type, index_type> helper;
  typedef typename helper::type map_type;
  typedef typename nearest_source_visitor<map_type> visitor_type;

  static map_type make_map(DijkstraState& state)
  {
    return helper::make(state.get_graph(), state.get<boost::vertex_index_t>());
  }

    static visitor_type make_visitor(DijkstraState& state)
  {
    map_type map = make_map(state);
    visitor_type visitor(map);
    visitor.init_map(state.get_graph());

    return visitor;
  }
};

template<typename Graph, typename Params>
struct nearest_source_helper_indirect : nearest_source_helper<typename dijkstra_state_helper<Graph, Params>::type>
{};

template<typename DijkstraState>
typename nearest_source_helper<DijkstraState>::visitor_type
  make_nearest_source_visitor(DijkstraState& state)
{
  return nearest_source_helper<DijkstraState>::make_visitor(state);
}

};// namespace blink;

#endif //BLINK_GRAPH_DIJKSTRA_VISITOR_NEAREST_SOURCE_VISITOR_HPP


#ifndef __fovis_calibration_hpp__
#define __fovis_calibration_hpp__

#include <Eigen/Geometry>

namespace fovis
{

/**
 * \ingroup FovisCore
 * \brief Intrinsic parameters for a pinhole camera with plumb-bob distortion model.
 *
 */
struct CameraIntrinsicsParameters
{
  CameraIntrinsicsParameters() :
    width(0), height(0), fx(0), fy(0), cx(0), cy(0),
    k1(0), k2(0), k3(0), p1(0), p2(0)
  {}

  /**
   * \code
   * [ fx 0  cx 0 ]
   * [ 0  fy cy 0 ]
   * [ 0  0  1  0 ]
   * \endcode
   *
   * \return a 3x4 projection matrix that transforms 3D homogeneous points in
   * the camera frame to 2D homogeneous points in the image plane.
   */
  Eigen::Matrix<double, 3, 4> toProjectionMatrix() const
  {
    Eigen::Matrix<double, 3, 4> result;
    result <<
      fx,  0, cx, 0,
       0, fy, cy, 0,
       0,  0,  1, 0;
    return result;
  }

  /**
   * Image width.
   */
  int width;

  /**
   * Image height.
   */
  int height;

  /**
   * focal length along the X axis.
   */
  double fx;

  /**
   * focal length along the Y axis.  Should generally be the same as \p fx.
   */
  double fy;

  /**
   * X-coordinate of the camera center of projection / principal point.
   */
  double cx;

  /**
   * Y-coordinate of the camera center of projection / principal point.
   */
  double cy;

  /**
   * First radial distortion coefficient (r^2) for a plumb-bob distortion model.
   *
   * \sa http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html
   */
  double k1;

  /**
   * Second radial distortion coefficient (r^4) for a plumb-bob distortion model.
   */
  double k2;

  /**
   * Third radial distortion coefficient (r^6) for a plumb-bob distortion model.
   */
  double k3;

  /**
   * First tangential distortion coefficient for a plumb-bob distortion model.
   */
  double p1;

  /**
   * Second tangential distortion coefficient for a plumb-bob distortion model.
   */
  double p2;
};

}

#endif


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/functional.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto plus = _ + _;
    BOOST_HANA_CONSTEXPR_ASSERT(plus(1, 2) == 1 + 2);

    BOOST_HANA_CONSTEXPR_LAMBDA auto increment = _ + 1;
    BOOST_HANA_CONSTEXPR_ASSERT(increment(1) == 2);

    BOOST_HANA_CONSTEXPR_LAMBDA auto double_ = 2 * _;
    BOOST_HANA_CONSTEXPR_ASSERT(double_(1) == 2);

    // Extra arguments are ignored.
    BOOST_HANA_CONSTEXPR_ASSERT(double_(1, "ignored") == 2);
    //! [main]
}


/*M///////////////////////////////////////////////////////////////////////////////////////
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2008-2011, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistributions of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistributions in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
// C by Benjamin Wassermann
//M*/

// detect junction or corner areas and skip them for optimization -> less error for wide range interpolation like cubic....

#ifndef _LINE_OPTIMIZER_HPP_
#define _LINE_OPTIMIZER_HPP_
#ifdef __cplusplus

#include <utility/mean.hpp>
#include <dlib/optimization.h>

namespace lsfm {
    template <
        class mat_type,
        class search_strategy_type = dlib::bfgs_search_strategy,
        class stop_strategy_type = dlib::objective_delta_stop_strategy
        >
        struct LineOptimizer {

        //! @brief optimize line for better fitting to gradient maginutde image (rotation and orthogonal translation to line)
        //! @param mag is the magnitude image as mat, it could also be the quadratic magnitude as integer
        //! @param l is the line segment to optimize parameters for
        //! @param d distnance in pixels, parameter to optimize
        //! @param r radian for rotation, parameter to optimize
        //! @param d_lower lower searching bound for distance parameter optimization
        //! @param d_upper upper searching bound for distance parameter optimization
        //! @param r_lower lower searching bound for rotation parameter optimization
        //! @param r_upper upper searching bound for rotation parameter optimization
        //! @param mean_param option for mean calculation (see mean variants)
        //! @param derivative_prec delta for computing approximating derivatives (done with dlib helper)
        //! @param interpolate_op operator for interpolaton (function pointer to linear or cubic interpolation)
        //! @param mean_op operator for mean computation (function pointer to variants of mean - step over line by
        //!        fixed distance or use fixed samples and compute distance to have always the same number of points)
        //! @param search search strategy of optimizer (see dlib for more informations)
        //! @param stop stop strategy of optimizer (see dlib for more informations)
        //! @return error for found values
        template<class FT, template<class> class LPT>
        static inline double optimize(const cv::Mat& mag, const LineSegment<FT, LPT>& l, FT& d, FT& r,
            double d_lower = -1, double d_upper = 1, double r_lower = -CV_PI / 180, double r_upper = CV_PI / 180,
            double mean_param = 1, double derivative_prec = 1e-7,
            typename MeanHelper<double,LPT>::func_type mean_op = Mean<double, mat_type, LinearInterpolator<double, mat_type>>::process,
            search_strategy_type search = dlib::bfgs_search_strategy(),
            stop_strategy_type stop = dlib::objective_delta_stop_strategy(1e-7)) {

            CV_Assert(cv::DataType<mat_type>::type == mag.type());

            typedef dlib::matrix<double, 0, 1> column_vector;
            auto eval = [&](const column_vector& v) -> double {
                LineSegment<double, LPT> tmp = l;
                tmp.translateOrtho(v(0));
                tmp.rotate(v(1), tmp.center());
                return mean_op(mag, tmp, mean_param);
            };

            column_vector starting_point(2), lower(2), upper(2);
            starting_point = d, r;
            lower = d_lower, r_lower;
            upper = d_upper, r_upper;
            double ret = dlib::find_max_box_constrained(search, stop,
                eval, dlib::derivative(eval, derivative_prec), starting_point, lower, upper);
            d = starting_point(0);
            r = starting_point(1);
            return ret;

        }

        //! This variant will optimize the line object instead of just calculating the optimized parameters for the line
        template<class FT, template<class> class LPT>
        static inline double optimize_line(const cv::Mat& mag, LineSegment<FT, LPT>& l,
                double d_lower = -1, double d_upper = 1, double r_lower = -CV_PI / 180, double r_upper = CV_PI / 180,
                double mean_param = 1, double derivative_prec = 1e-7,
                typename MeanHelper<double,LPT>::func_type mean_op = Mean<double, mat_type, LinearInterpolator<double, mat_type>>::processs,
                search_strategy_type search = dlib::bfgs_search_strategy(),
                stop_strategy_type stop = dlib::objective_delta_stop_strategy(1e-7)) {
            FT d = 0, r = 0;
            double ret = optimize(mag,l, d, r, d_lower, d_upper, r_lower, r_upper, mean_param, derivative_prec, mean_op, search, stop);
            l.translateOrtho(d);
            l.rotate(r, l.center());
            return ret;
        }

        //! This variant will optimize the line object instead of just calculating the optimized parameters for the line
        template<class FT, template<class> class LPT, class LV>
        static inline void optimizeLV(const cv::Mat& mag, LV& in,
            double d_lower = -1, double d_upper = 1, double r_lower = -CV_PI / 180, double r_upper = CV_PI / 180,
            FT mean_param = 1, double derivative_prec = 1e-7,
            typename MeanHelper<double,LPT>::func_type mean_op = Mean<double, mat_type, LinearInterpolator<double, mat_type>>::processs,
            search_strategy_type search = dlib::bfgs_search_strategy(),
            stop_strategy_type stop = dlib::objective_delta_stop_strategy(1e-7)) {
            for (size_t i = 0; i != in.size(); ++i)
                optimize_line(mag, in[i], d_lower, d_upper, r_lower, r_upper, mean_param, derivative_prec, mean_op, search, stop);
        }

        //! This variant will optimize the line object instead of just calculating the optimized parameters for the line
        template<class FT, template<class> class LPT, class LV>
        static inline void optimizeLV(const cv::Mat& mag, LV& in, std::vector<double> &err,
            double d_lower = -1, double d_upper = 1, double r_lower = -CV_PI / 180, double r_upper = CV_PI / 180,
            double mean_param = 1, double derivative_prec = 1e-7,
            typename MeanHelper<FT,LPT>::func_type mean_op = Mean<double, mat_type, LinearInterpolator<double, mat_type>>::processs,
            search_strategy_type search = dlib::bfgs_search_strategy(),
            stop_strategy_type stop = dlib::objective_delta_stop_strategy(1e-7)) {
            err.resize(in.size());
            for (size_t i = 0; i != in.size(); ++i)
                err[i] = optimize_line(mag, in[i], d_lower, d_upper, r_lower, r_upper, mean_param, derivative_prec, mean_op, search, stop);
        }

        //! This variant will optimize the line object instead of just calculating the optimized parameters for the line
        template<class FT, template<class> class LPT, class LV>
        static inline double optimizeLV(const cv::Mat& mag, const LV& in, LV& out,
            double d_lower = -1, double d_upper = 1, double r_lower = -CV_PI / 180, double r_upper = CV_PI / 180,
            double mean_param = 1, double derivative_prec = 1e-7,
            typename MeanHelper<FT,LPT>::func_type mean_op = Mean<double, mat_type, LinearInterpolator<double, mat_type>>::processs,
            search_strategy_type search = dlib::bfgs_search_strategy(),
            stop_strategy_type stop = dlib::objective_delta_stop_strategy(1e-7)) {
            out.resize(in.size());
            for (size_t i = 0; i != in.size(); ++i) {
                out[i] = in[i];
                optimize_line(mag, out[i], d_lower, d_upper, r_lower, r_upper, mean_param, derivative_prec, mean_op, search, stop);
            }
        }

        //! This variant will optimize the line object instead of just calculating the optimized parameters for the line
        template<class FT, template<class> class LPT, class LV>
        static inline double optimizeLV(const cv::Mat& mag, const LV& in, LV& out, std::vector<double> &err,
            double d_lower = -1, double d_upper = 1, double r_lower = -CV_PI / 180, double r_upper = CV_PI / 180,
            double mean_param = 1, double derivative_prec = 1e-7,
            typename MeanHelper<double,LPT>::func_type mean_op = Mean<double, mat_type, LinearInterpolator<double, mat_type>>::processs,
            search_strategy_type search = dlib::bfgs_search_strategy(),
            stop_strategy_type stop = dlib::objective_delta_stop_strategy(1e-7)) {
            out.resize(in.size());
            err.resize(in.size());
            for (size_t i = 0; i != in.size(); ++i) {
                out[i] = in[i];
                err[i] = optimize_line(mag, out[i], d_lower, d_upper, r_lower, r_upper, mean_param, derivative_prec, mean_op, search, stop);
            }
        }
    };

}
#endif
#endif


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

#include <boost/math/concepts/real_concept.hpp>
#include <boost/test/test_exec_monitor.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/stats.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/array.hpp>
#include "functor.hpp"

#include "test_beta_hooks.hpp"
#include "handle_test_result.hpp"
#include "table_type.hpp"

#ifndef SC_
#define SC_(x) static_cast<typename table_type<T>::type>(BOOST_JOIN(x, L))
#endif

template <class Real, class T>
void test_inverses(const T& data)
{
   using namespace std;
   typedef typename T::value_type row_type;
   typedef Real                   value_type;

   value_type precision = static_cast<value_type>(ldexp(1.0, 1-boost::math::policies::digits<value_type, boost::math::policies::policy<> >()/2)) * 100;
   if(boost::math::policies::digits<value_type, boost::math::policies::policy<> >() < 50)
      precision = 1;   // 1% or two decimal digits, all we can hope for when the input is truncated

   for(unsigned i = 0; i < data.size(); ++i)
   {
      //
      // These inverse tests are thrown off if the output of the
      // incomplete beta is too close to 1: basically there is insuffient
      // information left in the value we're using as input to the inverse
      // to be able to get back to the original value.
      //
      if(Real(data[i][5]) == 0)
         BOOST_CHECK_EQUAL(boost::math::ibeta_inv(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(0));
      else if((1 - Real(data[i][5]) > 0.001)
         && (fabs(Real(data[i][5])) > 2 * boost::math::tools::min_value<value_type>())
         && (fabs(Real(data[i][5])) > 2 * boost::math::tools::min_value<double>()))
      {
         value_type inv = boost::math::ibeta_inv(Real(data[i][0]), Real(data[i][1]), Real(data[i][5]));
         BOOST_CHECK_CLOSE(Real(data[i][2]), inv, precision);
      }
      else if(1 == Real(data[i][5]))
         BOOST_CHECK_EQUAL(boost::math::ibeta_inv(Real(data[i][0]), Real(data[i][1]), Real(data[i][5])), value_type(1));

      if(Real(data[i][6]) == 0)
         BOOST_CHECK_EQUAL(boost::math::ibetac_inv(Real(data[i][0]), Real(data[i][1]), Real(data[i][6])), value_type(1));
      else if((1 - Real(data[i][6]) > 0.001)
         && (fabs(Real(data[i][6])) > 2 * boost::math::tools::min_value<value_type>())
         && (fabs(Real(data[i][6])) > 2 * boost::math::tools::min_value<double>()))
      {
         value_type inv = boost::math::ibetac_inv(Real(data[i][0]), Real(data[i][1]), Real(data[i][6]));
         BOOST_CHECK_CLOSE(Real(data[i][2]), inv, precision);
      }
      else if(Real(data[i][6]) == 1)
         BOOST_CHECK_EQUAL(boost::math::ibetac_inv(Real(data[i][0]), Real(data[i][1]), Real(data[i][6])), value_type(0));
   }
}

template <class Real, class T>
void test_inverses2(const T& data, const char* type_name, const char* test_name)
{
   typedef typename T::value_type row_type;
   typedef Real                   value_type;

   typedef value_type (*pg)(value_type, value_type, value_type);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   pg funcp = boost::math::ibeta_inv<value_type, value_type, value_type>;
#else
   pg funcp = boost::math::ibeta_inv;
#endif

   boost::math::tools::test_result<value_type> result;

   std::cout << "Testing " << test_name << " with type " << type_name
      << "\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";

   //
   // test ibeta_inv(T, T, T) against data:
   //
   result = boost::math::tools::test_hetero<Real>(
      data,
      bind_func<Real>(funcp, 0, 1, 2),
      extract_result<Real>(3));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::ibeta_inv", test_name);
   //
   // test ibetac_inv(T, T, T) against data:
   //
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   funcp = boost::math::ibetac_inv<value_type, value_type, value_type>;
#else
   funcp = boost::math::ibetac_inv;
#endif
   result = boost::math::tools::test_hetero<Real>(
      data,
      bind_func<Real>(funcp, 0, 1, 2),
      extract_result<Real>(4));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::ibetac_inv", test_name);
}

template <class T>
void test_beta(T, const char* name)
{
   (void)name;
   //
   // The actual test data is rather verbose, so it's in a separate file
   //
   // The contents are as follows, each row of data contains
   // five items, input value a, input value b, integration limits x, beta(a, b, x) and ibeta(a, b, x):
   //
#if !defined(TEST_DATA) || (TEST_DATA == 1)
#  include "ibeta_small_data.ipp"

   test_inverses<T>(ibeta_small_data);
#endif

#if !defined(TEST_DATA) || (TEST_DATA == 2)
#  include "ibeta_data.ipp"

   test_inverses<T>(ibeta_data);
#endif

#if !defined(TEST_DATA) || (TEST_DATA == 3)
#  include "ibeta_large_data.ipp"

   test_inverses<T>(ibeta_large_data);
#endif

#if !defined(TEST_DATA) || (TEST_DATA == 4)
#  include "ibeta_inv_data.ipp"

   test_inverses2<T>(ibeta_inv_data, name, "Inverse incomplete beta");
#endif
}

template <class T>
void test_spots(T)
{
   //
   // basic sanity checks, tolerance is 100 epsilon expressed as a percentage:
   //
   T tolerance = boost::math::tools::epsilon<T>() * 10000;
   BOOST_CHECK_CLOSE(
      ::boost::math::ibeta_inv(
         static_cast<T>(1),
         static_cast<T>(2),
         static_cast<T>(0.5)),
      static_cast<T>(0.29289321881345247559915563789515096071516406231153L), tolerance);
   BOOST_CHECK_CLOSE(
      ::boost::math::ibeta_inv(
         static_cast<T>(3),
         static_cast<T>(0.5),
         static_cast<T>(0.5)),
      static_cast<T>(0.92096723292382700385142816696980724853063433975470L), tolerance);
   BOOST_CHECK_CLOSE(
      ::boost::math::ibeta_inv(
         static_cast<T>(20.125),
         static_cast<T>(0.5),
         static_cast<T>(0.5)),
      static_cast<T>(0.98862133312917003480022776106012775747685870929920L), tolerance);
   BOOST_CHECK_CLOSE(
      ::boost::math::ibeta_inv(
         static_cast<T>(40),
         static_cast<T>(80),
         static_cast<T>(0.5)),
      static_cast<T>(0.33240456430025026300937492802591128972548660643778L), tolerance);
}


#include <boost/test/unit_test.hpp>

#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include "PeriodVal.h"
#include "PatternScannerEngine.h"
#include "SegmentConstraint.h"
#include "SegmentValsCloseToLinearEq.h"
#include "SegmentListConstraint.h"
#include "SlopeIncreasesConstraint.h"
#include "PatternMatchValidator.h"
#include "EndWithinPercentOfStart.h"
#include "PeriodValSegment.h"
#include "DoubleBottomScanner.h"
#include "TestHelper.h"
#include "MultiPatternScanner.h"

using namespace boost::posix_time;
using namespace boost::gregorian;
using namespace testHelper;

BOOST_AUTO_TEST_CASE( DoubleBottomScanner_QCOR_20130819 )
{
	// Test a full double-bottom scan for QCOR in 2013. This unit test is built up from
	// VScanner_QCOR_20130819_LHSofDoubleBottom and VScanner_QCOR_20130819_RHSofDoubleBottom, which
	// test the LHS and RHS of the double-bottom individually. The number of pattern matches
	// should therefore be the number of matches from VScanner_QCOR_20130819_LHSofDoubleBottom
	// multiplied times the number of matches from VScanner_QCOR_20130819_RHSofDoubleBottom.

	PeriodValSegmentPtr chartData = PeriodValSegment::readFromFile("./patternScan/QCOR_DoubleBottom_Weekly.csv");
	genPeriodValSegmentInfo("Double bottom segment data",*chartData);

	DoubleBottomScanner scanner(DoubleRange(7.0,40.0)); // allow for a deeper depth than the default
	PatternMatchListPtr patternMatches = scanner.scanPatternMatches(chartData);

	verifyMatchList("Double bottom match",patternMatches,1);

	verifyPatternMatch("Double bottom match",
            ptime(date(2013,8,26)),ptime(date(2014,2,10)),4,patternMatches->front());

}

BOOST_AUTO_TEST_CASE( DoubleBottom_Synthesized )
{
	using namespace boost::gregorian;

	TestPerValRangeList ranges;
	ranges.push_back(TestPerValRange(4,100.0,92.0)); // initial 8% down-trend
	ranges.push_back(TestPerValRange(3,92.5,98.0)); // up-trend falling short of down-trend
	ranges.push_back(TestPerValRange(3,97.5,91.5)); // next down-trend, going below the initial down-trend
	ranges.push_back(TestPerValRange(4,92.0,100.5)); // final uptrend, goes above start
	PeriodValSegmentPtr chartData = synthesizePeriodValSegment(date(2014,1,1),ranges);

	DoubleBottomScanner scanner;
	PatternMatchListPtr patternMatches = scanner.scanPatternMatches(chartData);

    verifyMatchList("DoubleBottom_Synthesized",patternMatches,2);
	verifyPatternMatch("DoubleBottom_Synthesized match",
            ptime(date(2014,1,1)),ptime(date(2014,1,13)),4,patternMatches->front());

}

BOOST_AUTO_TEST_CASE( DoubleBottom_Synthesized_RHSHigherLow )
{
	using namespace boost::gregorian;

	TestPerValRangeList ranges;
	ranges.push_back(TestPerValRange(4,100.0,92.0)); // initial 8% down-trend
	ranges.push_back(TestPerValRange(3,92.5,98.0)); // up-trend falling short of down-trend
	ranges.push_back(TestPerValRange(3,97.5,92.5)); // next down-trend, *not* going below the initial down-trend
	ranges.push_back(TestPerValRange(4,93.0,100.5)); // final uptrend, goes above start
	PeriodValSegmentPtr chartData = synthesizePeriodValSegment(date(2014,1,1),ranges);

	DoubleBottomScanner scanner;
	PatternMatchListPtr patternMatches = scanner.scanPatternMatches(chartData);

	BOOST_TEST_MESSAGE("Should return 0 matches, since the lowest low on the RHS is not lower than the LHS");
	verifyMatchList("DoubleBottom_Synthesized_RHSHigherLow",patternMatches,0);

}

BOOST_AUTO_TEST_CASE( DoubleBottom_Synthesized_MinMaxDepth )
{
	using namespace boost::gregorian;

	TestPerValRangeList ranges;
	ranges.push_back(TestPerValRange(4,100.0,92.0)); // initial 8% down-trend
	ranges.push_back(TestPerValRange(3,92.5,98.0)); // up-trend falling short of down-trend
	ranges.push_back(TestPerValRange(3,97.5,91.5)); // next down-trend, going below the initial down-trend
	ranges.push_back(TestPerValRange(4,92.0,100.5)); // final uptrend, goes above start
	PeriodValSegmentPtr chartData = synthesizePeriodValSegment(date(2014,1,1),ranges);

	DoubleBottomScanner scanner(DoubleRange(9.0,30.0)); // Require at least a 10% depth to trigger depth constraints
	PatternMatchListPtr patternMatches = scanner.scanPatternMatches(chartData);

	BOOST_TEST_MESSAGE("Should return 0 matches, since the depth is not greater than 9%");
	verifyMatchList("DoubleBottom_Synthesized_ShallowDepth",patternMatches,0);

	DoubleBottomScanner scannerMaxDepth(DoubleRange(4.0,7.0)); // Require at least a 10% depth to trigger depth constraints
	patternMatches = scanner.scanPatternMatches(chartData);

	BOOST_TEST_MESSAGE("Should return 0 matches, since the depth is greater than 7%");
	verifyMatchList("DoubleBottom_Synthesized_ShallowDepth",patternMatches,0);

}

BOOST_AUTO_TEST_CASE( DoubleBottomScanner_GBX )
{
    PeriodValSegmentPtr chartData = PeriodValSegment::readFromFile("./patternScan/GBX_Daily.csv");
    genPeriodValSegmentInfo("Double bottom segment data",*chartData);

    MultiPatternScanner scanner(PatternScannerPtr(new DoubleBottomScanner(DoubleRange(7.0,40.0))));

    PatternMatchListPtr patternMatches = scanner.scanPatternMatches(chartData);

    // Shouldn't match any double bottoms. The potential double bottom in this pattern data
    // has a middle which goes higher than the beginning of the pattern, and is thus mal-formed.
    verifyMatchList("Double bottom match",patternMatches,0);

}


#include <iostream>
#include <boost/algorithm/string.hpp>
#include <string>
#include <sstream>
#include <stdlib.h>
#include <fstream>
#include <vector>
#include <cmath>

//#include <system.h>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/opencv.hpp"

using namespace cv;
using namespace std;

/* Keep the webcam from locking up when you interrupt a frame capture */
volatile int quit_signal=0;
#ifdef __unix__
#include <signal.h>
extern "C" void quit_signal_handler(int signum) {
 if (quit_signal!=0) exit(0); // just exit already
 quit_signal=1;
 printf("Will quit at next camera frame (repeat to kill now)\n");
}
#endif

int capture_video_spacebar(VideoCapture cap){
	unsigned int count = 0;
	namedWindow("Screen", CV_WINDOW_NORMAL);
	char setting = 'a';

	for(;;){  // Loop through, getting new images from the camera.
		//cout << "Setting " << setting + 'a' << endl;
		Mat frame;
		cap >> frame; // get a new frame from camera
		if (quit_signal) exit(0); // exit cleanly on interrupt

		imshow("Screen", frame);  // Show the image on the screen

		char key = waitKey(30);
		if (key == ' '){
			//cout << "setting is " << key + 0<< endl;
			cout << "space bar\n";
			ostringstream name;
			name << "calibrate_webcam/img" << count++ << ".jpg";
			string filename = name.str();
			imwrite(filename, frame);
			waitKey(1000);
		}else if(key == 'x'){
			return count;
		}
	}
}

void calibrate(int numImages, int numCornersHorizontal, int numCornersVertical, const char* intrinsic_name, const char* distortion_name){
	ofstream intrinsicFile;
	intrinsicFile.open(intrinsic_name);
	ofstream distortionParams;
	distortionParams.open(distortion_name);

// Task 2
	int numSquares = numCornersHorizontal * numCornersVertical;
	Size boardSize = Size(numCornersHorizontal, numCornersVertical);

	vector<vector<Point3f> > object_points;
	vector<vector<Point2f> > image_points;
	Mat image;

	vector<Point3f> obj;
	for(int j=0;j<numSquares;j++){
		obj.push_back(Point3f( j/numCornersHorizontal , j%numCornersHorizontal , 0.0f));
	}

	for (int i = 0; i < numImages; i++){
		vector<Point2f> pointBuf;
		ostringstream name;
		name << "calibrate_webcam/img" << i << ".jpg";
		string filename = name.str();
		image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE);

	// Find chessboard inner corners, 10 corners per row/7 per column
		bool found = findChessboardCorners(image, boardSize, pointBuf, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);

		if(found){
		// Find subpixels with the criteria
			TermCriteria criteria = TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 40, 0.01 );
			cornerSubPix(image, pointBuf, Size(10,10), Size(-1, -1), criteria);
			drawChessboardCorners(image, cvSize(10,7), pointBuf, found);

			cvtColor(image, image, CV_GRAY2BGR);
			image_points.push_back(pointBuf);
			object_points.push_back(obj);
		// Draw the corners on a colored image
	/*	// Write the image
			//imwrite("Task_1.jpg", image[i]);

			//imshow("Screen", image[i]);
			//waitKey(100);*/
			std::cout << i + 1 << " processed " << pointBuf[0] << "\n";
		}
	}

	Mat intrinsic = Mat(3,3, CV_32FC1);
	Mat distCoeffs;
	Size imageSize = image.size();
	vector<Mat> rvecs, tvecs;
	intrinsic.ptr<double>(0)[0] = 1;
	intrinsic.ptr<double>(1)[1] = 1;

	calibrateCamera(object_points, image_points, imageSize, intrinsic, distCoeffs, rvecs, tvecs);
	std::cout << "Matrix is " << intrinsic << std::endl << distCoeffs << std::endl;

	for(int i = 0; i < 3; i++){
		for(int j =0; j < 3; j++){
			intrinsicFile << intrinsic.at<double>(i, j) << " ";
		}
	}

	for(int k = 0; k < 5; k++)
		distortionParams << distCoeffs.at<double>(k) << " ";

//	intrinsicFile << intrinsic;
//	distortionParams << distCoeffs;

	intrinsicFile.close();
	distortionParams.close();
	return;
}

int main(int argc, char** argv){
	VideoCapture cap(0); // open the default camera
	if(!cap.isOpened()){ // check if we succeeded
		return -1;
	}
	#ifdef __unix__
	   signal(SIGINT,quit_signal_handler); // listen for ctrl-C
	#endif

	int count = capture_video_spacebar(cap);

	int cornersHoriz = 9;
	int cornersVert = 7;
	calibrate(count, cornersHoriz, cornersVert, "intrinsic_webcam.txt", "distortion_webcam.txt");

// the camera will be deinitialized automatically in VideoCapture destructor

return 0;
}


//          Copyright Gavin Band 2008 - 2012.
// 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 CORE_ASCENT_DIRECTION_PICKER_HPP
#define CORE_ASCENT_DIRECTION_PICKER_HPP

#include <iostream>
#include <boost/noncopyable.hpp>
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>
#include "integration/ModifiedCholesky.hpp"

// #define DEBUG_ASCENT_DIRECTION_PICKER 1

namespace integration {
	template< typename Function >
	struct CholeskyOrEigenvalueSolver: public boost::noncopyable {
	public:
		typedef Eigen::VectorXd Vector ;
		typedef Eigen::MatrixXd Matrix ;

	public:
		CholeskyOrEigenvalueSolver(
			double const delta = -0.1
		):
			m_delta( delta )
		{
			assert( delta < 0 ) ;
		}

		void set_delta( double const delta ) {
			assert( delta < 0 ) ;
			m_delta = delta ;
		}

		Vector compute( Function& function, Vector const& point ) {
			function.evaluate_at( point, 2 ) ;
			Matrix const matrix = function.get_value_of_second_derivative() ;
			Vector const v = -function.get_value_of_first_derivative() ;
			m_cholesky_solver.compute( matrix ) ;
			if( m_cholesky_solver.info() == Eigen::Success && m_cholesky_solver.vectorD().array().maxCoeff() < 0 ) {
				return m_cholesky_solver.solve( v ) ;
			} else {
				m_eigen_solver.compute( matrix ) ;
				if( m_eigen_solver.info() == Eigen::NoConvergence ) {
					throw NumericalError( "integration::CholeskyOrEigenvalueSolver::solve()", "Eigenvalue decomposition did not converge" ) ;
				}

				m_d = m_eigen_solver.eigenvalues() ;
				for( int i = 0; i < m_d.size(); ++i ) {
					if( m_d(i) > m_delta ) {
						m_d(i) = m_delta ;
					}
				}
				m_d = m_d.array().inverse() ;

				return (
					m_eigen_solver.eigenvectors() * m_d.asDiagonal() * m_eigen_solver.eigenvectors().transpose()
				) * v ;
			}
		}

	private:
		double m_delta ;
		Eigen::LDLT< Matrix > m_cholesky_solver ;
		Eigen::SelfAdjointEigenSolver< Matrix > m_eigen_solver ;
		Eigen::VectorXd m_d ;
	} ;

}

#endif


///////////////////////////////////////////////////////////////////////////////
// weighted_tail_quantile.hpp
//
//  Copyright 2006 Daniel Egloff, Olivier Gygi. 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_ACCUMULATORS_STATISTICS_WEIGHTED_TAIL_QUANTILE_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_TAIL_QUANTILE_HPP_DE_01_01_2006

#include <vector>
#include <limits>
#include <functional>
#include <sstream>
#include <stdexcept>
#include <boost/throw_exception.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/tail.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>
#include <boost/accumulators/statistics/parameters/quantile_probability.hpp>

#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 4127) // conditional expression is constant
#endif

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // weighted_tail_quantile_impl
    //  Tail quantile estimation based on order statistics of weighted samples
    /**
        @brief Tail quantile estimation based on order statistics of weighted samples (for both left and right tails)

        An estimator \f$\hat{q}\f$ of tail quantiles with level \f$\alpha\f$ based on order statistics
        \f$X_{1:n} \leq X_{2:n} \leq\dots\leq X_{n:n}\f$ of weighted samples are given by \f$X_{\lambda:n}\f$ (left tail)
        and \f$X_{\rho:n}\f$ (right tail), where

            \f[
                \lambda = \inf\left\{ l \left| \frac{1}{\bar{w}_n}\sum_{i=1}^{l} w_i \geq \alpha \right. \right\}
            \f]

        and

            \f[
                \rho = \sup\left\{ r \left| \frac{1}{\bar{w}_n}\sum_{i=r}^{n} w_i \geq (1 - \alpha) \right. \right\},
            \f]

        \f$n\f$ being the number of samples and \f$\bar{w}_n\f$ the sum of all weights.

        @param quantile_probability
    */
    template<typename Sample, typename Weight, typename LeftRight>
    struct weighted_tail_quantile_impl
      : accumulator_base
    {
        typedef typename numeric::functional::average<Weight, std::size_t>::result_type float_type;
        // for boost::result_of
        typedef Sample result_type;

        weighted_tail_quantile_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            float_type threshold = sum_of_weights(args)
                             * ( ( is_same<LeftRight, left>::value ) ? args[quantile_probability] : 1. - args[quantile_probability] );

            std::size_t n = 0;
            Weight sum = Weight(0);

            while (sum < threshold)
            {
                if (n < static_cast<std::size_t>(tail_weights(args).size()))
                {
                    sum += *(tail_weights(args).begin() + n);
                    n++;
                }
                else
                {
                    if (std::numeric_limits<result_type>::has_quiet_NaN)
                    {
                        return std::numeric_limits<result_type>::quiet_NaN();
                    }
                    else
                    {
                        std::ostringstream msg;
                        msg << "index n = " << n << " is not in valid range [0, " << tail(args).size() << ")";
                        boost::throw_exception(std::runtime_error(msg.str()));
                        return Sample(0);
                    }
                }
            }

            // Note that the cached samples of the left are sorted in ascending order,
            // whereas the samples of the right tail are sorted in descending order
            return *(boost::begin(tail(args)) + n - 1);
        }
    };
} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::weighted_tail_quantile<>
//
namespace tag
{
    template<typename LeftRight>
    struct weighted_tail_quantile
      : depends_on<sum_of_weights, tail_weights<LeftRight> >
    {
        /// INTERNAL ONLY
        typedef accumulators::impl::weighted_tail_quantile_impl<mpl::_1, mpl::_2, LeftRight> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::weighted_tail_quantile
//
namespace extract
{
    extractor<tag::quantile> const weighted_tail_quantile = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_tail_quantile)
}

using extract::weighted_tail_quantile;

}} // namespace boost::accumulators

#ifdef _MSC_VER
# pragma warning(pop)
#endif

#endif


// std c
#include <stdio.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <iostream>
#include <string>
#include <sstream>
#include <ctime>
#include<numeric>
// opencv pcl
#include <opencv/cv.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/imgproc/imgproc.hpp"

// ros
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>

// Eigen
#include <Eigen/Dense>
#include <Eigen/Core>

// ours
#include "detect_3d_cuboid/matrix_utils.h"
#include "detect_3d_cuboid/object_3d_util.h"
#include "tictoc_profiler/profiler.hpp"

using namespace std;
// using namespace cv;
using namespace Eigen;

void detect_3d_cuboid::set_calibration(const Matrix3d& Kalib)
{
      cam_pose.Kalib = Kalib;
      cam_pose.invK = Kalib.inverse();
}

void detect_3d_cuboid::set_cam_pose(const Matrix4d& transToWolrd)
{
      cam_pose.transToWolrd = transToWolrd;
      cam_pose.rotationToWorld = transToWolrd.topLeftCorner<3,3>();
      Vector3d euler_angles; quat_to_euler_zyx(Quaterniond(cam_pose.rotationToWorld),euler_angles(0),euler_angles(1),euler_angles(2));
      cam_pose.euler_angle = euler_angles;
      cam_pose.invR = cam_pose.rotationToWorld.inverse();
      cam_pose.projectionMatrix = cam_pose.Kalib*transToWolrd.inverse().topRows<3>();  // project world coordinate to camera
      cam_pose.KinvR = cam_pose.Kalib * cam_pose.invR;
      cam_pose.camera_yaw = cam_pose.euler_angle(2);
      //TODO relative measure? not good... then need to change transToWolrd.
}

void detect_3d_cuboid::detect_cuboid(const cv::Mat& rgb_img, const Matrix4d& transToWolrd, const MatrixXd& obj_bbox_coors,
				     MatrixXd all_lines_raw, std::vector<ObjectSet>& all_object_cuboids)
{
      set_cam_pose(transToWolrd);
      cam_pose_raw = cam_pose;

      cv::Mat gray_img;
      if (rgb_img.channels()==3)
	  cv::cvtColor(rgb_img, gray_img, CV_BGR2GRAY);
      else
	  gray_img = rgb_img;

      int img_width = rgb_img.cols;  int img_height = rgb_img.rows;

      int num_2d_objs = obj_bbox_coors.rows();
      all_object_cuboids.resize(num_2d_objs);

      vector<bool> all_configs;all_configs.push_back(consider_config_1);all_configs.push_back(consider_config_2);

      // parameters for cuboid generation
      double vp12_edge_angle_thre = 15; double vp3_edge_angle_thre = 10;  // 10  10  parameters
      double shorted_edge_thre = 20;  // if box edge are too short. box might be too thin. most possibly wrong.
      bool reweight_edge_distance = true;  // if want to compare with all configurations. we need to reweight

      // parameters for proposal scoring
      bool whether_normalize_two_errors = true; double weight_vp_angle = 0.8; double weight_skew_error = 1.5;
      // if also consider config2, need to weight two erros, in order to compare two configurations

      align_left_right_edges(all_lines_raw); // this should be guaranteed when detecting edges
      if(whether_plot_detail_images)
      {
	  cv::Mat output_img;  plot_image_with_edges(rgb_img, output_img, all_lines_raw, cv::Scalar(255,0,0));
	  cv::imshow("Raw detected Edges", output_img);	 //cv::waitKey(0);
      }

      // find ground-wall boundary edges
      Vector4d ground_plane_world(0,0,1,0);  // treated as column vector % in my pop-up code, I use [0 0 -1 0]. here I want the normal pointing innerwards, towards the camera to match surface normal prediction
      Vector4d ground_plane_sensor = cam_pose.transToWolrd.transpose()*ground_plane_world;

//       int object_id=1;
      for (int object_id=0;object_id<num_2d_objs;object_id++)
      {
// 	  std::cout<<"object id  "<<object_id<<std::endl;
	  ca::Profiler::tictoc("One 3D object total time");
	  int left_x_raw = obj_bbox_coors(object_id,0); int top_y_raw = obj_bbox_coors(object_id,1);
	  int obj_width_raw = obj_bbox_coors(object_id,2); int obj_height_raw = obj_bbox_coors(object_id,3);
	  int right_x_raw = left_x_raw+obj_bbox_coors(object_id,2); int down_y_raw = top_y_raw + obj_height_raw;

	  std::vector<int> down_expand_sample_all;
	  down_expand_sample_all.push_back(0);
	  if (whether_sample_bbox_height)  // 2D object detection might not be accurate
	  {
	      int down_expand_sample_ranges = max(min(20, obj_height_raw-90),20);
	      down_expand_sample_ranges = min(down_expand_sample_ranges,img_height-top_y_raw-obj_height_raw-1);  // should lie inside the image  -1 for c++ index
	      if (down_expand_sample_ranges>10)  // if expand large margin, give more samples.
		  down_expand_sample_all.push_back(round(down_expand_sample_ranges/2));
	      down_expand_sample_all.push_back(down_expand_sample_ranges);
	  }

	  // NOTE later if in video, could use previous object yaw..., also reduce search range
	  double yaw_init = cam_pose.camera_yaw-90.0/180.0*M_PI;  // yaw init is directly facing the camera, align with camera optical axis
	  std::vector<double> obj_yaw_samples; linespace<double>(yaw_init-45.0/180.0*M_PI, yaw_init+45.0/180.0*M_PI, 6.0/180.0*M_PI, obj_yaw_samples);

	  MatrixXd all_configs_errors(400,9); MatrixXd all_box_corners_2ds(800,8); // initialize a large eigen matrix
	  int valid_config_number_all_height=0; // all valid objects of all height samples
	  ObjectSet raw_obj_proposals;raw_obj_proposals.reserve(100);
// 	    int sample_down_expan_id=1;
	  for (int sample_down_expan_id=0;sample_down_expan_id<down_expand_sample_all.size();sample_down_expan_id++)
	  {
	      int down_expand_sample = down_expand_sample_all[sample_down_expan_id];
	      int obj_height_expan = obj_height_raw + down_expand_sample;
	      int down_y_expan = top_y_raw + obj_height_expan; double obj_diaglength_expan = sqrt(obj_width_raw*obj_width_raw+obj_height_expan*obj_height_expan);

	      // sample points on the top edges, if edge is too large, give more samples. give at least 10 samples for all edges. for small object, object pose changes lots
	      int top_sample_resolution = round(min(20,obj_width_raw/10 )); //  25 pixels
	      std::vector<int> top_x_samples; linespace<int>(left_x_raw+5, right_x_raw-5, top_sample_resolution, top_x_samples);
	      MatrixXd sample_top_pts(2,top_x_samples.size());
	      for (int ii=0;ii<top_x_samples.size();ii++)
	      {
		  sample_top_pts(0,ii)=top_x_samples[ii];
		  sample_top_pts(1,ii)=top_y_raw;
	      }

	      // expand some small margin for distance map  [10 20]
	      int distmap_expand_wid = min(max(min(20, obj_width_raw-100),10),max(min(20, obj_height_expan-100),10));
	      int left_x_expan_distmap = max(0,left_x_raw-distmap_expand_wid); int right_x_expan_distmap = min(img_width-1,right_x_raw+distmap_expand_wid);
	      int top_y_expan_distmap = max(0,top_y_raw-distmap_expand_wid); int down_y_expan_distmap = min(img_height-1,down_y_expan+distmap_expand_wid);
	      int height_expan_distmap = down_y_expan_distmap - top_y_expan_distmap; int width_expan_distmap = right_x_expan_distmap - left_x_expan_distmap;
	      Vector2d expan_distmap_lefttop = Vector2d(left_x_expan_distmap, top_y_expan_distmap);
	      Vector2d expan_distmap_rightbottom = Vector2d(right_x_expan_distmap, down_y_expan_distmap);

	      // find edges inside the object bounding box
	      MatrixXd all_lines_inside_object(all_lines_raw.rows(),all_lines_raw.cols()); // first allocate a large matrix, then only use the toprows to avoid copy, alloc
	      int inside_obj_edge_num = 0;
	      for (int edge_id=0;edge_id<all_lines_raw.rows();edge_id++)
		if (check_inside_box(all_lines_raw.row(edge_id).head<2>(),expan_distmap_lefttop, expan_distmap_rightbottom ))
		  if (check_inside_box(all_lines_raw.row(edge_id).tail<2>(),expan_distmap_lefttop, expan_distmap_rightbottom ))
		    {
			all_lines_inside_object.row(inside_obj_edge_num) = all_lines_raw.row(edge_id);
			inside_obj_edge_num++;
		    }

	      // merge edges and remove short lines, after finding object edges.  edge merge in small regions should be faster than all.
	      double pre_merge_dist_thre = 20; double pre_merge_angle_thre = 5; double edge_length_threshold=30;
	      MatrixXd all_lines_merge_inobj;
	      merge_break_lines(all_lines_inside_object.topRows(inside_obj_edge_num),all_lines_merge_inobj,pre_merge_dist_thre,
				pre_merge_angle_thre,edge_length_threshold);

	      // compute edge angels and middle points
	      VectorXd lines_inobj_angles(all_lines_merge_inobj.rows());
	      MatrixXd edge_mid_pts(all_lines_merge_inobj.rows(),2);
	      for (int i=0;i<all_lines_merge_inobj.rows();i++)
	      {
		  lines_inobj_angles(i) = std::atan2(all_lines_merge_inobj(i,3)-all_lines_merge_inobj(i,1), all_lines_merge_inobj(i,2)-all_lines_merge_inobj(i,0));   // [-pi/2 -pi/2]
		  edge_mid_pts.row(i).head<2>() = (all_lines_merge_inobj.row(i).head<2>()+all_lines_merge_inobj.row(i).tail<2>())/2;
	      }

	      // TODO could canny or distance map outside sampling height to speed up!!!!   Then only need to compute canny onces.
	      // detect canny edges and compute distance transform  NOTE opencv canny maybe different from matlab. but roughly same
	      cv::Rect object_bbox = cv::Rect(left_x_expan_distmap,top_y_expan_distmap,width_expan_distmap,height_expan_distmap);//
	      cv::Mat im_canny; cv::Canny(gray_img(object_bbox),im_canny,80,200); // low thre, high thre    im_canny 0 or 255   [80 200  40 100]
	      cv::Mat dist_map; cv::distanceTransform(255-im_canny,dist_map,CV_DIST_L2,3); // dist_map is float datatype

	      if (whether_plot_detail_images){
		  cv::imshow("im_canny",im_canny);
		  cv::Mat dist_map_img;cv::normalize(dist_map, dist_map_img, 0.0, 1.0, cv::NORM_MINMAX);
		  cv::imshow("normalized distance map", dist_map_img);cv::waitKey();
	      }

	      // Generate cuboids
	      MatrixXd all_configs_error_one_objH(200,9);    MatrixXd all_box_corners_2d_one_objH(400,8);
	      int valid_config_number_one_objH=0;

	      std::vector<double> cam_roll_samples; std::vector<double> cam_pitch_samples;
	      if (whether_sample_cam_roll_pitch)
	      {
		  linespace<double>(cam_pose_raw.euler_angle(0)-6.0/180.0*M_PI, cam_pose_raw.euler_angle(0)+6.0/180.0*M_PI, 3.0/180.0*M_PI, cam_roll_samples);
		  linespace<double>(cam_pose_raw.euler_angle(1)-6.0/180.0*M_PI, cam_pose_raw.euler_angle(1)+6.0/180.0*M_PI, 3.0/180.0*M_PI, cam_pitch_samples);
	      }
	      else
	      {
		  cam_roll_samples.push_back(cam_pose_raw.euler_angle(0));cam_pitch_samples.push_back(cam_pose_raw.euler_angle(1));
	      }
	      // different from matlab. first for loop yaw, then for configurations.
// 	      int obj_yaw_id=8;
	      for (int cam_roll_id=0;cam_roll_id<cam_roll_samples.size();cam_roll_id++)
	      for (int cam_pitch_id=0;cam_pitch_id<cam_pitch_samples.size();cam_pitch_id++)
	      for (int obj_yaw_id=0;obj_yaw_id<obj_yaw_samples.size();obj_yaw_id++)
	      {
		  if (whether_sample_cam_roll_pitch)
		  {
		      Matrix4d transToWolrd_new = transToWolrd;
		      transToWolrd_new.topLeftCorner<3,3>() = euler_zyx_to_rot<double>(cam_roll_samples[cam_roll_id], cam_pitch_samples[cam_pitch_id], cam_pose_raw.euler_angle(2));
		      set_cam_pose(transToWolrd_new);
		      ground_plane_sensor = cam_pose.transToWolrd.transpose()*ground_plane_world;
		  }

		  double obj_yaw_esti=obj_yaw_samples[obj_yaw_id];

		  Vector2d vp_1, vp_2, vp_3;
		  getVanishingPoints(cam_pose.KinvR, obj_yaw_esti, vp_1,vp_2,vp_3); // for object x y z  axis

		  MatrixXd all_vps(3,2);all_vps.row(0)=vp_1;all_vps.row(1)=vp_2;all_vps.row(2)=vp_3;
// 		  std::cout<<"obj_yaw_esti  "<<obj_yaw_esti<<"  "<<obj_yaw_id<<std::endl;
		  MatrixXd all_vp_bound_edge_angles = VP_support_edge_infos(all_vps, edge_mid_pts,lines_inobj_angles,
									    Vector2d(vp12_edge_angle_thre,vp3_edge_angle_thre));
// 		  int sample_top_pt_id=15;
		  for (int sample_top_pt_id=0;sample_top_pt_id<sample_top_pts.cols();sample_top_pt_id++)
		  {
// 		      std::cout<<"sample_top_pt_id "<<sample_top_pt_id<<std::endl;
		      Vector2d corner_1_top = sample_top_pts.col(sample_top_pt_id);
		      bool config_good = true;
		      int vp_1_position = 0;  // 0 initial as fail,  1  on left   2 on right
		      Vector2d corner_2_top = seg_hit_boundary(vp_1,corner_1_top,Vector4d(right_x_raw, top_y_raw, right_x_raw, down_y_expan));
		      if (corner_2_top(0)==-1){  // vp1-corner1 doesn't hit the right boundary. check whether hit left
			  corner_2_top = seg_hit_boundary(vp_1,corner_1_top,Vector4d(left_x_raw, top_y_raw, left_x_raw, down_y_expan));
			  if (corner_2_top(0)!=-1) // vp1-corner1 hit the left boundary   vp1 on the right
			      vp_1_position = 2;
		      }
		      else    // vp1-corner1 hit the right boundary   vp1 on the left
			  vp_1_position = 1;

		      config_good = vp_1_position>0;
		      if (!config_good){
			  if (print_details) printf("Configuration fails at corner 2, outside segment\n");
			  continue;
		      }
		      if ((corner_1_top-corner_2_top).norm()<shorted_edge_thre){
			  if (print_details) printf("Configuration fails at edge 1-2, too short\n");
			  continue;
		      }
// 		      cout<<"corner_1/2   "<<corner_1_top.transpose()<<"   "<<corner_2_top.transpose()<<endl;
// 		      int config_ind=0; // have to consider config now.
		      for (int config_id=1;config_id<3;config_id++)  // configuration one or two of matlab version
		      {
			  if (!all_configs[config_id-1])
			      continue;
			  Vector2d corner_3_top,corner_4_top;
			  if (config_id==1)
			  {
			      if (vp_1_position==1)   // then vp2 hit the left boundary
				  corner_4_top = seg_hit_boundary(vp_2, corner_1_top,Vector4d(left_x_raw, top_y_raw, left_x_raw, down_y_expan));
			      else  // or, then vp2 hit the right boundary
				  corner_4_top = seg_hit_boundary(vp_2, corner_1_top,Vector4d(right_x_raw, top_y_raw, right_x_raw, down_y_expan));
			      if (corner_4_top(1)==-1){
				  config_good = false;
				  if (print_details)  printf("Configuration %d fails at corner 4, outside segment\n",config_id);
				  continue;
			      }
			      if ((corner_1_top-corner_4_top).norm()<shorted_edge_thre){
				  if (print_details) printf("Configuration %d fails at edge 1-4, too short\n",config_id);
				  continue;
			      }
			      // compute the last point in the top face
			      corner_3_top=lineSegmentIntersect(vp_2,corner_2_top,vp_1,corner_4_top,true);
			      if (!check_inside_box( corner_3_top, Vector2d(left_x_raw,top_y_raw), Vector2d(right_x_raw, down_y_expan))){    // check inside boundary. otherwise edge visibility might be wrong
				  config_good=false;
				  if (print_details) printf("Configuration %d fails at corner 3, outside box\n",config_id);
				  continue;
			      }
			      if ( ((corner_3_top-corner_4_top).norm()<shorted_edge_thre) || ((corner_3_top-corner_2_top).norm()<shorted_edge_thre) ){
				  if (print_details) printf("Configuration %d fails at edge 3-4/3-2, too short\n",config_id);
				  continue;
			      }
// 			      cout<<"corner_3/4   "<<corner_3_top.transpose()<<"   "<<corner_4_top.transpose()<<endl;
			  }
			  if (config_id==2)
			  {
			      if (vp_1_position==1)   // then vp2 hit the left boundary
				  corner_3_top = seg_hit_boundary(vp_2, corner_2_top,Vector4d(left_x_raw, top_y_raw, left_x_raw, down_y_expan));
			      else  // or, then vp2 hit the right boundary
				  corner_3_top = seg_hit_boundary(vp_2, corner_2_top,Vector4d(right_x_raw, top_y_raw, right_x_raw, down_y_expan));
			      if (corner_3_top(1)==-1){
				  config_good = false;
				  if (print_details)  printf("Configuration %d fails at corner 3, outside segment\n",config_id);
				  continue;
			      }
			      if ((corner_2_top-corner_3_top).norm()<shorted_edge_thre){
				  if (print_details) printf("Configuration %d fails at edge 2-3, too short\n",config_id);
				  continue;
			      }
			      // compute the last point in the top face
			      corner_4_top=lineSegmentIntersect(vp_1,corner_3_top,vp_2,corner_1_top,true);
			      if (!check_inside_box( corner_4_top, Vector2d(left_x_raw,top_y_expan_distmap), Vector2d(right_x_raw, down_y_expan_distmap))){
				  config_good=false;
				  if (print_details) printf("Configuration %d fails at corner 4, outside box\n",config_id);
				  continue;
			      }
			      if ( ((corner_3_top-corner_4_top).norm()<shorted_edge_thre) || ((corner_4_top-corner_1_top).norm()<shorted_edge_thre) ){
				  if (print_details) printf("Configuration %d fails at edge 3-4/4-1, too short\n",config_id);
				  continue;
			      }
// 			      cout<<"corner_3/4   "<<corner_3_top.transpose()<<"   "<<corner_4_top.transpose()<<endl;
			  }
			  // compute first bottom points    computing bottom points is the same for config 1,2
			  Vector2d corner_5_down = seg_hit_boundary(vp_3, corner_3_top,Vector4d(left_x_raw,down_y_expan,right_x_raw,down_y_expan));
			  if (corner_5_down(1)==-1){
			      config_good = false;
			      if (print_details) printf("Configuration %d fails at corner 5, outside segment\n",config_id);
			      continue;
			  }
			  if ((corner_3_top-corner_5_down).norm()<shorted_edge_thre){
			      if (print_details) printf("Configuration %d fails at edge 3-5, too short\n",config_id);
			      continue;
			  }
			  Vector2d corner_6_down=lineSegmentIntersect(vp_2,corner_5_down,vp_3, corner_2_top,true);
			  if (!check_inside_box( corner_6_down, expan_distmap_lefttop, expan_distmap_rightbottom)){
			      config_good=false;
			      if (print_details) printf("Configuration %d fails at corner 6, outside box\n",config_id);
			      continue;
			  }
			  if ( ((corner_6_down-corner_2_top).norm()<shorted_edge_thre) || ((corner_6_down-corner_5_down).norm()<shorted_edge_thre) ){
			      if (print_details) printf("Configuration %d fails at edge 6-5/6-2, too short\n",config_id);
			      continue;
			  }
			  Vector2d corner_7_down=lineSegmentIntersect(vp_1,corner_6_down,vp_3, corner_1_top,true);
			  if (!check_inside_box( corner_7_down, expan_distmap_lefttop, expan_distmap_rightbottom)){// might be slightly different from matlab
			      config_good=false;
			      if (print_details) printf("Configuration %d fails at corner 7, outside box\n",config_id);
			      continue;
			  }
			  if ( ((corner_7_down-corner_1_top).norm()<shorted_edge_thre) || ((corner_7_down-corner_6_down).norm()<shorted_edge_thre) ){
			      if (print_details) printf("Configuration %d fails at edge 7-1/7-6, too short\n",config_id);
			      continue;
			  }
			  Vector2d corner_8_down=lineSegmentIntersect(vp_1,corner_5_down,vp_2, corner_7_down,true);
			  if (!check_inside_box( corner_8_down, expan_distmap_lefttop, expan_distmap_rightbottom)){
			      config_good=false;
			      if (print_details) printf("Configuration %d fails at corner 8, outside box\n",config_id);
			      continue;
			  }
			  if ( ((corner_8_down-corner_4_top).norm()<shorted_edge_thre) || ((corner_8_down-corner_5_down).norm()<shorted_edge_thre) || ((corner_8_down-corner_7_down).norm()<shorted_edge_thre)){
			      if (print_details) printf("Configuration %d fails at edge 8-4/8-5/8-7, too short\n",config_id);
			      continue;
			  }

			  MatrixXd box_corners_2d_float(2,8);
			  box_corners_2d_float<<corner_1_top,corner_2_top,corner_3_top,corner_4_top,corner_5_down,corner_6_down,corner_7_down,corner_8_down;
// 			  std::cout<<"box_corners_2d_float \n "<<box_corners_2d_float<<std::endl;
			  MatrixXd box_corners_2d_float_shift(2,8);box_corners_2d_float_shift.row(0)=box_corners_2d_float.row(0).array()-left_x_expan_distmap;
			  box_corners_2d_float_shift.row(1)=box_corners_2d_float.row(1).array()-top_y_expan_distmap;

			  MatrixXi visible_edge_pt_ids,vps_box_edge_pt_ids;
			  double sum_dist;
			  if (config_id==1)
			  {
			      visible_edge_pt_ids.resize(9,2); visible_edge_pt_ids<<1,2, 2,3, 3,4, 4,1, 2,6, 3,5, 4,8, 5,8, 5,6;
			      vps_box_edge_pt_ids.resize(3,4); vps_box_edge_pt_ids<<1,2,8,5, 4,1,5,6, 4,8,2,6; // six edges. each row represents two edges [e1_1 e1_2   e2_1 e2_2;...] of one VP
			      visible_edge_pt_ids.array() -=1; vps_box_edge_pt_ids.array() -=1; //change to c++ index
			      sum_dist = box_edge_sum_dists(dist_map,box_corners_2d_float_shift,visible_edge_pt_ids);
			  }
			  else
			  {
			      visible_edge_pt_ids.resize(7,2); visible_edge_pt_ids<<1,2, 2,3, 3,4, 4,1, 2,6, 3,5, 5,6;
			      vps_box_edge_pt_ids.resize(3,4); vps_box_edge_pt_ids<<1,2,3,4, 4,1,5,6, 3,5,2,6; // six edges. each row represents two edges [e1_1 e1_2   e2_1 e2_2;...] of one VP
			      visible_edge_pt_ids.array() -=1; vps_box_edge_pt_ids.array() -=1;
			      sum_dist = box_edge_sum_dists(dist_map,box_corners_2d_float_shift,visible_edge_pt_ids,reweight_edge_distance);
			  }
			  double total_angle_diff = box_edge_alignment_angle_error(all_vp_bound_edge_angles,vps_box_edge_pt_ids,box_corners_2d_float);
			  all_configs_error_one_objH.row(valid_config_number_one_objH).head<4>()=Vector4d(config_id, vp_1_position, obj_yaw_esti,sample_top_pt_id);
			  all_configs_error_one_objH.row(valid_config_number_one_objH).segment<3>(4) = Vector3d(sum_dist/obj_diaglength_expan, total_angle_diff, down_expand_sample);
			  if (whether_sample_cam_roll_pitch)
			      all_configs_error_one_objH.row(valid_config_number_one_objH).segment<2>(7) = Vector2d(cam_roll_samples[cam_roll_id],cam_pitch_samples[cam_pitch_id]);
			  else
			      all_configs_error_one_objH.row(valid_config_number_one_objH).segment<2>(7) = Vector2d(cam_pose_raw.euler_angle(0),cam_pose_raw.euler_angle(1));
			  all_box_corners_2d_one_objH.block(2*valid_config_number_one_objH,0,2,8)=box_corners_2d_float;
			  valid_config_number_one_objH++;
			  if (valid_config_number_one_objH>=all_configs_error_one_objH.rows())
			  {
			      all_configs_error_one_objH.conservativeResize(2*valid_config_number_one_objH,NoChange);
			      all_box_corners_2d_one_objH.conservativeResize(4*valid_config_number_one_objH,NoChange);
			  }
		      } //end of config loop
		  } //end of top id
	      } //end of yaw

// 	      std::cout<<"valid_config_number_one_hseight  "<<valid_config_number_one_objH<<std::endl;
// 	      std::cout<<"all_configs_error_one_objH  \n"<<all_configs_error_one_objH.topRows(valid_config_number_one_objH)<<std::endl;
// 	      MatrixXd all_corners = all_box_corners_2d_one_objH.topRows(2*valid_config_number_one_objH);
// 	      std::cout<<"all corners   "<<all_corners<<std::endl;

	      VectorXd normalized_score; vector<int> good_proposal_ids;
	      fuse_normalize_scores_v2(all_configs_error_one_objH.col(4).head(valid_config_number_one_objH), all_configs_error_one_objH.col(5).head(valid_config_number_one_objH),
				    normalized_score, good_proposal_ids, weight_vp_angle,whether_normalize_two_errors);

	      for (int box_id=0;box_id<good_proposal_ids.size();box_id++)
	      {
		  int raw_cube_ind = good_proposal_ids[box_id];

		  if (whether_sample_cam_roll_pitch)
		  {
		      Matrix4d transToWolrd_new = transToWolrd;
		      transToWolrd_new.topLeftCorner<3,3>() = euler_zyx_to_rot<double>(all_configs_error_one_objH(raw_cube_ind,7), all_configs_error_one_objH(raw_cube_ind,8), cam_pose_raw.euler_angle(2));
		      set_cam_pose(transToWolrd_new);
		      ground_plane_sensor = cam_pose.transToWolrd.transpose()*ground_plane_world;
		  }

		  cuboid* sample_obj = new cuboid();
		  change_2d_corner_to_3d_object(all_box_corners_2d_one_objH.block(2*raw_cube_ind,0,2,8), all_configs_error_one_objH.row(raw_cube_ind).head<3>(),
						ground_plane_sensor,  cam_pose.transToWolrd, cam_pose.invK, cam_pose.projectionMatrix,*sample_obj);
// 		  sample_obj->print_cuboid();
		  if ((sample_obj->scale.array() < 0).any())     continue;                        // scale should be positive
		  sample_obj->rect_detect_2d =  Vector4d(left_x_raw,top_y_raw,obj_width_raw,obj_height_raw);
		  sample_obj->edge_distance_error = all_configs_error_one_objH(raw_cube_ind,4); // record the original error
		  sample_obj->edge_angle_error = all_configs_error_one_objH(raw_cube_ind,5);
		  sample_obj->normalized_error = normalized_score(box_id);
		  double skew_ratio = sample_obj->scale.head(2).maxCoeff()/sample_obj->scale.head(2).minCoeff();
		  sample_obj->skew_ratio = skew_ratio;
		  sample_obj->down_expand_height = all_configs_error_one_objH(raw_cube_ind,6);
		  if (whether_sample_cam_roll_pitch)
		  {
		      sample_obj->camera_roll_delta = all_configs_error_one_objH(raw_cube_ind,7)-cam_pose_raw.euler_angle(0);
		      sample_obj->camera_pitch_delta = all_configs_error_one_objH(raw_cube_ind,8)-cam_pose_raw.euler_angle(1);
		  }
		  else
		  {   sample_obj->camera_roll_delta = 0;sample_obj->camera_pitch_delta = 0; }

		  raw_obj_proposals.push_back(sample_obj);
	      }
	  } // end of differnet object height sampling

	  // %finally rank all proposals. [normalized_error   skew_error]
	  int actual_cuboid_num_small = std::min(max_cuboid_num,(int)raw_obj_proposals.size());
	  VectorXd all_combined_score(raw_obj_proposals.size());
	  for (int box_id=0;box_id<raw_obj_proposals.size();box_id++)
	  {
	      cuboid* sample_obj = raw_obj_proposals[box_id];
	      double skew_error = weight_skew_error*std::max(sample_obj->skew_ratio-nominal_skew_ratio,0.0);
	      if (sample_obj->skew_ratio > max_cut_skew)
		    skew_error = 100;
	      double new_combined_error = sample_obj->normalized_error+weight_skew_error*skew_error;
	      all_combined_score(box_id) = new_combined_error;
	  }

	  std::vector<int> sort_idx_small(all_combined_score.rows());   iota(sort_idx_small.begin(), sort_idx_small.end(), 0);
	  sort_indexes(all_combined_score, sort_idx_small,actual_cuboid_num_small);
	  for (int ii=0;ii<actual_cuboid_num_small;ii++) // use sorted index
	  {
	      all_object_cuboids[object_id].push_back(raw_obj_proposals[sort_idx_small[ii]]);
	  }

	  ca::Profiler::tictoc("One 3D object total time");
      }// end of different objects

      if (whether_plot_final_images || whether_save_final_images)
      {
	  cv::Mat frame_all_cubes_img = rgb_img.clone();
	  for (int object_id=0;object_id< all_object_cuboids.size();object_id++)
	    if ( all_object_cuboids[object_id].size()>0 )
	    {
		plot_image_with_cuboid(frame_all_cubes_img, all_object_cuboids[object_id][0]);
	    }
	  if (whether_save_final_images)
	      cuboids_2d_img = frame_all_cubes_img;
	  if (whether_plot_final_images)
	  {
	      cv::imshow("frame_all_cubes_img", frame_all_cubes_img);	 cv::waitKey(0);
	  }
      }
}