/**
 * @file    testGaussianISAM2.cpp
 * @brief   Unit tests for GaussianISAM2
 * @author  Michael Kaess
 */

#include <CppUnitLite/TestHarness.h>

#include <tests/smallExample.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <boost/assign/list_of.hpp>
#include <gtsam/base/deprecated/LieScalar.h>
using namespace boost::assign;
#include <boost/range/adaptor/map.hpp>
namespace br { using namespace boost::adaptors; using namespace boost::range; }

using namespace std;
using namespace gtsam;
using boost::shared_ptr;

static const SharedNoiseModel model;
static const LieScalar Zero(0);

//  SETDEBUG("ISAM2 update", true);
//  SETDEBUG("ISAM2 update verbose", true);
//  SETDEBUG("ISAM2 recalculate", true);

// Set up parameters
SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0).finished());
SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1).finished());

ISAM2 createSlamlikeISAM2(
    boost::optional<Values&> init_values = boost::none,
    boost::optional<NonlinearFactorGraph&> full_graph = boost::none,
    const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true),
    size_t maxPoses = 10) {

  // These variables will be reused and accumulate factors and values
  ISAM2 isam(params);
  Values fullinit;
  NonlinearFactorGraph fullgraph;

  // i keeps track of the time step
  size_t i = 0;

  // Add a prior at time 0 and update isam
  {
    NonlinearFactorGraph newfactors;
    newfactors += PriorFactor<Pose2>(0, Pose2(0.0, 0.0, 0.0), odoNoise);
    fullgraph.push_back(newfactors);

    Values init;
    init.insert((0), Pose2(0.01, 0.01, 0.01));
    fullinit.insert((0), Pose2(0.01, 0.01, 0.01));

    isam.update(newfactors, init);
  }

  if(i > maxPoses)
    goto done;

  // Add odometry from time 0 to time 5
  for( ; i<5; ++i) {
    NonlinearFactorGraph newfactors;
    newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
    fullgraph.push_back(newfactors);

    Values init;
    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));

    isam.update(newfactors, init);

    if(i > maxPoses)
      goto done;
  }

  if(i > maxPoses)
    goto done;

  // Add odometry from time 5 to 6 and landmark measurement at time 5
  {
    NonlinearFactorGraph newfactors;
    newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
    newfactors += BearingRangeFactor<Pose2,Point2>(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
    newfactors += BearingRangeFactor<Pose2,Point2>(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
    fullgraph.push_back(newfactors);

    Values init;
    init.insert((i+1), Pose2(1.01, 0.01, 0.01));
    init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
    init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
    fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
    fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
    fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));

    isam.update(newfactors, init);
    ++ i;
  }

  if(i > maxPoses)
    goto done;

  // Add odometry from time 6 to time 10
  for( ; i<10; ++i) {
    NonlinearFactorGraph newfactors;
    newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
    fullgraph.push_back(newfactors);

    Values init;
    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));

    isam.update(newfactors, init);

    if(i > maxPoses)
      goto done;
  }

  if(i > maxPoses)
    goto done;

  // Add odometry from time 10 to 11 and landmark measurement at time 10
  {
    NonlinearFactorGraph newfactors;
    newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
    newfactors += BearingRangeFactor<Pose2,Point2>(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
    newfactors += BearingRangeFactor<Pose2,Point2>(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
    fullgraph.push_back(newfactors);

    Values init;
    init.insert((i+1), Pose2(6.9, 0.1, 0.01));
    fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));

    isam.update(newfactors, init);
    ++ i;
  }

done:

  if (full_graph)
    *full_graph = fullgraph;

  if (init_values)
    *init_values = fullinit;

  return isam;
}

/* ************************************************************************* */
//TEST(ISAM2, CheckRelinearization) {
//
//  typedef GaussianISAM2<Values>::Impl Impl;
//
//  // Create values where indices 1 and 3 are above the threshold of 0.1
//  VectorValues values;
//  values.reserve(4, 10);
//  values.push_back_preallocated(Vector2(0.09, 0.09));
//  values.push_back_preallocated(Vector3(0.11, 0.11, 0.09));
//  values.push_back_preallocated(Vector3(0.09, 0.09, 0.09));
//  values.push_back_preallocated(Vector2(0.11, 0.11));
//
//  // Create a permutation
//  Permutation permutation(4);
//  permutation[0] = 2;
//  permutation[1] = 0;
//  permutation[2] = 1;
//  permutation[3] = 3;
//
//  Permuted<VectorValues> permuted(permutation, values);
//
//  // After permutation, the indices above the threshold are 2 and 2
//  KeySet expected;
//  expected.insert(2);
//  expected.insert(3);
//
//  // Indices checked by CheckRelinearization
//  KeySet actual = Impl::CheckRelinearization(permuted, 0.1);
//
//  EXPECT(assert_equal(expected, actual));
//}

/* ************************************************************************* */
struct ConsistencyVisitor
{
  bool consistent;
  const ISAM2& isam;
  ConsistencyVisitor(const ISAM2& isam) :
    consistent(true), isam(isam) {}
  int operator()(const ISAM2::sharedClique& node, int& parentData)
  {
    if(find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end())
    {
      if(node->parent_.expired())
        consistent = false;
      if(find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end())
        consistent = false;
    }
    for(Key j: node->conditional()->frontals())
    {
      if(isam.nodes().at(j).get() != node.get())
        consistent = false;
    }
    return 0;
  }
};

/* ************************************************************************* */
bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) {

  TestResult& result_ = result;
  const string name_ = test.getName();

  Values actual = isam.calculateEstimate();
  Values expected = fullinit.retract(fullgraph.linearize(fullinit)->optimize());

  bool isamEqual = assert_equal(expected, actual);

  // Check information
  GaussianFactorGraph isamGraph(isam);
  isamGraph += isam.roots().front()->cachedFactor_;
  Matrix expectedHessian = fullgraph.linearize(isam.getLinearizationPoint())->augmentedHessian();
  Matrix actualHessian = isamGraph.augmentedHessian();
  expectedHessian.bottomRightCorner(1,1) = actualHessian.bottomRightCorner(1,1);
  bool isamTreeEqual = assert_equal(expectedHessian, actualHessian);

  // Check consistency
  ConsistencyVisitor visitor(isam);
  int data; // Unused
  treeTraversal::DepthFirstForest(isam, data, visitor);
  bool consistent = visitor.consistent;

  // The following two checks make sure that the cached gradients are maintained and used correctly

  // Check gradient at each node
  bool nodeGradientsOk = true;
  typedef ISAM2::sharedClique sharedClique;
  for(const sharedClique& clique: isam.nodes() | br::map_values) {
    // Compute expected gradient
    GaussianFactorGraph jfg;
    jfg += clique->conditional();
    VectorValues expectedGradient = jfg.gradientAtZero();
    // Compare with actual gradients
    DenseIndex variablePosition = 0;
    for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
      const DenseIndex dim = clique->conditional()->getDim(jit);
      Vector actual = clique->gradientContribution().segment(variablePosition, dim);
      bool gradOk = assert_equal(expectedGradient[*jit], actual);
      EXPECT(gradOk);
      nodeGradientsOk = nodeGradientsOk && gradOk;
      variablePosition += dim;
    }
    bool dimOk = clique->gradientContribution().rows() == variablePosition;
    EXPECT(dimOk);
    nodeGradientsOk = nodeGradientsOk && dimOk;
  }

  // Check gradient
  VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero();
  VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient));
  VectorValues actualGradient = isam.gradientAtZero();
  bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient);
  EXPECT(expectedGradOk);
  bool totalGradOk = assert_equal(expectedGradient, actualGradient);
  EXPECT(totalGradOk);

  return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual && isamTreeEqual && consistent;
}

/* ************************************************************************* */
TEST(ISAM2, AddFactorsStep1)
{
  NonlinearFactorGraph nonlinearFactors;
  nonlinearFactors += PriorFactor<LieScalar>(10, Zero, model);
  nonlinearFactors += NonlinearFactor::shared_ptr();
  nonlinearFactors += PriorFactor<LieScalar>(11, Zero, model);

  NonlinearFactorGraph newFactors;
  newFactors += PriorFactor<LieScalar>(1, Zero, model);
  newFactors += PriorFactor<LieScalar>(2, Zero, model);

  NonlinearFactorGraph expectedNonlinearFactors;
  expectedNonlinearFactors += PriorFactor<LieScalar>(10, Zero, model);
  expectedNonlinearFactors += PriorFactor<LieScalar>(1, Zero, model);
  expectedNonlinearFactors += PriorFactor<LieScalar>(11, Zero, model);
  expectedNonlinearFactors += PriorFactor<LieScalar>(2, Zero, model);

  const FactorIndices expectedNewFactorIndices = list_of(1)(3);

  FactorIndices actualNewFactorIndices;

  ISAM2::Impl::AddFactorsStep1(newFactors, true, nonlinearFactors, actualNewFactorIndices);

  EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors));
  EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices));
}

/* ************************************************************************* */
TEST(ISAM2, simple)
{
  for(size_t i = 0; i < 10; ++i) {
    // These variables will be reused and accumulate factors and values
    Values fullinit;
    NonlinearFactorGraph fullgraph;
    ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false), i);

    // Compare solutions
    EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_));
  }
}

/* ************************************************************************* */
TEST(ISAM2, slamlike_solution_gaussnewton)
{
  // These variables will be reused and accumulate factors and values
  Values fullinit;
  NonlinearFactorGraph fullgraph;
  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));

  // Compare solutions
  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
}

/* ************************************************************************* */
TEST(ISAM2, slamlike_solution_dogleg)
{
  // These variables will be reused and accumulate factors and values
  Values fullinit;
  NonlinearFactorGraph fullgraph;
  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));

  // Compare solutions
  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
}

/* ************************************************************************* */
TEST(ISAM2, slamlike_solution_gaussnewton_qr)
{
  // These variables will be reused and accumulate factors and values
  Values fullinit;
  NonlinearFactorGraph fullgraph;
  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR));

  // Compare solutions
  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
}

/* ************************************************************************* */
TEST(ISAM2, slamlike_solution_dogleg_qr)
{
  // These variables will be reused and accumulate factors and values
  Values fullinit;
  NonlinearFactorGraph fullgraph;
  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR));

  // Compare solutions
  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
}

/* ************************************************************************* */
TEST(ISAM2, clone) {

  ISAM2 clone1;

  {
    ISAM2 isam = createSlamlikeISAM2();
    clone1 = isam;

    ISAM2 clone2(isam);

    // Modify original isam
    NonlinearFactorGraph factors;
    factors += BetweenFactor<Pose2>(0, 10,
        isam.calculateEstimate<Pose2>(0).between(isam.calculateEstimate<Pose2>(10)), noiseModel::Unit::Create(3));
    isam.update(factors);

    CHECK(assert_equal(createSlamlikeISAM2(), clone2));
  }

  // This is to (perhaps unsuccessfully) try to currupt unallocated memory referenced
  // if the references in the iSAM2 copy point to the old instance which deleted at
  // the end of the {...} section above.
  ISAM2 temp = createSlamlikeISAM2();

  CHECK(assert_equal(createSlamlikeISAM2(), clone1));
  CHECK(assert_equal(clone1, temp));

  // Check clone empty
  ISAM2 isam;
  clone1 = isam;
  CHECK(assert_equal(ISAM2(), clone1));
}

/* ************************************************************************* */
TEST(ISAM2, removeFactors)
{
  // This test builds a graph in the same way as the "slamlike" test above, but
  // then removes the 2nd-to-last landmark measurement

  // These variables will be reused and accumulate factors and values
  Values fullinit;
  NonlinearFactorGraph fullgraph;
  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));

  // Remove the 2nd measurement on landmark 0 (Key 100)
  FactorIndices toRemove;
  toRemove.push_back(12);
  isam.update(NonlinearFactorGraph(), Values(), toRemove);

  // Remove the factor from the full system
  fullgraph.remove(12);

  // Compare solutions
  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
}

/* ************************************************************************* */
TEST(ISAM2, removeVariables)
{
  // These variables will be reused and accumulate factors and values
  Values fullinit;
  NonlinearFactorGraph fullgraph;
  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));

  // Remove the measurement on landmark 0 (Key 100)
  FactorIndices toRemove;
  toRemove.push_back(7);
  toRemove.push_back(14);
  isam.update(NonlinearFactorGraph(), Values(), toRemove);

  // Remove the factors and variable from the full system
  fullgraph.remove(7);
  fullgraph.remove(14);
  fullinit.erase(100);

  // Compare solutions
  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
}

/* ************************************************************************* */
TEST(ISAM2, swapFactors)
{
  // This test builds a graph in the same way as the "slamlike" test above, but
  // then swaps the 2nd-to-last landmark measurement with a different one

  Values fullinit;
  NonlinearFactorGraph fullgraph;
  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph);

  // Remove the measurement on landmark 0 and replace with a different one
  {
    size_t swap_idx = isam.getFactorsUnsafe().size()-2;
    FactorIndices toRemove;
    toRemove.push_back(swap_idx);
    fullgraph.remove(swap_idx);

    NonlinearFactorGraph swapfactors;
//    swapfactors += BearingRange<Pose2,Point2>(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise; // original factor
    swapfactors += BearingRangeFactor<Pose2,Point2>(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise);
    fullgraph.push_back(swapfactors);
    isam.update(swapfactors, Values(), toRemove);
  }

  // Compare solutions
  EXPECT(assert_equal(fullgraph, NonlinearFactorGraph(isam.getFactorsUnsafe())));
  EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_));

  // Check gradient at each node
  typedef ISAM2::sharedClique sharedClique;
  for(const sharedClique& clique: isam.nodes() | br::map_values) {
    // Compute expected gradient
    GaussianFactorGraph jfg;
    jfg += clique->conditional();
    VectorValues expectedGradient = jfg.gradientAtZero();
    // Compare with actual gradients
    DenseIndex variablePosition = 0;
    for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
      const DenseIndex dim = clique->conditional()->getDim(jit);
      Vector actual = clique->gradientContribution().segment(variablePosition, dim);
      EXPECT(assert_equal(expectedGradient[*jit], actual));
      variablePosition += dim;
    }
    EXPECT_LONGS_EQUAL((long)clique->gradientContribution().rows(), (long)variablePosition);
  }

  // Check gradient
  VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero();
  VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient));
  VectorValues actualGradient = isam.gradientAtZero();
  EXPECT(assert_equal(expectedGradient2, expectedGradient));
  EXPECT(assert_equal(expectedGradient, actualGradient));
}

/* ************************************************************************* */
TEST(ISAM2, constrained_ordering)
{
  // These variables will be reused and accumulate factors and values
  ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
  Values fullinit;
  NonlinearFactorGraph fullgraph;

  // We will constrain x3 and x4 to the end
  FastMap<Key, int> constrained;
  constrained.insert(make_pair((3), 1));
  constrained.insert(make_pair((4), 2));

  // i keeps track of the time step
  size_t i = 0;

  // Add a prior at time 0 and update isam
  {
    NonlinearFactorGraph newfactors;
    newfactors += PriorFactor<Pose2>(0, Pose2(0.0, 0.0, 0.0), odoNoise);
    fullgraph.push_back(newfactors);

    Values init;
    init.insert((0), Pose2(0.01, 0.01, 0.01));
    fullinit.insert((0), Pose2(0.01, 0.01, 0.01));

    isam.update(newfactors, init);
  }

  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));

  // Add odometry from time 0 to time 5
  for( ; i<5; ++i) {
    NonlinearFactorGraph newfactors;
    newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
    fullgraph.push_back(newfactors);

    Values init;
    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));

    if(i >= 3)
      isam.update(newfactors, init, FactorIndices(), constrained);
    else
      isam.update(newfactors, init);
  }

  // Add odometry from time 5 to 6 and landmark measurement at time 5
  {
    NonlinearFactorGraph newfactors;
    newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
    newfactors += BearingRangeFactor<Pose2,Point2>(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
    newfactors += BearingRangeFactor<Pose2,Point2>(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
    fullgraph.push_back(newfactors);

    Values init;
    init.insert((i+1), Pose2(1.01, 0.01, 0.01));
    init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
    init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
    fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
    fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
    fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));

    isam.update(newfactors, init, FactorIndices(), constrained);
    ++ i;
  }

  // Add odometry from time 6 to time 10
  for( ; i<10; ++i) {
    NonlinearFactorGraph newfactors;
    newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
    fullgraph.push_back(newfactors);

    Values init;
    init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
    fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));

    isam.update(newfactors, init, FactorIndices(), constrained);
  }

  // Add odometry from time 10 to 11 and landmark measurement at time 10
  {
    NonlinearFactorGraph newfactors;
    newfactors += BetweenFactor<Pose2>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
    newfactors += BearingRangeFactor<Pose2,Point2>(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
    newfactors += BearingRangeFactor<Pose2,Point2>(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
    fullgraph.push_back(newfactors);

    Values init;
    init.insert((i+1), Pose2(6.9, 0.1, 0.01));
    fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));

    isam.update(newfactors, init, FactorIndices(), constrained);
    ++ i;
  }

  // Compare solutions
  EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_));

  // Check gradient at each node
  typedef ISAM2::sharedClique sharedClique;
  for(const sharedClique& clique: isam.nodes() | br::map_values) {
    // Compute expected gradient
    GaussianFactorGraph jfg;
    jfg += clique->conditional();
    VectorValues expectedGradient = jfg.gradientAtZero();
    // Compare with actual gradients
    DenseIndex variablePosition = 0;
    for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
      const DenseIndex dim = clique->conditional()->getDim(jit);
      Vector actual = clique->gradientContribution().segment(variablePosition, dim);
      EXPECT(assert_equal(expectedGradient[*jit], actual));
      variablePosition += dim;
    }
    LONGS_EQUAL((long)clique->gradientContribution().rows(), (long)variablePosition);
  }

  // Check gradient
  VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero();
  VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient));
  VectorValues actualGradient = isam.gradientAtZero();
  EXPECT(assert_equal(expectedGradient2, expectedGradient));
  EXPECT(assert_equal(expectedGradient, actualGradient));
}

/* ************************************************************************* */
TEST(ISAM2, slamlike_solution_partial_relinearization_check)
{
  // These variables will be reused and accumulate factors and values
  Values fullinit;
  NonlinearFactorGraph fullgraph;
  ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false);
  params.enablePartialRelinearizationCheck = true;
  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, params);

  // Compare solutions
  CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
}

namespace {
  bool checkMarginalizeLeaves(ISAM2& isam, const FastList<Key>& leafKeys) {
    Matrix expectedAugmentedHessian, expected3AugmentedHessian;
    vector<Key> toKeep;
    for(Key j: isam.getDelta() | br::map_keys)
      if(find(leafKeys.begin(), leafKeys.end(), j) == leafKeys.end())
        toKeep.push_back(j);

    // Calculate expected marginal from iSAM2 tree
    expectedAugmentedHessian = GaussianFactorGraph(isam).marginal(toKeep, EliminateQR)->augmentedHessian();

    // Calculate expected marginal from cached linear factors
    //assert(isam.params().cacheLinearizedFactors);
    //Matrix expected2AugmentedHessian = isam.linearFactors_.marginal(toKeep, EliminateQR)->augmentedHessian();

    // Calculate expected marginal from original nonlinear factors
    expected3AugmentedHessian = isam.getFactorsUnsafe().linearize(isam.getLinearizationPoint())
      ->marginal(toKeep, EliminateQR)->augmentedHessian();

    // Do marginalization
    isam.marginalizeLeaves(leafKeys);

    // Check
    GaussianFactorGraph actualMarginalGraph(isam);
    Matrix actualAugmentedHessian = actualMarginalGraph.augmentedHessian();
    //Matrix actual2AugmentedHessian = linearFactors_.augmentedHessian();
    Matrix actual3AugmentedHessian = isam.getFactorsUnsafe().linearize(
      isam.getLinearizationPoint())->augmentedHessian();
    assert(actualAugmentedHessian.allFinite());

    // Check full marginalization
    //cout << "treeEqual" << endl;
    bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6);
    //actualAugmentedHessian.bottomRightCorner(1,1) = expected2AugmentedHessian.bottomRightCorner(1,1); bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6);
    //cout << "nonlinEqual" << endl;
    actualAugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1e-6);
    //bool linCorrect = assert_equal(expected3AugmentedHessian, expected2AugmentedHessian, 1e-6);
    //actual2AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterLinCorrect = assert_equal(expected3AugmentedHessian, actual2AugmentedHessian, 1e-6);
    //cout << "nonlinCorrect" << endl;
    bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1e-6);

    bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect;
    return ok;
  }
}

/* ************************************************************************* */
TEST(ISAM2, marginalizeLeaves1)
{
  ISAM2 isam;
  NonlinearFactorGraph factors;
  factors += PriorFactor<LieScalar>(0, Zero, model);

  factors += BetweenFactor<LieScalar>(0, 1, Zero, model);
  factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
  factors += BetweenFactor<LieScalar>(0, 2, Zero, model);

  Values values;
  values.insert(0, Zero);
  values.insert(1, Zero);
  values.insert(2, Zero);

  FastMap<Key,int> constrainedKeys;
  constrainedKeys.insert(make_pair(0,0));
  constrainedKeys.insert(make_pair(1,1));
  constrainedKeys.insert(make_pair(2,2));

  isam.update(factors, values, FactorIndices(), constrainedKeys);

  FastList<Key> leafKeys = list_of(0);
  EXPECT(checkMarginalizeLeaves(isam, leafKeys));
}

/* ************************************************************************* */
TEST(ISAM2, marginalizeLeaves2)
{
  ISAM2 isam;

  NonlinearFactorGraph factors;
  factors += PriorFactor<LieScalar>(0, Zero, model);

  factors += BetweenFactor<LieScalar>(0, 1, Zero, model);
  factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
  factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
  factors += BetweenFactor<LieScalar>(2, 3, Zero, model);

  Values values;
  values.insert(0, Zero);
  values.insert(1, Zero);
  values.insert(2, Zero);
  values.insert(3, Zero);

  FastMap<Key,int> constrainedKeys;
  constrainedKeys.insert(make_pair(0,0));
  constrainedKeys.insert(make_pair(1,1));
  constrainedKeys.insert(make_pair(2,2));
  constrainedKeys.insert(make_pair(3,3));

  isam.update(factors, values, FactorIndices(), constrainedKeys);

  FastList<Key> leafKeys = list_of(0);
  EXPECT(checkMarginalizeLeaves(isam, leafKeys));
}

/* ************************************************************************* */
TEST(ISAM2, marginalizeLeaves3)
{
  ISAM2 isam;

  NonlinearFactorGraph factors;
  factors += PriorFactor<LieScalar>(0, Zero, model);

  factors += BetweenFactor<LieScalar>(0, 1, Zero, model);
  factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
  factors += BetweenFactor<LieScalar>(0, 2, Zero, model);

  factors += BetweenFactor<LieScalar>(2, 3, Zero, model);

  factors += BetweenFactor<LieScalar>(3, 4, Zero, model);
  factors += BetweenFactor<LieScalar>(4, 5, Zero, model);
  factors += BetweenFactor<LieScalar>(3, 5, Zero, model);

  Values values;
  values.insert(0, Zero);
  values.insert(1, Zero);
  values.insert(2, Zero);
  values.insert(3, Zero);
  values.insert(4, Zero);
  values.insert(5, Zero);

  FastMap<Key,int> constrainedKeys;
  constrainedKeys.insert(make_pair(0,0));
  constrainedKeys.insert(make_pair(1,1));
  constrainedKeys.insert(make_pair(2,2));
  constrainedKeys.insert(make_pair(3,3));
  constrainedKeys.insert(make_pair(4,4));
  constrainedKeys.insert(make_pair(5,5));

  isam.update(factors, values, FactorIndices(), constrainedKeys);

  FastList<Key> leafKeys = list_of(0);
  EXPECT(checkMarginalizeLeaves(isam, leafKeys));
}

/* ************************************************************************* */
TEST(ISAM2, marginalizeLeaves4)
{
  ISAM2 isam;

  NonlinearFactorGraph factors;
  factors += PriorFactor<LieScalar>(0, Zero, model);
  factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
  factors += BetweenFactor<LieScalar>(1, 2, Zero, model);

  Values values;
  values.insert(0, Zero);
  values.insert(1, Zero);
  values.insert(2, Zero);

  FastMap<Key,int> constrainedKeys;
  constrainedKeys.insert(make_pair(0,0));
  constrainedKeys.insert(make_pair(1,1));
  constrainedKeys.insert(make_pair(2,2));

  isam.update(factors, values, FactorIndices(), constrainedKeys);

  FastList<Key> leafKeys = list_of(1);
  EXPECT(checkMarginalizeLeaves(isam, leafKeys));
}

/* ************************************************************************* */
TEST(ISAM2, marginalizeLeaves5)
{
  // Create isam2
  ISAM2 isam = createSlamlikeISAM2();

  // Marginalize
  FastList<Key> marginalizeKeys = list_of(0);
  EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys));
}

/* ************************************************************************* */
TEST(ISAM2, marginalCovariance)
{
  // Create isam2
  ISAM2 isam = createSlamlikeISAM2();

  // Check marginal
  Matrix expected = Marginals(isam.getFactorsUnsafe(), isam.getLinearizationPoint()).marginalCovariance(5);
  Matrix actual = isam.marginalCovariance(5);
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(ISAM2, calculate_nnz)
{
  ISAM2 isam = createSlamlikeISAM2();
  int expected = 241;
  int actual = calculate_nnz(isam.roots().front());

  EXPECT_LONGS_EQUAL(expected, actual);
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 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)

//[boost_range_reversed
//` Shows how to use a Boost.Geometry linestring, reversed by Boost.Range adaptor

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/adapted/boost_range/reversed.hpp>

int main()
{
    typedef boost::geometry::model::d2::point_xy<int> xy;
    boost::geometry::model::linestring<xy> line;
    line.push_back(xy(0, 0));
    line.push_back(xy(1, 1));

    std::cout
        << boost::geometry::dsv(line | boost::adaptors::reversed)
        << std::endl;

    return 0;
}

//]

//[boost_range_reversed_output
/*`
Output:
[pre
((1, 1), (0, 0))
]
*/
//]


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

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

#ifdef _MSC_VER
# pragma warning (disable : 4100) // unreferenced formal parameters
#endif

#include <iostream>
using std::cout;  using std::endl; using std::cerr;

//[policy_eg_8

/*`
Suppose we want our own user-defined error handlers rather than the
any of the default ones supplied by the library to be used.
If we set the policy for a specific type of error to `user_error`
then the library will call a user-supplied error handler.
These are forward declared, but not defined in
boost/math/policies/error_handling.hpp like this:

   namespace boost{ namespace math{ namespace policies{

   template <class T>
   T user_domain_error(const char* function, const char* message, const T& val);
   template <class T>
   T user_pole_error(const char* function, const char* message, const T& val);
   template <class T>
   T user_overflow_error(const char* function, const char* message, const T& val);
   template <class T>
   T user_underflow_error(const char* function, const char* message, const T& val);
   template <class T>
   T user_denorm_error(const char* function, const char* message, const T& val);
   template <class T>
   T user_evaluation_error(const char* function, const char* message, const T& val);
   template <class T, class TargetType>
   T user_rounding_error(const char* function, const char* message, const T& val, const TargetType& t);
   template <class T>
   T user_indeterminate_result_error(const char* function, const char* message, const T& val);

   }}} // namespaces

So out first job is to include the header we want to use, and then
provide definitions for our user-defined error handlers that we want to use.
We only provide our special domain and pole error handlers;
other errors like overflow and underflow use the default.
*/

#include <boost/math/special_functions.hpp>

namespace boost{ namespace math
{
  namespace policies
  {
    template <class T>
    T user_domain_error(const char* function, const char* message, const T& val)
    { // Ignoring function, message and val for this example, perhaps unhelpfully.
       cerr << "Domain Error!" << endl;
       return std::numeric_limits<T>::quiet_NaN();
    }

    template <class T>
    T user_pole_error(const char* function, const char* message, const T& val)
    { // Ignoring function, message and val for this example, perhaps unhelpfully.
       cerr << "Pole Error!" << endl;
       return std::numeric_limits<T>::quiet_NaN();
    }
  } // namespace policies
}} // namespace boost{ namespace math

/*`
Now we'll need to define a suitable policy that will call these handlers,
and define some forwarding functions that make use of the policy:
*/

namespace{

using namespace boost::math::policies;

typedef policy<
   domain_error<user_error>,
   pole_error<user_error>
> user_error_policy;

BOOST_MATH_DECLARE_SPECIAL_FUNCTIONS(user_error_policy)

} // close unnamed namespace

/*`
We now have a set of forwarding functions defined in an unnamed namespace
that all look something like this:

``
template <class RealType>
inline typename boost::math::tools::promote_args<RT>::type
   tgamma(RT z)
{
   return boost::math::tgamma(z, user_error_policy());
}
``

So that when we call `tgamma(z)` we really end up calling
`boost::math::tgamma(z, user_error_policy())`, and any
errors will get directed to our own error handlers.
*/

int main()
{
   cout << "Result of erf_inv(-10) is: "
      << erf_inv(-10) << endl;
   cout << "Result of tgamma(-10) is: "
      << tgamma(-10) << endl;
}

/*`

Which outputs:

[pre
  Domain Error!
  Pole Error!
  Result of erf_inv(-10) is: 1.#QNAN
  Result of tgamma(-10) is: 1.#QNAN
]
*/

//] // //[/policy_eg_8]


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 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)

//[assign_inverse
//` Usage of assign_inverse and expand to conveniently determine bounding 3D box of two points

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point.hpp>

using namespace boost::geometry;

int main()
{
    typedef model::point<float, 3, cs::cartesian> point;
    typedef model::box<point> box;

    box all;
    assign_inverse(all);
    std::cout << dsv(all) << std::endl;
    expand(all, point(0, 0, 0));
    expand(all, point(1, 2, 3));
    std::cout << dsv(all) << std::endl;

    return 0;
}

//]

//[assign_inverse_output
/*`
Output:
[pre
((3.40282e+038, 3.40282e+038, 3.40282e+038), (-3.40282e+038, -3.40282e+038, -3.40282e+038))
((0, 0, 0), (1, 2, 3))]
*/
//]


/// \file inner_product_grad.cc
/// \author David Stutz
/// \brief Implementation of the gradient of a inner product operation, see
/// inner_product.cc.

#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/shape_inference.h"

#include <sophus/so3.hpp>
#include <sophus/se3.hpp>
#include <Eigen/Dense>
#include <cmath>

using namespace Sophus;
using namespace Eigen;

// the gradients are simply passed as additional arguments as
// they are available in the Python function for registering the gradient operation.
REGISTER_OP("SE3toMatrixRtGrad")
  .Input("grad: float32")
  .Input("se3_vector: float32")
  .Output("grad_se3: float32");

namespace tensorflow{

using namespace tensorflow;

/// \brief Implementation of an inner product gradient operation.
/// Note that this operation is used in Python to register the gradient as
/// this is not possible in C*+ right now.
/// \param context
/// \author David Stutz
class SE3toMatrixRtGradOp : public OpKernel {
public:
  /// \brief Constructor.
  /// \param context
  explicit SE3toMatrixRtGradOp(OpKernelConstruction* context) : OpKernel(context) {

  }

  Eigen::Matrix<float,3,3> skew_symmetric(Eigen::Matrix<float,3,1>v)
  {
    Eigen::Matrix<float, 3,3>v_cross;
        v_cross << 0, -v(2,0), v(1,0),
                   v(2,0), 0, -v(0,0),
                  -v(1,0), v(0,0), 0;

    return v_cross;

  }

  /// \brief Compute the inner product gradients.
  /// \param context
  void Compute(OpKernelContext* context) override {

    // output and grad is provided as input
    DCHECK_EQ(2, context->num_inputs());

    // get the gradient tensor
    const Tensor& grad = context->input(0);

    // get the original input tensor
    const Tensor& se3_vector = context->input(1);

    // create input shape (inferred from the additional attribute `n`)
    TensorShape se3_vector_shape = se3_vector.shape();

    DCHECK_EQ(se3_vector_shape.dim_size(0), grad.shape().dim_size(0));

    // create output tensors
    Tensor* grad_se3 = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(0, se3_vector_shape, &grad_se3));

    // get the Eigen tensors for data access
    auto grad_tensor = grad.tensor<float, 3>();
    auto se3_vector_tensor = se3_vector.tensor<float,2>();
    auto grad_se3_tensor = grad_se3->tensor<float,2>();

    for(int b = 0; b < se3_vector_shape.dim_size(0); b++)
    {
        auto v1 = se3_vector_tensor(b,0);
        auto v2 = se3_vector_tensor(b,1);
        auto v3 = se3_vector_tensor(b,2);

        auto v_mag = sqrt(v1*v1 + v2*v2 + v3*v3);

        Eigen::Matrix<float,3,1>vec;
        vec<< v1, v2, v3;

        Eigen::Matrix<float,3,3> R = SO3<float>::exp(vec).matrix();
        Eigen::Matrix<float, 3,3>v_cross = skew_symmetric(vec);
        Eigen::Matrix<float,3,3> I = Eigen::Matrix<float,3,3>::Identity(3,3);

        Eigen::Matrix<float, 3, 3>v_cross_I_minus_R = v_cross  * (I-R);

        Eigen::Matrix<float,3,1> e_i;
        e_i << 1, 0, 0;

        Eigen::Matrix<float, 3, 3>dR_dv1 = v1 * v_cross;
        Eigen::Matrix<float, 3, 3>v_cross_I_minus_R_times_e1 = skew_symmetric(v_cross_I_minus_R * e_i);

        dR_dv1 += v_cross_I_minus_R_times_e1;
        dR_dv1 *= R;
        dR_dv1 /= v_mag;

        e_i << 0, 1, 0;

        Eigen::Matrix<float, 3, 3>dR_dv2 = v2 * v_cross;
        Eigen::Matrix<float, 3, 3>v_cross_I_minus_R_times_e2 = skew_symmetric(v_cross_I_minus_R * e_i);

        dR_dv2 += v_cross_I_minus_R_times_e2;
        dR_dv2 *= R;
        dR_dv2 /= v_mag;

        e_i << 0, 0, 1;

        Eigen::Matrix<float, 3, 3>dR_dv3 = v3 * v_cross;
        Eigen::Matrix<float, 3, 3>v_cross_I_minus_R_times_e3 = skew_symmetric(v_cross_I_minus_R * e_i);

        dR_dv3 += v_cross_I_minus_R_times_e3;
        dR_dv3 *= R;
        dR_dv3 /= v_mag;

    }

  }
};

REGISTER_KERNEL_BUILDER(Name("SE3toMatrixRtGrad").Device(DEVICE_CPU), SE3toMatrixRtGradOp);
};


/**
 * @file Transform.hpp
 * @author bwu
 * @brief Model of transform vector, matrix and quaternion concept
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_GEOMETRY_TRANSFORM_HPP
#define GENERIC_GEOMETRY_TRANSFORM_HPP
#include <boost/qvm/quat_vec_operations.hpp>
#include <boost/qvm/vec_mat_operations.hpp>
#include <boost/qvm/quat_operations.hpp>
#include <boost/qvm/mat_operations.hpp>
#include <boost/qvm/vec_operations.hpp>
#include <boost/qvm/vec_traits.hpp>
#include <boost/qvm/quat.hpp>
#include <boost/qvm/mat.hpp>
#include <boost/qvm/vec.hpp>
#include "generic/math/Numbers.hpp"
#include "Geometries.hpp"
#include "Vector.hpp"
namespace boost {
namespace qvm {

using namespace generic::geometry;
template <typename num_type>
struct vec_traits<Vector3D<num_type> >
{
    static const int dim = 3;
    using scalar_type = num_type;

    template <int I>
    static inline scalar_type & write_element(Vector3D<num_type> & vec) { return vec[I]; }

    template <int I>
    static inline scalar_type read_element(const Vector3D<num_type> & vec) { return vec[I]; }

    static inline scalar_type & write_element_idx(int i, Vector3D<num_type> & vec) { return vec[i]; }

    static inline scalar_type read_element_idx(int i, const Vector3D<num_type> & vec) { return vec[i]; }
};

template <typename num_type, size_t N>
struct vec_traits<VectorN<num_type, N> >
{
    static const int dim = static_cast<int>(N);
    using scalar_type = num_type;

    template <int I>
    static inline scalar_type & write_element(VectorN<num_type, N> & vec) { return vec[I]; }

    template <int I>
    static inline scalar_type read_element(const VectorN<num_type, N> & vec) { return vec[I]; }

    static inline scalar_type & write_element_idx(int i, VectorN<num_type, N> & vec) { return vec[i]; }

    static inline scalar_type read_element_idx(int i, const VectorN<num_type, N> & vec) { return vec[i]; }
};
}//namespace qvm
}//namespace boost

namespace generic {
namespace geometry{

template <typename num_type>
using Matrix2x2 = boost::qvm::mat<num_type, 2, 2>;

template <typename num_type>
using Matrix3x3 = boost::qvm::mat<num_type, 3, 3>;

template <typename num_type>
using Matrix4x4 = boost::qvm::mat<num_type, 4, 4>;

template <typename float_t> class Transform2D;
template <typename float_t> class Transform3D;
template <typename float_t> class Quaternion;

///@brief makes a 2d shift transform matrix by a vector2d
template <typename float_t, typename num_type>
inline Transform2D<float_t> makeShiftTransform2D(const Vector2D<num_type> & shift)
{
    Transform2D<float_t> trans;
    trans(0, 2) = float_t(shift[0]);
    trans(1, 2) = float_t(shift[1]);
    return trans;
}

///@brief makes a 2d rotation transform matrix by radian, unit: rad
template <typename float_t>
inline Transform2D<float_t> makeRotateTransform2D(float_t rot)
{
    float_t c = std::cos(rot);
    float_t s = std::sin(rot);
    Transform2D<float_t> trans;
    trans(0, 0) = c; trans(0, 1) = -s;
    trans(1, 0) = s; trans(1, 1) =  c;
    return trans;
}

///@brief makes a 2d scale transform matrix by scale factor
template <typename float_t>
inline Transform2D<float_t> makeScaleTransform2D(float_t scale)
{
    Transform2D<float_t> trans;
    trans(0, 0) = scale; trans(1,1) = scale;
    return trans;
}

///@brief makes a 2d mirror transform matrix by given axis
template <typename float_t>
inline Transform2D<float_t> makeMirroredTransform2D(Axis axis)
{
    Transform2D<float_t> trans;
    if(axis == Axis::X) trans(1, 1) *= -1;
    else if(axis == Axis::Y) trans(0, 0) *= -1;
    return trans;
}

///@brief makes a 3d shift transform matrix by a vector2d
template <typename float_t, typename num_type>
inline Transform3D<float_t> makeShiftTransform3D(const Vector3D<num_type> & shift)
{
    Transform3D<float_t> trans;
    trans(0, 3) = float_t(shift[0]);
    trans(1, 3) = float_t(shift[1]);
    trans(2, 3) = float_t(shift[2]);
    return trans;
}

///@brief makes a 3d scale transform matrix by scale factor
template <typename float_t>
inline Transform3D<float_t> makeScaleTransform3D(float_t scale)
{
    Transform3D<float_t> trans;
    trans(0, 0) = scale; trans(1,1) = scale; trans(2, 2) = scale;
    return trans;
}

template <typename float_t>
class Transform2D
{
    static_assert(std::is_floating_point<float_t>::value, "only floating point type support in transform construct!");
public:
    static const size_t dim = 2;
    Transform2D();
    ///@brief constructs a transform2d model by matrix 3x3
    explicit Transform2D(const Matrix3x3<float_t> & m);

    ///@brief accesses matrix element by row and col index 0-2
    float_t & operator() (size_t row, size_t col);
    const float_t & operator() (size_t row, size_t col) const;

    ///@brief inverses the transform matrix, return false if the matrix det = 0
    bool Inverse();

    ///@brief gets result transform of this * trans
    Transform2D<float_t> operator * (const Transform2D<float_t> & trans) const;
    ///@brief gets result transform of trans * this
    Transform2D<float_t> & Prod(const Transform2D<float_t> & trans);

    ///@brief gets transformed point of input
    template <typename num_type>
    Point2D<num_type> operator * (const Point2D<num_type> & point) const;

    ///@brief gets transformed segment of input
    template <typename num_type>
    Segment2D<num_type> operator * (const Segment2D<num_type> & segment) const;

    ///@brief gets transformed triangle of input
    template <typename num_type>
    Triangle2D<num_type> operator * (const Triangle2D<num_type> & triangle) const;

    ///@brief gets transformed box of input
    template <typename num_type>
    Polygon2D<num_type> operator * (const Box2D<num_type> & box) const;

    ///@brief gets transformed polyline of input
    template <typename num_type>
    Polyline2D<num_type> operator * (const Polyline2D<num_type> & polyline) const;

    ///@brief gets transformed polygon of input
    template <typename num_type>
    Polygon2D<num_type> operator * (const Polygon2D<num_type> & polygon) const;

    ///@brief gets transformed polygon with holes of input
    template <typename num_type>
    PolygonWithHoles2D<num_type> operator * (const PolygonWithHoles2D<num_type> & pwh) const;

    ///@brief accesses internal matrix data
    Matrix3x3<float_t> & GetMatrix() { return m_matrix; }
    const Matrix3x3<float_t> & GetMatrix() const { return m_matrix; }

    ///@brief gets matrix elements in array, adapt for OpenGL API
    void GetCoeffs(float_t coeffs[], bool rowMajor = true);

private:
    Matrix3x3<float_t> m_matrix;
};

template <typename float_t>
class Transform3D
{
    static_assert(std::is_floating_point<float_t>::value, "only floating point type support in transform construct!");
public:
    static const size_t dim = 3;
    Transform3D();
    ///@brief constructs a transform3d model from transform2d
    explicit Transform3D(const Transform2D<float_t> & trans2d);
    ///@brief constructs a transform3d model by matrix 4x4
    explicit Transform3D(const Matrix4x4<float_t> & m);

    ///@brief accesses matrix element by row and col index 0-2
    float_t & operator() (size_t row, size_t col);
    const float_t & operator() (size_t row, size_t col) const;

    ///@brief inverses the transform matrix, return false if the matrix det = 0
    bool Inverse();
    ///@brief gets transform2d(x, y) from transform3d(x, y, z)
    Transform2D<float_t> GetTransfrom2D() const;

    ///@brief gets result transform of this * trans
    Transform3D<float_t> operator * (const Transform3D<float_t> & trans) const;
    ///@brief gets result transform of trans * this
    Transform3D<float_t> & Prod(const Transform3D<float_t> & tran);

    ///@brief gets transformed point of input
    template <typename num_type>
    Point3D<num_type> operator * (const Point3D<num_type> & point) const;

    ///@brief gets transformed segment of input
    template <typename num_type>
    Segment3D<num_type> operator * (const Segment3D<num_type> & segment) const;

    ///@brief gets transformed triangle of input
    template <typename num_type>
    Triangle3D<num_type> operator * (const Triangle3D<num_type> & triangle) const;

    ///@brief accesses internal matrix data
    Matrix4x4<float_t> & GetMatrix() { return m_matrix; }
    const Matrix4x4<float_t> & GetMatrix() const { return m_matrix; }

    ///@brief gets matrix elements in array, adapt for OpenGL API
    void GetCoeffs(float_t coeffs[], bool rowMajor = true);

private:
    Matrix4x4<float_t> m_matrix;
};

template <typename float_t>
class Quaternion
{
    static_assert(std::is_floating_point<float_t>::value, "only floating point type support in transform construct!");
public:
    static const size_t dim = 3;
    Quaternion();
    Quaternion(float_t q0, float_t q1, float_t q2, float_t q3);
    Quaternion(const Vector3D<float_t> & axis, float_t angle);
    Quaternion(const boost::qvm::quat<float_t> & q);

    ///@brief accesses quaternion element by index 0-3
    float_t & operator[](size_t dim);
    const float_t & operator[](size_t dim) const;
    ///@brief multiples this with q
    Quaternion<float_t> & operator *= (const Quaternion<float_t> & q);
    ///@brief gets multiplie result of this * q
    Quaternion<float_t> operator * (const Quaternion<float_t> & q) const;

    ///@brief performs a rotation around the axis at angle radians
    void SetAxisAndAngle(const Vector3D<float_t> & axis, float angle);
    ///@brief gets axis reprented by this quaternion
    Vector3D<float_t> Axis() const;
    ///@brief gets angle reprented by this quaternion
    float_t Angle() const;

    ///@brief multiplicative inverse of this quaternion
    void Invert();
    ///@brief megates all the coefficients of this quaternion
    void Negate();
    ///@brief normalizes the quaternion oefficients with unit quaternions
    void Normalize();
    ///@brief returns the magnitude of this
    float_t Mag() const;
    ///@brief returns the squared magnitude of this
    float_t MagSqrt() const;
    Quaternion<float_t> Log() const;
    Quaternion<float_t> Exp() const;
    Quaternion<float_t> Inverse() const;
    Vector3D<float_t> Rotate(const Vector3D<float_t> & vec) const;
    Vector3D<float_t> InverseRotate(const Vector3D<float_t> & vec) const;

    ///@brief returns dot product of this and q
    float_t Dot(const Quaternion<float_t> & q) const;

    ///@brief accesses internal quat data
    boost::qvm::quat<float_t> & q() { return m_q; }
    const boost::qvm::quat<float_t> & q() const { return m_q; }

    static Quaternion<float_t> LnDif(const Quaternion<float_t> &a, const Quaternion<float_t> & b);
    static Quaternion<float_t> SquadTangent(const Quaternion<float_t> & before,
                                            const Quaternion<float_t> & center,
                                            const Quaternion<float_t> & after);
    static Quaternion<float_t> Squad(const Quaternion<float_t> & a, const Quaternion<float_t> & tgA,
                                     const Quaternion<float_t> & b, const Quaternion<float_t> & tgB, float_t t);

    ///@brief returns the result of spherical linear interpolation of the input quat `a`, `b` and interpolation parameter `t`
    static Quaternion<float_t> Slerp(const Quaternion<float_t> & a, const Quaternion<float_t> & b, float_t t);

private:
    boost::qvm::quat<float_t> m_q;
};

template <typename float_t>
inline Transform2D<float_t>::Transform2D()
{
    boost::qvm::set_identity(m_matrix);
}

template <typename float_t>
inline Transform2D<float_t>::Transform2D(const Matrix3x3<float_t> & m)
{
    boost::qvm::assign(m_matrix, m);
}

template <typename float_t>
inline float_t & Transform2D<float_t>::operator() (size_t row, size_t col)
{
    return m_matrix.a[row][col];
}

template <typename float_t>
inline const float_t & Transform2D<float_t>::operator() (size_t row, size_t col) const
{
    return m_matrix.a[row][col];
}

template <typename float_t>
inline bool Transform2D<float_t>::Inverse()
{
    float_t det = boost::qvm::determinant(m_matrix);
    if(math::EQ(det, float_t(0))) return false;
    m_matrix = boost::qvm::inverse(m_matrix, det);
    return true;
}

template <typename float_t>
inline Transform2D<float_t> Transform2D<float_t>::operator * (const Transform2D<float_t> & trans) const
{
    using namespace boost::qvm;
    return Transform2D<float_t>(m_matrix * trans.GetMatrix());
}

template <typename float_t>
inline Transform2D<float_t> & Transform2D<float_t>::Prod(const Transform2D<float_t> & trans)
{
    m_matrix = trans.GetMatrix() * m_matrix;
    return *this;
}

template <typename float_t>
template <typename num_type>
inline Point2D<num_type> Transform2D<float_t>::operator * (const Point2D<num_type> & point) const
{
    Point2D<num_type> res(point);
    Transform(res, *this);
    return res;
}

template <typename float_t>
template <typename num_type>
inline Segment2D<num_type> Transform2D<float_t>::operator * (const Segment2D<num_type> & segment) const
{
    Segment2D<num_type> res(segment);
    Transform(res, *this);
    return res;
}

template <typename float_t>
template <typename num_type>
inline Triangle2D<num_type> Transform2D<float_t>::operator * (const Triangle2D<num_type> & triangle) const
{
    Triangle2D<num_type> res(triangle);
    Transform(res, *this);
    return res;
}

template <typename float_t>
template <typename num_type>
inline Polygon2D<num_type> Transform2D<float_t>::operator * (const Box2D<num_type> & box) const
{
    Point2D<num_type> p[4] = { box[0], {box[1][0], box[0][1]}, box[1], {box[0][0], box[1][1]} };
    Polygon2D<num_type> res;
    for(size_t i = 0; i < 4; ++i){
        Transform(p[i], *this);
        res << p[i];
    }
    return res;
}

template <typename float_t>
template <typename num_type>
inline Polyline2D<num_type> Transform2D<float_t>::operator * (const Polyline2D<num_type> & polyline) const
{
    Polyline2D<num_type> res = polyline;
    Transform(res, *this);
    return res;
}

template <typename float_t>
template <typename num_type>
inline Polygon2D<num_type>  Transform2D<float_t>::operator * (const Polygon2D<num_type> & polygon) const
{
    Polygon2D<num_type> res(polygon);
    Transform(res, *this);
    return res;
}

template <typename float_t>
template <typename num_type>
inline PolygonWithHoles2D<num_type> Transform2D<float_t>::operator * (const PolygonWithHoles2D<num_type> & pwh) const
{
    PolygonWithHoles2D<num_type> res(pwh);
    Transform(res, *this);
    return res;
}

template <typename float_t>
inline void Transform2D<float_t>::GetCoeffs(float_t coeffs[], bool rowMajor)
{
    if(rowMajor){
        for(size_t i = 0; i <= dim; ++i)
            for(size_t j = 0; j <= dim; ++j)
                coeffs[(dim + 1) * i + j] = m_matrix.a[i][j];
    }
    else{
        for(size_t i = 0; i <= dim; ++i)
            for(size_t j = 0; j <= dim; ++j)
                coeffs[(dim + 1) * i + j] = m_matrix.a[j][i];
    }
}

template <typename float_t>
inline Transform3D<float_t>::Transform3D()
{
    boost::qvm::set_identity(m_matrix);
}

template <typename float_t>
inline Transform3D<float_t>::Transform3D(const Transform2D<float_t> & trans2d)
{
    boost::qvm::set_identity(m_matrix);
    for(size_t i = 0; i < 2; ++i)
        for(size_t j = 0; j < 2; ++j)
            m_matrix.a[i][j] = trans2d(i, j);
    m_matrix.a[0][3] = trans2d(0, 2);
    m_matrix.a[1][3] = trans2d(1, 2);
}

template <typename float_t>
inline Transform3D<float_t>::Transform3D(const Matrix4x4<float_t> & m)
{
    boost::qvm::assign(m_matrix, m);
}

template <typename float_t>
inline float_t & Transform3D<float_t>::operator() (size_t row, size_t col)
{
    return m_matrix.a[row][col];
}

template <typename float_t>
inline const float_t & Transform3D<float_t>::operator() (size_t row, size_t col) const
{
    return m_matrix.a[row][col];
}

template <typename float_t>
inline bool Transform3D<float_t>::Inverse()
{
    float_t det = boost::qvm::determinant(m_matrix);
    if(math::EQ(det, float_t(0))) return false;
    m_matrix = boost::qvm::inverse(m_matrix, det);
    return true;
}

template <typename float_t>
inline Transform2D<float_t> Transform3D<float_t>::GetTransfrom2D() const
{
    Transform2D<float_t> trans;
    for(size_t i = 0; i < 2; ++i)
        for(size_t j = 0; j < 2; ++j)
            trans(i, j) = m_matrix.a[i][j];
    trans(0, 2) = m_matrix.a[0][3];
    trans(1, 2) = m_matrix.a[1][3];
    return trans;
}

template <typename float_t>
inline Transform3D<float_t> Transform3D<float_t>::operator * (const Transform3D<float_t> & trans) const
{
    using namespace boost::qvm;
    return Transform3D<float_t>(m_matrix * trans.GetMatrix());
}

template <typename float_t>
inline Transform3D<float_t> & Transform3D<float_t>::Prod(const Transform3D<float_t> & trans)
{
    m_matrix = trans.GetMatrix() * m_matrix;
    return *this;
}

template <typename float_t>
template <typename num_type>
inline Point3D<num_type> Transform3D<float_t>::operator * (const Point3D<num_type> & point) const
{
    Point3D<num_type> res(point);
    Transform(res, *this);
    return res;
}

template <typename float_t>
template <typename num_type>
inline Segment3D<num_type> Transform3D<float_t>::operator * (const Segment3D<num_type> & segment) const
{
    Segment3D<num_type> res(segment);
    Transform(res, *this);
    return res;
}

template <typename float_t>
template <typename num_type>
inline Triangle3D<num_type> Transform3D<float_t>::operator * (const Triangle3D<num_type> & triangle) const
{
    Triangle3D<num_type> res(triangle);
    Transform(res, *this);
    return res;
}

template <typename float_t>
inline void Transform3D<float_t>::GetCoeffs(float_t coeffs[], bool rowMajor)
{
    if(rowMajor){
        for(size_t i = 0; i <= dim; ++i)
            for(size_t j = 0; j <= dim; ++j)
                coeffs[(dim + 1) * i + j] = m_matrix.a[i][j];
    }
    else{
        for(size_t i = 0; i <= dim; ++i)
            for(size_t j = 0; j <= dim; ++j)
                coeffs[(dim + 1) * i + j] = m_matrix.a[j][i];    }
}

template <typename float_t>
inline Quaternion<float_t>::Quaternion()
{
    m_q.a[0] = 1;
    m_q.a[1] = m_q.a[2] = m_q.a[3] = 0;
}

template <typename float_t>
inline Quaternion<float_t>::Quaternion(float_t q0, float_t q1, float_t q2, float_t q3)
{
    m_q.a[0] = q0;
    m_q.a[1] = q1; m_q.a[2] = q2; m_q.a[3] = q3;
}

template <typename float_t>
inline Quaternion<float_t>::Quaternion(const Vector3D<float_t> & axis, float_t angle)
{
    SetAxisAndAngle(axis, angle);
}

template <typename float_t>
inline Quaternion<float_t>::Quaternion(const boost::qvm::quat<float_t> & q)
{
    boost::qvm::assign(m_q, q);
}

template <typename float_t>
inline float_t & Quaternion<float_t>::operator[](size_t dim)
{
    return m_q.a[dim];
}

template <typename float_t>
inline const float_t & Quaternion<float_t>::operator[](size_t dim) const
{
    return m_q.a[dim];
}

template <typename float_t>
inline Quaternion<float_t> & Quaternion<float_t>::operator *= (const Quaternion<float_t> & q)
{
    *this = (*this) * q;
    return *this;
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::operator * (const Quaternion<float_t> & q) const
{
    return Quaternion<float_t>(m_q * q.q());
}

template <typename float_t>
inline void Quaternion<float_t>::SetAxisAndAngle(const Vector3D<float_t> & axis, float angle)
{
    boost::qvm::set_rot(m_q, axis, angle);
}

template <typename float_t>
inline Vector3D<float_t> Quaternion<float_t>::Axis() const
{
    Vector3D<float_t> res(m_q.a[1], m_q.a[2], m_q.a[3]);
    float_t sinus = res.Norm2();
    if(sinus > std::numeric_limits<float_t>::epsilon()) res /= sinus;
    return math::LE(std::acos(m_q.a[0]), float_t(math::pi_half)) ? res : -res;
}

template <typename float_t>
float_t Quaternion<float_t>::Angle() const
{
    float_t angle = 2.0 * std::acos(m_q.a[0]);
    return math::LE(angle, float_t(math::pi)) ? angle : math::pi_2 - angle;
}

template <typename float_t>
inline void Quaternion<float_t>::Invert()
{
    boost::qvm::assign(m_q, boost::qvm::inverse(m_q));
}

template <typename float_t>
inline void Quaternion<float_t>::Negate()
{
    Invert();
    m_q.a[0] = -m_q.a[0];
}

template <typename float_t>
inline void Quaternion<float_t>::Normalize()
{
    boost::qvm::normalize(m_q);
}

template <typename float_t>
inline float_t Quaternion<float_t>::Mag() const
{
    return boost::qvm::mag(m_q);
}

template <typename float_t>
inline float_t Quaternion<float_t>::MagSqrt() const
{
    return boost::qvm::mag_sqr(m_q);
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::Log() const
{
    float_t len = std::sqrt(m_q.a[1] * m_q.a[1] + m_q.a[2] * m_q.a[2] + m_q.a[3] * m_q.a[3]);
    if(len < std::numeric_limits<float_t>::epsilon())
        return Quaternion<float_t>(0, m_q.a[1], m_q.a[2], m_q.a[3]);
    else{
        float_t coef = std::acos(m_q.a[0]) / len;
        return Quaternion<float_t>(0, coef * m_q.a[1], coef * m_q.a[2], coef * m_q.a[3]);
    }
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::Exp() const
{
    float_t theta = std::sqrt(m_q.a[1] * m_q.a[1] + m_q.a[2] * m_q.a[2] + m_q.a[3] * m_q.a[3]);
    if(theta < std::numeric_limits<float_t>::epsilon())
        return Quaternion<float_t>(std::cos(theta), m_q.a[1], m_q.a[2], m_q.a[3]);
    else{
        float_t coef = std::sin(theta) / theta;
        return Quaternion<float_t>(std::cos(theta), coef * m_q.a[1], coef * m_q.a[2], coef * m_q.a[3]);
    }
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::Inverse() const
{
    return Quaternion<float_t>(boost::qvm::inverse(m_q));
}

template <typename float_t>
inline Vector3D<float_t> Quaternion<float_t>::Rotate(const Vector3D<float_t> & vec) const
{
   using namespace boost::qvm;
   return m_q * vec;
}

template <typename float_t>
inline Vector3D<float_t> Quaternion<float_t>::InverseRotate(const Vector3D<float_t> & vec) const
{
    return Inverse().Rotate(vec);
}

template <typename float_t>
inline float_t Quaternion<float_t>::Dot(const Quaternion<float_t> & q) const
{
    return boost::qvm::dot(m_q, q.q());
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::LnDif(const Quaternion<float_t> &a, const Quaternion<float_t> & b)
{
    auto dif = a.Inverse() * b;
    dif.Normalize();
    return dif.Log();
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::SquadTangent(const Quaternion<float_t> & before,
                                                                const Quaternion<float_t> & center,
                                                                    const Quaternion<float_t> & after)
{
    auto l1 = LnDif(center, before);
    auto l2 = LnDif(center, after);
    Quaternion<float_t> e;
    for(size_t i = 0; i < 4; ++i)
        e[i] = -0.25 * (l1[i] + l2[2]);
    e = center * e.Exp();
    return e;
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::Squad(const Quaternion<float_t> & a, const Quaternion<float_t> & tgA,
                                                        const Quaternion<float_t> & b, const Quaternion<float_t> & tgB, float_t t)
{
    auto ab = Slerp(a, b, t);
    auto tg = Slerp(tgA, tgB, t);
    return Slerp(ab, tg, 2.0 * t * (1.0 - t));
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::Slerp(const Quaternion<float_t> & a, const Quaternion<float_t> & b, float_t t)
{
    using namespace boost::qvm;
    return Quaternion<float_t>(slerp(a.q(), b.q(), t));
}

///@brief transforms the point by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Point2D<num_type> & point, const Transform2D<float_t> & trans)
{
    VectorN<float_t, 3> vec = trans.GetMatrix() * VectorN<float_t, 3>(point[0], point[1], float_t(1));
    point = Point2D<num_type>(vec[0], vec[1]);
}

///@brief transforms the segment by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Segment2D<num_type> & segment, const Transform2D<float_t> & trans)
{
    Transform(segment[0], trans);
    Transform(segment[1], trans);
}

///@brief transforms the triangle by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Triangle2D<num_type> & triangle, const Transform2D<float_t> & trans)
{
    Transform(triangle[0], trans);
    Transform(triangle[1], trans);
    Transform(triangle[2], trans);
}

///@brief transforms the polyline by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Polyline2D<num_type> & polyline, const Transform2D<float_t> & trans)
{
    for(auto & point : polyline)
        Transform(point, trans);
}

///@brief transforms the polygon by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Polygon2D<num_type> & polygon, const Transform2D<float_t> & trans)
{
    for(auto iter = polygon.Begin(); iter != polygon.End(); ++iter)
        Transform(*iter, trans);
}

///@brief transforms the polygon with holes by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(PolygonWithHoles2D<num_type> & pwh, const Transform2D<float_t> & trans)
{
    Transform(pwh.outline, trans);
    auto iter = pwh.BeginHoles();
    for(; iter != pwh.EndHoles(); ++iter)
        Transform(*iter, trans);
}

///@brief transforms the point by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Point3D<num_type> & point, const Transform3D<float_t> & trans)
{
    VectorN<float_t, 4> vec(trans.GetMatrix() * VectorN<float_t, 4>(point[0], point[1], point[2], float_t(1)));
    point = Point3D<num_type>(vec[0], vec[1], vec[2]);
}

///@brief transforms the segment by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Segment3D<num_type> & segment, const Transform3D<float_t> & trans)
{
    Transform(segment[0], trans);
    Transform(segment[1], trans);
}

///@brief transforms the triangle by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Triangle3D<num_type> & triangle, const Transform3D<float_t> & trans)
{
    Transform(triangle[0], trans);
    Transform(triangle[1], trans);
    Transform(triangle[2], trans);
}

///@brief transfroms a collection of geometries by the transform matrix
template <typename geometry_t, typename iterator, typename transform_t,
          typename std::enable_if<geometry_t::dim == transform_t::dim && std::is_same<geometry_t,
          typename std::iterator_traits<iterator>::value_type>::value, bool>::type = true>
inline void Transform(iterator begin, iterator end, const transform_t & trans)
{
    for(auto iter = begin; iter != end; ++iter){
        geometry_t & geom = *iter;
        Transform(geom, trans);
    }
}
}//namespace geometry
}//namespace generic
#endif//GENERIC_GEOMETRY_TRANSFORM_HPP


#include <boost/random/geometric_distribution.hpp>