// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#include <boost/numeric/odeint.hpp>

#include <cbr_math/lie/group_product.hpp>
#include <cbr_math/lie/Tn.hpp>

#include <cbr_control/asif++.hpp>

#include <matplot/matplot.h>

#include <algorithm>
#include <vector>

struct Dynamics
{
  template<typename T>
  using State = cbr::lie::T2<T>;

  template<typename T>
  using Input = Eigen::Matrix<T, 1, 1>;

  template<typename T>
  Eigen::Matrix<T, 2, 1> f(const State<T> & x) const
  {
    return Eigen::Matrix<T, 2, 1>(x.translation()(1), 0);
  }

  template<typename T>
  Eigen::Matrix<T, 2, 1> g(const State<T> & x) const
  {
    return Eigen::Matrix<T, 2, 1>(0, 1);
  }
};

struct SS
{
  template<typename T>
  Eigen::Matrix<T, 2, 1> operator()(const cbr::lie::T2<T> & x) const
  {
    return Eigen::Matrix<T, 2, 1>(
      T(2) - x.translation().x(),
      T(2) - x.translation().y()
    );
  }
};

struct Backup
{
  template<typename T>
  Eigen::Matrix<T, 1, 1> operator()(const Dynamics::State<T> & x) const
  {
    return Eigen::Matrix<T, 1, 1>(-0.6);
  }
};

int main()
{
  cbr::ASIFParams params;
  params.dt = 0.02;
  params.steps = 200;
  params.constr_dist = 10;
  params.relax_cost = 500;
  params.debug = false;

  Dynamics dyn;
  Backup bu;
  SS ss;

  cbr::ASIF asif(dyn, bu, ss, params);
  asif.setBounds(Eigen::Matrix<double, 1, 1>(-0.5), Eigen::Matrix<double, 1, 1>(1.5));

  std::vector<double> sol_t;
  std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> sol_x;
  std::vector<double> sol_u, sol_udes;

  Eigen::Vector2d x(-5, 1);
  double udes = 1;

  double t = 0;
  double dt = 0.01;

  boost::numeric::odeint::runge_kutta4<Eigen::Vector2d, double, Eigen::Vector2d, double,
    boost::numeric::odeint::vector_space_algebra> stepper {};

  for (int i = 0; i != static_cast<int>(10. / dt); ++i) {
    Eigen::Matrix<double, 1, 1> u(udes);
    asif.filter(cbr::lie::T2d(x), u);

    sol_t.push_back(t);
    sol_x.push_back(x);
    sol_udes.push_back(udes);
    sol_u.push_back(u(0));

    stepper.do_step(
      [&](const Eigen::Vector2d & x, Eigen::Vector2d & dx, double t) {dx(0) = x(1); dx(1) = u(0);},
      x, t, dt
    );
    t += dt;
  }

  // helper function to extract stuff from solutions
  auto ex_fn = [](const auto & item, auto ex_fn) {
      std::vector<double> ret;
      std::transform(item.cbegin(), item.cend(), std::back_inserter(ret), ex_fn);
      return ret;
    };

  matplot::figure();
  matplot::hold(matplot::on);
  matplot::plot(sol_t, ex_fn(sol_x, [](auto s) {return s(0);}))->line_width(2);
  matplot::plot(sol_t, ex_fn(sol_x, [](auto s) {return s(1);}))->line_width(2);
  matplot::title("states");
  matplot::legend({"x", "v"});
  matplot::figure();
  matplot::hold(matplot::on);
  matplot::plot(sol_t, sol_udes)->line_width(2);
  matplot::plot(sol_t, sol_u)->line_width(2);
  matplot::title("input");
  matplot::legend({"u_{des}", "u"});

  matplot::show();

  return EXIT_SUCCESS;
}


///////////////////////////////////////////////////////////////////////////////
// ars::example::search_reflection.cpp                                       //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under 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)         //
///////////////////////////////////////////////////////////////////////////////
#include <boost/format.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/normal/include.hpp>
#include <boost/ars/search_reflection.hpp>
#include <boost/ars/constant.hpp>

void example_search_reflection(std::ostream& out){
    std::cout << "-> example_search_reflection" << std::endl;
    using namespace boost;
    namespace st = boost::statistics::detail;

    typedef double value_t;
    typedef st::ars::point<value_t> point_t;
    typedef st::ars::constant<value_t> const_;

    struct local{
        static std::ostream& write(
            std::ostream& o,
            const value_t& x_0,
            const value_t& x_1,
            unsigned n,
            const point_t& p_0,
            const point_t& p_1
        ){
            boost::format f("x_0 = %1%, x_1 = %2%, ");
            f % x_0 % x_1;
            o << f.str();
            o << ", n = " << n << ", p_0 : " << p_0 << ", p_1 : " << p_1;
            return o;
        }
    };

    value_t x_min,x_max, x_0, x_1;
    point_t p_0,p_1;
    unsigned n_max, n = 0;

    {
        typedef math::normal_distribution<value_t> mdist_t;
        typedef const mdist_t& param_t;
        x_min = const_::inf_;
        x_max = const_::inf_;
        n_max = 1e2;
        const value_t mu = 0.0;
        const value_t sigma = 2.0;
        mdist_t mdist(mu,sigma);
        out << (boost::format("N(%1%,%2%)")%mu%sigma);

        {
            x_0 = 100.0;
            x_1 = 100.01;
            n = st::ars::search_reflection_dist(
                x_min,
                x_max,
                mdist,
                x_0,
                x_1,
                p_0,
                p_1,
                n_max
            );
            local::write(out,x_0,x_1,n,p_0,p_1);
            out << std::endl;
        }

        {
            x_0 = -100.01;
            x_1 = -100.00;
            n = st::ars::search_reflection_dist(
                x_min,
                x_max,
                mdist,
                x_0,
                x_1,
                p_0,
                p_1,
                n_max
            );
            local::write(out,x_0,x_1,n,p_0,p_1);
            out << std::endl;
        }

        {
            x_0 = -0.02;
            x_1 = -0.01;
            n = st::ars::search_reflection_dist(
                x_min,
                x_max,
                mdist,
                x_0,
                x_1,
                p_0,
                p_1,
                n_max
            );
            local::write(out,x_0,x_1,n,p_0,p_1);
            out << std::endl;
        }

    }
    out << "<-" << std::endl;
}


#include <gmock/gmock.h>

#include <Eigen/Dense>
#include <array>
#include <vector>

#include "larq_compute_engine/core/fused_bgemm_functor.h"
#include "larq_compute_engine/core/macros.h"

namespace compute_engine {
namespace testing {

namespace ce = compute_engine;
using ce::core::Layout;

template <typename TBitpacked, typename T>
void fused_test_rowmajor() {
  // clang-format off
  const int a_num_rows = 2;
  const int a_num_cols = 9;
  std::vector<T> a_data_float{1,  1, -1,  1, -1, -1, -1,  1,  1,
                              1, -1,  1, -1, -1, -1,  1,  1, -1};
  //const int b_num_rows = 9;
  const int b_num_cols = 2;
  std::vector<T> b_data_float{ -1,  1,
                                1, -1,
                               -1, -1,
                                1, -1,
                                1, -1,
                                1,  1,
                               -1,  1,
                                1,  1,
                                1, -1};
  // clang-format on

  // Both matrices above are stored row major so they
  // are as they are shaped just as they appear in the code.
  //
  //  3 = -1 + 1 + 1 + 1 - 1 - 1 + 1 + 1 + 1
  // -1 =  1 - 1 + 1 - 1 + 1 - 1 - 1 + 1 - 1
  // -7 = -1 - 1 - 1 - 1 - 1 - 1 - 1 + 1 - 1
  //  5 =  1 + 1 - 1 + 1 + 1 - 1 + 1 + 1 + 1
  std::vector<T> c_expected{3, -1, -7, 5};

  const int m = a_num_rows;
  const int k = a_num_cols;
  const int n = b_num_cols;
  const int lda = k;
  const int ldb = n;
  const int ldc = n;

  using TBGemmFunctor =
      ce::core::ReferenceBGemmFunctor<TBitpacked, Layout::RowMajor, TBitpacked,
                                      Layout::RowMajor, T>;
  using TFusedBGemmFunctor =
      ce::core::FusedBGemmFunctor<T, Layout::RowMajor, T, Layout::RowMajor, T,
                                  TBitpacked, TBGemmFunctor>;

  const int c_size = m * n;
  std::vector<T> c(c_size);
  TFusedBGemmFunctor fused_bgemm_functor;
  fused_bgemm_functor(m, n, k, a_data_float.data(), lda, b_data_float.data(),
                      ldb, c.data(), ldc);

  EXPECT_THAT(c, ::testing::ElementsAreArray(c_expected));
}

template <typename TBitpacked, typename T>
void fused_test_colmajor() {
  // clang-format off
  const int a_num_rows = 2;
  const int a_num_cols = 9;
  std::vector<T> a_data_float{1,  1, -1,  1, -1, -1, -1,  1,  1,
                              1, -1,  1, -1, -1, -1,  1,  1, -1};
  const int b_num_rows = 9;
  const int b_num_cols = 2;
  std::vector<T> b_data_float{-1,  1, -1,  1,  1,  1, -1, 1,  1,
                               1, -1, -1, -1, -1,  1,  1, 1, -1};
  // clang-format on

  // The `b` matrix is the transposed version of the one in the row major
  // test so the output should be the same.

  //  3 = -1 + 1 + 1 + 1 - 1 - 1 + 1 + 1 + 1
  // -1 =  1 - 1 + 1 - 1 + 1 - 1 - 1 + 1 - 1
  // -7 = -1 - 1 - 1 - 1 - 1 - 1 - 1 + 1 - 1
  //  5 =  1 + 1 - 1 + 1 + 1 - 1 + 1 + 1 + 1
  std::vector<T> c_expected{3, -1, -7, 5};

  const int m = a_num_rows;
  const int k = a_num_cols;
  const int n = b_num_cols;
  const int lda = k;
  const int ldb = b_num_rows;
  const int ldc = n;

  using TBGemmFunctor =
      ce::core::ReferenceBGemmFunctor<TBitpacked, Layout::RowMajor, TBitpacked,
                                      Layout::ColMajor, T>;
  using TFusedBGemmFunctor =
      ce::core::FusedBGemmFunctor<T, Layout::RowMajor, T, Layout::ColMajor, T,
                                  TBitpacked, TBGemmFunctor>;

  const int c_size = m * n;
  std::vector<T> c(c_size);
  TFusedBGemmFunctor fused_bgemm_functor;
  fused_bgemm_functor(m, n, k, a_data_float.data(), lda, b_data_float.data(),
                      ldb, c.data(), ldc);

  EXPECT_THAT(c, ::testing::ElementsAreArray(c_expected));
}

template <typename TBitpacked, typename T>
void fused_test_eigen(int m, int n, int k) {
  using MatRowMajor =
      Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
  using MatColMajor =
      Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;

  const int a_num_rows = m;
  const int a_num_cols = k;
  const int b_num_rows = k;
  const int b_num_cols = n;

  MatRowMajor a(a_num_rows, a_num_cols);
  MatColMajor b(b_num_rows, b_num_cols);

  // Fill with random values
  for (int i = 0; i < a_num_rows; ++i) {
    for (int j = 0; j < a_num_cols; ++j) {
      a(i, j) = (rand() % 2) ? 1 : -1;
    }
  }
  for (int i = 0; i < b_num_rows; ++i) {
    for (int j = 0; j < b_num_cols; ++j) {
      b(i, j) = (rand() % 2) ? 1 : -1;
    }
  }

  MatRowMajor c_correct = a * b;
  const T* c_expected = c_correct.data();

  const int lda = k;
  const int ldb = b_num_rows;
  const int ldc = n;

  using TBGemmFunctor =
      ce::core::ReferenceBGemmFunctor<TBitpacked, Layout::RowMajor, TBitpacked,
                                      Layout::ColMajor, T>;
  using TFusedBGemmFunctor =
      ce::core::FusedBGemmFunctor<T, Layout::RowMajor, T, Layout::ColMajor, T,
                                  TBitpacked, TBGemmFunctor>;

  const int c_size = m * n;
  std::vector<T> c(c_size);
  TFusedBGemmFunctor fused_bgemm_functor;
  fused_bgemm_functor(m, n, k, a.data(), lda, b.data(), ldb, c.data(), ldc);

  EXPECT_THAT(c, ::testing::ElementsAreArray(c_expected, (size_t)c_size));
}

TEST(BitpackingBGEMMTests, RowMajorWithBitPadding8) {
  fused_test_rowmajor<uint8_t, float>();
}

TEST(BitpackingBGEMMTests, RowMajorWithBitPadding64) {
  fused_test_rowmajor<uint64_t, float>();
}

TEST(BitpackingBGEMMTests, ColMajorWithBitPadding8) {
  fused_test_colmajor<uint8_t, float>();
}

TEST(BitpackingBGEMMTests, ColMajorWithBitPadding64) {
  fused_test_colmajor<uint64_t, float>();
}

TEST(BitpackingBGEMMTests, ColMajorEigen) {
  fused_test_eigen<uint64_t, float>(5, 7, 11);
}

}  // end namespace testing
}  // end namespace compute_engine


// ===============================================================================================================
// Copyright (c) 2019, Cornell University. All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are permitted provided that
// the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright otice, this list of conditions and
//       the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
//       the following disclaimer in the documentation and/or other materials provided with the distribution.
//
//     * Neither the name of Cornell University nor the names of its contributors may be used to endorse or
//       promote products derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
// OF SUCH DAMAGE.
//
// Author: Kai Zhang (kz298@cornell.edu)
//
// The research is based upon work supported by the Office of the Director of National Intelligence (ODNI),
// Intelligence Advanced Research Projects Activity (IARPA), via DOI/IBC Contract Number D17PC00287.
// The U.S. Government is authorized to reproduce and distribute copies of this work for Governmental purposes.
// ===============================================================================================================

#include "ceres/ceres.h"
#include "glog/logging.h"
#include <Eigen/Dense>

#include <iostream>
#include <vector>
#include <sstream>
#include <fstream>
#include <iomanip>
#include <limits>

using Eigen::MatrixXd;

using ceres::AutoDiffCostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;

using std::cout;
using std::vector;
using std::string;
using std::istringstream;
using std::ifstream;
using std::ofstream;
using std::stod;
using std::runtime_error;
using std::setprecision;

typedef std::numeric_limits<double> dbl;
#define INIT_VAL -1e10

struct RPCCamera {
    RPCCamera() {}

    // RPC camera parameters
    double col_numera[20] = {INIT_VAL};
    double col_denomi[20] = {INIT_VAL};
    double row_numera[20] = {INIT_VAL};
    double row_denomi[20] = {INIT_VAL};
    double lat_off = INIT_VAL, lat_scale = INIT_VAL;
    double lon_off = INIT_VAL, lon_scale = INIT_VAL;
    double alt_off = INIT_VAL, alt_scale = INIT_VAL;
    double row_off = INIT_VAL, row_scale = INIT_VAL;
    double col_off = INIT_VAL, col_scale = INIT_VAL;

    // affine approximation of the RPC camera
    // [col, row]^T = M * [lat, lon, alt]^T
    double M11;
    double M12;
    double M13;
    double M14;

    double M21;
    double M22;
    double M23;
    double M24;
};

struct Observation {
    Observation(RPCCamera* cam, double col, double row): cam(cam), col(col), row(row) {}

    RPCCamera* cam = NULL;
    double col = INIT_VAL;
    double row = INIT_VAL;
};

struct ReprojResidual {
    ReprojResidual(Observation* pixel): pixel(pixel) {}

    template <typename T>
    bool operator() (const T* const lat, const T* const lon, const T* const alt,
                     T* residuals) const {
        RPCCamera& cam = *(this->pixel->cam);

        T lat_normed = (lat[0] - T(cam.lat_off)) / T(cam.lat_scale);
        T lon_normed = (lon[0] - T(cam.lon_off)) / T(cam.lon_scale);
        T alt_normed = (alt[0] - T(cam.alt_off)) / T(cam.alt_scale);

        T row_numera = this->apply_poly(cam.row_numera, lat_normed, lon_normed, alt_normed);
        T row_denomi = this->apply_poly(cam.row_denomi, lat_normed, lon_normed, alt_normed);

        T predict_row = row_numera / row_denomi * T(cam.row_scale) + T(cam.row_off);

        T col_numera = this->apply_poly(cam.col_numera, lat_normed, lon_normed, alt_normed);
        T col_denomi = this->apply_poly(cam.col_denomi, lat_normed, lon_normed, alt_normed);

        T predict_col = col_numera / col_denomi * T(cam.col_scale) + T(cam.col_off);

        residuals[0] = predict_row - T(this->pixel->row);
        residuals[1] = predict_col - T(this->pixel->col);
        return true;
    }

private:
    template <typename T>
    T apply_poly(const double* const poly, T x, T y, T z) const {
        T out = T(poly[0]);
        out += poly[1]*y + poly[2]*x + poly[3]*z;
        out += poly[4]*y*x + poly[5]*y*z +poly[6]*x*z;
        out += poly[7]*y*y + poly[8]*x*x + poly[9]*z*z;
        out += poly[10]*x*y*z;
        out += poly[11]*y*y*y;
        out += poly[12]*y*x*x + poly[13]*y*z*z + poly[14]*y*y*x;
        out += poly[15]*x*x*x;
        out += poly[16]*x*z*z + poly[17]*y*y*z + poly[18]*x*x*z;
        out += poly[19]*z*z*z;

        return out;
    }

private:
    Observation* pixel = NULL;
};

void solve_initial(const vector<Observation*>& pixels, vector<double>& initial) {
    assert (pixels.size() >= 2 && initial.size() == 3);

    MatrixXd A(2 * pixels.size(), 3);  // one observation contributes to two equations
    MatrixXd b(2 * pixels.size(), 1);
    for (int i=0; i < pixels.size(); ++i) {
        A(i * 2, 0) = pixels[i]->cam->M11;
        A(i * 2, 1) = pixels[i]->cam->M12;
        A(i * 2, 2) = pixels[i]->cam->M13;
        b(i * 2, 0) = pixels[i]->col - pixels[i]->cam->M14;

        A(i * 2 + 1, 0) = pixels[i]->cam->M21;
        A(i * 2 + 1, 1) = pixels[i]->cam->M22;
        A(i * 2 + 1, 2) = pixels[i]->cam->M23;
        b(i * 2 + 1, 0) = pixels[i]->row - pixels[i]->cam->M24;
    }

    MatrixXd x = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    for (int i=0; i < 3; ++i) {
        initial[i] = x(i, 0);
    }
}

void refine_initial(const vector<Observation*>& pixels, const vector<double>& initial, vector <double>& final, vector <double>& reproj_error) {
    assert (initial.size() == 3 && final.size() == 3 && reproj_error.size() == 2);

    double lat = initial[0];
    double lon = initial[1];
    double alt = initial[2];

    Problem problem;
    for (int i=0; i < pixels.size(); ++i) {
        problem.AddResidualBlock(
                                 new AutoDiffCostFunction<ReprojResidual, 2, 1, 1, 1>(new ReprojResidual(pixels[i])),
                                 NULL, &lat, &lon, &alt);
    }

    Solver::Options options;
    options.max_num_iterations = 100;
    // options.function_tolerance = 1e-10;
    options.linear_solver_type = ceres::DENSE_QR;
    // set the following options to true for debugging
    options.minimizer_progress_to_stdout = false;

    Solver::Summary summary;
    Solve(options, &problem, &summary);

    double init_error = sqrt(summary.initial_cost * 2 / pixels.size());
    double final_error = sqrt(summary.final_cost * 2 / pixels.size());

    // for debugging
    // cout << summary.BriefReport() << "\n";
    // cout << "\ninitial Point: (" << initial[0] << "," << initial[1] << "," << initial[2] << "), reproj_error: " << init_error << " pixels\n";
    // cout << "final Point:  (" << lat << "," << lon << "," << alt << "), reproj_error: " << final_error << " pixels\n";

    // output
    final[0] = lat;
    final[1] = lon;
    final[2] = alt;
    reproj_error[0] = init_error;
    reproj_error[1] = final_error;
}

void read_rpc_cameras(const string& fname, vector<RPCCamera*>& rpc_cameras) {
    // number of rpc cameras
    // camera_id
    // 20 column numerator coefficients
    // 20 column denominator coefficients
    // 20 row numerator coefficients
    // 20 row denominator coefficients
    // 10 normalization constants: lat off, lat scale, lon off, lon scale, alt off, alt scale, col off, col scale
    // 8 affine approximation coefficients: M11, M12, M13, M14, M21, M22, M23, M24
    // ...

    ifstream infile;
    infile.open(fname);
    if (!infile) {
        throw runtime_error("unable to open " + fname);
    }

    int cnt;
    infile >> cnt;
    for (int i=0; i<cnt; ++i) {
        int cam_id;
        // read camera id
        infile >> cam_id;
        assert(cam_id == i);

        rpc_cameras.push_back(new RPCCamera());
        RPCCamera *cam = rpc_cameras.back();

        // read rpc camera parameters
        for (int i = 0; i < 20; ++i) {
            infile >> cam->col_numera[i];
        }
        for (int i = 0; i < 20; ++i) {
            infile >> cam->col_denomi[i];
        }
        for (int i = 0; i < 20; ++i) {
            infile >> cam->row_numera[i];
        }
        for (int i = 0; i < 20; ++i) {
            infile >> cam->row_denomi[i];
        }
        infile >> cam->lat_off >> cam->lat_scale;
        infile >> cam->lon_off >> cam->lon_scale;
        infile >> cam->alt_off >> cam->alt_scale;
        infile >> cam->col_off >> cam->col_scale;
        infile >> cam->row_off >> cam->row_scale;

        // read affine approximation parameters
        infile >> cam->M11 >> cam->M12 >> cam->M13 >> cam->M14;
        infile >> cam->M21 >> cam->M22 >> cam->M23 >> cam->M24;
    }

    infile.close();
}

void triangulate_tracks(const string& cameras_fname, const string& tracks_fname, const string& results_fname) {
    vector<RPCCamera*> rpc_cameras;
    read_rpc_cameras(cameras_fname, rpc_cameras);

    ifstream infile;
    infile.open(tracks_fname);
    if (!infile) {
        throw runtime_error("unable to open " + tracks_fname);
    }

    ofstream outfile;
    outfile.open(results_fname);
    if (!outfile) {
        throw runtime_error("unable to open " + results_fname);
    }
    outfile << setprecision(dbl::max_digits10);

    int cnt;
    infile >> cnt;
    outfile << cnt << '\n';
    for (int i=0; i<cnt; ++i) {
        // read feature track length
        int len;
        infile >> len;
        // read all the observations for this track
        vector<Observation*> pixels;
        for (int j=0; j < len; ++j) {
            int cam_id;
            double col, row;
            infile >> cam_id >> col >> row;

            assert(cam_id < rpc_cameras.size());
            pixels.push_back(new Observation(rpc_cameras[cam_id], col, row));
        }

        // solve for (lat, lon, alt)
        vector<double> initial(3, INIT_VAL);
        vector<double> final(3, INIT_VAL);
        vector<double> reproj_error(2, INIT_VAL);
        solve_initial(pixels, initial);
        refine_initial(pixels, initial, final, reproj_error);

        // write results to file
        // each line is "intial lat, initial lon, initial alt, initial reproj err, final lat, final lon, final alt, final reproj err"
        outfile << initial[0] << " " << initial[1] << " " << initial[2] << " " << reproj_error[0] << " ";
        outfile << final[0] << " " << final[1] << " " << final[2] << " " << reproj_error[1] << "\n";

        // free memory
        for (int i = 0; i < pixels.size(); ++i) {
            delete pixels[i];
        }
    }

    // close file
    infile.close();
    outfile.close();

    // free memory
    for (int i = 0; i < rpc_cameras.size(); ++i) {
        delete rpc_cameras[i];
    }
}

int main(int argc, char** argv) {
    // program name, cameras_fname, tracks_fname, results_fname
    assert(argc == 4);
    google::InitGoogleLogging(argv[0]);
    string cameras_fname = string(argv[1]);
    string tracks_fname = string(argv[2]);
    string results_fname = string(argv[3]);

    triangulate_tracks(cameras_fname, tracks_fname, results_fname);
    return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_HYPERBOLIC_FUNCTIONS_SIMD_COMMON_SINHC_HPP_INCLUDED
#define NT2_HYPERBOLIC_FUNCTIONS_SIMD_COMMON_SINHC_HPP_INCLUDED
#include <nt2/hyperbolic/functions/sinhc.hpp>
#include <nt2/hyperbolic/functions/details/sinhc_kernel.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/log_2.hpp>
#include <nt2/include/constants/maxlog.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/average.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/exp.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/inbtrue.hpp>
#include <nt2/include/functions/simd/is_greater.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/rec.hpp>
#include <nt2/include/functions/simd/sqr.hpp>
#include <nt2/include/functions/simd/unary_minus.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( sinhc_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      //////////////////////////////////////////////////////////////////////////////
      // if x = abs(a0) is less than 1 sinhc is computed using a polynomial(float)
      // respectively rational(double) approx inspired from cephes sinh approx.
      // else according x < Threshold e =  exp(x) or exp(x/2) is respectively
      // computed
      // * in the first case sinh is ((e-rec(e))/2)/x
      // * in the second     sinh is (e/2/x)*e (avoiding undue overflow)
      // Threshold is Maxlog - Log_2
      //////////////////////////////////////////////////////////////////////////////
      typedef typename meta::as_logical<A0>::type bA0;
      result_type x = nt2::abs(a0);
      bA0 lt1= lt(x, One<A0>());
      std::size_t nb = inbtrue(lt1);
      A0 z = Zero<A0>();
      if( nb > 0)
      {
        z = details::sinhc_kernel<A0>::compute(sqr(x));
        if(nb >= meta::cardinal_of<A0>::value) return z;
      }
      bA0 test1 = gt(x, Maxlog<A0>()-Log_2<A0>());
      A0 fac = if_else(test1, Half<A0>(), One<A0>());
      A0 tmp = exp(x*fac);
      A0 tmp1 = (Half<A0>()*tmp)/x;
      A0 r =  if_else(test1, tmp1*tmp, average(tmp, -rec(tmp))/x);
      #ifndef BOOST_SIMD_NO_INFINITIES
      r = if_else(eq(x, Inf<A0>()), x, r);
      #endif
      return if_else(lt1, z, r);
    }
  };
} }
#endif


//           Copyright Matthew Pulver 2018 - 2019.
// Distributed under the Boost Software License, Version 1.0.
//      (See accompanying file LICENSE_1_0.txt or copy at
//           https://www.boost.org/LICENSE_1_0.txt)

// THIS IS NOT MEANT TO BE COMPILED - ONLY FOR INCLUSION IN DOCUMENTATION.

#include <boost/math/differentiation/autodiff.hpp>

namespace boost {
namespace math {
namespace differentiation {

// Function returning a single variable of differentiation. Recommended: Use auto for type.
template <typename RealType, size_t Order, size_t... Orders>
autodiff_fvar<RealType, Order, Orders...> make_fvar(RealType const& ca);

// Function returning multiple independent variables of differentiation in a std::tuple.
template<typename RealType, size_t... Orders, typename... RealTypes>
auto make_ftuple(RealTypes const&... ca);

// Type of combined autodiff types. Recommended: Use auto for return type (C++14).
template <typename RealType, typename... RealTypes>
using promote = typename detail::promote_args_n<RealType, RealTypes...>::type;

namespace detail {

// Single autodiff variable. Use make_fvar() or make_ftuple() to instantiate.
template <typename RealType, size_t Order>
class fvar {
 public:
  // Query return value of function to get the derivatives.
  template <typename... Orders>
  get_type_at<RealType, sizeof...(Orders) - 1> derivative(Orders... orders) const;

  // All of the arithmetic and comparison operators are overloaded.
  template <typename RealType2, size_t Order2>
  fvar& operator+=(fvar<RealType2, Order2> const&);

  fvar& operator+=(root_type const&);

  // ...
};

// Standard math functions are overloaded and called via argument-dependent lookup (ADL).
template <typename RealType, size_t Order>
fvar<RealType, Order> floor(fvar<RealType, Order> const&);

template <typename RealType, size_t Order>
fvar<RealType, Order> exp(fvar<RealType, Order> const&);

// ...

}  // namespace detail

}  // namespace differentiation
}  // namespace math
}  // namespace boost
/**/


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_MATH_VELOCITYSCREW6D_HPP
#define RW_MATH_VELOCITYSCREW6D_HPP

/**
 * @file VelocityScrew6D.hpp
 */
#if !defined(SWIG)
#include "EAA.hpp"
#include "Math.hpp"
#include "Transform3D.hpp"
#include "Vector3D.hpp"

#include <rw/common/Serializable.hpp>

#include <Eigen/Core>
#endif

namespace rw { namespace math {

    /** @addtogroup math */
    /*@{*/

    /**
     * @brief Class for representing 6 degrees of freedom velocity screws.
     *
     * \f[
     * \mathbf{\nu} =
     * \left[
     *  \begin{array}{c}
     *  v_x\\
     *  v_y\\
     *  v_z\\
     *  \omega_x\\
     *  \omega_y\\
     *  \omega_z
     *  \end{array}
     * \right]
     * \f]
     *
     * A VelocityScrew is the description of a frames linear and rotational velocity
     * with respect to some reference frame.
     *
     */
    template< class T = double > class VelocityScrew6D
    {
      private:
        T _screw[6];

      public:
        /**
         * @brief Constructs a 6 degrees of freedom velocity screw
         *
         * @param vx [in] @f$ v_x @f$
         * @param vy [in] @f$ v_y @f$
         * @param vz [in] @f$ v_z @f$
         * @param wx [in] @f$ \omega_x @f$
         * @param wy [in] @f$ \omega_y @f$
         * @param wz [in] @f$ \omega_z @f$
         */
        VelocityScrew6D (T vx, T vy, T vz, T wx, T wy, T wz);

        /**
         * @brief Construct from Eigen vector representation.
         * @param v [in] Eigen matrix with either one row or one column.
         */
        template< class R > VelocityScrew6D (const Eigen::MatrixBase< R >& v)
        {
            if (v.cols () != 1 || v.rows () != 6)
                RW_THROW ("Unable to initialize VectorND with " << v.rows () << " x " << v.cols ()
                                                                << " matrix");
            /* For some reason the following does not WORK AT ALL (JIMMY)
            _screw[0] = v(0,0);
            _screw[1] = v(1,0);
            _screw[2] = v(2,0);
            _screw[3] = v(3,0);
            _screw[4] = v(4,0);
            _screw[5] = v(5,0);
            */ // instead use
            _screw[0] = v.row (0) (0);
            _screw[1] = v.row (1) (0);
            _screw[2] = v.row (2) (0);
            _screw[3] = v.row (3) (0);
            _screw[4] = v.row (4) (0);
            _screw[5] = v.row (5) (0);
        }

        /**
         * @brief Default Constructor. Initialized the velocity to 0
         */
        VelocityScrew6D ()
        {
            _screw[0] = _screw[1] = _screw[2] = _screw[3] = _screw[4] = _screw[5] = 0;
        }

        /**
         * @brief Copy Constructor
         * @param vs [in] the velocityscrew6D to copy
         */
        VelocityScrew6D (const VelocityScrew6D& vs)
        {
            for(size_t i = 0; i < vs.size(); i++){
                this->_screw[i]=vs[i];
            }
        }

        /**
         * @brief Constructs a velocity screw in frame @f$ a @f$ from a
         * transform @f$\robabx{a}{b}{\mathbf{T}} @f$.
         *
         * @param transform [in] the corresponding transform.
         */
        explicit VelocityScrew6D (const rw::math::Transform3D< T >& transform);

        /**
         * @brief Constructs a velocity screw from a linear and angular velocity
         *
         * @param linear [in] linear velocity
         * @param angular [in] angular velocity
         */
        VelocityScrew6D (const Vector3D< T >& linear, const EAA< T >& angular);

        /**
         * @brief Extracts the linear velocity
         *
         * @return the linear velocity
         */
        const Vector3D< T > linear () const
        {
            return Vector3D< T > (_screw[0], _screw[1], _screw[2]);
        }

        /**
         * @brief Extracts the angular velocity and represents it using an
         * equivalent-angle-axis as @f$ \dot{\Theta}\mathbf{k} @f$
         *
         * @return the angular velocity
         */
        const EAA< T > angular () const { return EAA< T > (_screw[3], _screw[4], _screw[5]); }

        /**
         * @brief get the size of the underlying vector
         */
        size_t size() const {return 6;}

#if !defined(SWIG)
        /**
         * @brief Returns reference to velocity screw element
         *
         * @param index [in] index in the screw, index must be @f$ < 6 @f$.
         *
         * @return reference to velocity screw element
         */
        T& operator() (std::size_t index)
        {
            assert (index < 6);
            return _screw[index];
        }

        /**
         * @brief Returns const reference to velocity screw element
         *
         * @param index [in] index in the screw, index must be @f$ < 6 @f$.
         *
         * @return const reference to velocity screw element
         */
        const T& operator() (std::size_t index) const
        {
            assert (index < 6);
            return _screw[index];
        }

        /**
         * @brief Returns const reference to velocity screw element
         *
         * @param i [in] index in the screw, index must be @f$ < 6 @f$.
         *
         * @return const reference to velocity screw element
         */
        const T& operator[] (size_t i) const { return (*this) (i); }

        /**
         * @brief Returns const reference to velocity screw element
         *
         * @param i [in] index in the screw, index must be @f$ < 6 @f$.
         *
         * @return const reference to velocity screw element
         */
        T& operator[] (size_t i) { return (*this) (i); }
#else
        ARRAYOPERATOR (T);
#endif

        /**
         * @brief Adds the velocity screw given as a parameter to the velocity screw.
         *
         * @param screw [in] Velocity screw to add
         *
         * @return reference to the VelocityScrew6D to support additional
         * assignments.
         */
        VelocityScrew6D< T >& operator+= (const VelocityScrew6D< T >& screw)
        {
            _screw[0] += screw (0);
            _screw[1] += screw (1);
            _screw[2] += screw (2);
            _screw[3] += screw (3);
            _screw[4] += screw (4);
            _screw[5] += screw (5);
            return *this;
        }

        /**
         * @brief Subtracts the velocity screw given as a parameter from the
         * velocity screw.
         *
         * @param screw [in] Velocity screw to subtract
         *
         * @return reference to the VelocityScrew6D to support additional
         * assignments.
         */
        VelocityScrew6D< T >& operator-= (const VelocityScrew6D< T >& screw)
        {
            _screw[0] -= screw (0);
            _screw[1] -= screw (1);
            _screw[2] -= screw (2);
            _screw[3] -= screw (3);
            _screw[4] -= screw (4);
            _screw[5] -= screw (5);
            return *this;
        }

        /**
         * @brief Scales velocity screw with s
         *
         * @param s [in] scaling value
         *
         * @return reference to the VelocityScrew6D to support additional
         * assigments
         */
        VelocityScrew6D< T >& operator*= (T s)
        {
            _screw[0] *= s;
            _screw[1] *= s;
            _screw[2] *= s;
            _screw[3] *= s;
            _screw[4] *= s;
            _screw[5] *= s;
            return *this;
        }

        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true only if all elements are equal.
         *
         * @param rhs [in] VelocityScrew6D to compare with
         * @return True if equal.
         */
        bool operator== (const VelocityScrew6D< T >& rhs) const
        {
            for (int i = 0; i < 6; ++i) {
                if (!(_screw[i] == rhs (i))) {
                    return false;
                }
            }
            return true;
        }

        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true if any of the elements are different.
         *
         * @param rhs [in] VelocityScrew6D to compare with
         * @return True if not equal.
         */
        bool operator!= (const VelocityScrew6D< T >& rhs) const { return !(*this == rhs); }
#if !defined(SWIGPYTHON)
        /**
         * @brief Scales velocity screw and returns scaled version
         *
         * @param s [in] scaling value
         * @param screw [in] Screw to scale
         * @return Scales screw
         */
        friend const VelocityScrew6D< T > operator* (T s, const VelocityScrew6D& screw)
        {
            VelocityScrew6D result = screw;
            result *= s;
            return result;
        }
#endif
        /**
         * @brief Scales velocity screw and returns scaled version
         * @param s [in] scaling value
         * @return Scales screw
         */
        const VelocityScrew6D< T > operator* (T s) const
        {
            VelocityScrew6D result = *this;
            result *= s;
            return result;
        }

#if !defined(SWIG)
        /**
         * @brief Changes frame of reference and velocity referencepoint of
         * velocityscrew: @f$ \robabx{b}{b}{\mathbf{\nu}}\to
         * \robabx{a}{a}{\mathbf{\nu}} @f$
         *
         * The frames @f$ \mathcal{F}_a @f$ and @f$ \mathcal{F}_b @f$ are
         * rigidly connected.
         *
         * @param aTb [in] the location of frame @f$ \mathcal{F}_b @f$ wrt.
         * frame @f$ \mathcal{F}_a @f$: @f$ \robabx{a}{b}{\mathbf{T}} @f$
         *
         * @param bV [in] velocity screw wrt. frame @f$ \mathcal{F}_b @f$: @f$
         * \robabx{b}{b}{\mathbf{\nu}} @f$
         *
         * @return the velocity screw wrt. frame @f$ \mathcal{F}_a @f$: @f$
         * \robabx{a}{a}{\mathbf{\nu}} @f$
         *
         * Transformation of both the velocity reference point and of the base to
         * which the VelocityScrew is expressed
         *
         * \f[
         * \robabx{a}{a}{\mathbf{\nu}} =
         * \left[
         *  \begin{array}{c}
         *  \robabx{a}{a}{\mathbf{v}} \\
         *  \robabx{a}{a}{\mathbf{\omega}}
         *  \end{array}
         * \right] =
         * \left[
         *  \begin{array}{cc}
         *    \robabx{a}{b}{\mathbf{R}} & S(\robabx{a}{b}{\mathbf{p}})
         *    \robabx{a}{b}{\mathbf{R}} \\
         *    \mathbf{0}^{3x3} & \robabx{a}{b}{\mathbf{R}}
         *  \end{array}
         * \right]
         * \robabx{b}{b}{\mathbf{\nu}} =
         * \left[
         *  \begin{array}{c}
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{b}{\mathbf{v}} +
         *    \robabx{a}{b}{\mathbf{p}} \times \robabx{a}{b}{\mathbf{R}}
         *    \robabx{b}{b}{\mathbf{\omega}}\\
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{b}{\mathbf{\omega}}
         *  \end{array}
         * \right]
         * \f]
         *
         */
        friend const VelocityScrew6D< T > operator* (const rw::math::Transform3D< T >& aTb,
                                                     const VelocityScrew6D< T >& bV)
        {
            const Vector3D< T >& bv = bV.linear ();
            const EAA< T >& bw      = bV.angular ();
            const EAA< T >& aw      = aTb.R () * bw;
            const Vector3D< T >& av = aTb.R () * bv + cross (aTb.P (), aw);
            return VelocityScrew6D< T > (av, aw);
        }
#endif
#if !defined(SWIG)
        /**
         * @brief Changes velocity referencepoint of
         * velocityscrew: @f$ \robabx{a}{q}{\mathbf{\nu}}\to
         * \robabx{a}{p}{\mathbf{\nu}} @f$
         *
         * The vector should describe a translation from the current
         * velocity reference point q to the wanted/new velocity reference point p
         * seen from frame @f$ \mathcal{F}_a @f$
         *
         * @param aPqTop [in] the translation from point q to point p seen in
         * frame @f$ \mathcal{F}_a @f$
         *
         * @param bV [in] velocity screw wrt. frame @f$ \mathcal{F}_a @f$: @f$
         * \robabx{a}{q}{\mathbf{\nu}} @f$
         *
         * @return the velocity screw wrt. frame @f$ \mathcal{F}_a @f$: @f$
         * \robabx{a}{p}{\mathbf{\nu}} @f$
         *
         * Transformation of the velocity reference point
         *
         * \f[
         * \robabx{a}{p}{\mathbf{\nu}} =
         * \left[
         *  \begin{array}{c}
         *  \robabx{a}{p}{\mathbf{v}} \\
         *  \robabx{a}{p}{\mathbf{\omega}}
         *  \end{array}
         * \right] =
         * \left[
         *  \begin{array}{cc}
         *    \robabx{a}{b}{\mathbf{R}} & S(\robabx{a}{b}{\mathbf{p}})
         *    \robabx{a}{b}{\mathbf{R}} \\
         *    \mathbf{0}^{3x3} & \robabx{a}{b}{\mathbf{R}}
         *  \end{array}
         * \right]
         * \robabx{a}{p}{\mathbf{\nu}} =
         * \left[
         *  \begin{array}{c}
         *    \robabx{a}{p}{\mathbf{v}} +
         *    \robabx{a}{qTop}{\mathbf{p}}
         *    \robabx{b}{b}{\mathbf{\omega}}\\
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{b}{\mathbf{\omega}}
         *  \end{array}
         * \right]
         * \f]
         *
         */
        friend const VelocityScrew6D< T > operator* (const Vector3D< T >& aPqTop,
                                                     const VelocityScrew6D< T >& bV)
        {
            const Vector3D< T >& bv = bV.linear ();
            const EAA< T >& bw      = bV.angular ();
            const Vector3D< T >& av = bv + cross (aPqTop, bw);
            return VelocityScrew6D< T > (av, bw);
        }
#endif
#if !defined(SWIG)
        /**
         * @brief Changes frame of reference for velocityscrew: @f$
         * \robabx{b}{i}{\mathbf{\nu}}\to \robabx{a}{i}{\mathbf{\nu}}
         * @f$
         *
         * @param aRb [in] the change in orientation between frame
         * @f$ \mathcal{F}_a @f$ and frame
         * @f$ \mathcal{F}_b @f$: @f$ \robabx{a}{b}{\mathbf{R}} @f$
         *
         * @param bV [in] velocity screw wrt. frame
         * @f$ \mathcal{F}_b @f$: @f$ \robabx{b}{i}{\mathbf{\nu}} @f$
         *
         * @return the velocity screw wrt. frame @f$ \mathcal{F}_a @f$:
         * @f$ \robabx{a}{i}{\mathbf{\nu}} @f$
         *
         * Transformation of the base to which the VelocityScrew is expressed. The velocity
         * reference point is left intact
         *
         * \f[
         * \robabx{a}{i}{\mathbf{\nu}} =
         * \left[
         *  \begin{array}{c}
         *  \robabx{a}{i}{\mathbf{v}} \\
         *  \robabx{a}{i}{\mathbf{\omega}}
         *  \end{array}
         * \right] =
         * \left[
         *  \begin{array}{cc}
         *    \robabx{a}{b}{\mathbf{R}} & \mathbf{0}^{3x3} \\
         *    \mathbf{0}^{3x3} & \robabx{a}{b}{\mathbf{R}}
         *  \end{array}
         * \right]
         * \robabx{b}{i}{\mathbf{\nu}} =
         * \left[
         *  \begin{array}{c}
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{i}{\mathbf{v}} \\
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{i}{\mathbf{\omega}}
         *  \end{array}
         * \right]
         * \f]
         */
        friend const VelocityScrew6D< T > operator* (const Rotation3D< T >& aRb,
                                                     const VelocityScrew6D< T >& bV)
        {
            Vector3D< T > bv = bV.linear ();
            EAA< T > bw      = bV.angular ();

            return VelocityScrew6D< T > (aRb * bv, aRb * bw);
        }
#endif

        /**
         * @brief Adds two velocity screws together @f$
         * \mathbf{\nu}_{12}=\mathbf{\nu}_1+\mathbf{\nu}_2 @f$
         *
         * @param screw2 [in] @f$ \mathbf{\nu}_2 @f$
         *
         * @return the velocity screw @f$ \mathbf{\nu}_{12} @f$
         */
        const VelocityScrew6D< T > operator+ (const VelocityScrew6D< T >& screw2) const
        {
            return VelocityScrew6D< T > (_screw[0] + screw2._screw[0],
                                         _screw[1] + screw2._screw[1],
                                         _screw[2] + screw2._screw[2],
                                         _screw[3] + screw2._screw[3],
                                         _screw[4] + screw2._screw[4],
                                         _screw[5] + screw2._screw[5]);
        }

        /**
         * @brief Subtracts two velocity screws
         * \f$\mathbf{\nu}_{12}=\mathbf{\nu}_1-\mathbf{\nu}_2\f$
         *
         * \param screw2 [in] \f$\mathbf{\nu}_2\f$
         * \return the velocity screw \f$\mathbf{\nu}_{12} \f$
         */
        const VelocityScrew6D< T > operator- (const VelocityScrew6D< T >& screw2) const
        {
            return VelocityScrew6D< T > (_screw[0] - screw2._screw[0],
                                         _screw[1] - screw2._screw[1],
                                         _screw[2] - screw2._screw[2],
                                         _screw[3] - screw2._screw[3],
                                         _screw[4] - screw2._screw[4],
                                         _screw[5] - screw2._screw[5]);
        }
#if !defined(SWIG)
        /**
         * @brief Ouputs velocity screw to stream
         *
         * @param os [in/out] stream to use
         * @param screw [in] velocity screw
         * @return the resulting stream
         */
        friend std::ostream& operator<< (std::ostream& os, const VelocityScrew6D< T >& screw)
        {
            return os << "{{" << screw (0) << "," << screw (1) << "," << screw (2) << "},{"
                      << screw (3) << "," << screw (4) << "," << screw (5) << "}}";
        }
#else
        TOSTRING (rw::math::VelocityScrew6D< T >);
#endif
        /**
         * @brief Takes the 1-norm of the velocity screw. All elements both
         * angular and linear are given the same weight.
         *
         * @return the 1-norm
         */
        T norm1 () const
        {
            return fabs (_screw[0]) + fabs (_screw[1]) + fabs (_screw[2]) + fabs (_screw[3]) +
                   fabs (_screw[4]) + fabs (_screw[5]);
        }

        /**
         * @brief Takes the 2-norm of the velocity screw. All elements both
         * angular and linear are given the same weight
         *
         * @return the 2-norm
         */
        T norm2 () const
        {
            return std::sqrt (Math::sqr (_screw[0]) + Math::sqr (_screw[1]) +
                              Math::sqr (_screw[2]) + Math::sqr (_screw[3]) +
                              Math::sqr (_screw[4]) + Math::sqr (_screw[5]));
        }

        /**
         * @brief Takes the infinite norm of the velocity screw. All elements
         * both angular and linear are given the same weight.
         *
         * @return the infinite norm
         */
        T normInf () const
        {
            return std::max (
                fabs (_screw[0]),
                std::max (fabs (_screw[1]),
                          std::max (fabs (_screw[2]),
                                    std::max (fabs (_screw[3]),
                                              std::max (fabs (_screw[4]), fabs (_screw[5]))))));
        }

        /**
           @brief Converter to Eigen vector
         */
        Eigen::Matrix< T, 6, 1 > e () const
        {
            Eigen::Matrix< T, 6, 1 > res;
            for (size_t i = 0; i < 6; i++)
                res (i) = _screw[i];
            return res;
        }
    };

    /**
     * @brief Takes the 1-norm of the velocity screw. All elements both
     * angular and linear are given the same weight.
     *
     * @param screw [in] the velocity screw
     * @return the 1-norm
     */
    template< class T > T norm1 (const VelocityScrew6D< T >& screw) { return screw.norm1 (); }

    /**
     * @brief Takes the 2-norm of the velocity screw. All elements both
     * angular and linear are given the same weight
     *
     * @param screw [in] the velocity screw
     * @return the 2-norm
     */
    template< class T > T norm2 (const VelocityScrew6D< T >& screw) { return screw.norm2 (); }

    /**
     * @brief Takes the infinite norm of the velocity screw. All elements
     * both angular and linear are given the same weight.
     *
     * @param screw [in] the velocity screw
     *
     * @return the infinite norm
     */
    template< class T > T normInf (const VelocityScrew6D< T >& screw) { return screw.normInf (); }

    /**
     * @brief Casts VelocityScrew6D<T> to VelocityScrew6D<Q>
     *
     * @param vs [in] VelocityScrew6D with type T
     *
     * @return VelocityScrew6D with type Q
     */
    template< class Q, class T > const VelocityScrew6D< Q > cast (const VelocityScrew6D< T >& vs)
    {
        return VelocityScrew6D< Q > (static_cast< Q > (vs (0)),
                                     static_cast< Q > (vs (1)),
                                     static_cast< Q > (vs (2)),
                                     static_cast< Q > (vs (3)),
                                     static_cast< Q > (vs (4)),
                                     static_cast< Q > (vs (5)));
    }
#if !defined(SWIG)
    extern template class rw::math::VelocityScrew6D< double >;
    extern template class rw::math::VelocityScrew6D< float >;
#else
    SWIG_DECLARE_TEMPLATE (VelocityScrew6Dd, rw::math::VelocityScrew6D< double >);
    SWIG_DECLARE_TEMPLATE (VelocityScrew6Df, rw::math::VelocityScrew6D< float >);
#endif
    using VelocityScrew6Dd = VelocityScrew6D< double >;
    using VelocityScrew6Df = VelocityScrew6D< float >;

    /*@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::VelocityScrew6D
         */
        template<>
        void write (const rw::math::VelocityScrew6D< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::VelocityScrew6D
         */
        template<>
        void write (const rw::math::VelocityScrew6D< float >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::VelocityScrew6D
         */
        template<>
        void read (rw::math::VelocityScrew6D< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::VelocityScrew6D
         */
        template<>
        void read (rw::math::VelocityScrew6D< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

#endif    // end include guard


#include "log_number.hpp"

#include <gtest/gtest.h>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

using namespace ProbabilityDistributions;

TEST(LogNumberTest, DefaultConstruction) {
  LogNumber val;
  EXPECT_EQ(0, val.to_double());
}

TEST(LogNumberTest, CopyConstruction) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 1000;
  boost::random::uniform_real_distribution<> dist(-5,5);

  for (unsigned int i = 0; i < n_samples; i++) {
    double val = dist(rng);
    LogNumber val2(val);
    EXPECT_NEAR(val, val2.to_double(), 1e-10);
  }
}

TEST(LogNumberTest, Assignment) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 1000;
  boost::random::uniform_real_distribution<> dist(-5,5);

  for (unsigned int i = 0; i < n_samples; i++) {
    double val = dist(rng);
    LogNumber val2;
    val2 = val;
    EXPECT_NEAR(val, val2.to_double(), 1e-10);
  }
}

TEST(LogNumberTest, FromLog) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 1000;
  boost::random::uniform_real_distribution<> dist(-5,5);

  for (unsigned int i = 0; i < n_samples; i++) {
    double val = dist(rng);
    LogNumber val2;
    val2.from_log(val);
    EXPECT_NEAR(std::exp(val), val2.to_double(), 1e-10);
  }
}

TEST(LogNumberTest, Sum) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 1000;
  boost::random::uniform_real_distribution<> dist(-5,5);

  for (unsigned int i = 0; i < n_samples; i++) {
    double v1 = dist(rng), v2 = dist(rng);
    LogNumber lv1(v1), lv2(v2);
    EXPECT_NEAR(v1+v2, (lv1+lv2).to_double(), 1e-10);
  }
}

TEST(LogNumberTest, Subtraction) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 1000;
  boost::random::uniform_real_distribution<> dist(-5,5);

  for (unsigned int i = 0; i < n_samples; i++) {
    double v1 = dist(rng), v2 = dist(rng);
    LogNumber lv1(v1), lv2(v2);
    EXPECT_NEAR(v1-v2, (lv1-lv2).to_double(), 1e-10);
  }
}

TEST(LogNumberTest, Multiplication) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 1000;
  boost::random::uniform_real_distribution<> dist(-5,5);

  for (unsigned int i = 0; i < n_samples; i++) {
    double v1 = dist(rng), v2 = dist(rng);
    LogNumber lv1(v1), lv2(v2);
    EXPECT_NEAR(v1*v2, (lv1*lv2).to_double(), 1e-10);
  }
}

TEST(LogNumberTest, Division) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 1000;
  boost::random::uniform_real_distribution<> dist(-5,5);

  for (unsigned int i = 0; i < n_samples; i++) {
    double v1 = dist(rng), v2 = dist(rng);
    LogNumber lv1(v1), lv2(v2);
    EXPECT_NEAR(v1/v2, (lv1/lv2).to_double(), 1e-10);
  }
}


#ifndef HMLIB_UBLAS_INC
#define HMLIB_UBLAS_INC 100

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>

namespace hmLib {
	namespace ublas {
		template<typename T>
		bool invert(const boost::numeric::ublas::matrix<T>& a, boost::numeric::ublas::matrix<T>& b) {
			namespace ub = boost::numeric::ublas;
			ub::matrix<T> tmp(a);
			ub::permutation_matrix<> pm(tmp.size1());

			//fail to factorize
			if(ub::lu_factorize(tmp, pm) != 0) return true;

			b = ub::identity_matrix<T>(tmp.size1());

			ub::lu_substitute(tmp, pm, b);

			return false;
		}
	}
}
#endif


#include <boost/random/lagged_fibonacci.hpp>


#include <iostream>
#include <stdio.h>
#include <fstream>
#include <vector>
//#include <map>
//#include <set>
#include <unordered_set>
//#include <unordered_map>
#include <cmath>
#include <algorithm>

// #include <gnuplot-iostream.h>
#include <assert.h>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

#include "vector3d.hpp"
#include "swiss_to_lat_lon.hpp"
#include "height_map.hpp"

const int horizon_angles = 360;
const int tile_size = 40;

namespace po = boost::program_options;

struct pos_hoz {
    vector3d pos;
    vector3d norm;
    short elevation_angles[horizon_angles];
    pos_hoz() : pos(0,0,0), norm(0,0,0) {
        memset(elevation_angles, 0, sizeof(short) * horizon_angles);
    }
};

bool EQ_DBL(double a, double b){
    return std::abs(a-b)<EPS_DBL;
}

int main(int ac, char** av) {

    bool test = true;
    //Variables to be assigned by program options
    double height_map_resolution = 25.0;;
    double north_bound = 1e100;
    double south_bound = -1e100;
    double east_bound = 1e100;
    double west_bound = -1e100;

    std::string input_file;
    std::string output_file;
    bool verbose = false;

    /*
    // Declare the supported options.
    po::options_description op_desc("Allowed options");

    op_desc.add_options()
    ("help", "print options table")
    ("input-file,i", po::value<std::string>(&input_file), "File containing height map data in x<space>y<space>z<newline> swiss coordinates format")
    ("output-dir,o", po::value<std::string>(&output_file), "File for output data (default data_out.hoz)")
    ("resolution,R", po::value<double>(&height_map_resolution)->default_value(25.0), "resolution of data (default: 25.0)")
    ("nmax",po::value<double>(&north_bound)->default_value(1e100), "maximum north coordinate to be treated (default 1e100)")
    ("nmin",po::value<double>(&south_bound)->default_value(-1e100), "minimum north coordinate to be treated (default -1e100)")
    ("emax",po::value<double>(&east_bound)->default_value(1e100), "maximum east coordinate to be treated (default 1e100)")
    ("emin",po::value<double>(&west_bound)->default_value(-1e100), "minimum east coordinate to be treated (default -1e100)")
    ("verbose, v", "Verbose: output lots of text")
    ;

    po::positional_options_description pd;
    pd.add("input-file", 1).add("output-file", 1);

    po::variables_map vm;
    po::store(po::parse_command_line(ac, av, op_desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout <<"MountainOutline [options] [input file] [output base]"<<std::endl<< op_desc << std::endl;
        return 1;
    }

    if (vm.count("verbose")){
        verbose = true;
    }
    if(!vm.count("input-file")){
        std::cout << "Input file must be specified" << std::endl;
        exit(255);
    }
    if(!vm.count("output-dir")){
        output_file = std::string("data_out.hoz");
        std::cout << "Output directory will be: " << std::endl;
    }
    */
    input_file = std::string("/Users/alexxx/HeightMaps/precieous/DHM25.xyz");
    std::ifstream ifs(input_file);
    if (!ifs.is_open())
        throw std::runtime_error("could not open file : " + std::string(input_file));

    height_map grid_points(ifs, south_bound, north_bound, east_bound, west_bound);

    //grid_points is a height_map, an unordered_set of all points in bounding box with
    //the maximum and minimum x,y,h of all values stored.

    std::pair<double,double> NE = swiss_to_lat_lon(grid_points.xmax()+height_map_resolution/2.0, grid_points.ymax()+height_map_resolution/2.0);
    std::pair<double,double> SW = swiss_to_lat_lon(grid_points.xmin()-height_map_resolution/2.0, grid_points.ymin()-height_map_resolution/2.0);

    std::cout << "NE: " << grid_points.xmax() <<", " << grid_points.ymax() << " SW: "<< grid_points.xmin() << " , " << grid_points.ymin() <<  std::endl;
    std::cout  << "NE: " << NE.first <<", " << NE.second << " SW: "<< SW.first << ", " << SW.second <<  std::endl;
    std::cout << "Maximum height: " << grid_points.hmax() << std::endl;
    std::cout << "Number of points in dataset: " << grid_points.size() << std::endl;

    if(test){
        double y_test = height_map_resolution*floor(601430.0/height_map_resolution);
        double x_test = height_map_resolution*floor(126243.0/height_map_resolution);
        auto it_v = grid_points.find_point(vector3d(x_test,y_test,0));      //get the gridpoint from the set
        std::cout << "Test point: " << x_test <<" "<< y_test << std::endl;
        std::cout << "Resolution: " << height_map_resolution << ", tile size: "<< tile_size << std::endl;
        vector3d v = *it_v;
        double elevation_angles[360];
        int theta_start = 0;
        int theta_end = 360;

        //        time_t start_time  = time(NULL);
        clock_t t = clock();
        for(int theta = theta_start;theta<theta_end;theta++){
            double phi = atan(grid_points.compute_elevation_angle(v,theta, height_map_resolution, 360.0/horizon_angles));
            short phi_short = (short)((phi / M_PI_2) * std::numeric_limits<short>::max());
            elevation_angles[theta] = phi_short;

        }
        double run_time = ((double)(clock()-t))/CLOCKS_PER_SEC;
        //        double run_time = difftime(time(NULL), start_time);
        std::cout << "phi = [";
        for(int theta = theta_start;theta<theta_end;theta++){
            std::cout << elevation_angles[theta] << ";";
        }
        std::cout << "];" << std::endl;
        std::cout << "runtime test point: " << run_time << std::endl;

        return 0;
    }

    return 0;
}


/**
 * @author Alessandro Bianco
 */

/**
 * @addtogroup DFNs
 * @{
 */

#include "CeresEstimation.hpp"

#include <Errors/Assert.hpp>
#include <Errors/AssertOnTest.hpp>
#include <Macros/YamlcppMacros.hpp>

#include <opencv2/calib3d.hpp>
#include <Eigen/Geometry>

#include <stdlib.h>
#include <fstream>

using namespace PoseWrapper;
using namespace MatrixWrapper;
using namespace CorrespondenceMap3DWrapper;
using namespace Helpers;
using namespace BaseTypesWrapper;

namespace CDFF
{
namespace DFN
{
namespace Transform3DEstimation
{

CeresEstimation::CeresEstimation()
{
        parameters = DEFAULT_PARAMETERS;

	parametersHelper.AddParameter<float>("GeneralParameters", "MaximumAllowedError", parameters.maximumAllowedError, DEFAULT_PARAMETERS.maximumAllowedError);
	parametersHelper.AddParameter<float>("GeneralParameters", "MaximumAllowedDeterminantError", parameters.maximumAllowedDeterminantError, DEFAULT_PARAMETERS.maximumAllowedDeterminantError);

	configurationFilePath = "";
}

CeresEstimation::~CeresEstimation()
{
}

void CeresEstimation::configure()
{
	parametersHelper.ReadFile(configurationFilePath);
	ValidateParameters();
}

void CeresEstimation::process()
{
	int numberOfCorrespondenceMaps = GetNumberOfCorrespondenceMaps(inMatches);
	int numberOfCameras = ComputeNumberOfCameras(numberOfCorrespondenceMaps);

	//If there are not enough correspondences, process fails.
	for(int mapIndex = 0; mapIndex < numberOfCorrespondenceMaps; mapIndex++)
		{
		if ( GetNumberOfCorrespondences ( GetCorrespondenceMap(inMatches, mapIndex) ) < 4 )
			{
			outError = -1;
			outSuccess = false;
			return;
			}
		}

	std::vector<Transform3d> transformList(numberOfCorrespondenceMaps);
	InitializeTransforms(transformList);
	outError = SolveEstimation(inMatches, numberOfCameras, transformList);

	if (outError > parameters.maximumAllowedError || outError < 0)
		{
		outSuccess = false;
		return;
		}

	outSuccess = SetOutputPoses(transformList);
}

CeresEstimation::Transform3DCostFunctor::Transform3DCostFunctor(Point3D source, Point3D sink)
	{
	this->source = source;
	this->sink = sink;
	}

template <typename T>
bool CeresEstimation::Transform3DCostFunctor::operator()(const T* const cameraTransform, T* residual) const
	{
	T originalPoint[3] = {T(source.x), T(source.y), T(source.z)};
	T transformedPoint[3];
	TransformPoint(cameraTransform, originalPoint, transformedPoint);

	residual[0] = T(sink.x) - transformedPoint[0];
	residual[1] = T(sink.y) - transformedPoint[1];
	residual[2] = T(sink.z) - transformedPoint[2];

	return true;
	}

//There is an extra computation repeated at this step (the first order transform is computed twice)
//Moreover, we are actually adding a method for each order transform. And we will need to create a functor for each combination, which means exponential data.
//Consider refactoring in one single functor that takes the full list of camera transform.
template <typename T>
bool CeresEstimation::Transform3DCostFunctor::operator()(const T* const firstCameraTransform, const T* const secondCameraTransform, T* residual) const
	{
	T originalPoint[3] = {T(source.x), T(source.y), T(source.z)};
	T firstOrderTransformedPoint[3];
	TransformPoint(firstCameraTransform, originalPoint, firstOrderTransformedPoint);
	T secondOrderTransformedPoint[3];
	TransformPoint(secondCameraTransform, firstOrderTransformedPoint, secondOrderTransformedPoint);

	residual[0] = T(sink.x) - secondOrderTransformedPoint[0];
	residual[1] = T(sink.y) - secondOrderTransformedPoint[1];
	residual[2] = T(sink.z) - secondOrderTransformedPoint[2];

	return true;
	}

template <typename T>
void CeresEstimation::Transform3DCostFunctor::TransformPoint(const T* const cameraTransform, const T* const originalPoint, T* transformedPoint) const
	{

	transformedPoint[0] = cameraTransform[0]*originalPoint[0] + cameraTransform[1]*originalPoint[1] + cameraTransform[2]*originalPoint[2] + cameraTransform[3];
	transformedPoint[1] = cameraTransform[4]*originalPoint[0] + cameraTransform[5]*originalPoint[1] + cameraTransform[6]*originalPoint[2] + cameraTransform[7];
	transformedPoint[2] = cameraTransform[8]*originalPoint[0] + cameraTransform[9]*originalPoint[1] + cameraTransform[10]*originalPoint[2] + cameraTransform[11];

	}

ceres::CostFunction* CeresEstimation::Transform3DCostFunctor::Create(Point3D source, Point3D sink, int transformChainLength)
	{
	if (transformChainLength == 1)
		{
		return
			(
			new ceres::AutoDiffCostFunction<Transform3DCostFunctor, 3, 12>
				(
				new Transform3DCostFunctor(source, sink)
				)
			);
		}
	else if (transformChainLength == 2)
		{
		return
			(
			new ceres::AutoDiffCostFunction<Transform3DCostFunctor, 3, 12, 12>
				(
				new Transform3DCostFunctor(source, sink)
				)
			);
		}
	else
		{
		ASSERT(false, "Ceres Estimation error, unhandled chain length");
		}
	}

const CeresEstimation::CeresEstimationOptionsSet CeresEstimation::DEFAULT_PARAMETERS =
{
	/*.maximumAllowedError =*/ 0.01,
	/*.maximumAllowedDeterminantError =*/ 0.05
};

int CeresEstimation::ComputeNumberOfCameras(int numberOfCorrespondenceMaps)
	{
	const int MAXIMUM_NUMBER_OF_CAMERAS = 8;
	for(int candidateNumber = 0; candidateNumber < MAXIMUM_NUMBER_OF_CAMERAS; candidateNumber++)
		{
		int expectedCorrespondencesNumber = (candidateNumber * (candidateNumber - 1)) / 2;
		if (expectedCorrespondencesNumber == numberOfCorrespondenceMaps )
			{
			return candidateNumber;
			}
		ASSERT(expectedCorrespondencesNumber < numberOfCorrespondenceMaps, "CorrespondenceMaps3DSequenceToMat error, number of correspondences is not as expected");
		}

	ASSERT(false, "CorrespondenceMaps3DSequenceToMatConverter error, number of images is too large");
	return 0;
	}

void CeresEstimation::InitializeTransforms(std::vector<Transform3d>& transformList)
	{
	for(int mapIndex = 0; mapIndex < GetNumberOfCorrespondenceMaps(inMatches); mapIndex++)
		{
		const CorrespondenceMap3D& map = GetCorrespondenceMap(inMatches, mapIndex);
		Transform3d& transform = transformList.at(mapIndex);

		cv::Mat coefficientMatrix, valueMatrix;
		bool success = CreateLinearSystem(map, coefficientMatrix, valueMatrix);

		cv::Mat solution;
		if (success)
			{
			float error = 0;
			solution = SolveLinearSystem(coefficientMatrix, valueMatrix, error);
			}
		for(int componentIndex = 0; componentIndex < 12; componentIndex++)
			{
			transform[componentIndex] = success ? solution.at<float>(componentIndex) : 0;
			}
		}
	}

float CeresEstimation::SolveEstimation(const CorrespondenceMap3DWrapper::CorrespondenceMaps3DSequence& sequence, int numberOfCameras, std::vector<Transform3d>& transformList)
	{
	ceres::Problem transformEstimation;
	int numberOfResiduals = 0;
	int sourceIndex = 0;
	int sinkIndex = 1;
	int firstMapIndexWithSinkAsSource = numberOfCameras - 1; //This is the first map where the current sink becomes source
	for(int mapIndex = 0; mapIndex < GetNumberOfCorrespondenceMaps(sequence); mapIndex++)
		{
		const CorrespondenceMap3D& map = GetCorrespondenceMap(sequence, mapIndex);
		for(int correspondenceIndex = 0; correspondenceIndex < GetNumberOfCorrespondences(map); correspondenceIndex++)
			{
			Point3D sourcePoint = GetSource(map, correspondenceIndex);
			Point3D sinkPoint = GetSink(map, correspondenceIndex);

			ceres::CostFunction* transform3DCostFunctor = Transform3DCostFunctor::Create ( sourcePoint, sinkPoint, 1);
			transformEstimation.AddResidualBlock( transform3DCostFunctor, NULL, transformList.at(mapIndex) );
			numberOfResiduals += 3;

			//These are the constraints for the double transform sourcePoint -> sinkPoint -> secondSinkPoint
			for(int secondSinkIndex = sinkIndex + 1; secondSinkIndex < numberOfCameras; secondSinkIndex++)
				{
				int secondMapIndex = firstMapIndexWithSinkAsSource + secondSinkIndex - sinkIndex - 1;
				const CorrespondenceMap3D& secondMap = GetCorrespondenceMap(sequence, secondMapIndex);
				for(int secondCorrespondenceIndex = 0; secondCorrespondenceIndex < GetNumberOfCorrespondences(secondMap); secondCorrespondenceIndex++)
					{
					Point3D secondSourcePoint = GetSource(secondMap, secondCorrespondenceIndex);
					Point3D secondSinkPoint = GetSink(secondMap, secondCorrespondenceIndex);
					if (sinkPoint.x == secondSourcePoint.x && sinkPoint.y == secondSourcePoint.y)
						{
						ceres::CostFunction* transform3DCostFunctor = Transform3DCostFunctor::Create ( sourcePoint, secondSinkPoint, 2);
						transformEstimation.AddResidualBlock( transform3DCostFunctor, NULL, transformList.at(mapIndex), transformList.at(secondMapIndex) );
						numberOfResiduals += 3;
						}
					}
				}

			}

		if (sinkIndex == numberOfCameras - 1)
			{
			sourceIndex++;
			sinkIndex = sourceIndex+1;
			firstMapIndexWithSinkAsSource = mapIndex + 1 + (numberOfCameras - sinkIndex);
			}
		else
			{
			sinkIndex++;
			firstMapIndexWithSinkAsSource += numberOfCameras - sinkIndex;
			}
		}

	if (numberOfResiduals < 6 * numberOfCameras)
		{
		return -1;
		}

	//Calling the solver
	ceres::Solver::Options ceresOptions;
	ceresOptions.linear_solver_type = ceres::DENSE_SCHUR;
	ceresOptions.minimizer_progress_to_stdout = true;
	ceresOptions.logging_type = ceres::SILENT;
	ceres::Solver::Summary summary;
	ceres::Solve(ceresOptions, &transformEstimation, &summary);
	return summary.final_cost / static_cast<float>(numberOfResiduals);
	}

bool CeresEstimation::SetOutputPoses(const std::vector<Transform3d>& transformList)
	{
	Clear(outTransforms);
	int validTransformCount = 0;
	for(int poseIndex = 0; poseIndex < transformList.size(); poseIndex++)
		{
		const Transform3d& transform = transformList.at(poseIndex);
		Pose3D pose;

		cv::Mat rotation = (cv::Mat_<double>(3, 3) <<
			transform[0], transform[1], transform[2],
			transform[4], transform[5], transform[6],
			transform[8], transform[9], transform[10] );
		double rotationDeterminant = cv::determinant(rotation);

		if (std::abs(rotationDeterminant) >= 1 - parameters.maximumAllowedDeterminantError && std::abs(rotationDeterminant) <= 1 + parameters.maximumAllowedDeterminantError)
			{
			cv::Mat translation = (cv::Mat_<double>(3, 1) << transform[3], transform[7], transform[11] );
			cv::Mat position = - rotation.inv() * translation;
			SetPosition(pose, position.at<double>(0,0), position.at<double>(1,0), position.at<double>(2,0));

			double qw = std::sqrt(1.00 + rotation.at<double>(0,0) + rotation.at<double>(1,1) + rotation.at<double>(2,2)) / 2;
			double qx = (rotation.at<double>(2,1) - rotation.at<double>(1,2)) / ( 4 * qw );
			double qy = (rotation.at<double>(0,2) - rotation.at<double>(2,0)) / ( 4 * qw );
			double qz = (rotation.at<double>(1,0) - rotation.at<double>(0,1)) / ( 4 * qw );
			SetOrientation(pose, qx, qy, qz, qw);

			validTransformCount++;
			}
		else
			{
			SetPosition(pose, 0, 0, 0);
			SetOrientation(pose, 0, 0, 0, 0);
			}

		AddPose(outTransforms, pose);
		}
	return (validTransformCount > 0);
	}

bool CeresEstimation::CreateLinearSystem(const CorrespondenceMap3D& map, cv::Mat& coefficientMatrix, cv::Mat& valueMatrix)
	{
	const float EPSILON = 0.00001;
	const int NUMBER_OF_DEGREES_OF_FREEDOM = 6;
	const int NUMBER_OF_VARIABLES = 12;

	coefficientMatrix = cv::Mat();
	valueMatrix = cv::Mat();
	for(int correspondenceIndex = 0; correspondenceIndex < GetNumberOfCorrespondences(map); correspondenceIndex++)
		{
		Point3D source = GetSource(map, correspondenceIndex);
		Point3D sink = GetSink(map, correspondenceIndex);

		if (source.x != source.x || source.y != source.y || source.z != source.z || sink.x != sink.x || sink.y != sink.y || sink.z != sink.z)
			{
			continue;
			}

		cv::Mat coefficientMatrixPart = ( cv::Mat_<float>(3, 12) <<
			source.x, source.y, source.z, 1,    0, 0, 0, 0,    0, 0, 0, 0,
			0, 0, 0, 0,   source.x, source.y, source.z, 1,     0, 0, 0, 0,
			0, 0, 0, 0,   0, 0, 0, 0,     source.x, source.y, source.z, 1 );
		cv::Mat valueMatrixPart = ( cv::Mat_<float>(3, 1) << sink.x, sink.y, sink.z);

		if (coefficientMatrix.rows == 0)
			{
			coefficientMatrix = coefficientMatrixPart;
			valueMatrix = valueMatrixPart;
			}
		else
			{
			cv::Mat matrixList[2] = {coefficientMatrix, coefficientMatrixPart};
			cv::vconcat(matrixList, 2, coefficientMatrix);
			matrixList[0] = valueMatrix;
			matrixList[1] = valueMatrixPart;
			cv::vconcat(matrixList, 2, valueMatrix);
			}
		}

	if ( coefficientMatrix.rows < NUMBER_OF_VARIABLES )
		{
		return false;
		}

	cv::Mat singulaValueMatrix;
	cv::SVD::compute(coefficientMatrix, singulaValueMatrix);
	int rank = cv::countNonZero( singulaValueMatrix > EPSILON );
	return ( rank >= NUMBER_OF_DEGREES_OF_FREEDOM );
	}

cv::Mat CeresEstimation::SolveLinearSystem(cv::Mat coefficientMatrix, cv::Mat valueMatrix, float& error)
	{
	cv::Mat pseudoInverse;
	cv::invert(coefficientMatrix, pseudoInverse, cv::DECOMP_SVD);
	cv::Mat transformMatrix = pseudoInverse * valueMatrix;

	cv::Mat errorMatrix = coefficientMatrix * transformMatrix - valueMatrix;
	error = cv::norm(errorMatrix);
	return transformMatrix;
	}

void CeresEstimation::ValidateParameters()
{
	ASSERT(parameters.maximumAllowedError > 0, "LeastSquaresMinimization Configuration Error: maximumAllowedError has to be positive");
}

void CeresEstimation::ValidateInputs(const CorrespondenceMap3D& map)
{

}

}
}
}

/** @} */