/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/make_shared.hpp>
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;

#include "tudat/math/statistics/randomVariableGenerator.h"
#include "tudat/math/statistics/boostProbabilityDistributions.h"

namespace tudat
{

namespace statistics
{

std::function< Eigen::VectorXd( const double ) > getIndependentGaussianNoiseFunction(
        const double standardDeviation,
        const double mean,
        const double seed,
        const int outputSize )
{
    std::function< double( ) > inputFreeNoiseFunction = statistics::createBoostContinuousRandomVariableGeneratorFunction(
                statistics::normal_boost_distribution, { mean, standardDeviation }, seed );
    if( outputSize == 1 )
    {
        return [=](const double){ return ( Eigen::VectorXd( outputSize )<<
                                           inputFreeNoiseFunction( ) ).finished( ); };
    }
    else if( outputSize == 2 )
    {
        return [=](const double){ return ( Eigen::VectorXd( outputSize )<<
                                           inputFreeNoiseFunction( ), inputFreeNoiseFunction( ) ).finished( ); };
    }
    else if( outputSize == 3 )
    {
        return [=](const double){ return ( Eigen::VectorXd( outputSize )<<
                                           inputFreeNoiseFunction( ), inputFreeNoiseFunction( ), inputFreeNoiseFunction( ) ).finished( ); };
    }
    else if( outputSize == 6 )
    {
        return [=](const double){ return ( Eigen::VectorXd( outputSize )<<
                                           inputFreeNoiseFunction( ), inputFreeNoiseFunction( ), inputFreeNoiseFunction( ),
                                           inputFreeNoiseFunction( ), inputFreeNoiseFunction( ), inputFreeNoiseFunction( ) ).finished( ); };
    }
    else
    {
        throw std::runtime_error( "Cannot simulate observation noise of size " + std::to_string( outputSize ) );
    }
}

//! Function to create a random number generating function from a continuous univariate distribution implemented in boost
std::function< double( ) > createBoostContinuousRandomVariableGeneratorFunction(
        const ContinuousBoostStatisticalDistributions boostDistribution,
        const std::vector< double >& parameters,
        const double seed )
{
    return std::bind( &RandomVariableGenerator< double >::getRandomVariableValue,
                        createBoostContinuousRandomVariableGenerator( boostDistribution, parameters, seed ) );
}

//! Function to create a random number generator from a continuous univariate distribution implemented in boost
std::shared_ptr< RandomVariableGenerator< double > > createBoostContinuousRandomVariableGenerator(
        const ContinuousBoostStatisticalDistributions boostDistribution,
        const std::vector< double >& parameters,
        const double seed )
{
    return std::make_shared< ContinuousRandomVariableGenerator >(
                createBoostRandomVariable( boostDistribution, parameters ), seed );
}

}

}


#include <cmath>
#include <vector>
#include <iostream>
#include <cassert>

#include <Eigen/Dense>

#include "startree.h"

namespace startree {
// Make a have the minimum x, y and z coordinates of a and b
static void minVec(Vector3d& a, const Vector3d& b) {
    a[X] = fmin(a[X], b[X]);
    a[Y] = fmin(a[Y], b[Y]);
    a[Z] = fmin(a[Z], b[Z]);
}

// Make a have the maximum x, y and z coordinates of a and b
static void maxVec(Vector3d& a, const Vector3d& b) {
    a[X] = fmax(a[X], b[X]);
    a[Y] = fmax(a[Y], b[Y]);
    a[Z] = fmax(a[Z], b[Z]);
}

/* Return new set of sub-bounds given the parent bounds, the split point
 * and the direction (which octant) */
static void
splitBounds(const Vector3d oldBounds[2], Vector3d newBounds[2],
            const Vector3d splitPoint, Vector3d& childSplit,
            TreeDirection dir) {
    switch(dir) {
    case NED:
        newBounds[0] = splitPoint;
        newBounds[1] = oldBounds[1];
        break;
    case NWD:
        newBounds[0] = splitPoint;
        newBounds[0][Y] = oldBounds[0][Y];
        newBounds[1] = oldBounds[1];
        newBounds[1][Y] = splitPoint[Y];
        break;
    case SED:
        newBounds[0] = splitPoint;
        newBounds[0][X] = oldBounds[0][X];
        newBounds[1] = oldBounds[1];
        newBounds[1][X] = splitPoint[X];
        break;
    case SWD:
        newBounds[0] = oldBounds[0];
        newBounds[0][Z] = splitPoint[Z];
        newBounds[1] = splitPoint;
        newBounds[1][Z] = oldBounds[1][Z];
        break;
    case NEU:
        newBounds[0] = splitPoint;
        newBounds[0][Z] = oldBounds[0][Z];
        newBounds[1] = oldBounds[1];
        newBounds[1][Z] = splitPoint[Z];
        break;
    case NWU:
        newBounds[0] = oldBounds[0];
        newBounds[0][X] = splitPoint[X];
        newBounds[1] = splitPoint;
        newBounds[1][X] = oldBounds[1][X];
        break;
    case SEU:
        newBounds[0] = oldBounds[0];
        newBounds[0][Y] = splitPoint[Y];
        newBounds[1] = splitPoint;
        newBounds[1][Y] = oldBounds[1][Y];
        break;
    case SWU:
        newBounds[0] = oldBounds[0];
        newBounds[1] = splitPoint;
        break;
    }
    childSplit = 0.5 * (newBounds[0] + newBounds[1]);
}

// Static variable to generate node ID's (not threadsafe)
static uint64_t nextNodeId = 1;
StarTree::StarTree(uint64_t maxLeafSize, Vector3d splitPoint,
                   Vector3d minBounds, Vector3d maxBounds) :
    nodeId_(0),
    isLeaf_(true),
    splitPoint_(splitPoint),
    branches_{nullptr, nullptr, nullptr, nullptr,
              nullptr, nullptr, nullptr, nullptr},
    isRoot_(true),
    parent_(nullptr),
    maxLeafSize_(maxLeafSize),
    numStars_(0),
    maxLuminosity_(0),
    sumLuminosity_(0),
    sumColor_(0, 0, 0),
    averageColor_(0, 0, 0),
    treeBounds_{minBounds, maxBounds},
    starBounds_{Vector3d::Zero(), Vector3d::Zero()},
    boundsCenter_(0, 0, 0),
    boundsRadius_(0) {}

StarTree::StarTree(uint64_t maxLeafSize, StarTree* parent) :
    nodeId_(nextNodeId++),
    isLeaf_(true),
    branches_{nullptr, nullptr, nullptr, nullptr,
              nullptr, nullptr, nullptr, nullptr},
    isRoot_(false),
    parent_(parent),
    maxLeafSize_(maxLeafSize),
    numStars_(0),
    maxLuminosity_(0),
    sumLuminosity_(0),
    sumColor_(0, 0, 0),
    averageColor_(0, 0, 0),
    treeBounds_{Vector3d::Zero(), Vector3d::Zero()},
    starBounds_{Vector3d::Zero(), Vector3d::Zero()},
    boundsCenter_(0, 0, 0),
    boundsRadius_(0) {}

// This employs a trick similar to the chmod permission bits
TreeDirection
StarTree::getDirection(const Vector3d& from, const Vector3d& to) {
    bool cx = from[X] >= to[X];
    bool cy = from[Y] >= to[Y];
    bool cz = from[Z] >= to[Z];
    int direction =
        (cx ? 1 : 0) +
        (cy ? 2 : 0) +
        (cz ? 4 : 0);
    return static_cast<TreeDirection>(direction);
}

void
StarTree::addStarMetadata(const Star* star) {
    //std::cerr << "Start" << std::endl;
    numStars_ += 1;

    maxLuminosity_ = fmax(maxLuminosity_, star->lum());
    //std::cerr << "Set max luminocity." << std::endl;
    sumLuminosity_ += star->lum();
    //std::cerr << "Updated sum of luminocities." << std::endl;

    sumColor_ += star->color();
    //std::cerr << "Updated sum of colours." << std::endl;
    averageColor_ = sumColor_ / numStars_;
    //std::cerr << "Updated average colour." << std::endl;

    minVec(starBounds_[0], star->position());
    //std::cerr << "Updated star min bounds." << std::endl;
    maxVec(starBounds_[1], star->position());
    //std::cerr << "Updated star max bounds." << std::endl;

    boundsCenter_ = 0.5 * (starBounds_[0] + starBounds_[1]);
    //std::cerr << "Updated star bounds center." << std::endl;
    boundsRadius_ = (boundsCenter_ - starBounds_[1]).norm();
    //std::cerr << "Updated star bounds radius." << std::endl;
}

void
StarTree::addStar(const Star* star,
                  map<uint64_t,const StarTree*>& treeMap) {
    TreeDirection dir;
    // Always add the metadata
    addStarMetadata(star);

    // Do things differently depending on what type of node we are
    if (isLeaf_) {
        // If we'd go over our maximum leaf size, we need to split up
        if (numStars_ > maxLeafSize_) {
            isLeaf_ = false;
            // Create the empty child nodes
            for (int i = 0; i < 8; i++) {
                assert(branches_[i] == nullptr);
                branches_[i] = new StarTree(maxLeafSize_, this);
                splitBounds(treeBounds_, branches_[i]->treeBounds_,
                            splitPoint_, branches_[i]->splitPoint_,
                            static_cast<TreeDirection>(i));
                uint64_t nodeId = branches_[i]->nodeId();
                assert(treeMap.find(nodeId) == treeMap.end());
                treeMap[nodeId] = branches_[i];
            }

            // Add all the old stars
            for (vector<const Star*>::iterator it = stars_.begin();
                 it != stars_.end(); ++it) {
                dir = getDirection(splitPoint_, (*it)->position());
                branches_[dir]->addStar(*it, treeMap);
            }
            // And don't forget the new one
            dir = getDirection(splitPoint_, star->position());
            branches_[dir]->addStar(star, treeMap);
        } else {
            // Otherwise just add the star to the list
            stars_.push_back(star);
        }
    } else {
        // We're a branch, figure out the child that gets the star
        dir = getDirection(splitPoint_, star->position());
        branches_[dir]->addStar(star, treeMap);
    }
}

static double getDistance(const Vector3d& pointA,
                          const Vector3d& pointB) {
    return (pointA - pointB).norm();
}

// Check if an octant approximately intersects the given sphere
static bool octantApproxIntersect(const Vector3d& point, double radius,
                                  const StarTree* t) {
    const double centerDistance = getDistance(point, t->splitPoint());
    const double subRadius = getDistance(t->splitPoint(),
                                         t->maxTreeBounds());
    return (radius + subRadius) >= centerDistance;
}

// Get all stars within a given radius from a point
void starsInRadius(const Vector3d& point, double radius,
                   vector<const StarTree*>& searchList,
                   vector<const Star*>& starsFound) {
    // Loop until the search list is empty
    while (searchList.size() > 0) {
        // Look at the first thing on the list
        const StarTree* t = searchList[0];
        searchList.erase(searchList.begin());

        // Does this octant possibly intersect our sphere?
        if (octantApproxIntersect(point, radius, t)) {
            if (t->isLeaf()) {
                // If it's a leaf check all the stars
                for (unsigned int i = 0; i < t->numStars(); i++) {
                    if (getDistance(point, (t->stars()[i])->position()) <=
                        radius) {
                        starsFound.push_back(t->stars()[i]);
                    }
                }
            } else {
                // If it's a branch, add all of the subnodes to be
                // searched
                for (int i = 0; i < 8; i++) {
                    const StarTree* tmp =
                        t->branch(static_cast<TreeDirection>(i));
                    searchList.push_back(tmp);
                }
            }
        }
    }
}

static bool canSeeStar(const Vector3d& point, double minLum,
                       const Star* star) {
    const Vector3d& starPosition = star->position();
    const double starDistance = getDistance(point, starPosition);
    const double starLuminosity = star->lum();

    return (starLuminosity /
            (4 * M_PI * starDistance * starDistance)) >= minLum;
}

static bool hasVisibleStars(const Vector3d& point, double minLum,
                            const StarTree* t) {
    const double centerDistance = getDistance(point, t->splitPoint());
    const double subRadius = getDistance(t->splitPoint(),
                                         t->maxTreeBounds());
    const double minDistance = fmax((centerDistance - subRadius), 0);

    return (t->maxLuminosity() /
            (4 * M_PI * minDistance * minDistance)) >= minLum;
}

void visibleStars(const Vector3d& point, double minLuminosity,
                  vector<const StarTree*>& searchList,
                  vector<const Star*>& starsFound) {
    // Loop until the search list is empty
    while (searchList.size() > 0) {
        // Look at the first thing on the list
        const StarTree* t = searchList[0];
        searchList.erase(searchList.begin());

        if (hasVisibleStars(point, minLuminosity, t)) {
            if (t->isLeaf()) {
                for (unsigned int i = 0; i < t->numStars(); i++) {
                    if (canSeeStar(point, minLuminosity, t->stars()[i])) {
                        starsFound.push_back(t->stars()[i]);
                    }
                }
            } else {
                for (int i = 0; i < 8; i++) {
                    const StarTree* tmp =
                        t->branch(static_cast<TreeDirection>(i));
                    searchList.push_back(tmp);
                }
            }
        }
    }
}

static double sigmoid(double x) {
    return 1.0 / (1.0 + exp(x));
}

static double magicFormula(double distance, double blurRadius) {
    double a = log(distance) - log(blurRadius);
    double b = sigmoid((-2.03935397) * a);
    double c = sigmoid(-3.065390091 * a + 1.851112427);

    return ( (b + 1.872968808 * c * (1.0 - c)) /
             (4 * M_PI * distance * distance) );
}

static bool canSeeStarMagic(const Vector3d& point, double minLum,
                            double blurRadius, const Star* star) {
    const Vector3d& starPosition = star->position();
    const double starDistance = getDistance(point, starPosition);
    const double starLuminosity = star->lum();
    const double magic =
        magicFormula(starDistance, blurRadius) * starLuminosity;

    return magic >= minLum;
}

static bool hasVisibleStarsMagic(const Vector3d& point, double minLum,
                                 double blurRadius, const StarTree* t) {
    const double centerDistance = getDistance(point, t->splitPoint());
    const double subRadius = getDistance(t->splitPoint(),
                                         t->maxTreeBounds());
    const double minDistance = fmax((centerDistance - subRadius),
                                    (4.84814e-7));
    const double magic =
        magicFormula(minDistance, blurRadius) * t->maxLuminosity();

    return magic >= minLum;
}

void visibleStarsMagic(const Vector3d& point, double minLuminosity,
                       double blurRadius,
                       vector<const StarTree*>& searchList,
                       vector<const Star*>& starsFound) {
    // Loop until the searchlist is empty
    while (searchList.size() > 0) {
        // Look at the first thing on the list
        const StarTree* t = searchList[0];
        searchList.erase(searchList.begin());

        if (hasVisibleStarsMagic(point, minLuminosity, blurRadius, t)) {
            if (t->isLeaf()) {
                for (unsigned int i = 0; i < t->numStars(); i++) {
                    if (canSeeStarMagic(point, minLuminosity, blurRadius,
                                        t->stars()[i])) {
                        starsFound.push_back(t->stars()[i]);
                    }
                }
            } else {
                for (int i = 0; i < 8; i++) {
                    const StarTree* tmp =
                        t->branch(static_cast<TreeDirection>(i));
                    searchList.push_back(tmp);
                }
            }
        }
    }
}

void visibleOctants(const Vector3d& point, double minLuminosity,
                    vector<const StarTree*>& searchList,
                    vector<uint64_t>& octantsFound) {
    // Loop until the searchlist is empty
    while (searchList.size() > 0) {
        // Look at the first thing on the list
        const StarTree* t = searchList[0];
        searchList.erase(searchList.begin());

        if (hasVisibleStars(point, minLuminosity, t)) {
            if (t->isLeaf()) {
                octantsFound.push_back(t->nodeId());
            } else {
                for (int i = 0; i < 8; i++) {
                    const StarTree* tmp =
                        t->branch(static_cast<TreeDirection>(i));
                    searchList.push_back(tmp);
                }
            }
        }
    }
}

void visibleOctantsMagic(const Vector3d& point, double minLuminosity,
                         double blurRadius,
                         vector<const StarTree*>& searchList,
                         vector<uint64_t>& octantsFound) {
    // Loop until the searchlist is empty
    while (searchList.size() > 0) {
        // Look at the first thing on the list
        const StarTree* t = searchList[0];
        searchList.erase(searchList.begin());

        if (hasVisibleStarsMagic(point, minLuminosity, blurRadius, t)) {
            if (t->isLeaf()) {
                octantsFound.push_back(t->nodeId());
            } else {
                for (int i = 0; i < 8; i++) {
                    const StarTree* tmp =
                        t->branch(static_cast<TreeDirection>(i));
                    searchList.push_back(tmp);
                }
            }
        }
    }
}
} // namespace StarTree


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
 *                                                                            *
 * Redistribution and use in source and binary forms, with or without         *
 * modification, are permitted provided that the following conditions         *
 * are met:                                                                   *
 * * Redistributions of source code must retain the above copyright           *
 *   notice, this list of conditions and the following disclaimer.            *
 * * Redistributions in binary form must reproduce the above copyright        *
 *   notice, this list of conditions and the following disclaimer in the      *
 *   documentation and/or other materials provided with the distribution.     *
 * * Neither the name of ANU nor the names of its contributors may be         *
 *   used to endorse or promote products derived from this software without   *
 *   specific prior written permission.                                       *
 *                                                                            *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE  *
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
 * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE        *
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT         *
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY  *
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF     *
 * SUCH DAMAGE.                                                               *
 ******************************************************************************/

#include <opengv/relative_pose/methods.hpp>
#include <opengv/Indices.hpp>

#include <Eigen/NonLinearOptimization>
#include <Eigen/NumericalDiff>

#include <opengv/OptimizationFunctor.hpp>
#include <opengv/math/arun.hpp>
#include <opengv/math/cayley.hpp>
#include <opengv/relative_pose/modules/main.hpp>
#include <opengv/triangulation/methods.hpp>

#include <cmath>
#include <iostream>

opengv::translation_t
opengv::relative_pose::twopt(
    const RelativeAdapterBase & adapter,
    bool unrotate,
    const std::vector<int> & indices )
{
  assert(indices.size()>1);
  return twopt( adapter, unrotate, indices[0], indices[1] );
};

opengv::translation_t
opengv::relative_pose::twopt(
    const RelativeAdapterBase & adapter,
    bool unrotate,
    size_t index0,
    size_t index1 )
{
  bearingVector_t f1 = adapter.getBearingVector1(index0);
  bearingVector_t f1prime = adapter.getBearingVector2(index0);
  bearingVector_t f2 = adapter.getBearingVector1(index1);
  bearingVector_t f2prime = adapter.getBearingVector2(index1);

  if(unrotate)
  {
    rotation_t R12 = adapter.getR12();
    f1prime = R12 * f1prime;
    f2prime = R12 * f2prime;
  }

  Eigen::Vector3d normal1 = f1.cross(f1prime);
  Eigen::Vector3d normal2 = f2.cross(f2prime);

  translation_t translation = normal1.cross(normal2);
  translation = translation/translation.norm();

  Eigen::Vector3d opticalFlow = f1 - f1prime;
  if( opticalFlow.dot(translation) < 0 )
    translation = -translation;

  return translation;
};

opengv::rotation_t
opengv::relative_pose::twopt_rotationOnly(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  assert(indices.size() > 1);
  return twopt_rotationOnly( adapter, indices[0], indices[1] );
};

opengv::rotation_t
opengv::relative_pose::twopt_rotationOnly(
    const RelativeAdapterBase & adapter,
    size_t index0,
    size_t index1)
{
  Eigen::Vector3d pointsCenter1 =
      adapter.getBearingVector1(index0) + adapter.getBearingVector1(index1);
  Eigen::Vector3d pointsCenter2 =
      adapter.getBearingVector2(index0) + adapter.getBearingVector2(index1);
  pointsCenter1 = pointsCenter1/3.0;
  pointsCenter2 = pointsCenter2/3.0;

  Eigen::MatrixXd Hcross(3,3);
  Hcross = Eigen::Matrix3d::Zero();

  Eigen::Vector3d f = adapter.getBearingVector1(index0) - pointsCenter1;
  Eigen::Vector3d fprime = adapter.getBearingVector2(index0) - pointsCenter2;
  Hcross += fprime * f.transpose();
  f = adapter.getBearingVector1(index1) - pointsCenter1;
  fprime = adapter.getBearingVector2(index1) - pointsCenter2;
  Hcross += fprime * f.transpose();

  return math::arun(Hcross);
};

namespace opengv
{
namespace relative_pose
{

rotation_t rotationOnly(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 2);

  Eigen::Vector3d pointsCenter1 = Eigen::Vector3d::Zero();
  Eigen::Vector3d pointsCenter2 = Eigen::Vector3d::Zero();

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    pointsCenter1 += adapter.getBearingVector1(indices[i]);
    pointsCenter2 += adapter.getBearingVector2(indices[i]);
  }

  pointsCenter1 = pointsCenter1 / numberCorrespondences;
  pointsCenter2 = pointsCenter2 / numberCorrespondences;

  Eigen::MatrixXd Hcross(3,3);
  Hcross = Eigen::Matrix3d::Zero();

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    Eigen::Vector3d f = adapter.getBearingVector1(indices[i]) - pointsCenter1;
    Eigen::Vector3d fprime =
        adapter.getBearingVector2(indices[i]) - pointsCenter2;
    Hcross += fprime * f.transpose();
  }

  return math::arun(Hcross);
};

}
}

opengv::rotation_t
opengv::relative_pose::rotationOnly( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return rotationOnly(adapter,idx);
};

opengv::rotation_t
opengv::relative_pose::rotationOnly(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return rotationOnly(adapter,idx);
};

namespace opengv
{
namespace relative_pose
{

complexEssentials_t fivept_stewenius(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 4);

  Eigen::MatrixXd Q(numberCorrespondences,9);
  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    //bearingVector_t f = adapter.getBearingVector1(indices[i]);
    //bearingVector_t fprime = adapter.getBearingVector2(indices[i]);
    //Stewenius' algorithm is computing the inverse transformation, so we simply
    //invert the input here
    bearingVector_t f = adapter.getBearingVector2(indices[i]);
    bearingVector_t fprime = adapter.getBearingVector1(indices[i]);
    Eigen::Matrix<double,1,9> row;
    row <<  f[0]*fprime[0], f[1]*fprime[0], f[2]*fprime[0],
        f[0]*fprime[1], f[1]*fprime[1], f[2]*fprime[1],
        f[0]*fprime[2], f[1]*fprime[2], f[2]*fprime[2];
    Q.row(i) = row;
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVD(Q, Eigen::ComputeFullV );
  Eigen::Matrix<double,9,4> EE = SVD.matrixV().block(0,5,9,4);
  complexEssentials_t complexEssentials;
  modules::fivept_stewenius_main(EE,complexEssentials);
  return complexEssentials;
};

}
}

opengv::complexEssentials_t
opengv::relative_pose::fivept_stewenius( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return fivept_stewenius(adapter,idx);
};

opengv::complexEssentials_t
opengv::relative_pose::fivept_stewenius(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return fivept_stewenius(adapter,idx);
};

namespace opengv
{
namespace relative_pose
{

essentials_t fivept_nister(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 4);

  Eigen::MatrixXd Q(numberCorrespondences,9);
  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    //bearingVector_t f = adapter.getBearingVector1(indices[i]);
    //bearingVector_t fprime = adapter.getBearingVector2(indices[i]);
    //Nister's algorithm is computing the inverse transformation, so we simply
    //invert the input here
    bearingVector_t f = adapter.getBearingVector2(indices[i]);
    bearingVector_t fprime = adapter.getBearingVector1(indices[i]);
    Eigen::Matrix<double,1,9> row;
    row <<  f[0]*fprime[0], f[1]*fprime[0], f[2]*fprime[0],
        f[0]*fprime[1], f[1]*fprime[1], f[2]*fprime[1],
        f[0]*fprime[2], f[1]*fprime[2], f[2]*fprime[2];
    Q.row(i) = row;
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVD(Q, Eigen::ComputeFullV );
  Eigen::Matrix<double,9,4> EE = SVD.matrixV().block(0,5,9,4);
  essentials_t essentials;
  modules::fivept_nister_main(EE,essentials);

  return essentials;
};

}
}

opengv::essentials_t
opengv::relative_pose::fivept_nister( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return fivept_nister(adapter,idx);
};

opengv::essentials_t
opengv::relative_pose::fivept_nister(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return fivept_nister(adapter,idx);
};

opengv::rotations_t
opengv::relative_pose::fivept_kneip(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences == 5);

  Eigen::Matrix<double,3,5> f1;
  Eigen::Matrix<double,3,5> f2;

  for(size_t i = 0; i < numberCorrespondences; i++)
  {
    f1.col(i) = adapter.getBearingVector1(indices[i]);
    f2.col(i) = adapter.getBearingVector2(indices[i]);
  }

  rotations_t rotations;
  modules::fivept_kneip_main( f1, f2, rotations );
  return rotations;
}

namespace opengv
{
namespace relative_pose
{

essentials_t sevenpt(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 6);

  Eigen::MatrixXd A(numberCorrespondences,9);

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    //bearingVector_t f1 = adapter.getBearingVector1(indices[i]);
    //bearingVector_t f2 = adapter.getBearingVector2(indices[i]);
    //The seven-point is computing the inverse transformation, which is why we
    //invert the input
    bearingVector_t f1 = adapter.getBearingVector2(indices[i]);
    bearingVector_t f2 = adapter.getBearingVector1(indices[i]);

    A.block<1,3>(i,0) = f2[0] * f1.transpose();
    A.block<1,3>(i,3) = f2[1] * f1.transpose();
    A.block<1,3>(i,6) = f2[2] * f1.transpose();
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVD(
      A,
      Eigen::ComputeFullU | Eigen::ComputeFullV );

  Eigen::Matrix<double,9,1> f1 = SVD.matrixV().col(8);
  Eigen::Matrix<double,9,1> f2 = SVD.matrixV().col(7);

  Eigen::MatrixXd F1_temp(3,3);
  F1_temp.col(0) = f1.block<3,1>(0,0);
  F1_temp.col(1) = f1.block<3,1>(3,0);
  F1_temp.col(2) = f1.block<3,1>(6,0);
  essential_t F1 = F1_temp.transpose();

  Eigen::MatrixXd F2_temp(3,3);
  F2_temp.col(0) = f2.block<3,1>(0,0);
  F2_temp.col(1) = f2.block<3,1>(3,0);
  F2_temp.col(2) = f2.block<3,1>(6,0);
  essential_t F2 = F2_temp.transpose();

  double eps = 0.00000001;
  essentials_t essentials;

  if( fabs(F1.determinant()) < eps || numberCorrespondences > 7 )
  {
    essentials.push_back(F1);
  }
  else
  {
    essential_t M = F2.inverse() * F1;
    Eigen::EigenSolver< essential_t > Eig(M,true);
    Eigen::Matrix< std::complex<double>,3,1 > D = Eig.eigenvalues();

    double val1 = fabs(D(0,0).imag());
    double val2 = fabs(D(1,0).imag());
    double val3 = fabs(D(2,0).imag());

    if( val1 < eps && val2 < eps && val3 < eps )
    {
      essentials.push_back( F1 - D(0,0).real() * F2 );
      essentials.push_back( F1 - D(1,0).real() * F2 );
      essentials.push_back( F1 - D(2,0).real() * F2 );
    }
    else
    {
      double min = val1;
      int minIndex = 0;
      if( val2 < min )
      {
        min = val2;
        minIndex = 1;
      }
      if( val3 < min )
        minIndex = 2;

      essentials.push_back( F1 - D(minIndex,0).real() * F2 );
    }
  }

  return essentials;
}

}
}

opengv::essentials_t
opengv::relative_pose::sevenpt( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return sevenpt(adapter,idx);
}

opengv::essentials_t
opengv::relative_pose::sevenpt(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return sevenpt(adapter,idx);
}

namespace opengv
{
namespace relative_pose
{

essential_t eightpt(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 7);

  Eigen::MatrixXd A(numberCorrespondences,9);

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    //bearingVector_t f1 = adapter.getBearingVector1(indices[i]);
    //bearingVector_t f2 = adapter.getBearingVector2(indices[i]);
    //The eight-point essentially computes the inverse transformation, which is
    //why we invert the input here
    bearingVector_t f1 = adapter.getBearingVector2(indices[i]);
    bearingVector_t f2 = adapter.getBearingVector1(indices[i]);

    A.block<1,3>(i,0) = f2[0] * f1.transpose();
    A.block<1,3>(i,3) = f2[1] * f1.transpose();
    A.block<1,3>(i,6) = f2[2] * f1.transpose();
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVD(
      A,
      Eigen::ComputeFullU | Eigen::ComputeFullV );
  Eigen::Matrix<double,9,1> f = SVD.matrixV().col(8);

  Eigen::MatrixXd F_temp(3,3);
  F_temp.col(0) = f.block<3,1>(0,0);
  F_temp.col(1) = f.block<3,1>(3,0);
  F_temp.col(2) = f.block<3,1>(6,0);
  essential_t F = F_temp.transpose();

  Eigen::JacobiSVD< Eigen::MatrixXd > SVD2(
      F,
      Eigen::ComputeFullU | Eigen::ComputeFullV );
  Eigen::Matrix3d S = Eigen::Matrix3d::Zero();
  S(0,0) = SVD2.singularValues()[0];
  S(1,1) = SVD2.singularValues()[1];

  Eigen::Matrix3d U = SVD2.matrixU();
  Eigen::Matrix3d Vtr = SVD2.matrixV().transpose();

  essential_t essential = U * S * Vtr;
  return essential;
}

}
}

opengv::essential_t
opengv::relative_pose::eightpt( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return eightpt(adapter,idx);
}

opengv::essential_t
opengv::relative_pose::eightpt(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return eightpt(adapter,idx);
}

namespace opengv
{
namespace relative_pose
{

rotation_t eigensolver(
    const RelativeAdapterBase & adapter,
    const Indices & indices,
    eigensolverOutput_t & output,
    bool useWeights )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 4);

  Eigen::Matrix3d xxF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d yyF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d zzF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d xyF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d yzF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d zxF = Eigen::Matrix3d::Zero();

  //compute the norm of all the scores
  double norm = 0.0;
  for(size_t i=0; i < numberCorrespondences; i++)
    norm += pow(adapter.getWeight(indices[i]),2);
  norm = sqrt(norm);

  //Fill summation terms
  for(size_t i=0; i < numberCorrespondences; i++)
  {
    bearingVector_t f1 = adapter.getBearingVector1(indices[i]);
    bearingVector_t f2 = adapter.getBearingVector2(indices[i]);
    Eigen::Matrix3d F = f2*f2.transpose();

    double weight = 1.0;
    if( useWeights )
      weight = adapter.getWeight(indices[i])/norm;

    xxF = xxF + weight*f1[0]*f1[0]*F;
    yyF = yyF + weight*f1[1]*f1[1]*F;
    zzF = zzF + weight*f1[2]*f1[2]*F;
    xyF = xyF + weight*f1[0]*f1[1]*F;
    yzF = yzF + weight*f1[1]*f1[2]*F;
    zxF = zxF + weight*f1[2]*f1[0]*F;
  }

  //Do minimization
  modules::eigensolver_main(xxF,yyF,zzF,xyF,yzF,zxF,output);

  //Correct the translation
  bearingVector_t f1 = adapter.getBearingVector1(indices[0]);
  bearingVector_t f2 = adapter.getBearingVector2(indices[0]);
  f2 = output.rotation * f2;
  Eigen::Vector3d opticalFlow = f1 - f2;
  if( opticalFlow.dot(output.translation) < 0.0 )
    output.translation = -output.translation;

  return output.rotation;
}

}
}

opengv::rotation_t
opengv::relative_pose::eigensolver(
    const RelativeAdapterBase & adapter,
    eigensolverOutput_t & output,
    bool useWeights )
{
  Indices idx(adapter.getNumberCorrespondences());
  return eigensolver(adapter,idx,output,useWeights);
}

opengv::rotation_t
opengv::relative_pose::eigensolver(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices,
    eigensolverOutput_t & output,
    bool useWeights )
{
  Indices idx(indices);
  return eigensolver(adapter,idx,output,useWeights);
}

opengv::rotation_t
opengv::relative_pose::eigensolver(
    const RelativeAdapterBase & adapter,
    bool useWeights )
{
  eigensolverOutput_t output;
  output.rotation = adapter.getR12();
  return eigensolver(adapter,output,useWeights);
}

opengv::rotation_t
opengv::relative_pose::eigensolver(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices,
    bool useWeights )
{
  eigensolverOutput_t output;
  output.rotation = adapter.getR12();
  return eigensolver(adapter,indices,output,useWeights);
}

namespace opengv
{
namespace relative_pose
{

rotations_t sixpt(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences == 6);

  Eigen::Matrix<double,6,6> L1;
  Eigen::Matrix<double,6,6> L2;

  for(size_t i = 0; i < numberCorrespondences; i++)
  {
    bearingVector_t f1 =
        adapter.getCamRotation1(indices[i]) * adapter.getBearingVector1(indices[i]);
    bearingVector_t f2 =
        adapter.getCamRotation2(indices[i]) * adapter.getBearingVector2(indices[i]);

    L1.block<3,1>(0,i) = f1;
    L2.block<3,1>(0,i) = f2;

    L1.block<3,1>(3,i) = f1.cross(adapter.getCamOffset1(indices[i]));
    L2.block<3,1>(3,i) = f2.cross(adapter.getCamOffset2(indices[i]));
  }

  rotations_t solutions;
  modules::sixpt_main( L1, L2, solutions );
  return solutions;
}

}
}

opengv::rotations_t
opengv::relative_pose::sixpt(
    const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return sixpt(adapter,idx);
}

opengv::rotations_t
opengv::relative_pose::sixpt(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return sixpt(adapter,idx);
}

namespace opengv
{
namespace relative_pose
{

transformation_t ge(
    const RelativeAdapterBase & adapter,
    const Indices & indices,
    geOutput_t & output,
    bool useWeights )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 5);

  Eigen::Matrix3d xxF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d yyF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d zzF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d xyF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d yzF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d zxF = Eigen::Matrix3d::Zero();

  Eigen::Matrix<double,3,9> x1P = Eigen::Matrix<double,3,9>::Zero();
  Eigen::Matrix<double,3,9> y1P = Eigen::Matrix<double,3,9>::Zero();
  Eigen::Matrix<double,3,9> z1P = Eigen::Matrix<double,3,9>::Zero();
  Eigen::Matrix<double,3,9> x2P = Eigen::Matrix<double,3,9>::Zero();
  Eigen::Matrix<double,3,9> y2P = Eigen::Matrix<double,3,9>::Zero();
  Eigen::Matrix<double,3,9> z2P = Eigen::Matrix<double,3,9>::Zero();

  Eigen::Matrix<double,9,9> m11P = Eigen::Matrix<double,9,9>::Zero();
  Eigen::Matrix<double,9,9> m12P = Eigen::Matrix<double,9,9>::Zero();
  Eigen::Matrix<double,9,9> m22P = Eigen::Matrix<double,9,9>::Zero();

  //compute the norm of all the scores
  double norm = 0.0;
  for(size_t i=0; i < numberCorrespondences; i++)
    norm += pow(adapter.getWeight(indices[i]),2);
  norm = sqrt(norm);

  //Fill summation terms
  for(size_t i=0; i < numberCorrespondences; i++)
  {
    //get the weight of this feature
    double weight = 1.0;
    if( useWeights )
      weight = adapter.getWeight(indices[i])/norm;

    //unrotate the bearing vectors
    bearingVector_t f1 = adapter.getCamRotation1(indices[i]) *
        adapter.getBearingVector1(indices[i]);
    bearingVector_t f2 = adapter.getCamRotation2(indices[i]) *
        adapter.getBearingVector2(indices[i]);

    //compute the standard summation terms
    Eigen::Matrix3d F = f2*f2.transpose();

    xxF = xxF + weight*f1[0]*f1[0]*F;
    yyF = yyF + weight*f1[1]*f1[1]*F;
    zzF = zzF + weight*f1[2]*f1[2]*F;
    xyF = xyF + weight*f1[0]*f1[1]*F;
    yzF = yzF + weight*f1[1]*f1[2]*F;
    zxF = zxF + weight*f1[2]*f1[0]*F;

    //now compute the "cross"-summation terms
    Eigen::Vector3d t1 = adapter.getCamOffset1(indices[i]);
    Eigen::Vector3d t2 = adapter.getCamOffset2(indices[i]);

    Eigen::Matrix<double,1,9> f2_19;
    double temp = f1[1]*t1[2]-f1[2]*t1[1];
    f2_19(0,0) = f2[0] * temp;
    f2_19(0,1) = f2[1] * temp;
    f2_19(0,2) = f2[2] * temp;
    temp = f1[2]*t1[0]-f1[0]*t1[2];
    f2_19(0,3) = f2[0] * temp;
    f2_19(0,4) = f2[1] * temp;
    f2_19(0,5) = f2[2] * temp;
    temp = f1[0]*t1[1]-f1[1]*t1[0];
    f2_19(0,6) = f2[0] * temp;
    f2_19(0,7) = f2[1] * temp;
    f2_19(0,8) = f2[2] * temp;

    Eigen::Matrix<double,1,9> f1_19;
    temp = f2[1]*t2[2]-f2[2]*t2[1];
    f1_19(0,0) = f1[0] * temp;
    f1_19(0,1) = f1[1] * temp;
    f1_19(0,2) = f1[2] * temp;
    temp = f2[2]*t2[0]-f2[0]*t2[2];
    f1_19(0,3) = f1[0] * temp;
    f1_19(0,4) = f1[1] * temp;
    f1_19(0,5) = f1[2] * temp;
    temp = f2[0]*t2[1]-f2[1]*t2[0];
    f1_19(0,6) = f1[0] * temp;
    f1_19(0,7) = f1[1] * temp;
    f1_19(0,8) = f1[2] * temp;

    if( useWeights )
    {
      x1P = x1P + ( (weight * f1[0]) * f2 ) * f1_19;
      y1P = y1P + ( (weight * f1[1]) * f2 ) * f1_19;
      z1P = z1P + ( (weight * f1[2]) * f2 ) * f1_19;

      x2P = x2P + ( (weight * f1[0]) * f2 ) * f2_19;
      y2P = y2P + ( (weight * f1[1]) * f2 ) * f2_19;
      z2P = z2P + ( (weight * f1[2]) * f2 ) * f2_19;

      m11P = m11P - ( weight * f1_19.transpose() ) * f1_19;
      m22P = m22P - ( weight * f2_19.transpose() ) * f2_19;
      m12P = m12P - ( weight * f2_19.transpose() ) * f1_19;
    }
    else
    {
      x1P = x1P + ( f1[0] * f2 ) * f1_19;
      y1P = y1P + ( f1[1] * f2 ) * f1_19;
      z1P = z1P + ( f1[2] * f2 ) * f1_19;

      x2P = x2P + ( f1[0]) * f2 * f2_19;
      y2P = y2P + ( f1[1]) * f2 * f2_19;
      z2P = z2P + ( f1[2]) * f2 * f2_19;

      m11P = m11P - f1_19.transpose() * f1_19;
      m22P = m22P - f2_19.transpose() * f2_19;
      m12P = m12P - f2_19.transpose() * f1_19;
    }
  }

  Eigen::Vector3d pointsCenter1 = Eigen::Vector3d::Zero();
  Eigen::Vector3d pointsCenter2 = Eigen::Vector3d::Zero();

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    pointsCenter1 += adapter.getCamRotation1(indices[i]) *
        adapter.getBearingVector1(indices[i]);
    pointsCenter2 += adapter.getCamRotation2(indices[i]) *
        adapter.getBearingVector2(indices[i]);
  }

  pointsCenter1 = pointsCenter1 / numberCorrespondences;
  pointsCenter2 = pointsCenter2 / numberCorrespondences;

  Eigen::MatrixXd Hcross(3,3);
  Hcross = Eigen::Matrix3d::Zero();

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    Eigen::Vector3d f =      adapter.getCamRotation1(indices[i]) *
        adapter.getBearingVector1(indices[i]) - pointsCenter1;
    Eigen::Vector3d fprime = adapter.getCamRotation2(indices[i]) *
        adapter.getBearingVector2(indices[i]) - pointsCenter2;
    Hcross += fprime * f.transpose();
  }

  rotation_t startingRotation = math::arun(Hcross);

  //Do minimization
  modules::ge_main2(
      xxF, yyF, zzF, xyF, yzF, zxF,
      x1P, y1P, z1P, x2P, y2P, z2P,
      m11P, m12P, m22P, math::rot2cayley(startingRotation), output);

  //return output.rotation;
  transformation_t transformation;
  transformation.block<3,3>(0,0) = output.rotation;
  transformation.col(3) = output.translation.block<3,1>(0,0);

  return transformation;
}

}
}

opengv::transformation_t
opengv::relative_pose::ge(
    const RelativeAdapterBase & adapter,
    geOutput_t & output,
    bool useWeights )
{
  Indices idx(adapter.getNumberCorrespondences());
  return ge(adapter,idx,output,useWeights);
}

opengv::transformation_t
opengv::relative_pose::ge(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices,
    geOutput_t & output,
    bool useWeights )
{
  Indices idx(indices);
  return ge(adapter,idx,output,useWeights);
}

opengv::transformation_t
opengv::relative_pose::ge( const RelativeAdapterBase & adapter, bool useWeights )
{
  geOutput_t output;
  //output.rotation = adapter.getR12(); //finding starting value using arun
  return ge(adapter,output,useWeights);
}

opengv::transformation_t
opengv::relative_pose::ge(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices,
    bool useWeights )
{
  geOutput_t output;
  //output.rotation = adapter.getR12(); //finding starting value using arun
  return ge(adapter,indices,output,useWeights);
}

namespace opengv
{
namespace relative_pose
{

transformation_t seventeenpt(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 16);

  Eigen::MatrixXd AE(numberCorrespondences,9);
  Eigen::MatrixXd AR(numberCorrespondences,9);

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    bearingVector_t d1 = adapter.getBearingVector1(indices[i]);
    bearingVector_t d2 = adapter.getBearingVector2(indices[i]);
    translation_t v1 = adapter.getCamOffset1(indices[i]);
    translation_t v2 = adapter.getCamOffset2(indices[i]);
    rotation_t R1 = adapter.getCamRotation1(indices[i]);
    rotation_t R2 = adapter.getCamRotation2(indices[i]);

    //unrotate the bearing-vectors to express everything in the body frame
    d1 = R1*d1;
    d2 = R2*d2;

    //generate the Plücker line coordinates
    Eigen::Matrix<double,6,1> l1;
    l1.block<3,1>(0,0) = d1;
    l1.block<3,1>(3,0) = v1.cross(d1);
    Eigen::Matrix<double,6,1> l2;
    l2.block<3,1>(0,0) = d2;
    l2.block<3,1>(3,0) = v2.cross(d2);

    //fill line of matrix A
    AE(i,0) = l2[0]*l1[0];
    AE(i,1) = l2[0]*l1[1];
    AE(i,2) = l2[0]*l1[2];
    AE(i,3) = l2[1]*l1[0];
    AE(i,4) = l2[1]*l1[1];
    AE(i,5) = l2[1]*l1[2];
    AE(i,6) = l2[2]*l1[0];
    AE(i,7) = l2[2]*l1[1];
    AE(i,8) = l2[2]*l1[2];

    AR(i,0) = l2[0]*l1[3]+l2[3]*l1[0];
    AR(i,1) = l2[0]*l1[4]+l2[3]*l1[1];
    AR(i,2) = l2[0]*l1[5]+l2[3]*l1[2];
    AR(i,3) = l2[1]*l1[3]+l2[4]*l1[0];
    AR(i,4) = l2[1]*l1[4]+l2[4]*l1[1];
    AR(i,5) = l2[1]*l1[5]+l2[4]*l1[2];
    AR(i,6) = l2[2]*l1[3]+l2[5]*l1[0];
    AR(i,7) = l2[2]*l1[4]+l2[5]*l1[1];
    AR(i,8) = l2[2]*l1[5]+l2[5]*l1[2];
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVDARP(
      AR,
      Eigen::ComputeThinU | Eigen::ComputeThinV );

  Eigen::VectorXd sigma_ = SVDARP.singularValues();
  double pinvtoler_ =
      sigma_(0)*numberCorrespondences*NumTraits<double>::epsilon();
  Eigen::MatrixXd SigmaInverse_(9,9);
  SigmaInverse_ = Eigen::MatrixXd::Zero(9,9);
  for ( size_t i=0; i < 9; ++i)
  {
    double temp = sigma_(i);
    if( temp > pinvtoler_ )
      SigmaInverse_(i,i) = 1.0/temp;
  }

  Eigen::MatrixXd ARP(9,numberCorrespondences);
  ARP = SVDARP.matrixV()*SigmaInverse_*SVDARP.matrixU().transpose();

  Eigen::MatrixXd B(numberCorrespondences,numberCorrespondences);
  B = -Eigen::MatrixXd::Identity(numberCorrespondences,numberCorrespondences);
  B = B + AR*ARP;

  Eigen::MatrixXd C = B*AE;

  Eigen::JacobiSVD< Eigen::MatrixXd > SVDE(
      C,
      Eigen::ComputeThinU | Eigen::ComputeThinV );
  Eigen::Matrix<double,9,1> e = SVDE.matrixV().col(8);

  Eigen::MatrixXd E_temp(3,3);
  E_temp.col(0) = e.block<3,1>(0,0);
  E_temp.col(1) = e.block<3,1>(3,0);
  E_temp.col(2) = e.block<3,1>(6,0);
  essential_t E = E_temp.transpose();

  Eigen::JacobiSVD< Eigen::MatrixXd > SVDR(
      E,
      Eigen::ComputeFullV | Eigen::ComputeFullU );

  Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
  W(0,1) = -1.0;
  W(1,0) = 1.0;
  W(2,2) = 1.0;

  // get possible rotation and translation vectors
  rotation_t Ra = SVDR.matrixU() * W * SVDR.matrixV().transpose();
  rotation_t Rb = SVDR.matrixU() * W.transpose() * SVDR.matrixV().transpose();

  // change sign if det = -1
  if( Ra.determinant() < 0 ) Ra = -Ra;
  if( Rb.determinant() < 0 ) Rb = -Rb;

  Ra.transposeInPlace();
  Rb.transposeInPlace();

  Eigen::MatrixXd A_tra(numberCorrespondences,3);
  Eigen::MatrixXd A_trb(numberCorrespondences,3);
  Eigen::VectorXd b_tra(numberCorrespondences);
  Eigen::VectorXd b_trb(numberCorrespondences);

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    bearingVector_t d1 = adapter.getBearingVector1(indices[i]);
    bearingVector_t d2 = adapter.getBearingVector2(indices[i]);
    translation_t v1 = adapter.getCamOffset1(indices[i]);
    translation_t v2 = adapter.getCamOffset2(indices[i]);
    rotation_t R1 = adapter.getCamRotation1(indices[i]);
    rotation_t R2 = adapter.getCamRotation2(indices[i]);

    //unrotate the bearing-vectors to express everything in the body frame
    d1 = R1*d1;
    d2 = R2*d2;

    A_tra(i,0) = d1[2]*d2[0]*Ra(1,0)+d1[2]*d2[1]*Ra(1,1)+d1[2]*d2[2]*Ra(1,2)
                -d1[1]*d2[0]*Ra(2,0)-d1[1]*d2[1]*Ra(2,1)-d1[1]*d2[2]*Ra(2,2);
    A_tra(i,1) = d1[0]*d2[0]*Ra(2,0)+d1[0]*d2[1]*Ra(2,1)+d1[0]*d2[2]*Ra(2,2)
                -d1[2]*d2[0]*Ra(0,0)-d1[2]*d2[1]*Ra(0,1)-d1[2]*d2[2]*Ra(0,2);
    A_tra(i,2) = d1[1]*d2[0]*Ra(0,0)+d1[1]*d2[1]*Ra(0,1)+d1[1]*d2[2]*Ra(0,2)
                -d1[0]*d2[0]*Ra(1,0)-d1[0]*d2[1]*Ra(1,1)-d1[0]*d2[2]*Ra(1,2);

    A_trb(i,0) = d1[2]*d2[0]*Rb(1,0)+d1[2]*d2[1]*Rb(1,1)+d1[2]*d2[2]*Rb(1,2)
                -d1[1]*d2[0]*Rb(2,0)-d1[1]*d2[1]*Rb(2,1)-d1[1]*d2[2]*Rb(2,2);
    A_trb(i,1) = d1[0]*d2[0]*Rb(2,0)+d1[0]*d2[1]*Rb(2,1)+d1[0]*d2[2]*Rb(2,2)
                -d1[2]*d2[0]*Rb(0,0)-d1[2]*d2[1]*Rb(0,1)-d1[2]*d2[2]*Rb(0,2);
    A_trb(i,2) = d1[1]*d2[0]*Rb(0,0)+d1[1]*d2[1]*Rb(0,1)+d1[1]*d2[2]*Rb(0,2)
                -d1[0]*d2[0]*Rb(1,0)-d1[0]*d2[1]*Rb(1,1)-d1[0]*d2[2]*Rb(1,2);

    Eigen::Vector3d temp1 = v1.cross(d1);
    Eigen::Vector3d temp2 = v2.cross(d2);
    b_tra(i) = -d1.dot(Ra*temp2) -temp1.dot(Ra*d2);
    b_trb(i) = -d1.dot(Rb*temp2) -temp1.dot(Rb*d2);
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVDa(
      A_tra,
      Eigen::ComputeThinU | Eigen::ComputeThinV );

  Eigen::VectorXd sigma = SVDa.singularValues();
  double pinvtoler =
      numberCorrespondences*sigma(0)*NumTraits<double>::epsilon();
  Eigen::MatrixXd SigmaInverse(3,3);
  SigmaInverse = Eigen::MatrixXd::Zero(3,3);
  for ( size_t i=0; i < 3; ++i)
  {
    double temp = sigma(i);
    if( temp > pinvtoler )
      SigmaInverse(i,i) = 1.0/temp;
  }

  Eigen::MatrixXd PI(3,numberCorrespondences);
  PI = SVDa.matrixV()*SigmaInverse*SVDa.matrixU().transpose();
  Eigen::Vector3d ta = PI*b_tra;

  Eigen::JacobiSVD< Eigen::MatrixXd > SVDb(
      A_trb,
      Eigen::ComputeThinU | Eigen::ComputeThinV );

  sigma = SVDb.singularValues();
  pinvtoler = numberCorrespondences*sigma(0)*NumTraits<double>::epsilon();
  SigmaInverse = Eigen::MatrixXd::Zero(3,3);
  for ( size_t i=0; i < 3; ++i)
  {
    double temp = sigma(i);
    if( temp > pinvtoler )
      SigmaInverse(i,i) = 1.0/temp;
  }

  PI = SVDb.matrixV()*SigmaInverse*SVDb.matrixU().transpose();
  Eigen::Vector3d tb = PI*b_trb;

  Eigen::VectorXd fita = A_tra * ta - b_tra;
  Eigen::VectorXd fitb = A_trb * tb - b_trb;

  transformation_t transformation;
  if( fita.norm() < fitb.norm() )
  {
    transformation.block<3,3>(0,0) = Ra;
    transformation.col(3) = ta;
  }
  else
  {
    transformation.block<3,3>(0,0) = Rb;
    transformation.col(3) = tb;
  }

  return transformation;
}

}
}

opengv::transformation_t
opengv::relative_pose::seventeenpt( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return seventeenpt(adapter,idx);
}

opengv::transformation_t
opengv::relative_pose::seventeenpt(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return seventeenpt(adapter,idx);
}

namespace opengv
{
namespace relative_pose
{

struct OptimizeNonlinearFunctor1 : OptimizationFunctor<double>
{
  RelativeAdapterBase & _adapter;
  const Indices & _indices;

  OptimizeNonlinearFunctor1(
      RelativeAdapterBase & adapter,
      const Indices & indices ) :
      OptimizationFunctor<double>(6,indices.size()),
      _adapter(adapter),
      _indices(indices) {}

  int operator()(const VectorXd &x, VectorXd &fvec) const
  {
    assert( x.size() == 6 );
    assert( (unsigned int) fvec.size() == _indices.size());

    //compute the current position
    translation_t translation = x.block<3,1>(0,0);
    cayley_t cayley = x.block<3,1>(3,0);
    rotation_t rotation = math::cayley2rot(cayley);

    Eigen::Matrix<double,4,1> p_hom;
    p_hom[3] = 1.0;

    for( size_t i = 0; i < _indices.size(); i++ )
    {
      translation_t cam1Offset = _adapter.getCamOffset1(_indices[i]);
      rotation_t cam1Rotation = _adapter.getCamRotation1(_indices[i]);
      translation_t cam2Offset = _adapter.getCamOffset2(_indices[i]);
      rotation_t cam2Rotation = _adapter.getCamRotation2(_indices[i]);

      translation_t directTranslation =
          cam1Rotation.transpose() *
          ((translation - cam1Offset) + rotation * cam2Offset);
      rotation_t directRotation =
          cam1Rotation.transpose() * rotation * cam2Rotation;

      _adapter.sett12(directTranslation);
      _adapter.setR12(directRotation);

      transformation_t inverseSolution;
      inverseSolution.block<3,3>(0,0) = directRotation.transpose();
      inverseSolution.col(3) =
          -inverseSolution.block<3,3>(0,0)*directTranslation;

      p_hom.block<3,1>(0,0) =
          opengv::triangulation::triangulate2(_adapter,_indices[i]);
      bearingVector_t reprojection1 = p_hom.block<3,1>(0,0);
      bearingVector_t reprojection2 = inverseSolution * p_hom;
      reprojection1 = reprojection1 / reprojection1.norm();
      reprojection2 = reprojection2 / reprojection2.norm();
      bearingVector_t f1 = _adapter.getBearingVector1(_indices[i]);
      bearingVector_t f2 = _adapter.getBearingVector2(_indices[i]);

      //bearing-vector based outlier criterium (select threshold accordingly):
      //1-(f1'*f2) = 1-cos(alpha) \in [0:2]
      double reprojError1 = 1.0 - (f1.transpose() * reprojection1);
      double reprojError2 = 1.0 - (f2.transpose() * reprojection2);
      double factor = 1.0;
      fvec[i] = factor*(reprojError1 + reprojError2);
    }

    return 0;
  }
};

transformation_t optimize_nonlinear(
    RelativeAdapterBase & adapter,
    const Indices & indices )
{
  const int n=6;
  VectorXd x(n);

  x.block<3,1>(0,0) = adapter.gett12();
  x.block<3,1>(3,0) = math::rot2cayley(adapter.getR12());

  OptimizeNonlinearFunctor1 functor( adapter, indices );
  NumericalDiff<OptimizeNonlinearFunctor1> numDiff(functor);
  LevenbergMarquardt< NumericalDiff<OptimizeNonlinearFunctor1> >
      lm(numDiff);

  lm.resetParameters();
  lm.parameters.ftol = 1.E1*NumTraits<double>::epsilon();
  lm.parameters.xtol = 1.E1*NumTraits<double>::epsilon();
  lm.parameters.maxfev = 1000;
  lm.minimize(x);

  transformation_t transformation;
  transformation.col(3) = x.block<3,1>(0,0);
  transformation.block<3,3>(0,0) = math::cayley2rot(x.block<3,1>(3,0));
  return transformation;
}

}
}

opengv::transformation_t
opengv::relative_pose::optimize_nonlinear( RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return optimize_nonlinear(adapter,idx);
}

opengv::transformation_t
opengv::relative_pose::optimize_nonlinear(
    RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return optimize_nonlinear(adapter,idx);
}


#include <assert.h>
#include <unistd.h>

#include <sstream>
//#include <boost/algorithm/string.hpp>
#include <algorithm> // for is_permutation
#include <tuple>
#include <functional>
#include <iterator>

#include "./permutation.h"

#include "./dm_english.h"
#include "./dm_perm.h"
#include "./sagemath_alter.h"

Permutation::Permutation(){}

Permutation::Permutation(const int n)
{
	if(n==-1) non_perm();
	else identify(n);
}

Permutation::Permutation(const std::vector<int> vec)
{
	std::vector<int>::const_iterator it;
	for(it = vec.begin() ; it != vec.end() ; ++it){
		perm.push_back(*it);
	}
	label.resize( length() );
	computeInverse();
// 	canonization();
}

// construct a canonical permutation for G (induced by modules)
// for parallel, series, prime modules.
Permutation::Permutation(const int n, const ModuleType type)
{
	switch(type){
	case FEUILLE:
		identify(1);
		break;
		      /*
		    case PREMIER:
			// This case will not be reached
		      prime(G);
		      break;
		      */
	case PARALLELE:
		identify(n);
		break;
	case SERIE:
		reverse(n);
		break;
	default:
		std::cerr << "Error! Strange the module type.\n\n";
	}
}

  // For a prime module.
  // ここで sagemath を使っている．
  // sagemath を使わなければ，1000倍は早くなるはず．
void Permutation::prime(const graphe G)
{
	std::vector< std::vector< int > > adj_matrix( G.n, std::vector< int >( G.n ) );
	for ( int i = 0; i < G.n; ++i )
	{
		for ( adj* adj_list = G.G[i]; adj_list != NULL; adj_list = adj_list->suiv )
		{
			const int j = adj_list->s;
			adj_matrix[i][j] = adj_matrix[j][i] = 1;
		}
	}

	std::vector< int > vec1, vec2;
	std::tie( vec1, vec2 ) = is_permutation_certificate( adj_matrix );
	if ( vec1.empty() )
	{
		non_perm();
		return;
	}

// #define D_PRIME

	label = vec1;
	// 正規化（一方の permutation を(1, 2, 3, ..., n)に変換）し，perm に格納
	std::vector<int> v1(vec1.size());
	for(int i=0 ; i<vec1.size() ; i++){
		v1[vec1[i]-1] = i+1;
	}
	for(int i=0 ; i<vec2.size() ; i++){
		perm.push_back(v1[vec2[i]-1]);
	}
	// -----（ここまで）出力データから二つの permutation を読み込んで正規化 -----

	canonization(); // 標準形に変換

}

void Permutation::identify(const int n)
{
	for(int i=0 ; i<n ; i++){
		perm.push_back(i+1);
	}
	computeInverse();
}

void Permutation::reverse(const int n)
{
	for(int i=n ; i>0 ; i--){
		perm.push_back(i);
	}
	computeInverse();
}

void Permutation::non_perm(){
	std::vector<int>().swap(perm);
	perm.push_back(-1);
}

  /******************************************/

void Permutation::canonization()
{
	std::vector<int> vecH = computeHorizontalFlip();
	std::vector<int> vecV = computeVerticalFlip();
	std::vector<int> vecR = computeRotation();

	computeInverse();

	if(is_SmallerEqual(vecH) && is_SmallerEqual(vecV) && is_SmallerEqual(vecR)){
		return;
	}

	if(is_small(vecH, vecV)){ // checked vecH < vecV
		if(is_small(vecH, vecR)){ // vecH < vecR
			perm = vecH;
			computeHorizontalFlip_label();
		}
		else{ // vecR < vecH
			perm = vecR;
			computeRotation_label();
		}
	}
	else{ // vecV < vecH
		if(is_small(vecV, vecR)){ // vecV < vecR
			perm = vecV;
			computeVerticalFlip_label();
		}
		else{ // vecR < vecV
			perm = vecR;
			computeRotation_label();
		}
	}

	computeInverse();
}

void Permutation::computeInverse()
{
	inverse.resize( length() );
	for ( int i = 0; i < length(); ++i )
	{
		inverse[ perm[i] - 1 ] = i + 1;
	}
	return;
}

void Permutation::computeHorizontalFlip_label()
{
// 	cerr << "H-flip" << endl;
	std::vector<int> vec(length());
	for(int i=0 ; i<length() ; i++){
		vec[pi(i)-1] = label[i];
// 		cerr << i << " -> " << perm[i] << endl;
	}
	label = vec;
}

std::vector<int> Permutation::computeHorizontalFlip() const
{
	std::vector<int> vec(length());
	for(int i=0 ; i<length() ; i++){
		vec[pi(i)-1] = i+1;
	}
	//perm = vec;
	return vec;
}

void Permutation::computeVerticalFlip_label()
{
	std::vector<int> vec(length());
	for(int i=0 ; i<length() ; i++){
		vec[length()-i-1] = label[i];
	}
	label = vec;
}

std::vector<int> Permutation::computeVerticalFlip() const
{
	std::vector<int> vec(length());
	for(int i=0 ; i<length() ; i++){
		vec[length()-i-1] = length() - pi(i) + 1;
	}
	//perm = vec;
	return vec;
}

std::vector<int> Permutation::computeVerticalFlip(std::vector<int> p) const
{
	assert(p.size()==length());
	std::vector<int> vec(length());
	for(int i=0 ; i<length() ; i++){
		vec[length()-i-1] = length() - p[i] + 1;
	}
	//perm = vec;
	return vec;
}

void Permutation::computeRotation_label()
{
	vector< int > vec( length() );
	for ( int i = 0; i < length(); ++i )
	{
		vec[ length() - inverse[i] ] = label[i];
	}
	label = vec;

// 	computeHorizontalFlip_label();
// 	computeVerticalFlip_label();
}

std::vector<int> Permutation::computeRotation() const
{
	std::vector<int> vecH = computeHorizontalFlip();
	std::vector<int> vec = computeVerticalFlip(vecH);
	return vec;
}

bool Permutation::is_canonical()
{
	Permutation p(perm);
	p.canonization();
	if(is_Smaller(p.perm)) return true;
	else return false;
}

bool Permutation::is_v_symmetry() const
{
	for(int i=0 ; i<length()/2 ; i++){
		if (perm[i] != length() - perm[length()-i-1] + 1){
// 			cerr << "i : " << i << endl;
// 			cerr << "perm[i] : " << perm[i] << endl;
// 			cerr << "length() - perm[length()-i-1] + 1) : " << length() - perm[length()-i-1] + 1 << endl;
			return false;
		}
	}
	return true;
}

bool Permutation::is_h_symmetry() const
{
	for ( int i = 0; i < length(); ++i )
	{
		if ( i != perm[ perm[i] - 1 ] - 1 )
		{
			return false;
		}
	}
	return true;
}

bool Permutation::is_rotational_symmetry() const
{
	std::vector<int> vecR = computeRotation();
	return is_equal(vecR);
}

bool Permutation::is_permutation()
{
	//std::cout << perm[0] << std::endl;
	if(perm[0] == -1) return false;
	else return true;
}

int Permutation::length() const
{
	return perm.size();
}

int Permutation::pi(int k) const
{
	return perm[k];
}

bool Permutation::operator==(const Permutation& p) const
{
	if(length() != p.length()) return false;

	for(int i=0 ; i<length() ; i++){
		if(perm[i] != p.pi(i)) return false;
	}
	return true;
}

bool Permutation::operator==(const std::vector<int>& vec) const
{
	if(length() != vec.size()) return false;

	for(int i=0 ; i<length() ; i++){
		if(perm[i] != vec[i]) return false;
	}
	return true;
}

bool Permutation::is_equal(const std::vector<int>& vec) const
{
	if(length() != vec.size()) return false;

	for(int i=0 ; i<length() ; i++){
		if(perm[i] != vec[i]) return false;
	}
	return true;
}

bool Permutation::operator<(const Permutation& p) const {

	if(length() < p.length()) return true;
	if(length() > p.length()) return false;

	for(int i=0 ; i<length() ; i++){
		if(perm[i] < p.pi(i)) return true;
		if(perm[i] > p.pi(i)) return false;
	}
	return false;
}

bool Permutation::is_Smaller(const std::vector<int>& vec) const
{
	if(length() < vec.size()) return true;
	if(length() > vec.size()) return false;

	for(int i=0 ; i<length() ; i++){
		if(perm[i] < vec[i]) return true;
		if(perm[i] > vec[i]) return false;
	}
	return false;
}

bool Permutation::is_SmallerEqual(const std::vector<int>& vec) const
{
	if(length() < vec.size()) return true;
	if(length() > vec.size()) return false;

	for(int i=0 ; i<length() ; i++){
		if(perm[i] < vec[i]) return true;
		if(perm[i] > vec[i]) return false;
	}
	return true;
}

bool Permutation::is_Smaller(const Permutation& p) const
{
	if(length() < p.length()) return true;
	if(length() > p.length()) return false;

	for(int i=0 ; i<length() ; i++){
		if(perm[i] < p.pi(i)) return true;
		if(perm[i] > p.pi(i)) return false;
	}
	return false;
}

bool Permutation::is_small(const std::vector<int> v1, const std::vector<int> v2) const
{
	if(v1.size() < v2.size()) return true;
	if(v1.size() > v2.size()) return false;

	for(int i=0 ; i<v1.size() ; i++){
		if(v1[i] < v2[i]) return true;
		if(v1[i] > v2[i]) return false;
	}
	return false;
}

bool Permutation::is_smallequal(const std::vector<int> v1, const std::vector<int> v2) const
{
	if(v1.size() < v2.size()) return true;
	if(v1.size() > v2.size()) return false;

	for(int i=0 ; i<v1.size() ; i++){
		if(v1[i] < v2[i]) return true;
		if(v1[i] > v2[i]) return false;
	}
	return true;
}

// for Prime module
void Permutation::merge(const std::vector<Permutation> p)
{

	vector< vector< int > > perms;
	for ( int i = 0; i < p.size(); ++i )
	{
		perms.push_back( p[i].perm );
	}

	vector< int > psum( length() + 1 );
	for ( int i = 0; i < length(); ++i )
	{
		psum[ i + 1 ] = perms[i].size();
	}
	//partial_sum( begin( psum ), end( psum ), begin( psum ) );

	for ( int i = 0, s = 0; i < length(); s += perms[ inverse[ i++ ] - 1 ].size() )
	{
		// 		cerr << "perms[ " << inverse[i] - 1 << " ]" << endl;
		// 		cerr << s << endl;
		transform( begin( perms[ inverse[i] - 1 ] ), end( perms[ inverse[i] - 1 ] ), begin( perms[ inverse[i] - 1 ] ), bind( plus< int >(), placeholders::_1, s ) );
	}

	vector< int > vec;
	for ( const auto &pp : perms )
	{

		copy( begin( pp ), end( pp ), back_inserter( vec ) );
// 				copy( begin( pp ), end( pp ), ostream_iterator< int >( cerr, " " ) );
// 				cerr << endl;
	}

	perm = vec;
	label.resize( perm.size() );
}

Permutation Permutation::parentPermutation() const
{
	std::vector< int > vec( perm );
	for ( int i = 0; i + 1 < length(); ++i )
	{
		if ( vec[i] < vec[ i + 1 ] )
		{
			std::swap( vec[i], vec[ i + 1 ] );
			break;
		}
	}

	Permutation p(vec);
// 	cerr << "parent permutation from : ";
// 	p.print();
	graphe G = p.makeGraphe();
	noeud *root = decomposition_modulaire( G );
	Permutation pp = createPermutation( root, G );
	return pp;
}

graphe Permutation::makeGraphe() const
{
	graphe G;
	G.n = length();
	G.G=(adj **)malloc(G.n*sizeof(adj *));
	for(int i=0 ; i<G.n ; i++) G.G[i] = NULL;

	for(int i=0 ; i<length() ; i++){
		for(int j=i+1 ; j<length() ; j++){
			if(perm[i] > perm[j]){
				// add an edge perm[i] to perm[j] (and its reversal).
				adj *a = (adj *)malloc(sizeof(adj));
				a->s = perm[j]-1;
				a->suiv = G.G[perm[i]-1];
				G.G[perm[i]-1] = a;

				a = (adj *)malloc(sizeof(adj)); a->s = perm[i]-1;
				a->suiv = G.G[perm[j]-1];
				G.G[perm[j]-1] = a;

			}
		}
	}
	return G;
}

void Permutation::print() const
{
	if(length()<1){
		std::cout << "length 0" << std::endl;
		return;
	}

	std::vector<int>::const_iterator it;
	for(it = perm.begin() ; it != perm.end() ; ++it){
		std::cout << *it << ( it + 1 != perm.end() ? ' ' : '\n' );
	}
	std::cout << std::flush;
}

void Permutation::print(std::ofstream &ofs) const
{
	if(length()<1){
		ofs << "length 0" << std::endl;
		return;
	}

	std::vector<int>::const_iterator it;
	for(it = perm.begin() ; it != perm.end() ; ++it){
		ofs << *it << ( it + 1 != perm.end() ? ' ' : '\n' );
	}
	ofs << std::flush;
}

void Permutation::print(std::ofstream &ofs, std::vector<int> vec) const
{
	if(vec.size()<1){
		ofs << "length 0" << std::endl;
		return;
	}

	std::vector<int>::const_iterator it;
	for(it = vec.begin() ; it != vec.end() ; ++it){
		ofs << *it << ( it + 1 != vec.end() ? ' ' : '\n' );
	}
	ofs << std::flush;
}

void Permutation::print(std::vector<int> vec)
{
	for(int i=0 ; i<vec.size() ; i++){
		std::cout << vec[i] << ( i + 1 < vec.size() ? ' ' : '\n' );
	}
	std::cout << std::endl;
}


#include <PCP/Geometry/Normal.h>
#include <PCP/Geometry/Geometry.h>

#include <PCP/Common/Assert.h>
#include <PCP/Common/Progress.h>
#include <PCP/Common/Log.h>

#include <PCP/SpacePartitioning/KnnGraph.h>

#include <set>

#include <Eigen/Eigenvalues>

// https://www.geeksforgeeks.org/kruskals-minimum-spanning-tree-using-stl-in-c/
namespace mst {

using namespace std;

// Creating shortcut for an integer pair
typedef pair<int, int> iPair;

// Structure to represent a graph
struct Graph
{
    int V, E;
    vector< pair<float, iPair> > edges;

    // Constructor
    Graph(int V, int E)
    {
        this->V = V;
        this->E = E;
    }

    // Utility function to add an edge
    void addEdge(int u, int v, float w)
    {
        edges.push_back({w, {u, v}});
    }

    // Function to find MST using Kruskal's
    // MST algorithm
    float kruskalMST(std::vector<iPair>& result);
};

// To represent Disjoint Sets
struct DisjointSets
{
    int *parent, *rnk;
    int n;

    // Constructor.
    DisjointSets(int n)
    {
        // Allocate memory
        this->n = n;
        parent = new int[n+1];
        rnk = new int[n+1];

        // Initially, all vertices are in
        // different sets and have rank 0.
        for (int i = 0; i <= n; i++)
        {
            rnk[i] = 0;

            //every element is parent of itself
            parent[i] = i;
        }
    }

    // Find the parent of a node 'u'
    // Path Compression
    int find(int u)
    {
        /* Make the parent of the nodes in the path
           from u--> parent[u] point to parent[u] */
        if (u != parent[u])
            parent[u] = find(parent[u]);
        return parent[u];
    }

    // Union by rank
    void merge(int x, int y)
    {
        x = find(x), y = find(y);

        /* Make tree with smaller height
           a subtree of the other tree  */
        if (rnk[x] > rnk[y])
            parent[y] = x;
        else // If rnk[x] <= rnk[y]
            parent[x] = y;

        if (rnk[x] == rnk[y])
            rnk[y]++;
    }
};

 /* Functions returns weight of the MST*/

float Graph::kruskalMST(std::vector<iPair>& result)
{
    result.clear();
    float mst_wt = 0; // Initialize result

    // Sort edges in increasing order on basis of cost
    sort(edges.begin(), edges.end());

    // Create disjoint sets
    DisjointSets ds(V);

    // Iterate through all sorted edges
    vector< pair<float, iPair> >::iterator it;
    for (it=edges.begin(); it!=edges.end(); it++)
    {
        int u = it->second.first;
        int v = it->second.second;

        int set_u = ds.find(u);
        int set_v = ds.find(v);

        // Check if the selected edge is creating
        // a cycle or not (Cycle is created if u
        // and v belong to same set)
        if (set_u != set_v)
        {
            // Current edge will be in the MST
            // so print it
//            cout << u << " - " << v << endl;
            result.push_back(std::make_pair(u, v));

            // Update MST weight
            mst_wt += it->first;

            // Merge two sets
            ds.merge(set_u, set_v);
        }
    }

    return mst_wt;
}

// for my std::set
struct Comp
{
    // compare only the first index
    bool operator()(const pair<float, iPair>& e1, const pair<float, iPair>& e2)
    {
        return e1.second.first < e2.second.first;
    }
};

} // namespace mst

namespace pcp {

void compute_normals(Geometry& g, bool v)
{
    PCP_ASSERT(g.has_knn_graph());

    const int point_count = g.size();

    info().iff(v) << "0/5 Computing normals";
    internal::compute_unoriented_normals(g, v);

    info().iff(v) << "1/5 Extracting unique edges";
    std::set<std::pair<int,int>> edges;
    auto prog = Progress(point_count, v);
    for(int i=0; i<point_count; ++i)
    {
        for(int j : g.knn_graph().k_nearest_neighbors(i))
        {
            auto e = std::make_pair(i,j);
            if(e.first > e.second) std::swap(e.first, e.second);

            edges.insert(e);
        }
        ++prog;
    }
    info().iff(v) << "  " << edges.size() << " edges found";

    info().iff(v) << "2/5 Building graph for mst";
    mst::Graph graph(point_count, edges.size());
    for(const auto& e : edges)
    {
        const int i = e.first;
        const int j = e.second;
        const Vector3& n_i = g.normal(i);
        const Vector3& n_j = g.normal(j);
        const Scalar w = 1 - std::abs(n_i.dot(n_j));
        graph.addEdge(i, j, w);
    }

    info().iff(v) << "3/5 Computing mst";
    std::vector<mst::iPair> result;
    graph.kruskalMST(result);
    info().iff(v) << "  " << result.size() << " edges found in mst";

    info().iff(v) << "4. Building mst";
    std::vector<std::vector<int>> mst(point_count);
    for(const auto& e : result)
    {
        mst[e.first].push_back(e.second);
        mst[e.second].push_back(e.first);
    }

    info().iff(v) << "5/5 Propagate orientention";
    int topmost = 0;
    for(int i=0; i<point_count; ++i)
    {
        if(g[i].z() > g[topmost].z()) topmost = i;
    }
    std::stack<mst::iPair> stack;
    std::vector<bool> traversed(point_count, false);
    traversed[topmost] = true;
    for(int i : mst[topmost]) stack.push(std::make_pair(topmost, i));

    while(!stack.empty())
    {
        const auto e = stack.top();
        stack.pop();

        const int source = e.first;
        const int target = e.second;

        const Vector3& n_source = g.normal(source);
        Vector3& n_target = g.normal(target);

        if(n_source.dot(n_target) < 0) n_target *= -1;

        traversed[target] = true;

        for(int i : mst[target]) if(!traversed[i]) stack.push(std::make_pair(target, i));
    }
}

void compute_normals_robust(Geometry& g, float sigma, int iter, bool v)
{
    PCP_ASSERT(g.has_knn_graph());

    const int point_count = g.size();

    info().iff(v) << "0/5 Computing normals";
    internal::compute_unoriented_normals_robust(g, sigma, iter, v);

    info().iff(v) << "1/5 Extracting unique edges";
    std::set<std::pair<int,int>> edges;
    auto prog = Progress(point_count, v);
    for(int i=0; i<point_count; ++i)
    {
        for(int j : g.knn_graph().k_nearest_neighbors(i))
        {
            auto e = std::make_pair(i,j);
            if(e.first > e.second) std::swap(e.first, e.second);

            edges.insert(e);
        }
        ++prog;
    }
    info().iff(v) << "  " << edges.size() << " edges found";

    info().iff(v) << "2/5 Building graph for mst";
    mst::Graph graph(point_count, edges.size());
    for(const auto& e : edges)
    {
        const int i = e.first;
        const int j = e.second;
        const Vector3& n_i = g.normal(i);
        const Vector3& n_j = g.normal(j);
        const Scalar w = 1 - std::abs(n_i.dot(n_j));
        graph.addEdge(i, j, w);
    }

    info().iff(v) << "3/5 Computing mst";
    std::vector<mst::iPair> result;
    graph.kruskalMST(result);
    info().iff(v) << "  " << result.size() << " edges found in mst";

    info().iff(v) << "4. Building mst";
    std::vector<std::vector<int>> mst(point_count);
    for(const auto& e : result)
    {
        mst[e.first].push_back(e.second);
        mst[e.second].push_back(e.first);
    }

    info().iff(v) << "5/5 Propagate orientention";
    int topmost = 0;
    for(int i=0; i<point_count; ++i)
    {
        if(g[i].z() > g[topmost].z()) topmost = i;
    }
    std::stack<mst::iPair> stack;
    std::vector<bool> traversed(point_count, false);
    traversed[topmost] = true;
    for(int i : mst[topmost]) stack.push(std::make_pair(topmost, i));

    while(!stack.empty())
    {
        const auto e = stack.top();
        stack.pop();

        const int source = e.first;
        const int target = e.second;

        const Vector3& n_source = g.normal(source);
        Vector3& n_target = g.normal(target);

        if(n_source.dot(n_target) < 0) n_target *= -1;

        traversed[target] = true;

        for(int i : mst[target]) if(!traversed[i]) stack.push(std::make_pair(target, i));
    }
}

namespace internal {

void compute_unoriented_normals(Geometry& g, bool v)
{
    const int point_count = g.size();
    const int K = g.knn_graph().k();

    g.request_normals();

    auto prog = Progress(point_count, v);

    #pragma omp parallel for
    for(int i=0; i<point_count; ++i)
    {
        Matrix3 C = Matrix3::Zero();
        Vector3 m = Vector3::Zero();

        for(int j : g.knn_graph().k_nearest_neighbors(i))
        {
            const Vector3 p = g[j] - g[i];

            C += p * p.transpose();
            m += p;
        }
        m /= K;
        C = C/K - m * m.transpose();

        Eigen::SelfAdjointEigenSolver<Matrix3> eig(C);
        g.normal(i) = eig.eigenvectors().col(0);

        ++prog;
    }
}

namespace internal {
Scalar robust_weight(const Vector3& p, Scalar uc, const Vector3& ul, Scalar sigma)
{
    const Scalar f = uc + ul.dot(p);
    return std::exp(-f*f/(sigma*sigma));
}
} // namespace internal

void compute_unoriented_normals_robust(Geometry& g, float sigma, int iter, bool v)
{
    const int point_count = g.size();
//    const int K = g.knn_graph().k();

    g.request_normals();

    auto prog = Progress(point_count, v);

    #pragma omp parallel for
    for(int i=0; i<point_count; ++i)
    {
        Scalar  uc = 0;
        Vector3 ul = Vector3::Zero();

        for(int n=0; n<iter; ++n)
        {
            Matrix3 C = Matrix3::Zero();
            Vector3 m = Vector3::Zero();
            Scalar sum_w = 0;

            for(int j : g.knn_graph().k_nearest_neighbors(i))
            {
                const Vector3 p = g[j] - g[i];
                const Scalar w = n == 0 ? 1 : internal::robust_weight(p, uc, ul, sigma);

                C += w * p * p.transpose();
                m += w * p;
                sum_w += w;
            }

            if(sum_w <= 0)
            {
                warning().iff(v) << "sum_w=0 at i=" << i << " (n=" << n << ")";
//                PCP_ERROR;
                break;
            }

            m /= sum_w;
            C = C/sum_w - m * m.transpose();

            Eigen::SelfAdjointEigenSolver<Matrix3> eig(C);
            ul = eig.eigenvectors().col(0);
            uc = - ul.dot(m);
        }

        g.normal(i) = ul;

        ++prog;
    }
}

} // namespace internal

} // namespace pcp


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2019 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include <Eigen/Dense>
#include <queue>
#include <tuple>

#include "open3d/geometry/TriangleMesh.h"
#include "open3d/utility/Console.h"

namespace open3d {
namespace geometry {

/// Error quadric that is used to minimize the squared distance of a point to
/// its neigbhouring triangle planes.
/// Cf. "Simplifying Surfaces with Color and Texture using Quadric Error
/// Metrics" by Garland and Heckbert.
class Quadric {
public:
    Quadric() {
        A_.fill(0);
        b_.fill(0);
        c_ = 0;
    }

    Quadric(const Eigen::Vector4d& plane, double weight = 1) {
        Eigen::Vector3d n = plane.head<3>();
        A_ = weight * n * n.transpose();
        b_ = weight * plane(3) * n;
        c_ = weight * plane(3) * plane(3);
    }

    Quadric& operator+=(const Quadric& other) {
        A_ += other.A_;
        b_ += other.b_;
        c_ += other.c_;
        return *this;
    }

    Quadric operator+(const Quadric& other) const {
        Quadric res;
        res.A_ = A_ + other.A_;
        res.b_ = b_ + other.b_;
        res.c_ = c_ + other.c_;
        return res;
    }

    double Eval(const Eigen::Vector3d& v) const {
        Eigen::Vector3d Av = A_ * v;
        double q = v.dot(Av) + 2 * b_.dot(v) + c_;
        return q;
    }

    bool IsInvertible() const { return std::fabs(A_.determinant()) > 1e-4; }

    Eigen::Vector3d Minimum() const { return -A_.ldlt().solve(b_); }

public:
    /// A_ = n . n^T, where n is the plane normal
    Eigen::Matrix3d A_;
    /// b_ = d . n, where n is the plane normal and d the non-normal component
    /// of the plane parameters
    Eigen::Vector3d b_;
    /// c_ = d . d, where d the non-normal component pf the plane parameters
    double c_;
};

std::shared_ptr<TriangleMesh> TriangleMesh::SimplifyVertexClustering(
        double voxel_size,
        SimplificationContraction
                contraction /* = SimplificationContraction::Average */) const {
    if (HasTriangleUvs()) {
        utility::LogWarning(
                "[SimplifyVertexClustering] This mesh contains triangle uvs "
                "that are not handled in this function");
    }
    auto mesh = std::make_shared<TriangleMesh>();
    if (voxel_size <= 0.0) {
        utility::LogError("[VoxelGridFromPointCloud] voxel_size <= 0.0");
    }

    Eigen::Vector3d voxel_size3 =
            Eigen::Vector3d(voxel_size, voxel_size, voxel_size);
    Eigen::Vector3d voxel_min_bound = GetMinBound() - voxel_size3 * 0.5;
    Eigen::Vector3d voxel_max_bound = GetMaxBound() + voxel_size3 * 0.5;
    if (voxel_size * std::numeric_limits<int>::max() <
        (voxel_max_bound - voxel_min_bound).maxCoeff()) {
        utility::LogError("[VoxelGridFromPointCloud] voxel_size is too small.");
    }

    auto GetVoxelIdx = [&](const Eigen::Vector3d& vert) {
        Eigen::Vector3d ref_coord = (vert - voxel_min_bound) / voxel_size;
        Eigen::Vector3i idx(int(floor(ref_coord(0))), int(floor(ref_coord(1))),
                            int(floor(ref_coord(2))));
        return idx;
    };

    std::unordered_map<Eigen::Vector3i, std::unordered_set<int>,
                       utility::hash_eigen<Eigen::Vector3i>>
            voxel_vertices;
    std::unordered_map<Eigen::Vector3i, int,
                       utility::hash_eigen<Eigen::Vector3i>>
            voxel_vert_ind;
    int new_vidx = 0;
    for (size_t vidx = 0; vidx < vertices_.size(); ++vidx) {
        const Eigen::Vector3i vox_idx = GetVoxelIdx(vertices_[vidx]);
        voxel_vertices[vox_idx].insert(int(vidx));

        if (voxel_vert_ind.count(vox_idx) == 0) {
            voxel_vert_ind[vox_idx] = new_vidx;
            new_vidx++;
        }
    }

    // aggregate vertex info
    bool has_vert_normal = HasVertexNormals();
    bool has_vert_color = HasVertexColors();
    mesh->vertices_.resize(voxel_vertices.size());
    if (has_vert_normal) {
        mesh->vertex_normals_.resize(voxel_vertices.size());
    }
    if (has_vert_color) {
        mesh->vertex_colors_.resize(voxel_vertices.size());
    }

    auto AvgVertex = [&](const std::unordered_set<int> ind) {
        Eigen::Vector3d aggr(0, 0, 0);
        for (int vidx : ind) {
            aggr += vertices_[vidx];
        }
        aggr /= double(ind.size());
        return aggr;
    };
    auto AvgNormal = [&](const std::unordered_set<int> ind) {
        Eigen::Vector3d aggr(0, 0, 0);
        for (int vidx : ind) {
            aggr += vertex_normals_[vidx];
        }
        aggr /= double(ind.size());
        return aggr;
    };
    auto AvgColor = [&](const std::unordered_set<int> ind) {
        Eigen::Vector3d aggr(0, 0, 0);
        for (int vidx : ind) {
            aggr += vertex_colors_[vidx];
        }
        aggr /= double(ind.size());
        return aggr;
    };

    if (contraction == SimplificationContraction::Average) {
        for (const auto& voxel : voxel_vertices) {
            int vox_vidx = voxel_vert_ind[voxel.first];
            mesh->vertices_[vox_vidx] = AvgVertex(voxel.second);
            if (has_vert_normal) {
                mesh->vertex_normals_[vox_vidx] = AvgNormal(voxel.second);
            }
            if (has_vert_color) {
                mesh->vertex_colors_[vox_vidx] = AvgColor(voxel.second);
            }
        }
    } else if (contraction == SimplificationContraction::Quadric) {
        // Map triangles
        std::unordered_map<int, std::unordered_set<int>> vert_to_triangles;
        for (size_t tidx = 0; tidx < triangles_.size(); ++tidx) {
            vert_to_triangles[triangles_[tidx](0)].emplace(int(tidx));
            vert_to_triangles[triangles_[tidx](1)].emplace(int(tidx));
            vert_to_triangles[triangles_[tidx](2)].emplace(int(tidx));
        }

        for (const auto& voxel : voxel_vertices) {
            int vox_vidx = voxel_vert_ind[voxel.first];

            Quadric q;
            for (int vidx : voxel.second) {
                for (int tidx : vert_to_triangles[vidx]) {
                    Eigen::Vector4d p = GetTrianglePlane(tidx);
                    double area = GetTriangleArea(tidx);
                    q += Quadric(p, area);
                }
            }
            if (q.IsInvertible()) {
                Eigen::Vector3d v = q.Minimum();
                mesh->vertices_[vox_vidx] = v;
            } else {
                mesh->vertices_[vox_vidx] = AvgVertex(voxel.second);
            }

            if (has_vert_normal) {
                mesh->vertex_normals_[vox_vidx] = AvgNormal(voxel.second);
            }
            if (has_vert_color) {
                mesh->vertex_colors_[vox_vidx] = AvgColor(voxel.second);
            }
        }
    }

    //  connect vertices
    std::unordered_set<Eigen::Vector3i, utility::hash_eigen<Eigen::Vector3i>>
            triangles;
    for (const auto& triangle : triangles_) {
        int vidx0 = voxel_vert_ind[GetVoxelIdx(vertices_[triangle(0)])];
        int vidx1 = voxel_vert_ind[GetVoxelIdx(vertices_[triangle(1)])];
        int vidx2 = voxel_vert_ind[GetVoxelIdx(vertices_[triangle(2)])];

        // only connect if in different voxels
        if (vidx0 == vidx1 || vidx0 == vidx2 || vidx1 == vidx2) {
            continue;
        }

        // Note: there can be still double faces with different orientation
        // The user has to clean up manually
        if (vidx1 < vidx0 && vidx1 < vidx2) {
            int tmp = vidx0;
            vidx0 = vidx1;
            vidx1 = vidx2;
            vidx2 = tmp;
        } else if (vidx2 < vidx0 && vidx2 < vidx1) {
            int tmp = vidx1;
            vidx1 = vidx0;
            vidx0 = vidx2;
            vidx2 = tmp;
        }

        triangles.emplace(Eigen::Vector3i(vidx0, vidx1, vidx2));
    }

    mesh->triangles_.resize(triangles.size());
    int tidx = 0;
    for (const Eigen::Vector3i& triangle : triangles) {
        mesh->triangles_[tidx] = triangle;
        tidx++;
    }

    if (HasTriangleNormals()) {
        mesh->ComputeTriangleNormals();
    }

    return mesh;
}

std::shared_ptr<TriangleMesh> TriangleMesh::SimplifyQuadricDecimation(
        int target_number_of_triangles) const {
    if (HasTriangleUvs()) {
        utility::LogWarning(
                "[SimplifyQuadricDecimation] This mesh contains triangle uvs "
                "that are not handled in this function");
    }
    typedef std::tuple<double, int, int> CostEdge;

    auto mesh = std::make_shared<TriangleMesh>();
    mesh->vertices_ = vertices_;
    mesh->vertex_normals_ = vertex_normals_;
    mesh->vertex_colors_ = vertex_colors_;
    mesh->triangles_ = triangles_;

    std::vector<bool> vertices_deleted(vertices_.size(), false);
    std::vector<bool> triangles_deleted(triangles_.size(), false);

    // Map vertices to triangles and compute triangle planes and areas
    std::vector<std::unordered_set<int>> vert_to_triangles(vertices_.size());
    std::vector<Eigen::Vector4d> triangle_planes(triangles_.size());
    std::vector<double> triangle_areas(triangles_.size());
    for (size_t tidx = 0; tidx < triangles_.size(); ++tidx) {
        vert_to_triangles[triangles_[tidx](0)].emplace(static_cast<int>(tidx));
        vert_to_triangles[triangles_[tidx](1)].emplace(static_cast<int>(tidx));
        vert_to_triangles[triangles_[tidx](2)].emplace(static_cast<int>(tidx));

        triangle_planes[tidx] = GetTrianglePlane(tidx);
        triangle_areas[tidx] = GetTriangleArea(tidx);
    }

    // Compute the error metric per vertex
    std::vector<Quadric> Qs(vertices_.size());
    for (size_t vidx = 0; vidx < vertices_.size(); ++vidx) {
        for (int tidx : vert_to_triangles[vidx]) {
            Qs[vidx] += Quadric(triangle_planes[tidx], triangle_areas[tidx]);
        }
    }

    // For boundary edges add perpendicular plane quadric
    auto edge_triangle_count = GetEdgeToTrianglesMap();
    auto AddPerpPlaneQuadric = [&](int vidx0, int vidx1, int vidx2,
                                   double area) {
        int min = std::min(vidx0, vidx1);
        int max = std::max(vidx0, vidx1);
        Eigen::Vector2i edge(min, max);
        if (edge_triangle_count[edge].size() != 1) {
            return;
        }
        const auto& vert0 = mesh->vertices_[vidx0];
        const auto& vert1 = mesh->vertices_[vidx1];
        const auto& vert2 = mesh->vertices_[vidx2];
        Eigen::Vector3d vert2p = (vert2 - vert0).cross(vert2 - vert1);
        Eigen::Vector4d plane = ComputeTrianglePlane(vert0, vert1, vert2p);
        Quadric quad(plane, area);
        Qs[vidx0] += quad;
        Qs[vidx1] += quad;
    };
    for (size_t tidx = 0; tidx < triangles_.size(); ++tidx) {
        const auto& tria = triangles_[tidx];
        double area = triangle_areas[tidx];
        AddPerpPlaneQuadric(tria(0), tria(1), tria(2), area);
        AddPerpPlaneQuadric(tria(1), tria(2), tria(0), area);
        AddPerpPlaneQuadric(tria(2), tria(0), tria(1), area);
    }

    // Get valid edges and compute cost
    // Note: We could also select all vertex pairs as edges with dist < eps
    std::unordered_map<Eigen::Vector2i, Eigen::Vector3d,
                       utility::hash_eigen<Eigen::Vector2i>>
            vbars;
    std::unordered_map<Eigen::Vector2i, double,
                       utility::hash_eigen<Eigen::Vector2i>>
            costs;
    auto CostEdgeComp = [](const CostEdge& a, const CostEdge& b) {
        return std::get<0>(a) > std::get<0>(b);
    };
    std::priority_queue<CostEdge, std::vector<CostEdge>, decltype(CostEdgeComp)>
            queue(CostEdgeComp);

    auto AddEdge = [&](int vidx0, int vidx1, bool update) {
        int min = std::min(vidx0, vidx1);
        int max = std::max(vidx0, vidx1);
        Eigen::Vector2i edge(min, max);
        if (update || vbars.count(edge) == 0) {
            const Quadric& Q0 = Qs[min];
            const Quadric& Q1 = Qs[max];
            Quadric Qbar = Q0 + Q1;
            double cost;
            Eigen::Vector3d vbar;
            if (Qbar.IsInvertible()) {
                vbar = Qbar.Minimum();
                cost = Qbar.Eval(vbar);
            } else {
                const Eigen::Vector3d& v0 = mesh->vertices_[vidx0];
                const Eigen::Vector3d& v1 = mesh->vertices_[vidx0];
                Eigen::Vector3d vmid = (v0 + v1) / 2;
                double cost0 = Qbar.Eval(v0);
                double cost1 = Qbar.Eval(v1);
                double costmid = Qbar.Eval(vbar);
                cost = std::min(cost0, std::min(cost1, costmid));
                if (cost == costmid) {
                    vbar = vmid;
                } else if (cost == cost0) {
                    vbar = v0;
                } else {
                    vbar = v1;
                }
            }
            vbars[edge] = vbar;
            costs[edge] = cost;
            queue.push(CostEdge(cost, min, max));
        }
    };

    // add all edges to priority queue
    for (const auto& triangle : triangles_) {
        AddEdge(triangle(0), triangle(1), false);
        AddEdge(triangle(1), triangle(2), false);
        AddEdge(triangle(2), triangle(0), false);
    }

    // perform incremental edge collapse
    bool has_vert_normal = HasVertexNormals();
    bool has_vert_color = HasVertexColors();
    int n_triangles = int(triangles_.size());
    while (n_triangles > target_number_of_triangles && !queue.empty()) {
        // retrieve edge from queue
        double cost;
        int vidx0, vidx1;
        std::tie(cost, vidx0, vidx1) = queue.top();
        queue.pop();

        // test if the edge has been updated (reinserted into queue)
        Eigen::Vector2i edge(vidx0, vidx1);
        bool valid = !vertices_deleted[vidx0] && !vertices_deleted[vidx1] &&
                     cost == costs[edge];
        if (!valid) {
            continue;
        }

        // avoid flip of triangle normal
        bool flipped = false;
        for (int tidx : vert_to_triangles[vidx1]) {
            if (triangles_deleted[tidx]) {
                continue;
            }

            const Eigen::Vector3i& tria = mesh->triangles_[tidx];
            bool has_vidx0 =
                    vidx0 == tria(0) || vidx0 == tria(1) || vidx0 == tria(2);
            bool has_vidx1 =
                    vidx1 == tria(0) || vidx1 == tria(1) || vidx1 == tria(2);
            if (has_vidx0 && has_vidx1) {
                continue;
            }

            Eigen::Vector3d vert0 = mesh->vertices_[tria(0)];
            Eigen::Vector3d vert1 = mesh->vertices_[tria(1)];
            Eigen::Vector3d vert2 = mesh->vertices_[tria(2)];
            Eigen::Vector3d norm_before = (vert1 - vert0).cross(vert2 - vert0);
            norm_before /= norm_before.norm();

            if (vidx1 == tria(0)) {
                vert0 = vbars[edge];
            } else if (vidx1 == tria(1)) {
                vert1 = vbars[edge];
            } else if (vidx1 == tria(2)) {
                vert2 = vbars[edge];
            }

            Eigen::Vector3d norm_after = (vert1 - vert0).cross(vert2 - vert0);
            norm_after /= norm_after.norm();
            if (norm_before.dot(norm_after) < 0) {
                flipped = true;
                break;
            }
        }
        if (flipped) {
            continue;
        }

        // Connect triangles from vidx1 to vidx0, or mark deleted
        for (int tidx : vert_to_triangles[vidx1]) {
            if (triangles_deleted[tidx]) {
                continue;
            }

            Eigen::Vector3i& tria = mesh->triangles_[tidx];
            bool has_vidx0 =
                    vidx0 == tria(0) || vidx0 == tria(1) || vidx0 == tria(2);
            bool has_vidx1 =
                    vidx1 == tria(0) || vidx1 == tria(1) || vidx1 == tria(2);

            if (has_vidx0 && has_vidx1) {
                triangles_deleted[tidx] = true;
                n_triangles--;
                continue;
            }

            if (vidx1 == tria(0)) {
                tria(0) = vidx0;
            } else if (vidx1 == tria(1)) {
                tria(1) = vidx0;
            } else if (vidx1 == tria(2)) {
                tria(2) = vidx0;
            }
            vert_to_triangles[vidx0].insert(tidx);
        }

        // update vertex vidx0 to vbar
        mesh->vertices_[vidx0] = vbars[edge];
        Qs[vidx0] += Qs[vidx1];
        if (has_vert_normal) {
            mesh->vertex_normals_[vidx0] = 0.5 * (mesh->vertex_normals_[vidx0] +
                                                  mesh->vertex_normals_[vidx1]);
        }
        if (has_vert_color) {
            mesh->vertex_colors_[vidx0] = 0.5 * (mesh->vertex_colors_[vidx0] +
                                                 mesh->vertex_colors_[vidx1]);
        }
        vertices_deleted[vidx1] = true;

        // Update edge costs for all triangles connecting to vidx0
        for (const auto& tidx : vert_to_triangles[vidx0]) {
            if (triangles_deleted[tidx]) {
                continue;
            }
            const Eigen::Vector3i& tria = mesh->triangles_[tidx];
            if (tria(0) == vidx0 || tria(1) == vidx0) {
                AddEdge(tria(0), tria(1), true);
            }
            if (tria(1) == vidx0 || tria(2) == vidx0) {
                AddEdge(tria(1), tria(2), true);
            }
            if (tria(2) == vidx0 || tria(0) == vidx0) {
                AddEdge(tria(2), tria(0), true);
            }
        }
    }

    // Apply changes to the triangle mesh
    int next_free = 0;
    std::unordered_map<int, int> vert_remapping;
    for (size_t idx = 0; idx < mesh->vertices_.size(); ++idx) {
        if (!vertices_deleted[idx]) {
            vert_remapping[int(idx)] = next_free;
            mesh->vertices_[next_free] = mesh->vertices_[idx];
            if (has_vert_normal) {
                mesh->vertex_normals_[next_free] = mesh->vertex_normals_[idx];
            }
            if (has_vert_color) {
                mesh->vertex_colors_[next_free] = mesh->vertex_colors_[idx];
            }
            next_free++;
        }
    }
    mesh->vertices_.resize(next_free);
    if (has_vert_normal) {
        mesh->vertex_normals_.resize(next_free);
    }
    if (has_vert_color) {
        mesh->vertex_colors_.resize(next_free);
    }

    next_free = 0;
    for (size_t idx = 0; idx < mesh->triangles_.size(); ++idx) {
        if (!triangles_deleted[idx]) {
            Eigen::Vector3i tria = mesh->triangles_[idx];
            mesh->triangles_[next_free](0) = vert_remapping[tria(0)];
            mesh->triangles_[next_free](1) = vert_remapping[tria(1)];
            mesh->triangles_[next_free](2) = vert_remapping[tria(2)];
            next_free++;
        }
    }
    mesh->triangles_.resize(next_free);

    if (HasTriangleNormals()) {
        mesh->ComputeTriangleNormals();
    }

    return mesh;
}

}  // namespace geometry
}  // namespace open3d