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

#include <nt2/linalg/functions/mtimes.hpp>
#include <nt2/linalg/details/blas/mm.hpp>
#include <nt2/core/functions/transpose.hpp>
#include <nt2/core/functions/ctranspose.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/multiplies.hpp>
#include <nt2/include/functions/ndims.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/core/container/dsl/size.hpp>
#include <nt2/core/container/dsl/alias.hpp>
#include <nt2/core/container/dsl/as_terminal.hpp>
#include <nt2/core/utility/assign_swap.hpp>
#include <nt2/sdk/memory/forward/container.hpp>
#include <nt2/sdk/memory/category.hpp>
#include <boost/proto/traits.hpp>
#include <boost/assert.hpp>

namespace nt2 { namespace tag
{
  struct blas_normal_
  {
    BOOST_FORCEINLINE static char call()
    {
      return 'N';
    }
  };

  struct blas_transpose_
  {
    BOOST_FORCEINLINE static char call()
    {
      return 'T';
    }
  };

  struct blas_ctranspose_
  {
    BOOST_FORCEINLINE static char call()
    {
      return 'C';
    }
  };
} }

namespace nt2 { namespace ext
{
  template<class Domain, int N,class Expr>
  struct size_of<tag::mtimes_, Domain, N, Expr>
  {
    typedef typename boost::proto::result_of::child_c<Expr&, 0>::value_type child0;
    typedef typename boost::proto::result_of::child_c<Expr&, 1>::value_type child1;

    typedef typename boost::proto::result_of::value< typename boost::proto::result_of::child_c<Expr&, 4>::value_type >::value_type trs0;
    typedef typename boost::proto::result_of::value< typename boost::proto::result_of::child_c<Expr&, 5>::value_type >::value_type trs1;

    typedef typename child0::extent_type sz0;
    typedef typename child1::extent_type sz1;

    static const std::size_t sz0_0 = boost::is_same<trs0, tag::blas_normal_>::value ? 0 : 1;
    static const std::size_t sz0_1 = boost::is_same<trs0, tag::blas_normal_>::value ? 1 : 0;
    static const std::size_t sz1_0 = boost::is_same<trs1, tag::blas_normal_>::value ? 0 : 1;
    static const std::size_t sz1_1 = boost::is_same<trs1, tag::blas_normal_>::value ? 1 : 0;

    typedef of_size_< mpl_value< typename boost::fusion::result_of::at_c<sz0, sz0_0>::type>::value
                    , mpl_value< typename boost::fusion::result_of::at_c<sz1, sz1_1>::type>::value
                    >  result_type;

    result_type operator()(Expr& e) const
    {
      sz0 const& size0 = boost::proto::child_c<0>(e).extent();
      sz1 const& size1 = boost::proto::child_c<1>(e).extent();

      BOOST_ASSERT_MSG( ndims(size0) <= 2 && ndims(size1) <= 2
                      , "Inputs must be 2-D, or at least one input must be scalar"
                      );

      BOOST_ASSERT_MSG( boost::fusion::at_c<sz0_1>(size0) == boost::fusion::at_c<sz1_0>(size1)
                      , "Inner dimensions must agree"
                      );

      return result_type( boost::fusion::at_c<sz0_0>(size0)
                        , boost::fusion::at_c<sz1_1>(size1)
                        );
    }
  };

  template<class Domain, int N,class Expr>
  struct value_type<tag::mtimes_, Domain, N, Expr>
  {
    typedef typename boost::proto::result_of::child_c<Expr&, 0>::value_type child0;
    typedef typename boost::proto::result_of::child_c<Expr&, 1>::value_type child1;
    typedef typename meta::call<tag::multiplies_(typename child0::value_type, typename child1::value_type)>::type type;
  };
} }

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( mtimes_, tag::cpu_
                            , (A0)(A1)
                            , ((ast_<A0, nt2::container::domain>))
                              ((ast_<A1, nt2::container::domain>))
                            )
  {
    typedef typename meta::call<tag::multiplies_(typename A0::value_type, typename A1::value_type)>::type T;
    BOOST_DISPATCH_RETURNS(2, (A0 const& a0, A1 const& a1),
      mtimes(a0, a1, Zero<T>(), One<T>(), tag::blas_normal_(), tag::blas_normal_())
    )
  };

  BOOST_DISPATCH_IMPLEMENT  ( mtimes_, tag::cpu_
                            , (A0)(A1)(A2)
                            , ((ast_<A0, nt2::container::domain>))
                              ((ast_<A1, nt2::container::domain>))
                              (scalar_< unspecified_<A2> >)
                            )
  {
    typedef typename meta::call<tag::multiplies_(typename A0::value_type, typename A1::value_type)>::type T;
    BOOST_DISPATCH_RETURNS(3, (A0 const& a0, A1 const& a1, A2 const& a2),
      mtimes(a0, a1, a2, One<T>(), tag::blas_normal_(), tag::blas_normal_())
    )
  };

  // Recognize scalar/matrix, matrix/scalar and scalar/scalar
  BOOST_DISPATCH_IMPLEMENT  ( mtimes_, tag::cpu_
                            , (A0)(A1)(T1)(N1)
                            , (scalar_< unspecified_<A0> >)
                              ((expr_< generic_< unspecified_<A1> >, T1, N1 >))
                            )
  {
    typedef typename meta::call<tag::multiplies_(A0 const&, A1 const&)>::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const& a1) const
    {
      return nt2::multiplies(a0, a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( mtimes_, tag::cpu_
                            , (A0)(T0)(N0)(A1)
                            , ((expr_< generic_< unspecified_<A0> >, T0, N0 >))
                              (scalar_< unspecified_<A1> >)
                            )
  {
    typedef typename meta::call<tag::multiplies_(A0 const&, A1 const&)>::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const& a1) const
    {
      return nt2::multiplies(a0, a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( mtimes_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< unspecified_<A0> >)
                              (scalar_< unspecified_<A1> >)
                            )
  {
    typedef typename meta::call<tag::multiplies_(A0 const&, A1 const&)>::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const& a1) const
    {
      return nt2::multiplies(a0, a1);
    }
  };

  // Recognize transpose
  BOOST_DISPATCH_IMPLEMENT  ( mtimes_, tag::cpu_
                            , (A0)(A1)(A2)(A3)(A4)(A5)
                            , ((node_< A0, nt2::tag::transpose_, boost::mpl::long_<1> , nt2::container::domain>))
                              ((ast_< A1, nt2::container::domain>))
                              (scalar_< unspecified_<A2> >)
                              (scalar_< unspecified_<A3> >)
                              (unspecified_<A4>)
                              (unspecified_<A5>)
                            )
  {
    BOOST_DISPATCH_RETURNS_ARGS(6, (A0 const& a0, A1 const& a1, A2 const& a2, A3 const& a3, A4 const& a4, A5 const& a5), (A0 const& a0, A1 const& a1, A2 const& a2, A3 const& a3, A4 const&, A5 const& a5),
      mtimes(boost::proto::child_c<0>(a0), a1, a2, a3, tag::blas_transpose_(), a5)
    )
  };

  BOOST_DISPATCH_IMPLEMENT  ( mtimes_, tag::cpu_
                            , (A0)(A1)(A2)(A3)(A4)(A5)
                            , ((ast_< A0, nt2::container::domain>))
                              ((node_< A1, nt2::tag::transpose_, boost::mpl::long_<1> , nt2::container::domain>))
                              (scalar_< unspecified_<A2> >)
                              (scalar_< unspecified_<A3> >)
                              (unspecified_<A4>)
                              (unspecified_<A5>)
                            )
  {
    BOOST_DISPATCH_RETURNS_ARGS(6, (A0 const& a0, A1 const& a1, A2 const& a2, A3 const& a3, A4 const& a4, A5 const& a5), (A0 const& a0, A1 const& a1, A2 const& a2, A3 const& a3, A4 const& a4, A5 const&),
      mtimes(a0, boost::proto::child_c<0>(a1), a2, a3, a4, tag::blas_transpose_())
    )
  };

  BOOST_DISPATCH_IMPLEMENT  ( mtimes_, tag::cpu_
                            , (A0)(A1)(A2)(A3)(A4)(A5)
                            , ((node_< A0, nt2::tag::transpose_, boost::mpl::long_<1> , nt2::container::domain>))
                              ((node_< A1, nt2::tag::transpose_, boost::mpl::long_<1> , nt2::container::domain>))
                              (scalar_< unspecified_<A2> >)
                              (scalar_< unspecified_<A3> >)
                              (unspecified_<A4>)
                              (unspecified_<A5>)
                            )
  {
    BOOST_DISPATCH_RETURNS_ARGS(6, (A0 const& a0, A1 const& a1, A2 const& a2, A3 const& a3, A4 const& a4, A5 const& a5), (A0 const& a0, A1 const& a1, A2 const& a2, A3 const& a3, A4 const&, A5 const&),
      mtimes(boost::proto::child_c<0>(a0), boost::proto::child_c<0>(a1), a2, a3, tag::blas_transpose_(), tag::blas_transpose_())
    )
  };

  // Recognize ctranspose
  BOOST_DISPATCH_IMPLEMENT  ( mtimes_, tag::cpu_
                            , (A0)(A1)(A2)(A3)(A4)(A5)
                            , ((node_< A0, nt2::tag::ctranspose_, boost::mpl::long_<1> , nt2::container::domain>))
                              ((ast_< A1, nt2::container::domain>))
                              (scalar_< unspecified_<A2> >)
                              (scalar_< unspecified_<A3> >)
                              (unspecified_<A4>)
                              (unspecified_<A5>)
                            )
  {
    BOOST_DISPATCH_RETURNS_ARGS(6, (A0 const& a0, A1 const& a1, A2 const& a2, A3 const& a3, A4 const& a4, A5 const& a5), (A0 const& a0, A1 const& a1, A2 const& a2, A3 const& a3, A4 const&, A5 const& a5),
      mtimes(boost::proto::child_c<0>(a0), a1, a2, a3, tag::blas_ctranspose_(), a5)
    )
  };

  BOOST_DISPATCH_IMPLEMENT  ( mtimes_, tag::cpu_
                            , (A0)(A1)(A2)(A3)(A4)(A5)
                            , ((ast_< A0, nt2::container::domain>))
                              ((node_< A1, nt2::tag::ctranspose_, boost::mpl::long_<1> , nt2::container::domain>))
                              (scalar_< unspecified_<A2> >)
                              (scalar_< unspecified_<A3> >)
                              (unspecified_<A4>)
                              (unspecified_<A5>)
                            )
  {
    BOOST_DISPATCH_RETURNS_ARGS(6, (A0 const& a0, A1 const& a1, A2 const& a2, A3 const& a3, A4 const& a4, A5 const& a5), (A0 const& a0, A1 const& a1, A2 const& a2, A3 const& a3, A4 const& a4, A5 const&),
      mtimes(a0, boost::proto::child_c<0>(a1), a2, a3, a4, tag::blas_ctranspose_())
    )
  };

  BOOST_DISPATCH_IMPLEMENT  ( mtimes_, tag::cpu_
                            , (A0)(A1)(A2)(A3)(A4)(A5)
                            , ((node_< A0, nt2::tag::ctranspose_, boost::mpl::long_<1> , nt2::container::domain>))
                              ((node_< A1, nt2::tag::ctranspose_, boost::mpl::long_<1> , nt2::container::domain>))
                              (scalar_< unspecified_<A2> >)
                              (scalar_< unspecified_<A3> >)
                              (unspecified_<A4>)
                              (unspecified_<A5>)
                            )
  {
    BOOST_DISPATCH_RETURNS_ARGS(6, (A0 const& a0, A1 const& a1, A2 const& a2, A3 const& a3, A4 const& a4, A5 const& a5), (A0 const& a0, A1 const& a1, A2 const& a2, A3 const& a3, A4 const&, A5 const&),
      mtimes(boost::proto::child_c<0>(a0), boost::proto::child_c<0>(a1), a2, a3, tag::blas_ctranspose_(), tag::blas_ctranspose_())
    )
  };
} }

namespace boost { namespace simd { namespace ext
{
  // Recognize alpha
  BOOST_DISPATCH_IMPLEMENT  ( multiplies_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< unspecified_<A0> >)
                              ((node_< A1, nt2::tag::mtimes_, boost::mpl::long_<6> , nt2::container::domain>))
                            )
  {
    BOOST_DISPATCH_RETURNS(2, (A0 const& a0, A1 const& a1),
      mtimes(boost::proto::child_c<0>(a1), boost::proto::child_c<1>(a1), boost::proto::child_c<2>(a1), a0 * boost::proto::value(boost::proto::child_c<3>(a1)), boost::proto::child_c<4>(a1), boost::proto::child_c<5>(a1))
    )
  };

  BOOST_DISPATCH_IMPLEMENT  ( multiplies_, tag::cpu_
                            , (A0)(A1)
                            , ((node_< A0, nt2::tag::mtimes_, boost::mpl::long_<6> , nt2::container::domain>))
                              (scalar_< unspecified_<A1> >)
                            )
  {
    BOOST_DISPATCH_RETURNS(2, (A0 const& a0, A1 const& a1),
      mtimes(boost::proto::child_c<0>(a0), boost::proto::child_c<1>(a0), boost::proto::child_c<2>(a0), a1 * boost::proto::value(boost::proto::child_c<3>(a0)), boost::proto::child_c<4>(a0), boost::proto::child_c<5>(a0))
    )
  };
} } }

namespace nt2 { namespace ext
{
  // run_assign
  BOOST_DISPATCH_IMPLEMENT  ( run_assign_, tag::cpu_
                            , (A0)(A1)
                            , ((ast_<A0, nt2::container::domain>))
                              ((node_< A1, nt2::tag::mtimes_, boost::mpl::long_<6> , nt2::container::domain>))
                            )
  {
    typedef A0& result_type;

    result_type operator()(A0& a0, A1& a1) const
    {
      using boost::fusion::at_c;
      typedef typename A1::value_type value_type;

      if(at_c<1>(boost::proto::child_c<0>(a1).extent()) == 0u)
      {
        a0 = zeros(a1.extent(), meta::as_<value_type>());
        return a0;
      }

      typedef nt2::memory::container<tag::table_, value_type, nt2::_2D> desired_semantic;
      NT2_AS_TERMINAL_IN(desired_semantic, child0, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_IN(desired_semantic, child1, boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT(desired_semantic, result, a0);

      value_type alpha = boost::proto::value(boost::proto::child_c<3>(a1));
      value_type beta = boost::proto::value(boost::proto::child_c<2>(a1));

      char transA = A1::proto_child4::proto_child0::call();
      char transB = A1::proto_child5::proto_child0::call();

      nt2_la_int m = transA == 'N' ? nt2_la_int(at_c<0>(child0.extent())) : nt2_la_int(at_c<1>(child0.extent()));
      nt2_la_int n = transB == 'N' ? nt2_la_int(at_c<1>(child1.extent())) : nt2_la_int(at_c<0>(child1.extent()));
      nt2_la_int k = transA == 'N' ? nt2_la_int(at_c<1>(child0.extent())) : nt2_la_int(at_c<0>(child0.extent()));
      nt2_la_int lda = at_c<0>(child0.extent());
      nt2_la_int ldb = at_c<0>(child1.extent());
      nt2_la_int ldc = at_c<0>(a1.extent());

      typedef typename container::as_terminal<desired_semantic>::type dummy_type;
      typedef typename container::as_view_impl<dummy_type>::type view_type;

      dummy_type dummy;
      view_type result_view;
      bool swap = (void*)&result != (void*)&a0;

      if( swap || ( container::alias(result, child0) || container::alias(result, child1) ) )
      {
        // overlapping of input and output data
        // so we provide dummy space and put it back in result later
        dummy.resize(a1.extent());
        result_view.reset(dummy);
        swap = true;
      }
      else
      {
        result.resize(a1.extent());
        result_view.reset(result);
      }

      nt2::details::
      gemm( &transA, &transB
          , &m, &n, &k
          , &alpha
          , child0.data(), &lda
          , child1.data(), &ldb
          , &beta
          , result_view.data(), &ldc
          );

      if(swap)
        container::assign_swap(a0, dummy);
      return a0;
    }
  };

} }

#endif


/*
 *  divide_by.h
 *  MTL4
 *
 *  Created by Hui Li (huil@Princeton.EDU)
 *
 */

#ifndef MTL_DIVIDE_BY_INCLUDE
#define MTL_DIVIDE_BY_INCLUDE

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/concept/std_concept.hpp>
#include <boost/numeric/mtl/matrix/map_view.hpp>
#include <boost/numeric/mtl/vector/map_view.hpp>
#include <boost/numeric/mtl/utility/algebraic_category.hpp>

namespace mtl { namespace tfunctor {

    // AlgebraicCategory is by default tag::scalar
    template <typename Value1, typename Value2, typename AlgebraicCategory>
    struct divide_by
    {
	typedef typename Multiplicable<Value1, Value2>::result_type result_type;

	explicit divide_by(const Value2& v2) : v2(v2) {}

	result_type operator() (const Value1& v1) const
	{
	    return v1 / v2;
	}
    private:
	Value2 v2;
    };

    template <typename Matrix, typename Value2>
    struct divide_by<Matrix, Value2, tag::matrix>
    {
	typedef mat::divide_by_view<Matrix,Value2> result_type;

	explicit divide_by(const Value2& v2) : v2(v2) {}

	result_type operator() (const Matrix& matrix) const
	{
	    return result_type(matrix, v2);
	}
    private:
	Value2 v2;
    };

    template <typename Vector, typename Value2>
    struct divide_by<Vector, Value2, tag::vector>
    {
	typedef vec::divide_by_view<Vector, Value2> result_type;

	explicit divide_by(const Value2& v2) : v2(v2) {}

	result_type operator() (const Vector& vector) const
	{
	    return result_type(vector, v2);
	}
    private:
	Value2 v2;
    };

} // namespace tfunctor

namespace mat {

    template <typename Value1, typename Value2>
    typename tfunctor::divide_by<Value1, Value2, typename traits::algebraic_category<Value1>::type>::result_type
    inline divide_by(const Value1& value1, const Value2& value2)
    {
	return tfunctor::divide_by<Value1, Value2, typename traits::algebraic_category<Value1>::type>(value2)(value1);
    }
}

namespace vec {

    template <typename Value1, typename Value2>
    typename tfunctor::divide_by<Value1, Value2, typename traits::algebraic_category<Value1>::type>::result_type
    inline divide_by(const Value1& value1, const Value2& value2)
    {
	return tfunctor::divide_by<Value1, Value2, typename traits::algebraic_category<Value1>::type>(value2)(value1);
    }
}

} // namespace mtl

#endif // MTL_DIVIDE_BY_INCLUDE


/**
BSD 3-Clause License

This file is part of the code accompanying the paper
Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction
by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution).

Copyright (c) 2021, Christiane Sommer and Lu Sang.
All rights reserved.

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.

* Neither the name of the copyright holder nor the names of its
  contributors may 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 COPYRIGHT HOLDER 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.
*/

#include "ColorUpsampler.h"
#include "mesh/HrLayeredMarchingCubes.h"

#include <fstream>
// #include <Eigen/Sparse>

using Vec8f = Vec8f;
using Arr8f = Eigen::Array<float, 8, 1>;

// ========== non-class functions ==========

//! from pose matrix to 6-DoF
Eigen::Matrix<float, 1, 7> MatrixTo7DoF(Mat4f& pose){
	Eigen::Matrix<float, 1, 7> xi;
	Vec3f t(pose.topRightCorner(3,1));
	Eigen::Quaternion<float> q(pose.topLeftCorner<3,3>());
	xi << t[0], t[1], t[2],  q.x(), q.y(), q.z(), q.w();
	return xi;
}

template<typename T>
static void printVec(std::vector<T> vec){
	for (const auto& i: vec)
  		std::cout << i << " ";
}

//! check for NaN values inside Eigen object
template<class S>
bool checkNan(S vector)
{
    return vector.array().isNaN().sum();
}

//! interpolation for RGB image
static Vec3f interpolateImage(const float m, const float n, const cv::Mat& img)
{
	int x = std::floor(m);
	int y = std::floor(n);
	cv::Vec3f tmp;
	if ((x+1) < img.rows && (y+1) < img.cols){
		tmp = (y+1.0-n)*(m-x)*img.at<cv::Vec3f>(x+1,y) + (y+1.0-n)*(x+1.0-m)*img.at<cv::Vec3f>(x,y) + (n-y)*(m-x)*img.at<cv::Vec3f>(x+1,y+1) + (n-y)*(x+1.0-m)*img.at<cv::Vec3f>(x,y+1);
	}
	else if ((y+1) < img.cols && x >= img.rows){
		tmp = (y+1.0-n)*img.at<cv::Vec3f>(x,y) + (n-y)*img.at<cv::Vec3f>(x,y+1);
	}
	else if ( y >= img.cols && (x+1) < img.rows){
		tmp = (m-x)*img.at<cv::Vec3f>(x+1,y) + (x+1.0-m)*img.at<cv::Vec3f>(x,y);
	}
	else{
		tmp = img.at<cv::Vec3f>(x,y);
	}

	Vec3f intensity(tmp[2],tmp[1],tmp[0]); // OpenCV stores image colors as BGR.
	return intensity;
}

//! corners of cube [-1, 1]^3
Eigen::Matrix<float, 3, 8> centeredCubeCorners()
{
	Eigen::Matrix<float, 3, 8> corners;

	corners.col(0) << -1.0, -1.0, -1.0;
	corners.col(1) << 1.0, -1.0, -1.0;
	corners.col(2) << -1.0, 1.0, -1.0;
	corners.col(3) << 1.0, 1.0, -1.0;
	corners.col(4) << -1.0, -1.0, 1.0;
	corners.col(5) << 1.0, -1.0, 1.0;
	corners.col(6) << -1.0, 1.0, 1.0;
	corners.col(7) << 1.0, 1.0, 1.0;

	return corners;
}

// ========== initialization ==========

//! constructor
ColorUpsampler::ColorUpsampler(const SdfLrMap& sdf_lr,
                            phmap::parallel_flat_hash_map<Vec3i, std::vector<bool>>& vis_map,
                            std::vector<std::shared_ptr<cv::Mat>>& images,
                            std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>& poses,
                            std::vector<int>& frame_idx,
                            const float voxel_size,
                            const Mat3f& K) :
    images_(images),
    poses_(poses),
    frame_idx_(frame_idx),
    voxel_size_(voxel_size),
    voxel_size_inv_(1.f / voxel_size),
    K_(K)
{
    num_frames_ = frame_idx_.size(); // does not work inside colon initializer
    init(sdf_lr, vis_map);
    // selectPoses();
}

//! init
void ColorUpsampler::init(const SdfLrMap& sdf_lr, phmap::parallel_flat_hash_map<Vec3i, std::vector<bool>>& vis_map)
{
	indices_.clear();
	vis_.clear();
    sdf_.clear();
	// convert low-res voxels to high-res voxels
    const float voxel_diameter = std::sqrt(3.) * voxel_size_;
	for (const auto& voxel: sdf_lr) {
        if (std::fabs(voxel.second.dist) < voxel_diameter) { // only accept voxels close to surface
            sdf_.emplace(voxel.first, SdfVoxelHr(voxel.second, voxel_size_));
        }
	}

	num_voxels_ = sdf_.size();
	indices_.resize(num_voxels_);
	vis_.resize(num_voxels_);

	// store indices and visibility vectors in correct order (that of high-res voxel map)
	int count = 0;
	for (const auto& voxel: sdf_) {
		indices_[count] = voxel.first;
		vis_[count] = vis_map[voxel.first];
		vis_[count].resize(frame_idx_.back()+1);
		++count;
    }
}

// ========== helper functions for Jacobians ==========

//! get I_i(R_iv+t) RGB/gray value for 8 subvoxels
bool ColorUpsampler::getIntensity(const int frame, const Vec3i& idx, const Mat3f& R, const Vec3f& t, Mat3x8f& intensity)
{
	const float fx = K_(0,0);
	const float fy = K_(1,1);
	const float cx = K_(0,2);
	const float cy = K_(1,2);

	SdfVoxelHr voxel(getSdf(idx));

	Mat3x8f point = R.transpose()*(getSubvoxelFloat(idx) - voxel.grad * voxel.d.transpose() - t.replicate(1,8));

    auto m = fx * point.row(0).array()/point.row(2).array() + cx;
    auto n = fy * point.row(1).array()/point.row(2).array() + cy;

	//DEBUG
	if (m.hasNaN()|| n.hasNaN()){
		std::cout << "invalid pixel coordinate at voxel "<< idx.transpose() << " at frame " << frame << std::endl;
		return false;
	}

    const cv::Mat& img = getFrame(frame);

	// Mat3x8f intensity;
	for (size_t i = 0; i < 8; i++){
        if (m(i)<0 || m(i)>=img.cols || n(i)<0 || n(i)>=img.rows) {

			return false;
		}
        else {
            intensity.col(i) = interpolateImage(n(i), m(i), img);
		}
	}

	return true;
}

//! coordinates of subvoxel centers
Mat3x8f ColorUpsampler::getSubvoxelFloat(const Vec3i& voxel_in)
{
	Vec3f voxel_float = voxel_in.cast<float>();
    return voxel_size_ * (.25 * centeredCubeCorners() + voxel_float.replicate<1,8>());
}

// ========== set values for individual voxels ==========

//! set albedo
void ColorUpsampler::setAlbedo(const Vec3i& idx, const Vec8f& r, const Vec8f& g, const Vec8f& b)
{
	Vec8f rr, gg, bb;
	rr = r;
	rr = rr.cwiseMax(0.0);
	rr = rr.cwiseMin(1.0);

	gg = g;
	gg = gg.cwiseMax(0.0);
	gg = gg.cwiseMin(1.0);

	bb = b;
	bb = bb.cwiseMax(0.0);
	bb = bb.cwiseMin(1.0);

	sdf_.at(idx).r = rr;
	sdf_.at(idx).g = gg;
	sdf_.at(idx).b = bb;

}

//! extract mesh from SDF to debug geometry
bool ColorUpsampler::extractMesh(std::string filename)
{
    HrLayeredMarchingCubes lmc(Vec3f(voxel_size_, voxel_size_, voxel_size_));
    lmc.computeIsoSurface(&sdf_);
    bool success = lmc.savePly(filename + ".ply");
	if (success)
		std::cout << "Mesh " << filename << ".ply successfully saved." << std::endl;

	return success;
}

bool ColorUpsampler::extractCloud(std::string filename)
{

	const float voxel_size_4 = .25 * voxel_size_;
	filename += ".ply";

	int voxel_id = 0;
    std::vector<Eigen::Matrix<float, 9, 1>> points_normals_colors;
    for (const auto& el : sdf_) {

		std::vector<bool> vis = vis_[voxel_id];
		bool visible = false;
		for (size_t i = 0; i < num_frames_; i++){
			if (vis[frame_idx_[i]]){
				visible = true;
				break;
			}
		}

		if(!visible){
			voxel_id++;
			continue;
		}

        const SdfVoxelHr& v = el.second;
        if (v.weight < 5){
			voxel_id++;
            continue;
		}

		Mat3x8f voxel_normal = -v.grad.normalized().replicate(1,8);
		Mat3x8f voxel_float = getSubvoxelFloat(el.first);
		Mat3x8f distances = voxel_normal * v.d.asDiagonal();
		for (int i=0; i<8; ++i)
		{
			Vec3f d = distances.col(i);
			if (std::fabs(d[0]) < voxel_size_4 && std::fabs(d[1]) < voxel_size_4 && std::fabs(d[2]) < voxel_size_4 && !std::isnan(v.r[i]) && !std::isnan(v.g[i]) && !std::isnan(v.b[i]))
			{
				Eigen::Matrix<float, 9, 1> pnc;
				pnc.segment<3>(0) = voxel_float.col(i) + d;
				pnc.segment<3>(3) = voxel_normal.col(i);
				pnc.segment<3>(6) = Vec3f(v.r[i], v.g[i], v.b[i]);
				points_normals_colors.push_back(pnc);
			}
		}
		voxel_id++;
    }

    std::ofstream plyFile;
    plyFile.open(filename.c_str());
    if (!plyFile.is_open())
        return false;

    plyFile << "ply" << std::endl;
    plyFile << "format ascii 1.0" << std::endl;
    plyFile << "element vertex " << points_normals_colors.size() << std::endl;
    plyFile << "property float x" << std::endl;
    plyFile << "property float y" << std::endl;
    plyFile << "property float z" << std::endl;
    plyFile << "property float nx" << std::endl;
    plyFile << "property float ny" << std::endl;
    plyFile << "property float nz" << std::endl;
    plyFile << "property uchar red" << std::endl;
    plyFile << "property uchar green" << std::endl;
    plyFile << "property uchar blue" << std::endl;
    plyFile << "end_header" << std::endl;

    for (const Eigen::Matrix<float, 9, 1>& p : points_normals_colors)
	{
        plyFile << p[0] << " " << p[1] << " " << p[2] << " " << p[3] << " " << p[4] << " " << p[5] << " " << int(255 * p[6]) << " " << int(255 * p[7]) << " " << int(255 * p[8]) << std::endl;
    }

    plyFile.close();

	std::cout << "Cloud " << filename << " successfully saved." << std::endl;
    return true;
}

 // ------------------- solvers -----------------------------------------------------------------------------------------------------------------

//! compute albedo from ambient light assumption, i.e. average over all observations
void ColorUpsampler::computeColor()
{
	// size_t num_tot_frames = getVis(0).size();

	size_t count = 0; // counter (same for all three color channels)
    Vec8f br, bb, bg;
    int voxel_id = 0;

	for (const auto& idx : indices_) {

		br.setZero();
		bb.setZero();
		bg.setZero();

		std::vector<bool> vis = getVis(voxel_id);
		SdfVoxelHr voxel(getSdf(idx));

		for(size_t i = 0; i < num_frames_; i++){
			Mat3f R = getRotation(i);
			Vec3f t = getTranslation(i);

            if(vis[frame_idx_[i]]) {
				// r(v) = \sum_i (I(pi(Rv+t)) - pho(v)(<n(v),Rl>))
				Mat3x8f intensity;
				if(!getIntensity(i, idx, R, t, intensity)){
					continue;
				}
				br += intensity.row(0);
				bg += intensity.row(1);
				bb += intensity.row(2);
				++count;
			}
		}

		Vec8f r,g,b;
		float inv_count = 1.f / float(count);
		r = inv_count * br;
		g = inv_count * bg;
		b = inv_count * bb;
		setAlbedo(idx, r, g, b); // cut-off happens here
		count = 0;
		voxel_id++;
	}
}


#include <cassert>
#include <cctype>
#include <cerrno>
#include <cfloat>
#include <ciso646>
#include <climits>
#include <clocale>
#include <cmath>
#include <csetjmp>
#include <csignal>
#include <cstdarg>
#include <cstddef>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <ctime>
#include <cwchar>
#include <cwctype>
#include <ccomplex>
#include <cfenv>
#include <cinttypes>
#include <cstdbool>
#include <cstdint>
#include <ctgmath>
#include <algorithm>
#include <bitset>
#include <complex>
#include <deque>
#include <exception>
#include <fstream>
#include <functional>
#include <iomanip>
#include <ios>
#include <iosfwd>
#include <iostream>
#include <istream>
#include <iterator>
#include <limits>
#include <list>
#include <locale>
#include <map>
#include <memory>
#include <new>
#include <numeric>
#include <ostream>
#include <queue>
#include <set>
#include <sstream>
#include <stack>
#include <stdexcept>
#include <streambuf>
#include <string>
#include <typeinfo>
#include <utility>
#include <valarray>
#include <vector>
#include <array>
#include <atomic>
#include <chrono>
#include <codecvt>
#include <condition_variable>
#include <forward_list>
#include <future>
#include <initializer_list>
#include <mutex>
#include <random>
#include <ratio>
#include <regex>
#include <scoped_allocator>
#include <system_error>
#include <thread>
#include <tuple>
#include <typeindex>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <shared_mutex>
#include <boost/format.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/optional.hpp>

#ifdef DEBUG
#include "libs/debug.cpp"
#define DEBUG 1
#else
#define DEBUG 0
#endif

#define rep(i, n) repr((i), 0, (n))
#define repr(i, a, t) reps((i), (a), (t), 1)
#define reps(i, a, t, s) for (long long (i) = (a); (i) < (long long)(t); (i) += (s))
#define rrep(i, n) rrepr((i), (n), 0)
#define rrepr(i, a, t) rreps((i), (a), (t), 1)
#define rreps(i, a, t, s) for (long long (i) = (a) - 1; (i) >= (long long)(t); (i) -= (s))
#define each(v, c) for (auto &&(v) : (c))
#define all(c) (c).begin(), (c).end()

using namespace std;

// コンテナのメンバ関数を使わないと O(N)
template<typename T>
typename set<T>::iterator lower_bound(typename set<T>::iterator l, typename set<T>::iterator r, const T &v);
template<typename T>
typename set<T>::iterator upper_bound(typename set<T>::iterator l, typename set<T>::iterator r, const T &v);
template<typename S, typename T>
typename map<S, T>::iterator lower_bound(typename map<S, T>::iterator l, typename map<S, T>::iterator r, const T &v);
template<typename S, typename T>
typename map<S, T>::iterator upper_bound(typename map<S, T>::iterator l, typename map<S, T>::iterator r, const T &v);

namespace mp = boost::multiprecision;
using ll = long long;
using ull = unsigned long long;
using ld = long double;
using ml = mp::cpp_int;
template<typename T>
using optional = boost::optional<T>;
const auto none = boost::none;
const long long MOD = 1e9 + 7;
const long long INF = 1e18;

int main() {
  cout << fixed << setprecision(10);
  cin.tie(nullptr);
  ios::sync_with_stdio(false);
  ifstream ifs;
  if (DEBUG) {
    ifs.open("src/_in.txt");
    cin.rdbuf(ifs.rdbuf());
  }

}


#ifndef DART_CONSTRAINT_LCPUTILS_HPP_
#define DART_CONSTRAINT_LCPUTILS_HPP_

#include <memory>

#include <Eigen/Dense>

#include "dart/constraint/BoxedLcpSolver.hpp"

namespace dart {
namespace constraint {

class LCPUtils
{
public:
  static bool isLCPSolutionValid(
      const Eigen::MatrixXs& mA,
      const Eigen::VectorXs& mX,
      const Eigen::VectorXs& mB,
      const Eigen::VectorXs& mHi,
      const Eigen::VectorXs& mLo,
      const Eigen::VectorXi& mFIndex,
      bool ignoreFrictionIndices);

  /// This applies a simple algorithm to guess the solution to the LCP problem.
  /// It's not guaranteed to be correct, but it often can be if there is no
  /// sliding friction on this timestep.
  static Eigen::VectorXs guessSolution(
      const Eigen::MatrixXs& mA,
      const Eigen::VectorXs& mB,
      const Eigen::VectorXs& mHi,
      const Eigen::VectorXs& mLo,
      const Eigen::VectorXi& mFIndex);

  /// This reduces an LCP problem by merging any near-identical contact points.
  /// It returns a mapOut matrix, such that if you solve this LCP and then
  /// multiply the resulting x as mapOut*x, you'll get the solution to the
  /// original LCP.
  static Eigen::MatrixXs reduce(
      Eigen::MatrixXs& A,
      Eigen::VectorXs& X,
      Eigen::VectorXs& b,
      Eigen::VectorXs& hi,
      Eigen::VectorXs& lo,
      Eigen::VectorXi& fIndex);

  /// This cuts a problem down to just the normal forces, ignoring friction.
  /// It returns a mapOut matrix, such that if you solve this LCP and then
  /// multiply the resulting x as mapOut*x, you'll get the solution to the
  /// original LCP, but with friction forces all 0.
  static Eigen::MatrixXs removeFriction(
      Eigen::MatrixXs& A,
      Eigen::VectorXs& X,
      Eigen::VectorXs& b,
      Eigen::VectorXs& hi,
      Eigen::VectorXs& lo,
      Eigen::VectorXi& fIndex);

  /// This solves the LCP problem by first automatically de-duplicating columns
  /// to create a reduced version of an equivalent problem, ideally with a
  /// full-rank A. Then the solution to the original LCP is recovered by
  /// re-inflating the solution to the reduced problem.
  static bool solveDeduplicated(
      std::shared_ptr<BoxedLcpSolver>& solver,
      const Eigen::MatrixXs& A,
      Eigen::VectorXs& X,
      const Eigen::VectorXs& b,
      const Eigen::VectorXs& hi,
      const Eigen::VectorXs& lo,
      const Eigen::VectorXi& fIndex);

  /// This will modify the LCP problem formulation to merge two columns
  /// together, and rewrite and resize all the matrices appropriately. It will
  /// also yell and scream (throw asserts) if the columns shouldn't be merged.
  /// It'll also update the mapOut matrix, so that it's possible to simply
  /// multiply mapOut*x on the reduced problem to get a valid solution to the
  /// larger problem.
  static void mergeLCPColumns(
      int colA,
      int colB,
      Eigen::MatrixXs& A,
      Eigen::VectorXs& X,
      Eigen::VectorXs& b,
      Eigen::VectorXs& hi,
      Eigen::VectorXs& lo,
      Eigen::VectorXi& fIndex,
      Eigen::MatrixXs& mapOut);

  /// This will modify the LCP problem formulation to drop a column
  /// and rewrite and resize all the matrices appropriately.
  /// It'll also update the mapOut matrix, so that it's possible to simply
  /// multiply mapOut*x on the reduced problem to get a valid solution to the
  /// larger problem.
  static void dropLCPColumn(
      int col,
      Eigen::MatrixXs& A,
      Eigen::VectorXs& X,
      Eigen::VectorXs& b,
      Eigen::VectorXs& hi,
      Eigen::VectorXs& lo,
      Eigen::VectorXi& fIndex,
      Eigen::MatrixXs& mapOut);

  /// Print replication code info
  static void printReplicationCode(
      Eigen::MatrixXs A,
      Eigen::VectorXs x,
      Eigen::VectorXs lo,
      Eigen::VectorXs hi,
      Eigen::VectorXs b,
      Eigen::VectorXi fIndex);
};

} // namespace constraint
} // namespace dart

#endif


#ifndef AMGCL_BACKEND_VEXCL_HPP
#define AMGCL_BACKEND_VEXCL_HPP

/*
The MIT License

Copyright (c) 2012-2020 Denis Demidov <dennis.demidov@gmail.com>

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

/**
 * \file   amgcl/backend/vexcl.hpp
 * \author Denis Demidov <dennis.demidov@gmail.com>
 * \brief  VexCL backend.
 */

#include <iostream>
#include <memory>

#include <boost/range/iterator_range.hpp>

#include <amgcl/solver/skyline_lu.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/gather.hpp>
#include <vexcl/sparse/matrix.hpp>
#include <vexcl/sparse/distributed.hpp>

#include <amgcl/util.hpp>
#include <amgcl/backend/builtin.hpp>

namespace amgcl {

namespace solver {

/** Wrapper around solver::skyline_lu for use with the VexCL backend.
 * Copies the rhs to the host memory, solves the problem using the host CPU,
 * then copies the solution back to the compute device(s).
 */
template <class value_type>
struct vexcl_skyline_lu : solver::skyline_lu<value_type> {
    typedef solver::skyline_lu<value_type> Base;
    typedef typename math::rhs_of<value_type>::type rhs_type;

    mutable std::vector<rhs_type> _rhs, _x;

    template <class Matrix, class Params>
    vexcl_skyline_lu(const Matrix &A, const Params&)
        : Base(*A), _rhs(backend::rows(*A)), _x(backend::rows(*A))
    { }

    template <class Vec1, class Vec2>
    void operator()(const Vec1 &rhs, Vec2 &x) const {
        vex::copy(rhs, _rhs);
        static_cast<const Base*>(this)->operator()(_rhs, _x);
        vex::copy(_x, x);
    }

    size_t bytes() const {
        return
            backend::bytes(*static_cast<const Base*>(this)) +
            backend::bytes(_rhs) +
            backend::bytes(_x);
    }
};

}

namespace backend {

/// The VexCL backend parameters.
struct vexcl_params {

    std::vector< vex::backend::command_queue > q; ///< Command queues that identify compute devices to use with VexCL.

    /// Do CSR to ELL conversion on the GPU side.
    /** This will result in faster setup, but will require more GPU memory. */
    bool fast_matrix_setup;

    vexcl_params() : fast_matrix_setup(true) {}

#ifndef AMGCL_NO_BOOST
    vexcl_params(const boost::property_tree::ptree &p)
        : fast_matrix_setup(p.get("fast_matrix_setup", vexcl_params().fast_matrix_setup))
    {
        std::vector<vex::backend::command_queue> *ptr = 0;
        ptr = p.get("q", ptr);
        if (ptr) q = *ptr;
        check_params(p, {"q", "fast_matrix_setup"});
    }

    void get(boost::property_tree::ptree &p, const std::string &path) const {
        p.put(path + "q", &q);
        p.put(path + "fast_matrix_setup", fast_matrix_setup);
    }
#endif

    const std::vector<vex::backend::command_queue>& context() const {
        if (q.empty())
            return vex::current_context().queue();
        else
            return q;
    }
};

/**
 * The backend uses the VexCL
 * library for accelerating solution on the modern GPUs and multicore
 * processors with the help of OpenCL or CUDA technologies.
 * The VexCL backend stores the system matrix as ``vex::SpMat<real>`` and
 * expects the right hand side and the solution vectors to be instances of the
 * ``vex::vector<real>`` type.
 */
template <typename real, class DirectSolver = solver::vexcl_skyline_lu<real> >
struct vexcl {
    typedef real      value_type;
    typedef ptrdiff_t index_type;

    typedef vex::sparse::distributed<
                vex::sparse::matrix<value_type, index_type, index_type>
                > matrix;
    typedef typename math::rhs_of<value_type>::type rhs_type;
    typedef vex::vector<rhs_type>                          vector;
    typedef vex::vector<value_type>                        matrix_diagonal;
    typedef DirectSolver                                   direct_solver;

    struct provides_row_iterator : std::false_type {};

    typedef vexcl_params params;

    static std::string name() { return "vexcl"; }

    // Copy matrix from builtin backend.
    static std::shared_ptr<matrix>
    copy_matrix(std::shared_ptr< typename builtin<real>::matrix > A, const params &prm)
    {
        precondition(!prm.context().empty(), "Empty VexCL context!");

        const typename builtin<real>::matrix &a = *A;

        const size_t n   = rows(*A);
        const size_t m   = cols(*A);
        const size_t nnz = a.ptr[n];

        return std::make_shared<matrix>(prm.context(), n, m,
                boost::make_iterator_range(a.ptr, a.ptr + n+1),
                boost::make_iterator_range(a.col, a.col + nnz),
                boost::make_iterator_range(a.val, a.val + nnz),
                prm.fast_matrix_setup
                );
    }

    // Copy vector from builtin backend.
    template <class T>
    static std::shared_ptr< vex::vector<T> >
    copy_vector(const std::vector<T> &x, const params &prm)
    {
        precondition(!prm.context().empty(), "Empty VexCL context!");
        return std::make_shared< vex::vector<T> >(prm.context(), x);
    }

    template <class T>
    static std::shared_ptr< vex::vector<T> >
    copy_vector(const numa_vector<T> &x, const params &prm)
    {
        precondition(!prm.context().empty(), "Empty VexCL context!");
        return std::make_shared< vex::vector<T> >(prm.context(), x.size(), x.data());
    }

    // Copy vector from builtin backend.
    template <class T>
    static std::shared_ptr< vex::vector<T> >
    copy_vector(std::shared_ptr< numa_vector<T> > x, const params &prm)
    {
        return copy_vector(*x, prm);
    }

    // Create vector of the specified size.
    static std::shared_ptr<vector>
    create_vector(size_t size, const params &prm)
    {
        precondition(!prm.context().empty(), "Empty VexCL context!");

        return std::make_shared<vector>(prm.context(), size);
    }

    struct gather {
        size_t n;
        mutable vex::gather G;
        mutable std::vector<char> buf;

        gather(size_t src_size, const std::vector<ptrdiff_t> &I, const params &prm)
            : n(I.size()), G(prm.context(), src_size, std::vector<size_t>(I.begin(), I.end()))
        { }

        template <class S, class D>
        void operator()(const vex::vector<S> &src, vex::vector<D> &dst) const {
            if (buf.size() < sizeof(D) * n) buf.resize(sizeof(D) * n);
            auto t = reinterpret_cast<D*>(buf.data());
            G(src, t);
            vex::copy(t, t + n, dst.begin());
        }

        template <class S, class D>
        void operator()(const vex::vector<S> &vec, std::vector<D> &vals) const {
            G(vec, vals);
        }
    };

    struct scatter {
        size_t n;
        mutable vex::scatter S;
        mutable std::vector<char> buf;

        scatter(size_t size, const std::vector<ptrdiff_t> &I, const params &prm)
            : n(I.size()), S(prm.context(), size, std::vector<size_t>(I.begin(), I.end()))
        { }

        template <class S, class D>
        void operator()(const vex::vector<S> &src, vex::vector<D> &dst) const {
            if (buf.size() < sizeof(D) * n) buf.resize(sizeof(D) * n);
            auto t = reinterpret_cast<D*>(buf.data());
            vex::copy(src.begin(), src.end(), t);
            S(t, dst);
        }
    };

    // Create direct solver for coarse level
    static std::shared_ptr<direct_solver>
    create_solver(std::shared_ptr< typename builtin<real>::matrix > A, const params &prm)
    {
        return std::make_shared<direct_solver>(A, prm);
    }
};

//---------------------------------------------------------------------------
// Backend interface implementation
//---------------------------------------------------------------------------
template <typename T1, typename T2>
struct backends_compatible< vexcl<T1>, vexcl<T2> > : std::true_type {};

template < typename V, typename C, typename P >
struct bytes_impl< vex::sparse::distributed<vex::sparse::matrix<V,C,P> > > {
    static size_t get(const vex::sparse::distributed<vex::sparse::matrix<V,C,P> > &A) {
        return
            sizeof(P) * (A.rows() + 1) +
            sizeof(C) * A.nonzeros() +
            sizeof(V) * A.nonzeros();
    }
};

template < typename V >
struct bytes_impl< vex::vector<V> > {
    static size_t get(const vex::vector<V> &v) {
        return v.size() * sizeof(V);
    }
};

template < typename Alpha, typename Beta, typename Va, typename Vx, typename Vy, typename C, typename P >
struct spmv_impl<
    Alpha, vex::sparse::distributed<vex::sparse::matrix<Va,C,P>>, vex::vector<Vx>,
    Beta,  vex::vector<Vy>,
    typename std::enable_if<
        math::static_rows<Va>::value == 1 &&
        math::static_rows<Vx>::value == 1 &&
        math::static_rows<Vy>::value == 1
        >::type
    >
{
    typedef vex::sparse::distributed<vex::sparse::matrix<Va,C,P>> matrix;

    static void apply(Alpha alpha, const matrix &A, const vex::vector<Vx> &x,
            Beta beta, vex::vector<Vy> &y)
    {
        if (beta)
            y = alpha * (A * x) + beta * y;
        else
            y = alpha * (A * x);
    }
};

template < typename Va, typename Vf, typename Vx, typename Vr, typename C, typename P >
struct residual_impl<
    vex::sparse::distributed<vex::sparse::matrix<Va,C,P>>,
    vex::vector<Vf>,
    vex::vector<Vx>,
    vex::vector<Vr>
    >
{
    typedef vex::sparse::distributed<vex::sparse::matrix<Va,C,P>> matrix;

    static void apply(const vex::vector<Vf> &rhs, const matrix &A, const vex::vector<Vx> &x,
            vex::vector<Vr> &r)
    {
        r = rhs - A * x;
    }
};

template < typename V >
struct clear_impl< vex::vector<V> >
{
    static void apply(vex::vector<V> &x)
    {
        x = 0;
    }
};

template < class V, class T >
struct copy_impl<V, vex::vector<T> >
{
    static void apply(const V &x, vex::vector<T> &y)
    {
        vex::copy(x, y);
    }
};

template < class T, class V >
struct copy_impl<vex::vector<T>, V>
{
    static void apply(const vex::vector<T> &x, V &y)
    {
        vex::copy(x, y);
    }
};

template < class T1, class T2 >
struct copy_impl<vex::vector<T1>, vex::vector<T2>>
{
    static void apply(const vex::vector<T1> &x, vex::vector<T2> &y)
    {
        vex::copy(x, y);
    }
};

template < typename V >
struct inner_product_impl<
    vex::vector<V>,
    vex::vector<V>
    >
{
    static V get(const vex::vector<V> &x, const vex::vector<V> &y)
    {
        vex::Reductor<V, vex::SUM_Kahan> sum( x.queue_list() );
        return sum(x * y);
    }
};

template < typename A, typename B, typename V1, typename V2 >
struct axpby_impl<
    A, vex::vector<V1>,
    B, vex::vector<V2>
    > {
    static void apply(A a, const vex::vector<V1> &x, B b, vex::vector<V2> &y)
    {
        if (b)
            y = a * x + b * y;
        else
            y = a * x;
    }
};

template < typename A, typename B, typename C, typename V1, typename V2, typename V3 >
struct axpbypcz_impl<
    A, vex::vector<V1>,
    B, vex::vector<V2>,
    C, vex::vector<V3>
    >
{
    static void apply(
            A a, const vex::vector<V1> &x,
            B b, const vex::vector<V2> &y,
            C c,       vex::vector<V3> &z
            )
    {
        if (c)
            z = a * x + b * y + c * z;
        else
            z = a * x + b * y;
    }
};

template < typename A, typename B, typename Vx, typename Vy, typename Vz >
struct vmul_impl<
    A, vex::vector<Vx>, vex::vector<Vy>,
    B, vex::vector<Vz>
    >
{
    static void apply(A a, const vex::vector<Vx> &x, const vex::vector<Vy> &y,
            B b, vex::vector<Vz> &z)
    {
        if (b)
            z = a * x * y + b * z;
        else
            z = a * x * y;
    }
};

template <class T, class V>
struct reinterpret_impl<T, vex::vector<V>>
{
    typedef vex::vector<typename std::decay<T>::type> return_type;

    static return_type get(const vex::vector<V> &x) {
        return x.template reinterpret<typename std::decay<T>::type>();
    }
};

} // namespace backend
} // namespace amgcl

#endif


/* Copyright (c) 2014, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#include <iostream>
#include <string>

// Utilities and system includes
//#include <helper_functions.h>
#include <boost/program_options.hpp>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/contrib/contrib.hpp>

#include <Eigen/Dense>
#include <cudaPcl/openniSmoothDepth.hpp>
#include <cudaPcl/openniSmoothDepthGpu.hpp>
#include <cudaPcl/openniSmoothNormalsGpu.hpp>

namespace po = boost::program_options;
using namespace Eigen;
using std::cout;
using std::endl;

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

  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "produce help message")
    ("f_d,f", po::value<double>(), "focal length of depth camera")
    ("eps,e", po::value<double>(), "sqrt of the epsilon parameter of the guided filter")
    ("B,b", po::value<int>(), "guided filter windows size (size will be (2B+1)x(2B+1))")
    ("compress,c", "compress the computed normals")
//    ("out,o", po::value<std::string>(), "output path where surfae normal images are saved to")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

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

  double f_d = 540.;
  double eps = 0.2*0.2;
  int32_t B = 10;
  bool compress = false;
  if(vm.count("f_d")) f_d = vm["f_d"].as<double>();
  if(vm.count("eps")) eps = vm["eps"].as<double>();
  if(vm.count("B")) B = vm["B"].as<int>();
  if(vm.count("compress")) compress = true;
//  std::string outPath = ""
//  if(vm.count("out")) outPath = vm["out"].as<std::string>();

  findCudaDevice(argc,(const char**)argv);
  cudaPcl::OpenniSmoothNormalsGpu v(f_d, eps, B, compress);

  if(true)
  { // load a specific image and process
    cv::Mat depth = cv::imread("../../python/table_0_d.png",CV_LOAD_IMAGE_ANYDEPTH);
    for(uint32_t t=0; t<1; ++t)
    {
      v.depth_cb((uint16_t*)depth.data,depth.cols,depth.rows);
      v.visualizeD();
      v.visualizePC();
      cv::waitKey(0);
    }
  }

  // run the grabber
  v.run ();

  cout<<cudaDeviceReset()<<endl;
  return (0);
}


/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2011, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  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.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may 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
 *  COPYRIGHT OWNER 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.
 *
 */

#include <iostream>
#include <algorithm>

#include <pcl/common/time.h>
#include <pcl/gpu/kinfu/kinfu.h>
#include "internal.h"

#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/Geometry>
#include <Eigen/LU>

#ifdef HAVE_OPENCV
  #include <opencv2/opencv.hpp>
  #include <opencv2/gpu/gpu.hpp>
  #include <pcl/gpu/utils/timers_opencv.hpp>
#endif

using namespace std;
using namespace pcl::device;
using namespace pcl::gpu;

using Eigen::AngleAxisf;
using Eigen::Array3f;
using Eigen::Vector3i;
using Eigen::Vector3f;

namespace pcl
{
  namespace gpu
  {
    Eigen::Vector3f rodrigues2(const Eigen::Matrix3f& matrix);
  }
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
pcl::gpu::KinfuTracker::KinfuTracker (int rows, int cols) : rows_(rows), cols_(cols), global_time_(0), max_icp_distance_(0), integration_metric_threshold_(0.f)
{
  const Vector3f volume_size = Vector3f::Constant (VOLUME_SIZE);
  const Vector3i volume_resolution(VOLUME_X, VOLUME_Y, VOLUME_Z);

  tsdf_volume_ = TsdfVolume::Ptr( new TsdfVolume(volume_resolution) );
  tsdf_volume_->setSize(volume_size);

  setDepthIntrinsics (525.f, 525.f); // default values, can be overwritten

  init_Rcam_ = Eigen::Matrix3f::Identity ();// * AngleAxisf(-30.f/180*3.1415926, Vector3f::UnitX());
  init_tcam_ = volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);

  const int iters[] = {10, 5, 4};
  std::copy (iters, iters + LEVELS, icp_iterations_);

  const float default_distThres = 0.10f; //meters
  const float default_angleThres = sin (20.f * 3.14159254f / 180.f);
  const float default_tranc_dist = 0.03f; //meters

  setIcpCorespFilteringParams (default_distThres, default_angleThres);
  tsdf_volume_->setTsdfTruncDist (default_tranc_dist);

  allocateBufffers (rows, cols);

  rmats_.reserve (30000);
  tvecs_.reserve (30000);

  reset ();
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::gpu::KinfuTracker::setDepthIntrinsics (float fx, float fy, float cx, float cy)
{
  fx_ = fx;
  fy_ = fy;
  cx_ = (cx == -1) ? cols_/2-0.5f : cx;
  cy_ = (cy == -1) ? rows_/2-0.5f : cy;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::gpu::KinfuTracker::setInitalCameraPose (const Eigen::Affine3f& pose)
{
  init_Rcam_ = pose.rotation ();
  init_tcam_ = pose.translation ();
  reset ();
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::gpu::KinfuTracker::setDepthTruncationForICP (float max_icp_distance)
{
  max_icp_distance_ = max_icp_distance;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::gpu::KinfuTracker::setCameraMovementThreshold(float threshold)
{
  integration_metric_threshold_ = threshold;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::gpu::KinfuTracker::setIcpCorespFilteringParams (float distThreshold, float sineOfAngle)
{
  distThres_  = distThreshold; //mm
  angleThres_ = sineOfAngle;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int
pcl::gpu::KinfuTracker::cols ()
{
  return (cols_);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int
pcl::gpu::KinfuTracker::rows ()
{
  return (rows_);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::gpu::KinfuTracker::reset()
{
  if (global_time_)
    cout << "Reset" << endl;

  global_time_ = 0;
  rmats_.clear ();
  tvecs_.clear ();

  rmats_.push_back (init_Rcam_);
  tvecs_.push_back (init_tcam_);

  tsdf_volume_->reset();

  if (color_volume_) // color integration mode is enabled
    color_volume_->reset();
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::gpu::KinfuTracker::allocateBufffers (int rows, int cols)
{
  depths_curr_.resize (LEVELS);
  vmaps_g_curr_.resize (LEVELS);
  nmaps_g_curr_.resize (LEVELS);

  vmaps_g_prev_.resize (LEVELS);
  nmaps_g_prev_.resize (LEVELS);

  vmaps_curr_.resize (LEVELS);
  nmaps_curr_.resize (LEVELS);

  coresps_.resize (LEVELS);

  for (int i = 0; i < LEVELS; ++i)
  {
    int pyr_rows = rows >> i;
    int pyr_cols = cols >> i;

    depths_curr_[i].create (pyr_rows, pyr_cols);

    vmaps_g_curr_[i].create (pyr_rows*3, pyr_cols);
    nmaps_g_curr_[i].create (pyr_rows*3, pyr_cols);

    vmaps_g_prev_[i].create (pyr_rows*3, pyr_cols);
    nmaps_g_prev_[i].create (pyr_rows*3, pyr_cols);

    vmaps_curr_[i].create (pyr_rows*3, pyr_cols);
    nmaps_curr_[i].create (pyr_rows*3, pyr_cols);

    coresps_[i].create (pyr_rows, pyr_cols);
  }
  depthRawScaled_.create (rows, cols);
  // see estimate tranform for the magic numbers
  gbuf_.create (27, 20*60);
  sumbuf_.create (27);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl::gpu::KinfuTracker::operator() (const DepthMap& depth_raw)
{
  device::Intr intr (fx_, fy_, cx_, cy_);
  {
    //ScopeTime time(">>> Bilateral, pyr-down-all, create-maps-all");
    //depth_raw.copyTo(depths_curr[0]);
    device::bilateralFilter (depth_raw, depths_curr_[0]);

	if (max_icp_distance_ > 0)
		device::truncateDepth(depths_curr_[0], max_icp_distance_);

    for (int i = 1; i < LEVELS; ++i)
      device::pyrDown (depths_curr_[i-1], depths_curr_[i]);

    for (int i = 0; i < LEVELS; ++i)
    {
      device::createVMap (intr(i), depths_curr_[i], vmaps_curr_[i]);
      //device::createNMap(vmaps_curr_[i], nmaps_curr_[i]);
      computeNormalsEigen (vmaps_curr_[i], nmaps_curr_[i]);
    }
    pcl::device::sync ();
  }

  //can't perform more on first frame
  if (global_time_ == 0)
  {
    Matrix3frm init_Rcam = rmats_[0]; //  [Ri|ti] - pos of camera, i.e.
    Vector3f   init_tcam = tvecs_[0]; //  transform from camera to global coo space for (i-1)th camera pose

    Mat33&  device_Rcam = device_cast<Mat33> (init_Rcam);
    float3& device_tcam = device_cast<float3>(init_tcam);

    Matrix3frm init_Rcam_inv = init_Rcam.inverse ();
    Mat33&   device_Rcam_inv = device_cast<Mat33> (init_Rcam_inv);
    float3 device_volume_size = device_cast<const float3>(tsdf_volume_->getSize());

    //integrateTsdfVolume(depth_raw, intr, device_volume_size, device_Rcam_inv, device_tcam, tranc_dist, volume_);
    device::integrateTsdfVolume(depth_raw, intr, device_volume_size, device_Rcam_inv, device_tcam, tsdf_volume_->getTsdfTruncDist(), tsdf_volume_->data(), depthRawScaled_);

    for (int i = 0; i < LEVELS; ++i)
      device::tranformMaps (vmaps_curr_[i], nmaps_curr_[i], device_Rcam, device_tcam, vmaps_g_prev_[i], nmaps_g_prev_[i]);

    ++global_time_;
    return (false);
  }

  ///////////////////////////////////////////////////////////////////////////////////////////
  // Iterative Closest Point
  Matrix3frm Rprev = rmats_[global_time_ - 1]; //  [Ri|ti] - pos of camera, i.e.
  Vector3f   tprev = tvecs_[global_time_ - 1]; //  tranfrom from camera to global coo space for (i-1)th camera pose
  Matrix3frm Rprev_inv = Rprev.inverse (); //Rprev.t();

  //Mat33&  device_Rprev     = device_cast<Mat33> (Rprev);
  Mat33&  device_Rprev_inv = device_cast<Mat33> (Rprev_inv);
  float3& device_tprev     = device_cast<float3> (tprev);

  Matrix3frm Rcurr = Rprev; // tranform to global coo for ith camera pose
  Vector3f   tcurr = tprev;
  {
    //ScopeTime time("icp-all");
    for (int level_index = LEVELS-1; level_index>=0; --level_index)
    {
      int iter_num = icp_iterations_[level_index];

      MapArr& vmap_curr = vmaps_curr_[level_index];
      MapArr& nmap_curr = nmaps_curr_[level_index];

      //MapArr& vmap_g_curr = vmaps_g_curr_[level_index];
      //MapArr& nmap_g_curr = nmaps_g_curr_[level_index];

      MapArr& vmap_g_prev = vmaps_g_prev_[level_index];
      MapArr& nmap_g_prev = nmaps_g_prev_[level_index];

      //CorespMap& coresp = coresps_[level_index];

      for (int iter = 0; iter < iter_num; ++iter)
      {
        Mat33&  device_Rcurr = device_cast<Mat33> (Rcurr);
        float3& device_tcurr = device_cast<float3>(tcurr);

        Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
        Eigen::Matrix<double, 6, 1> b;
#if 0
        device::tranformMaps(vmap_curr, nmap_curr, device_Rcurr, device_tcurr, vmap_g_curr, nmap_g_curr);
        findCoresp(vmap_g_curr, nmap_g_curr, device_Rprev_inv, device_tprev, intr(level_index), vmap_g_prev, nmap_g_prev, distThres_, angleThres_, coresp);
        device::estimateTransform(vmap_g_prev, nmap_g_prev, vmap_g_curr, coresp, gbuf_, sumbuf_, A.data(), b.data());

        //cv::gpu::GpuMat ma(coresp.rows(), coresp.cols(), CV_32S, coresp.ptr(), coresp.step());
        //cv::Mat cpu;
        //ma.download(cpu);
        //cv::imshow(names[level_index] + string(" --- coresp white == -1"), cpu == -1);
#else
        estimateCombined (device_Rcurr, device_tcurr, vmap_curr, nmap_curr, device_Rprev_inv, device_tprev, intr (level_index),
                          vmap_g_prev, nmap_g_prev, distThres_, angleThres_, gbuf_, sumbuf_, A.data (), b.data ());
#endif
        //checking nullspace
        double det = A.determinant ();

		if (fabs (det) < 1e-15 || pcl_isnan (det))
        {
          if (pcl_isnan (det)) cout << "qnan" << endl;

          reset ();
          return (false);
        }
        //float maxc = A.maxCoeff();

        Eigen::Matrix<float, 6, 1> result = A.llt ().solve (b).cast<float>();
        //Eigen::Matrix<float, 6, 1> result = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);

        float alpha = result (0);
        float beta  = result (1);
        float gamma = result (2);

        Eigen::Matrix3f Rinc = (Eigen::Matrix3f)AngleAxisf (gamma, Vector3f::UnitZ ()) * AngleAxisf (beta, Vector3f::UnitY ()) * AngleAxisf (alpha, Vector3f::UnitX ());
        Vector3f tinc = result.tail<3> ();

        //compose
        tcurr = Rinc * tcurr + tinc;
        Rcurr = Rinc * Rcurr;
      }
    }
  }
  //save tranform
  rmats_.push_back (Rcurr);
  tvecs_.push_back (tcurr);

  ///////////////////////////////////////////////////////////////////////////////////////////
  // Integration check - We do not integrate volume if camera does not move.
  float rnorm = rodrigues2(Rcurr.inverse() * Rprev).norm();
  float tnorm = (tcurr - tprev).norm();
  const float alpha = 1.f;
  bool integrate = (rnorm + alpha * tnorm)/2 >= integration_metric_threshold_;

  ///////////////////////////////////////////////////////////////////////////////////////////
  // Volume integration
  float3 device_volume_size = device_cast<const float3> (tsdf_volume_->getSize());

  Matrix3frm Rcurr_inv = Rcurr.inverse ();
  Mat33&  device_Rcurr_inv = device_cast<Mat33> (Rcurr_inv);
  float3& device_tcurr = device_cast<float3> (tcurr);
  if (integrate)
  {
    //ScopeTime time("tsdf");
    //integrateTsdfVolume(depth_raw, intr, device_volume_size, device_Rcurr_inv, device_tcurr, tranc_dist, volume_);
    integrateTsdfVolume (depth_raw, intr, device_volume_size, device_Rcurr_inv, device_tcurr, tsdf_volume_->getTsdfTruncDist(), tsdf_volume_->data(), depthRawScaled_);
  }

  ///////////////////////////////////////////////////////////////////////////////////////////
  // Ray casting
  Mat33& device_Rcurr = device_cast<Mat33> (Rcurr);
  {
    //ScopeTime time("ray-cast-all");
    raycast (intr, device_Rcurr, device_tcurr, tsdf_volume_->getTsdfTruncDist(), device_volume_size, tsdf_volume_->data(), vmaps_g_prev_[0], nmaps_g_prev_[0]);
    for (int i = 1; i < LEVELS; ++i)
    {
      resizeVMap (vmaps_g_prev_[i-1], vmaps_g_prev_[i]);
      resizeNMap (nmaps_g_prev_[i-1], nmaps_g_prev_[i]);
    }
    pcl::device::sync ();
  }

  ++global_time_;
  return (true);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Affine3f
pcl::gpu::KinfuTracker::getCameraPose (int time) const
{
  if (time > (int)rmats_.size () || time < 0)
    time = rmats_.size () - 1;

  Eigen::Affine3f aff;
  aff.linear () = rmats_[time];
  aff.translation () = tvecs_[time];
  return (aff);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

size_t
pcl::gpu::KinfuTracker::getNumberOfPoses () const
{
  return rmats_.size();
}

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

const TsdfVolume&
pcl::gpu::KinfuTracker::volume() const
{
  return *tsdf_volume_;
}

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

TsdfVolume&
pcl::gpu::KinfuTracker::volume()
{
  return *tsdf_volume_;
}

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

const ColorVolume&
pcl::gpu::KinfuTracker::colorVolume() const
{
  return *color_volume_;
}

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

ColorVolume&
pcl::gpu::KinfuTracker::colorVolume()
{
  return *color_volume_;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::gpu::KinfuTracker::getImage (View& view) const
{
  //Eigen::Vector3f light_source_pose = tsdf_volume_->getSize() * (-3.f);
  Eigen::Vector3f light_source_pose = tvecs_[tvecs_.size () - 1];

  device::LightSource light;
  light.number = 1;
  light.pos[0] = device_cast<const float3>(light_source_pose);

  view.create (rows_, cols_);
  generateImage (vmaps_g_prev_[0], nmaps_g_prev_[0], light, view);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::gpu::KinfuTracker::getLastFrameCloud (DeviceArray2D<PointType>& cloud) const
{
  cloud.create (rows_, cols_);
  DeviceArray2D<float4>& c = (DeviceArray2D<float4>&)cloud;
  device::convert (vmaps_g_prev_[0], c);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::gpu::KinfuTracker::getLastFrameNormals (DeviceArray2D<NormalType>& normals) const
{
  normals.create (rows_, cols_);
  DeviceArray2D<float8>& n = (DeviceArray2D<float8>&)normals;
  device::convert (nmaps_g_prev_[0], n);
}

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

void
pcl::gpu::KinfuTracker::initColorIntegration(int max_weight)
{
  color_volume_ = pcl::gpu::ColorVolume::Ptr( new ColorVolume(*tsdf_volume_, max_weight) );
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl::gpu::KinfuTracker::operator() (const DepthMap& depth, const View& colors)
{
  bool res = (*this)(depth);

  if (res && color_volume_)
  {
    const float3 device_volume_size = device_cast<const float3> (tsdf_volume_->getSize());
    device::Intr intr(fx_, fy_, cx_, cy_);

    Matrix3frm R_inv = rmats_.back().inverse();
    Vector3f   t     = tvecs_.back();

    Mat33&  device_Rcurr_inv = device_cast<Mat33> (R_inv);
    float3& device_tcurr = device_cast<float3> (t);

    device::updateColorVolume(intr, tsdf_volume_->getTsdfTruncDist(), device_Rcurr_inv, device_tcurr, vmaps_g_prev_[0],
        colors, device_volume_size, color_volume_->data(), color_volume_->getMaxWeight());
  }

  return res;
}

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

namespace pcl
{
  namespace gpu
  {
    PCL_EXPORTS void
    paint3DView(const KinfuTracker::View& rgb24, KinfuTracker::View& view, float colors_weight = 0.5f)
    {
      device::paint3DView(rgb24, view, colors_weight);
    }

    PCL_EXPORTS void
    mergePointNormal(const DeviceArray<PointXYZ>& cloud, const DeviceArray<Normal>& normals, DeviceArray<PointNormal>& output)
    {
      const size_t size = min(cloud.size(), normals.size());
      output.create(size);

      const DeviceArray<float4>& c = (const DeviceArray<float4>&)cloud;
      const DeviceArray<float8>& n = (const DeviceArray<float8>&)normals;
      const DeviceArray<float12>& o = (const DeviceArray<float12>&)output;
      device::mergePointNormal(c, n, o);
    }

    Eigen::Vector3f rodrigues2(const Eigen::Matrix3f& matrix)
    {
      Eigen::JacobiSVD<Eigen::Matrix3f> svd(matrix, Eigen::ComputeFullV | Eigen::ComputeFullU);
      Eigen::Matrix3f R = svd.matrixU() * svd.matrixV().transpose();

      double rx = R(2, 1) - R(1, 2);
      double ry = R(0, 2) - R(2, 0);
      double rz = R(1, 0) - R(0, 1);

      double s = sqrt((rx*rx + ry*ry + rz*rz)*0.25);
      double c = (R.trace() - 1) * 0.5;
      c = c > 1. ? 1. : c < -1. ? -1. : c;

      double theta = acos(c);

      if( s < 1e-5 )
      {
        double t;

        if( c > 0 )
          rx = ry = rz = 0;
        else
        {
          t = (R(0, 0) + 1)*0.5;
          rx = sqrt( std::max(t, 0.0) );
          t = (R(1, 1) + 1)*0.5;
          ry = sqrt( std::max(t, 0.0) ) * (R(0, 1) < 0 ? -1.0 : 1.0);
          t = (R(2, 2) + 1)*0.5;
          rz = sqrt( std::max(t, 0.0) ) * (R(0, 2) < 0 ? -1.0 : 1.0);

          if( fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R(1, 2) > 0) != (ry*rz > 0) )
            rz = -rz;
          theta /= sqrt(rx*rx + ry*ry + rz*rz);
          rx *= theta;
          ry *= theta;
          rz *= theta;
        }
      }
      else
      {
        double vth = 1/(2*s);
        vth *= theta;
        rx *= vth; ry *= vth; rz *= vth;
      }
      return Eigen::Vector3d(rx, ry, rz).cast<float>();
    }
  }
}


#ifndef CAFFE_HP_NET_H_
#define CAFFE_HP_NET_H_

#include "caffe/caffe.hpp"
#include "caffe/proto/caffe.pb.h"
#include <boost/shared_ptr.hpp>

#include "caffe/tracker/bounding_box.hpp"

#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>

namespace caffe {

class HPNetWrapper {
public:
  HPNetWrapper(const std::string& proto, const std::string& model);
  HPNetWrapper(const std::string& proto, const std::string& model, float scale);

  int hpmode(const cv::Mat& image, const BoundingBox<float>& roi, float* score);
  void getPoints(const cv::Mat& image, const BoundingBox<float>& roi, float& x1_r, float& y1_r, float& x2_r, float& y2_r);
  void getInputPatch(const cv::Mat& image, cv::Mat& rsz_patch, const BoundingBox<float>& roi);

  float kContextFactor;

private:
  void getCropPatch(const cv::Mat& image, const BoundingBox<float>& roi, cv::Mat* patch);
  void load(const cv::Mat& image);

  boost::shared_ptr<caffe::Net<float> > net_;
};

}

#endif