text:
#pragma once

#include <optional>
#include <memory>

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

#include <oxm/field.hh>
#include <oxm/field_set.hh>

#include "policies.hh"
#include "trace_tree.hh"

namespace runos {
namespace retic {
namespace fdd {

struct action_unit {
    action_unit() = default;
    action_unit(oxm::field_set acts) : pred_actions(acts) { }

    action_unit(oxm::field_set acts, PacketFunction body)
        : pred_actions(acts)
        , body(body)
    { }

    action_unit(oxm::field_set acts, PacketFunction body, const action_unit &post)
        : pred_actions(acts)
        , body(body)
        , post_actions(new action_unit(post))
    { }

    action_unit(oxm::field_set acts, std::optional<PacketFunction> body, const std::unique_ptr<action_unit>& post)
        : pred_actions(acts)
        , body(body)
    {
        if (post != nullptr) {
            post_actions.reset(new action_unit(*post));
         }
    }

    action_unit(const action_unit& acts)
        : pred_actions(acts.pred_actions)
        , body(acts.body)
    {
        if (acts.post_actions != nullptr) {
            post_actions.reset(new action_unit(*acts.post_actions));
        }
    }

    action_unit& operator=(const action_unit& acts) {
        pred_actions = acts.pred_actions;
        body = acts.body;
        if (acts.post_actions != nullptr) {
            post_actions.reset(new action_unit(*acts.post_actions));
        }
        return *this;
    }

    oxm::field_set pred_actions;
    std::optional<PacketFunction> body;
    std::unique_ptr<action_unit> post_actions;

    friend bool operator==(const action_unit& lhs, const action_unit& rhs) {
        bool pred_eq = lhs.pred_actions == rhs.pred_actions;
        bool body_eq = lhs.body == rhs.body;
        if (lhs.post_actions != nullptr) {
            if (rhs.post_actions != nullptr) {
                return pred_eq && body_eq && *lhs.post_actions == *rhs.post_actions;
            } else {
                return false;
            }
        } else if (rhs.post_actions != nullptr) {
            return false;
        } else {
            return pred_eq && body_eq;
        }
        return pred_eq;
    }
};

struct leaf {
    std::vector<action_unit> sets;
    FlowSettings flow_settings;
    trace_tree::node maple_tree;
    mutable uint16_t prio_down, prio_up; // TODO: unhack me
};

struct node;
struct compiler;

using diagram = boost::variant<
        leaf,
        boost::recursive_wrapper<node>
    >;

struct node {
    oxm::field<> field;
    diagram positive;
    diagram negative;
};

struct diagram_holder {
    diagram value;
};

} // namespace fdd
} // namespace retic
} // namespace runos

ext: hh

max_stars_repo_name: enotnadoske/drunos

max_issues_repo_name: enotnadoske/drunos

max_forks_repo_name: enotnadoske/drunos


text:
#include "common.h"

#include <dependency_graph/metadata_register.h>
#include <dependency_graph/node.h>

#include <boost/test/unit_test.hpp>
#include <dependency_graph/attr.inl>
#include <dependency_graph/metadata.inl>
#include <dependency_graph/node_base.inl>
#include <dependency_graph/port.inl>
#include <dependency_graph/values.inl>

using namespace dependency_graph;

namespace std {

ostream& operator<<(ostream& out, const type_info& t) {
	out << t.name();

	return out;
}

}  // namespace std

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

namespace {
static unsigned s_counter = 0;
}

TestStruct::TestStruct() : id(s_counter++) {
}

bool TestStruct::operator==(const TestStruct& ts) const {
	return id == ts.id;
}

bool TestStruct::operator!=(const TestStruct& ts) const {
	return id != ts.id;
}

std::ostream& operator<<(std::ostream& out, const TestStruct& t) {
	out << t.id;

	return out;
}

NoncopyableStruct::NoncopyableStruct() : id(s_counter++) {
}

std::ostream& operator<<(std::ostream& out, const NoncopyableStruct& t) {
	out << t.id;

	return out;
}

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

const MetadataHandle& additionNode() {
	static std::unique_ptr<MetadataHandle> s_handle;

	if(s_handle == nullptr) {
		std::unique_ptr<Metadata> meta(new Metadata("addition"));

		// create attributes
		static InAttr<float> additionInput1, additionInput2;
		static OutAttr<float> additionOutput;
		BOOST_CHECK(not additionInput1.isValid());
		BOOST_CHECK(not additionInput2.isValid());
		BOOST_CHECK(not additionOutput.isValid());

		// add attributes to the Metadata instance
		meta->addAttribute(additionInput1, "input_1");
		meta->addAttribute(additionInput2, "input_2");
		meta->addAttribute(additionOutput, "output");

		BOOST_REQUIRE_EQUAL(meta->attributeCount(), 3u);
		BOOST_CHECK_EQUAL(meta->attr(0), additionInput1);
		BOOST_CHECK_EQUAL(meta->attr(1), additionInput2);
		BOOST_CHECK_EQUAL(meta->attr(2), additionOutput);

		// setup influences
		BOOST_CHECK_NO_THROW(meta->addInfluence(additionInput1, additionOutput));
		BOOST_CHECK_NO_THROW(meta->addInfluence(additionInput2, additionOutput));

		std::vector<std::reference_wrapper<const Attr>> influences;

		BOOST_CHECK_NO_THROW(influences = meta->influences(additionInput1));
		BOOST_REQUIRE_EQUAL(influences.size(), 1u);
		BOOST_CHECK_EQUAL(influences.begin()->get(), additionOutput);

		BOOST_CHECK_NO_THROW(influences = meta->influencedBy(additionOutput));
		BOOST_REQUIRE_EQUAL(influences.size(), 2u);
		BOOST_CHECK_EQUAL(influences[0].get(), additionInput1);
		BOOST_CHECK_EQUAL(influences[1].get(), additionInput2);

		std::function<State(Values&)> additionCompute = [&](Values& data) {
			const float a = data.get(additionInput1);
			const float b = data.get(additionInput2);

			data.set(additionOutput, a + b);

			return State();
		};
		meta->setCompute(additionCompute);

		s_handle = std::unique_ptr<MetadataHandle>(new MetadataHandle(std::move(meta)));

		dependency_graph::MetadataRegister::singleton().add(*s_handle);
	}

	return *s_handle;
}

const MetadataHandle& multiplicationNode() {
	static std::unique_ptr<MetadataHandle> s_handle;

	if(s_handle == nullptr) {
		std::unique_ptr<Metadata> meta(new Metadata("multiplication"));

		static InAttr<float> multiplicationInput1, multiplicationInput2;
		static OutAttr<float> multiplicationOutput;
		std::function<State(Values&)> multiplicationCompute = [&](Values& data) {
			const float a = data.get(multiplicationInput1);
			const float b = data.get(multiplicationInput2);

			data.set(multiplicationOutput, a * b);

			return State();
		};

		meta->addAttribute(multiplicationInput1, "input_1");
		meta->addAttribute(multiplicationInput2, "input_2");
		meta->addAttribute(multiplicationOutput, "output");

		meta->addInfluence(multiplicationInput1, multiplicationOutput);
		meta->addInfluence(multiplicationInput2, multiplicationOutput);

		meta->setCompute(multiplicationCompute);

		s_handle = std::unique_ptr<MetadataHandle>(new MetadataHandle(std::move(meta)));

		dependency_graph::MetadataRegister::singleton().add(*s_handle);
	}

	return *s_handle;
}

ext: cpp

max_stars_repo_name: martin-pr/possumwood

max_issues_repo_name: LIUJUN-liujun/possumwood

max_forks_repo_name: LIUJUN-liujun/possumwood


text:
/*
 * DIPlib 3.0
 * This file contains definitions of functions for adaptive Gaussian filtering.
 *
 * (c)2018, Erik Schuitema.
 * Based on original DIPlib code: (c)1995-2014, Delft University of Technology.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "diplib/nonlinear.h"
#include "diplib/framework.h"
#include "diplib/generation.h"
#include "diplib/overload.h"
#include "diplib/pixel_table.h"
#include "diplib/private/constfor.h"

#if defined(__GNUG__) || defined(__clang__)
// For this file, turn off -Wsign-conversion, Eigen is really bad at this!
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
#pragma GCC diagnostic ignored "-Wconversion"
#if __GNUC__ >= 7 || __clang_major__ >= 12
#pragma GCC diagnostic ignored "-Wint-in-bool-context"
#endif
#if __GNUC__ >= 9
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif
#endif

#include <Eigen/Geometry>

namespace dip {

namespace {

// KernelTransform: transforms kernel pixel coordinates according to one or more parameter images
// The base class performs no specific transformation,
// other than simply adding kernel coordinates to the current image coordinates
class KernelTransform {
   public:
      // Virtual destructor
      virtual ~KernelTransform() = default;

      // Clone the kernel transform. This is done when creating a copy for each thread
      // to avoid unwanted sharing between threads of the members altered inside SetImageCoords().
      virtual KernelTransform* Clone() const { return new KernelTransform( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) {
         // Cast-copy
         imgCoords_.resize( imgCoords.size() );
         for( UnsignedArray::size_type ii = 0; ii < imgCoords.size(); ++ii ) {
            imgCoords_[ ii ] = static_cast< dfloat >( imgCoords[ ii ] );
         }
         // Note for writing derived classes: perform any parameter computation or other preparation here,
         // so it is done only once per input pixel.
      }

      // Transforms kernel coordinates to input image coordinates
      virtual void Transform( IntegerArray const& kernelCoords, dip::uint /*tensorIndex*/, FloatArray& transformedCoords ) const {
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + static_cast< dfloat >( kernelCoords[ 0 ] );
         transformedCoords[ 1 ] = imgCoords_[ 1 ] + static_cast< dfloat >( kernelCoords[ 1 ] );
      }

   protected:
      FloatArray imgCoords_;
      // Makes a quick copy of a parameter image and expands its tensor if necessary
      static void ParamImageQuickCopyAndExpandTensor( Image const& paramImage, Image& expandedParamImage, dip::uint inputTensorElements ) {
         expandedParamImage = paramImage.QuickCopy();
         if( expandedParamImage.TensorElements() != inputTensorElements ) {
            expandedParamImage.ExpandSingletonTensor( inputTensorElements );
         }
      }
};

// Scaling helper class
template< dip::uint nDims >
class KernelTransformScale {
   public:
      // The `kernelScale` image tensor need not be expanded beforehand
      KernelTransformScale( Image const& kernelScale, dip::uint inputTensorElements )
            : scaleAtImgCoords_( inputTensorElements ), inputTensorElements_( inputTensorElements ) {
         // The kernel scale image must be a COL_MAJOR_MATRIX tensor image with the following tensor size:
         // Rows == input tensor size (the input image has a column vector tensor)
         // Cols == input dimensionality == kernelScaleDimensionality
         kernelScale_ = kernelScale.QuickCopy();
         dip::uint tensorRows = inputTensorElements;
         dip::uint tensorCols = kernelScale.Dimensionality();
         // See if we need to reshape the tensor
         if( kernelScale_.TensorElements() == 1 ) {
            // Singleton expansion to COL_MAJOR_MATRIX tensor
            kernelScale_.ExpandSingletonTensor( tensorRows * tensorCols );
            kernelScale_.ReshapeTensor( tensorRows, tensorCols );
            scaleTensorLUT_ = kernelScale_.Tensor().LookUpTable();
         } else if( kernelScale_.TensorColumns() == tensorCols && kernelScale_.TensorRows() == tensorRows ) {
            // Dimensions are ok, only create LUT
            scaleTensorLUT_ = kernelScale_.Tensor().LookUpTable();
         } else if( inputTensorElements == 1 && kernelScale_.TensorShape() == Tensor::Shape::COL_VECTOR && kernelScale_.TensorRows() == tensorCols ) {
            // The scale tensor is a col vector but must be a row vector
            kernelScale_.ReshapeTensor( tensorRows, tensorCols );
            scaleTensorLUT_ = kernelScale_.Tensor().LookUpTable();
         } else if( kernelScale_.TensorColumns() == 1 && kernelScale_.TensorRows() == tensorRows ) {
            // Only the row size matches: tensor contains a scalar per input tensor element -> create special LUT but leave the tensor itself unchanged
            // LUT is indexed as: [col * NUM_ROWS + row]
            scaleTensorLUT_ = kernelScale_.Tensor().LookUpTable(); // LUT for column vector
            //kernelScale_.ExpandSingletonTensor(tensorRows * tensorCols);
            //kernelScale_.ReshapeTensor( tensorRows, tensorCols );
            // Complete LUT by adding the same indices for each column
            dip::sint LUTColSize = static_cast< dip::sint >( scaleTensorLUT_.size() );
            for( dip::uint iDim = 1; iDim < nDims; ++iDim ) {
               scaleTensorLUT_.insert( scaleTensorLUT_.end(), scaleTensorLUT_.begin(), scaleTensorLUT_.begin() + LUTColSize );
            }
         } else {
            DIP_THROW( "Scale parameter image tensor has wrong size, must have " + std::to_string( inputTensorElements ) + " rows and " + std::to_string( nDims ) + " columns" );
         }
      }

      // Computes scaleAtImgCoords_.
      // Prerequisite: SetImageCoords() must have been called to populate the image coords
      void SetScaleAtImgCoords( UnsignedArray const& imgCoords ) {
         // Given a tensor with `M` rows and `N` columns, tensor element `(m,n)` has the linear
         // index given by `scaleTensorLUT_[n*M+m] = Tensor::LookUpTable()[n*M+m]`.
         Image::Pixel scalePixel = kernelScale_.At( imgCoords );
         for( dip::uint iTE = 0; iTE < inputTensorElements_; ++iTE ) {
            // scaleTensorRowIndex = iTE : The scale tensor has a row for each input tensor element.
            for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
               // scaleTensorColIndex = iDim : The scale tensor has a column for each kernel/input dimension ( 0 for X, 1 for Y ).
               scaleAtImgCoords_[ iTE ][ iDim ] = scalePixel[ scaleTensorLUT_[ iDim * inputTensorElements_ + iTE ]].template As< dfloat >();
            }
         }
      }

   protected:
      std::vector< std::array< dfloat, nDims >> scaleAtImgCoords_; // The vector is over the input tensor elements; the array is over the kernel dimensions
      Image kernelScale_;
      dip::uint inputTensorElements_;
      std::vector< dip::sint > scaleTensorLUT_;
};

// Kernel transformation: 2D rotation
class KernelTransform2DRotation : public KernelTransform {
   public:
      // The `orientation` image tensor need not be expanded beforehand
      KernelTransform2DRotation( Image const& orientation, dip::uint inputTensorElements ) {
         ParamImageQuickCopyAndExpandTensor( orientation, orientation_, inputTensorElements );
         csn_.resize( inputTensorElements );
         sn_.resize( inputTensorElements );
      }

      virtual KernelTransform* Clone() const override { return new KernelTransform2DRotation( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform::SetImageCoords( imgCoords );
         Image::Pixel dirPixel = orientation_.At( imgCoords );
         // Iterate over tensor elements
         for( dip::uint iTE = 0; iTE < orientation_.TensorElements(); ++iTE ) {
            csn_[ iTE ] = std::cos( dip::pi * 0.5 - dirPixel[ iTE ].As< dfloat >() );
            sn_[ iTE ] = std::sin( dip::pi * 0.5 - dirPixel[ iTE ].As< dfloat >() );
         }
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + static_cast< dfloat >( kernelCoords[ 0 ] ) * csn_[ tensorIndex ]
                                  + static_cast< dfloat >( kernelCoords[ 1 ] ) * sn_[ tensorIndex ];
         transformedCoords[ 1 ] = imgCoords_[ 1 ] - static_cast< dfloat >( kernelCoords[ 0 ] ) * sn_[ tensorIndex ]
                                  + static_cast< dfloat >( kernelCoords[ 1 ] ) * csn_[ tensorIndex ];
      }

   protected:
      std::vector< dfloat > csn_, sn_;   // Length is equal to number of input tensor elements
      Image orientation_;
};

class KernelTransform2DScaledRotation : public KernelTransform2DRotation, public KernelTransformScale< 2 > {
   public:
      // The `orientation` and `kernelScale` image tensors need not be expanded beforehand
      KernelTransform2DScaledRotation( Image const& orientation, Image const& kernelScale, dip::uint inputTensorElements )
            : KernelTransform2DRotation( orientation, inputTensorElements ), KernelTransformScale< 2 >( kernelScale, inputTensorElements ) {}

      virtual KernelTransform* Clone() const override { return new KernelTransform2DScaledRotation( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform2DRotation::SetImageCoords( imgCoords );
         SetScaleAtImgCoords( imgCoords );
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         // First scale, then rotate
         dfloat scaledKernelCoords[ 2 ] = {
               scaleAtImgCoords_[ tensorIndex ][ 0 ] * static_cast< dfloat >( kernelCoords[ 0 ] ),
               scaleAtImgCoords_[ tensorIndex ][ 1 ] * static_cast< dfloat >( kernelCoords[ 1 ] )
         };
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + scaledKernelCoords[ 0 ] * csn_[ tensorIndex ]
                                  + scaledKernelCoords[ 1 ] * sn_[ tensorIndex ];
         transformedCoords[ 1 ] = imgCoords_[ 1 ] - scaledKernelCoords[ 0 ] * sn_[ tensorIndex ]
                                  + scaledKernelCoords[ 1 ] * csn_[ tensorIndex ];
      }
};

// Kernel transformation: 3D rotation using phi3 and theta3
class KernelTransform3DRotationZ : public KernelTransform {
   public:
      // The `phi3` and `theta3` image tensors need not be expanded beforehand
      KernelTransform3DRotationZ( Image const& phi3, Image const& theta3, dip::uint inputTensorElements ) {
         ParamImageQuickCopyAndExpandTensor( phi3, phi3_, inputTensorElements );
         ParamImageQuickCopyAndExpandTensor( theta3, theta3_, inputTensorElements );
         R_.resize( inputTensorElements );
      }

      virtual KernelTransform* Clone() const override { return new KernelTransform3DRotationZ( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform::SetImageCoords( imgCoords );
         Image::Pixel phiPixel = phi3_.At( imgCoords );
         Image::Pixel thetaPixel = theta3_.At( imgCoords );
         for( dip::uint iTE = 0; iTE < phi3_.TensorElements(); ++iTE ) {
            dfloat phi = phiPixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements
            dfloat theta = thetaPixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements
            dfloat cs_p = std::cos( phi );
            dfloat sn_p = std::sin( phi );
            dfloat cs_t = std::cos( theta );
            dfloat sn_t = std::sin( theta );

            RotMatrix& R = R_[ iTE ];
            R[ 0 ] = cs_p * cs_t;   R[ 1 ] = -sn_p;   R[ 2 ] = cs_p * sn_t;
            R[ 3 ] = sn_p * cs_t;   R[ 4 ] = cs_p;    R[ 5 ] = sn_p * sn_t;
            R[ 6 ] = -sn_t;         R[ 7 ] = 0;       R[ 8 ] = cs_t;
         }
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         RotMatrix const& R = R_[ tensorIndex ];
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + static_cast< dfloat >( kernelCoords[ 0 ] ) * R[ 0 ]
                                  + static_cast< dfloat >( kernelCoords[ 1 ] ) * R[ 1 ]
                                  + static_cast< dfloat >( kernelCoords[ 2 ] ) * R[ 2 ];
         transformedCoords[ 1 ] = imgCoords_[ 1 ] + static_cast< dfloat >( kernelCoords[ 0 ] ) * R[ 3 ]
                                  + static_cast< dfloat >( kernelCoords[ 1 ] ) * R[ 4 ]
                                  + static_cast< dfloat >( kernelCoords[ 2 ] ) * R[ 5 ];
         transformedCoords[ 2 ] = imgCoords_[ 2 ] + static_cast< dfloat >( kernelCoords[ 0 ] ) * R[ 6 ]
                                  + static_cast< dfloat >( kernelCoords[ 2 ] ) * R[ 8 ];
      }

   protected:
      Image phi3_;
      Image theta3_;
      using RotMatrix = std::array< dfloat, 9 >;
      std::vector< RotMatrix > R_;  // Rotation matrix. The vector is over the input tensor elements. // TODO: use Matrix class?
};

// Kernel transformation: 3D rotation using phi2, theta2, phi3 and theta3
class KernelTransform3DRotationXY : public KernelTransform {
   public:
      // The `phi2`, `theta2`, `phi3` and `theta3` image tensors need not be expanded beforehand
      KernelTransform3DRotationXY( Image const& phi2, Image const& theta2, Image const& phi3, Image const& theta3, dip::uint inputTensorElements ) {
         // Expand parameter images if necessary
         ParamImageQuickCopyAndExpandTensor( phi2, phi2_, inputTensorElements );
         ParamImageQuickCopyAndExpandTensor( theta2, theta2_, inputTensorElements );
         ParamImageQuickCopyAndExpandTensor( phi3, phi3_, inputTensorElements );
         ParamImageQuickCopyAndExpandTensor( theta3, theta3_, inputTensorElements );
         T_.resize( inputTensorElements );
      }

      virtual KernelTransform* Clone() const override { return new KernelTransform3DRotationXY( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         Image::Pixel phi2Pixel = phi2_.At( imgCoords );
         Image::Pixel theta2Pixel = theta2_.At( imgCoords );
         Image::Pixel phi3Pixel = phi3_.At( imgCoords );
         Image::Pixel theta3Pixel = theta3_.At( imgCoords );
         for( dip::uint iTE = 0; iTE < phi2_.TensorElements(); ++iTE ) {
            dfloat phi2 = phi2Pixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements
            dfloat theta2 = theta2Pixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements
            dfloat phi3 = phi3Pixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements
            dfloat theta3 = theta3Pixel[ iTE ].As< dfloat >();  // TODO: handle tensor elements

            // Create transformation matrix from basis vectors
            Eigen::Vector3d xAxis( GetAxis( phi2, theta2 ));
            Eigen::Vector3d yAxis( GetAxis( phi3, theta3 ));
            Eigen::Vector3d zAxis( xAxis.cross( yAxis ).normalized() ); // TODO: do we need to normalize? Only needed if xAxis and zAxis are not perpendicular..
            // Set rotation part
            T_[ iTE ].linear() << xAxis, yAxis, zAxis;
            // Set translation part
            T_[ iTE ].translation() << Eigen::Vector3d( imgCoords_[ 0 ], imgCoords_[ 1 ], imgCoords_[ 2 ] );
         }
      }

      virtual void Transform( IntegerArray const& /*kernelCoords*/, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         Eigen::Map< Eigen::Vector3d > output( transformedCoords.data() );   // Create an Eigen map that directly accesses transformedCoords
         output = T_[ tensorIndex ] * output;
      }

   protected:
      Eigen::Vector3d GetAxis( dfloat phi, dfloat theta ) {
         dfloat sin_phi = std::sin( phi );
         dfloat cos_phi = std::cos( phi );
         dfloat sin_theta = std::sin( theta );
         dfloat cos_theta = std::cos( theta );
         return Eigen::Vector3d( sin_phi * cos_theta, sin_phi * sin_theta, cos_phi );
      }

      Image phi2_;
      Image theta2_;
      Image phi3_;
      Image theta3_;
      using CompactTransform = Eigen::Transform< dfloat, 3, Eigen::AffineCompact >;
      std::vector< CompactTransform, Eigen::aligned_allocator< CompactTransform >> T_;  // Transformation matrix. The vector is over the input tensor elements.
};

// Kernel transformation: 2D skew
class KernelTransform2DSkew : public KernelTransform {
   public:
      // The `skew` image tensor need not be expanded beforehand
      KernelTransform2DSkew( Image const& skew, dip::uint inputTensorElements ) {
         ParamImageQuickCopyAndExpandTensor( skew, skew_, inputTensorElements );
         s_.resize( inputTensorElements );
      }

      virtual KernelTransform* Clone() const override { return new KernelTransform2DSkew( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform::SetImageCoords( imgCoords );
         Image::Pixel skewPixel = skew_.At( imgCoords );
         for( dip::uint iTE = 0; iTE < skew_.TensorElements(); ++iTE ) {
            s_[ iTE ] = skewPixel[ iTE ].As< dfloat >();
         }
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         dfloat kernelCoordX = static_cast< dfloat >( kernelCoords[ 0 ] );
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + kernelCoordX;
         transformedCoords[ 1 ] = imgCoords_[ 1 ] + static_cast< dfloat >( kernelCoords[ 1 ] ) + s_[ tensorIndex ] * kernelCoordX;
      }

   protected:
      Image skew_;
      std::vector< dfloat > s_;  // The vector is over the input tensor elements
};

// Kernel transformation: 2D banana
class KernelTransform2DBanana : public KernelTransform2DRotation {
   public:
      // The `orientation` and `hcurvature` image tensors need not be expanded beforehand
      KernelTransform2DBanana( Image const& orientation, Image const& hcurvature, dip::uint inputTensorElements )
            : KernelTransform2DRotation( orientation, inputTensorElements ) {
         ParamImageQuickCopyAndExpandTensor( hcurvature, hcurvature_, inputTensorElements );
         hcurv_.resize( inputTensorElements );
      }

      virtual KernelTransform* Clone() const override { return new KernelTransform2DBanana( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform2DRotation::SetImageCoords( imgCoords );
         Image::Pixel cPixel = hcurvature_.At( imgCoords );
         for( dip::uint iTE = 0; iTE < orientation_.TensorElements(); ++iTE ) {
            hcurv_[ iTE ] = -0.5 * cPixel[ iTE ].As< dfloat >();
         }
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         dfloat kernelCoordX = static_cast< dfloat> ( kernelCoords[ 0 ] );
         dfloat kernelCoordY = static_cast< dfloat >( kernelCoords[ 1 ] ) + ( hcurv_[ tensorIndex ] * kernelCoordX * kernelCoordX );
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + kernelCoordX * csn_[ tensorIndex ] + kernelCoordY * sn_[ tensorIndex ];
         transformedCoords[ 1 ] = imgCoords_[ 1 ] - kernelCoordX * sn_[ tensorIndex ] + kernelCoordY * csn_[ tensorIndex ];
      }

   protected:
      Image hcurvature_;
      std::vector< dfloat > hcurv_; // The vector is over the input tensor elements
};

class KernelTransform2DScaledBanana : public KernelTransform2DBanana, public KernelTransformScale< 2 > {
   public:
      // The `orientation`, `hcurvature` and `kernelScale` image tensors need not be expanded beforehand
      KernelTransform2DScaledBanana( Image const& orientation, Image const& hcurvature, Image const& kernelScale, dip::uint inputTensorElements )
            : KernelTransform2DBanana( orientation, hcurvature, inputTensorElements ), KernelTransformScale< 2 >( kernelScale, inputTensorElements ) {}

      virtual KernelTransform* Clone() const override { return new KernelTransform2DScaledBanana( *this ); }

      virtual void SetImageCoords( UnsignedArray const& imgCoords ) override {
         KernelTransform2DBanana::SetImageCoords( imgCoords );
         SetScaleAtImgCoords( imgCoords );
      }

      virtual void Transform( IntegerArray const& kernelCoords, dip::uint tensorIndex, FloatArray& transformedCoords ) const override {
         // First scale, then curve, then rotate
         dfloat kernelCoordX = scaleAtImgCoords_[ tensorIndex ][ 0 ] * static_cast< dfloat >( kernelCoords[ 0 ] );
         dfloat kernelCoordY = scaleAtImgCoords_[ tensorIndex ][ 1 ] * static_cast< dfloat >( kernelCoords[ 1 ] ) + ( hcurv_[ tensorIndex ] * kernelCoordX * kernelCoordX );
         transformedCoords[ 0 ] = imgCoords_[ 0 ] + kernelCoordX * csn_[ tensorIndex ] + kernelCoordY * sn_[ tensorIndex ];
         transformedCoords[ 1 ] = imgCoords_[ 1 ] - kernelCoordX * sn_[ tensorIndex ] + kernelCoordY * csn_[ tensorIndex ];
      }
};

// Input interpolation class
template< typename TPI, typename TPO >
class InputInterpolator {
   public:
      explicit InputInterpolator( Image const& in )
            : in_( in ), inOrigin_( static_cast< TPI* >( in_.Origin() )), inTensorStride_( in_.TensorStride() ) {}

      virtual ~InputInterpolator() = default;

      virtual TPO GetInputValue( FloatArray& /*coords*/, dip::uint /*tensorIndex*/, bool /*mirrorAtImageBoundaries*/ ) const { return 0; }

   protected:
      // Maps coords to a location inside the image using mirroring at the image boundaries.
      // Returns false if coordinates don't fall inside the image even after mirroring.
      template< dip::uint nDims >
      bool MapCoords_Mirror( dfloat* coords ) const {
         // Make sure all coordinates are within the image. Mirror at the boundaries.
         for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
            const dfloat cMax = static_cast< dfloat >( in_.Size( iDim ) - 1 );
            dfloat& c = coords[ iDim ];
            if( c < 0 ) {
               c = -c;
               if( c > cMax ) {
                  return false; // Mirroring exceeds border
               }
            } else if( c > cMax ) {
               c = cMax - ( c - cMax );
               if( c < 0 ) {
                  return false; // Mirroring exceeds border
               }
            }
         }
         return true;
      }

      Image const& in_; // Input image
      TPI* inOrigin_;   // Input image origin
      dip::sint inTensorStride_;   // Input image tensor stride
};

// Input interpolation class, templated in the number of input dimensions
template< dip::uint nDims, typename TPI, typename TPO >
class InputInterpolatorFixedDims : public InputInterpolator< TPI, TPO > {
   public:
      explicit InputInterpolatorFixedDims( Image const& in ) : InputInterpolator< TPI, TPO >( in ) {
         // Verify input dimensionality
         DIP_THROW_IF( in_.Dimensionality() != nDims, "Interpolation dimensionality incorrect" );
         // Cache strides and sizes
         for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
            inStrides_[ iDim ] = in_.Stride( iDim );
            inSizes_[ iDim ] = in_.Size( iDim );
         }
      }

   protected:
      using InputInterpolator< TPI, TPO >::in_;
      std::array< dip::sint, nDims > inStrides_;
      std::array< dip::uint, nDims > inSizes_;
};

// Zero order hold input interpolator
template< dip::uint nDims, typename TPI, typename TPO >
class InputInterpolatorZOH : public InputInterpolatorFixedDims< nDims, TPI, TPO > {
   public:
      explicit InputInterpolatorZOH( Image const& in ) : InputInterpolatorFixedDims< nDims, TPI, TPO >( in ) {}

      // Get zero-order-hold input value at floating point coordinates, arbitrary input dimensionality
      TPO GetInputValue( FloatArray& coords, dip::uint tensorIndex, bool mirrorAtImageBoundaries ) const override {
         // If not mirroring at the image boundaries, the input value is considered 0 outside the image
         if( !mirrorAtImageBoundaries ) {
            if( !in_.IsInside( coords )) {
               return 0;
            }
         } else {
            // Mirror input coordinates. May fail -> return 0.
            if( !this->template MapCoords_Mirror< nDims >( &coords[ 0 ] )) {
               return 0;
            }
         }

         // Compute pixel offset
         dip::sint pixelOffset = 0;
         for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
            dip::sint integerCoord = floor_cast( coords[ iDim ] );
            pixelOffset += integerCoord * inStrides_[ iDim ];
         }

         // Add tensor offset
         pixelOffset += static_cast< dip::sint >( tensorIndex ) * inTensorStride_;

         // Output single input value
         TPI inValue = *( inOrigin_ + pixelOffset );
         return static_cast< TPO >(inValue);
      }

      using InputInterpolatorFixedDims< nDims, TPI, TPO >::inStrides_;
      using InputInterpolator< TPI, TPO >::inTensorStride_;
      using InputInterpolator< TPI, TPO >::inOrigin_;
      using InputInterpolator< TPI, TPO >::in_;
};

// First order hold input interpolator
template< dip::uint nDims, typename TPI, typename TPO >
class InputInterpolatorFOH : public InputInterpolatorFixedDims< nDims, TPI, TPO > {
   public:
      explicit InputInterpolatorFOH( Image const& in ) : InputInterpolatorFixedDims< nDims, TPI, TPO >( in ) {}

      // Get linearly interpolated input value at floating point coordinates, arbitrary input dimensionality
      TPO GetInputValue( FloatArray& coords, dip::uint tensorIndex, bool mirrorAtImageBoundaries ) const override {
         using InputAsFloat = FloatType< TPI >;

         // If not mirroring at the image boundaries, the input value is considered 0 outside the image
         if( !mirrorAtImageBoundaries ) {
            if( !in_.IsInside( coords )) {
               return 0;
            }
         } else {
            // Mirror input coordinates. May fail -> return 0.
            if( !this->template MapCoords_Mirror< nDims >( &coords[ 0 ] )) {
               return 0;
            }
         }

         // Start linear interpolation
         // Compute lower bound index and interpolation factor for all coordinates
         //  E.g.: if coords[i] == 1.1, then loBoundIndices[i] == 1.0 and factors[i] == 0.1
         std::array< dip::sint, nDims > loBoundIndices;
         std::array< InputAsFloat, nDims > factors; // Linear interpolation factors
         for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
            loBoundIndices[ iDim ] = floor_cast( coords[ iDim ] );   // Take lower bound using floor_cast
            if( loBoundIndices[ iDim ] == static_cast< dip::sint >( inSizes_[ iDim ] - 1 )) { // Because we interpolate between loBound and loBound+1, make sure we don't go beyond the image borders
               loBoundIndices[ iDim ]--;
            }
            factors[ iDim ] = static_cast< InputAsFloat >( coords[ iDim ] ) - static_cast< InputAsFloat >( loBoundIndices[ iDim ] );
         }

         // Compute pixel offset for first pixel of the interpolation input window, including tensor offset
         dip::sint interpOriginOffset = 0;
         for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
            interpOriginOffset += loBoundIndices[ iDim ] * inStrides_[ iDim ];
         }

         // Compute pixel offsets and composite interpolation factors for a total of 2 ^ nDims pixels
         // We can avoid an ND-loop by exploiting the fact that only 2 coordinates are processed per dimension.
         constexpr dip::uint numInterpPixels = dip::uint( 1 ) << nDims;

         TPO result = 0.0;
         //for( dip::uint iOffset = 0; iOffset < numInterpPixels; ++iOffset ) {
         const_for< numInterpPixels >(
               [ & ]( dip::uint iOffset ) {
                  dip::sint pixelOffset = interpOriginOffset;
                  InputAsFloat pixelFactor = 1.0;
                  //for( dip::uint iDim = 0; iDim < nDims; ++iDim ) {
                  const_for< nDims >(
                        [ & ]( dip::uint iDim ) {
                           // To process all permutations of lowerbound and upperbound (= lowerbound + 1) in each dimension,
                           // decide whether to process the upperbound in this dimension by testing the bits in iOffset
                           bool useUpperBound = ( iOffset & ( dip::uint( 1 ) << iDim )) != 0;
                           if( useUpperBound ) {
                              pixelOffset += inStrides_[ iDim ];
                              pixelFactor *= factors[ iDim ];
                           } else {
                              pixelFactor *= 1 - factors[ iDim ];
                           }
                        } );
                  // Add tensor offset
                  pixelOffset += static_cast< dip::sint >( tensorIndex ) * inTensorStride_;
                  // Multiply pixel input values with composite interpolation factors
                  TPI inValue = *( inOrigin_ + pixelOffset );
                  result += static_cast< TPO >(inValue) * pixelFactor;
               } );

         return result;
      }

      /*
      // GetInputValue_LinearInterpolation() 2D reference implementation for speed comparison
      TPO GetInputValue_LinearInterpolation_2D( FloatArray& coords, bool mirrorAtImageBoundaries ) const {
         using InputAsFloat = FloatType< TPI >;
         assert( in_.Dimensionality() == nDims );

         // Cache strides
         std::array< dip::sint, 2 > strides = { in_.Stride( 0 ), in_.Stride( 1 ) };

         // If not mirroring at the image boundaries, the input value is considered 0 outside the image
         if( !mirrorAtImageBoundaries ) {
            if( !in_.IsInside( coords ) )
               return 0;
         } else {
            // Mirror input coordinates. May fail -> return 0.
            if( !this->template MapCoords_Mirror< 2 >( &coords[ 0 ] ) )
               return 0;
         }

         // Start linear interpolation
         // Compute lower bound index and interpolation factor for all coordinates
         //  E.g.: if coords[i] == 1.1, then loBoundIndices[i] == 1.0 and factors[i] == 0.1
         std::array< dip::uint, 2> loBoundIndices;
         for( dip::uint iDim = 0; iDim < 2; ++iDim ) {
            loBoundIndices[ iDim ] = (floor_cast( coords[ iDim ] ));   // Take lower bound using floor_cast
            if( coords[ iDim ] == in_.Size( iDim ) - 1 )  // Because we interpolate between loBound and loBound+1, make sure we don't go beyond the image borders
               loBoundIndices[ iDim ]--;
         }

         // Compute pixel offset of p00
         dip::sint offset = loBoundIndices[ 0 ] * strides[ 0 ] + loBoundIndices[ 1 ] * strides[ 1 ];

         TPI* origin = static_cast<TPI*>(in_.Origin());
         TPO p00 = static_cast<TPO>(*(origin + offset));
         TPO p01 = static_cast<TPO>(*(origin + offset + strides[ 1 ]));
         TPO p10 = static_cast<TPO>(*(origin + offset + strides[ 0 ]));
         TPO p11 = static_cast<TPO>(*(origin + offset + strides[ 0 ] + strides[ 1 ]));
         InputAsFloat xo = coords[ 0 ] - loBoundIndices[ 0 ];
         InputAsFloat yo = coords[ 1 ] - loBoundIndices[ 1 ];
         InputAsFloat xs = 1 - xo;
         InputAsFloat ys = 1 - yo;

         return (TPO)((p00 * xs * ys)
            + (p10 * xo * ys)
            + (p01 * xs * yo)
            + (p11 * xo * yo));
      }
      */

      using InputInterpolatorFixedDims< nDims, TPI, TPO >::inSizes_;
      using InputInterpolatorFixedDims< nDims, TPI, TPO >::inStrides_;
      using InputInterpolator< TPI, TPO >::inTensorStride_;
      using InputInterpolator< TPI, TPO >::inOrigin_;
      using InputInterpolator< TPI, TPO >::in_;
};

// The adaptive window convolution filter for adaptive gauss and its variants
template< typename TPI, typename TPO = FlexType< TPI >>
class AdaptiveWindowConvolutionLineFilter : public Framework::FullLineFilter {
   public:
      AdaptiveWindowConvolutionLineFilter( Image const& in, Kernel const& kernel, ImageArray const& params, String const& interpolation, BoundaryCondition bc, String const& transform )
            : in_( in ), kernel_( kernel ) {
         // Determine kernel transformation
         if( in_.Dimensionality() == 2 ) {
            // === 2D ===
            // Construct input interpolator
            ConstructInputInterpolator< 2 >( in, interpolation );

            // Construct kernel transformation
            ConstructKernelTransform2D( transform, params, in.TensorElements() );

         } else if( in_.Dimensionality() == 3 ) {
            // === 3D ===
            // Construct input interpolator
            ConstructInputInterpolator< 3 >( in, interpolation );

            // Determine kernel transformation
            ConstructKernelTransform3D( transform, params, in.TensorElements() );
         } else {
            DIP_THROW( "No transform \"" + transform + "\" known for input dimensionality " + std::to_string( in_.Dimensionality() ));
         }

         // Store boundary condition. We only support mirroring or zeros for now.
         DIP_THROW_IF( bc != BoundaryCondition::SYMMETRIC_MIRROR && bc != BoundaryCondition::ADD_ZEROS, "Unsupported boundary condition" );
         mirrorAtInputBoundaries_ = ( bc == BoundaryCondition::SYMMETRIC_MIRROR );
      }

      virtual void SetNumberOfThreads( dip::uint numThreads, PixelTableOffsets const& pixelTable ) override {
         offsets_ = pixelTable.Offsets();
         kernelTransforms_.resize( numThreads - 1 );  // kernelTransform_ is used for thread 0
      }

      virtual void Filter( Framework::FullLineFilterParameters const& params ) override {
         TPI* in = static_cast< TPI* >( params.inBuffer.buffer );
         dip::sint inStride = params.inBuffer.stride;
         TPO* out = static_cast< TPO* >( params.outBuffer.buffer );
         dip::sint outStride = params.outBuffer.stride;
         dip::sint outTensorStride = params.outBuffer.tensorStride;
         dip::uint length = params.bufferLength;
         PixelTableOffsets const& pixelTableOffsets = params.pixelTable;
         std::vector< dfloat > const& weights = pixelTableOffsets.Weights();
         UnsignedArray inCoords( params.position );
         PixelTable pixelTable = kernel_.PixelTable( in_.Dimensionality(), params.dimension );  // TODO: move to constructor
         FloatArray transformedKernelCoords( in_.Dimensionality() );

         // Obtain kernel transform for this thread
         std::unique_ptr< KernelTransform >& kernelTransform = params.thread == 0 ? kernelTransform_ : kernelTransforms_[ params.thread - 1 ];
         // Clone kernelTransform_ for this thread if not done already.
         // Needed because the KernelTransform computes and stores values per input pixel and uses those repeatedly for each kernel element.
         if( !kernelTransform ) {
            kernelTransform.reset( kernelTransform_->Clone() );
         }

         for( dip::uint ii = 0; ii < length; ++ii ) {
            for( dip::uint iTE = 0; iTE < in_.TensorElements(); ++iTE ) {
               *( out + static_cast< dip::sint >( iTE ) * outTensorStride ) = 0;
            }
            auto itWeight = weights.begin();
            // Prepare kernel transform for current input coordinates
            kernelTransform->SetImageCoords( inCoords );
            // Apply kernel
            for( PixelTable::iterator itPT = pixelTable.begin(); itPT != pixelTable.end(); ++itPT ) {
               for( dip::uint iTE = 0; iTE < in_.TensorElements(); ++iTE ) {
                  // Apply kernel transformation
                  kernelTransform->Transform( *itPT, iTE, transformedKernelCoords );
                  // Obtain input value at the transformed kernel coords
                  *( out + static_cast< dip::sint >( iTE ) * outTensorStride ) +=
                        inputInterpolator_->GetInputValue( transformedKernelCoords, iTE, mirrorAtInputBoundaries_ ) * static_cast< FloatType< TPO >>( *itWeight );
               }
               ++itWeight;
            }
            // Prepare next buffer element
            inCoords[ params.dimension ]++;
            in += inStride;
            out += outStride;
         }
      }

   private:
      template< dip::uint nDims >
      void ConstructInputInterpolator( Image const& in, String const& interpolation ) {
         // Determine input interpolator
         if( interpolation == S::ZERO_ORDER ) {
            inputInterpolator_ = std::make_unique< InputInterpolatorZOH< nDims, TPI, TPO >>( in );
         } else if( interpolation == S::LINEAR ) {
            inputInterpolator_ = std::make_unique< InputInterpolatorFOH< nDims, TPI, TPO >>( in );
         } else {
            DIP_THROW( "Unknown interpolation \"" + interpolation + "\"" );
         }
      }

      void ConstructKernelTransform2D( String const& transform, ImageArray const& params, dip::uint inputTensorElements ) {
         // Determine kernel transformation
         if( transform == "none" ) {
            kernelTransform_ = std::make_unique< KernelTransform >();
         } else if( transform == "ellipse" ) {
            if( params.size() == 1 ) {
               kernelTransform_ = std::make_unique< KernelTransform2DRotation >( params[ 0 ], inputTensorElements );
            } else if( params.size() == 2 ) {
               kernelTransform_ = std::make_unique< KernelTransform2DScaledRotation >( params[ 0 ], params[ 1 ], inputTensorElements );
            } else {
               DIP_THROW( E::ARRAY_PARAMETER_WRONG_LENGTH );
            }
         } else if( transform == "banana" ) {
            if( params.size() == 2 ) {
               kernelTransform_ = std::make_unique< KernelTransform2DBanana >( params[ 0 ], params[ 1 ], inputTensorElements );
            } else if( params.size() == 3 ) {
               kernelTransform_ = std::make_unique< KernelTransform2DScaledBanana >( params[ 0 ], params[ 1 ], params[ 2 ], inputTensorElements );
            } else {
               DIP_THROW( E::ARRAY_PARAMETER_WRONG_LENGTH );
            }
         } else if( transform == "skew" ) {
            DIP_THROW_IF( params.size() != 1, E::ARRAY_PARAMETER_WRONG_LENGTH );
            kernelTransform_ = std::make_unique< KernelTransform2DSkew >( params[ 0 ], inputTensorElements );
         } else {
            DIP_THROW( "Unknown 2D transform \"" + transform + "\"" );
         }
      }

      void ConstructKernelTransform3D( String const& transform, ImageArray const& params, dip::uint inputTensorElements ) {
         if( transform == "none" ) {
            kernelTransform_ = std::make_unique< KernelTransform >();
         } else if( transform == "ellipse" ) {
            if( params.size() == 2 ) {
               kernelTransform_ = std::make_unique< KernelTransform3DRotationZ >( params[ 0 ], params[ 1 ], inputTensorElements );
            } else if( params.size() == 4 ) {
               kernelTransform_ = std::make_unique< KernelTransform3DRotationXY >( params[ 0 ], params[ 1 ], params[ 2 ], params[ 3 ], inputTensorElements );
            }
         } else {
            DIP_THROW( "Unknown 3D transform \"" + transform + "\"" );
         }
      }

      std::vector< dip::sint > offsets_;  // Pixel table offsets
      Image const& in_; // Input image
      Kernel const& kernel_;  // Kernel
      std::unique_ptr< KernelTransform > kernelTransform_;  // Kernel transform for thread 0
      std::vector< std::unique_ptr< KernelTransform >> kernelTransforms_; // Kernel transforms for remaining threads
      std::unique_ptr< InputInterpolator< TPI, TPO >> inputInterpolator_; // Input interpolator
      bool mirrorAtInputBoundaries_;   // Boundary condition: either mirror or zeros
};

} // namespace

void AdaptiveFilter(
      Image const& in,
      ImageConstRefArray const& params,
      Image& out,
      FloatArray sigmas,
      UnsignedArray const& orders,
      dfloat truncation,
      UnsignedArray const& exponents,
      String const& interpolationMethod,
      String const& boundaryCondition,
      String const& transform
) {
   DIP_THROW_IF( !in.IsForged(), E::IMAGE_NOT_FORGED );

   // Prepare parameter images: expand singleton dimensions, including the tensor
   ImageArray paramImages( params.size() );
   for( dip::uint iP = 0; iP < params.size(); ++iP ) {
      paramImages[ iP ] = params[ iP ].get().QuickCopy();
      paramImages[ iP ].ExpandSingletonDimensions( in.Sizes() );
      // The param image's tensor is expanded while constructing the kernel transformation
      // TODO: ExpandSingletonDimensions() could also be done while constructing the kernel transformation, so this loop is removed altogether
   }
   DIP_STACK_TRACE_THIS( ArrayUseParameter( sigmas, in.Dimensionality(), 1.0 ));

   DIP_START_STACK_TRACE
      // Create gaussian kernel
      Kernel kernel{ CreateGauss( sigmas, orders, truncation, exponents ) };

      BoundaryCondition bc = StringToBoundaryCondition( boundaryCondition );
      DataType outputType = DataType::SuggestFlex( in.DataType() );
      std::unique_ptr< Framework::FullLineFilter > lineFilter;
      DIP_OVL_NEW_ALL( lineFilter, AdaptiveWindowConvolutionLineFilter, ( in, kernel, paramImages, interpolationMethod, bc, transform ), in.DataType() );
      // We use the full framework to allow multi-threading. Its parameters prevent input or output buffering to minimize overhead. Border expansion is not used either.
      Framework::Full( in, out, in.DataType(), outputType, outputType, in.TensorElements(), { bc }, kernel, *lineFilter, Framework::FullOption::BorderAlreadyExpanded );// for performance comparisons: +Framework::FullOption::NoMultiThreading );

   DIP_END_STACK_TRACE
}

void AdaptiveGauss(
      Image const& in,
      ImageConstRefArray const& params,
      Image& out,
      FloatArray const& sigmas,
      UnsignedArray const& orders,
      dfloat truncation,
      UnsignedArray const& exponents,
      String const& interpolationMethod,
      String const& boundaryCondition
) {
   AdaptiveFilter( in, params, out, sigmas, orders, truncation, exponents, interpolationMethod, boundaryCondition, "ellipse" );
}

void AdaptiveBanana(
      Image const& in,
      ImageConstRefArray const& params,
      Image& out,
      FloatArray const& sigmas,
      UnsignedArray const& orders,
      dfloat truncation,
      UnsignedArray const& exponents,
      String const& interpolationMethod,
      String const& boundaryCondition
) {
   AdaptiveFilter( in, params, out, sigmas, orders, truncation, exponents, interpolationMethod, boundaryCondition, "banana" );
}

} // namespace dip

#if defined(__GNUG__) || defined(__clang__)
#pragma GCC diagnostic pop
#endif

max_stars_repo_name: KDAB/diplib

max_issues_repo_name: KDAB/diplib

max_forks_repo_name: KDAB/diplib


text:
/*

    HUANG Jing
    haungjing@mae.cuhk.edu.hk
    Partially based on correll lab's script http://104.236.202.16/?p=3064.

*/

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/photo.hpp>    // denoising module
#include <opencv2/calib3d.hpp>  // solvePnP
#include <iostream>
#include <algorithm> // for std::max_element()
#include <vector>
#include <math.h>
#include <iterator>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include "dvrk_retraction/visual_feedback.h"
#include "dvrk_retraction/visual_info_srv.h"

// namespace enc = sensor_msgs::image_encodings;

static const char WINDOW[] = "New OpenCV Image";

bool compareCvPoint_x(cv::Point point1, cv::Point point2)
{
  return (point1.x > point2.x);
}

bool compareCvPoint_y(cv::Point point1, cv::Point point2)
{
  return (point1.y < point2.y);
}

class ImageConverter
{
    ros::NodeHandle nh_;
    ros::Publisher image_info_pub = nh_.advertise<dvrk_retraction::visual_feedback>("visual_info", 1000);
    ros::ServiceServer visual_info_service = nh_.advertiseService("vusual_info_srv", &ImageConverter::get_visual_info_srv, this);
    image_transport::ImageTransport it_;
    image_transport::Subscriber image_sub_;
    image_transport::Publisher image_pub_;
    float focal_x = 683;
    float focal_y = 683;
    float depth = 58;
public:
    std::vector<float> class_centroid;
    std::vector<float> class_rotate_origin;
    std::vector<float> class_principal_axis;
    float class_rotate_angle;
    std::vector<float> class_target_region_center;
    float class_target_region_radius;

public:
    ImageConverter(char* ros_image_stream): it_(nh_)
    {
        image_pub_ = it_.advertise("image_processed", 1);
        image_sub_ = it_.subscribe(ros_image_stream, 1, &ImageConverter::imageCb, this);
        cv::namedWindow(WINDOW);
    }

    ~ImageConverter()
    {
        cv::destroyWindow(WINDOW);
    }

    void imageCb(const sensor_msgs::ImageConstPtr& msg)
    {
      cv_bridge::CvImagePtr cv_ptr;
      try
      {
        //std::cout << "I am trying" << std::endl;
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
      }
      catch (cv_bridge::Exception& e)
      {
        std::cout << "ROS image msg conversion failed." << std::endl;
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
      }

    cv::Point imageOrigin(320, 240), xAxisEnd(220, 240), yAxisEnd(320, 340);
    cv::Scalar redLow(0, 173, 152), redHigh(10, 255, 255), blackLow(0, 0, 0), blackHigh(149, 218, 71);
    std::vector< std::vector<cv::Point>> contours;
    cv::Mat trackedImage, trackedImage_denoised, dst, trackedImage_undist;
    cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 683.9731, 0, 320., 0, 683.9731, 240, 0, 0, 1);
    cv::Mat distCoef = (cv::Mat_<double>(1, 5) << -0.7175, 2.8528, 0, 0, -5.1548);
    dvrk_retraction::visual_feedback image_info_msg;

    cv::cvtColor(cv_ptr->image, trackedImage, CV_BGR2HSV);
    // cv::fastNlMeansDenoisingColored(trackedImage,trackedImage_denoised, 3, 3, 7, 21); // This function costs much time.
    cv::blur(trackedImage, trackedImage, cv::Size(3, 3));   // blurring is very important to get rid of noise.
    // cv::inRange(trackedImage, redLow, redHigh, dst);
    // cv::imshow("Binary Image", dst);

    //*** Use bgr difference to generate binary image rather than inRange()-START ***//
    cv::Mat bgr_thr = cv::Mat::zeros(trackedImage.size(), CV_8UC1);
    int Rows = bgr_thr.rows, Cols = bgr_thr.cols;
    for (int r = 0; r < Rows; r ++)
        for (int c = 0; c < Cols; c++)
        {
            cv::Vec3b intensity = cv_ptr->image.at<cv::Vec3b>(cv::Point(c, r));
            if (intensity[2] - intensity[0] > 80 && intensity[2] - intensity[1] > 80)
            {
                bgr_thr.at<uchar>(cv::Point(c, r)) = 255;
            }
        }
    dst = bgr_thr;
    //*** Use bgr difference to generate binary image rather than inRange()-END ***//

    cv::Moments m_dst = moments(dst, true);
    cv::Point p(m_dst.m10 / m_dst.m00, m_dst.m01 / m_dst.m00);
    if (p.x < 0 || p.y < 0)
    {
      std::cout << "No feature shape detected." << std::endl;
    }
    else
    {
      cv::findContours(dst, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE, cv::Point(0, 0));
      int largestContourIndex = 0;
      for (int i = 0; i < contours.size(); i++)
        largestContourIndex = (contours[i].size() > contours[largestContourIndex].size()) ? i : largestContourIndex;

      cv::Moments shapeMoments = moments(contours[largestContourIndex]);
      cv::Point2f shapeCenter(static_cast<float>(shapeMoments.m10 / shapeMoments.m00),
                              static_cast<float>(shapeMoments.m01 / shapeMoments.m00));
      Eigen::Matrix2f I;        // inertia tensor
      I <<  shapeMoments.mu20, shapeMoments.mu11,
            shapeMoments.mu11, shapeMoments.mu02;
      Eigen::EigenSolver<Eigen::Matrix2f> es(I);
      Eigen::Vector2d eigenVector1(es.eigenvectors().col(0).real()[0], es.eigenvectors().col(0).real()[1]),
                      eigenVector2(es.eigenvectors().col(1).real()[0], es.eigenvectors().col(1).real()[1]);

      // ***The procedure is to get rid of the possible random switch of the principal axis display.
      // ***There is a need of look at the characters of the retrun eigenvectors first to know how to
      // ***better set the processing.
      if (eigenVector1[0] < 0 )
      {
        Eigen::Vector2d tempVector = eigenVector1;
        eigenVector1 = eigenVector2;
      }
      eigenVector2 = Eigen::Vector2d(eigenVector1[1], -eigenVector1[0]);

      cv::RotatedRect rectShape = cv::minAreaRect(contours[largestContourIndex]);
      cv::Point2f rectShapeVertices[4];
      rectShape.points(rectShapeVertices);    // order of vertices: bottomLeft, topLeft, topRight, bottomRight. And the bottom
                                              // is the most bottom.

      std::vector<float> rectLine0_1{rectShapeVertices[0].x - rectShapeVertices[1].x,
                                     rectShapeVertices[0].y - rectShapeVertices[1].y };
      std::vector<float> rectLine1_2{rectShapeVertices[2].x - rectShapeVertices[1].x,
                                     rectShapeVertices[2].y - rectShapeVertices[1].y };
      float rectLineLengthSquare0_1 = pow(rectLine0_1[0], 2) + pow(rectLine0_1[1], 2),
            rectLineLengthSquare1_2 = pow(rectLine1_2[0], 2) + pow(rectLine1_2[1], 2);
      cv::Point2f rotateOrigin;
      //if (rectShapeVertices[2].x > rectShapeVertices[0].x)
      if (rectLineLengthSquare0_1 < rectLineLengthSquare1_2)
      {
        rotateOrigin.x = (rectShapeVertices[0].x + rectShapeVertices[1].x) / 2;
        rotateOrigin.y = (rectShapeVertices[0].y + rectShapeVertices[1].y) / 2;
      }
      else
      {
        rotateOrigin.x = (rectShapeVertices[1].x + rectShapeVertices[2].x) / 2;
        rotateOrigin.y = (rectShapeVertices[1].y + rectShapeVertices[2].y) / 2;
      }

      //*** Drawing part1-START ***//
      for (int i = 0; i < 4; i++)  // draw the rectangle
      {
        cv::line(cv_ptr->image, rectShapeVertices[i], rectShapeVertices[(i+1) % 4], cv::Scalar(0, 255, 0), 2);
      }
      // cv::circle(cv_ptr->image, rotateOrigin, 8, cv::Scalar(155, 245, 66), -1);
      cv::Point2f principleAxie1 = shapeCenter + 80 * cv::Point2f( eigenVector1[0], eigenVector1[1]),
                  principleAxie2 = shapeCenter + 80 * cv::Point2f( eigenVector2[0], eigenVector2[1]);
      // cv::arrowedLine(cv_ptr->image, shapeCenter, principleAxie1, cv::Scalar(0, 0, 255), 2);
      // cv::arrowedLine(cv_ptr->image, shapeCenter, principleAxie2, cv::Scalar(0, 255, 0), 2);
      cv::circle(cv_ptr->image, shapeCenter, 5, cv::Scalar(0, 0, 0), -1);
      cv::drawContours(cv_ptr->image, contours, largestContourIndex, cv::Scalar(255, 0, 0), 2);
      //*** Drawing part1-END ***//

      //*** get the posture of the attached plane ***//
      std::cout << "This is test for finding extreme points: \n"
                << (*std::max_element(contours[largestContourIndex].begin(), contours[largestContourIndex].end(),
                    compareCvPoint_x)).x
                << std::endl;
      std::vector<cv::Point>::iterator leftestVertexItr = std::max_element(contours[largestContourIndex].begin(),
                                                                            contours[largestContourIndex].end(),
                                                                            compareCvPoint_x);
      cv::Point2f leftestVertex = *leftestVertexItr,
                  vertexRef1 = *(leftestVertexItr + 20),
                  vertexRef2 = *(leftestVertexItr - 20),  // Index of Point in contours increases anticlosewise.
                  axisXref = *(leftestVertexItr + 35) - *(leftestVertexItr + 15),
                  axisYref = *(leftestVertexItr - 35) - *(leftestVertexItr - 15),
                  axisZref;
      axisXref = axisXref / (sqrt(pow(axisXref.x, 2) + pow(axisXref.y, 2))), // normalize
      axisYref = axisYref / (sqrt(pow(axisYref.x, 2) + pow(axisYref.y, 2)));
      cv::Point2f objectPoint1 = shapeCenter + 50 * axisXref,
                  objectPoint2 = shapeCenter + 50 * axisYref,
                  objectPoint3;
      /* cv::Mat objectPointsPnP = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1),
             // imagePointsPnP = (cv::Mat_<double>(2, 3) << objectPoint1.x, objectPoint2.x, (objectPoint1.x + objectPoint2.x)-20,
             //                                             objectPoint1.y, objectPoint2.y, (objectPoint1.y + objectPoint2.y)/2),
              rvecPnP, tvecPnP;
      std::vector<cv::Point2f> imagePointsPnP{objectPoint1, objectPoint2, (objectPoint1 + objectPoint2)/2};
      // cv::solvePnPRansac(objectPointsPnP, imagePointsPnP, cameraMatrix, distCoef, rvecPnP, tvecPnP);
      std::cout << rvecPnP << "\nPnP\n" << tvecPnP << std::endl; */
      Eigen::Matrix3d cameraMatrixEigen;
      for(int i = 0; i < 3; ++i)
        for(int j= 0; j < 3; ++j)
          cameraMatrixEigen(i, j) = cameraMatrix.at<double>(i, j);

      double  f_u = cameraMatrix.at<double>(0, 0), f_v = cameraMatrix.at<double>(1, 1),
              s_u = cameraMatrix.at<double>(0, 2), s_v = cameraMatrix.at<double>(1, 2),
              z_1 = 0.8, x_1 = (objectPoint1.x - s_u) * z_1 / f_u,
                        y_1 = (objectPoint1.y - s_v) * z_1 / f_v,
              z_2 = 0.82, x_2 = (objectPoint2.x - s_u) * z_2 / f_u,
                          y_2 = (objectPoint2.y - s_v) * z_2 / f_v,
              x_3, y_3, z_3;
      Eigen::Vector3d planeXCamera(x_1, y_1, z_1), planeYCamera(x_2, y_2, z_2),
                      planeZCamera = planeXCamera.cross(planeYCamera),
                      imageZPoint;
      planeZCamera = planeZCamera / planeZCamera(2);
      imageZPoint = cameraMatrixEigen * planeZCamera;
      objectPoint3.x = imageZPoint(0);
      objectPoint3.y = imageZPoint(1);
      axisZref = objectPoint3 / sqrt(pow(objectPoint3.x, 2) + pow(objectPoint3.y, 2));

      cv::circle(cv_ptr->image, leftestVertex, 5, cv::Scalar(255, 255, 255), -1);
      cv::circle(cv_ptr->image, vertexRef1, 5, cv::Scalar(0, 0, 255), -1);
      cv::circle(cv_ptr->image, vertexRef2, 5, cv::Scalar(255, 0, 0), -1);
      cv::arrowedLine(cv_ptr->image, shapeCenter, shapeCenter + 50 * axisXref, cv::Scalar(100, 120, 150), 3);
      cv::arrowedLine(cv_ptr->image, shapeCenter, shapeCenter + 50 * axisYref, cv::Scalar(50, 60, 70), 3);
      cv::arrowedLine(cv_ptr->image, shapeCenter, shapeCenter + 50 * axisZref, cv::Scalar(150, 100, 70), 3);

      //*** visual_feedback ros message ***//
      double rotateAngle = acos(eigenVector1[0]) / M_PI * 180;   // eigenvector is given with norm = 1.
      double targetAngle = 45;
      cv::Point2f rotateRadius = shapeCenter - rotateOrigin;
      Eigen::Vector2d rotateRadiusVec(rotateRadius.x, rotateRadius.y);
      cv::Point2f targetRegionCenterVec(rotateRadiusVec.norm() * cos(targetAngle / 180 * M_PI),
                                        -rotateRadiusVec.norm() * sin(targetAngle / 180 * M_PI));
      cv::Point2f targetRegionCenter = rotateOrigin + targetRegionCenterVec;
      const double targetRegionRadius = 15;
      // cv::circle(cv_ptr->image, targetRegionCenter, targetRegionRadius, cv::Scalar(155, 245, 66), 2);
      image_info_msg.centroid.push_back(shapeCenter.x);
      image_info_msg.centroid.push_back(shapeCenter.y);
      image_info_msg.rotate_origin.push_back(rotateOrigin.x);
      image_info_msg.rotate_origin.push_back(rotateOrigin.y);
      image_info_msg.principal_axis.push_back(eigenVector1[0]);
      image_info_msg.principal_axis.push_back(eigenVector1[1]);
      image_info_msg.rotate_angle = rotateAngle;
      image_info_msg.target_angle = targetAngle;
      image_info_msg.target_region_center.push_back(targetRegionCenter.x);
      image_info_msg.target_region_center.push_back(targetRegionCenter.y);
      image_info_msg.target_region_radius = targetRegionRadius;

      std::cout << "visual info: \nrotate angle: " << rotateAngle << std::endl;

      //***** visual_info_srv response assignment*****
      class_centroid.clear();
      class_rotate_origin.clear();
      class_principal_axis.clear();
      class_target_region_center.clear();

      class_centroid.push_back(shapeCenter.x);
      class_centroid.push_back(shapeCenter.y);
      class_rotate_origin.push_back(rotateOrigin.x);
      class_rotate_origin.push_back(rotateOrigin.y);
      class_principal_axis.push_back(eigenVector1[0]);
      class_principal_axis.push_back(eigenVector1[1]);
      class_rotate_angle = rotateAngle;
      class_target_region_center.push_back(targetRegionCenter.x);
      class_target_region_center.push_back(targetRegionCenter.y);
      class_target_region_radius = targetRegionRadius;

    }

    // cv::circle(cv_ptr->image, imageOrigin, 5, cv::Scalar(0, 0, 0), -1);
    // cv::arrowedLine(cv_ptr->image, imageOrigin, xAxisEnd, cv::Scalar(0, 0, 255), 2);
    // cv::putText(cv_ptr->image, "x", xAxisEnd - cv::Point(10, 10), cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 0, 255), 2);
    // cv::arrowedLine(cv_ptr->image, imageOrigin, yAxisEnd, cv::Scalar(0, 255, 0), 2);
    // cv::putText(cv_ptr->image, "y", yAxisEnd + cv::Point(10, 10), cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 255, 0), 2);

   // end of processing
    cv::imshow(WINDOW, cv_ptr->image);
    cv::waitKey(3);

    // image_pub_.publish(cv_ptr->toImageMsg());
    image_info_pub.publish(image_info_msg);
    }

  bool get_visual_info_srv(dvrk_retraction::visual_info_srv::Request &req,
                          dvrk_retraction::visual_info_srv::Response &res)
  {
    res.centroid.push_back(class_centroid[0]);
    res.centroid.push_back(class_centroid[1]);
    res.rotate_origin.push_back(class_rotate_origin[0]);
    res.rotate_origin.push_back(class_rotate_origin[1]);
    res.principal_axis.push_back(class_principal_axis[0]);
    res.principal_axis.push_back(class_principal_axis[1]);
    res.rotate_angle = class_rotate_angle;
    res.target_region_center.push_back(class_target_region_center[0]);
    res.target_region_center.push_back(class_target_region_center[1]);
    res.target_region_radius = class_target_region_radius;
    ROS_INFO("Successfully obtain visual information.\n");
    return true;
  }
};

int main(int argc, char** argv)
{
  if (argc == 2)
  {
    ros::init(argc, argv, "image_processing");
    ImageConverter ic(argv[1]);
    ros::spin();
    return 0;
  }
  else if (argc == 1)
  {
  // set default ros image source
    char *image_topic = (char*) "/cameras/source_camera/image";
    ros::init(argc, argv, "image_processing");
    ImageConverter ic(image_topic);
    ros::spin();
    return 1;
  }
  else
  {
    std::cout << "ERROR:\tusage - RosToOpencvImage <ros_image_topic>" << std::endl;
    return 1;
  }
}