//==============================================================================
//         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


/**
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++;
	}
}


#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


#ifndef BOOST_GEOMETRY_PROJECTIONS_OCEA_HPP
#define BOOST_GEOMETRY_PROJECTIONS_OCEA_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

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

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// 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.

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>

namespace boost { namespace geometry { namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace ocea
    {

            struct par_ocea
            {
                double    rok;
                double    rtk;
                double    sinphi;
                double    cosphi;
                double    singam;
                double    cosgam;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_ocea_spheroid : public base_t_fi<base_ocea_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_ocea m_proj_parm;

                inline base_ocea_spheroid(const Parameters& par)
                    : base_t_fi<base_ocea_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double t;

                    xy_y = sin(lp_lon);
                /*
                    xy_x = atan2((tan(lp_lat) * this->m_proj_parm.cosphi + this->m_proj_parm.sinphi * xy_y) , cos(lp_lon));
                */
                    t = cos(lp_lon);
                    xy_x = atan((tan(lp_lat) * this->m_proj_parm.cosphi + this->m_proj_parm.sinphi * xy_y) / t);
                    if (t < 0.)
                        xy_x += geometry::math::pi<double>();
                    xy_x *= this->m_proj_parm.rtk;
                    xy_y = this->m_proj_parm.rok * (this->m_proj_parm.sinphi * sin(lp_lat) - this->m_proj_parm.cosphi * cos(lp_lat) * xy_y);
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    double t, s;

                    xy_y /= this->m_proj_parm.rok;
                    xy_x /= this->m_proj_parm.rtk;
                    t = sqrt(1. - xy_y * xy_y);
                    lp_lat = asin(xy_y * this->m_proj_parm.sinphi + t * this->m_proj_parm.cosphi * (s = sin(xy_x)));
                    lp_lon = atan2(t * this->m_proj_parm.sinphi * s - xy_y * this->m_proj_parm.cosphi,
                        t * cos(xy_x));
                }

                static inline std::string get_name()
                {
                    return "ocea_spheroid";
                }

            };

            // Oblique Cylindrical Equal Area
            template <typename Parameters>
            void setup_ocea(Parameters& par, par_ocea& proj_parm)
            {
                double phi_0=0.0, phi_1, phi_2, lam_1, lam_2, lonz, alpha;

                proj_parm.rok = par.a / par.k0;
                proj_parm.rtk = par.a * par.k0;
                if ( pj_param(par.params, "talpha").i) {
                    alpha    = pj_param(par.params, "ralpha").f;
                    lonz = pj_param(par.params, "rlonc").f;
                    proj_parm.singam = atan(-cos(alpha)/(-sin(phi_0) * sin(alpha))) + lonz;
                    proj_parm.sinphi = asin(cos(phi_0) * sin(alpha));
                } else {
                    phi_1 = pj_param(par.params, "rlat_1").f;
                    phi_2 = pj_param(par.params, "rlat_2").f;
                    lam_1 = pj_param(par.params, "rlon_1").f;
                    lam_2 = pj_param(par.params, "rlon_2").f;
                    proj_parm.singam = atan2(cos(phi_1) * sin(phi_2) * cos(lam_1) -
                        sin(phi_1) * cos(phi_2) * cos(lam_2),
                        sin(phi_1) * cos(phi_2) * sin(lam_2) -
                        cos(phi_1) * sin(phi_2) * sin(lam_1) );
                    proj_parm.sinphi = atan(-cos(proj_parm.singam - lam_1) / tan(phi_1));
                }
                par.lam0 = proj_parm.singam + geometry::math::half_pi<double>();
                proj_parm.cosphi = cos(proj_parm.sinphi);
                proj_parm.sinphi = sin(proj_parm.sinphi);
                proj_parm.cosgam = cos(proj_parm.singam);
                proj_parm.singam = sin(proj_parm.singam);
                par.es = 0.;
            }

        }} // namespace detail::ocea
    #endif // doxygen

    /*!
        \brief Oblique Cylindrical Equal Area projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
        \par Projection parameters
         - lonc: Longitude (only used if alpha (or gamma) is specified) (degrees)
         - alpha: Alpha (degrees)
         - lat_1: Latitude of first standard parallel (degrees)
         - lat_2: Latitude of second standard parallel (degrees)
         - lon_1 (degrees)
         - lon_2 (degrees)
        \par Example
        \image html ex_ocea.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct ocea_spheroid : public detail::ocea::base_ocea_spheroid<Geographic, Cartesian, Parameters>
    {
        inline ocea_spheroid(const Parameters& par) : detail::ocea::base_ocea_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::ocea::setup_ocea(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class ocea_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<ocea_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void ocea_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("ocea", new ocea_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_OCEA_HPP


#include <boost/pending/disjoint_sets.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <algorithm>
#include <cfloat>
#include <cmath>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iterator>
#include <map>
#include <numeric>
#include <queue>
#include <utility>
#include <vector>

#include "caffe/layer.hpp"
#include "caffe/layer_factory.hpp"
#include "caffe/layers/malis_loss_layer.hpp"
#include "caffe/util/math_functions.hpp"

namespace caffe {

template<class Dtype>
class MalisAffinityGraphCompare {
 private:
  const Dtype * mEdgeWeightArray;
 public:
  explicit MalisAffinityGraphCompare(const Dtype * EdgeWeightArray) {
    mEdgeWeightArray = EdgeWeightArray;
  }
  bool operator()(const uint64_t& ind1, const uint64_t& ind2) const {
    return (mEdgeWeightArray[ind1] > mEdgeWeightArray[ind2]);
  }
};

// Derived from https://github.com/srinituraga/malis/blob/master/matlab/malis_loss_mex.cpp
template<typename Dtype, typename MItype, typename MOtype>
void MalisLossLayer<Dtype, MItype, MOtype>::Malis(const Dtype* conn_data,
                                  const int_tp conn_num_dims,
                                  const int_tp* conn_dims,
                                  const int_tp* nhood_data,
                                  const int_tp* nhood_dims,
                                  const Dtype* seg_data, const bool pos,
                                  Dtype* dloss_data, Dtype* loss_out,
                                  Dtype *classerr_out, Dtype *rand_index_out) {
  if ((nhood_dims[1] != (conn_num_dims - 1))
      || (nhood_dims[0] != conn_dims[0])) {
    LOG(FATAL) << "nhood and conn dimensions don't match"
        << " (" << nhood_dims[1] << " vs. " << (conn_num_dims - 1)
        << " and " << nhood_dims[0] << " vs. "
        << conn_dims[conn_num_dims - 1] <<")";
  }

  /* Cache for speed to access neighbors */
  // nVert stores (X * Y * z)
  uint64_t nVert = 1;
  for (uint64_t i = 1; i < conn_num_dims; ++i) {
    nVert *= conn_dims[i];
    // std::cout << i << " nVert: " << nVert << std::endl;
  }

  // prodDims stores X, X*Y, X*Y*z offsets
  vector<uint64_t> prodDims(conn_num_dims - 1);
  prodDims[conn_num_dims - 2] = 1;
  for (uint64_t i = 1; i < conn_num_dims - 1; ++i) {
    prodDims[conn_num_dims - 2 - i] = prodDims[conn_num_dims - 1 - i]
                                      * conn_dims[conn_num_dims - i];
    // std::cout << conn_num_dims - 2 - i << " dims: "
    //   << prodDims[conn_num_dims - 2 - i] << std::endl;
  }

  /* convert n-d offset vectors into linear array offset scalars */
  // nHood is a vector of size #edges

  vector<uint32_t> nHood(nhood_dims[0]);
  for (uint64_t i = 0; i < nhood_dims[0]; ++i) {
    nHood[i] = 0;
    for (uint64_t j = 0; j < nhood_dims[1]; ++j) {
      nHood[i] += (uint32_t) nhood_data[j + i * nhood_dims[1]] * prodDims[j];
    }
    // std::cout << i << " nHood: " << nHood[i] << std::endl;
  }

  /* Disjoint sets and sparse overlap vectors */
  vector<std::map<uint64_t, uint64_t> > overlap(nVert);
  vector<uint64_t> rank(nVert);
  vector<uint64_t> parent(nVert);
  std::map<uint64_t, uint64_t> segSizes;
  uint64_t nLabeledVert = 0;
  uint64_t nPairPos = 0;
  boost::disjoint_sets<uint64_t*, uint64_t*> dsets(&rank[0], &parent[0]);
  // Loop over all seg data items
  for (uint64_t i = 0; i < nVert; ++i) {
    dsets.make_set(i);
    if (0 != seg_data[i]) {
      overlap[i].insert(std::pair<uint64_t, uint64_t>(seg_data[i], 1));
      ++nLabeledVert;
      ++segSizes[seg_data[i]];
      nPairPos += (segSizes[seg_data[i]] - 1);
    }
  }

  uint64_t nPairTot = (nLabeledVert * (nLabeledVert - 1)) / 2;
  uint64_t nPairNeg = nPairTot - nPairPos;
  uint64_t nPairNorm;

  if (pos) {
    nPairNorm = nPairPos;
  } else {
    nPairNorm = nPairNeg;
  }

  uint64_t edgeCount = 0;
  // Loop over #edges
  for (uint64_t d = 0, i = 0; d < conn_dims[0]; ++d) {
    // Loop over Z
    for (uint64_t z = 0; z < conn_dims[1]; ++z) {
      // Loop over Y
      for (uint64_t Y = 0; Y < conn_dims[2]; ++Y) {
        // Loop over X
        for (uint64_t X = 0; X < conn_dims[3]; ++X, ++i) {
          // Out-of-bounds check:
          if (!((z + nhood_data[d * nhood_dims[1] + 0] < 0)
              ||(z + nhood_data[d * nhood_dims[1] + 0] >= conn_dims[1])
              ||(Y + nhood_data[d * nhood_dims[1] + 1] < 0)
              ||(Y + nhood_data[d * nhood_dims[1] + 1] >= conn_dims[2])
              ||(X + nhood_data[d * nhood_dims[1] + 2] < 0)
              ||(X + nhood_data[d * nhood_dims[1] + 2] >= conn_dims[3]))) {
            ++edgeCount;
          }
        }
      }
    }
  }

  /* Sort all the edges in increasing order of weight */
  vector<uint64_t> pqueue(edgeCount);
  uint64_t j = 0;
  // Loop over #edges
  for (uint64_t d = 0, i = 0; d < conn_dims[0]; ++d) {
    // Loop over Z
    for (uint64_t z = 0; z < conn_dims[1]; ++z) {
      // Loop over Y
      for (uint64_t Y = 0; Y < conn_dims[2]; ++Y) {
        // Loop over X
        for (uint64_t X = 0; X < conn_dims[3]; ++X, ++i) {
          // Out-of-bounds check:
          if (!((z + nhood_data[d * nhood_dims[1] + 0] < 0)
              ||(z + nhood_data[d * nhood_dims[1] + 0] >= conn_dims[1])
              ||(Y + nhood_data[d * nhood_dims[1] + 1] < 0)
              ||(Y + nhood_data[d * nhood_dims[1] + 1] >= conn_dims[2])
              ||(X + nhood_data[d * nhood_dims[1] + 2] < 0)
              ||(X + nhood_data[d * nhood_dims[1] + 2] >= conn_dims[3]))) {
            pqueue[j++] = i;
          }
        }
      }
    }
  }

  pqueue.resize(j);

  std::sort(pqueue.begin(), pqueue.end(),
       MalisAffinityGraphCompare<Dtype>(conn_data));

  /* Start MST */
  uint64_t minEdge;
  uint64_t e, v1, v2;
  uint64_t set1, set2;
  uint64_t nPair = 0;
  double loss = 0, dl = 0;
  uint64_t nPairIncorrect = 0;
  std::map<uint64_t, uint64_t>::iterator it1, it2;

  /* Start Kruskal's */
  for (uint64_t i = 0; i < pqueue.size(); ++i) {
    minEdge = pqueue[i];
    // nVert = X * Y * z, minEdge in [0, X * Y * z * #edges]

    // e: edge dimension
    e = minEdge / nVert;

    // v1: node at edge beginning
    v1 = minEdge % nVert;

    // v2: neighborhood node at edge e
    v2 = v1 + nHood[e];

    // std::cout << "V1: " << v1 << ", V2: " << v2 << std::endl;

    set1 = dsets.find_set(v1);
    set2 = dsets.find_set(v2);

    if (set1 != set2) {
      dsets.link(set1, set2);

      /* compute the dloss for this MST edge */
      for (it1 = overlap[set1].begin(); it1 != overlap[set1].end(); ++it1) {
        for (it2 = overlap[set2].begin(); it2 != overlap[set2].end(); ++it2) {
          nPair = it1->second * it2->second;

          if (pos && (it1->first == it2->first)) {
            // +ve example pairs
            dl = (Dtype(1.0) - conn_data[minEdge]);
            loss += dl * dl * nPair;
            // Use hinge loss
            dloss_data[minEdge] += dl * nPair;
            if (conn_data[minEdge] <= Dtype(0.5)) {  // an error
              nPairIncorrect += nPair;
            }

          } else if ((!pos) && (it1->first != it2->first)) {
            // -ve example pairs
            dl = (-conn_data[minEdge]);
            loss += dl * dl * nPair;
            // Use hinge loss
            dloss_data[minEdge] += dl * nPair;
            if (conn_data[minEdge] > Dtype(0.5)) {  // an error
              nPairIncorrect += nPair;
            }
          }
        }
      }

      if (nPairNorm > 0) {
        dloss_data[minEdge] /= nPairNorm;
      } else {
        dloss_data[minEdge] = 0;
      }

      if (dsets.find_set(set1) == set2) {
        std::swap(set1, set2);
      }

      for (it2 = overlap[set2].begin();
          it2 != overlap[set2].end(); ++it2) {
        it1 = overlap[set1].find(it2->first);
        if (it1 == overlap[set1].end()) {
          overlap[set1].insert(pair<uint64_t, uint64_t>
            (it2->first, it2->second));
        } else {
          it1->second += it2->second;
        }
      }
      overlap[set2].clear();
    }  // end link
  }  // end while

  /* Return items */
  double classerr, randIndex;
  if (nPairNorm > 0) {
    loss /= nPairNorm;
  } else {
    loss = 0;
  }

  // std::cout << "nPairIncorrect: " << nPairIncorrect << std::endl;
  // std::cout << "nPairNorm: " << nPairNorm << std::endl;

  *loss_out = loss;
  classerr = static_cast<double>(nPairIncorrect)
      / static_cast<double>(nPairNorm);
  *classerr_out = classerr;
  randIndex = 1.0 - static_cast<double>(nPairIncorrect)
      / static_cast<double>(nPairNorm);
  *rand_index_out = randIndex;
}

template<typename Dtype, typename MItype, typename MOtype>
void MalisLossLayer<Dtype, MItype, MOtype>::LayerSetUp(
    const vector<Blob<MItype>*>& bottom, const vector<Blob<MOtype>*>& top) {
  LossLayer<Dtype, MItype, MOtype>::LayerSetUp(bottom, top);

  // Expected inputs:
  // Required (bottom 0 to 2):
  // Bottom 0: Predicted affinity, shaped     (batch size, #edges, (Z), (Y), X)
  // Bottom 1: Ground truth affinity, shaped  (batch size, #edges, (Z), (Y), X)
  // Bottom 2: Segmented ground truth, shaped (batch size, 1,      (Z), (Y), X)

  // Optional (bottom 3):
  // Bottom 3: Edge connectivity, size #edges * 3, shaped (Z,Y,X);(Z,Y,X);...
  // (this means pairs of 3 per edge)
}

template<typename Dtype, typename MItype, typename MOtype>
void MalisLossLayer<Dtype, MItype, MOtype>::Reshape(
    const vector<Blob<MItype>*>& bottom, const vector<Blob<MOtype>*>& top) {
  LossLayer<Dtype, MItype, MOtype>::Reshape(bottom, top);

  if (top.size() >= 2) {
    top[1]->ReshapeLike(*bottom[0]);
  }

  // Up to 5 dimensional; supported modes:
  // batch, channels (edges), Z, Y, X    => 3D affinity
  // batch, channels (edges), Y, X       => 2D affinity
  // batch, channels (edges), X          => 1D affinity
  vector<int_tp> shape = bottom[0]->shape();

  conn_dims_.clear();
  nhood_dims_.clear();

  // #edges, Z, Y, X specification (4 dimensions)
  conn_num_dims_ = 4;

  // Channel axis equals number of edges
  nedges_ = shape[1];

  // #edges
  conn_dims_.push_back(nedges_);
  // Z-axis
  conn_dims_.push_back(shape.size() >= 5 ? shape[shape.size() - 3] : 1);
  // Y-axis
  conn_dims_.push_back(shape.size() >= 4 ? shape[shape.size() - 2] : 1);
  // X-axis
  conn_dims_.push_back(shape.size() >= 3 ? shape[shape.size() - 1] : 1);

  // #edges
  nhood_dims_.push_back(nedges_);
  // 3 dimensional (always, to simplify things;
  // can just set unused spatials to 0)
  nhood_dims_.push_back(3);

  affinity_pos_.Reshape(shape);
  affinity_neg_.Reshape(shape);
  dloss_pos_.Reshape(shape);
  dloss_neg_.Reshape(shape);
}

template<typename Dtype, typename MItype, typename MOtype>
void MalisLossLayer<Dtype, MItype, MOtype>::Forward_cpu(const vector<Blob<MItype>*>& bottom,
                                        const vector<Blob<MOtype>*>& top) {
  // Set up the neighborhood
  nhood_data_.clear();
  if (bottom.size() == 4) {
    // Custom edges
    for (int_tp i = 0; i < nedges_; ++i) {
      // Z edge direction
      nhood_data_.push_back(bottom[3]->cpu_data()[i * 3 + 0]);
      // Y edge direction
      nhood_data_.push_back(bottom[3]->cpu_data()[i * 3 + 1]);
      // X edge direction
      nhood_data_.push_back(bottom[3]->cpu_data()[i * 3 + 2]);
    }
  } else {
    // Dimension primary edges (+Z, +Y, +X) only:
    // 1 edge:    +X          (0,0,1)
    // 2 edges:   +Y, +X      (0,1,0); (0,0,1)
    // 3 edges:   +Z, +Y, +X  (1,0,0); (0,1,0); (0,0,1)
    for (int_tp i = 3 - nedges_; i < 3; ++i) {
      nhood_data_.push_back((i + 3) % 3 == 0 ? 1 : 0);
      nhood_data_.push_back((i + 2) % 3 == 0 ? 1 : 0);
      nhood_data_.push_back((i + 1) % 3 == 0 ? 1 : 0);
    }
  }

  // Predicted affinity
  const Dtype* affinity_prob = bottom[0]->cpu_data();

  // Effective affinity
  const Dtype* affinity = bottom[1]->cpu_data();

  Dtype* affinity_data_pos = affinity_pos_.mutable_cpu_data();
  Dtype* affinity_data_neg = affinity_neg_.mutable_cpu_data();

// Affinity graph must be in the range (0,1)
// square loss (euclidean) is used by MALIS
  for (int_tp i = 0; i < bottom[0]->count(); ++i) {
    affinity_data_pos[i] = std::min(affinity_prob[i], affinity[i]);
    affinity_data_neg[i] = std::max(affinity_prob[i], affinity[i]);
  }

  uint_tp batch_offset = 1;
  for (int_tp i = 1; i < bottom[0]->shape().size(); ++i) {
    batch_offset *= bottom[0]->shape()[i];
  }

  uint_tp components_batch_offset = 1;
  uint_tp components_channel_offset = bottom[2]->shape()[1] == 2 ? 1 : 0;
  for (int_tp i = 1; i < bottom[2]->shape().size(); ++i) {
    components_batch_offset *= bottom[2]->shape()[i];
    if (i > 1) {
      components_channel_offset *= bottom[2]->shape()[i];
    }
  }

  float loss = 0;

#pragma omp parallel for reduction(+:loss)
  for (int_tp batch = 0; batch < bottom[0]->shape()[0]; ++batch) {
    Dtype loss_out = 0;
    Dtype classerr_out = 0;
    Dtype rand_index_out = 0;

    caffe_set(dloss_neg_.count(), Dtype(0.0), dloss_neg_.mutable_cpu_data());
    caffe_set(dloss_pos_.count(), Dtype(0.0), dloss_pos_.mutable_cpu_data());

    Malis(&affinity_data_neg[batch_offset * batch], conn_num_dims_,
          &conn_dims_[0], &nhood_data_[0], &nhood_dims_[0],
          bottom[2]->cpu_data() + components_batch_offset * batch, false,
          dloss_neg_.mutable_cpu_data() + batch_offset * batch, &loss_out,
          &classerr_out, &rand_index_out);

    loss += 0.5 * loss_out;
    // std::cout << "NEG: " << loss_out << std::endl;

    Malis(&affinity_data_pos[batch_offset * batch], conn_num_dims_,
          &conn_dims_[0], &nhood_data_[0], &nhood_dims_[0],
          bottom[2]->cpu_data() + components_batch_offset * batch
          + components_channel_offset, true,
          dloss_pos_.mutable_cpu_data() + batch_offset * batch, &loss_out,
          &classerr_out, &rand_index_out);

    loss += 0.5 * loss_out;
    // std::cout << "POS: " << loss_out << std::endl;
  }

  // Normalized loss over batch size
  top[0]->mutable_cpu_data()[0] = loss
      / (static_cast<Dtype>(bottom[0]->shape()[0]));

  if (top.size() == 2) {
    top[1]->ShareData(*(bottom[0]));
  }
}

template<typename Dtype, typename MItype, typename MOtype>
void MalisLossLayer<Dtype, MItype, MOtype>::Backward_cpu(const vector<Blob<MOtype>*>& top,
                                         const vector<bool>& propagate_down,
                                         const vector<Blob<MItype>*>& bottom) {
  if (propagate_down[0]) {
    Dtype* bottom_diff = bottom[0]->mutable_cpu_diff();
    const Dtype* dloss_pos_data = dloss_pos_.cpu_data();
    const Dtype* dloss_neg_data = dloss_neg_.cpu_data();

    // Clear the diff
    caffe_set(bottom[0]->count(), Dtype(0.0), bottom_diff);

    for (int_tp i = 0; i < bottom[0]->count(); ++i) {
      bottom_diff[i] = -(dloss_neg_data[i] + dloss_pos_data[i]) / 2.0;
    }
  }
}

INSTANTIATE_CLASS_3T_GUARDED(MalisLossLayer, (half_fp), (half_fp), (half_fp));
INSTANTIATE_CLASS_3T_GUARDED(MalisLossLayer, (float), (float), (float));
INSTANTIATE_CLASS_3T_GUARDED(MalisLossLayer, (double), (double), (double));

REGISTER_LAYER_CLASS(MalisLoss);
REGISTER_LAYER_CLASS_INST(MalisLoss, (half_fp), (half_fp), (half_fp));
REGISTER_LAYER_CLASS_INST(MalisLoss, (float), (float), (float));
REGISTER_LAYER_CLASS_INST(MalisLoss, (double), (double), (double));

}  // namespace caffe


/*
 * cuConebeamProjectionOperator.cpp
 *
 *  Created on: Feb 18, 2015
 *      Author: u051747
 */

#include "cuConebeamProjectionOperator.h"
#include "conebeam_projection.h"
#include <boost/math/constants/constants.hpp>
#include "hoNDArray_math.h"
using boost::math::constants::pi;
namespace Gadgetron {

cuConebeamProjectionOperator::cuConebeamProjectionOperator() {
	// TODO Auto-generated constructor stub

	samples_per_pixel_ = 1.5f;
	allow_offset_correction_override_=true;

}
void cuConebeamProjectionOperator
::offset_correct(cuNDArray<float>* projections){

	if( !preprocessed_ ){
		throw std::runtime_error( "Error: cuConebeamProjectionOperator::offset_correct: setup not performed");
	}
	float SDD = acquisition_->get_geometry()->get_SDD();
	float SAD = acquisition_->get_geometry()->get_SAD();
	floatd2 ps_dims_in_mm = acquisition_->get_geometry()->get_FOV();
	apply_offset_correct( projections,acquisition_->get_geometry()->get_offsets(),ps_dims_in_mm, SDD, SAD);
}

cuConebeamProjectionOperator::~cuConebeamProjectionOperator() {
	// TODO Auto-generated destructor stub
}

void cuConebeamProjectionOperator::mult_M(cuNDArray<float>* input,
		cuNDArray<float>* output, bool accumulate) {

	auto dims = *input->get_dimensions();
	std::vector<size_t> dims3d(dims);

	if (dims3d.size() ==4) dims3d.pop_back();
	auto outdims = *output->get_dimensions();
	std::vector<size_t> outbindims(outdims);
	float* input_ptr = input->get_data_ptr();
	float* output_ptr = output->get_data_ptr();
	for (int bin = 0; bin < binning_->get_number_of_bins(); bin++){
		//Check for empty bins
		if (binning_->get_bin(bin).size() == 0)
			continue;
		cuNDArray<float> input_view(dims3d,input_ptr);
		outbindims.back() = angles[bin].size();
		auto  output_view = boost::make_shared<cuNDArray<float>>(outbindims,output_ptr);
		auto output_view2 = output_view;
		/*
		if (use_offset_correction_ && accumulate){
			output_view2 = boost::make_shared<cuNDArray<float>>(outbindims);
			clear(output_view2.get());
		}
		 */

		conebeam_forwards_projection(output_view2.get(),&input_view,angles[bin],offsets[bin],samples_per_pixel_,is_dims_in_mm_,acquisition_->get_geometry()->get_FOV(),acquisition_->get_geometry()->get_SDD(),acquisition_->get_geometry()->get_SAD(),accumulate);

		/*
		if (use_offset_correction_){
			apply_offset_correct(output_view2.get(),offsets[bin],acquisition_->get_geometry()->get_FOV(),acquisition_->get_geometry()->get_SDD(),acquisition_->get_geometry()->get_SAD());
			if (accumulate)
				*output_view += *output_view2;
		}
		 */

		input_ptr += input_view.get_number_of_elements();
		output_ptr += output_view->get_number_of_elements();

	}

}

void cuConebeamProjectionOperator::mult_MH(cuNDArray<float>* input,
		cuNDArray<float>* output, bool accumulate) {
	auto dims = *output->get_dimensions();
	std::vector<size_t> dims3d = dims;
	if (dims3d.size() ==4) dims3d.pop_back();

	auto indims = *input->get_dimensions();
	std::vector<size_t> inbindims(indims);
	float* input_ptr = input->get_data_ptr();
	float* output_ptr = output->get_data_ptr();
	for (int bin = 0; bin < binning_->get_number_of_bins(); bin++){
		//Check for empty bins
		if (binning_->get_bin(bin).size() == 0)
			continue;

		cuNDArray<float> output_view(dims3d,output_ptr);
		inbindims.back() = angles[bin].size();
		auto input_view = boost::make_shared<cuNDArray<float>>(inbindims,input_ptr);
		auto input_view2 = input_view;
		if(use_offset_correction_)
			input_view2 = boost::make_shared<cuNDArray<float>>(*input_view);

		vector_td<int,3> is_dims_in_pixels{dims3d[0], dims3d[1],dims3d[2]};

		conebeam_backwards_projection(input_view2.get(),&output_view, angles[bin],offsets[bin],is_dims_in_pixels, is_dims_in_mm_,acquisition_->get_geometry()->get_FOV(),acquisition_->get_geometry()->get_SDD(),acquisition_->get_geometry()->get_SAD(),use_offset_correction_,accumulate);

		input_ptr += input_view->get_number_of_elements();
		output_ptr += output_view.get_number_of_elements();

	}

	if (mask)
		apply_mask(output,mask.get());

}

boost::shared_ptr<cuNDArray<bool>> cuConebeamProjectionOperator::calculate_mask( cuNDArray<float>* projections,float limit)
{
	auto dims = *this->get_domain_dimensions();
	std::vector<size_t> dims3d(dims.begin(),dims.end()-1);
	auto indims = *projections->get_dimensions();
	std::vector<size_t> inbindims(indims);
	float* input_ptr = projections->get_data_ptr();
	auto mask = boost::make_shared<cuNDArray<bool>>(dims);
	bool* mask_ptr = mask->get_data_ptr();
	for (int bin = 0; bin < binning_->get_number_of_bins(); bin++){
		//Check for empty bins
		if (binning_->get_bin(bin).size() == 0)
			continue;

		cuNDArray<bool> mask_view(dims3d,mask_ptr);
		inbindims.back() = angles[bin].size();
		auto input_view = boost::make_shared<cuNDArray<float>>(inbindims,input_ptr);

		vector_td<int,3> is_dims_in_pixels{dims3d[0], dims3d[1],dims3d[2]};

		conebeam_spacecarver(input_view.get(),&mask_view, angles[bin],offsets[bin],is_dims_in_pixels, is_dims_in_mm_,acquisition_->get_geometry()->get_FOV(),acquisition_->get_geometry()->get_SDD(),acquisition_->get_geometry()->get_SAD(),limit);

		input_ptr += input_view->get_number_of_elements();
		mask_ptr += mask_view.get_number_of_elements();

	}

	return mask;

}
void Gadgetron::cuConebeamProjectionOperator::setup( boost::shared_ptr<CBCT_acquisition> acquisition,
		floatd3 is_dims_in_mm, bool transform_angles )
{
	acquisition_ = acquisition;
	is_dims_in_mm_ = is_dims_in_mm;

	// Determine the minimum and maximum angles scanned and transform array angles from [0;max_angle_].
	//

	std::vector<float> &all_angles = acquisition->get_geometry()->get_angles();
	if (transform_angles){
		float min_value = *std::min_element(all_angles.begin(), all_angles.end() );
		transform(all_angles.begin(), all_angles.end(), all_angles.begin(), bind2nd(std::minus<float>(), min_value));
	}
	// Are we in a short scan setup?
	// - we say yes if we have covered less than PI+3*delta radians
	//

	float angle_span = *std::max_element(all_angles.begin(), all_angles.end() );
	floatd2 ps_dims_in_mm = acquisition_->get_geometry()->get_FOV();
	float SDD = acquisition_->get_geometry()->get_SDD();
	float delta = std::atan(ps_dims_in_mm[0]/(2.0f*SDD)); // Fan angle

	if( angle_span*pi<float>()/180.0f > pi<float>()+3.0f*delta )
		short_scan_ = false;
	else
		short_scan_ = true;

	std::vector<floatd2> all_offsets = acquisition_->get_geometry()->get_offsets();
	floatd2 mean_offset = std::accumulate(all_offsets.begin(),all_offsets.end(),floatd2(0,0))/float(all_offsets.size());
	if( allow_offset_correction_override_ && std::abs(mean_offset[0]) > ps_dims_in_mm[0]*0.1f )
		use_offset_correction_ = true;

	std::cout << "Mean offset " << mean_offset << " Use offset correct: " << (use_offset_correction_ ? "true" : "false") << std::endl;
	preprocessed_ = true;
	if (!binning_){
		std::vector<unsigned int> bins(angles.size());
		std::iota(bins.begin(),bins.end(),0);
		binning_ = boost::make_shared<CBCT_binning>(std::vector<std::vector<unsigned int>>(1,bins));
	}

	angles = std::vector<std::vector<float>>();
	offsets = std::vector<std::vector<floatd2>>();
	for (int bin =0; bin < binning_->get_number_of_bins(); bin++){
		auto binvec = binning_->get_bin(bin);
		angles.push_back(std::vector<float>());
		offsets.push_back(std::vector<floatd2>());
		for (auto index : binvec){
			angles.back().push_back(all_angles[index]);
			offsets.back().push_back(all_offsets[index]);
		}

	}
	auto permutations = new_order(binning_->get_bins());
	auto proj = acquisition->get_projections();
	if (proj)
		*proj =	*permute_projections(proj,permutations);

}

void Gadgetron::cuConebeamProjectionOperator::setup( boost::shared_ptr<CBCT_acquisition> acquisition,
		boost::shared_ptr<CBCT_binning> binning,floatd3 is_dims_in_mm, 		bool transform_angles)

{

	binning_ = binning;
	setup( acquisition, is_dims_in_mm,transform_angles );
}

std::vector<unsigned int> Gadgetron::cuConebeamProjectionOperator::new_order(std::vector<std::vector<unsigned int>> bins){
	std::vector<unsigned int> result;
	for (auto & b : bins)
		for (auto p : b)
			result.push_back(p);
	return result;
}

boost::shared_ptr<hoCuNDArray<float> > Gadgetron::cuConebeamProjectionOperator::permute_projections(
		boost::shared_ptr<hoCuNDArray<float> > projections,
		std::vector<unsigned int>  & permutations) {

	std::vector<size_t> new_proj_dims = {projections->get_size(0),projections->get_size(1),permutations.size()};

	auto result = boost::make_shared<hoCuNDArray<float>>(new_proj_dims);

	size_t nproj = permutations.size();
	size_t proj_size = projections->get_size(0)*projections->get_size(1);

	float * res_ptr = result->get_data_ptr();
	float * proj_ptr = projections->get_data_ptr();

	for (unsigned int i = 0; i < nproj; i++){
		cudaMemcpy(res_ptr+i*proj_size,proj_ptr+proj_size*permutations[i],proj_size*sizeof(float),cudaMemcpyHostToHost);
	}
	return result;

}

} /* namespace Gadgetron */


// Copyright (C) 2011-2012 by the BEM++ Authors
//
// 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.

#include "laplace_3d_hypersingular_boundary_operator.hpp"

#include "blas_quadrature_helper.hpp"
#include "context.hpp"
#include "general_elementary_local_operator_imp.hpp"
#include "general_hypersingular_integral_operator_imp.hpp"
#include "laplace_3d_single_layer_boundary_operator.hpp"
#include "synthetic_integral_operator.hpp"

#include "../common/boost_make_shared_fwd.hpp"

#include "../fiber/explicit_instantiation.hpp"

#include "../fiber/laplace_3d_single_layer_potential_kernel_functor.hpp"
#include "../fiber/laplace_3d_hypersingular_off_diagonal_kernel_functor.hpp"
#include "../fiber/surface_curl_3d_functor.hpp"
#include "../fiber/scalar_function_value_functor.hpp"
#include "../fiber/simple_test_scalar_kernel_trial_integrand_functor.hpp"
#include "../fiber/single_component_test_trial_integrand_functor.hpp"

#include "../fiber/default_collection_of_kernels.hpp"
#include "../fiber/default_collection_of_basis_transformations.hpp"
#include "../fiber/default_test_kernel_trial_integral.hpp"

#include "general_elementary_singular_integral_operator_imp.hpp"
#include "../fiber/typical_test_scalar_kernel_trial_integral.hpp"

#include <boost/type_traits/is_complex.hpp>

namespace Bempp {

template <typename BasisFunctionType, typename ResultType>
BoundaryOperator<BasisFunctionType, ResultType>
laplace3dSyntheticHypersingularBoundaryOperator(
    const shared_ptr<const Context<BasisFunctionType, ResultType>> &context,
    const shared_ptr<const Space<BasisFunctionType>> &domain,
    const shared_ptr<const Space<BasisFunctionType>> &range,
    const shared_ptr<const Space<BasisFunctionType>> &dualToRange,
    std::string label, int internalSymmetry,
    const BoundaryOperator<BasisFunctionType, ResultType> &externalSlp) {
  typedef typename ScalarTraits<BasisFunctionType>::RealType CoordinateType;

  typedef Fiber::ScalarFunctionValueFunctor<CoordinateType> ValueFunctor;
  typedef Fiber::SurfaceCurl3dFunctor<CoordinateType> CurlFunctor;
  typedef Fiber::SingleComponentTestTrialIntegrandFunctor<
      BasisFunctionType, ResultType> IntegrandFunctor;

  typedef GeneralElementaryLocalOperator<BasisFunctionType, ResultType> LocalOp;
  typedef SyntheticIntegralOperator<BasisFunctionType, ResultType> SyntheticOp;

  if (!domain || !range || !dualToRange)
    throw std::invalid_argument(
        "laplace3dSyntheticHypersingularBoundaryOperator(): "
        "domain, range and dualToRange must not be null");

  shared_ptr<const Space<BasisFunctionType>> newDomain = domain;
  shared_ptr<const Space<BasisFunctionType>> newDualToRange = dualToRange;

  bool isBarycentric =
      (domain->isBarycentric() || dualToRange->isBarycentric());

  if (isBarycentric) {
    newDomain = domain->barycentricSpace(domain);
    newDualToRange = dualToRange->barycentricSpace(dualToRange);
  }

  shared_ptr<const Context<BasisFunctionType, ResultType>> internalContext,
      auxContext;
  SyntheticOp::getContextsForInternalAndAuxiliaryOperators(
      context, internalContext, auxContext);
  shared_ptr<const Space<BasisFunctionType>> internalTrialSpace =
      newDomain->discontinuousSpace(newDomain);
  shared_ptr<const Space<BasisFunctionType>> internalTestSpace =
      newDualToRange->discontinuousSpace(newDualToRange);

  // Note: we don't really need to care about ranges and duals to domains of
  // the internal operator. The only range space that matters is that of the
  // leftmost operator in the product.

  const char xyz[] = "xyz";
  const size_t dimWorld = 3;

  if (label.empty())
    label =
        AbstractBoundaryOperator<BasisFunctionType, ResultType>::uniqueLabel();

  BoundaryOperator<BasisFunctionType, ResultType> slp;

  if (!externalSlp.isInitialized()) {

    slp = laplace3dSingleLayerBoundaryOperator<BasisFunctionType, ResultType>(
        internalContext, internalTrialSpace,
        internalTestSpace /* or whatever */, internalTestSpace,
        "(" + label + ")_internal_SLP", internalSymmetry);

  } else {

    slp = externalSlp;
  }

  std::vector<BoundaryOperator<BasisFunctionType, ResultType>>
  trialCurlComponents;
  std::vector<BoundaryOperator<BasisFunctionType, ResultType>>
  testCurlComponents;
  testCurlComponents.resize(3);
  for (size_t i = 0; i < dimWorld; ++i)
    testCurlComponents[i] = BoundaryOperator<BasisFunctionType, ResultType>(
        auxContext, boost::make_shared<LocalOp>(
                        internalTestSpace, range, newDualToRange,
                        ("(" + label + ")_test_curl_") + xyz[i], NO_SYMMETRY,
                        CurlFunctor(), ValueFunctor(), IntegrandFunctor(i, 0)));
  int syntheseSymmetry = 0; // symmetry of the decomposition
  if (newDomain == newDualToRange && internalTrialSpace == internalTestSpace)
    syntheseSymmetry =
        HERMITIAN | (boost::is_complex<BasisFunctionType>() ? 0 : SYMMETRIC);
  else {
    trialCurlComponents.resize(3);
    for (size_t i = 0; i < dimWorld; ++i)
      trialCurlComponents[i] = BoundaryOperator<BasisFunctionType, ResultType>(
          auxContext,
          boost::make_shared<LocalOp>(
              newDomain, internalTrialSpace /* or whatever */,
              internalTrialSpace, ("(" + label + ")_trial_curl_") + xyz[i],
              NO_SYMMETRY, ValueFunctor(), CurlFunctor(),
              IntegrandFunctor(0, i)));
  }

  return BoundaryOperator<BasisFunctionType, ResultType>(
      context, boost::make_shared<SyntheticOp>(testCurlComponents, slp,
                                               trialCurlComponents, label,
                                               syntheseSymmetry));
}

template <typename BasisFunctionType, typename ResultType>
BoundaryOperator<BasisFunctionType, ResultType>
laplace3dHypersingularBoundaryOperator(
    const shared_ptr<const Context<BasisFunctionType, ResultType>> &context,
    const shared_ptr<const Space<BasisFunctionType>> &domain,
    const shared_ptr<const Space<BasisFunctionType>> &range,
    const shared_ptr<const Space<BasisFunctionType>> &dualToRange,
    const std::string &label, int symmetry,
    const BoundaryOperator<BasisFunctionType, ResultType> &externalSlp) {
  const AssemblyOptions &assemblyOptions = context->assemblyOptions();
  if ((assemblyOptions.assemblyMode() == AssemblyOptions::ACA &&
       assemblyOptions.acaOptions().mode == AcaOptions::LOCAL_ASSEMBLY) ||
      (externalSlp.isInitialized()))
    return laplace3dSyntheticHypersingularBoundaryOperator(
        context, domain, range, dualToRange, label, symmetry, externalSlp);

  typedef typename ScalarTraits<BasisFunctionType>::RealType KernelType;
  typedef typename ScalarTraits<BasisFunctionType>::RealType CoordinateType;

  typedef Fiber::Laplace3dSingleLayerPotentialKernelFunctor<KernelType>
  KernelFunctor;
  typedef Fiber::SurfaceCurl3dFunctor<CoordinateType> TransformationFunctor;
  typedef Fiber::SimpleTestScalarKernelTrialIntegrandFunctorExt<
      BasisFunctionType, KernelType, ResultType, 3> IntegrandFunctor;

  typedef Fiber::Laplace3dHypersingularOffDiagonalKernelFunctor<KernelType>
  OffDiagonalKernelFunctor;
  typedef Fiber::ScalarFunctionValueFunctor<CoordinateType>
  OffDiagonalTransformationFunctor;
  typedef Fiber::SimpleTestScalarKernelTrialIntegrandFunctorExt<
      BasisFunctionType, KernelType, ResultType, 1> OffDiagonalIntegrandFunctor;

  typedef GeneralHypersingularIntegralOperator<BasisFunctionType, KernelType,
                                               ResultType> Op;

  shared_ptr<Fiber::TestKernelTrialIntegral<BasisFunctionType, KernelType,
                                            ResultType>> integral,
      offDiagonalIntegral;
  if (shouldUseBlasInQuadrature(assemblyOptions, *domain, *dualToRange)) {
    integral.reset(new Fiber::TypicalTestScalarKernelTrialIntegral<
        BasisFunctionType, KernelType, ResultType>());
    offDiagonalIntegral = integral;
  } else {
    integral.reset(new Fiber::DefaultTestKernelTrialIntegral<IntegrandFunctor>(
        IntegrandFunctor()));
    offDiagonalIntegral.reset(
        new Fiber::DefaultTestKernelTrialIntegral<OffDiagonalIntegrandFunctor>(
            OffDiagonalIntegrandFunctor()));
  }

  shared_ptr<Op> newOp(
      new Op(domain, range, dualToRange, label, symmetry, KernelFunctor(),
             TransformationFunctor(), TransformationFunctor(), integral,
             OffDiagonalKernelFunctor(), OffDiagonalTransformationFunctor(),
             OffDiagonalTransformationFunctor(), offDiagonalIntegral));

  return BoundaryOperator<BasisFunctionType, ResultType>(context, newOp);
}

template <typename BasisFunctionType, typename ResultType>
BoundaryOperator<BasisFunctionType, ResultType>
laplace3dHypersingularBoundaryOperator(
    const ParameterList& parameterList,
    const shared_ptr<const Space<BasisFunctionType>> &domain,
    const shared_ptr<const Space<BasisFunctionType>> &range,
    const shared_ptr<const Space<BasisFunctionType>> &dualToRange,
    const std::string &label, int symmetry,
    const BoundaryOperator<BasisFunctionType, ResultType> &externalSlp){

  shared_ptr<const Context<BasisFunctionType, ResultType>> context(
      new Context<BasisFunctionType, ResultType>(parameterList));
  return laplace3dHypersingularBoundaryOperator(context, domain, range,
                                              dualToRange, label, symmetry,
                                              externalSlp);

}

#define INSTANTIATE_NONMEMBER_CONSTRUCTOR(BASIS, RESULT)                       \
  template BoundaryOperator<BASIS, RESULT>                                     \
  laplace3dHypersingularBoundaryOperator(                                      \
      const ParameterList&,                                                    \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &, const std::string &, int,        \
      const BoundaryOperator<BASIS, RESULT> &);                                \
  template BoundaryOperator<BASIS, RESULT>                                     \
  laplace3dHypersingularBoundaryOperator(                                      \
      const shared_ptr<const Context<BASIS, RESULT>> &,                        \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &, const std::string &, int,        \
      const BoundaryOperator<BASIS, RESULT> &)
FIBER_ITERATE_OVER_BASIS_AND_RESULT_TYPES(INSTANTIATE_NONMEMBER_CONSTRUCTOR);

} // namespace Bempp