/**
 * @date Wed Apr 20 20:21:19 2011 +0200
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 *
 * Rewritten:
 * @date Wed Apr 10 17:39:21 CEST 2013
 * @author Manuel Guenther <manuel.guenther@idiap.ch>
 *
 * @brief LBP implementation
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <bob.ip.base/LBP.h>

#include <boost/math/constants/constants.hpp>

bob::ip::base::LBP::LBP(
    const int P,
    const double R_y,
    const double R_x,
    const bool circular,
    const bool to_average,
    const bool add_average_bit,
    const bool uniform,
    const bool rotation_invariant,
    const bob::ip::base::ELBPType eLBP_type,
    const bob::ip::base::LBPBorderHandling border_handling
):
  m_P(P),
  m_R_y(R_y),
  m_R_x(R_x),
  m_mb_y(-1),
  m_mb_x(-1),
  m_ov_y(0),
  m_ov_x(0),
  m_circular(circular),
  m_to_average(to_average),
  m_add_average_bit(add_average_bit),
  m_uniform(uniform),
  m_rotation_invariant(rotation_invariant),
  m_eLBP_type(eLBP_type),
  m_border_handling(border_handling),
  m_lut(0),
  m_positions(0,0),
  m_int_positions(0,0)
{
  // sanity check
  if (m_eLBP_type == ELBP_DIRECTION_CODED && m_P%2) {
    throw std::runtime_error("Direction coded LBP types require an even number of neighbors.");
  }
  init();
}

bob::ip::base::LBP::LBP(
    const int P,
    const double R,
    const bool circular,
    const bool to_average,
    const bool add_average_bit,
    const bool uniform,
    const bool rotation_invariant,
    const bob::ip::base::ELBPType eLBP_type,
    const bob::ip::base::LBPBorderHandling border_handling
):
  m_P(P),
  m_R_y(R),
  m_R_x(R),
  m_mb_y(-1),
  m_mb_x(-1),
  m_ov_y(0),
  m_ov_x(0),
  m_circular(circular),
  m_to_average(to_average),
  m_add_average_bit(add_average_bit),
  m_uniform(uniform),
  m_rotation_invariant(rotation_invariant),
  m_eLBP_type(eLBP_type),
  m_border_handling(border_handling),
  m_lut(0),
  m_positions(0,0),
  m_int_positions(0,0)
{
  // sanity check
  if (m_eLBP_type == ELBP_DIRECTION_CODED && m_P%2) {
    throw std::runtime_error("Direction coded LBP types require an even number of neighbors.");
  }
  init();
}

bob::ip::base::LBP::LBP(
    const int P,
    const blitz::TinyVector<int,2> block_size,
    const blitz::TinyVector<int,2> block_overlap,
    const bool to_average,
    const bool add_average_bit,
    const bool uniform,
    const bool rotation_invariant,
    const bob::ip::base::ELBPType eLBP_type,
    const bob::ip::base::LBPBorderHandling border_handling):
  m_P(P),
  m_R_y(-1.),
  m_R_x(-1.),
  m_mb_y(block_size[0]),
  m_mb_x(block_size[1]),
  m_ov_y(block_overlap[0]),
  m_ov_x(block_overlap[1]),
  m_circular(false),
  m_to_average(to_average),
  m_add_average_bit(add_average_bit),
  m_uniform(uniform),
  m_rotation_invariant(rotation_invariant),
  m_eLBP_type(eLBP_type),
  m_border_handling(border_handling),
  m_lut(0),
  m_positions(0,0),
  m_int_positions(0,0)
{
  // sanity check
  if (m_eLBP_type == ELBP_DIRECTION_CODED && m_P%2) {
    throw std::runtime_error("Direction coded LBP types require an even number of neighbors.");
  }
  init();
}

bob::ip::base::LBP::LBP(bob::io::base::HDF5File file):
  m_P(0),
  m_R_y(0),
  m_R_x(0),
  m_mb_y(0),
  m_mb_x(0),
  m_ov_y(0),
  m_ov_x(0),
  m_circular(false),
  m_to_average(false),
  m_add_average_bit(false),
  m_uniform(false),
  m_rotation_invariant(false),
  m_eLBP_type(bob::ip::base::ELBP_REGULAR),
  m_border_handling(bob::ip::base::LBP_BORDER_SHRINK),
  m_lut(0),
  m_positions(0,0),
  m_int_positions(0,0)
{
  // sanity check
  load(file);
}

bob::ip::base::LBP::LBP(const bob::ip::base::LBP& other):
  m_P(other.m_P),
  m_R_y(other.m_R_y),
  m_R_x(other.m_R_x),
  m_mb_y(other.m_mb_y),
  m_mb_x(other.m_mb_x),
  m_ov_y(other.m_ov_y),
  m_ov_x(other.m_ov_x),
  m_circular(other.m_circular),
  m_to_average(other.m_to_average),
  m_add_average_bit(other.m_add_average_bit),
  m_uniform(other.m_uniform),
  m_rotation_invariant(other.m_rotation_invariant),
  m_eLBP_type(other.m_eLBP_type),
  m_border_handling(other.m_border_handling),
  m_lut(0),
  m_positions(0,0),
  m_int_positions(0,0)
{
  // sanity check
  if (m_eLBP_type == ELBP_DIRECTION_CODED && m_P%2) {
    throw std::runtime_error("Direction coded LBP types require an even number of neighbors.");
  }
  init();
}

bob::ip::base::LBP::~LBP() { }

bob::ip::base::LBP& bob::ip::base::LBP::operator=(const bob::ip::base::LBP& other) {
  m_P = other.m_P;
  m_R_y = other.m_R_y;
  m_R_x = other.m_R_x;
  m_mb_y = other.m_mb_y;
  m_mb_x = other.m_mb_x;
  m_ov_y = other.m_ov_y;
  m_ov_x = other.m_ov_x;
  m_circular = other.m_circular;
  m_to_average = other.m_to_average;
  m_add_average_bit = other.m_add_average_bit;
  m_uniform = other.m_uniform;
  m_rotation_invariant = other.m_rotation_invariant;
  m_eLBP_type = other.m_eLBP_type;
  m_border_handling = other.m_border_handling;
  init();
  return *this;
}

bool bob::ip::base::LBP::operator==(const bob::ip::base::LBP& other) const{
  return
    m_P == other.m_P &&
    m_R_y == other.m_R_y &&
    m_R_x == other.m_R_x &&
    m_mb_y == other.m_mb_y &&
    m_mb_x == other.m_mb_x &&
    m_ov_y == other.m_ov_y &&
    m_ov_x == other.m_ov_x &&
    m_circular == other.m_circular &&
    m_to_average == other.m_to_average &&
    m_add_average_bit == other.m_add_average_bit &&
    m_uniform == other.m_uniform &&
    m_rotation_invariant == other.m_rotation_invariant &&
    m_eLBP_type == other.m_eLBP_type &&
    m_border_handling == other.m_border_handling;
}

uint16_t bob::ip::base::LBP::right_shift_circular(uint16_t pattern, int spaces)
{
  return (pattern >> spaces | pattern << (m_P-spaces)) & ((1 << m_P) - 1);
}

void bob::ip::base::LBP::init()
{
  if (m_P < 4)
    throw std::runtime_error("LBP codes with less than 4 bits are not supported.");
  // check that the parameters are something useful, what we can handle
  if (m_P != 4 && m_P != 8 && m_P != 16 &&
      (m_uniform || m_rotation_invariant || (m_add_average_bit && m_to_average)))
    throw std::runtime_error("Special LBP types are only implemented for 4, 8, or 16 neighbors.");
  if (m_P == 16 && m_add_average_bit && m_to_average){
    throw std::runtime_error("LBP codes with average bit require 17 bits, but our representation is UINT16.");
  }
  if (isMultiBlockLBP() && m_P == 16){
    throw std::runtime_error("LBP codes are not supported for multi-block LBP's.");
  }
  if (m_P > 16){
    throw std::runtime_error("LBP codes with more than 16 neighbors are not supported since our representation is UINT16.");
  }
  if ((m_mb_y < 0 || m_mb_x < 0) && (m_R_y < 0 || m_R_x < 0)){
    throw std::runtime_error("LPB codes with negative radius or negative multi-block dimensions are not supported.");
  }

  if (isMultiBlockLBP() && m_border_handling != LBP_BORDER_SHRINK){
    throw std::runtime_error("Multi-block LBP codes cannot handle other border handling than LBP_BORDER_SHRINK");
  }

  if (isMultiBlockLBP() && (m_ov_y < 0 || m_ov_y >= m_mb_y || m_ov_x < 0 || m_ov_x >= m_mb_x)){
    throw std::runtime_error("Overlap of Multi-block LBP's must be positive and smaller than the multi-block size");
  }

  // initialize temporal memory
  _pixels.resize(m_P);

  // initialize the positions
  if (m_mb_y > 0 && m_mb_x > 0){
    // multi-block LBP requested; store the top-left and bottom-right entry for all our positions
    // compute the top-left of the central pixel
    blitz::TinyVector<int, 8> d_y, d_x;
    int top_y = -m_mb_y/2, left_x = -m_mb_x/2;
    switch (m_P){
      case 4:{
        // 4 neighbors: (-y,0), (0,x), (y,0), (0,-x)
        d_y = -m_mb_y + m_ov_y, 0, m_mb_y - m_ov_y, 0;
        d_x = 0, m_mb_x - m_ov_x, 0, -m_mb_x + m_ov_x;
      }break;
      case 8:{
        // 8 neighbors: (-y,-x), (-y,0), (-y,x), (0,x), (y,x), (y,0), (y,-x), (0,-x)
        d_y = -m_mb_y + m_ov_y, -m_mb_y + m_ov_y, -m_mb_y + m_ov_y, 0, m_mb_y - m_ov_y, m_mb_y - m_ov_y, m_mb_y - m_ov_y, 0;
        d_x = -m_mb_x + m_ov_x, 0, m_mb_x - m_ov_x, m_mb_x - m_ov_x, m_mb_x - m_ov_x, 0, -m_mb_x + m_ov_x, -m_mb_x + m_ov_x;
        break;
      }
      default:
        // any other number of neighbors is not supported
        throw std::runtime_error("Multi-block LBP's with other than 4 and 8 neighbors are not supported.");
    }
    // fill the positions
    m_int_positions.resize(m_P+1, 4);
    for (int p = 0; p < m_P; ++p){
      // top of the region
      m_int_positions(p,0) = d_y[p] + top_y;
      // bottom of the region (not included)
      m_int_positions(p,1) = d_y[p] + top_y + m_mb_y;
      // left of the region
      m_int_positions(p,2) = d_x[p] + left_x;
      // right of the region (not included)
      m_int_positions(p,3) = d_x[p] + left_x + m_mb_x;
    }
    // fill the last position, which is the central pixel
    // top of the region
    m_int_positions(m_P,0) = top_y;
    // bottom of the region (not included)
    m_int_positions(m_P,1) = top_y + m_mb_y;
    // left of the region
    m_int_positions(m_P,2) = left_x;
    // right of the region (not included)
    m_int_positions(m_P,3) = left_x + m_mb_x;
  }else{
    if (m_circular){
      m_positions.resize(m_P,2);
      double PI = boost::math::constants::pi<double>();
      // compute angle offset since LBP codes do not start at the x axis
      double angle_offset = m_P == 4 ? - 0.5 * PI : - 0.75 * PI;
      for (int p = 0; p < m_P; ++p){
        double angle = angle_offset + 2. * PI * p / m_P;
        m_positions(p,0) = m_R_y * sin(angle);
        m_positions(p,1) = m_R_x * cos(angle);
      }
    }else{ // circular
      blitz::TinyVector<int, 8> d_y, d_x;
      int r_y = (int)round(m_R_y), r_x = (int)round(m_R_x);
      switch (m_P){
        case 4:{
          // 4 neighbors: (-y,0), (0,x), (y,0), (0,-x)
          d_y = -r_y, 0, r_y, 0;
          d_x = 0, r_x, 0, -r_x;
        }break;
        case 8:{
          // 8 neighbors: (-y,-x), (-y,0), (-y,x), (0,x), (y,x), (y,0), (y,-x), (0,-x)
          d_y = -r_y, -r_y, -r_y, 0, r_y, r_y, r_y, 0;
          d_x = -r_x, 0, r_x, r_x, r_x, 0, -r_x, -r_x;
          break;
        }
        case 16:
          // 16 neighbors: ...
          throw std::runtime_error("Rectangular LBP16 codes are not yet implemented.");
        default:
          // any other number of neighbors is not supported
          throw std::runtime_error("Rectangular LBP's with other than 4 and 8 neighbors are not supported.");
      }
      // fill the positions
      m_int_positions.resize(m_P,2);
      for (int p = 0; p < m_P; ++p){
        m_int_positions(p,0) = d_y[p];
        m_int_positions(p,1) = d_x[p];
      }
    }
  }

  // initialize the look up table for the current setup
  // initialize all values with 0
  m_lut.resize(1 << m_P);
  m_lut = 0;
  uint16_t lbp_code = 0;
  if (m_uniform){
    // pre-compute uniform pattern bases (i.e., which are rotated to build all uniform patterns)
    std::vector<uint16_t> uniform_pattern_bases(m_P+1);
    for (int p = 1; p < m_P+1; ++p){
      // the pattern generation is adapted to be identical to the old LBP8R uniform pattern generator
      uniform_pattern_bases[p] = ((1 << p) -1) << (m_P - p);
    }
    // all non uniform patterns have a label of 0.
    m_lut = lbp_code++;
    // LBP pattern with all zero bits equal to 1
    m_lut(0) = lbp_code++;
    // compute patterns
    // all the other LBP patterns with bases[i] next-to-each-other bits equal to 1
    for (int p = 1; p < m_P; ++p){
      // assign all shifted versions of the base pattern the same lbp code
      for (int shift = 0; shift < m_P; ++shift){
        int shifted_pattern = right_shift_circular(uniform_pattern_bases[p], shift);
        // assign the shifted pattern
        m_lut(shifted_pattern) = lbp_code;
        if (!m_rotation_invariant)
          // change lbp code for each shift of each pattern
          ++lbp_code;
      }
      if (m_rotation_invariant)
        // change lbp code for each pattern
        ++lbp_code;
    }
    //LBP pattern with all unit bits gets the last code
    m_lut(uniform_pattern_bases[m_P]) = lbp_code;
  }else{ // not uniform
    if (m_rotation_invariant){
      // rotation invariant patterns are not that easy...
      // first, collect all possible RI patterns and assign all patterns to them
      std::vector<bool> found_pattern(1 << m_P);
      std::fill(found_pattern.begin(), found_pattern.end(), false);

      for (int c = 0; c < (1 << m_P); ++c){
        uint16_t pattern = static_cast<uint16_t>(c);
        // search for the LBP code with the smallest integral value
        bool this_pattern_is_new = false;
        for (int p = 0; p < m_P; ++p)
        {
          // generate shifted version of the code
          uint16_t shifted_code = right_shift_circular(pattern, p);
          if (!found_pattern[shifted_code]){
            found_pattern[shifted_code] = true;
            this_pattern_is_new = true;
            m_lut(shifted_code) = lbp_code;
          }
        }
        if (this_pattern_is_new){
          ++lbp_code;
        }
      }
    }else{ // not rotation invariant
      // initialize LUT with non-special values
      if(m_add_average_bit && m_to_average)
        m_lut.resize(1 << (m_P+1));
      blitz::firstIndex i;
      m_lut = i;
    }
  }
}

const blitz::TinyVector<int,2> bob::ip::base::LBP::getLBPShape(const blitz::TinyVector<int,2>& resolution, bool is_integral_image) const
{
  int dy, dx;
  if (m_border_handling == LBP_BORDER_WRAP){
    // when wrapping borders, the resolution is not altered
    dy = 0;
    dx = 0;
  } else if (m_mb_y > 0 && m_mb_x > 0){
    dy = 3 * m_mb_y - 2 * m_ov_y - 1;
    dx = 3 * m_mb_x - 2 * m_ov_x - 1;
  } else {
    dy = 2*(int)ceil(m_R_y);
    dx = 2*(int)ceil(m_R_x);
  }

  if (is_integral_image){
    // if the given image is an integral image, we have to subtract one pixel more
    dy += 1;
    dx += 1;
  }
  return blitz::TinyVector<int,2> (std::max(0, resolution[0] - dy), std::max(0, resolution[1] - dx));
}

blitz::TinyVector<int, 2> bob::ip::base::LBP::getOffset() const {
  blitz::TinyVector<int, 2> offset;
  if (m_border_handling == LBP_BORDER_WRAP){
    // for wrapping boarders, all pixels are allowed
    offset[0] = 0;
    offset[1] = 0;
  } else if (m_mb_y > 0 && m_mb_x > 0){
    // for multi-block LBP features, the offset is defined as 1.5 times the block size
    offset[0] = m_mb_y - m_ov_y + m_mb_y/2;
    offset[1] = m_mb_x - m_ov_x + m_mb_x/2;
  } else {
    /// for normal LBP, the offset is equal to the radius
    offset[0] = (int)ceil(m_R_y);
    offset[1] = (int)ceil(m_R_x);
  }
  return offset;
}

int bob::ip::base::LBP::getMaxLabel() const {
  if (m_rotation_invariant){
    if (m_uniform)
      // rotation invariant uniform LBP
      return m_P + 2;
    else
      // rotation invariant non-uniform LBP
      // simply return the highest label plus 1
      return m_lut((1 << m_P) - 1) + 1;
  }else{
    if (m_uniform)
      // uniform LBP
      return m_P * (m_P-1) + 3;
    else{
      // regular LBP
      if (m_to_average && m_add_average_bit)
        return 1 << (m_P+1);
      else
        return 1 << m_P;
    }
  }
}

void bob::ip::base::LBP::save(bob::io::base::HDF5File file) const{
  file.set("Neighbors", m_P);
  if (m_mb_y > 0 && m_mb_y > 0) {
    file.append("BlockSize", m_mb_y);
    file.append("BlockSize", m_mb_x);
    file.append("BlockOverlap", m_ov_y);
    file.append("BlockOverlap", m_ov_x);
  } else {
    file.append("Radius", m_R_y);
    file.append("Radius", m_R_x);
    file.set("Circular", m_circular ? 1 : 0);
    file.set("BorderHandling", m_border_handling);
  }
  file.set("Uniform", m_uniform ? 1 : 0);
  file.set("RotationInvariant", m_rotation_invariant ? 1 : 0);
  file.set("ToAverage", m_to_average ? 1 : 0);
  file.set("AddAverageBit", m_add_average_bit ? 1 : 0);
  file.set("ELBPType", m_eLBP_type);
}

void bob::ip::base::LBP::load(bob::io::base::HDF5File file){
  m_P = file.read<int>("Neighbors");
  m_ov_y = m_ov_x = 0;
  if (file.contains("BlockSize")){
    // multi-block LBP
    m_mb_y = file.read<int>("BlockSize", 0);
    m_mb_x = file.read<int>("BlockSize", 1);
    // block overlap
    if (file.contains("BlockOverlap")){
      m_ov_y = file.read<int>("BlockOverlap", 0);
      m_ov_x = file.read<int>("BlockOverlap", 1);
    }
    m_R_y = m_R_x = -1.;
    m_circular = false;
    m_border_handling = LBP_BORDER_SHRINK;
  } else {
    // regular LBP
    m_R_y = file.read<double>("Radius", 0);
    m_R_x = file.read<double>("Radius", 1);
    m_border_handling = static_cast<bob::ip::base::LBPBorderHandling>(file.read<int>("BorderHandling"));
    m_circular = file.read<int>("Circular") > 0;
    m_mb_y = m_mb_x = -1;
  }

  m_uniform = file.read<int>("Uniform") > 0;
  m_rotation_invariant = file.read<int>("RotationInvariant") > 0;
  m_to_average = file.read<int>("ToAverage") > 0;
  m_add_average_bit = file.read<int>("AddAverageBit") > 0;
  m_eLBP_type = static_cast<bob::ip::base::ELBPType>(file.read<int>("ELBPType"));

  // initialize
  init();
}


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

#ifndef BTL_Eigen_UTILITY_HEADER
#define BTL_Eigen_UTILITY_HEADER

//eigen-based helpers
#include "OtherUtil.hpp"
#include <Eigen/Dense>
#include <opencv2/core.hpp>

namespace btl
{
namespace utility
{
using namespace Eigen;
using namespace btl::utility;

template< class T >
Eigen::Matrix< T, 4, 4 > inver(const Eigen::Matrix< T, 4, 4 >& F_ctw_){
	using namespace Eigen;
	Matrix< T, 4, 4 > F_wtc; F_wtc.setIdentity();

	Matrix< T, 3, 3 > R_trans = F_ctw_.block(0,0,3,3);
	Matrix< T, 3, 1 > Cw = F_ctw_.block(0,3,3,1);

	F_wtc.block(0,0,3,3) = R_trans.transpose();
	F_wtc.block(0,3,3,1) = -R_trans*Cw;
	return F_wtc;
}

template< class T >
void getCwVwFromPrj_Cam2World(const Eigen::Matrix< T, 4, 4 >&  Prj_ctw_,Eigen::Matrix< T, 3, 1 >* pCw_,Eigen::Matrix< T, 3, 1 >* pVw_){
	*pCw_ = Prj_ctw_.template block<3,1>(0,3); //4th column is camera centre
	*pVw_ = Prj_ctw_.template block<3,1>(0,0); //1st column is viewing direction
}

template< class T >
void getRTCVfromModelViewGL ( const Eigen::Matrix< T, 4, 4 >&  mMat_, Eigen::Matrix< T, 3, 3 >* pmR_, Eigen::Matrix< T, 3, 1 >* pvT_ )
{
    (* pmR_) ( 0, 0 ) =  mMat_ ( 0, 0 );   (* pmR_) ( 0, 1 ) =   mMat_ ( 0, 1 );  (* pmR_) ( 0, 2 ) = mMat_ ( 0, 2 );
    (* pmR_) ( 1, 0 ) = -mMat_ ( 1, 0 );   (* pmR_) ( 1, 1 ) = - mMat_ ( 1, 1 );  (* pmR_) ( 1, 2 ) = -mMat_ ( 1, 2 );
    (* pmR_) ( 2, 0 ) = -mMat_ ( 2, 0 );   (* pmR_) ( 2, 1 ) = - mMat_ ( 2, 1 );  (* pmR_) ( 2, 2 ) = -mMat_ ( 2, 2 );

	(*pvT_) ( 0 ) = mMat_ ( 0, 3 );
    (*pvT_) ( 1 ) = -mMat_ ( 1, 3 );
    (*pvT_) ( 2 ) = -mMat_ ( 2, 3 );

    return;
}

template< class T >
Eigen::Matrix< T, 4, 4 > setModelViewGLfromPrj(const Eigen::Transform<T, 3, Eigen::Affine> & Prj_)
{
	// column first for pGLMat_[16];
	// row first for Matrix3d;
	// pGLMat_[ 0] =  mR_(0,0); pGLMat_[ 4] =  mR_(0,1); pGLMat_[ 8] =  mR_(0,2); pGLMat_[12] =  vT_(0);
	// pGLMat_[ 1] = -mR_(1,0); pGLMat_[ 5] = -mR_(1,1); pGLMat_[ 9] = -mR_(1,2); pGLMat_[13] = -vT_(1);
	// pGLMat_[ 2] = -mR_(2,0); pGLMat_[ 6] = -mR_(2,1); pGLMat_[10] = -mR_(2,2); pGLMat_[14] = -vT_(2);
	// pGLMat_[ 3] =  0;        pGLMat_[ 7] =  0;        pGLMat_[11] =  0;        pGLMat_[15] = 1;

	Eigen::Matrix< T , 4, 4 > mMat;
	mMat.row( 0 ) =  Prj_.matrix().row( 0 );
	mMat.row( 1 ) = -Prj_.matrix().row( 1 );
	mMat.row( 2 ) = -Prj_.matrix().row( 2 );
	mMat.row( 3 ) =  Prj_.matrix().row( 3 );

	return mMat;
}

template< class T >
Eigen::Matrix< T , 4, 4 > setModelViewGLfromRTCV ( const Eigen::Matrix< T, 3, 3 >& mR_, const Eigen::Matrix< T, 3, 1 >& vT_ )
{
    // column first for pGLMat_[16];
    // row first for Matrix3d;
    // pGLMat_[ 0] =  mR_(0,0); pGLMat_[ 4] =  mR_(0,1); pGLMat_[ 8] =  mR_(0,2); pGLMat_[12] =  vT_(0);
    // pGLMat_[ 1] = -mR_(1,0); pGLMat_[ 5] = -mR_(1,1); pGLMat_[ 9] = -mR_(1,2); pGLMat_[13] = -vT_(1);
    // pGLMat_[ 2] = -mR_(2,0); pGLMat_[ 6] = -mR_(2,1); pGLMat_[10] = -mR_(2,2); pGLMat_[14] = -vT_(2);
    // pGLMat_[ 3] =  0;        pGLMat_[ 7] =  0;        pGLMat_[11] =  0;        pGLMat_[15] = 1;

    Eigen::Matrix< T , 4, 4 > mMat;
    mMat ( 0, 0 ) =  mR_ ( 0, 0 ); mMat ( 0, 1 ) =  mR_ ( 0, 1 ); mMat ( 0, 2 ) =  mR_ ( 0, 2 ); mMat ( 0, 3 ) =  vT_ ( 0 );
    mMat ( 1, 0 ) = -mR_ ( 1, 0 ); mMat ( 1, 1 ) = -mR_ ( 1, 1 ); mMat ( 1, 2 ) = -mR_ ( 1, 2 ); mMat ( 1, 3 ) = -vT_ ( 1 );
    mMat ( 2, 0 ) = -mR_ ( 2, 0 ); mMat ( 2, 1 ) = -mR_ ( 2, 1 ); mMat ( 2, 2 ) = -mR_ ( 2, 2 ); mMat ( 2, 3 ) = -vT_ ( 2 );
    mMat ( 3, 0 ) =  0;            mMat ( 3, 1 ) =  0;            mMat ( 3, 2 ) =  0;            mMat ( 3, 3 ) =  1;

    return mMat;
}

template< class T >
Eigen::Matrix< T , 4, 4 > setModelViewGLfromRCCV ( const Eigen::Matrix< T, 3, 3 >& mR_, const Eigen::Matrix< T, 3, 1 >& vC_ )
{
	Eigen::Matrix< T, 3,1> eivT = -mR_.transpose()*vC_;
	return setModelViewGLfromRTCV(mR_,vC_);
}

template< class T1, class T2 >
void unprojectCamera2World ( const int& nX_, const int& nY_, const unsigned short& nD_, const Eigen::Matrix< T1, 3, 3 >& mK_, Eigen::Matrix< T2, 3, 1 >* pVec_ )
{
	//the pixel coordinate is defined w.r.t. opencv camera reference, which is defined as x-right, y-downward and z-forward. It's
	//a right hand system.
	//when rendering the point using opengl's camera reference which is defined as x-right, y-upward and z-backward. the
	//	glVertex3d ( Pt(0), -Pt(1), -Pt(2) );
	if ( nD_ > 400 ) {
		T2 dZ = nD_ / 1000.; //convert to meter
		T2 dX = ( nX_ - mK_ ( 0, 2 ) ) / mK_ ( 0, 0 ) * dZ;
		T2 dY = ( nY_ - mK_ ( 1, 2 ) ) / mK_ ( 1, 1 ) * dZ;
		( *pVec_ ) << dX + 0.0025, dY, dZ + 0.00499814; // the value is esimated using CCalibrateKinectExtrinsics::calibDepth()
		// 0.0025 by experience.
	}
	else {
		( *pVec_ ) << 0, 0, 0;
	}
}

template< class T >
void projectWorld2Camera ( const Eigen::Matrix< T, 3, 1 >& vPt_, const Eigen::Matrix3d& mK_, Eigen::Matrix< short, 2, 1>* pVec_  )
{
	// this is much faster than the function
	// eiv2DPt = mK * vPt; eiv2DPt /= eiv2DPt(2);
	( *pVec_ ) ( 0 ) = short ( mK_ ( 0, 0 ) * vPt_ ( 0 ) / vPt_ ( 2 ) + mK_ ( 0, 2 ) + 0.5 );
	( *pVec_ ) ( 1 ) = short ( mK_ ( 1, 1 ) * vPt_ ( 1 ) / vPt_ ( 2 ) + mK_ ( 1, 2 ) + 0.5 );
}

template< class T >
void convertPrj2Rnt(const Eigen::Transform< T, 3, Eigen::Affine >& Prj_, Eigen::Matrix< T, 3, 3 >* pR_, Eigen::Matrix< T, 3, 1 >* pT_)
{
	*pR_ = Prj_.linear();
	*pT_ = Prj_.translation();
	return;
}
template< class T >
Eigen::Transform< T, 3, Eigen::Affine > convertRnt2Prj(const Eigen::Matrix< T, 3, 3 >& R_, const Eigen::Matrix< T, 3, 1 >& T_)
{
	Eigen::Transform< T, 3, Eigen::Affine > prj;
	prj.setIdentity();
	prj.linear() = R_;
	prj.translation() = T_;
	return prj;
}
template< class T >
void convertPrjInv2RpnC( const Eigen::Matrix< T, 4, 4 >& Prj_, Eigen::Matrix< T, 3, 3 >* pR_trans_, Eigen::Matrix< T, 3, 1 >* pT_)
{
	*pR_trans_ = Prj_.template block<3,3>(0,0);
	*pT_ = Prj_.template block<3,1>(0,3);
	return;
}
template< class T >
Eigen::Matrix< T, 4, 4 > convertRpnC2PrjInv(  const Eigen::Matrix< T, 3, 3 >& R_trans_, const Eigen::Matrix< T, 3, 1 >& C_ )
{
	Eigen::Matrix< T, 4, 4 > prj;
	prj.setIdentity();
	prj.template block<3,3>(0,0) = R_trans_;
	prj.template block<3,1>(0,3) = C_;
	return prj;
}

template< class T > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
T aoHorn ( const Eigen::Matrix<T,-1,-1,0,-1,-1> & eimXw_, const Eigen::Matrix<T,-1,-1,0,-1,-1>&  eimXc_, const double dThreSecondIter_, const T dDegree_,
 					   bool bEstimateScale_, Eigen::Matrix< T, 3, 3>* pRw_, Eigen::Matrix< T , 3, 1 >* pTw_, T* pdScale_, cv::Mat* pInliers_ ){
	// A is Ref B is Cur
	// eimB_ = R * eimA_ + T;
	// main references: http://www.mathworks.com/matlabcentral/fileexchange/22422-absolute-orientation
	//CHECK ( 	eimXw_.rows() == 3, " absoluteOrientation() requires the input matrix A_ is a 3 x N matrix. " );
	//CHECK ( 	eimXc_.rows() == 3, " absoluteOrientation() requires the input matrix B_ is a 3 x N matrix. " );
	//CHECK ( 	eimXw_.cols() == eimXc_.cols(), " absoluteOrientation() requires the columns of input matrix A_ and B_ are equal. " );

	//Compute the centroid of each point set

	Eigen::Matrix<T,3,1> eivCentroidA(0,0,0), eivCentroidB(0,0,0); //Matrix<float,3,1,0,3,1> = Vector3f
	for ( int nCount = 0; nCount < eimXw_.cols(); nCount++ ){
		eivCentroidA += eimXw_.col ( nCount );
		eivCentroidB += eimXc_.col ( nCount );
	}
	eivCentroidA /= (float)eimXw_.cols();
	eivCentroidB /= (float)eimXw_.cols();
	//PRINT(eimA_.cols());
	//PRINT( nCount );
	//PRINT( eivCentroidA );
	//PRINT( eivCentroidB );

	//Remove the centroid
	float fSquaredSin = sinf(float(M_PI_4)/45.f*dDegree_); 	fSquaredSin *= fSquaredSin;
	Eigen::MatrixXi eimMask ( 1, eimXw_.cols() );
	cv::Mat inliers(1,(int)eimXw_.cols(),CV_32SC1);
	Eigen::Vector3f eimD;
	Eigen::Matrix<T,-1,-1,0,-1,-1> An ( 3, eimXw_.cols() ), Bn ( 3, eimXw_.cols() );
	int nInliers1 = 0;
	for ( int nC = 0; nC < eimXw_.cols(); nC++ ){
		An.col ( nC ) = eimXw_.col ( nC ) - eivCentroidA;
		Bn.col ( nC ) = eimXc_.col ( nC ) - eivCentroidB;

		eimD = An.col ( nC ) - Bn.col ( nC );
		float fDist = eimD.squaredNorm();
		float fDistThresh = eimXc_.col(nC).squaredNorm() * fSquaredSin;
		if ( fDist < fDistThresh ){
			inliers.at<int>(0,nInliers1) = nC;
			nInliers1++;
		}
	}

	cv::Mat mask; inliers.colRange(0,nInliers1).copyTo(mask);
	//PRINT(nInliers1);
	//PRINT( An );
	//PRINT( Bn );
	T dE = 0; // final energy
	int nTotalInlier = -1;
	for(int nIter=0; nIter < 2; nIter++){
		if ( nTotalInlier == 0)	{ break; }
		nTotalInlier = 0;
		//Recompute centroid and
		eivCentroidA.setZero();
		eivCentroidB.setZero();
		for( int i = 0; i< mask.cols; i++){
			int idx = mask.ptr<int>()[i];
			eivCentroidA += eimXw_.col ( idx );
			eivCentroidB += eimXc_.col ( idx );
		}
		eivCentroidA /= (float)mask.cols;
		eivCentroidB /= (float)mask.cols;
		// transform coordinate
		for( int i = 0; i< mask.cols; i++){
			int idx = mask.ptr<int>()[i];
			An.col ( idx ) = eimXw_.col ( idx ) - eivCentroidA;
			Bn.col ( idx ) = eimXc_.col ( idx ) - eivCentroidB;
		}

		//Compute the quaternions
		Eigen::Matrix<T,4,4> M; M.setZero();
		Eigen::Matrix<T,4,4> Ma, Mb;
		for( int i = 0; i< mask.cols; i++){
			int nC = mask.ptr<int>()[i];
			//pure imaginary Shortcuts
			Eigen::Matrix<T,4,1> a(0,0,0,0), b(0,0,0,0);
			a ( 1 ) = An ( 0, nC );
			a ( 2 ) = An ( 1, nC );
			a ( 3 ) = An ( 2, nC );
			b ( 1 ) = Bn ( 0, nC );
			b ( 2 ) = Bn ( 1, nC );
			b ( 3 ) = Bn ( 2, nC );
			//cross products
			Ma << a ( 0 ), -a ( 1 ), -a ( 2 ), -a ( 3 ),
				a ( 1 ),  a ( 0 ),  a ( 3 ), -a ( 2 ),
				a ( 2 ), -a ( 3 ),  a ( 0 ),  a ( 1 ),
				a ( 3 ),  a ( 2 ), -a ( 1 ),  a ( 0 );
			Mb << b ( 0 ), -b ( 1 ), -b ( 2 ), -b ( 3 ),
				b ( 1 ),  b ( 0 ), -b ( 3 ),  b ( 2 ),
				b ( 2 ),  b ( 3 ),  b ( 0 ), -b ( 1 ),
				b ( 3 ), -b ( 2 ),  b ( 1 ),  b ( 0 );
			//Add up
			M += Ma.transpose() * Mb;
		}

		Eigen::EigenSolver <Eigen::Matrix<T,4,4> > eigensolver ( M );
		Eigen::Matrix< std::complex< T >, 4, 1 > v = eigensolver.eigenvalues();

		//find the largest eigenvalue;
		float dLargest = -1000000;
		int n;

		for ( int i = 0; i < 4; i++ ) {
			if ( dLargest < v ( i ).real() ) {
				dLargest = v ( i ).real();
				n = i;
			}
		}

		//PRINT( dLargest );
		//PRINT( n );

		Eigen::Matrix<T,4,1> e;
		e << eigensolver.eigenvectors().col ( n ) ( 0 ).real(),
			 eigensolver.eigenvectors().col ( n ) ( 1 ).real(),
			 eigensolver.eigenvectors().col ( n ) ( 2 ).real(),
			 eigensolver.eigenvectors().col ( n ) ( 3 ).real();

		//PRINT( e );

		Eigen::Matrix<T,4,4>M1, M2, R;
		//Compute the rotation matrix
		M1 <<  e ( 0 ), -e ( 1 ), -e ( 2 ), -e ( 3 ),
			   e ( 1 ),  e ( 0 ),  e ( 3 ), -e ( 2 ),
			   e ( 2 ), -e ( 3 ),  e ( 0 ),  e ( 1 ),
			   e ( 3 ),  e ( 2 ), -e ( 1 ),  e ( 0 );
		M2 <<  e ( 0 ), -e ( 1 ), -e ( 2 ), -e ( 3 ),
			   e ( 1 ),  e ( 0 ), -e ( 3 ),  e ( 2 ),
			   e ( 2 ),  e ( 3 ),  e ( 0 ), -e ( 1 ),
			   e ( 3 ), -e ( 2 ),  e ( 1 ),  e ( 0 );
		R = M1.transpose() * M2;
		( *pRw_ ) = R.block ( 1, 1, 3, 3 );

		//Compute the scale factor if necessary
		if ( bEstimateScale_ ){
			T a = 0, b = 0;
			for( int i = 0; i< mask.cols; i++){
				int nC = mask.ptr<int>()[i];
				a += Bn.col ( nC ).transpose() * ( *pRw_ ) * An.col ( nC );
				b += Bn.col ( nC ).transpose() * Bn.col ( nC );
			}
			//PRINT( a );
			//PRINT( b );
			( *pdScale_ ) = b / a;
		}
		else{
			( *pdScale_ ) = 1;
		}
		//Compute the final translation
		( *pTw_ ) = eivCentroidB - ( *pdScale_ ) * ( *pRw_ ) * eivCentroidA;

		//Compute the residual error
		if(dThreSecondIter_ < 0.) break;
		dE = 0;
		Eigen::Matrix<T,3,1> eivE;
		//recollect inliers
		nTotalInlier = 0;
		for ( int nC = 0; nC < eimXc_.cols(); nC++ ) {
			eivE = eimXc_.col ( nC ) - ( ( *pdScale_ ) * ( *pRw_ ) * eimXw_.col ( nC ) + ( *pTw_ ) );
			T dEN = eivE.norm();
			if( dEN > dThreSecondIter_ ) {
				eimMask(nC) = 0;
			}
			else{
				inliers.at<int>(0,nTotalInlier) = nC;
				nTotalInlier ++;
				dE += dEN;
			}
		}
		inliers.colRange(0,nTotalInlier).copyTo(mask);
		//PRINT(nTotalInlier);
		dE /= nTotalInlier;
	}//for iterations
	mask.copyTo(*pInliers_);
	PRINT(nTotalInlier);
	return dE;
}

enum {
	HORN = 1, // Horn, B. K. P. (1987). Closed-form solution of absolute orientation using unit quaternions. Journal of the Optical Society of America A, 6(4), 422. doi:10.1364/JOSAA.4.000629
	ARUN = 2  // Arun, S. K., Huang, T. S., & Blostein, S. D. (1987). Least-Squares Fitting of Two 3-D Point Sets. In PAMI (pp. 698\96700).
};
//more stable than absoluteOirentationSimple() when the # of pairs are small. It requires minimum 3 non-linear pairs of points to get R,t
//Xw = Rw * Xc + tw; <==> A = Rw * B + tw;
//absoluteOirentationSimple() requires 4.
template< class T > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
T aoHorn ( const Eigen::Matrix<T,-1,-1,0,-1,-1> & eimXw_, const Eigen::Matrix<T,-1,-1,0,-1,-1>&  eimXc_, Eigen::Matrix< T, 3, 3>* pRw_, Eigen::Matrix< T , 3, 1 >* pTw_ ){
	// A is Ref B is Cur
	// eimB_ = R * eimA_ + T;
	// main references: http://www.mathworks.com/matlabcentral/fileexchange/22422-absolute-orientation
	//CHECK ( 	eimXw_.rows() == 3, " absoluteOrientation() requires the input matrix A_ is a 3 x N matrix. " );
	//CHECK ( 	eimXc_.rows() == 3, " absoluteOrientation() requires the input matrix B_ is a 3 x N matrix. " );
	//CHECK ( 	eimXw_.cols() == eimXc_.cols(), " absoluteOrientation() requires the columns of input matrix A_ and B_ are equal. " );

	//Compute the centroid of each point set
	Eigen::Matrix<T,3,1> eivCentroidA(0,0,0), eivCentroidB(0,0,0); //Matrix<float,3,1,0,3,1> = Vector3f
	for ( int nCount = 0; nCount < eimXw_.cols(); nCount++ ){
		eivCentroidA += eimXw_.col ( nCount );
		eivCentroidB += eimXc_.col ( nCount );
	}
	eivCentroidA /= (float)eimXw_.cols();
	eivCentroidB /= (float)eimXw_.cols();
	//PRINT(eimA_.cols());
	//PRINT( nCount );
	//PRINT( eivCentroidA );
	//PRINT( eivCentroidB );
	T dE = 0; // final energy
	int nTotalInlier = 0;

	// transform coordinate
	Eigen::Matrix<T,-1,-1,0,-1,-1> An ( 3, eimXw_.cols() ), Bn ( 3, eimXw_.cols() );
	for ( int nC = 0; nC < eimXw_.cols(); nC++ ){
		An.col ( nC ) = eimXw_.col ( nC ) - eivCentroidA;
		Bn.col ( nC ) = eimXc_.col ( nC ) - eivCentroidB;
	}

	//Compute the quaternions
	Eigen::Matrix<T,4,4> M; M.setZero();
	Eigen::Matrix<T,4,4> Ma, Mb;
	for ( int nC = 0; nC < eimXw_.cols(); nC++ ){
		//pure imaginary Shortcuts
		Eigen::Matrix<T,4,1> a(0,0,0,0), b(0,0,0,0);
		a ( 1 ) = An ( 0, nC );
		a ( 2 ) = An ( 1, nC );
		a ( 3 ) = An ( 2, nC );
		b ( 1 ) = Bn ( 0, nC );
		b ( 2 ) = Bn ( 1, nC );
		b ( 3 ) = Bn ( 2, nC );
		//cross products
		Ma << a ( 0 ), -a ( 1 ), -a ( 2 ), -a ( 3 ),
			a ( 1 ),  a ( 0 ),  a ( 3 ), -a ( 2 ),
			a ( 2 ), -a ( 3 ),  a ( 0 ),  a ( 1 ),
			a ( 3 ),  a ( 2 ), -a ( 1 ),  a ( 0 );
		Mb << b ( 0 ), -b ( 1 ), -b ( 2 ), -b ( 3 ),
			b ( 1 ),  b ( 0 ), -b ( 3 ),  b ( 2 ),
			b ( 2 ),  b ( 3 ),  b ( 0 ), -b ( 1 ),
			b ( 3 ), -b ( 2 ),  b ( 1 ),  b ( 0 );
		//Add up
		M += Ma.transpose() * Mb;
	}

	Eigen::EigenSolver <Eigen::Matrix<T,4,4> > eigensolver ( M );
	Eigen::Matrix< std::complex< T >, 4, 1 > v = eigensolver.eigenvalues();

	//find the largest eigenvalue;
	float dLargest = -1000000;
	int n;

	for ( int i = 0; i < 4; i++ ) {
		if ( dLargest < v ( i ).real() ) {
			dLargest = v ( i ).real();
			n = i;
		}
	}

	//PRINT( dLargest );
	//PRINT( n );

	Eigen::Matrix<T,4,1> e;
	e <<eigensolver.eigenvectors().col ( n ) ( 0 ).real(),
		eigensolver.eigenvectors().col ( n ) ( 1 ).real(),
		eigensolver.eigenvectors().col ( n ) ( 2 ).real(),
		eigensolver.eigenvectors().col ( n ) ( 3 ).real();

	//PRINT( e );

	Eigen::Matrix<T,4,4>M1, M2, R;
	//Compute the rotation matrix
	M1 <<  e ( 0 ), -e ( 1 ), -e ( 2 ), -e ( 3 ),
			e ( 1 ),  e ( 0 ),  e ( 3 ), -e ( 2 ),
			e ( 2 ), -e ( 3 ),  e ( 0 ),  e ( 1 ),
			e ( 3 ),  e ( 2 ), -e ( 1 ),  e ( 0 );
	M2 <<  e ( 0 ), -e ( 1 ), -e ( 2 ), -e ( 3 ),
			e ( 1 ),  e ( 0 ), -e ( 3 ),  e ( 2 ),
			e ( 2 ),  e ( 3 ),  e ( 0 ), -e ( 1 ),
			e ( 3 ), -e ( 2 ),  e ( 1 ),  e ( 0 );
	R = M1.transpose() * M2;
	Eigen::Matrix< T, 3, 3> R_tmp;
	R_tmp = R.block ( 1, 1, 3, 3 );

	//Compute the final translation
	Eigen::Matrix< T, 3, 1> T_tmp;
	T_tmp = eivCentroidB - R_tmp * eivCentroidA;

	//Compute the residual error
	dE = 0;
	Eigen::Matrix<T,3,1> eivE;
	//recollect inliers
	for ( int nC = 0; nC < eimXw_.cols(); nC++ ) {
		eivE = eimXc_.col ( nC ) - ( R_tmp * eimXw_.col ( nC ) + T_tmp );
		T dEN = eivE.norm();
		dE += dEN;
	}
	dE /= eimXw_.cols();

	( *pRw_ ) = R_tmp;
	( *pTw_ ) = T_tmp;
	return dE;
}

template< class T > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
T aoArun ( const Eigen::Matrix<T,-1,-1,0,-1,-1> & eimXw_, const Eigen::Matrix<T,-1,-1,0,-1,-1>&  eimXc_, Eigen::Matrix< T, 3, 3>* pR_, Eigen::Matrix< T , 3, 1 >* pT_ ){
	// A is from World  B is Local coordinate system
	// eimXc_ = R * eimXw_ + T; //R and t is defined in world and transform a point in world to local
	// main references: http://www.mathworks.com/matlabcentral/fileexchange/22422-absolute-orientation
	using namespace Eigen;
	//CHECK ( 	eimXw_.rows() == 3, " absoluteOrientation() requires the input matrix A_ is a 3 x N matrix. " );
	//CHECK ( 	eimXc_.rows() == 3, " absoluteOrientation() requires the input matrix B_ is a 3 x N matrix. " );
	//CHECK ( 	eimXw_.cols() == eimXc_.cols(), " absoluteOrientation() requires the columns of input matrix A_ and B_ are equal. " );

	//Compute the centroid of each point set
	Eigen::Matrix<double, 3,1> eivCentroidW(0,0,0), eivCentroidC(0,0,0); //Matrix<float,3,1,0,3,1> = Vector3f
	for ( int nCount = 0; nCount < eimXw_.cols(); nCount++ ){
		eivCentroidW += eimXw_.col ( nCount ).template cast<double>();
		eivCentroidC += eimXc_.col ( nCount ).template cast<double>();
	}
	eivCentroidW /= (double)eimXw_.cols();
	eivCentroidC /= (double)eimXw_.cols();
	//PRINT(eimA_.cols());
	//PRINT( nCount );
	//PRINT( eivCentroidA );
	//PRINT( eivCentroidB );

	// transform coordinate
	Eigen::Matrix<T,-1,-1,0,-1,-1> Aw ( 3, eimXw_.cols() ), Ac ( 3, eimXw_.cols() );
	for ( int nC = 0; nC < eimXw_.cols(); nC++ ){
		Aw.col ( nC ) = eimXw_.col ( nC ) - eivCentroidW.template cast<float>();
		Ac.col ( nC ) = eimXc_.col ( nC ) - eivCentroidC.template cast<float>();
	}

	//Compute the quaternions
	Eigen::Matrix<double,3,3> M; M.setZero();
	Eigen::Matrix<T,3,3> N;
	//M=M+x3d_c(i,:)'*x3d_w(i,:);
	for ( int nC = 0; nC < eimXw_.cols(); nC++ ){
		//calc the N = x3d_c(i,:)'*x3d_w(i,:);
		N = Ac.col( nC ) * Aw.col( nC ).transpose();
		M += N.template cast<double>();
	}

	JacobiSVD<Eigen::Matrix<double,-1,-1,0,-1,-1> > svd(M, ComputeFullU | ComputeFullV );
	//[U S V]=svd(M);
	//R=U*V';
	Matrix<double, 3, 3> U = svd.matrixU();
	Matrix<double, 3, 3> V = svd.matrixV();
	Matrix<double, 3, 3> R_tmp;
	if( U.determinant()*V.determinant() < 0){
		Matrix<double, 3, 3> I = Matrix<double, 3, 3>::Identity(); I(2, 2) = -1;
		R_tmp = U*I*V.transpose();
	}
	else{
		R_tmp = U*V.transpose();
	}
	//T=ccent'-R*wcent';
	Eigen::Matrix< double, 3, 1> T_tmp = eivCentroidC - R_tmp * eivCentroidW;

	( *pR_ ) = R_tmp.template cast<T>();
	( *pT_ ) = T_tmp.template cast<T>();
	//recollect inliers
	T dE = 0; // final energy
	for ( int nC = 0; nC < eimXw_.cols(); nC++ ) {
		Eigen::Matrix< T, 3, 1> eivE = eimXc_.col ( nC ) - ( ( *pR_ ) * eimXw_.col ( nC ) + ( *pT_ ) );
		dE += eivE.norm();
	}
	dE /= eimXw_.cols();

	return dE;
}

template< class T > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
T aoRansac ( const Eigen::Matrix<T,-1,-1,0,-1,-1> & eimXw_, const Eigen::Matrix<T,-1,-1,0,-1,-1>&  eimXc_,
			const T dist_thre_, const int Iter,
			Eigen::Matrix< T, 3, 3>* pR_, Eigen::Matrix< T , 3, 1 >* pT_, cv::Mat * pInliers_, int nMethod_ = HORN ){
	// A is from World  B is Local coordinate system
	// eimB_ = R * eimA_ + T; //R and t is defined in world and transform a point in world to local
	// main references: http://www.mathworks.com/matlabcentral/fileexchange/22422-absolute-orientation
	using namespace Eigen;
	using namespace cv;
	//CHECK (	eimXw_.rows() == 3, " absoluteOrientation() requires the input matrix A_ is a 3 x N matrix. " );
	//CHECK (	eimXc_.rows() == 3, " absoluteOrientation() requires the input matrix B_ is a 3 x N matrix. " );
	//CHECK (	eimXw_.cols() == eimXc_.cols(), " absoluteOrientation() requires the columns of input matrix A_ and B_ are equal. " );

	RandomElements<int> re( (int)eimXw_.cols() );
	const int K = 3;
	vector<vector<int> > _v_v_inlier_idx;

	cv::Mat all_votes(1,Iter,CV_32FC1);
	for (int i=0; i < Iter; i++)	{
		//randomly select K candidates
		vector<int> selected_cols;
		re.run(K, &selected_cols);
		Matrix<T,-1,-1,0,-1,-1> eimX_world(3,K),eimX_cam(3,K);
		for (int c=0; c<K; c++) {
			eimX_world.col( c ) = eimXw_.col( selected_cols[c] );
			eimX_cam.col( c )   = eimXc_.col( selected_cols[c] );
		}
		//calc R&t
		Eigen::Matrix< T, 3, 3> R_tmp;
		Eigen::Matrix< T, 3, 1> T_tmp;
		if( nMethod_ == HORN )
			aoHorn<T>(eimX_world,eimX_cam,&R_tmp,&T_tmp);
		else
			aoArun<T>(eimX_world,eimX_cam,&R_tmp,&T_tmp);
		//collect votes
		int nVotes = 0;
		vector<int> inlier_idx;
		for (int c=0; c < eimXw_.cols(); c++  )	{
			Eigen::Matrix< T, 3, 1> eivE = eimXc_.col ( c ) - ( R_tmp * eimXw_.col ( c ) + T_tmp );
			if ( eivE.norm() < dist_thre_ ){
				inlier_idx.push_back( c );
				nVotes ++;
			}
		}
		all_votes.at<int>( 0,i ) = nVotes;
		_v_v_inlier_idx.push_back( inlier_idx );
	}
	cv::Mat most;
	sortIdx(all_votes,most, CV_SORT_DESCENDING + CV_SORT_EVERY_ROW );
	//collect inliers
	////////////////////////////////////////////////////////////////
	int best_idx = most.at<int>(0,0);
	cout << ( all_votes.at<int>( 0,best_idx) ) << endl;
	vector<int>& v_inlier_idx = _v_v_inlier_idx[best_idx];
	pInliers_->create(1,(int)v_inlier_idx.size(),CV_32SC1);
	Matrix<T,-1,-1,0,-1,-1> eimX_world_selected( 3,v_inlier_idx.size() ),    eimX_cam_selected(3,v_inlier_idx.size());

	for ( int i=0; i< v_inlier_idx.size(); i++)
	{
		eimX_world_selected.col(i) = eimXw_.col( v_inlier_idx[i] );
		eimX_cam_selected.col(i) = eimXc_.col( v_inlier_idx[i] );
		pInliers_->ptr<int>()[i] = v_inlier_idx[i];
	}

	Eigen::Matrix< T, 3, 3> R_tmp;
	Eigen::Matrix< T, 3, 1> T_tmp;
	T dE;
	if( nMethod_ == HORN )
		dE = aoHorn<T>( eimX_world_selected, eimX_cam_selected, &R_tmp, &T_tmp);
	else
		dE = aoArun<T>( eimX_world_selected, eimX_cam_selected, &R_tmp, &T_tmp);

	*pR_ = R_tmp;
	*pT_ = T_tmp;

	return dE;
}

template< class T > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
Eigen::Matrix<T,2,1> absoluteOrientationWithNormal ( const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimXw_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimNlw_,
													 const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimXc_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimNlc_, Eigen::Matrix< T, 3, 3>* pRw_, Eigen::Matrix< T , 3, 1 >* pTw_ ){
	using namespace Eigen;
	// A is Ref B is Cur
	// eimB_ = R * eimA_ + T;
	// main references: http://www.mathworks.com/matlabcentral/fileexchange/22422-absolute-orientation
	//CHECK ( 	eimXw_.rows() == 3, " absoluteOrientation() requires the input matrix A_ is a 3 x N matrix. " );
	//CHECK ( 	eimXc_.rows() == 3, " absoluteOrientation() requires the input matrix B_ is a 3 x N matrix. " );
	//CHECK ( 	eimXw_.cols() == eimXc_.cols(), " absoluteOrientation() requires the columns of input matrix A_ and B_ are equal. " );

	//Compute the centroid of each point set
	Eigen::Matrix<T,3,1> eivCentroidA(0,0,0), eivCentroidB(0,0,0); //Matrix<float,3,1,0,3,1> = Vector3f
	for ( int nCount = 0; nCount < eimXw_.cols(); nCount++ ){
		eivCentroidA += eimXw_.col ( nCount );
		eivCentroidB += eimXc_.col ( nCount );
	}
	eivCentroidA /= (float)eimXw_.cols();
	eivCentroidB /= (float)eimXw_.cols();
	//PRINT(eimA_.cols());
	//PRINT( nCount );
	//PRINT( eivCentroidA );
	//PRINT( eivCentroidB );
	int nTotalInlier = 0;

	// transform coordinate
	Eigen::Matrix<T,-1,-1,0,-1,-1> An ( 3, eimXw_.cols() ), Bn ( 3, eimXw_.cols() );
	for ( int nC = 0; nC < eimXw_.cols(); nC++ ){
		An.col ( nC ) = eimXw_.col ( nC ) - eivCentroidA;
		Bn.col ( nC ) = eimXc_.col ( nC ) - eivCentroidB;
	}

	//Compute the quaternions
	Eigen::Matrix<T,4,4> M; M.setZero();
	Eigen::Matrix<T,4,4> Ma, Mb;
	T forNormalisation = 0.f;
	for ( int nC = 0; nC < eimXw_.cols(); nC++ ){
		//pure imaginary Shortcuts
		Eigen::Matrix<T,4,1> a(0,0,0,0), b(0,0,0,0);
		a ( 1 ) = An ( 0, nC );
		a ( 2 ) = An ( 1, nC );
		a ( 3 ) = An ( 2, nC );
		b ( 1 ) = Bn ( 0, nC );
		b ( 2 ) = Bn ( 1, nC );
		b ( 3 ) = Bn ( 2, nC );
		forNormalisation += An.col( nC ).norm();
		forNormalisation += Bn.col( nC ).norm();
		//cross products
		Ma << a ( 0 ), -a ( 1 ), -a ( 2 ), -a ( 3 ),
			a ( 1 ),  a ( 0 ),  a ( 3 ), -a ( 2 ),
			a ( 2 ), -a ( 3 ),  a ( 0 ),  a ( 1 ),
			a ( 3 ),  a ( 2 ), -a ( 1 ),  a ( 0 );
		Mb << b ( 0 ), -b ( 1 ), -b ( 2 ), -b ( 3 ),
			b ( 1 ),  b ( 0 ), -b ( 3 ),  b ( 2 ),
			b ( 2 ),  b ( 3 ),  b ( 0 ), -b ( 1 ),
			b ( 3 ), -b ( 2 ),  b ( 1 ),  b ( 0 );
		//Add up
		M += Ma.transpose() * Mb;
	}

	forNormalisation /= ( An.cols() * 2 );
	//PRINT( forNormalisation );
	for ( int nC = 0; nC < eimNlw_.cols(); nC++ ){
		//pure imaginary Shortcuts
		Eigen::Matrix<T,4,1> a(0,0,0,0), b(0,0,0,0);
		a ( 1 ) = eimNlw_ ( 0, nC );
		a ( 2 ) = eimNlw_ ( 1, nC );
		a ( 3 ) = eimNlw_ ( 2, nC );
		a *= forNormalisation;
		b ( 1 ) = eimNlc_ ( 0, nC );
		b ( 2 ) = eimNlc_ ( 1, nC );
		b ( 3 ) = eimNlc_ ( 2, nC );
		b *= forNormalisation;
		//cross products
		Ma << a ( 0 ), -a ( 1 ), -a ( 2 ), -a ( 3 ),
			a ( 1 ),  a ( 0 ),  a ( 3 ), -a ( 2 ),
			a ( 2 ), -a ( 3 ),  a ( 0 ),  a ( 1 ),
			a ( 3 ),  a ( 2 ), -a ( 1 ),  a ( 0 );
		Mb << b ( 0 ), -b ( 1 ), -b ( 2 ), -b ( 3 ),
			b ( 1 ),  b ( 0 ), -b ( 3 ),  b ( 2 ),
			b ( 2 ),  b ( 3 ),  b ( 0 ), -b ( 1 ),
			b ( 3 ), -b ( 2 ),  b ( 1 ),  b ( 0 );
		//Add up
		M += Ma.transpose() * Mb;
	}

	Eigen::EigenSolver <Eigen::Matrix<T,4,4> > eigensolver ( M );
	Eigen::Matrix< std::complex< T >, 4, 1 > v = eigensolver.eigenvalues();

	//find the largest eigenvalue;
	float dLargest = -1000000;
	int n;

	for ( int i = 0; i < 4; i++ ) {
		if ( dLargest < v ( i ).real() ) {
			dLargest = v ( i ).real();
			n = i;
		}
	}

	//PRINT( dLargest );
	//PRINT( n );

	Eigen::Matrix<T,4,1> e;
	e <<eigensolver.eigenvectors().col ( n ) ( 0 ).real(),
		eigensolver.eigenvectors().col ( n ) ( 1 ).real(),
		eigensolver.eigenvectors().col ( n ) ( 2 ).real(),
		eigensolver.eigenvectors().col ( n ) ( 3 ).real();

	//PRINT( e );

	Eigen::Matrix<T,4,4>M1, M2, R;
	//Compute the rotation matrix
	M1 <<  e ( 0 ), -e ( 1 ), -e ( 2 ), -e ( 3 ),
		e ( 1 ),  e ( 0 ),  e ( 3 ), -e ( 2 ),
		e ( 2 ), -e ( 3 ),  e ( 0 ),  e ( 1 ),
		e ( 3 ),  e ( 2 ), -e ( 1 ),  e ( 0 );
	M2 <<  e ( 0 ), -e ( 1 ), -e ( 2 ), -e ( 3 ),
		e ( 1 ),  e ( 0 ), -e ( 3 ),  e ( 2 ),
		e ( 2 ),  e ( 3 ),  e ( 0 ), -e ( 1 ),
		e ( 3 ), -e ( 2 ),  e ( 1 ),  e ( 0 );
	R = M1.transpose() * M2;
	Eigen::Matrix< T, 3, 3> R_tmp;
	R_tmp = R.block ( 1, 1, 3, 3 );

	//Compute the final translation
	Eigen::Matrix< T, 3, 1> T_tmp;
	T_tmp = eivCentroidB - R_tmp * eivCentroidA;

	//Compute the residual error
	T dE = 0; // final energy
	Eigen::Matrix<T,3,1> eivE;

	//recollect inliers
	for ( int nC = 0; nC < eimXw_.cols(); nC++ ) {
		eivE = eimXc_.col ( nC ) - ( R_tmp * eimXw_.col ( nC ) + T_tmp );
		T dEN = eivE.norm();
		dE += dEN;
	}
	dE /= eimXw_.cols();

	T dA = 0;
	for ( int nC = 0; nC < eimNlw_.cols(); nC++ ) {
		T dEN = eimNlc_.col( nC ).dot( R_tmp * eimNlw_.col ( nC ) );
		dEN = dEN > 1.f ? 1.f : dEN;
		dEN = dEN <-1.f ?-1.f : dEN;
		dA += acos( dEN );
	}
	dA /= eimNlw_.cols();

	( *pRw_ ) = R_tmp;
	( *pTw_ ) = T_tmp;

	Matrix<T,2,1> eivEA( dE, dA );
	return eivEA;
}

template< class T > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
Eigen::Matrix<T,2,1> absoluteOrientationWithNormalnMainDirection ( const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimXw_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimNlw_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimMDw_,
											  const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimXc_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimNlc_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimMDc_,
											  Eigen::Matrix< T, 3, 3>* pRw_, Eigen::Matrix< T , 3, 1 >* pTw_ ){
	using namespace Eigen;
	// A is Ref B is Cur
	// eimB_ = R * eimA_ + T;
	// main references: http://www.mathworks.com/matlabcentral/fileexchange/22422-absolute-orientation
	//CHECK ( 	eimXw_.rows() == 3, " absoluteOrientation() requires the input matrix A_ is a 3 x N matrix. " );
	//CHECK ( 	eimXc_.rows() == 3, " absoluteOrientation() requires the input matrix B_ is a 3 x N matrix. " );
	//CHECK ( 	eimXw_.cols() == eimXc_.cols(), " absoluteOrientation() requires the columns of input matrix A_ and B_ are equal. " );

	//Compute the centroid of each point set
	Eigen::Matrix<T,3,1> eivCentroidA(0,0,0), eivCentroidB(0,0,0); //Matrix<float,3,1,0,3,1> = Vector3f
	for ( int nCount = 0; nCount < eimXw_.cols(); nCount++ ){
		eivCentroidA += eimXw_.col ( nCount );
		eivCentroidB += eimXc_.col ( nCount );
	}
	eivCentroidA /= (float)eimXw_.cols();
	eivCentroidB /= (float)eimXw_.cols();
	//PRINT(eimA_.cols());
	//PRINT( nCount );
	//PRINT( eivCentroidA );
	//PRINT( eivCentroidB );
	int nTotalInlier = 0;

	// transform coordinate
	Eigen::Matrix<T,-1,-1,0,-1,-1> An ( 3, eimXw_.cols() ), Bn ( 3, eimXw_.cols() );
	for ( int nC = 0; nC < eimXw_.cols(); nC++ ){
		An.col ( nC ) = eimXw_.col ( nC ) - eivCentroidA;
		Bn.col ( nC ) = eimXc_.col ( nC ) - eivCentroidB;
	}

	//Compute the quaternions
	Eigen::Matrix<T,4,4> M; M.setZero();
	Eigen::Matrix<T,4,4> Ma, Mb;
	T forNormalisation = 0.f;
	for ( int nC = 0; nC < eimXw_.cols(); nC++ ){
		//pure imaginary Shortcuts
		Eigen::Matrix<T,4,1> a(0,0,0,0), b(0,0,0,0);
		a ( 1 ) = An ( 0, nC );
		a ( 2 ) = An ( 1, nC );
		a ( 3 ) = An ( 2, nC );
		b ( 1 ) = Bn ( 0, nC );
		b ( 2 ) = Bn ( 1, nC );
		b ( 3 ) = Bn ( 2, nC );
		forNormalisation += An.col( nC ).norm();
		forNormalisation += Bn.col( nC ).norm();
		//cross products
		Ma << a ( 0 ), -a ( 1 ), -a ( 2 ), -a ( 3 ),
			a ( 1 ),  a ( 0 ),  a ( 3 ), -a ( 2 ),
			a ( 2 ), -a ( 3 ),  a ( 0 ),  a ( 1 ),
			a ( 3 ),  a ( 2 ), -a ( 1 ),  a ( 0 );
		Mb << b ( 0 ), -b ( 1 ), -b ( 2 ), -b ( 3 ),
			b ( 1 ),  b ( 0 ), -b ( 3 ),  b ( 2 ),
			b ( 2 ),  b ( 3 ),  b ( 0 ), -b ( 1 ),
			b ( 3 ), -b ( 2 ),  b ( 1 ),  b ( 0 );
		//Add up
		M += Ma.transpose() * Mb;
	}

	forNormalisation /= ( An.cols() * 2 );
	//PRINT( forNormalisation );
	for ( int nC = 0; nC < eimNlw_.cols(); nC++ ){
		//pure imaginary Shortcuts
		Eigen::Matrix<T,4,1> a(0,0,0,0), b(0,0,0,0);
		a ( 1 ) = eimNlw_ ( 0, nC );
		a ( 2 ) = eimNlw_ ( 1, nC );
		a ( 3 ) = eimNlw_ ( 2, nC );
		a *= forNormalisation;
		b ( 1 ) = eimNlc_ ( 0, nC );
		b ( 2 ) = eimNlc_ ( 1, nC );
		b ( 3 ) = eimNlc_ ( 2, nC );
		b *= forNormalisation;
		//cross products
		Ma << a ( 0 ), -a ( 1 ), -a ( 2 ), -a ( 3 ),
			a ( 1 ),  a ( 0 ),  a ( 3 ), -a ( 2 ),
			a ( 2 ), -a ( 3 ),  a ( 0 ),  a ( 1 ),
			a ( 3 ),  a ( 2 ), -a ( 1 ),  a ( 0 );
		Mb << b ( 0 ), -b ( 1 ), -b ( 2 ), -b ( 3 ),
			b ( 1 ),  b ( 0 ), -b ( 3 ),  b ( 2 ),
			b ( 2 ),  b ( 3 ),  b ( 0 ), -b ( 1 ),
			b ( 3 ), -b ( 2 ),  b ( 1 ),  b ( 0 );
		//Add up
		M += Ma.transpose() * Mb;
	}
	float fWeightMD = 0;
	for ( int nC = 0; nC < eimMDw_.cols(); nC++ ){
		//pure imaginary Shortcuts
		Eigen::Matrix<T,4,1> a(0,0,0,0), b(0,0,0,0);
		a ( 1 ) = eimMDw_ ( 0, nC );
		a ( 2 ) = eimMDw_ ( 1, nC );
		a ( 3 ) = eimMDw_ ( 2, nC );
		a *= fWeightMD;
		b ( 1 ) = eimMDc_ ( 0, nC );
		b ( 2 ) = eimMDc_ ( 1, nC );
		b ( 3 ) = eimMDc_ ( 2, nC );
		b *= fWeightMD;
		//cross products
		Ma << a ( 0 ), -a ( 1 ), -a ( 2 ), -a ( 3 ),
			a ( 1 ),  a ( 0 ),  a ( 3 ), -a ( 2 ),
			a ( 2 ), -a ( 3 ),  a ( 0 ),  a ( 1 ),
			a ( 3 ),  a ( 2 ), -a ( 1 ),  a ( 0 );
		Mb << b ( 0 ), -b ( 1 ), -b ( 2 ), -b ( 3 ),
			b ( 1 ),  b ( 0 ), -b ( 3 ),  b ( 2 ),
			b ( 2 ),  b ( 3 ),  b ( 0 ), -b ( 1 ),
			b ( 3 ), -b ( 2 ),  b ( 1 ),  b ( 0 );
		//Add up
		M += Ma.transpose() * Mb;
	}

	Eigen::EigenSolver <Eigen::Matrix<T,4,4> > eigensolver ( M );
	Eigen::Matrix< std::complex< T >, 4, 1 > v = eigensolver.eigenvalues();

	//find the largest eigenvalue;
	float dLargest = -1000000;
	int n;

	for ( int i = 0; i < 4; i++ ) {
		if ( dLargest < v ( i ).real() ) {
			dLargest = v ( i ).real();
			n = i;
		}
	}

	//PRINT( dLargest );
	//PRINT( n );

	Eigen::Matrix<T,4,1> e;
	e <<eigensolver.eigenvectors().col ( n ) ( 0 ).real(),
		eigensolver.eigenvectors().col ( n ) ( 1 ).real(),
		eigensolver.eigenvectors().col ( n ) ( 2 ).real(),
		eigensolver.eigenvectors().col ( n ) ( 3 ).real();

	//PRINT( e );

	Eigen::Matrix<T,4,4>M1, M2, R;
	//Compute the rotation matrix
	M1 <<  e ( 0 ), -e ( 1 ), -e ( 2 ), -e ( 3 ),
		e ( 1 ),  e ( 0 ),  e ( 3 ), -e ( 2 ),
		e ( 2 ), -e ( 3 ),  e ( 0 ),  e ( 1 ),
		e ( 3 ),  e ( 2 ), -e ( 1 ),  e ( 0 );
	M2 <<  e ( 0 ), -e ( 1 ), -e ( 2 ), -e ( 3 ),
		e ( 1 ),  e ( 0 ), -e ( 3 ),  e ( 2 ),
		e ( 2 ),  e ( 3 ),  e ( 0 ), -e ( 1 ),
		e ( 3 ), -e ( 2 ),  e ( 1 ),  e ( 0 );
	R = M1.transpose() * M2;
	Eigen::Matrix< T, 3, 3> R_tmp;
	R_tmp = R.block ( 1, 1, 3, 3 );

	//Compute the final translation
	Eigen::Matrix< T, 3, 1> T_tmp;
	T_tmp = eivCentroidB - R_tmp * eivCentroidA;

	//Compute the residual error
	T dE = 0; // final energy
	Eigen::Matrix<T,3,1> eivE;

	//recollect inliers
	for ( int nC = 0; nC < eimXw_.cols(); nC++ ) {
		eivE = eimXc_.col ( nC ) - ( R_tmp * eimXw_.col ( nC ) + T_tmp );
		T dEN = eivE.norm();
		dE += dEN;
	}
	dE /= eimXw_.cols();

	T dA = 0;
	for ( int nC = 0; nC < eimNlw_.cols(); nC++ ) {
		//normal
		{
			T dEN = eimNlc_.col( nC ).dot( R_tmp * eimNlw_.col ( nC ) );
			dEN = dEN > 1.f ? 1.f : dEN;
			dEN = dEN <-1.f ?-1.f : dEN;
			dA += acos( dEN );
		}
		//main direction
		{
			T dEN = eimMDc_.col( nC ).dot( R_tmp * eimMDw_.col ( nC ) );
			dEN = dEN > 1.f ? 1.f : dEN;
			dEN = dEN <-1.f ?-1.f : dEN;
			dA += acos( dEN );
		}
	}
	dA /= (eimNlw_.cols()*2);

	( *pRw_ ) = R_tmp;
	( *pTw_ ) = T_tmp;

	Matrix<T,2,1> eivEA( dE, dA );
	return eivEA;
}

template< class T > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
Eigen::Matrix<T,2,1> p1p_3d_nl_md ( const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimXw_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimNlw_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimMDw_,
											  const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimXc_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimNlc_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimMDc_,
											  Eigen::Matrix< T, 3, 3>* pRw_, Eigen::Matrix< T , 3, 1 >* pTw_ ){

	Eigen::Matrix< T , 3, 1 > Xw = eimXw_.col(0);
	Eigen::Matrix< T , 3, 1 > Xc = eimXc_.col(0);

	Eigen::Matrix< T , 3, 1 > Nlw = eimNlw_.col(0);
	Eigen::Matrix< T , 3, 1 > Nlc = eimNlc_.col(0);

	Eigen::Matrix< T , 3, 1 > Mdw = eimMDw_.col(0);
	Eigen::Matrix< T , 3, 1 > Mdc = eimMDc_.col(0);

	Eigen::Matrix< T , 3, 1 > nmw = Nlw.cross(Mdw);
	Eigen::Matrix< T , 3, 1 > nmc = Nlc.cross(Mdc);

	Eigen::Matrix< T , 3, 3 > Rc;
	Rc.col(0) = Nlc;
	Rc.col(1) = Mdc;
	Rc.col(2) = nmc;

	Eigen::Matrix< T , 3, 3 > Rw;
	Rw.col(0) = Nlw;
	Rw.col(1) = Mdw;
	Rw.col(2) = nmw;

	Eigen::Matrix< T , 3, 3 >  Rcw = Rw.transpose()*Rc;
	*pRw_ = Rcw;
	*pTw_ = Xc - Rcw*Xw;

	Matrix<T,2,1> eivEA( 0, 0 );
	return eivEA;
}

template< class T > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
Eigen::Matrix<T,2,1> aoWithNormalRansac ( const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimXw_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimNlw_,
														   const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimXc_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimNlc_,
														   const T dist_thre_, const T angle_thre_, const int Iter,
														   Eigen::Matrix< T, 3, 3>* pR_, Eigen::Matrix< T , 3, 1 >* pT_, cv::Mat * pInliers_ ){
	// A is from World  B is Local coordinate system
	// eimB_ = R * eimA_ + T; //R and t is defined in world and transform a point in world to local
	// main references: http://www.mathworks.com/matlabcentral/fileexchange/22422-absolute-orientation
	using namespace Eigen;
	using namespace cv;
	//CHECK ( 	eimXw_.rows() == 3, " absoluteOrientation() requires the input matrix A_ is a 3 x N matrix. " );
	//CHECK ( 	eimXc_.rows() == 3, " absoluteOrientation() requires the input matrix B_ is a 3 x N matrix. " );
	//CHECK ( 	eimXw_.cols() == eimXc_.cols(), " absoluteOrientation() requires the columns of input matrix A_ and B_ are equal. " );

	RandomElements<int> re( (int)eimXw_.cols() );
	const int K = 2;
	vector<vector<int> > _v_v_inlier_idx;

	cv::Mat all_votes(1,Iter,CV_32FC1);
	for (int i=0; i < Iter; i++)	{
		//randomly select K candidates
		vector<int> selected_cols;
		re.run(K, &selected_cols);
		Matrix<T,-1,-1,0,-1,-1> eimX_world(3,K),eimNl_world(3,K),eimX_cam(3,K),eimNl_cam(3,K);
		for (int c=0; c<K; c++) {
			eimX_world.col( c )  = eimXw_.col( selected_cols[c] );
			eimX_cam.col( c )    = eimXc_.col( selected_cols[c] );
			eimNl_world.col( c ) = eimNlw_.col( selected_cols[c] );
			eimNl_cam.col( c )   = eimNlc_.col( selected_cols[c] );
		}
		//calc R&t
		Eigen::Matrix< T, 3, 3> R_tmp;
		Eigen::Matrix< T, 3, 1> T_tmp;
		absoluteOrientationWithNormal<T>( eimX_world,eimNl_world,
										  eimX_cam, eimNl_cam, &R_tmp,&T_tmp );
		//collect votes
		int nVotes = 0;
		vector<int> inlier_idx;
		for (int c=0; c < eimXw_.cols(); c++  )	{
			Eigen::Matrix< T, 3, 1> eivE = eimXc_.col ( c ) - ( R_tmp * eimXw_.col ( c ) + T_tmp );
			T dCosA = eimNlc_.col( c ).dot( R_tmp * eimNlw_.col ( c ) );
			dCosA = dCosA > 1.f ? 1.f : dCosA;	dCosA = dCosA <-1.f ?-1.f : dCosA;
			T dA = acos( dCosA );
			if ( eivE.norm() < dist_thre_ && dA < angle_thre_ ){
				inlier_idx.push_back( c );
				nVotes ++;
			}
		}
		all_votes.at<int>( 0,i ) = nVotes;
		_v_v_inlier_idx.push_back( inlier_idx );
	}
	cv::Mat most;
	sortIdx(all_votes,most, CV_SORT_DESCENDING + CV_SORT_EVERY_ROW );
	//collect inliers
	////////////////////////////////////////////////////////////////
	int best_idx = most.at<int>(0,0);
	PRINT( all_votes.at<int>( 0,best_idx) );

	vector<int>& v_inlier_idx = _v_v_inlier_idx[best_idx];
	pInliers_->create(1,(int)v_inlier_idx.size(),CV_32SC1);
	Matrix<T,-1,-1,0,-1,-1> eimX_world_selected( 3,v_inlier_idx.size() ),   eimNl_world_selected( 3,v_inlier_idx.size() ),
		                    eimX_cam_selected(3,v_inlier_idx.size()),       eimNl_cam_selected( 3,v_inlier_idx.size() );

	for ( int i=0; i< v_inlier_idx.size(); i++)
	{
		eimX_world_selected.col(i)  = eimXw_.col( v_inlier_idx[i] );
		eimNl_world_selected.col(i) = eimNlw_.col( v_inlier_idx[i] );
		eimX_cam_selected.col(i)  = eimXc_.col( v_inlier_idx[i] );
		eimNl_cam_selected.col(i) = eimNlc_.col( v_inlier_idx[i] );
		pInliers_->ptr<int>()[i] = v_inlier_idx[i];
	}

	Eigen::Matrix< T, 3, 3> R_tmp;
	Eigen::Matrix< T, 3, 1> T_tmp;
	Eigen::Matrix< T, 2, 1> dEA = absoluteOrientationWithNormal<T>( eimX_world_selected, eimNl_world_selected, eimX_cam_selected, eimNl_cam_selected, &R_tmp, &T_tmp);

	*pR_ = R_tmp;
	*pT_ = T_tmp;

	return dEA;
}

template< class T > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
Eigen::Matrix<T,2,1> aoWithNormalWith2dConstraintRansac ( const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimXw_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimNlw_,
														   const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimXc_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimNlc_,
														   const T dist_thre_, const T angle_thre_, const T visual_angle_thre_, const int Iter,
														   Eigen::Matrix< T, 3, 3>* pR_, Eigen::Matrix< T , 3, 1 >* pT_, cv::Mat * pInliers_ ){
	// A is from World  B is Local coordinate system
	// eimB_ = R * eimA_ + T; //R and t is defined in world and transform a point in world to local
	// main references: http://www.mathworks.com/matlabcentral/fileexchange/22422-absolute-orientation
	using namespace Eigen;
	using namespace cv;
	//CHECK ( 	eimXw_.rows() == 3, " absoluteOrientation() requires the input matrix A_ is a 3 x N matrix. " );
	//CHECK ( 	eimXc_.rows() == 3, " absoluteOrientation() requires the input matrix B_ is a 3 x N matrix. " );
	//CHECK ( 	eimXw_.cols() == eimXc_.cols(), " absoluteOrientation() requires the columns of input matrix A_ and B_ are equal. " );

	RandomElements<int> re( (int)eimXw_.cols() );
	const int K = 2;
	vector<vector<int> > _v_v_inlier_idx;

	//vector<vector<int> > _v_v_selected_cols;

	vector<Matrix3f> R_tmp; R_tmp.resize(Iter);//[Iter];
	vector<Vector3f> T_tmp; T_tmp.resize(Iter);//[Iter];

	//Matrix3f R_tmp[Iter];
	//Vector3f T_tmp[Iter];

	cv::Mat all_votes(1,Iter,CV_32FC1);

	//double t = (double)getTickCount() ;
	Matrix<T ,-1,-1,0,-1,-1> eimX_world(3,K),eimNl_world(3,K),eimX_cam(3,K),eimNl_cam(3,K);
	for (int i=0; i < Iter; i++)	{
		//randomly select K candidates
		vector<int> selected_cols;
		re.run(K, &selected_cols);
		for (int c=0; c<K; c++) {
			eimX_world.col( c )  = eimXw_.col( selected_cols[c] );
			eimX_cam.col( c )    = eimXc_.col( selected_cols[c] );
			eimNl_world.col( c ) = eimNlw_.col( selected_cols[c] );
			eimNl_cam.col( c )   = eimNlc_.col( selected_cols[c] );
		}
		//calc R&t
		//Eigen::Matrix< T , 3, 3> R_tmp;
		//Eigen::Matrix< T , 3, 1> T_tmp;
		absoluteOrientationWithNormal<T>( eimX_world,eimNl_world,
										  eimX_cam, eimNl_cam, &(R_tmp[i]),&(T_tmp[i]) );
	}
	/*t = ((double)getTickCount() - t)/getTickFrequency();
	cout << "1: " << t << endl;
	t = (double)getTickCount();*/

	Eigen::Matrix< T , 3, 1> eivXc_tmp;
	Eigen::Matrix< T , 3, 1> eivE;
	for (int i=0; i < Iter; i++)	{
		//collect votes
		int nVotes = 0;
		vector<int> inlier_idx;
		for (int c=0; c < eimXw_.cols(); c++  )	{
			eivXc_tmp = R_tmp[i] * eimXw_.col ( c ) + T_tmp[i];
			//dist constraint
			eivE = eimXc_.col ( c ) - eivXc_tmp;
			//visual angle constraint
			T dCosVisualAngle = eimXc_.col ( c ).dot( eivXc_tmp )/eimXc_.col ( c ).norm()/eivXc_tmp.norm();
			dCosVisualAngle = dCosVisualAngle > 1.f ? 1.f : dCosVisualAngle;	dCosVisualAngle = dCosVisualAngle <-1.f ?-1.f : dCosVisualAngle;
			T dVisualAngle = acos( dCosVisualAngle );

			//normal constraint
			T dCosA = eimNlc_.col( c ).dot( R_tmp[i] * eimNlw_.col ( c ) );
			dCosA = dCosA > 1.f ? 1.f : dCosA;	dCosA = dCosA <-1.f ?-1.f : dCosA;
			T dA = acos( dCosA );
			if ( eivE.norm() < dist_thre_ && dA < angle_thre_ && dVisualAngle < visual_angle_thre_ ){
				inlier_idx.push_back( c );
				nVotes ++;
			}
		}
		all_votes.at<int>( 0,i ) = nVotes;
		_v_v_inlier_idx.push_back( inlier_idx );
	}

	/*t = ((double)getTickCount() - t)/getTickFrequency();
	cout << "2: " << t << endl;
	t = (double)getTickCount();*/

	cv::Mat most;
	sortIdx(all_votes,most, CV_SORT_DESCENDING + CV_SORT_EVERY_ROW );
	//collect inliers
	////////////////////////////////////////////////////////////////
	int best_idx = most.at<int>(0,0);
	//PRINT( all_votes.at<int>( 0,best_idx) );

	vector<int>& v_inlier_idx = _v_v_inlier_idx[best_idx];
	pInliers_->create(1,(int)v_inlier_idx.size(),CV_32SC1);
	Matrix<T ,-1,-1,0,-1,-1> eimX_world_selected( 3,v_inlier_idx.size() ),   eimNl_world_selected( 3,v_inlier_idx.size() ),
		                    eimX_cam_selected(3,v_inlier_idx.size()),       eimNl_cam_selected( 3,v_inlier_idx.size() );

	for ( int i=0; i< v_inlier_idx.size(); i++)
	{
		eimX_world_selected.col(i)  = eimXw_.col( v_inlier_idx[i] );
		eimNl_world_selected.col(i) = eimNlw_.col( v_inlier_idx[i] );
		eimX_cam_selected.col(i)  = eimXc_.col( v_inlier_idx[i] );
		eimNl_cam_selected.col(i) = eimNlc_.col( v_inlier_idx[i] );
		pInliers_->ptr<int>()[i] = v_inlier_idx[i];
	}

	Eigen::Matrix< T , 3, 3> R_final;
	Eigen::Matrix< T , 3, 1> T_final;
	Eigen::Matrix< T , 2, 1> dEA = absoluteOrientationWithNormal<T>( eimX_world_selected, eimNl_world_selected, eimX_cam_selected, eimNl_cam_selected, &R_final, &T_final);

	*pR_ = R_final;
	*pT_ = T_final;

	return dEA;
}

template< class T > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
Eigen::Matrix<T,2,1> aoWithNormaln2dConstraintRansac2( const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimXw_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimNlw_,
													   const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimXc_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimNlc_,
													   const T dist_thre_, const T angle_thre_, const T visual_angle_thre_, const int Iter_,
													   Eigen::Matrix< T , 3, 3>* pR_, Eigen::Matrix< T , 3, 1 >* pT_, cv::Mat * pInliers_ ){
	// A is from World  B is Local coordinate system
	// eimB_ = R * eimA_ + T; //R and t is defined in world and transform a point in world to local
	// main references: http://www.mathworks.com/matlabcentral/fileexchange/22422-absolute-orientation
	using namespace Eigen;
	using namespace cv;
	//CHECK ( 	eimXw_.rows() == 3, " absoluteOrientation() requires the input matrix A_ is a 3 x N matrix. " );
	//CHECK ( 	eimXc_.rows() == 3, " absoluteOrientation() requires the input matrix B_ is a 3 x N matrix. " );
	//CHECK ( 	eimXw_.cols() == eimXc_.cols(), " absoluteOrientation() requires the columns of input matrix A_ and B_ are equal. " );

	RandomElements<int> re( (int)eimXw_.cols() );
	const int K = 2;
	vector<vector<int> > _v_v_inlier_pt_idx;
	vector<vector<int> > _v_v_inlier_nl_idx;
	vector<vector<int> > _v_v_inlier_2d_idx;

	//vector<vector<int> > _v_v_selected_cols;

	vector<Matrix3f> R_tmp; R_tmp.resize(Iter_);//[Iter];
	vector<Vector3f> T_tmp; T_tmp.resize(Iter_);//[Iter];

	//Matrix3f R_tmp[Iter];
	//Vector3f T_tmp[Iter];

	cv::Mat all_votes(1,Iter_,CV_32FC1);
	cv::Mat all_votes_3d(1,Iter_,CV_32FC1);
	cv::Mat all_votes_nl(1,Iter_,CV_32FC1);
	cv::Mat all_votes_2d(1,Iter_,CV_32FC1);

	//double t = (double)getTickCount() ;
	Matrix<T ,-1,-1,0,-1,-1> eimX_world(3,K),eimNl_world(3,K),eimX_cam(3,K),eimNl_cam(3,K);
	for (int i=0; i < Iter_; i++)	{
		//randomly select K candidates
		vector<int> selected_cols;
		re.run(K, &selected_cols);
		for (int c=0; c<K; c++) {
			eimX_world.col( c )  = eimXw_.col( selected_cols[c] );
			eimX_cam.col( c )    = eimXc_.col( selected_cols[c] );
			eimNl_world.col( c ) = eimNlw_.col( selected_cols[c] );
			eimNl_cam.col( c )   = eimNlc_.col( selected_cols[c] );
		}
		//calc R&t
		//Eigen::Matrix< T , 3, 3> R_tmp;
		//Eigen::Matrix< T , 3, 1> T_tmp;
		absoluteOrientationWithNormal<T>( eimX_world,eimNl_world,
											  eimX_cam,  eimNl_cam, &(R_tmp[i]),&(T_tmp[i]) );
	}

	/*t = ((double)getTickCount() - t)/getTickFrequency();
	cout << "1: " << t << endl;
	t = (double)getTickCount();*/

	Eigen::Matrix< T , 3, 1> eivXc_tmp;
	Eigen::Matrix< T , 3, 1> eivE;
	for (int i=0; i < Iter_; i++)	{
		//collect votes
		int nVotes_3d = 0;
		int nVotes_nl = 0;
		int nVotes_2d = 0;
		vector<int> inlier_pt_idx;
		vector<int> inlier_nl_idx;
		vector<int> inlier_2d_idx;
		Vector3f C_tmp = - R_tmp[i].transpose() * T_tmp[i] ;
		for (int c=0; c < eimXw_.cols(); c++  )	{
			eivXc_tmp = R_tmp[i] * ( eimXw_.col ( c ) - C_tmp );
			//dist constraint
			eivE = eimXc_.col ( c ) - eivXc_tmp;
			//visual angle constraint
			T dCosVisualAngle = eimXc_.col ( c ).dot( eivXc_tmp )/eimXc_.col ( c ).norm()/eivXc_tmp.norm();
			dCosVisualAngle = dCosVisualAngle > 1.f ? 1.f : dCosVisualAngle;	dCosVisualAngle = dCosVisualAngle <-1.f ?-1.f : dCosVisualAngle;
			T dVisualAngle = acos( dCosVisualAngle );

			//normal constraint
			T dCosA = eimNlc_.col( c ).dot( R_tmp[i] * eimNlw_.col ( c ) );
			dCosA = dCosA > 1.f ? 1.f : dCosA;	dCosA = dCosA <-1.f ?-1.f : dCosA;
			T dA = acos( dCosA );
			if ( eivE.norm() < dist_thre_ && dVisualAngle < visual_angle_thre_ ){
				inlier_pt_idx.push_back( c );
				nVotes_3d ++;
			}
			if ( dA < angle_thre_ && dVisualAngle < visual_angle_thre_ ){
				inlier_nl_idx.push_back( c );
				nVotes_nl ++;
			}
			if ( dVisualAngle < visual_angle_thre_ ){
				inlier_2d_idx.push_back( c );
				nVotes_2d ++;
			}
		}
		all_votes_3d.at<int>( 0,i ) = nVotes_3d;
		all_votes_nl.at<int>( 0,i ) = nVotes_nl;
		all_votes_2d.at<int>( 0,i ) = nVotes_2d;
		all_votes.at<int>( 0,i ) = nVotes_nl + nVotes_3d;
		_v_v_inlier_pt_idx.push_back( inlier_pt_idx );
		_v_v_inlier_nl_idx.push_back( inlier_nl_idx );
		_v_v_inlier_2d_idx.push_back( inlier_2d_idx );
	}

	/*t = ((double)getTickCount() - t)/getTickFrequency();
	cout << "2: " << t << endl;
	t = (double)getTickCount();*/

	cv::Mat most;
	sortIdx(all_votes, most, CV_SORT_DESCENDING + CV_SORT_EVERY_ROW );
	//collect inliers
	////////////////////////////////////////////////////////////////
	int ii = 0;
	int best_idx = most.at<int>(0,ii);
	while( !(all_votes_2d.at<int>( 0,best_idx ) >= 2 && all_votes_nl.at<int>( 0,best_idx ) >= 2 && all_votes_3d .at<int>( 0,best_idx ) >= 2 ) && ii< most.cols ){
		best_idx = most.at<int>(0,ii); ii++;
	}
	if( ii >= most.cols ){
		pR_->setZero();
		return Eigen::Matrix< T , 2, 1>( 1000.f, 1000.f );
	}
	//PRINT( all_votes.at<int>( 0,best_idx) );

	vector<int>& v_inlier_pt_idx = _v_v_inlier_pt_idx[best_idx];
	vector<int>& v_inlier_nl_idx = _v_v_inlier_nl_idx[best_idx];
	vector<int>& v_inlier_2d_idx = _v_v_inlier_2d_idx[best_idx];
	pInliers_->create(1,(int)v_inlier_pt_idx.size(),CV_32SC1);
	Matrix<T ,-1,-1,0,-1,-1> eimX_world_selected( 3,v_inlier_pt_idx.size() ),   eimNl_world_selected( 3,v_inlier_nl_idx.size() ),
		                         eimX_cam_selected(3,v_inlier_pt_idx.size() ),       eimNl_cam_selected( 3,v_inlier_nl_idx.size() );

	for ( int i=0; i< v_inlier_pt_idx.size(); i++)
	{
		eimX_world_selected.col(i)  = eimXw_.col( v_inlier_pt_idx[i] );
		eimX_cam_selected.col(i)  = eimXc_.col( v_inlier_pt_idx[i] );
		pInliers_->ptr<int>()[i] = v_inlier_pt_idx[i];
	}

	for ( int i=0; i< v_inlier_nl_idx.size(); i++)
	{
		eimNl_world_selected.col(i) = eimNlw_.col( v_inlier_nl_idx[i] );
		eimNl_cam_selected.col(i) = eimNlc_.col( v_inlier_nl_idx[i] );
	}

	/*for ( int i=0; i< v_inlier_2d_idx.size(); i++)
	{
		int idx = i + v_inlier_nl_idx.size();

		eimNl_world_selected.col(idx) = eimNlw_.col( v_inlier_2d_idx[i] );
		eimNl_cam_selected.col(idx) = eimNlc_.col( v_inlier_2d_idx[i] );
	}*/

	Eigen::Matrix< T , 3, 3> R_final;
	Eigen::Matrix< T , 3, 1> T_final;
	Eigen::Matrix< T , 2, 1> dEA = absoluteOrientationWithNormal<T>( eimX_world_selected, eimNl_world_selected, eimX_cam_selected, eimNl_cam_selected, &R_final, &T_final);

	*pR_ = R_final;
	*pT_ = T_final;

	return dEA;
}

template< class T > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
Eigen::Matrix<T,2,1> aoWithNormalMainDirectionWith2dConstraintRansac ( const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimXw_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimNlw_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimMDw_,
														   const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimXc_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimNlc_, const Eigen::Matrix<T,-1,-1,0,-1,-1>& eimMDc_,
														   const T dist_thre_, const T angle_thre_, const T visual_angle_thre_, const int Iter,
														   Eigen::Matrix< T , 3, 3>* pR_, Eigen::Matrix< T , 3, 1 >* pT_, cv::Mat * pInliers_ ){
	// A is from World  B is Local coordinate system
	// eimB_ = R * eimA_ + T; //R and t is defined in world and transform a point in world to local
	// main references: http://www.mathworks.com/matlabcentral/fileexchange/22422-absolute-orientation
	using namespace Eigen;
	using namespace cv;
	//CHECK ( eimXw_.rows() == 3, " absoluteOrientation() requires the input matrix A_ is a 3 x N matrix. " );
	//CHECK ( eimXc_.rows() == 3, " absoluteOrientation() requires the input matrix B_ is a 3 x N matrix. " );
	//CHECK ( eimXw_.cols() == eimXc_.cols(), " absoluteOrientation() requires the columns of input matrix A_ and B_ are equal. " );

	T md_thre = angle_thre_*20;

	RandomElements<int> re( (int)eimXw_.cols() );
	const int K = 2;
	vector<vector<int> > _v_v_inlier_idx;

	//vector<vector<int> > _v_v_selected_cols;

	vector<Matrix3f> R_tmp; R_tmp.resize(Iter);//[Iter];
	vector<Vector3f> T_tmp; T_tmp.resize(Iter);//[Iter];

	cv::Mat all_votes(1,Iter,CV_32FC1);

	//double t = (double)getTickCount() ;
	Matrix<T ,-1,-1,0,-1,-1> eimX_world(3,K),eimNl_world(3,K),eimMD_world(3,K),eimX_cam(3,K),eimNl_cam(3,K),eimMD_cam(3,K);
	for (int i=0; i < Iter; i++)	{
		//randomly select K candidates
		vector<int> selected_cols;
		re.run(K, &selected_cols);
		for (int c=0; c<K; c++) {
			eimX_world.col( c )  = eimXw_.col( selected_cols[c] );
			eimX_cam.col( c )    = eimXc_.col( selected_cols[c] );
			eimNl_world.col( c ) = eimNlw_.col( selected_cols[c] );
			eimNl_cam.col( c )   = eimNlc_.col( selected_cols[c] );
			eimMD_world.col( c ) = eimMDw_.col( selected_cols[c] );
			eimMD_cam.col( c )   = eimMDc_.col( selected_cols[c] );
		}
		//calc R&t
		//Eigen::Matrix< T , 3, 3> R_tmp;
		//Eigen::Matrix< T , 3, 1> T_tmp;
		//p1p_3d_nl_md<T>( eimX_world,eimNl_world,eimMD_world,
		//					 eimX_cam, eimNl_cam, eimMD_cam, &(R_tmp[i]),&(T_tmp[i]) );
		absoluteOrientationWithNormalnMainDirection<T>( eimX_world, eimNl_world, eimMD_world,
														eimX_cam, eimNl_cam, eimMD_cam, &(R_tmp[i]), &(T_tmp[i]));
	}
	/*t = ((double)getTickCount() - t)/getTickFrequency();
	cout << "1: " << t << endl;
	t = (double)getTickCount();*/

	Eigen::Matrix< T , 3, 1> eivXc_tmp;
	Eigen::Matrix< T , 3, 1> eivE;
	for (int i=0; i < Iter; i++)	{
		//collect votes
		int nVotes = 0;
		vector<int> inlier_idx;
		for (int c=0; c < eimXw_.cols(); c++  )	{
			eivXc_tmp = R_tmp[i] * eimXw_.col ( c ) + T_tmp[i];
			//dist constraint
			eivE = eimXc_.col ( c ) - eivXc_tmp;
			T dVisualAngle, dNormalAngle, dMDAngle;
			//visual angle constraint
			{
				T dCosA = eimXc_.col ( c ).dot( eivXc_tmp )/eimXc_.col ( c ).norm()/eivXc_tmp.norm();
				dCosA = dCosA > 1.f ? 1.f : dCosA;	dCosA = dCosA <-1.f ?-1.f : dCosA;
				dVisualAngle = acos( dCosA );
			}
			//normal constraint
			{
				T dCosA = eimNlc_.col( c ).dot( R_tmp[i] * eimNlw_.col ( c ) );
				dCosA = dCosA > 1.f ? 1.f : dCosA;	dCosA = dCosA <-1.f ?-1.f : dCosA;
				dNormalAngle = acos( dCosA );
			}
			//main direction constraint
			{
				T dCosA = eimMDc_.col( c ).dot( R_tmp[i] * eimMDw_.col ( c ) );
				dCosA = dCosA > 1.f ? 1.f : dCosA;	dCosA = dCosA <-1.f ?-1.f : dCosA;
				dMDAngle = acos( dCosA );
			}
			if ( eivE.norm() < dist_thre_ && dNormalAngle < angle_thre_ && dMDAngle < md_thre  && dVisualAngle < visual_angle_thre_ ){
				inlier_idx.push_back( c );
				nVotes ++;
			}
		}
		all_votes.at<int>( 0,i ) = nVotes;
		_v_v_inlier_idx.push_back( inlier_idx );
	}

	/*t = ((double)getTickCount() - t)/getTickFrequency();
	cout << "2: " << t << endl;
	t = (double)getTickCount();*/

	cv::Mat most;
	sortIdx(all_votes,most, CV_SORT_DESCENDING + CV_SORT_EVERY_ROW );
	//collect inliers
	////////////////////////////////////////////////////////////////
	int best_idx = most.at<int>(0,0);
	//PRINT( all_votes.at<int>( 0,best_idx) );

	vector<int>& v_inlier_idx = _v_v_inlier_idx[best_idx];
	pInliers_->create(1,(int)v_inlier_idx.size(),CV_32SC1);
	Matrix<T ,-1,-1,0,-1,-1> eimX_world_selected( 3,v_inlier_idx.size() ),   eimNl_world_selected( 3,v_inlier_idx.size() ),  eimMD_world_selected( 3,v_inlier_idx.size() ),
		                    eimX_cam_selected(3,v_inlier_idx.size()),       eimNl_cam_selected( 3,v_inlier_idx.size() ),  eimMD_cam_selected( 3,v_inlier_idx.size() );

	for ( int i=0; i< v_inlier_idx.size(); i++)
	{
		eimX_world_selected.col(i)  = eimXw_.col( v_inlier_idx[i] );
		eimNl_world_selected.col(i) = eimNlw_.col( v_inlier_idx[i] );
		eimMD_world_selected.col(i) = eimMDw_.col( v_inlier_idx[i] );
		eimX_cam_selected.col(i)  = eimXc_.col( v_inlier_idx[i] );
		eimNl_cam_selected.col(i) = eimNlc_.col( v_inlier_idx[i] );
		eimMD_cam_selected.col(i) = eimMDc_.col( v_inlier_idx[i] );
		pInliers_->ptr<int>()[i] = v_inlier_idx[i];
	}

	Eigen::Matrix< T , 3, 3> R_final;
	Eigen::Matrix< T , 3, 1> T_final;
	Eigen::Matrix< T , 2, 1> dEA = absoluteOrientationWithNormalnMainDirection<T>( eimX_world_selected, eimNl_world_selected,eimMD_world_selected, eimX_cam_selected, eimNl_cam_selected,eimMD_cam_selected, &R_final, &T_final);

	*pR_ = R_final;
	*pT_ = T_final;

	return dEA;
}

template< class T, int ROW, int COL >
T matNormL1 ( const Eigen::Matrix< T, ROW, COL >& eimMat1_, const Eigen::Matrix< T, ROW, COL >& eimMat2_ )
{
	Eigen::Matrix< T, ROW, COL > eimTmp = eimMat1_ - eimMat2_;
	Eigen::Matrix< T, ROW, COL > eimAbs = eimTmp.cwiseAbs();
	return (T) eimAbs.sum();
}

template< class T >
void setSkew( T x_, T y_, T z_, Eigen::Matrix< T, 3,3 >* peimMat_){
	*peimMat_ << 0, -z_, y_, z_, 0, -x_, -y_, x_, 0 ;
}

template< class T >
void setRotMatrixUsingExponentialMap( T x_, T y_, T z_, Eigen::Matrix< T, 3,3 >* peimR_ ){
	//http://opencv.itseez.com/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?highlight=rodrigues#void Rodrigues(InputArray src, OutputArray dst, OutputArray jacobian)
	T theta = sqrt( x_*x_ + y_*y_ + z_*z_ );
	if(	theta < std::numeric_limits<T>::epsilon() ){
		*peimR_ = Eigen::Matrix< T, 3,3 >::Identity();
		return;
	}
	T sinTheta = sin(theta);
	T cosTheta = cos(theta);
	Eigen::Matrix< T, 3,3 > eimSkew;
	setSkew< T >(x_/theta,y_/theta,z_/theta,&eimSkew);
	*peimR_ = Eigen::Matrix< T, 3,3 >::Identity() + eimSkew*sinTheta + eimSkew*eimSkew*(1-cosTheta);
}

}//utility
}//btl
#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <complex>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

#if defined(MTL_WITH_INITLIST) && defined(MTL_WITH_AUTO) && defined(MTL_WITH_RANGEDFOR)
#include <initializer_list>

template <typename Matrix>
void test(const char* name)
{
    const Matrix A={{3, 4},
		    {5, 6}};

    cout << "Test " << name << ", A(initialization):\n" << A << "\n";
    MTL_THROW_IF(A[1][0] != 5.0, mtl::runtime_error("wrong value or position"));

    Matrix B;
    B= {{3, 4},
	{5, 6}};
    cout << "B(assignment):\n" << B << "\n";
    MTL_THROW_IF(B[1][0] != 5.0, mtl::runtime_error("wrong value or position"));
}
#else
template <typename Matrix>
void test(const char* ) {}
#endif

int main(int , char**)
{
    using mtl::mat::parameters;
    using namespace mtl;

    test<dense2D<double> >("dense2D<double>");
    test<dense2D<float> >("dense2D<float>");
    test<dense2D<complex<double> > >("dense2D<complex<double>>");
    test<dense2D<float, mat::parameters<col_major> > >("dense2D<float> col_major");

    test<compressed2D<float> >("compressed2D<float>");
    test<compressed2D<float, mat::parameters<col_major> > >("compressed2D<float> col_major");

    test<morton_dense<double, recursion::morton_z_mask> >("Morton Z-order");

    return 0;
}


/**
 *
 * @file faceBlendCommon.hpp
 * @author Shawn Zhang
 * @date 2018-09-09
 */
#include <opencv2/opencv.hpp>
#include <dlib/opencv.h>
#include <dlib/image_processing.h>
#include <dlib/image_processing/frontal_face_detector.h>

using namespace cv;
using namespace std;

#ifndef M_PI
#define M_PI 3.14159
#endif

/**
 * @brief 将点约束在图像的内部
 *
 * @param p 点坐标
 * @param size 图像尺寸
 */
void constrainPoint(cv::Point2f &p, cv::Size size)
{
    p.x = min(max((double)p.x, 0.0), (double)(size.width - 1));
    p.y = min(max((double)p.y, 0.0), (double)(size.height - 1));
}

/**
 * @brief 获取顶点的索引
 *
 * @param points 顶点集合
 * @param point 点
 * @return int 返回索引值
 */
static int findIndex(vector<Point2f>& points, Point2f &point)
{
    int minIndex = 0;
    double minDistance = norm(points[0] - point);
    for(int i=0; i<points.size();i++)
    {
        double distance = norm(points[i]-point);
        if(distance < minDistance)
        {
            minIndex = i;
            minDistance = distance;
        }
    }
    return minIndex;
}

/**
 * @brief 在图片边缘获取八个边界点
 *
 * @param size
 * @param boundaryPts
 */
void getEightBoundaryPoints(Size size, vector<Point2f> &boundaryPts)
{
    int h = size.height, w = size.width;
    boundaryPts.push_back(Point2f(0, 0));
    boundaryPts.push_back(Point2f(w / 2, 0));
    boundaryPts.push_back(Point2f(w - 1, 0));
    boundaryPts.push_back(Point2f(w - 1, h / 2));
    boundaryPts.push_back(Point2f(w - 1, h - 1));
    boundaryPts.push_back(Point2f(w / 2, h - 1));
    boundaryPts.push_back(Point2f(0, h - 1));
    boundaryPts.push_back(Point2f(0, h / 2));
}

/**
 * @brief 计算相似变换矩阵
 *
 * @param inPoints 变换前的点
 * @param outPoints 变换后的点
 * @param tfrom 相似变换矩阵
 */
void similarityTransform(std::vector<cv::Point2f> &inPoints, std::vector<cv::Point2f> &outPoints, cv::Mat &tfrom)
{
    double s60 = sin(60 * M_PI / 180.0);
    double c60 = cos(60 * M_PI / 180.0);

    std::vector<cv::Point2f> inPts = inPoints;
    std::vector<cv::Point2f> outPts = outPoints;

    // opencv计算相似变换矩阵的时候需要3个点
    inPts.push_back(cv::Point2f(0.0, 0.0));
    outPts.push_back(cv::Point2f(0.0, 0.0));

    // 这里假设给出的两个点是水平的，第三个点与给出的两个点构成等边三角形，所有可以计算第三个点；这里的计算方法使用的是矩阵变换的方法
    inPts[2].x = c60 * (inPts[1].x - inPts[0].x) + s60 * (inPts[1].y - inPts[0].y) + inPts[0].x;
    inPts[2].y = c60 * (inPts[1].y - inPts[0].y) - s60 * (inPts[1].x - inPts[0].x) + inPts[0].y;

    outPts[2].x = c60 * (outPts[1].x - outPts[0].x) + s60 * (outPts[1].y - outPts[0].y) + outPts[0].x;
    outPts[2].y = c60 * (outPts[1].y - outPts[0].y) - s60 * (outPts[1].x - outPts[0].x) + outPts[0].y;

    tfrom = cv::estimateRigidTransform(inPts, outPts, false);
}

/**
 * @brief 使用指定的尺寸规范化图片
 * 使用dlib检测到的脸部关键特征点做为输入点，使用两眼的外眼角进行规范化计算。
 * 图片规范化之后，左眼的外眼角在输出图像的坐标是(0.3*w,h/3),右眼的外眼角的坐标是(0.7*w,h/3)。w和h分别表示输出图像的宽和高
 *
 * @param outSize 输出的图片尺寸
 * @param imgIn 输入的图片
 * @param imgOut 输出的图片
 * @param pointsIn 输入的点
 * @param pointsOut 输出的点
 */
void normalizeImagesAndLandmarks(cv::Size outSize, cv::Mat &imgIn, cv::Mat &imgOut, std::vector<cv::Point2f> &pointsIn, std::vector<cv::Point2f> &pointsOut)
{
    int h = outSize.height;
    int w = outSize.width;

    std::vector<Point2f> eyecornerSrc;
    // 左眼角
    eyecornerSrc.push_back(pointsIn[36]);
    // 右眼角
    eyecornerSrc.push_back(pointsIn[45]);

    std::vector<Point2f> eyecornerDst;
    eyecornerDst.push_back(cv::Point2f(0.3 * w, h / 3.0));
    eyecornerDst.push_back(cv::Point2f(0.7 * w, h / 3.0));

    // 计算相似变换矩阵
    cv::Mat tform;
    similarityTransform(eyecornerSrc, eyecornerDst, tform);

    // 将相似变换矩阵用在输出图像上
    imgOut = Mat::zeros(h, w, CV_32FC3);
    cv::warpAffine(imgIn, imgOut, tform, imgOut.size());

    cv::transform(pointsIn, pointsOut, tform);
}

/**
 * @brief 对图像使用仿射变换
 *
 * @param warpImage
 * @param src
 * @param srcTri
 * @param dstTri
 */
void applyAffineTransform(cv::Mat &warpImage, cv::Mat &src, std::vector<cv::Point2f> &srcTri, std::vector<cv::Point2f> &dstTri)
{
    cv::Mat warpMat = cv::getAffineTransform(srcTri, dstTri);
    warpAffine(src, warpImage, warpMat, warpImage.size(), cv::INTER_LINEAR, cv::BORDER_REFLECT101);
}

/**
 * @brief 变换图像中的三角区域
 *
 * @param imgIn
 * @param imgOut
 * @param tin
 * @param tout
 */
void warpTriangle(cv::Mat &imgIn, cv::Mat &imgOut, std::vector<cv::Point2f> tin, std::vector<cv::Point2f> tout)
{
    imshow("imgIn", imgIn);
    imshow("imgOut-be", imgOut);
    // 计算包围盒
    cv::Rect rect1 = cv::boundingRect(tin);
    cv::Rect rect2 = cv::boundingRect(tout);

    std::vector<cv::Point2f> tinRect, toutRect;
    std::vector<cv::Point> toutRectInt;
    for (int i = 0; i < 3; i++)
    {
        toutRectInt.push_back(cv::Point((int)(tout[i].x - rect2.x), (int)(tout[i].y - rect2.y)));

        tinRect.push_back(cv::Point2f(tin[i].x - rect1.x, tin[i].y - rect1.y));
        toutRect.push_back(cv::Point2f(tout[i].x - rect2.x, tout[i].y - rect2.y));
    }

    // Mask
    cv::Mat mask = cv::Mat::zeros(rect2.height, rect2.width, CV_32FC3);
    cv::fillConvexPoly(mask, toutRectInt, cv::Scalar(1.0, 1.0, 1.0), 16, 0);

    cv::Mat imgInRect, imgOutRect;
    imgIn(rect1).copyTo(imgInRect);

    cv::Mat warpImage = cv::Mat::zeros(rect2.height, rect2.width, imgInRect.type());

    applyAffineTransform(warpImage, imgInRect, tinRect, toutRect);

    cv::multiply(warpImage, mask, warpImage);
    cv::multiply(imgOut(rect2), cv::Scalar(1.0, 1.0, 1.0) - mask, imgOut(rect2));

    imgOut(rect2) = imgOut(rect2) + warpImage;
    cv::imshow("imgOut", imgOut);
    cv::waitKey(100);
}

/**
 * @brief 按照Delaunay剖分法变换图像
 *
 * @param imgIn
 * @param imgOut
 * @param pointsIn
 * @param pointsOut
 * @param delaunayTri
 */
void warpImage(cv::Mat &imgIn, cv::Mat &imgOut, std::vector<cv::Point2f> &pointsIn, std::vector<cv::Point2f> &pointsOut, std::vector<std::vector<int>> &delaunayTri)
{
    // 输出图像和输出图像的尺寸应该是相同的
    cv::Size size = imgIn.size();
    imgOut = cv::Mat::zeros(size, imgIn.type());

    // 将delaunay剖分出的三角形映射到每个输出三角形中
    for (size_t j = 0; j < delaunayTri.size(); j++)
    {
        std::vector<Point2f> tin, tout;

        for (int k = 0; k < 3; k++)
        {
            cv::Point2f pIn = pointsIn[delaunayTri[j][k]];
            // 确保点在图像内部
            constrainPoint(pIn, size);

            cv::Point2f pOut = pointsOut[delaunayTri[j][k]];
            // 确保点在图像内部
            constrainPoint(pOut, size);

            tin.push_back(pIn);
            tout.push_back(pOut);

        }

        // cv::line(imgIn, tin[0], tin[1], Scalar(1.0,1.0,1.0),2);
        // cv::line(imgIn, tin[1], tin[2], Scalar(1.0,1.0,1.0),2);
        // cv::line(imgIn, tin[2], tin[0], Scalar(1.0,1.0,1.0),2);

        // cv::line(imgOut, tout[0], tout[1], Scalar(1.0,1.0,1.0),2);
        // cv::line(imgOut, tout[1], tout[2], Scalar(1.0,1.0,1.0),2);
        // cv::line(imgOut, tout[2], tout[0], Scalar(1.0,1.0,1.0),2);
        warpTriangle(imgIn, imgOut, tin, tout);
    }

}

/**
 * @brief 从detection提出关键点坐标
 *
 * @param landmarks
 * @param points
 */
void dlibLandmarksToPoints( dlib::full_object_detection &landmarks, std::vector<cv::Point2f> &points)
{
    for (int i = 0; i < landmarks.num_parts(); i++)
    {
        cv::Point2f p(landmarks.part(i).x(), landmarks.part(i).y());
        points.push_back(p);
    }
}

/**
 * @brief 对比区域大小
 *
 * @param r1
 * @param r2
 * @return true
 * @return false
 */
bool rectAreaComparator(dlib::rectangle &r1, dlib::rectangle &r2)
{
    return r1.area() < r2.area();
}

/**
 * @brief 获取图像的面部关键点
 *
 * @param faceDetector
 * @param landmarkDetector
 * @param img
 * @param FACE_DOWNSAMPLE_RATIO
 * @return std::vector<cv::Point2f>
 */
std::vector<cv::Point2f> getLandmarks(dlib::frontal_face_detector &faceDetector, dlib::shape_predictor &landmarkDetector, cv::Mat &img, float FACE_DOWNSAMPLE_RATIO = 1)
{
    std::vector<cv::Point2f> points;
    cv::Mat imgSmall;
    cv::resize(img, imgSmall, cv::Size(), 1.0 / FACE_DOWNSAMPLE_RATIO, 1.0 / FACE_DOWNSAMPLE_RATIO);

    // 将图像转化为dlib格式
    dlib::cv_image<dlib::bgr_pixel> dlibIm(img);
    dlib::cv_image<dlib::bgr_pixel> dlibImSmall(imgSmall);

    std::vector<dlib::rectangle> faceRects = faceDetector(dlibImSmall);

    if (faceRects.size() > 0)
    {
        // 只取最大的脸
        dlib::rectangle rect = *std::max_element(faceRects.begin(), faceRects.end(), rectAreaComparator);

        dlib::rectangle scaleRect(
            (long)(rect.left() * FACE_DOWNSAMPLE_RATIO),
            (long)(rect.top() * FACE_DOWNSAMPLE_RATIO),
            (long)(rect.right() * FACE_DOWNSAMPLE_RATIO),
            (long)(rect.bottom() * FACE_DOWNSAMPLE_RATIO));

        dlib::full_object_detection landmarks = landmarkDetector(dlibIm, scaleRect);
        dlibLandmarksToPoints(landmarks, points);
    }

    return points;
}

/**
 * @brief 计算Delaunay三角形
 *
 * @param rect
 * @param points
 * @param delaunayTri
 */
void calculateDelaunayTriangles(cv::Rect rect, std::vector<cv::Point2f> &points, std::vector<std::vector<int>> &delaunayTri)
{
    Subdiv2D subdiv(rect);
    for( std::vector<cv::Point2f>::iterator it = points.begin(); it != points.end(); it++)
    {
        subdiv.insert(*it);
    }

    std::vector<cv::Vec6f> triangleList;
    subdiv.getTriangleList(triangleList);

     vector<Point2f> pt(3);

    vector<int> ind(3);

    for( size_t i = 0; i < triangleList.size(); i++ )
    {
        Vec6f t = triangleList[i];

        pt[0] = Point2f(t[0], t[1]);
        pt[1] = Point2f(t[2], t[3]);
        pt[2] = Point2f(t[4], t[5]);

        if ( rect.contains(pt[0]) && rect.contains(pt[1]) && rect.contains(pt[2]))
        {
            for(int j = 0; j < 3; j++)
            {
                ind[j] = findIndex(points, pt[j]);
            }
            delaunayTri.push_back(ind);
        }
    }
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "test_optimizers"

#include <boost/test/unit_test.hpp>
#include "nanocv/timer.h"
#include "nanocv/logger.h"
#include "nanocv/minimize.h"
#include "nanocv/math/abs.hpp"
#include "nanocv/math/stats.hpp"
#include "nanocv/math/random.hpp"
#include "nanocv/math/numeric.hpp"
#include "nanocv/math/epsilon.hpp"

#include "nanocv/functions/function_trid.h"
#include "nanocv/functions/function_beale.h"
#include "nanocv/functions/function_booth.h"
#include "nanocv/functions/function_sphere.h"
#include "nanocv/functions/function_matyas.h"
#include "nanocv/functions/function_powell.h"
#include "nanocv/functions/function_mccormick.h"
#include "nanocv/functions/function_himmelblau.h"
#include "nanocv/functions/function_rosenbrock.h"
#include "nanocv/functions/function_3hump_camel.h"
#include "nanocv/functions/function_sum_squares.h"
#include "nanocv/functions/function_dixon_price.h"
#include "nanocv/functions/function_goldstein_price.h"
#include "nanocv/functions/function_rotated_ellipsoid.h"

namespace test
{
        using namespace ncv;

        static void check_solution(const string_t&, const string_t&,
                const opt_state_t& state, const std::vector<std::pair<vector_t, scalar_t>>& solutions)
        {
                // Check convergence
                BOOST_CHECK_LE(state.g.lpNorm<Eigen::Infinity>(), math::epsilon3<scalar_t>());

                // Find the closest solution
                size_t best_index = std::string::npos;
                scalar_t best_distance = std::numeric_limits<scalar_t>::max();

                for (size_t index = 0; index < solutions.size(); index ++)
                {
                        const scalar_t distance = (state.x - solutions[index].first).lpNorm<Eigen::Infinity>();
                        if (distance < best_distance)
                        {
                                best_distance = distance;
                                best_index = index;
                        }
                }

                // Check accuracy
                BOOST_CHECK_LT(best_index, solutions.size());
                if (best_index < solutions.size())
                {
                        const scalar_t dfx = math::abs(state.f - solutions[best_index].second);
                        const scalar_t dx = (state.x - solutions[best_index].first).lpNorm<Eigen::Infinity>();

                        BOOST_CHECK_LE(dfx, math::epsilon3<scalar_t>());
                        BOOST_CHECK_LE(dx, math::epsilon3<scalar_t>());

//                        if (dx > math::epsilon3<scalar_t>())
//                        {
//                                log_info() << problem_name
//                                           << ", x = (" << state.x.transpose() << ")"
//                                           << ", dx = " << dx
//                                           << ", x0 = (" << solutions[best_index].first.transpose() << ")"
//                                           << ", fx = " << state.f
//                                           << ", gx = " << state.g.lpNorm<Eigen::Infinity>();
//                        }
                }
        }

        static void check_problem(
                const string_t& problem_name,
                const opt_opsize_t& fn_size, const opt_opfval_t& fn_fval, const opt_opgrad_t& fn_grad,
                const std::vector<std::pair<vector_t, scalar_t>>& solutions)
        {
                const size_t iterations = 64 * 1024;
                const scalar_t epsilon = math::epsilon2<scalar_t>();
                const size_t trials = 1024;

                const size_t dims = fn_size();

                // generate fixed random trials
                vectors_t x0s;
                for (size_t t = 0; t < trials; t ++)
                {
                        random_t<scalar_t> rgen(-1.0, +1.0);

                        vector_t x0(dims);
                        rgen(x0.data(), x0.data() + x0.size());

                        x0s.push_back(x0);
                }

                // optimizers to try
                const auto optimizers =
                {
                        optim::batch_optimizer::GD,
                        optim::batch_optimizer::CGD,
                        optim::batch_optimizer::LBFGS
                };

                for (optim::batch_optimizer optimizer : optimizers)
                {
                        for (size_t t = 0; t < trials; t ++)
                        {
                                const vector_t& x0 = x0s[t];

                                // check gradient
                                const opt_problem_t problem(fn_size, fn_fval, fn_grad);
                                BOOST_CHECK_LE(problem.grad_accuracy(x0), math::epsilon2<scalar_t>());

                                // optimize
                                const opt_state_t state = ncv::minimize(
                                        fn_size, fn_fval, fn_grad, nullptr, nullptr, nullptr,
                                        x0, optimizer, iterations, 1e-2 * epsilon);

                                // check solution
                                check_solution(problem_name, text::to_string(optimizer), state, solutions);
                        }
                }
        }

        static void check_problems(const std::vector<test::function_t>& funcs)
        {
                BOOST_CHECK_EQUAL(funcs.empty(), false);

                for (const test::function_t& func : funcs)
                {
                        test::check_problem(func.m_name, func.m_opsize, func.m_opfval, func.m_opgrad, func.m_solutions);
                }
        }
}

BOOST_AUTO_TEST_CASE(test_optimizers)
{
        using namespace ncv;

//        test::check_problems(ncv::make_beale_funcs());
        test::check_problems(ncv::make_booth_funcs());
        test::check_problems(ncv::make_matyas_funcs());
//        test::check_problems(ncv::make_trid_funcs(32));
        test::check_problems(ncv::make_sphere_funcs(8));
//        test::check_problems(ncv::make_powell_funcs(32));
//        test::check_problems(ncv::make_mccormick_funcs());
        test::check_problems(ncv::make_himmelblau_funcs());
//        test::check_problems(ncv::make_rosenbrock_funcs(7));
//        test::check_problems(ncv::make_3hump_camel_funcs());
//        test::check_problems(ncv::make_dixon_price_funcs(32));
        test::check_problems(ncv::make_sum_squares_funcs(32));
//        test::check_problems(ncv::make_goldstein_price_funcs());
        test::check_problems(ncv::make_rotated_ellipsoid_funcs(32));
}