/*
 * Sigmoid.h
 *
 *  Created on: Dec 8, 2016
 *      Author: ken
 */

#ifndef LAYER_SIGMOID_HPP_
#define LAYER_SIGMOID_HPP_

#include <armadillo>
#include "Layer.hpp"

class Sigmoid: public Layer {
private:
	arma::field<arma::Cube<double>> mYs;
	friend class SigmoidTest;
public:
	Sigmoid();
	virtual ~Sigmoid();

	arma::field<arma::Cube<double>> feedForward(const arma::field<arma::Cube<double>>& xs);
	arma::field<arma::Cube<double>> backProp(const arma::field<arma::Cube<double>>& deltas);
};

#endif /* LAYER_SIGMOID_HPP_ */


/*
Author: Rohan Chetan Thanki
Date created: 16-Oct-2021
*/

// This file contains functions which are used in the main file. This ensures that the main file is not cluttered

#include "Hedging_Portfolio.hpp"
#include <iostream>
#include <fstream>
#include <vector>

#include <boost/random.hpp>
#include <boost/math/distributions.hpp>
#include <boost/date_time.hpp>

using namespace std;

/**************************************** GENERIC UTIL FUNCTIONS ****************************************************/

// split a string into substrings based on a delimiter
inline vector<string> string_splitter(const string& str, char delimiter)
{
    vector<string> str_vec;
    stringstream ss(str);
    string token;
    while (getline(ss, token, delimiter))
    {
        str_vec.push_back(token);
    }
    return str_vec;
}

// Writing a 2D Vector to a CSV file
inline void write2DVectorToCSV(const vector<vector<double>>& vect, const string& filepath)
{
    std::ofstream out(filepath);
    for (int i = 0; i <= vect.size() - 1; i++)
    {
        for (int j = 0; j <= vect[0].size() - 1; j++)
        {
            out << vect[i][j] << ",";
        }
        out << "\n";
    }
}

// Get CDF of Standard Normal
inline double N(const double& x)
{
    boost::math::normal_distribution<> stdNormal(0.0, 1.0);
    return(cdf(stdNormal, x));
}

// print a 2D vector to console
template <typename T>
inline void printVect(vector<vector<T>> vect)
{
    for (int i = 0; i <= vect[0].size() - 1; i++)
    {
        for (int j = 0; j <= vect.size() - 1; j++)
        {
            cout << vect[j][i] << "\t";
        }
        cout << "\n";
    }
}

// convert vector of string to vector of double
inline vector<double> stringToDoubleVect(const vector<string>& inVect)
{
    vector<double> outVect;
    for (int i = 0; i <= inVect.size() - 1; i++)
    {
        outVect.push_back(stod(inVect[i]));
    }
    return(outVect);
}

// convert vector of string to vector of dates
inline vector<boost::gregorian::date> stringtoDateVect(const vector<string>& inVect)
{
    vector<boost::gregorian::date> outVect;
    for (int i = 0; i <= inVect.size() - 1; i++)
    {
        outVect.push_back(boost::gregorian::from_simple_string(inVect[i]));
    }
    return(outVect);
}

// function to read a CSV where each column is returned as a vector
inline vector<vector<string>> readCSV(const string& filepath, const unsigned int& ignoreLines, const int& numCols)
{
    ifstream infile(filepath);
    string line;

    // ignore the first few lines of the file
    for (int lineCount = 1; lineCount <= ignoreLines && infile.good(); lineCount++)
        getline(infile, line);

    // initialising vectors which will be populated when the file is read
    vector<vector<string>> csvData;
    for (int i = 0; i <= numCols - 1; i++)
    {
        vector<string> temp;
        csvData.push_back(temp);
    }

    // Read the data line by line
    while (getline(infile, line))
    {
        vector<string> rowData = string_splitter(line, ',');
        for (int i = 0; i <= numCols - 1; i++)
        {
            csvData[i].push_back(rowData[i]);
        }
    }

    infile.close();
    return(csvData);
}

/************************************ FUNCTIONS SPECIFIC TO PART 1 ****************************************************/

// Simulate stock prices
inline vector<vector<double>> simulateStockPrices(unsigned int numPaths, unsigned int N, double S0, double T, double u, double sigma)
{
    double dt = T / N;          // time step
    vector < vector<double> > stockPrices;          // creating a vector to store stock prices for all paths

    boost::random::mt19937 rng;     // creating a random number generator
    //rng.seed(static_cast<unsigned int> (std::time(0)));     // setting the seed of the random number generator
    rng.seed(123);
    boost::random::normal_distribution<double> stdNormal(0, 1);     // creating the distribution object

    for (int i = 0; i <= numPaths - 1; i++)
    {
        vector<double> pathStockPrices;             // creating a vector to store stock prices of the current path
        double S = S0;
        pathStockPrices.push_back(S);

        // add simulated stock prices of the current path to the  current path vector
        for (int j = 1; j <= N; j++)
        {
            S = (S)+(S * u * dt) + (S * sigma * sqrt(dt) * stdNormal(rng));
            pathStockPrices.push_back(S);
        }

        // add the stock prices of the current path to the overall stock price vector
        stockPrices.push_back(pathStockPrices);
    }

    return(stockPrices);
}

// Computing Hedging Error of Hedged Portfolio
inline vector<vector<Hedging_Portfolio>> computeHedgingErrors(const vector<vector<double>>& stockPrices, const double& K, const double& r, const double& T, const double& sigma, const char& optionFlag)
{
    unsigned int numPaths = stockPrices.size(); // number of paths
    unsigned int pathLength = stockPrices[0].size() - 1;
    double dt = T / pathLength;     // time step

    vector<vector<Hedging_Portfolio>> allHedgingPortfolio;     // creating a vector to store the details of the hedging error

    for (int i = 0; i <= numPaths - 1; i++)
    {
        vector<Hedging_Portfolio> pathHedgingPortfolio;
        for (int j = 0; j <= pathLength; j++)
        {
            // creating variables
            double S = stockPrices[i][j];
            double TMat = T - (j * dt);
            double deltaPrev, BPrev, rPrev;
            double delta, V, B, HE;

            // computing d1 and d2 for Black Scholes model
            double d1 = (log(S / K) + ((r + (pow(sigma, 2) / 2)) * TMat)) / (sigma * sqrt(TMat));
            double d2 = d1 - (sigma * sqrt(TMat));

            // creating an object of the hedged portfolio at the current state
            Hedging_Portfolio portfolio(K, S, r, TMat, sigma, optionFlag);

            // setting delta
            delta = N(d1);
            portfolio.setDelta(delta);

            // setting option Price
            if (optionFlag == 'c' || optionFlag == 'C')
                V = (S * N(d1)) - (K * exp(-r * TMat) * N(d2));
            else if (optionFlag == 'p' || optionFlag == 'P')
                V = (K * exp(-r * TMat) * N(-d2)) - (S * N(-d1));
            else
            {
                cout << "Flag " << optionFlag << "is incorrect. Flag must be either 'c', 'C', 'p' or 'P'" << endl;
                throw(10);
            }
            portfolio.setOptionPrice(V);

            // setting B
            if (j == 0)
            {
                B = V - delta * S;
            }
            else
            {
                rPrev = pathHedgingPortfolio[j - 1].getRiskFreeRate();
                deltaPrev = pathHedgingPortfolio[j - 1].getDelta();
                BPrev = pathHedgingPortfolio[j - 1].getB();
                B = ((deltaPrev - delta) * S) + BPrev * exp(rPrev * dt);
            }
            portfolio.setB(B);

            // setting Hedging Error
            if (j == 0)
            {
                HE = 0;
            }
            else
            {
                HE = deltaPrev * S + BPrev * exp(rPrev * dt) - V;
            }
            portfolio.setHedgingError(HE);

            // adding current state of the portfolio to the vector of states of the portfolio for current path
            pathHedgingPortfolio.push_back(portfolio);
        }

        // adding the path of portfolio to the vector containinf portfolio states of all paths
        allHedgingPortfolio.push_back(pathHedgingPortfolio);
    }
    return(allHedgingPortfolio);
}

// function to create a 2D Vector of a specific attribute from the hedged portfolio state vector
inline vector<vector<double>> getHedgedPortfolioParam(const vector<vector<Hedging_Portfolio>>& allPortfolioStates, const string param)
{
    vector<vector<double>> allVect;
    int numPaths = allPortfolioStates.size();
    int pathSize = allPortfolioStates[0].size() - 1;

    for (int i = 0; i <= numPaths - 1; i++)
    {
        vector<double> pathVect;
        for (int j = 0; j <= pathSize; j++)
        {
            double val;

            if (param == "S")
                val = allPortfolioStates[i][j].getSpotPrice();
            else if (param == "DELTA")
                val = allPortfolioStates[i][j].getDelta();
            else if (param == "V")
                val = allPortfolioStates[i][j].getOptionPrice();
            else if (param == "B")
                val = allPortfolioStates[i][j].getB();
            else if (param == "HE")
                val = allPortfolioStates[i][j].getHedgingError();
            else
                val = 0;

            pathVect.push_back(val);
        }
        allVect.push_back(pathVect);
    }

    return(allVect);
}

/************************************** FUNCTIONS SPECIFIC TO PART 2 ****************************************************/

// Count the number of weekdays between 2 days
inline long countWeekDays(string d0str, string d1str)
{
    boost::gregorian::date d0(boost::gregorian::from_simple_string(d0str));
    boost::gregorian::date d1(boost::gregorian::from_simple_string(d1str));
    long ndays = (d1 - d0).days() + 1; // +1 for inclusive
    long nwkends = 2 * ((ndays + d0.day_of_week()) / 7); // 2*Saturdays
    if (d0.day_of_week() == boost::date_time::Sunday) ++nwkends;
    if (d1.day_of_week() == boost::date_time::Saturday) --nwkends;
    return ndays - nwkends;
}

// finding corresponding value from a vector given the date
inline double findVal(vector<string> dateVec, vector<double> valVec, string date)
{
    auto it = find(dateVec.begin(), dateVec.end(), date);
    return valVec.at(it - dateVec.begin());  // this will throw an exception if array is out of bounds
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file  testSmartProjectionPoseFactor.cpp
 *  @brief Unit tests for ProjectionFactor Class
 *  @author Chris Beall
 *  @author Luca Carlone
 *  @author Zsolt Kira
 *  @author Frank Dellaert
 *  @date   Sept 2013
 */

#include "smartFactorScenarios.h"
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <iostream>

using namespace boost::assign;
using namespace std::placeholders;

static const double rankTol = 1.0;
// Create a noise model for the pixel error
static const double sigma = 0.1;
static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));

// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;

// tests data
static Symbol x1('X', 1);
static Symbol x2('X', 2);
static Symbol x3('X', 3);

static Point2 measurement1(323.0, 240.0);

LevenbergMarquardtParams lmParams;
// Make more verbose like so (in tests):
// lmParams.verbosityLM = LevenbergMarquardtParams::SUMMARY;

/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor) {
  using namespace vanillaPose;
  SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
}

/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor2) {
  using namespace vanillaPose;
  SmartProjectionParams params;
  params.setRankTolerance(rankTol);
  SmartFactor factor1(model, sharedK, params);
}

/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor3) {
  using namespace vanillaPose;
  SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
  factor1->add(measurement1, x1);
}

/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor4) {
  using namespace vanillaPose;
  SmartProjectionParams params;
  params.setRankTolerance(rankTol);
  SmartFactor factor1(model, sharedK, params);
  factor1.add(measurement1, x1);
}

/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, params) {
  using namespace vanillaPose;
  SmartProjectionParams params;
  double rt = params.getRetriangulationThreshold();
  EXPECT_DOUBLES_EQUAL(1e-5, rt, 1e-7);
  params.setRetriangulationThreshold(1e-3);
  rt = params.getRetriangulationThreshold();
  EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7);
}

/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Equals ) {
  using namespace vanillaPose;
  SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
  factor1->add(measurement1, x1);

  SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK));
  factor2->add(measurement1, x1);

  CHECK(assert_equal(*factor1, *factor2));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, noiseless ) {

  using namespace vanillaPose;

  // Project two landmarks into two cameras
  Point2 level_uv = level_camera.project(landmark1);
  Point2 level_uv_right = level_camera_right.project(landmark1);

  SmartFactor factor(model, sharedK);
  factor.add(level_uv, x1);
  factor.add(level_uv_right, x2);

  Values values; // it's a pose factor, hence these are poses
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());

  double actualError = factor.error(values);
  double expectedError = 0.0;
  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);

  SmartFactor::Cameras cameras = factor.cameras(values);
  double actualError2 = factor.totalReprojectionError(cameras);
  EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);

  // Calculate expected derivative for point (easiest to check)
  std::function<Vector(Point3)> f = //
      std::bind(&SmartFactor::whitenedError<Point3>, factor, cameras, std::placeholders::_1);

  // Calculate using computeEP
  Matrix actualE;
  factor.triangulateAndComputeE(actualE, values);

  // get point
  boost::optional<Point3> point = factor.point();
  CHECK(point);

  // calculate numerical derivative with triangulated point
  Matrix expectedE = sigma * numericalDerivative11<Vector, Point3>(f, *point);
  EXPECT(assert_equal(expectedE, actualE, 1e-7));

  // Calculate using reprojectionError
  SmartFactor::Cameras::FBlocks F;
  Matrix E;
  Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E);
  EXPECT(assert_equal(expectedE, E, 1e-7));

  EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7));

  // Calculate using computeJacobians
  Vector b;
  SmartFactor::FBlocks Fs;
  factor.computeJacobians(Fs, E, b, cameras, *point);
  double actualError3 = b.squaredNorm();
  EXPECT(assert_equal(expectedE, E, 1e-7));
  EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6);
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, noisy ) {

  using namespace vanillaPose;

  // Project two landmarks into two cameras
  Point2 pixelError(0.2, 0.2);
  Point2 level_uv = level_camera.project(landmark1) + pixelError;
  Point2 level_uv_right = level_camera_right.project(landmark1);

  Values values;
  values.insert(x1, cam1.pose());
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
      Point3(0.5, 0.1, 0.3));
  values.insert(x2, pose_right.compose(noise_pose));

  SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK));
  factor->add(level_uv, x1);
  factor->add(level_uv_right, x2);

  double actualError1 = factor->error(values);

  SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK));
  Point2Vector measurements;
  measurements.push_back(level_uv);
  measurements.push_back(level_uv_right);

  KeyVector views {x1, x2};

  factor2->add(measurements, views);
  double actualError2 = factor2->error(values);
  DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
}

/* *************************************************************************/
TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) {
  using namespace vanillaPose;

  // create arbitrary body_T_sensor (transforms from sensor to body)
  Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));

  // These are the poses we want to estimate, from camera measurements
  const Pose3 sensor_T_body = body_T_sensor.inverse();
  Pose3 wTb1 = cam1.pose() * sensor_T_body;
  Pose3 wTb2 = cam2.pose() * sensor_T_body;
  Pose3 wTb3 = cam3.pose() * sensor_T_body;

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0);

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  // Create smart factors
  KeyVector views {x1, x2, x3};

  SmartProjectionParams params;
  params.setRankTolerance(1.0);
  params.setDegeneracyMode(IGNORE_DEGENERACY);
  params.setEnableEPI(false);

  SmartFactor smartFactor1(model, sharedK, body_T_sensor, params);
  smartFactor1.add(measurements_cam1, views);

  SmartFactor smartFactor2(model, sharedK, body_T_sensor, params);
  smartFactor2.add(measurements_cam2, views);

  SmartFactor smartFactor3(model, sharedK, body_T_sensor, params);
  smartFactor3.add(measurements_cam3, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  // Put all factors in factor graph, adding priors
  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, wTb1, noisePrior);
  graph.addPrior(x2, wTb2, noisePrior);

  // Check errors at ground truth poses
  Values gtValues;
  gtValues.insert(x1, wTb1);
  gtValues.insert(x2, wTb2);
  gtValues.insert(x3, wTb3);
  double actualError = graph.error(gtValues);
  double expectedError = 0.0;
  DOUBLES_EQUAL(expectedError, actualError, 1e-7)

  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
                           Point3(0.1, 0.1, 0.1));
  Values values;
  values.insert(x1, wTb1);
  values.insert(x2, wTb2);
  // initialize third pose with some noise, we expect it to move back to
  // original pose3
  values.insert(x3, wTb3 * noise_pose);

  LevenbergMarquardtParams lmParams;
  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(wTb3, result.at<Pose3>(x3)));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {

  using namespace vanillaPose2;
  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  KeyVector views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2));
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2));
  smartFactor2->add(measurements_cam2, views);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2));
  smartFactor3->add(measurements_cam3, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, cam1.pose(), noisePrior);
  graph.addPrior(x2, cam2.pose(), noisePrior);

  Values groundTruth;
  groundTruth.insert(x1, cam1.pose());
  groundTruth.insert(x2, cam2.pose());
  groundTruth.insert(x3, cam3.pose());
  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
      Point3(0.1, 0.1, 0.1)); // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  // initialize third pose with some noise, we expect it to move back to original pose_above
  values.insert(x3, pose_above * noise_pose);
  EXPECT(
      assert_equal(
          Pose3(
              Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
                  -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
              Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, Factors ) {

  using namespace vanillaPose;

  // Default cameras for simple derivatives
  Rot3 R;
  static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0));
  Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2(
      Pose3(R, Point3(1, 0, 0)), sharedK);

  // one landmarks 1m in front of camera
  Point3 landmark1(0, 0, 10);

  Point2Vector measurements_cam1;

  // Project 2 landmarks into 2 cameras
  measurements_cam1.push_back(cam1.project(landmark1));
  measurements_cam1.push_back(cam2.project(landmark1));

  // Create smart factors
  KeyVector views {x1, x2};

  SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(model, sharedK);
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::Cameras cameras;
  cameras.push_back(cam1);
  cameras.push_back(cam2);

  // Make sure triangulation works
  CHECK(smartFactor1->triangulateSafe(cameras));
  CHECK(!smartFactor1->isDegenerate());
  CHECK(!smartFactor1->isPointBehindCamera());
  boost::optional<Point3> p = smartFactor1->point();
  CHECK(p);
  EXPECT(assert_equal(landmark1, *p));

  VectorValues zeroDelta;
  Vector6 delta;
  delta.setZero();
  zeroDelta.insert(x1, delta);
  zeroDelta.insert(x2, delta);

  VectorValues perturbedDelta;
  delta.setOnes();
  perturbedDelta.insert(x1, delta);
  perturbedDelta.insert(x2, delta);
  double expectedError = 2500;

  // After eliminating the point, A1 and A2 contain 2-rank information on cameras:
  Matrix16 A1, A2;
  A1 << -10, 0, 0, 0, 1, 0;
  A2 << 10, 0, 1, 0, -1, 0;
  A1 *= 10. / sigma;
  A2 *= 10. / sigma;
  Matrix expectedInformation; // filled below
  {
    // createHessianFactor
    Matrix66 G11 = 0.5 * A1.transpose() * A1;
    Matrix66 G12 = 0.5 * A1.transpose() * A2;
    Matrix66 G22 = 0.5 * A2.transpose() * A2;

    Vector6 g1;
    g1.setZero();
    Vector6 g2;
    g2.setZero();

    double f = 0;

    RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f);
    expectedInformation = expected.information();

    boost::shared_ptr<RegularHessianFactor<6> > actual =
        smartFactor1->createHessianFactor(cameras, 0.0);
    EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
    EXPECT(assert_equal(expected, *actual, 1e-6));
    EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
    EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
  }

  {
    Matrix26 F1;
    F1.setZero();
    F1(0, 1) = -100;
    F1(0, 3) = -10;
    F1(1, 0) = 100;
    F1(1, 4) = -10;
    Matrix26 F2;
    F2.setZero();
    F2(0, 1) = -101;
    F2(0, 3) = -10;
    F2(0, 5) = -1;
    F2(1, 0) = 100;
    F2(1, 2) = 10;
    F2(1, 4) = -10;
    Matrix E(4, 3);
    E.setZero();
    E(0, 0) = 10;
    E(1, 1) = 10;
    E(2, 0) = 10;
    E(2, 2) = 1;
    E(3, 1) = 10;
    SmartFactor::FBlocks Fs = list_of<Matrix>(F1)(F2);
    Vector b(4);
    b.setZero();

    // Create smart factors
    KeyVector keys;
    keys.push_back(x1);
    keys.push_back(x2);

    // createJacobianQFactor
    SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
    Matrix3 P = (E.transpose() * E).inverse();
    JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n);
    EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-6));

    boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
        smartFactor1->createJacobianQFactor(cameras, 0.0);
    CHECK(actualQ);
    EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-6));
    EXPECT(assert_equal(expectedQ, *actualQ));
    EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-6);
    EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-6);

    // Whiten for RegularImplicitSchurFactor (does not have noise model)
    model->WhitenSystem(E, b);
    Matrix3 whiteP = (E.transpose() * E).inverse();
    Fs[0] = model->Whiten(Fs[0]);
    Fs[1] = model->Whiten(Fs[1]);

    // createRegularImplicitSchurFactor
    RegularImplicitSchurFactor<Camera> expected(keys, Fs, E, whiteP, b);

    boost::shared_ptr<RegularImplicitSchurFactor<Camera> > actual =
        smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
    CHECK(actual);
    EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6));
    EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
    EXPECT(assert_equal(expected, *actual));
    EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
    EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
  }

  {
    // createJacobianSVDFactor
    Vector1 b;
    b.setZero();
    double s = sigma * sin(M_PI_4);
    SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma);
    JacobianFactor expected(x1, s * A1, x2, s * A2, b, n);
    EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6));

    boost::shared_ptr<JacobianFactor> actual =
        smartFactor1->createJacobianSVDFactor(cameras, 0.0);
    CHECK(actual);
    EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
    EXPECT(assert_equal(expected, *actual));
    EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
    EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
  }
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {

  using namespace vanillaPose;

  KeyVector views {x1, x2, x3};

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK));
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK));
  smartFactor2->add(measurements_cam2, views);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK));
  smartFactor3->add(measurements_cam3, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, cam1.pose(), noisePrior);
  graph.addPrior(x2, cam2.pose(), noisePrior);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
      Point3(0.1, 0.1, 0.1)); // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  // initialize third pose with some noise, we expect it to move back to original pose_above
  values.insert(x3, pose_above * noise_pose);
  EXPECT(
      assert_equal(
          Pose3(
              Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656,
                  -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364,
                  -0.0313952598), Point3(0.1, -0.1, 1.9)),
          values.at<Pose3>(x3)));

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, jacobianSVD ) {

  using namespace vanillaPose;

  KeyVector views {x1, x2, x3};

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  SmartProjectionParams params;
  params.setRankTolerance(1.0);
  params.setLinearizationMode(gtsam::JACOBIAN_SVD);
  params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
  params.setEnableEPI(false);
  SmartFactor factor1(model, sharedK, params);

  SmartFactor::shared_ptr smartFactor1(
      new SmartFactor(model, sharedK, params));
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr smartFactor2(
      new SmartFactor(model, sharedK, params));
  smartFactor2->add(measurements_cam2, views);

  SmartFactor::shared_ptr smartFactor3(
      new SmartFactor(model, sharedK, params));
  smartFactor3->add(measurements_cam3, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, cam1.pose(), noisePrior);
  graph.addPrior(x2, cam2.pose(), noisePrior);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
      Point3(0.1, 0.1, 0.1)); // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  values.insert(x3, pose_above * noise_pose);

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, landmarkDistance ) {

  using namespace vanillaPose;

  double excludeLandmarksFutherThanDist = 2;

  KeyVector views {x1, x2, x3};

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  SmartProjectionParams params;
  params.setRankTolerance(1.0);
  params.setLinearizationMode(gtsam::JACOBIAN_SVD);
  params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
  params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
  params.setEnableEPI(false);

  SmartFactor::shared_ptr smartFactor1(
      new SmartFactor(model, sharedK, params));
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr smartFactor2(
      new SmartFactor(model, sharedK, params));
  smartFactor2->add(measurements_cam2, views);

  SmartFactor::shared_ptr smartFactor3(
      new SmartFactor(model, sharedK, params));
  smartFactor3->add(measurements_cam3, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, cam1.pose(), noisePrior);
  graph.addPrior(x2, cam2.pose(), noisePrior);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
      Point3(0.1, 0.1, 0.1)); // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  values.insert(x3, pose_above * noise_pose);

  // All factors are disabled and pose should remain where it is
  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {

  using namespace vanillaPose;

  double excludeLandmarksFutherThanDist = 1e10;
  double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error

  KeyVector views {x1, x2, x3};

  // add fourth landmark
  Point3 landmark4(5, -0.5, 1);

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
      measurements_cam4;

  // Project 4 landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
  measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier

  SmartProjectionParams params;
  params.setLinearizationMode(gtsam::JACOBIAN_SVD);
  params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
  params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);

  SmartFactor::shared_ptr smartFactor1(
      new SmartFactor(model, sharedK, params));
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr smartFactor2(
      new SmartFactor(model, sharedK, params));
  smartFactor2->add(measurements_cam2, views);

  SmartFactor::shared_ptr smartFactor3(
      new SmartFactor(model, sharedK, params));
  smartFactor3->add(measurements_cam3, views);

  SmartFactor::shared_ptr smartFactor4(
      new SmartFactor(model, sharedK, params));
  smartFactor4->add(measurements_cam4, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.push_back(smartFactor4);
  graph.addPrior(x1, cam1.pose(), noisePrior);
  graph.addPrior(x2, cam2.pose(), noisePrior);

  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  values.insert(x3, cam3.pose());

  // All factors are disabled and pose should remain where it is
  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3)));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, jacobianQ ) {

  using namespace vanillaPose;

  KeyVector views {x1, x2, x3};

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  SmartProjectionParams params;
  params.setLinearizationMode(gtsam::JACOBIAN_Q);

  SmartFactor::shared_ptr smartFactor1(
      new SmartFactor(model, sharedK, params));
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr smartFactor2(
      new SmartFactor(model, sharedK, params));
  smartFactor2->add(measurements_cam2, views);

  SmartFactor::shared_ptr smartFactor3(
      new SmartFactor(model, sharedK, params));
  smartFactor3->add(measurements_cam3, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, cam1.pose(), noisePrior);
  graph.addPrior(x2, cam2.pose(), noisePrior);

  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
      Point3(0.1, 0.1, 0.1)); // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  values.insert(x3, pose_above * noise_pose);

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {

  using namespace vanillaPose2;

  KeyVector views {x1, x2, x3};

  typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;
  NonlinearFactorGraph graph;

  // Project three landmarks into three cameras
  graph.emplace_shared<ProjectionFactor>(cam1.project(landmark1), model, x1, L(1), sharedK2);
  graph.emplace_shared<ProjectionFactor>(cam2.project(landmark1), model, x2, L(1), sharedK2);
  graph.emplace_shared<ProjectionFactor>(cam3.project(landmark1), model, x3, L(1), sharedK2);

  graph.emplace_shared<ProjectionFactor>(cam1.project(landmark2), model, x1, L(2), sharedK2);
  graph.emplace_shared<ProjectionFactor>(cam2.project(landmark2), model, x2, L(2), sharedK2);
  graph.emplace_shared<ProjectionFactor>(cam3.project(landmark2), model, x3, L(2), sharedK2);

  graph.emplace_shared<ProjectionFactor>(cam1.project(landmark3), model, x1, L(3), sharedK2);
  graph.emplace_shared<ProjectionFactor>(cam2.project(landmark3), model, x2, L(3), sharedK2);
  graph.emplace_shared<ProjectionFactor>(cam3.project(landmark3), model, x3, L(3), sharedK2);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
  graph.addPrior(x1, level_pose, noisePrior);
  graph.addPrior(x2, pose_right, noisePrior);

  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
      Point3(0.5, 0.1, 0.3));
  Values values;
  values.insert(x1, level_pose);
  values.insert(x2, pose_right);
  values.insert(x3, pose_above * noise_pose);
  values.insert(L(1), landmark1);
  values.insert(L(2), landmark2);
  values.insert(L(3), landmark3);

  DOUBLES_EQUAL(48406055, graph.error(values), 1);

  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  Values result = optimizer.optimize();

  DOUBLES_EQUAL(0, graph.error(result), 1e-9);

  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, CheckHessian) {

  KeyVector views {x1, x2, x3};

  using namespace vanillaPose;

  // Two slightly different cameras
  Pose3 pose2 = level_pose
      * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
  Camera cam2(pose2, sharedK);
  Camera cam3(pose3, sharedK);

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  SmartProjectionParams params;
  params.setRankTolerance(10);

  SmartFactor::shared_ptr smartFactor1(
      new SmartFactor(model, sharedK, params)); // HESSIAN, by default
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr smartFactor2(
      new SmartFactor(model, sharedK, params)); // HESSIAN, by default
  smartFactor2->add(measurements_cam2, views);

  SmartFactor::shared_ptr smartFactor3(
      new SmartFactor(model, sharedK, params)); // HESSIAN, by default
  smartFactor3->add(measurements_cam3, views);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
      Point3(0.1, 0.1, 0.1)); // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  // initialize third pose with some noise, we expect it to move back to original pose_above
  values.insert(x3, pose3 * noise_pose);
  EXPECT(
      assert_equal(
          Pose3(
              Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
                  -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
                  -0.130455917),
              Point3(0.0897734171, -0.110201006, 0.901022872)),
          values.at<Pose3>(x3)));

  boost::shared_ptr<GaussianFactor> factor1 = smartFactor1->linearize(values);
  boost::shared_ptr<GaussianFactor> factor2 = smartFactor2->linearize(values);
  boost::shared_ptr<GaussianFactor> factor3 = smartFactor3->linearize(values);

  Matrix CumulativeInformation = factor1->information() + factor2->information()
      + factor3->information();

  boost::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(
      values);
  Matrix GraphInformation = GaussianGraph->hessian().first;

  // Check Hessian
  EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6));

  Matrix AugInformationMatrix = factor1->augmentedInformation()
      + factor2->augmentedInformation() + factor3->augmentedInformation();

  // Check Information vector
  Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector

  // Check Hessian
  EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) {
  using namespace vanillaPose2;

  KeyVector views {x1, x2, x3};

  // Two different cameras, at the same position, but different rotations
  Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
  Camera cam2(pose2, sharedK2);
  Camera cam3(pose3, sharedK2);

  Point2Vector measurements_cam1, measurements_cam2;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);

  SmartProjectionParams params;
  params.setRankTolerance(50);
  params.setDegeneracyMode(gtsam::HANDLE_INFINITY);

  SmartFactor::shared_ptr smartFactor1(
      new SmartFactor(model, sharedK2, params));
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr smartFactor2(
      new SmartFactor(model, sharedK2, params));
  smartFactor2->add(measurements_cam2, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
  const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
  Point3 positionPrior = Point3(0, 0, 1);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.addPrior(x1, cam1.pose(), noisePrior);
  graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
  graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);

  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
      Point3(0.1, 0.1, 0.1)); // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, pose2 * noise_pose);
  values.insert(x3, pose3 * noise_pose);

  // params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  Values result = optimizer.optimize();
  EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) {

  // this test considers a condition in which the cheirality constraint is triggered
  using namespace vanillaPose;

  KeyVector views {x1, x2, x3};

  // Two different cameras, at the same position, but different rotations
  Pose3 pose2 = level_pose
      * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
  Camera cam2(pose2, sharedK);
  Camera cam3(pose3, sharedK);

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  SmartProjectionParams params;
  params.setRankTolerance(10);
  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);

  SmartFactor::shared_ptr smartFactor1(
      new SmartFactor(model, sharedK, params));
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr smartFactor2(
      new SmartFactor(model, sharedK, params));
  smartFactor2->add(measurements_cam2, views);

  SmartFactor::shared_ptr smartFactor3(
      new SmartFactor(model, sharedK, params));
  smartFactor3->add(measurements_cam3, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
  const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,
      0.10);
  Point3 positionPrior = Point3(0, 0, 1);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, cam1.pose(), noisePrior);
  graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
  graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
      Point3(0.1, 0.1, 0.1)); // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  values.insert(x3, pose3 * noise_pose);
  EXPECT(
      assert_equal(
          Pose3(
              Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
                  -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
                  -0.130455917),
              Point3(0.0897734171, -0.110201006, 0.901022872)),
          values.at<Pose3>(x3)));

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();

  // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY)
  // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
  EXPECT(assert_equal(Pose3(values.at<Pose3>(x3).rotation(),
      Point3(0,0,1)), result.at<Pose3>(x3)));
#else
  // if the check is disabled, no cheirality exception if thrown and the pose converges to the right rotation
  // with modest accuracy since the configuration is essentially degenerate without the translation due to noise (noise_pose)
  EXPECT(assert_equal(pose3, result.at<Pose3>(x3),1e-3));
#endif
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, Hessian ) {

  using namespace vanillaPose2;

  KeyVector views {x1, x2};

  // Project three landmarks into 2 cameras
  Point2 cam1_uv1 = cam1.project(landmark1);
  Point2 cam2_uv1 = cam2.project(landmark1);
  Point2Vector measurements_cam1;
  measurements_cam1.push_back(cam1_uv1);
  measurements_cam1.push_back(cam2_uv1);

  SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2));
  smartFactor1->add(measurements_cam1, views);

  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
      Point3(0.5, 0.1, 0.3));
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());

  boost::shared_ptr<GaussianFactor> factor = smartFactor1->linearize(values);

  // compute triangulation from linearization point
  // compute reprojection errors (sum squared)
  // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance)
  // check that it is correctly scaled when using noiseProjection = [1/4  0; 0 1/4]
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
  // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl;

  using namespace vanillaPose;

  KeyVector views {x1, x2, x3};

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);

  SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK));
  smartFactorInstance->add(measurements_cam1, views);

  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  values.insert(x3, cam3.pose());

  boost::shared_ptr<GaussianFactor> factor = smartFactorInstance->linearize(
      values);

  Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));

  Values rotValues;
  rotValues.insert(x1, poseDrift.compose(level_pose));
  rotValues.insert(x2, poseDrift.compose(pose_right));
  rotValues.insert(x3, poseDrift.compose(pose_above));

  boost::shared_ptr<GaussianFactor> factorRot = smartFactorInstance->linearize(
      rotValues);

  // Hessian is invariant to rotations in the nondegenerate case
  EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7));

  Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
      Point3(10, -4, 5));

  Values tranValues;
  tranValues.insert(x1, poseDrift2.compose(level_pose));
  tranValues.insert(x2, poseDrift2.compose(pose_right));
  tranValues.insert(x3, poseDrift2.compose(pose_above));

  boost::shared_ptr<GaussianFactor> factorRotTran =
      smartFactorInstance->linearize(tranValues);

  // Hessian is invariant to rotations and translations in the nondegenerate case
  EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {

  using namespace vanillaPose2;

  KeyVector views {x1, x2, x3};

  // All cameras have the same pose so will be degenerate !
  Camera cam2(level_pose, sharedK2);
  Camera cam3(level_pose, sharedK2);

  Point2Vector measurements_cam1;
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);

  SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2));
  smartFactor->add(measurements_cam1, views);

  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  values.insert(x3, cam3.pose());

  boost::shared_ptr<GaussianFactor> factor = smartFactor->linearize(values);

  Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));

  Values rotValues;
  rotValues.insert(x1, poseDrift.compose(level_pose));
  rotValues.insert(x2, poseDrift.compose(level_pose));
  rotValues.insert(x3, poseDrift.compose(level_pose));

  boost::shared_ptr<GaussianFactor> factorRot = //
      smartFactor->linearize(rotValues);

  // Hessian is invariant to rotations in the nondegenerate case
  EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7));

  Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
      Point3(10, -4, 5));

  Values tranValues;
  tranValues.insert(x1, poseDrift2.compose(level_pose));
  tranValues.insert(x2, poseDrift2.compose(level_pose));
  tranValues.insert(x3, poseDrift2.compose(level_pose));

  boost::shared_ptr<GaussianFactor> factorRotTran = smartFactor->linearize(
      tranValues);

  // Hessian is invariant to rotations and translations in the nondegenerate case
  EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7));
}

/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
  using namespace bundlerPose;
  SmartProjectionParams params;
  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
  SmartFactor factor(model, sharedBundlerK, params);
  factor.add(measurement1, x1);
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, Cal3Bundler ) {

  using namespace bundlerPose;

  // three landmarks ~5 meters in front of camera
  Point3 landmark3(3, 0, 3.0);

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  KeyVector views {x1, x2, x3};

  SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK));
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK));
  smartFactor2->add(measurements_cam2, views);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK));
  smartFactor3->add(measurements_cam3, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, cam1.pose(), noisePrior);
  graph.addPrior(x2, cam2.pose(), noisePrior);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
      Point3(0.1, 0.1, 0.1)); // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  // initialize third pose with some noise, we expect it to move back to original pose_above
  values.insert(x3, pose_above * noise_pose);
  EXPECT(
      assert_equal(
          Pose3(
              Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
                  -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
              Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3), 1e-6));
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {

  using namespace bundlerPose;

  KeyVector views {x1, x2, x3};

  // Two different cameras
  Pose3 pose2 = level_pose
      * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
  Camera cam2(pose2, sharedBundlerK);
  Camera cam3(pose3, sharedBundlerK);

  // landmark3 at 3 meters now
  Point3 landmark3(3, 0, 3.0);

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  SmartProjectionParams params;
  params.setRankTolerance(10);
  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);

  SmartFactor::shared_ptr smartFactor1(
      new SmartFactor(model, sharedBundlerK, params));
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr smartFactor2(
      new SmartFactor(model, sharedBundlerK, params));
  smartFactor2->add(measurements_cam2, views);

  SmartFactor::shared_ptr smartFactor3(
      new SmartFactor(model, sharedBundlerK, params));
  smartFactor3->add(measurements_cam3, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
  const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,
      0.10);
  Point3 positionPrior = Point3(0, 0, 1);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, cam1.pose(), noisePrior);
  graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
  graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
      Point3(0.1, 0.1, 0.1)); // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  // initialize third pose with some noise, we expect it to move back to original pose_above
  values.insert(x3, pose3 * noise_pose);
  EXPECT(
      assert_equal(
          Pose3(
              Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
                  -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
                  -0.130455917),
              Point3(0.0897734171, -0.110201006, 0.901022872)),
          values.at<Pose3>(x3)));

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();

  EXPECT(
      assert_equal(
          Pose3(
              Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
                  -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
                  -0.130455917),
              Point3(0.0897734171, -0.110201006, 0.901022872)),
          values.at<Pose3>(x3)));
}

/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")

TEST(SmartProjectionPoseFactor, serialize) {
  using namespace vanillaPose;
  using namespace gtsam::serializationTestHelpers;
  SmartProjectionParams params;
  params.setRankTolerance(rankTol);
  SmartFactor factor(model, sharedK, params);

  EXPECT(equalsObj(factor));
  EXPECT(equalsXML(factor));
  EXPECT(equalsBinary(factor));
}

TEST(SmartProjectionPoseFactor, serialize2) {
  using namespace vanillaPose;
  using namespace gtsam::serializationTestHelpers;
  SmartProjectionParams params;
  params.setRankTolerance(rankTol);
  Pose3 bts;
  SmartFactor factor(model, sharedK, bts, params);

  // insert some measurments
  KeyVector key_view;
  Point2Vector meas_view;
  key_view.push_back(Symbol('x', 1));
  meas_view.push_back(Point2(10, 10));
  factor.add(meas_view, key_view);

  EXPECT(equalsObj(factor));
  EXPECT(equalsXML(factor));
  EXPECT(equalsBinary(factor));
}

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


#ifndef RNN_TYPE_HPP
#define RNN_TYPE_HPP

#include <Eigen/Core>

namespace rnn::generic {

#ifdef USE_FLOAT
typedef float real;
typedef Eigen::MatrixXf MatD;
typedef Eigen::VectorXf VecD;
#else
typedef double real;
typedef Eigen::MatrixXd MatD;
typedef Eigen::VectorXd VecD;
#endif

typedef Eigen::MatrixXi MatI;
typedef Eigen::VectorXi VecI;
#define REAL_MAX std::numeric_limits<real>::max()

} // namespace rnn

#endif


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_EXPONENTIAL_FUNCTION_SCALAR_CBRT_HPP_INCLUDED
#define NT2_TOOLBOX_EXPONENTIAL_FUNCTION_SCALAR_CBRT_HPP_INCLUDED

#include <nt2/toolbox/exponential/details/math.hpp>
#include <boost/math/special_functions/cbrt.hpp>
#include <nt2/include/functions/is_invalid.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::cbrt_, tag::cpu_,
                      (A0),
                      (arithmetic_<A0>)
                     )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::cbrt_(tag::arithmetic_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)> :
      std::tr1::result_of<meta::floating(A0)>{};

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename NT2_RETURN_TYPE(1)::type type;
      return nt2::cbrt(type(a0));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::cbrt_, tag::cpu_,
                      (A0),
                      (double_<A0>)
                     )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::cbrt_(tag::double_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)> :
      std::tr1::result_of<meta::floating(A0)>{};

    NT2_FUNCTOR_CALL(1)
    {
      if (is_invalid(a0)) return a0;
#ifdef NT2_TOOLBOX_EXPONENTIAL_HAS_CBRT
      return ::cbrt(a0);
#else
      return boost::math::cbrt(a0);
#endif
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::cbrt_, tag::cpu_,
                      (A0),
                      (float_<A0>)
                     )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::cbrt_(tag::float_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)> :
      std::tr1::result_of<meta::floating(A0)>{};

    NT2_FUNCTOR_CALL(1)
    {
      if (is_invalid(a0)) return a0;
#ifdef NT2_TOOLBOX_EXPONENTIAL_HAS_CBRTF
      return ::cbrtf(a0);
#else
      return boost::math::cbrt(a0);
#endif
    }
  };
} }

#endif
// modified by jt the 26/12/2010


#include <iostream>

#define BOOST_TEST_MODULE SparseAssignTests

#include "MIAConfig.h"

#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif
#include "SparseMIA.h"

#include "Index.h"

//typedef LibMIA::DenseMIA<double,3> dmia;

template<class MIAType>
void assign_work(){

    LibMIA::MIAINDEX i;
    LibMIA::MIAINDEX j;
    LibMIA::MIAINDEX k;

    MIAType a(5,7,4);
    MIAType b;
    MIAType c;

    c=a;
    BOOST_CHECK_MESSAGE(a==c && c==a,"Empty SparseMIA assignment");

    a.resize(a.dimensionality()/2);
    a.randu(-5,5);
    a.rand_indices();
    a.collect_duplicates();
    c=a;
    BOOST_CHECK_MESSAGE(a==c && c==a,"Straight SparseMIA assignment");

    b.zeros();
    c.zeros();
    b(i,j,k)=a(i,j,k);
    c(i,j,k)=b(i,j,k);
    BOOST_CHECK_MESSAGE(a==c,"Non-shuffled MIA Expression assignment");

    b.zeros();
    c.zeros();
    b(i,j,k)=a(i,k,j);
    c(i,j,k)=b(i,k,j);
    BOOST_CHECK_MESSAGE(a==c,"Shuffled MIA Expression assignment");

    b.zeros();
    c.zeros();
    b(i,j,k)=a(i,!k,j);
    c(!i,j,k)=b(i,k,j);
    BOOST_CHECK_MESSAGE(a==c,"Shuffled MIA Expression assignment with mismatched elem-wise indices");

}

BOOST_AUTO_TEST_CASE( SparseAssignTests )
{

    assign_work<LibMIA::SparseMIA<double,3>>();
    assign_work<LibMIA::SparseMIA<float,3>>();
    assign_work<LibMIA::SparseMIA<int,3>>();
    assign_work<LibMIA::SparseMIA<long,3>>();

}


#ifndef COMMAND
#define COMMAND

#include <cstdlib>
#include <string>
#include <boost/algorithm/string.hpp>

enum class CommandType {
  ON, OFF, TOGGLE
    };

struct Command {

  Command(CommandType t, size_t x0,size_t y0, size_t x1, size_t y1)
    : type(t), x0(x0), y0(y0), x1(x1), y1(y1) {}

  const CommandType type;
  const size_t x0, y0, x1, y1;
};

Command buildCommand(const std::string& line) {
  std::vector<std::string> chunks;
  boost::split(chunks, line, boost::is_any_of(" "));

  CommandType t;
  size_t from_index, to_index;

  //Determine CommandType
  if(chunks.size() == 4) {
    t = CommandType::TOGGLE;
    from_index = 1;
    to_index = 3;
  } else {
    from_index = 2;
    to_index = 4;

    if(chunks[1] == "on") {
      t = CommandType::ON;
    } else {
      t = CommandType::OFF;
    }
  }

  std::vector<std::string> coord;
  boost::split(coord, chunks[from_index], boost::is_any_of(","));
  size_t x0 = stoi(coord[0]);
  size_t y0 = stoi(coord[1]);

  coord.clear();
  boost::split(coord, chunks[to_index], boost::is_any_of(","));
  size_t x1 = stoi(coord[0]);
  size_t y1 = stoi(coord[1]);

  return Command(t, x0, y0, x1, y1);
}

#endif // COMMAND


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    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.
 *
 */

#define BOOST_TEST_MAIN

#include <limits>
#include <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Mathematics/Interpolators/cubicSplineInterpolator.h"
#include "Tudat/InputOutput/basicInputOutput.h"

#include "Tudat/Astrodynamics/EarthOrientation/earthOrientationCalculator.h"
#include "Tudat/Mathematics/Interpolators/jumpDataLinearInterpolator.h"

namespace tudat
{
namespace unit_tests
{

using namespace tudat::earth_orientation;
using namespace tudat::interpolators;

BOOST_AUTO_TEST_SUITE( test_eop_reader )

//! Tests whether EOP data is properly read and processed, and whether data in correctly interpolated (linearly)
BOOST_AUTO_TEST_CASE( testEopReaderData )
{
    std::shared_ptr< EOPReader > eopReader = std::make_shared< EOPReader >(
                tudat::input_output::getEarthOrientationDataFilesPath( ) + "eopc04_08_IAU2000.62-now.txt",
                "C04", basic_astrodynamics::iau_2000_a );

    // Define current time/
    int year = 2007;
    int month = 4;
    int day = 5;
    int hour = 0;
    int minute = 0;
    double seconds = 0.0;
    double utcSecondsSinceJ2000 = tudat::basic_astrodynamics::convertCalendarDateToJulianDaysSinceEpoch(
                year, month, day, hour, minute, seconds, basic_astrodynamics::JULIAN_DAY_ON_J2000 )
            * physical_constants::JULIAN_DAY;

    // Set EOP corrections read manually from file for 5-4-2007
    double arcSecondToRadian = 4.848136811095359935899141E-6;
    double expectedXp = 0.033227 * arcSecondToRadian;
    double expectedYp = 0.483135 * arcSecondToRadian;
    double expectedUT1COffset = -0.0714209;
    double expecteddX = 0.000247 * arcSecondToRadian;
    double expecteddY = -0.000280 * arcSecondToRadian;

    // Create interpolator for x_{p} and y_{p} (polar motion) and retrieve current value
    std::shared_ptr< OneDimensionalInterpolator< double, Eigen::Vector2d > > cipInItrsInterpolator =
            tudat::earth_orientation::createStandardEarthOrientationCalculator( eopReader )->getPolarMotionCalculator( )->
            getDailyIersValueInterpolator( );
    Eigen::Vector2d cipInItrs = cipInItrsInterpolator->interpolate( utcSecondsSinceJ2000 );

    // Create interpolator for dX and dY (nutation correction) and retrieve current value
    std::shared_ptr< OneDimensionalInterpolator< double, Eigen::Vector2d > > cipInGcrsCorrectionInterpolator =
            tudat::earth_orientation::createStandardEarthOrientationCalculator( eopReader )->getPrecessionNutationCalculator( )->
            getDailyCorrectionInterpolator( );
    Eigen::Vector2d cipInGcrs = cipInGcrsCorrectionInterpolator->interpolate( utcSecondsSinceJ2000 );

    // Create interpolator for d(UT1-UTC) correction  and retrieve current value
    std::shared_ptr< OneDimensionalInterpolator< double, double > > ut1MinusUtcInterpolator =
            createDefaultTimeConverter( eopReader )->getDailyUtcUt1CorrectionInterpolator( );
    double utcMinusUt1 = ut1MinusUtcInterpolator->interpolate( utcSecondsSinceJ2000 );

    // Check interpolated values against test data.
    BOOST_CHECK_CLOSE_FRACTION( utcMinusUt1, expectedUT1COffset, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInItrs.x( ), expectedXp, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInItrs.y( ), expectedYp, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInGcrs.x( ), expecteddX, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInGcrs.y( ), expecteddY, std::numeric_limits< double >::epsilon( ) );

    // Reset time
    int day2 = 6;
    utcSecondsSinceJ2000 = tudat::basic_astrodynamics::convertCalendarDateToJulianDaysSinceEpoch(
                year, month, day2, hour, minute, seconds, basic_astrodynamics::JULIAN_DAY_ON_J2000 )
            * physical_constants::JULIAN_DAY;

    // Set EOP corrections read manually from file for 6-4-2007
    double expectedXp2 = 0.035739  * arcSecondToRadian;
    double expectedYp2 = 0.484209 * arcSecondToRadian;
    double expectedUT1COffset2 = -0.0727562;
    double expecteddX2 = 0.000252 * arcSecondToRadian;
    double expecteddY2 = -0.000294 * arcSecondToRadian;

    // Read correction interpolators
    cipInItrs = cipInItrsInterpolator->interpolate( utcSecondsSinceJ2000 );
    cipInGcrs = cipInGcrsCorrectionInterpolator->interpolate( utcSecondsSinceJ2000 );
    utcMinusUt1 = ut1MinusUtcInterpolator->interpolate( utcSecondsSinceJ2000 );

    // Check interpolated values against test data.
    BOOST_CHECK_CLOSE_FRACTION( utcMinusUt1, expectedUT1COffset2, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInItrs.x( ), expectedXp2, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInItrs.y( ), expectedYp2, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInGcrs.x( ), expecteddX2, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInGcrs.y( ), expecteddY2, std::numeric_limits< double >::epsilon( ) );

    // Reset time to halfway between previous times
    int hour2 = 12;
    utcSecondsSinceJ2000 = tudat::basic_astrodynamics::convertCalendarDateToJulianDaysSinceEpoch(
                year, month, day, hour2, minute, seconds, basic_astrodynamics::JULIAN_DAY_ON_J2000 )
            * physical_constants::JULIAN_DAY;

    // Read correction interpolators
    cipInItrs = cipInItrsInterpolator->interpolate( utcSecondsSinceJ2000 );
    cipInGcrs = cipInGcrsCorrectionInterpolator->interpolate( utcSecondsSinceJ2000 );
    utcMinusUt1 = ut1MinusUtcInterpolator->interpolate( utcSecondsSinceJ2000 );

    BOOST_CHECK_CLOSE_FRACTION( utcMinusUt1, ( expectedUT1COffset + expectedUT1COffset2 ) / 2.0,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInItrs.x( ), ( expectedXp + expectedXp2 ) / 2.0, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInItrs.y( ), ( expectedYp + expectedYp2 ) / 2.0, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInGcrs.x( ), ( expecteddX + expecteddX2 ) / 2.0, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInGcrs.y( ), ( expecteddY + expecteddY2 ) / 2.0, std::numeric_limits< double >::epsilon( ) );

    // Reset time to halfway between previous times
    int hour3 = 1;
    utcSecondsSinceJ2000 = tudat::basic_astrodynamics::convertCalendarDateToJulianDaysSinceEpoch(
                year, month, day, hour3, minute, seconds, basic_astrodynamics::JULIAN_DAY_ON_J2000 )
            * physical_constants::JULIAN_DAY;

    // Read correction interpolators
    cipInItrs = cipInItrsInterpolator->interpolate( utcSecondsSinceJ2000 );
    cipInGcrs = cipInGcrsCorrectionInterpolator->interpolate( utcSecondsSinceJ2000 );
    utcMinusUt1 = ut1MinusUtcInterpolator->interpolate( utcSecondsSinceJ2000 );

    BOOST_CHECK_CLOSE_FRACTION( utcMinusUt1, expectedUT1COffset + ( expectedUT1COffset2 - expectedUT1COffset ) / 24.0,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInItrs.x( ), expectedXp + ( expectedXp2 - expectedXp ) / 24.0,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInItrs.y( ), expectedYp + ( expectedYp2 - expectedYp ) / 24.0,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInGcrs.x( ), expecteddX + ( expecteddX2 - expecteddX ) / 24.0,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( cipInGcrs.y( ), expecteddY + ( expecteddY2 - expecteddY ) / 24.0,
                                std::numeric_limits< double >::epsilon( ) );
}

//! Check whether UT1-UTC interpolator correctly handles leap secons
BOOST_AUTO_TEST_CASE( testLeapSecondIdentification )
{
    // Read EOP file and get UT1-UTC interpolator
    std::shared_ptr< EOPReader > eopReader = std::make_shared< EOPReader >(
                tudat::input_output::getEarthOrientationDataFilesPath( ) + "eopc04_08_IAU2000.62-now.txt",
                "C04", basic_astrodynamics::iau_2000_a );
    std::shared_ptr< OneDimensionalInterpolator< double, double > > ut1MinusUtcInterpolator =
            createDefaultTimeConverter( eopReader )->getDailyUtcUt1CorrectionInterpolator( );

    // Define list of leap seconds
    Eigen::Matrix< int, Eigen::Dynamic, 3 > leapSecondDays;
    leapSecondDays.resize( 27, 3 );
    leapSecondDays << 1, 7, 1972,
            1, 1, 1973,
            1, 1, 1974,
            1, 1, 1975,
            1, 1, 1976,
            1, 1, 1977,
            1, 1, 1978,
            1, 1, 1979,
            1, 1, 1980,
            1, 7, 1981,
            1, 7, 1982,
            1, 7, 1983,
            1, 7, 1985,
            1, 1, 1988,
            1, 1, 1990,
            1, 1, 1991,
            1, 7, 1992,
            1, 7, 1993,
            1, 7, 1994,
            1, 1, 1996,
            1, 7, 1997,
            1, 1, 1999,
            1, 1, 2006,
            1, 1, 2009,
            1, 7, 2012,
            1, 7, 2015,
            1, 1, 2017;

    // Declare test variables.
    double differenceMinusOneDay, differenceMinusOneMilliSecond,
            differenceAtEpochPlusOneMilliSecond, differencePlusOneDay;

    // Set check time: at midnight
    int hour = 0;
    int minute = 0;
    double seconds = 0.0;

    // Iterate over all leap seconds
    for( int i = 0; i < leapSecondDays.rows( ); i++ )
    {
        // Get UTC time of current leap second
        int year, month, day;
        year = leapSecondDays( i, 2 );
        month = leapSecondDays( i, 1 );
        day = leapSecondDays( i, 0 );
        double utcSecondsSinceJ2000 = tudat::basic_astrodynamics::convertCalendarDateToJulianDaysSinceEpoch(
                    year, month, day, hour, minute, seconds, basic_astrodynamics::JULIAN_DAY_ON_J2000 )
                * physical_constants::JULIAN_DAY;

        // Define test times
        differenceMinusOneDay = ut1MinusUtcInterpolator->interpolate( utcSecondsSinceJ2000 - physical_constants::JULIAN_DAY );
        differenceMinusOneMilliSecond = ut1MinusUtcInterpolator->interpolate( utcSecondsSinceJ2000 - 1.0E-3 );
        differenceAtEpochPlusOneMilliSecond = ut1MinusUtcInterpolator->interpolate( utcSecondsSinceJ2000 + 1.0E-3 );
        differencePlusOneDay = ut1MinusUtcInterpolator->interpolate( utcSecondsSinceJ2000 + physical_constants::JULIAN_DAY );

        // Check if leap second is properly handled
        BOOST_CHECK_SMALL( std::fabs( differenceMinusOneDay - differenceMinusOneMilliSecond ), 0.005 );
        BOOST_CHECK_SMALL( std::fabs( differencePlusOneDay - differenceAtEpochPlusOneMilliSecond ), 0.005 );

        BOOST_CHECK_SMALL( std::fabs( differenceMinusOneMilliSecond - differenceAtEpochPlusOneMilliSecond + 1.0 ), 1.0E-10 );
        BOOST_CHECK_SMALL( std::fabs( differenceMinusOneDay - differencePlusOneDay + 1.0 ), 0.01 );

    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


// ----------------------------------------------------------------
// Gunrock -- Fast and Efficient GPU Graph Library
// ----------------------------------------------------------------
// This source code is distributed under the terms of LICENSE.TXT
// in the root directory of this source distribution.
// ----------------------------------------------------------------

/**
 * @file
 * cpu_graph_lib.cpp
 *
 * @brief library implementation of the CPU versions of the algorithms
 */

#include <stdio.h>

#if defined(_WIN32) || defined(_WIN64)
    #include <windows.h>
#else
    #include <sys/resource.h>
#endif

#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/bc_clustering.hpp>
#include <boost/graph/iteration_macros.hpp>

#include "cpu_graph_lib.hpp"

/******************************************************************************
 * Timing
 ******************************************************************************/
namespace timer {
struct CpuTimer
{
#if defined(_WIN32) || defined(_WIN64)

    LARGE_INTEGER ll_freq;
    LARGE_INTEGER ll_start;
    LARGE_INTEGER ll_stop;

    CpuTimer()
    {
        QueryPerformanceFrequency(&ll_freq);
    }

    void Start()
    {
        QueryPerformanceCounter(&ll_start);
    }

    void Stop()
    {
        QueryPerformanceCounter(&ll_stop);
    }

    float ElapsedMillis()
    {
        double start = double(ll_start.QuadPart) / double(ll_freq.QuadPart);
        double stop  = double(ll_stop.QuadPart) / double(ll_freq.QuadPart);

        return static_cast<float>((stop - start) * 1000);
    }

#else

    rusage start;
    rusage stop;

    void Start()
    {
        getrusage(RUSAGE_SELF, &start);
    }

    void Stop()
    {
        getrusage(RUSAGE_SELF, &stop);
    }

    float ElapsedMillis()
    {
        float sec = stop.ru_utime.tv_sec - start.ru_utime.tv_sec;
        float usec = stop.ru_utime.tv_usec - start.ru_utime.tv_usec;

        return (sec * 1000) + (usec / 1000);
    }

#endif
};
}

// Graph edge properties (bundled properties)
struct EdgeProperties
{
    int weight;
};

template<
    typename VertexId,
    typename Value,
    typename SizeT>
void RefCPUBC(
    SizeT                                   *row_offsets,
    VertexId                                *column_indices,
    Value                                   *bc_values,
    SizeT                                   num_nodes,
    VertexId                                src)
{
    // Perform full exact BC using BGL

    using namespace boost;
    typedef adjacency_list <setS, vecS, undirectedS, no_property,
                            EdgeProperties> Graph;
    typedef Graph::vertex_descriptor Vertex;
    typedef Graph::edge_descriptor Edge;

    Graph G;
    for (int i = 0; i < num_nodes; ++i)
    {
        for (int j = row_offsets[i]; j < row_offsets[i+1]; ++j)
        {
            add_edge(vertex(i, G), vertex(column_indices[j], G), G);
        }
    }

    typedef std::map<Edge, int> StdEdgeIndexMap;
    StdEdgeIndexMap my_e_index;
    typedef boost::associative_property_map< StdEdgeIndexMap > EdgeIndexMap;
    EdgeIndexMap e_index(my_e_index);

    // Define EdgeCentralityMap
    std::vector< double > e_centrality_vec(boost::num_edges(G), 0.0);
    // Create the external property map
    boost::iterator_property_map< std::vector< double >::iterator, EdgeIndexMap >
        e_centrality_map(e_centrality_vec.begin(), e_index);

    // Define VertexCentralityMap
    typedef boost::property_map< Graph, boost::vertex_index_t>::type VertexIndexMap;
    VertexIndexMap v_index = get(boost::vertex_index, G);
    std::vector< double > v_centrality_vec(boost::num_vertices(G), 0.0);

    // Create the external property map
    boost::iterator_property_map< std::vector< double >::iterator, VertexIndexMap>
        v_centrality_map(v_centrality_vec.begin(), v_index);

    //
    //Perform BC
    //
    timer::CpuTimer cpu_timer;
    cpu_timer.Start();
    brandes_betweenness_centrality( G, v_centrality_map, e_centrality_map );
    cpu_timer.Stop();
    float elapsed = cpu_timer.ElapsedMillis();

    BGL_FORALL_VERTICES(vertex, G, Graph)
    {
        bc_values[vertex] = (Value)v_centrality_map[vertex];
    }

    printf("CPU BC finished in %lf msec.", elapsed);

}

template<typename VertexId, typename SizeT>
unsigned int RefCPUCC(SizeT *row_offsets, VertexId *column_indices,
                      int num_nodes, int *labels)
{
    using namespace boost;
    typedef adjacency_list <vecS, vecS, undirectedS> Graph;
    Graph G;
    for (int i = 0; i < num_nodes; ++i)
    {
        for (int j = row_offsets[i]; j < row_offsets[i+1]; ++j)
        {
            add_edge(i, column_indices[j], G);
        }
    }
    timer::CpuTimer cpu_timer;
    cpu_timer.Start();
    int num_components = connected_components(G, &labels[0]);
    cpu_timer.Stop();
    float elapsed = cpu_timer.ElapsedMillis();
    printf("CPU CC finished in %lf msec.\n", elapsed);
    return num_components;
}

template void RefCPUBC<int, float, int>(
    int                                     *row_offsets,
    int                                     *column_indices,
    float                                   *bc_values,
    int                                      num_nodes,
    int                                      src);

template unsigned int RefCPUCC<int, int>(
	int *row_offsets,
	int *column_indices,
    int num_nodes,
	int *labels);


/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2015 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_WELL_KNOWN_SRS_HPP
#define MAPNIK_WELL_KNOWN_SRS_HPP

// mapnik
#include <mapnik/global.hpp> // for M_PI on windows
#include <mapnik/enumeration.hpp>
#include <mapnik/geometry.hpp>

#pragma GCC diagnostic push
#include <mapnik/warning_ignore.hpp>
#include <boost/optional.hpp>
#pragma GCC diagnostic pop

// stl
#include <cmath>

namespace mapnik {

enum well_known_srs_enum : std::uint8_t {
    WGS_84,
    G_MERC,
    well_known_srs_enum_MAX
};

DEFINE_ENUM( well_known_srs_e, well_known_srs_enum );

static const double EARTH_RADIUS = 6378137.0;
static const double EARTH_DIAMETER = EARTH_RADIUS * 2.0;
static const double EARTH_CIRCUMFERENCE = EARTH_DIAMETER * M_PI;
static const double MAXEXTENT = EARTH_CIRCUMFERENCE / 2.0;
static const double M_PI_by2 = M_PI / 2;
static const double D2R = M_PI / 180;
static const double R2D = 180 / M_PI;
static const double M_PIby360 = M_PI / 360;
static const double MAXEXTENTby180 = MAXEXTENT / 180;
static const double MAX_LATITUDE = R2D * (2 * std::atan(std::exp(180 * D2R)) - M_PI_by2);
static const std::string MAPNIK_LONGLAT_PROJ = "+proj=longlat +ellps=WGS84 +datum=WGS84 +no_defs";
static const std::string MAPNIK_GMERC_PROJ = "+proj=merc +a=6378137 +b=6378137 +lat_ts=0.0 +lon_0=0.0 +x_0=0.0 +y_0=0.0 +k=1.0 +units=m +nadgrids=@null +wktext +no_defs +over";

boost::optional<well_known_srs_e> is_well_known_srs(std::string const& srs);

boost::optional<bool> is_known_geographic(std::string const& srs);

static inline bool lonlat2merc(double * x, double * y , int point_count)
{
    for(int i=0; i<point_count; ++i)
    {
        if (x[i] > 180) x[i] = 180;
        else if (x[i] < -180) x[i] = -180;
        if (y[i] > MAX_LATITUDE) y[i] = MAX_LATITUDE;
        else if (y[i] < -MAX_LATITUDE) y[i] = -MAX_LATITUDE;
        x[i] = x[i] * MAXEXTENTby180;
        y[i] = std::log(std::tan((90 + y[i]) * M_PIby360)) * R2D;
        y[i] = y[i] * MAXEXTENTby180;
    }
    return true;
}

static inline bool merc2lonlat(double * x, double * y , int point_count)
{
    for(int i=0; i<point_count; i++)
    {
        if (x[i] > MAXEXTENT) x[i] = MAXEXTENT;
        else if (x[i] < -MAXEXTENT) x[i] = -MAXEXTENT;
        if (y[i] > MAXEXTENT) y[i] = MAXEXTENT;
        else if (y[i] < -MAXEXTENT) y[i] = -MAXEXTENT;
        x[i] = (x[i] / MAXEXTENT) * 180;
        y[i] = (y[i] / MAXEXTENT) * 180;
        y[i] = R2D * (2 * std::atan(std::exp(y[i] * D2R)) - M_PI_by2);
    }
    return true;
}

static inline bool lonlat2merc(geometry::line_string<double> & ls)
{
    for(auto & p : ls)
    {
        if (p.x > 180) p.x = 180;
        else if (p.x < -180) p.x = -180;
        if (p.y > MAX_LATITUDE) p.y = MAX_LATITUDE;
        else if (p.y < -MAX_LATITUDE) p.y = -MAX_LATITUDE;
        p.x = p.x * MAXEXTENTby180;
        p.y = std::log(std::tan((90 + p.y) * M_PIby360)) * R2D;
        p.y = p.y * MAXEXTENTby180;
    }
    return true;
}

static inline bool merc2lonlat(geometry::line_string<double> & ls)
{
    for (auto & p : ls)
    {
        if (p.x > MAXEXTENT) p.x = MAXEXTENT;
        else if (p.x < -MAXEXTENT) p.x = -MAXEXTENT;
        if (p.y > MAXEXTENT) p.y = MAXEXTENT;
        else if (p.y < -MAXEXTENT) p.y = -MAXEXTENT;
        p.x = (p.x / MAXEXTENT) * 180;
        p.y = (p.y / MAXEXTENT) * 180;
        p.y = R2D * (2 * std::atan(std::exp(p.y * D2R)) - M_PI_by2);
    }
    return true;
}

}

#endif // MAPNIK_WELL_KNOWN_SRS_HPP


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHM_REMOVE_HOLES_IF_HPP
#define BOOST_GEOMETRY_ALGORITHM_REMOVE_HOLES_IF_HPP

#include <algorithm>

#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/perimeter.hpp>

#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/multi/core/closure.hpp>
#include <boost/geometry/multi/core/tags.hpp>
#include <boost/geometry/multi/algorithms/detail/modify_with_predicate.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace remove_holes_if
{

template<typename Polygon, typename Predicate>
struct polygon_remove_holes_if
{
    static inline void apply(Polygon& poly, Predicate const& predicate)
    {
        // TODO: evaluate this behaviour w.r.t. writable concepts
        typename interior_return_type<Polygon>::type rings = interior_rings(poly);

        // Remove rings using erase-remove-idiom
        // http://en.wikipedia.org/wiki/Erase-remove_idiom
        rings.erase(
            std::remove_if(boost::begin(rings), boost::end(rings), predicate),
            boost::end(rings));
    }
};

}} // namespace detail::remove_holes_if

#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// Default implementation does nothing
template <typename Tag, typename Geometry, typename Predicate>
struct remove_holes_if
{};

template <typename Geometry, typename Predicate>
struct remove_holes_if<polygon_tag, Geometry, Predicate>
    : detail::remove_holes_if::polygon_remove_holes_if<Geometry, Predicate>
{};

template <typename MultiPolygon, typename Predicate>
struct remove_holes_if<multi_polygon_tag, MultiPolygon, Predicate>
    : detail::multi_modify_with_predicate
        <
            MultiPolygon,
            Predicate,
            detail::remove_holes_if::polygon_remove_holes_if
                <
                    typename boost::range_value<MultiPolygon>::type, Predicate
                >
        >
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
    \brief Remove holes from a geometry (polygon, multi-polygon) using a specified condition
 */
template <typename Geometry, typename Predicate>
inline void remove_holes_if(Geometry& geometry, Predicate const& predicate)
{
    concept::check<Geometry>();

    dispatch::remove_holes_if
        <
            typename tag<Geometry>::type,
            Geometry,
            Predicate
        >::apply(geometry, predicate);
}

// CONVENIENT PREDICATES might be moved elsewhere
template <typename Ring>
struct elongated_hole
{
    inline elongated_hole(double ratio)
        : m_ratio(ratio)
    {}

    inline bool operator()(Ring const& ring) const
    {
        if (ring.size() >=
                core_detail::closure::minimum_ring_size
                    <
                       geometry::closure<Ring>::value
                    >::value)
        {
            double a = area(ring);
            double p = perimeter(ring);
            return geometry::math::abs(a / p) < m_ratio;
        }
        // Rings with less then 4 points (including closing)
        // are also considered as small and thus removed
        return true;
    }
private :
    double m_ratio;
};

template <typename Ring>
struct invalid_hole
{
    inline bool operator()(Ring const& ring) const
    {
        return ring.size()
                < core_detail::closure::minimum_ring_size
                        <
                            geometry::closure<Ring>::value
                        >::value;
    }
};

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHM_REMOVE_HOLES_IF_HPP