//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

//Distributed under the Boost Software License, Version 1.0. (See accompanying
//file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef UUID_5265FC7CA1C011DE9EBDFFA956D89593
#define UUID_5265FC7CA1C011DE9EBDFFA956D89593

#include <boost/qvm/inline.hpp>
#include <boost/qvm/mat_traits.hpp>
#include <boost/qvm/deduce_vec.hpp>
#include <boost/qvm/assert.hpp>
#include <boost/qvm/enable_if.hpp>

namespace
boost
    {
    namespace
    qvm
        {
        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int Col,class OriginalMatrix>
            class
            col_
                {
                col_( col_ const & );
                col_ & operator=( col_ const & );
                ~col_();

                public:

                template <class T>
                BOOST_QVM_INLINE_TRIVIAL
                col_ &
                operator=( T const & x )
                    {
                    assign(*this,x);
                    return *this;
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };
            }

        template <int Col,class OriginalMatrix>
        struct
        vec_traits< qvm_detail::col_<Col,OriginalMatrix> >
            {
            typedef qvm_detail::col_<Col,OriginalMatrix> this_vector;
            typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
            static int const dim=mat_traits<OriginalMatrix>::rows;
            BOOST_QVM_STATIC_ASSERT(Col>=0);
            BOOST_QVM_STATIC_ASSERT(Col<mat_traits<OriginalMatrix>::cols);

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_vector const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template read_element<I,Col>(reinterpret_cast<OriginalMatrix const &>(x));
                }

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element( this_vector & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template write_element<I,Col>(reinterpret_cast<OriginalMatrix &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element_idx( int i, this_vector const & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::read_element_idx(i,Col,reinterpret_cast<OriginalMatrix const &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element_idx( int i, this_vector & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::write_element_idx(i,Col,reinterpret_cast<OriginalMatrix &>(x));
                }
            };

        template <int Col,class OriginalMatrix,int D>
        struct
        deduce_vec<qvm_detail::col_<Col,OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <int Col,class OriginalMatrix,int D>
        struct
        deduce_vec2<qvm_detail::col_<Col,OriginalMatrix>,qvm_detail::col_<Col,OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <int Col,class A>
        typename boost::enable_if_c<
            is_mat<A>::value,
            qvm_detail::col_<Col,A> const &>::type
        BOOST_QVM_INLINE_TRIVIAL
        col( A const & a )
            {
            return reinterpret_cast<typename qvm_detail::col_<Col,A> const &>(a);
            }

        template <int Col,class A>
        typename boost::enable_if_c<
            is_mat<A>::value,
            qvm_detail::col_<Col,A> &>::type
        BOOST_QVM_INLINE_TRIVIAL
        col( A & a )
            {
            return reinterpret_cast<typename qvm_detail::col_<Col,A> &>(a);
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int Row,class OriginalMatrix>
            class
            row_
                {
                row_( row_ const & );
                row_ & operator=( row_ const & );
                ~row_();

                public:

                template <class T>
                BOOST_QVM_INLINE_TRIVIAL
                row_ &
                operator=( T const & x )
                    {
                    assign(*this,x);
                    return *this;
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };
            }

        template <int Row,class OriginalMatrix>
        struct
        vec_traits< qvm_detail::row_<Row,OriginalMatrix> >
            {
            typedef qvm_detail::row_<Row,OriginalMatrix> this_vector;
            typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
            static int const dim=mat_traits<OriginalMatrix>::cols;
            BOOST_QVM_STATIC_ASSERT(Row>=0);
            BOOST_QVM_STATIC_ASSERT(Row<mat_traits<OriginalMatrix>::rows);

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_vector const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template read_element<Row,I>(reinterpret_cast<OriginalMatrix const &>(x));
                }

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element( this_vector & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template write_element<Row,I>(reinterpret_cast<OriginalMatrix &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element_idx( int i, this_vector const & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::read_element_idx(Row,i,reinterpret_cast<OriginalMatrix const &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element_idx( int i, this_vector & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::write_element_idx(Row,i,reinterpret_cast<OriginalMatrix &>(x));
                }
            };

        template <int Row,class OriginalMatrix,int D>
        struct
        deduce_vec<qvm_detail::row_<Row,OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <int Row,class OriginalMatrix,int D>
        struct
        deduce_vec2<qvm_detail::row_<Row,OriginalMatrix>,qvm_detail::row_<Row,OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <int Row,class A>
        typename boost::enable_if_c<
            is_mat<A>::value,
            qvm_detail::row_<Row,A> const &>::type
        BOOST_QVM_INLINE_TRIVIAL
        row( A const & a )
            {
            return reinterpret_cast<typename qvm_detail::row_<Row,A> const &>(a);
            }

        template <int Row,class A>
        typename boost::enable_if_c<
            is_mat<A>::value,
            qvm_detail::row_<Row,A> &>::type
        BOOST_QVM_INLINE_TRIVIAL
        row( A & a )
            {
            return reinterpret_cast<typename qvm_detail::row_<Row,A> &>(a);
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class OriginalMatrix>
            class
            diag_
                {
                diag_( diag_ const & );
                diag_ & operator=( diag_ const & );
                ~diag_();

                public:

                template <class T>
                BOOST_QVM_INLINE_TRIVIAL
                diag_ &
                operator=( T const & x )
                    {
                    assign(*this,x);
                    return *this;
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };

            template <int X,int Y,bool Which>
            struct diag_bool_dispatch;

            template <int X,int Y>
            struct
            diag_bool_dispatch<X,Y,true>
                {
                static int const value=X;
                };

            template <int X,int Y>
            struct
            diag_bool_dispatch<X,Y,false>
                {
                static int const value=Y;
                };
            }

        template <class OriginalMatrix>
        struct
        vec_traits< qvm_detail::diag_<OriginalMatrix> >
            {
            typedef qvm_detail::diag_<OriginalMatrix> this_vector;
            typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
            static int const dim=qvm_detail::diag_bool_dispatch<
                    mat_traits<OriginalMatrix>::rows,
                    mat_traits<OriginalMatrix>::cols,
                    mat_traits<OriginalMatrix>::rows<=mat_traits<OriginalMatrix>::cols>::value;

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_vector const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template read_element<I,I>(reinterpret_cast<OriginalMatrix const &>(x));
                }

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element( this_vector & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template write_element<I,I>(reinterpret_cast<OriginalMatrix &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element_idx( int i, this_vector const & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::read_element_idx(i,i,reinterpret_cast<OriginalMatrix const &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element_idx( int i, this_vector & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::write_element_idx(i,i,reinterpret_cast<OriginalMatrix &>(x));
                }
            };

        template <class OriginalMatrix,int D>
        struct
        deduce_vec<qvm_detail::diag_<OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <class OriginalMatrix,int D>
        struct
        deduce_vec2<qvm_detail::diag_<OriginalMatrix>,qvm_detail::diag_<OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <class A>
        typename boost::enable_if_c<
            is_mat<A>::value,
            qvm_detail::diag_<A> const &>::type
        BOOST_QVM_INLINE_TRIVIAL
        diag( A const & a )
            {
            return reinterpret_cast<typename qvm_detail::diag_<A> const &>(a);
            }

        template <class A>
        typename boost::enable_if_c<
            is_mat<A>::value,
            qvm_detail::diag_<A> &>::type
        BOOST_QVM_INLINE_TRIVIAL
        diag( A & a )
            {
            return reinterpret_cast<typename qvm_detail::diag_<A> &>(a);
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class OriginalMatrix>
            class
            translation_
                {
                translation_( translation_ const & );
                ~translation_();

                public:

                translation_ &
                operator=( translation_ const & x )
                    {
                    assign(*this,x);
                    return *this;
                    }

                template <class T>
                BOOST_QVM_INLINE_TRIVIAL
                translation_ &
                operator=( T const & x )
                    {
                    assign(*this,x);
                    return *this;
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };
            }

        template <class OriginalMatrix>
        struct
        vec_traits< qvm_detail::translation_<OriginalMatrix> >
            {
            typedef qvm_detail::translation_<OriginalMatrix> this_vector;
            typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
            static int const dim=mat_traits<OriginalMatrix>::rows-1;
            BOOST_QVM_STATIC_ASSERT(mat_traits<OriginalMatrix>::rows==mat_traits<OriginalMatrix>::cols);
            BOOST_QVM_STATIC_ASSERT(mat_traits<OriginalMatrix>::rows>=3);

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_vector const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template read_element<I,dim>(reinterpret_cast<OriginalMatrix const &>(x));
                }

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element( this_vector & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template write_element<I,dim>(reinterpret_cast<OriginalMatrix &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element_idx( int i, this_vector const & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::read_element_idx(i,dim,reinterpret_cast<OriginalMatrix const &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element_idx( int i, this_vector & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::write_element_idx(i,dim,reinterpret_cast<OriginalMatrix &>(x));
                }
            };

        template <class OriginalMatrix,int D>
        struct
        deduce_vec<qvm_detail::translation_<OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <class OriginalMatrix,int D>
        struct
        deduce_vec2<qvm_detail::translation_<OriginalMatrix>,qvm_detail::translation_<OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <class A>
        typename boost::enable_if_c<
            is_mat<A>::value && mat_traits<A>::rows==mat_traits<A>::cols && mat_traits<A>::rows>=3,
            qvm_detail::translation_<A> const &>::type
        BOOST_QVM_INLINE_TRIVIAL
        translation( A const & a )
            {
            return reinterpret_cast<typename qvm_detail::translation_<A> const &>(a);
            }

        template <class A>
        typename boost::enable_if_c<
            is_mat<A>::value && mat_traits<A>::rows==mat_traits<A>::cols && mat_traits<A>::rows>=3,
            qvm_detail::translation_<A> &>::type
        BOOST_QVM_INLINE_TRIVIAL
        translation( A & a )
            {
            return reinterpret_cast<typename qvm_detail::translation_<A> &>(a);
            }

        ////////////////////////////////////////////////
        }
    }

#endif


// Copyright (c) Facebook, Inc. and its affiliates.
//
// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory of this source tree.
#include "compiler_gym/envs/mlir/service/ActionSpace.h"

#include <fmt/format.h>
#include <stdint.h>

#include <boost/format.hpp>
#include <limits>
#include <magic_enum.hpp>

#include "compiler_gym/util/EnumUtil.h"
#include "compiler_gym/util/Unreachable.h"

namespace compiler_gym::mlir_service {

namespace {

Space getTileSizes() {
  Space res;
  Int64Box* box = res.mutable_int64_box();
  *box->mutable_low()->mutable_shape()->Add() = 3;
  *box->mutable_high()->mutable_shape()->Add() = 3;
  for (int i = 0; i < 3; ++i) {
    *box->mutable_low()->mutable_value()->Add() = 1;
    *box->mutable_high()->mutable_value()->Add() = std::numeric_limits<uint32_t>::max();
  }
  return res;
}

Space getInterchangeVector() {
  Space res;
  *res.mutable_type_id() = "permutation";
  res.mutable_int64_sequence()->mutable_length_range()->set_min(3);
  res.mutable_int64_sequence()->mutable_length_range()->set_max(3);
  res.mutable_int64_sequence()->mutable_scalar_range()->set_min(0);
  res.mutable_int64_sequence()->mutable_scalar_range()->set_max(2);
  return res;
}

Space getPromote() {
  Space res;
  res.mutable_boolean_value();
  return res;
}

Space getPromoteFullTile() {
  Space res;
  res.mutable_boolean_value();
  return res;
}

Space getLoopType() {
  Space res;
  NamedDiscreteSpace* space = res.mutable_named_discrete();
  *space->mutable_name()->Add() = "loops";
  *space->mutable_name()->Add() = "affine_loops";
  // Tailing fails with parallel loops. See
  // https://discourse.llvm.org/t/could-not-tile-namedstructuredop-in-linalg-whith-parallel-loop-type/60134
  //*space->mutable_name()->Add() = "parallel_loops";
  return res;
}

Space getTileOptionsSpace() {
  Space res;
  auto& map = *res.mutable_space_dict()->mutable_space();
  map["tile_sizes"] = getTileSizes();
  map["interchange_vector"] = getInterchangeVector();
  map["promote"] = getPromote();
  map["promote_full_tile"] = getPromoteFullTile();
  map["loop_type"] = getLoopType();
  return res;
}

Space getVectorizeTo() {
  Space res;
  NamedDiscreteSpace* space = res.mutable_named_discrete();
  // TODO(boian): query mmperf for these values.
  *space->mutable_name()->Add() = "dot";
  *space->mutable_name()->Add() = "matmul";
  *space->mutable_name()->Add() = "outer_product";
  return res;
}

Space getVectorTransferSplit() {
  Space res;
  NamedDiscreteSpace* space = res.mutable_named_discrete();
  // TODO(boian): query mmperf for these values.
  *space->mutable_name()->Add() = "none";
  *space->mutable_name()->Add() = "linalg_copy";
  *space->mutable_name()->Add() = "vector_transfer";
  return res;
}

Space getUnrollVectorTransfers() {
  Space res;
  res.mutable_boolean_value();
  return res;
}

Space getVectorizeOptions() {
  Space res;
  auto& map = *res.mutable_space_dict()->mutable_space();
  map["vectorize_to"] = getVectorizeTo();
  map["vector_transfer_split"] = getVectorTransferSplit();
  map["unroll_vector_transfers"] = getUnrollVectorTransfers();
  return res;
}

Space getMatMulOpSpace() {
  Space space;
  auto& map = *space.mutable_space_dict()->mutable_space();
  map["tile_options"] = getTileOptionsSpace();
  map["vectorize_options"] = getVectorizeOptions();
  return space;
}

Space getMatMulSpace() {
  Space space;
  space.mutable_space_sequence()->mutable_length_range()->set_min(1);
  space.mutable_space_sequence()->mutable_length_range()->set_max(4);
  *space.mutable_space_sequence()->mutable_space() = getMatMulOpSpace();
  return space;
}

ActionSpace getMatMulActionSpace() {
  ActionSpace space;
  space.set_name(
      util::enumNameToPascalCase<MlirActionSpace>(MlirActionSpace::MATRIX_MULTIPLICATION));
  *space.mutable_space() = getMatMulSpace();
  return space;
}

}  // namespace

std::vector<ActionSpace> getMlirActionSpaceList() {
  std::vector<ActionSpace> spaces;
  spaces.reserve(magic_enum::enum_count<MlirActionSpace>());

  for (const auto& value : magic_enum::enum_values<MlirActionSpace>()) {
    switch (value) {
      case MlirActionSpace::MATRIX_MULTIPLICATION: {
        spaces.push_back(getMatMulActionSpace());
      } break;
      default:
        UNREACHABLE(fmt::format("Unknown MLIR action space {}",
                                util::enumNameToPascalCase<MlirActionSpace>(value)));
    }
  }
  return spaces;
}

}  // namespace compiler_gym::mlir_service


#include <boost/geometry/geometries/adapted/boost_polygon.hpp>


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

#include "omxExpectation.h"
#include "omxFitFunction.h"
#include "omxDefines.h"
#include "omxGREMLExpectation.h"
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/Dense>

void omxInitGREMLExpectation(omxExpectation* ox){

  SEXP rObj = ox->rObj;
  SEXP Rmtx, casesToDrop, yXcolnames;
  int i=0;
  omxState* currentState = ox->currentState;

  if(OMX_DEBUG) { mxLog("Initializing GREML expectation."); }

  //omxGREMLExpectation *oge = (omxGREMLExpectation*) R_alloc(1, sizeof(omxGREMLExpectation));
  omxGREMLExpectation *oge = new omxGREMLExpectation;

  /* Set Expectation Calls and Structures */
  ox->computeFun = omxComputeGREMLExpectation;
	ox->destructFun = omxDestroyGREMLExpectation;
	ox->componentFun = omxGetGREMLExpectationComponent;
	ox->populateAttrFun = omxPopulateGREMLAttributes;
	ox->argStruct = (void*) oge;

    /* Set up expectation structures */
  //y:
  oge->y = new omxData();
  {ScopedProtect p1(Rmtx, R_do_slot(rObj, Rf_install("y")));
	  oge->y->newDataStatic(currentState, Rmtx);
  }
  //V:
	if(OMX_DEBUG) { mxLog("Processing V."); }
	oge->cov = omxNewMatrixFromSlot(rObj, currentState, "V");
  if( oge->cov->rows != oge->cov->cols ){
    Rf_error("'V' matrix is not square");
  }
  //X:
	if(OMX_DEBUG) { mxLog("Processing X."); }
  {ScopedProtect p1(Rmtx, R_do_slot(rObj, Rf_install("X")));
	oge->X = omxNewMatrixFromRPrimitive(Rmtx, currentState, 0, 0);
  }
  //Eigy (local) will have however many rows and 1 column:
  Eigen::Map< Eigen::MatrixXd > Eigy(omxMatrixDataColumnMajor(oge->y->dataMat), oge->y->dataMat->cols, 1);
  if(oge->X->rows != Eigy.rows()){Rf_error("'X' and 'y' matrices have different numbers of rows");}
  //means:
  oge->means = omxInitMatrix(Eigy.rows(), 1, 1, currentState);
  //logdetV_om:
  oge->logdetV_om = omxInitMatrix(1, 1, 1, currentState);
  oge->logdetV_om->data[0] = 0;
  //cholV_fail_om:
  oge->cholV_fail_om = omxInitMatrix(1, 1, 1, currentState);
  oge->cholV_fail_om->data[0] = 0;
  //quadXinv:
  oge->quadXinv.setZero(oge->X->cols, oge->X->cols);

  //Deal with missing data:
  int* casesToDrop_intptr;
  oge->numcases2drop = 0;
  {
  ScopedProtect p1(casesToDrop, R_do_slot(rObj, Rf_install("casesToDrop")));
  if(Rf_length(casesToDrop)){
    if(OMX_DEBUG) { mxLog("Preparing GREML expectation to handle missing data."); }
    oge->numcases2drop = Rf_length(casesToDrop);
    casesToDrop_intptr = INTEGER(casesToDrop);
    oge->dropcase.assign(oge->cov->rows,0);
    for(i=0; i < Rf_length(casesToDrop); i++){
      if(casesToDrop_intptr[i] >= oge->cov->rows){
        Rf_warning("casesToDrop vector in GREML expectation contains indices greater than the number of datapoints");
        oge->numcases2drop--;
      }
      //Need to subtract 1 from the index because R begins array indexing with 1, not 0:
      else{oge->dropcase[casesToDrop_intptr[i]-1] = 1;}
  }}
  }
  if(Eigy.rows() != oge->cov->rows - oge->numcases2drop){
    Rf_error("y and V matrices do not have equal numbers of rows");
  }

  //column names of y and X:
  {
  ScopedProtect p1(yXcolnames, R_do_slot(rObj, Rf_install("yXcolnames")));
  oge->yXcolnames.resize(Rf_length(yXcolnames));
  for(i=0; i < Rf_length(yXcolnames); i++){
    SEXP elem;
    {ScopedProtect p2(elem, STRING_ELT(yXcolnames, i));
  	oge->yXcolnames[i] = CHAR(elem);}
  }
  }

  //Initially compute everything involved in computing means:
  oge->alwaysComputeMeans = 1;
  oge->cholquadX_fail = 0;
  EigenMatrixAdaptor EigX(oge->X);
  Eigen::Map< Eigen::MatrixXd > yhat(omxMatrixDataColumnMajor(oge->means), oge->means->rows, oge->means->cols);
  Eigen::MatrixXd EigV(Eigy.rows(), Eigy.rows());
  Eigen::MatrixXd quadX(oge->X->cols, oge->X->cols);
  //Apparently you need to initialize a matrix's elements before you try to write to its lower triangle:
  quadX.setZero(oge->X->cols, oge->X->cols);
  Eigen::LLT< Eigen::MatrixXd > cholV(Eigy.rows());
  Eigen::LLT< Eigen::MatrixXd > cholquadX(oge->X->cols);
  if( oge->numcases2drop ){
    dropCasesAndEigenize(oge->cov, EigV, oge->numcases2drop, oge->dropcase, 1);
  }
  else{EigV = Eigen::Map< Eigen::MatrixXd >(omxMatrixDataColumnMajor(oge->cov), oge->cov->rows, oge->cov->cols);}
  //invcov:
  oge->invcov = omxInitMatrix(EigV.rows(), EigV.cols(), 1, currentState);
  Eigen::Map< Eigen::MatrixXd > Vinv(omxMatrixDataColumnMajor(oge->invcov), EigV.rows(), EigV.cols());
  cholV.compute(EigV.selfadjointView<Eigen::Lower>());
  if(cholV.info() != Eigen::Success){
    Rf_error("Expected covariance matrix is non-positive-definite at initial values");
  }
  oge->cholV_vectorD = (( Eigen::MatrixXd )(cholV.matrixL())).diagonal();
  for(i=0; i < oge->X->rows; i++){
    oge->logdetV_om->data[0] += log(oge->cholV_vectorD[i]);
  }
  oge->logdetV_om->data[0] *= 2;
  Vinv = cholV.solve(Eigen::MatrixXd::Identity( EigV.rows(), EigV.cols() )); //<-- V inverse
  oge->XtVinv = EigX.transpose() * Vinv;
  quadX.triangularView<Eigen::Lower>() = oge->XtVinv * EigX;
  cholquadX.compute(quadX.selfadjointView<Eigen::Lower>());
  if(cholquadX.info() != Eigen::Success){
    Rf_error("Cholesky factorization failed at initial values; possibly, the matrix of covariates is rank-deficient");
  }
  oge->cholquadX_vectorD = (( Eigen::MatrixXd )(cholquadX.matrixL())).diagonal();
  oge->quadXinv = ( cholquadX.solve(Eigen::MatrixXd::Identity(oge->X->cols, oge->X->cols)) ).triangularView<Eigen::Lower>();
  yhat = EigX * oge->quadXinv.selfadjointView<Eigen::Lower>() * oge->XtVinv * Eigy;

  /*Prepare y as the data that the FIML fitfunction will use:*/
  oge->data2 = ox->data;
  ox->data = oge->y;
  if (oge->data2->hasDefinitionVariables()) {
	  Rf_error("definition variables are incompatible (and unnecessary) with GREML expectation");
  }
}

void omxComputeGREMLExpectation(omxExpectation* ox, const char *, const char *) {
  omxGREMLExpectation* oge = (omxGREMLExpectation*) (ox->argStruct);
	omxRecompute(oge->cov, NULL);
  int i=0;
  oge->cholV_fail_om->data[0] = 0;
  oge->cholquadX_fail = 0;
  oge->logdetV_om->data[0] = 0;

  EigenMatrixAdaptor EigX(oge->X);
  Eigen::Map< Eigen::MatrixXd > Eigy(omxMatrixDataColumnMajor(oge->y->dataMat), oge->y->dataMat->cols, 1);
  Eigen::Map< Eigen::MatrixXd > yhat(omxMatrixDataColumnMajor(oge->means), oge->means->rows, oge->means->cols);
  Eigen::MatrixXd EigV(Eigy.rows(), Eigy.rows());
  Eigen::Map< Eigen::MatrixXd > Vinv(omxMatrixDataColumnMajor(oge->invcov), oge->invcov->rows, oge->invcov->cols);
  Eigen::MatrixXd quadX(oge->X->cols, oge->X->cols);
  quadX.setZero(oge->X->cols, oge->X->cols);
  Eigen::LLT< Eigen::MatrixXd > cholV(oge->y->dataMat->rows);
  Eigen::LLT< Eigen::MatrixXd > cholquadX(oge->X->cols);
  if( oge->numcases2drop ){
    dropCasesAndEigenize(oge->cov, EigV, oge->numcases2drop, oge->dropcase, 1);
  }
  else{EigV = Eigen::Map< Eigen::MatrixXd >(omxMatrixDataColumnMajor(oge->cov), oge->cov->rows, oge->cov->cols);}
  cholV.compute(EigV.selfadjointView<Eigen::Lower>());
  if(cholV.info() != Eigen::Success){
    oge->cholV_fail_om->data[0] = 1;
    return;
  }
  oge->cholV_vectorD = (( Eigen::MatrixXd )(cholV.matrixL())).diagonal();
  for(i=0; i < oge->X->rows; i++){
    oge->logdetV_om->data[0] += log(oge->cholV_vectorD[i]);
  }
  oge->logdetV_om->data[0] *= 2;
  if(oge->alwaysComputeMeans){
  	Vinv = cholV.solve(Eigen::MatrixXd::Identity( EigV.rows(), EigV.cols() )); //<-- V inverse
  	oge->XtVinv = EigX.transpose() * Vinv;
  }
  /*alwaysComputeMeans is initialized as true, and the only way it can be set to false is by the GREML
  fitfunction.  If its false, that means that the GREML fitfunction is being used, and it knows how to handle
  a "half-full" Vinv.*/
  else{
  	//V inverse:
  	Vinv.triangularView<Eigen::Lower>() = ( cholV.solve(Eigen::MatrixXd::Identity( EigV.rows(), EigV.cols() )) ).triangularView<Eigen::Lower>();
  	oge->XtVinv = EigX.transpose() * Vinv.selfadjointView<Eigen::Lower>();
  }
  quadX.triangularView<Eigen::Lower>() = oge->XtVinv * EigX;
  cholquadX.compute(quadX.selfadjointView<Eigen::Lower>());
  if(cholquadX.info() != Eigen::Success){
    oge->cholquadX_fail = 1;
    return;
  }
  oge->cholquadX_vectorD = (( Eigen::MatrixXd )(cholquadX.matrixL())).diagonal();
  oge->quadXinv = ( cholquadX.solve(Eigen::MatrixXd::Identity(oge->X->cols, oge->X->cols)) ).triangularView<Eigen::Lower>();
  if(oge->alwaysComputeMeans){
    yhat = EigX * oge->quadXinv.selfadjointView<Eigen::Lower>() * oge->XtVinv * Eigy;
  }
}

void omxDestroyGREMLExpectation(omxExpectation* ox) {
	if(OMX_DEBUG) { mxLog("Destroying GREML Expectation."); }
  omxGREMLExpectation* argStruct = (omxGREMLExpectation*)(ox->argStruct);
  ox->data = argStruct->data2;
  omxFreeMatrix(argStruct->means);
  omxFreeMatrix(argStruct->invcov);
  omxFreeMatrix(argStruct->logdetV_om);
  omxFreeMatrix(argStruct->cholV_fail_om);
}

/*Possible TODO: it will require some additional computation, but it is probably best to calculate the final
regression coefficients using QR, which is more numerically stable*/
void omxPopulateGREMLAttributes(omxExpectation *ox, SEXP algebra) {
  if(OMX_DEBUG) { mxLog("Populating GREML expectation attributes."); }

  omxGREMLExpectation* oge = (omxGREMLExpectation*) (ox->argStruct);

  Rf_setAttrib(algebra, Rf_install("numStats"), Rf_ScalarReal(oge->y->dataMat->cols));
  Rf_setAttrib(algebra, Rf_install("numFixEff"), Rf_ScalarInteger(oge->X->cols));

  Eigen::Map< Eigen::MatrixXd > Eigy(omxMatrixDataColumnMajor(oge->y->dataMat), oge->y->dataMat->cols, 1);
  SEXP b_ext, bcov_ext, yXcolnames;
  oge->quadXinv = oge->quadXinv.selfadjointView<Eigen::Lower>();
  Eigen::MatrixXd GREML_b = oge->quadXinv * oge->XtVinv * Eigy;

  {
  ScopedProtect p1(b_ext, Rf_allocMatrix(REALSXP, GREML_b.rows(), 1));
  for(int row = 0; row < GREML_b.rows(); row++){
    REAL(b_ext)[0 * GREML_b.rows() + row] = GREML_b(row,0);
  }
  Rf_setAttrib(algebra, Rf_install("b"), b_ext);
  }

  {
  ScopedProtect p1(bcov_ext, Rf_allocMatrix(REALSXP, oge->quadXinv.rows(),
  	oge->quadXinv.cols()));
  for(int row = 0; row < oge->quadXinv.rows(); row++){
    for(int col = 0; col < oge->quadXinv.cols(); col++){
      REAL(bcov_ext)[col * oge->quadXinv.rows() + row] = oge->quadXinv(row,col);
  }}
  Rf_setAttrib(algebra, Rf_install("bcov"), bcov_ext);
  }

  //yXcolnames:
  {
  ScopedProtect p1(yXcolnames, Rf_allocVector(STRSXP, oge->yXcolnames.size()));
  for(int i=0; i < (int)(oge->yXcolnames.size()); i++){
    SET_STRING_ELT(yXcolnames, i, Rf_mkChar(oge->yXcolnames[i]));
  }
  Rf_setAttrib(algebra, Rf_install("yXcolnames"), yXcolnames);
  }

}

omxMatrix* omxGetGREMLExpectationComponent(omxExpectation* ox, omxFitFunction* off, const char* component){
/* Return appropriate parts of Expectation to the Fit Function */
  if(OMX_DEBUG) { mxLog("GREML expectation: %s requested--", component); }

	omxGREMLExpectation* oge = (omxGREMLExpectation*)(ox->argStruct);
	omxMatrix* retval = NULL;

  if(strEQ("y", component)) {
    retval = oge->y->dataMat;
	}
  else if(strEQ("invcov", component)) {
    retval = oge->invcov;
  }
  else if(strEQ("means", component)) {
  	retval = oge->means;
  }
  else if(strEQ("cholV_fail_om", component)){
    retval = oge->cholV_fail_om;
  }
  else if(strEQ("logdetV_om", component)){
    retval = oge->logdetV_om;
  }
  else if(strEQ("cov", component)) {
		retval = oge->cov;
	}
  else if(strEQ("X", component)) {
		retval = oge->X;
	}

	if (retval) omxRecompute(retval, NULL);

	return retval;
}

static double omxAliasedMatrixElement(omxMatrix *om, int row, int col)
{
  int index = 0;
  if(row >= om->originalRows || col >= om->originalCols) {
  	char *errstr = (char*) calloc(250, sizeof(char));
		sprintf(errstr, "Requested improper value (%d, %d) from (%d, %d) matrix.",
			row + 1, col + 1, om->originalRows, om->originalCols);
		Rf_error(errstr);
		free(errstr);  // TODO not reached
        return (NA_REAL);
	}
	if(om->colMajor) {
		index = col * om->originalRows + row;
	} else {
		index = row * om->originalCols + col;
	}
	return om->data[index];
}

void dropCasesAndEigenize(omxMatrix* om, Eigen::MatrixXd &em, int num2drop, std::vector< int > todrop,
	int symmetric){

  if(OMX_DEBUG) { mxLog("Trimming out cases with missing data..."); }

  if(num2drop < 1){ return; }

  omxEnsureColumnMajor(om);

  if(om->algebra == NULL){ //i.e., if omxMatrix is from a frontend MxMatrix

    em.setZero(om->rows - num2drop, om->cols - num2drop);

    int nextCol = 0;
    int nextRow = 0;

    for(int j = 0; j < om->cols; j++) {
  	  if(todrop[j]) continue;
  		nextRow = (symmetric ? nextCol : 0);
  		for(int k = (symmetric ? j : 0); k < om->rows; k++) {
  			if(todrop[k]) continue;
  			em(nextRow,nextCol) = omxAliasedMatrixElement(om, k, j);
  			nextRow++;
  		}
  		nextCol++;
  	}
  }
  else{ /*If the omxMatrix is from an algebra, then copying is not necessary; it can be resized directly
  and Eigen-mapped, since the algebra will be recalculated back to its original dimensions anyhow.*/
    if(om->originalRows == 0 || om->originalCols == 0) Rf_error("Not allocated");
    if (om->rows != om->originalRows || om->cols != om->originalCols) {
      // Feasible, but the code is currently not robust to this case
      Rf_error("Can only omxRemoveRowsAndColumns once");
    }

    int oldRows = om->originalRows;
    int oldCols = om->originalCols;

    int nextCol = 0;
    int nextRow = 0;

    om->rows = oldRows - num2drop;
    om->cols = oldCols - num2drop;

    for(int j = 0; j < oldCols; j++) {
      if(todrop[j]) continue;
      nextRow = (symmetric ? nextCol : 0);
      for(int k = (symmetric ? j : 0); k < oldRows; k++) {
        if(todrop[k]) continue;
        omxSetMatrixElement(om, nextRow, nextCol, omxAliasedMatrixElement(om, k, j));
        nextRow++;
      }
      nextCol++;
    }
    em = Eigen::Map< Eigen::MatrixXd >(om->data, om->rows, om->cols);
    omxMarkDirty(om); //<--Need to mark it dirty so that it gets recalculated back to original dimensions.
  }
  if(OMX_DEBUG) { mxLog("Finished trimming out cases with missing data..."); }
}


#include <iostream>
#include <ifaddrs.h>
#include <sys/sysctl.h>
#include <net/if_dl.h>
#include <net/if_types.h>

#include <boost/asio.hpp>

#include <cryptopp/sha.h>
#include <cryptopp/filters.h>
#include <cryptopp/hex.h>

#include "ComputerID.h"

namespace utils
{

std::string SHA1(const std::string& data)
{
    std::string digest;
    CryptoPP::SHA1 hash;

    CryptoPP::StringSource foo(data, true,
        new CryptoPP::HashFilter(hash,
            new CryptoPP::HexEncoder(
                new CryptoPP::StringSink(digest), false)));

    return digest;
}

std::string getMacString()
{
    struct ifaddrs *ifaddr;
    int family, s;
    char host[NI_MAXHOST];

    if (getifaddrs(&ifaddr) == -1)
    {
        throw std::runtime_error("call to getifaddrs failed");
    }

    int mib[6];
    mib[0] = CTL_NET;
    mib[1] = AF_ROUTE;
    mib[2] = 0;
    mib[3] = AF_LINK;
    mib[4] = NET_RT_IFLIST;

    std::size_t len;
    int idx = 0;
    char *buf;

    struct if_msghdr    *ifm;
    struct sockaddr_dl  *sdl;
    unsigned char       *ptr;

    std::stringstream   builder;

    while (ifaddr != nullptr)
    {
        if (ifaddr->ifa_addr != nullptr)
        {
            auto family = ifaddr->ifa_addr->sa_family;

            if ((mib[5] = if_nametoindex(ifaddr->ifa_name)) == 0)
            {
                continue;
            }

            if (sysctl(mib, 6, nullptr, &len, nullptr, 0) < 0)
            {
                continue;
            }

            if ((buf = static_cast<char*>(malloc(len))) == nullptr)
            {
                continue;
            }

            if (sysctl(mib, 6, buf, &len, nullptr, 0) < 0)
            {
                continue;
            }

            ifm = (struct if_msghdr *)buf;
            sdl = (struct sockaddr_dl *)(ifm + 1);

            if (sdl->sdl_type == IFT_ETHER)
            {
                ptr = (unsigned char *)LLADDR(sdl);

                builder
                    << std::hex
                    << static_cast<int>(*ptr)
                    << ':'
                    << static_cast<int>(*(ptr + 1))
                    << ':'
                    << static_cast<int>(*(ptr + 2))
                    << ':'
                    << static_cast<int>(*(ptr + 3))
                    << ':'
                    << static_cast<int>(*(ptr + 4))
                    << ':'
                    << static_cast<int>(*(ptr + 5));
            }

            free(buf);
        }

        ++idx;
        ifaddr = ifaddr->ifa_next;
    }

    return builder.str();
}

ComputerID::ComputerID() = default;

std::string ComputerID::getUUID()
{
    namespace ip = boost::asio::ip;
    boost::asio::io_context io_service;

    std::stringstream ss;
    ss << ip::host_name()
        << getMacString();
    if (_uniquePerProcess)
    {
        ss << static_cast<void*>(this);
    }

    if (_customData.size() > 0)
    {
        ss << _customData;
    }

    return SHA1(ss.str());
}

} // namespace utils


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IF_SUB_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IF_SUB_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-boolean
    Function object implementing if_sub capabilities

    The function returns the second entry or the difference of the
    second and third entries, according to the first entry being False
    or True

    This is a convenient alias of @ref selsub
  **/
  const boost::dispatch::functor<tag::if_sub_> if_sub = {};
} }
#endif

#include <boost/simd/function/scalar/selsub.hpp>
#include <boost/simd/function/simd/if_sub.hpp>

#endif


/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 *     its contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Jan 28, 2015
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *    Modified: Andreas Forster (an.forster@gmail.com)
 *********************************************************************************/

/**
 * @file cameras/PinholeCamera.hpp
 * @brief Header file for the PinholeCamera class.
 * @author Stefan Leutenegger
 * @author Andreas Forster
 */

#ifndef INCLUDE_OKVIS_CAMERAS_PINHOLECAMERA_HPP_
#define INCLUDE_OKVIS_CAMERAS_PINHOLECAMERA_HPP_

#include <vector>
#include <memory>
#include <stdint.h>
#include <Eigen/Core>
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
#include <opencv2/core/core.hpp> // Code that causes warning goes here
#pragma GCC diagnostic pop
#include "okvis/cameras/CameraBase.hpp"
#include "okvis/cameras/DistortionBase.hpp"

/// \brief okvis Main namespace of this package.
namespace okvis {
/// \brief cameras Namespace for camera-related functionality.
namespace cameras {

/// \class PinholeCamera<DISTORTION_T>
/// \brief This implements a standard pinhole camera projection model.
/// \tparam DISTORTION_T the distortion type, e.g. okvis::cameras::RadialTangentialDistortion
template<class DISTORTION_T>
class PinholeCamera : public CameraBase
{
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typedef DISTORTION_T distortion_t; ///< Makes the distortion type accessible.

  /// \brief Constructor that will figure out the type of distortion
  /// @param[in] imageWidth The width in pixels.
  /// @param[in] imageHeight The height in pixels.
  /// @param[in] focalLengthU The horizontal focal length in pixels.
  /// @param[in] focalLengthV The vertical focal length in pixels.
  /// @param[in] imageCenterU The horizontal centre in pixels.
  /// @param[in] imageCenterV The vertical centre in pixels.
  /// @param[in] distortion The distortion object to be used.
  /// @param[in] id Assign a generic ID, if desired.
  PinholeCamera(int imageWidth, int imageHeight, double focalLengthU,
                double focalLengthV, double imageCenterU, double imageCenterV,
                const distortion_t & distortion, uint64_t id=-1);

  /// \brief Destructor.
  ~PinholeCamera()
  {
  }

  static const int NumProjectionIntrinsics = 4;  ///< optimisable projection intrinsics
  static const int NumIntrinsics = NumProjectionIntrinsics
      + distortion_t::NumDistortionIntrinsics; ///< total number of intrinsics

  /// \brief Get the focal length along the u-dimension.
  /// \return The horizontal focal length in pixels.
  double focalLengthU() const
  {
    return fu_;
  }

  /// \brief Get the focal length along the v-dimension.
  /// \return The vertical focal length in pixels.
  double focalLengthV() const
  {
    return fv_;
  }

  /// \brief Get the image centre along the u-dimension.
  /// \return The horizontal centre in pixels.
  double imageCenterU() const
  {
    return cu_;
  }

  /// \brief Get the focal image centre along the v-dimension.
  /// \return The vertical centre in pixels.
  double imageCenterV() const
  {
    return cv_;
  }

  /// \brief Get the intrinsics as a concatenated vector.
  /// \return The intrinsics as a concatenated vector.
  inline void getIntrinsics(Eigen::VectorXd & intrinsics) const ;

  /// \brief overwrite all intrinsics - use with caution !
  /// \param[in] intrinsics The intrinsics as a concatenated vector.
  inline bool setIntrinsics(const Eigen::VectorXd & intrinsics);

  /// \brief Get the total number of intrinsics.
  /// \return Number of intrinsics parameters.
  inline int noIntrinsicsParameters() const
  {
    return NumIntrinsics;
  }

  //////////////////////////////////////////////////////////////
  /// \name Methods to project points
  /// @{

  /// \brief Projects a Euclidean point to a 2d image point (projection).
  ///        Uses projection including distortion models.
  /// @param[in]  point      The point in Euclidean coordinates.
  /// @param[out] imagePoint The image point.
  /// @return     Get information about the success of the projection. See
  ///             \ref ProjectionStatus for more information.
  inline CameraBase::ProjectionStatus project(
      const Eigen::Vector3d & point, Eigen::Vector2d * imagePoint) const;

  /// \brief Projects a Euclidean point to a 2d image point (projection).
  ///        Uses projection including distortion models.
  /// @param[in]  point              The point in Euclidean coordinates.
  /// @param[out] imagePoint         The image point.
  /// @param[out] pointJacobian      The Jacobian of the projection function w.r.t. the point..
  /// @param[out] intrinsicsJacobian The Jacobian of the projection function w.r.t. the intinsics.
  /// @return     Get information about the success of the projection. See
  ///             \ref ProjectionStatus for more information.
  inline CameraBase::ProjectionStatus project(
      const Eigen::Vector3d & point, Eigen::Vector2d * imagePoint,
      Eigen::Matrix<double, 2, 3> * pointJacobian,
      Eigen::Matrix2Xd * intrinsicsJacobian = NULL) const;

  /// \brief Projects a Euclidean point to a 2d image point (projection).
  ///        Uses projection including distortion models.
  /// @param[in]  point              The point in Euclidean coordinates.
  /// @param[in]  parameters         The intrinsics.
  /// @param[out] imagePoint         The image point.
  /// @param[out] pointJacobian      The Jacobian of the projection function w.r.t. the point..
  /// @param[out] intrinsicsJacobian The Jacobian of the projection function w.r.t. the intinsics.
  /// @return     Get information about the success of the projection. See
  ///             \ref ProjectionStatus for more information.
  inline CameraBase::ProjectionStatus projectWithExternalParameters(
      const Eigen::Vector3d & point, const Eigen::VectorXd & parameters,
      Eigen::Vector2d * imagePoint, Eigen::Matrix<double, 2, 3> * pointJacobian,
      Eigen::Matrix2Xd * intrinsicsJacobian = NULL) const;

  /// \brief Projects Euclidean points to 2d image points (projection) in a batch.
  ///        Uses projection including distortion models.
  /// @param[in]  points      The points in Euclidean coordinates (one point per column).
  /// @param[out] imagePoints The image points (one point per column).
  /// @param[out] stati       Get information about the success of the projections. See
  ///                         \ref ProjectionStatus for more information.
  inline void projectBatch(
      const Eigen::Matrix3Xd & points, Eigen::Matrix2Xd * imagePoints,
      std::vector<CameraBase::ProjectionStatus> * stati) const;

  /// \brief Projects a point in homogenous coordinates to a 2d image point (projection).
  ///        Uses projection including distortion models.
  /// @param[in]  point      The point in Homogeneous coordinates.
  /// @param[out] imagePoint The image point.
  /// @return     Get information about the success of the projection. See
  ///             \ref ProjectionStatus for more information.
  inline CameraBase::ProjectionStatus projectHomogeneous(
      const Eigen::Vector4d & point, Eigen::Vector2d * imagePoint) const;

  /// \brief Projects a point in homogenous coordinates to a 2d image point (projection).
  ///        Uses projection including distortion models.
  /// @param[in]  point              The point in Homogeneous coordinates.
  /// @param[out] imagePoint         The image point.
  /// @param[out] pointJacobian      The Jacobian of the projection function w.r.t. the point.
  /// @param[out] intrinsicsJacobian The Jacobian of the projection function w.r.t. the intrinsics.
  /// @return     Get information about the success of the projection. See
  ///             \ref ProjectionStatus for more information.
  inline CameraBase::ProjectionStatus projectHomogeneous(
      const Eigen::Vector4d & point, Eigen::Vector2d * imagePoint,
      Eigen::Matrix<double, 2, 4> * pointJacobian,
      Eigen::Matrix2Xd * intrinsicsJacobian = NULL) const;

  /// \brief Projects a point in homogenous coordinates to a 2d image point (projection).
  ///        Uses projection including distortion models.
  /// @param[in]  point              The point in Homogeneous coordinates.
  /// @param[in]  parameters         The intrinsics.
  /// @param[out] imagePoint         The image point.
  /// @param[out] pointJacobian      The Jacobian of the projection function w.r.t. the point.
  /// @param[out] intrinsicsJacobian The Jacobian of the projection function w.r.t. the intrinsics.
  /// @return     Get information about the success of the projection. See
  ///             \ref ProjectionStatus for more information.
  inline CameraBase::ProjectionStatus projectHomogeneousWithExternalParameters(
      const Eigen::Vector4d & point, const Eigen::VectorXd & parameters,
      Eigen::Vector2d * imagePoint,
      Eigen::Matrix<double, 2, 4> * pointJacobian = NULL,
      Eigen::Matrix2Xd * intrinsicsJacobian = NULL) const;

  /// \brief Projects points in homogenous coordinates to 2d image points (projection) in a batch.
  ///        Uses projection including distortion models.
  /// @param[in]  points      The points in homogeneous coordinates (one point per column).
  /// @param[out] imagePoints The image points (one point per column).
  /// @param[out] stati       Get information about the success of the projections. See
  ///                         \ref ProjectionStatus for more information.
  inline void projectHomogeneousBatch(
      const Eigen::Matrix4Xd & points, Eigen::Matrix2Xd * imagePoints,
      std::vector<CameraBase::ProjectionStatus> * stati) const;
  /// @}

  //////////////////////////////////////////////////////////////
  /// \name Methods to backproject points
  /// @{

  /// \brief Back-project a 2d image point into Euclidean space (direction vector).
  /// @param[in]  imagePoint The image point.
  /// @param[out] direction  The Euclidean direction vector.
  /// @return     true on success.
  inline bool backProject(const Eigen::Vector2d & imagePoint,
                          Eigen::Vector3d * direction) const;

  /// \brief Back-project a 2d image point into Euclidean space (direction vector).
  /// @param[in]  imagePoint         The image point.
  /// @param[out] direction          The Euclidean direction vector.
  /// @param[out] pointJacobian      Jacobian of the back-projection function  w.r.t. the point.
  /// @return     true on success.
  inline bool backProject(const Eigen::Vector2d & imagePoint,
                          Eigen::Vector3d * direction,
                          Eigen::Matrix<double, 3, 2> * pointJacobian) const;

  /// \brief Back-project 2d image points into Euclidean space (direction vectors).
  /// @param[in]  imagePoints The image points (one point per column).
  /// @param[out] directions  The Euclidean direction vectors (one point per column).
  /// @param[out] success     Success of each of the back-projection
  inline bool backProjectBatch(const Eigen::Matrix2Xd & imagePoints,
                               Eigen::Matrix3Xd * directions,
                               std::vector<bool> * success) const;

  /// \brief Back-project a 2d image point into homogeneous point (direction vector).
  /// @param[in]  imagePoint The image point.
  /// @param[out] direction  The homogeneous point as direction vector.
  /// @return     true on success.
  inline bool backProjectHomogeneous(const Eigen::Vector2d & imagePoint,
                                     Eigen::Vector4d * direction) const;

  /// \brief Back-project a 2d image point into homogeneous point (direction vector).
  /// @param[in]  imagePoint         The image point.
  /// @param[out] direction          The homogeneous point as direction vector.
  /// @param[out] pointJacobian      Jacobian of the back-projection function.
  /// @return     true on success.
  inline bool backProjectHomogeneous(
      const Eigen::Vector2d & imagePoint, Eigen::Vector4d * direction,
      Eigen::Matrix<double, 4, 2> * pointJacobian) const;

  /// \brief Back-project 2d image points into homogeneous points (direction vectors).
  /// @param[in]  imagePoints The image points (one point per column).
  /// @param[out] directions  The homogeneous points as direction vectors (one point per column).
  /// @param[out] success     Success of each of the back-projection
  inline bool backProjectHomogeneousBatch(const Eigen::Matrix2Xd & imagePoints,
                                          Eigen::Matrix4Xd * directions,
                                          std::vector<bool> * success) const;
  /// @}

  /// \brief get a test instance
  static std::shared_ptr<CameraBase> createTestObject()
  {
    return std::shared_ptr<CameraBase>(new PinholeCamera(752, 480, 350, 360, 378, 238,
                         distortion_t::testObject()));
  }
  /// \brief get a test instance
  static PinholeCamera testObject()
  {
    return PinholeCamera(752, 480, 350, 360, 378, 238,
                          distortion_t::testObject());
  }

  /// \brief Obtain the projection type
  std::string type() const
  {
    return "PinholeCamera<" + distortion_.type() + ">";
  }

  /// \brief Obtain the projection type
  const std::string distortionType() const
  {
    return distortion_.type();
  }

 protected:

  /// \brief No default constructor.
  PinholeCamera() = delete;

  distortion_t distortion_;  ///< the distortion to be used

  Eigen::Matrix<double, NumIntrinsics, 1> intrinsics_;  ///< summary of all intrinsics parameters
  double fu_;  ///< focalLengthU
  double fv_;  ///< focalLengthV
  double cu_;  ///< imageCenterU
  double cv_;  ///< imageCenterV
  double one_over_fu_;  ///< 1.0 / fu_
  double one_over_fv_;  ///< 1.0 / fv_
  double fu_over_fv_;  ///< fu_ / fv_

};

}  // namespace cameras
}  // namespace okvis

#include "implementation/PinholeCamera.hpp"

#endif /* INCLUDE_OKVIS_CAMERAS_PINHOLECAMERA_HPP_ */


#include <vector>
#include <cmath>
#include <boost/dynamic_bitset.hpp>
using std::vector;
using std::pow;
using boost::dynamic_bitset;

class BNode
{
private:
    vector<dynamic_bitset<>> description;
    int strings;
    int bits;
    int height;
public:
    BNode(vector<dynamic_bitset<>> desc, int strings, int bits, int height);
    ~BNode();
    int numChildren();
    bool canFirstChild();
    BNode* firstChild();
    bool canNextSibling();
    BNode* nextSibling();
};


/*
 * B3M Hardware interface for OPR_V3
 */
#include <boost/shared_ptr.hpp>
#include <math.h>
#include "ros/ros.h"
#include "controller_manager/controller_manager.h"
#include "hardware_interface/joint_command_interface.h"
#include "hardware_interface/joint_state_interface.h"
#include "hardware_interface/actuator_command_interface.h"
#include "hardware_interface/actuator_state_interface.h"
#include "hardware_interface/robot_hw.h"
#include "opr_kondo_driver/b3m.hpp"

#define SERV_NUM 19
#define REDUCTION_RATIO (2.0f)

const double RAD_TO_DEG = 180.0/M_PI*100;

double deg100_to_radian (double deg100)
{
    return deg100/RAD_TO_DEG;
}

int radian_to_deg100 (double radian)
{
    return radian*RAD_TO_DEG;
}

class KondoMotor
{
    public:
        int dir;
        double offset;
        double cmd, pos, vel, eff;
        std::string joint_name;
        int id, min_angle, max_angle, control_gain_preset;
        int control_kp0, control_kp1, control_kp2;
        int control_kd0, control_kd1, control_kd2;
        int control_ki0, control_ki1, control_ki2;
        int control_static_friction0, control_static_friction1, control_static_friction2;
        int control_dynamic_friction0, control_dynamic_friction1, control_dynamic_friction2;

        KondoMotor (std::string actuator_name, hardware_interface::JointStateInterface& state_interface, hardware_interface::PositionJointInterface& pos_interface) : cmd(0.0), pos(0.0), vel(0.0), eff(0.0) {

            ros::NodeHandle nh(std::string("~")+actuator_name);
            if (nh.getParam("id", id)) {
                ROS_INFO("id: %d", id);
            }
            if (nh.getParam("joint_name", joint_name)) {
                ROS_INFO("joint_name: %s", joint_name.c_str());
            }
            if (nh.getParam("min_angle", min_angle)) {
                ROS_INFO("min_angle: %d", min_angle);
            }
            if (nh.getParam("max_angle", max_angle)) {
                ROS_INFO("max_angle: %d", max_angle);
            }
            if (nh.getParam("offset", offset)) {
                ROS_INFO("offset: %f", offset);
            }
            if (nh.getParam("direction", dir)) {
                ROS_INFO("direction: %d", dir);
            }
            if (nh.getParam("control_kp0", control_kp0)) {
                ROS_INFO("control_kp0: %d", control_kp0);
            }
            if (nh.getParam("control_kd0", control_kd0)) {
                ROS_INFO("control_kd0: %d", control_kd0);
            }
            if (nh.getParam("control_ki0", control_ki0)) {
                ROS_INFO("control_ki0: %d", control_ki0);
            }
            if (nh.getParam("control_static_friction0", control_static_friction0)) {
                ROS_INFO("control_static_friction0: %d", control_static_friction0);
            }
            if (nh.getParam("control_dynamic_friction0", control_dynamic_friction0)) {
                ROS_INFO("control_dynamic_friction0: %d", control_dynamic_friction0);
            }
            if (nh.getParam("control_kp1", control_kp1)) {
                ROS_INFO("control_kp1: %d", control_kp1);
            }
            if (nh.getParam("control_kd1", control_kd1)) {
                ROS_INFO("control_kd1: %d", control_kd1);
            }
            if (nh.getParam("control_ki1", control_ki1)) {
                ROS_INFO("control_ki1: %d", control_ki1);
            }
            if (nh.getParam("control_static_friction1", control_static_friction1)) {
                ROS_INFO("control_static_friction1: %d", control_static_friction1);
            }
            if (nh.getParam("control_dynamic_friction1", control_dynamic_friction1)) {
                ROS_INFO("control_dynamic_friction1: %d", control_dynamic_friction1);
            }
            if (nh.getParam("control_kp2", control_kp2)) {
                ROS_INFO("control_kp2: %d", control_kp2);
            }
            if (nh.getParam("control_kd2", control_kd2)) {
                ROS_INFO("control_kd2: %d", control_kd2);
            }
            if (nh.getParam("control_ki2", control_ki2)) {
                ROS_INFO("control_ki2: %d", control_ki2);
            }
            if (nh.getParam("control_static_friction2", control_static_friction2)) {
                ROS_INFO("control_static_friction2: %d", control_static_friction2);
            }
            if (nh.getParam("control_dynamic_friction2", control_dynamic_friction2)) {
                ROS_INFO("control_dynamic_friction2: %d", control_dynamic_friction2);
            }
            if (nh.getParam("control_gain_preset", control_gain_preset)) {
                ROS_INFO("control_gain_preset: %d", control_gain_preset);
            }

            hardware_interface::JointStateHandle state_handle(joint_name, &pos, &vel, &eff);
            state_interface.registerHandle(state_handle);
            hardware_interface::JointHandle pos_handle(state_interface.getHandle(joint_name), &cmd);
            pos_interface.registerHandle(pos_handle);
        }

        void update(int *deg100)
        {
            double radian = cmd;
            if (radian < min_angle * 3.14 / 180) {
                radian = min_angle * 3.14 / 180;
            }
            if (radian > max_angle * 3.14 / 180) {
                radian = max_angle * 3.14 / 180;
            }

            if (joint_name == "left_waist_roll_joint" || joint_name == "right_waist_roll_joint"){
                radian *= REDUCTION_RATIO;
            }

            *deg100 = dir * radian_to_deg100(radian + offset);

            //TODO: get pos, vel, eff(torque) from b3m
            //pos = deg100_to_radian(dir * deg100) - offset;
            pos = cmd;
        }
};

class KondoDriver : public hardware_interface::RobotHW
{
    private:
        hardware_interface::JointStateInterface jnt_state_interface;
        hardware_interface::PositionJointInterface jnt_pos_interface;
        std::vector<boost::shared_ptr<KondoMotor> > actuator_vector;
        unsigned short goal_position[SERV_NUM] = {0};

    public:
        KondoDriver (int num, char** actuators)
        {
            ros::NodeHandle nh("~");

            std::string serial_port;
            nh.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0");
            ROS_INFO("serial_port: %s", serial_port.c_str());

            if(RSOpen(serial_port.c_str()) < 0){
                ROS_ERROR ("Could not open B3M port");
                exit(0);
            }

            for (int i=0; i<num; i++) {
                boost::shared_ptr<KondoMotor> actuator(new KondoMotor(std::string(actuators[i]), jnt_state_interface, jnt_pos_interface));
                actuator_vector.push_back(actuator);
            }
            registerInterface(&jnt_state_interface);
            registerInterface(&jnt_pos_interface);

            ROS_INFO("initialize b3m");
            B3MInitializeVariable();
            for (int i=0; i<actuator_vector.size(); i++) {
                int index = actuator_vector[i]->id - 1;
                b3m_control_kp0[index] = actuator_vector[i]->control_kp0;
                b3m_control_kd0[index] = actuator_vector[i]->control_kd0;
                b3m_control_ki0[index] = actuator_vector[i]->control_ki0;
                b3m_control_static_friction0[index] = actuator_vector[i]->control_static_friction0;
                b3m_control_dynamic_friction0[index] = actuator_vector[i]->control_dynamic_friction0;
                b3m_control_kp1[index] = actuator_vector[i]->control_kp1;
                b3m_control_kd1[index] = actuator_vector[i]->control_kd1;
                b3m_control_ki1[index] = actuator_vector[i]->control_ki1;
                b3m_control_static_friction1[index] = actuator_vector[i]->control_static_friction1;
                b3m_control_dynamic_friction1[index] = actuator_vector[i]->control_dynamic_friction1;
                b3m_control_kp2[index] = actuator_vector[i]->control_kp2;
                b3m_control_kd2[index] = actuator_vector[i]->control_kd2;
                b3m_control_ki2[index] = actuator_vector[i]->control_ki2;
                b3m_control_static_friction2[index] = actuator_vector[i]->control_static_friction2;
                b3m_control_dynamic_friction2[index] = actuator_vector[i]->control_dynamic_friction2;
                b3m_gain_preset[index] = actuator_vector[i]->control_gain_preset;
                b3m_servo_offset[index] = actuator_vector[i]->dir * radian_to_deg100(actuator_vector[i]->offset);
            }

            B3MInitializeSequence();
            ROS_INFO("initialize complete");
        }

        void update()
        {
            int deg100 = 0;
            int index = 0;
            for (int i=0; i<actuator_vector.size(); i++) {
                actuator_vector[i]->update(&deg100);
                index = actuator_vector[i]->id - 1;
                goal_position[index] = deg100;
            }

            //TODO: adjust gain depending on situation
            Write_Servo_B3M_All_2Kport(B3M_SERVO_DESIRED_POSITION, &goal_position[0], 2);
        }

        ~KondoDriver () {
            //Write_Servo_B3M_All_2Kport(B3M_SERVO_SERVO_MODE, &b3m_free_mode[0], 1);
            RSClose();
        }

        ros::Time getTime() const {return ros::Time::now();}
        ros::Duration getPeriod() const {return ros::Duration(0.05);}
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "kondo_driver");
    ros::NodeHandle nh;

    KondoDriver robot(argc-1, &argv[1]);
    controller_manager::ControllerManager cm(&robot, nh);

    ros::Rate rate(1.0 / robot.getPeriod().toSec());
    ros::AsyncSpinner spinner(1);
    spinner.start();

    while(ros::ok()){
        cm.update(robot.getTime(), robot.getPeriod());
        robot.update();
        rate.sleep();
    }
    spinner.stop();

    return 0;
}


#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <glog/logging.h>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <Eigen/Geometry>
#include <thread>

namespace modelify {
namespace loop_closure {
class PoseToTfNode {
 public:
  PoseToTfNode(const ros::NodeHandle& node_handle);
  void newPoseCallback(const geometry_msgs::PoseStamped& pose_msg);
  void PoseStampedToTransformStamped(const geometry_msgs::PoseStamped& pose,
                                     tf::StampedTransform* tf);

  /*
   * Gets T_I_D from parameter server and publishes as static transform. T_I_D
   * brings points in the depth camera frame to the imu frame.
   */
  void getAndPublishT_I_D();
  /*
   * Gets T_W_M from parameter server and publishes as static transform. T_W_M
   * brings points in the map frame used by rovioli to the world frame used by
   * tango.
   */
  void getAndPublishT_W_M();

  /*
   * Given quaternions and translation returns 4x4 transformation matrix.
   */
  Eigen::Matrix4f getMatrix(const std::vector<double>& quaternions,
                            const std::vector<double>& translation);

  void publishStaticTransform(const Eigen::Matrix4f& matrix,
                              const std::string& frame_id,
                              const std::string& child_frame_id);

  ros::NodeHandle node_handle_;
  ros::Subscriber markers_sub;
  tf::TransformBroadcaster tf_broadcaster;
  tf2_ros::StaticTransformBroadcaster tf_static;
  tf::TransformListener tf_listener_;
  Eigen::Matrix4f T_R_D;

  std::string world_frame_ = "world";
  std::string map_frame_ = "map";
  std::string imu_estimated_frame_ = "imu_estimated";
  std::string depth_estimated_frame_ = "depth_estimated";
  bool first_run = true;
};

PoseToTfNode::PoseToTfNode(const ros::NodeHandle& node_handle)
    : node_handle_(node_handle) {
  std::string topic = "/maplab_rovio/T_G_I";
  node_handle.param<std::string>("/pose_to_tf/pose_topic", topic, topic);
  markers_sub = node_handle_.subscribe(topic, 200000000,
                                       &PoseToTfNode::newPoseCallback, this);

  getAndPublishT_I_D();
}

void PoseToTfNode::newPoseCallback(const geometry_msgs::PoseStamped& pose_msg) {
  tf::StampedTransform T_W_I;
  PoseStampedToTransformStamped(pose_msg, &T_W_I);
  tf_broadcaster.sendTransform(T_W_I);

  if (first_run) {
    getAndPublishT_W_M();
    first_run = false;
  }
}

void PoseToTfNode::getAndPublishT_W_M() {
  ros::Time latest = ros::Time(0);
  tf_listener_.waitForTransform(map_frame_, depth_estimated_frame_, latest,
                                ros::Duration(50.0));
  tf::StampedTransform tf_m_d;
  tf_listener_.lookupTransform(map_frame_, depth_estimated_frame_, latest,
                               tf_m_d);

  tf::StampedTransform tf_w_d;
  tf_listener_.waitForTransform(world_frame_, "depth", tf_m_d.stamp_,
                                ros::Duration(50.0));
  tf_listener_.lookupTransform(world_frame_, "depth", tf_m_d.stamp_, tf_w_d);

  tf::Transform tf_w_m;
  tf_w_m = tf_w_d * tf_m_d.inverse();
  tf_w_m.setRotation(tf_w_m.getRotation().normalize());

  geometry_msgs::TransformStamped tf_w_m_stamped;
  tf_w_m_stamped.header.stamp = latest;
  tf_w_m_stamped.header.frame_id = world_frame_;
  tf_w_m_stamped.child_frame_id = map_frame_;
  tf_w_m_stamped.transform.translation.x = tf_w_m.getOrigin().x();
  tf_w_m_stamped.transform.translation.y = tf_w_m.getOrigin().y();
  tf_w_m_stamped.transform.translation.z = tf_w_m.getOrigin().z();
  tf_w_m_stamped.transform.rotation.x = tf_w_m.getRotation().x();
  tf_w_m_stamped.transform.rotation.y = tf_w_m.getRotation().y();
  tf_w_m_stamped.transform.rotation.z = tf_w_m.getRotation().z();
  tf_w_m_stamped.transform.rotation.w = tf_w_m.getRotation().w();
  tf_static.sendTransform(tf_w_m_stamped);
}

void PoseToTfNode::PoseStampedToTransformStamped(
    const geometry_msgs::PoseStamped& pose, tf::StampedTransform* tf) {
  CHECK_NOTNULL(tf);
  tf->frame_id_ = pose.header.frame_id;
  tf->child_frame_id_ = imu_estimated_frame_;
  tf->stamp_ = pose.header.stamp;
  tf->setRotation(
      tf::Quaternion(pose.pose.orientation.x, pose.pose.orientation.y,
                     pose.pose.orientation.z, pose.pose.orientation.w));
  tf->setOrigin(tf::Vector3(pose.pose.position.x, pose.pose.position.y,
                            pose.pose.position.z));
}

Eigen::Matrix4f PoseToTfNode::getMatrix(
    const std::vector<double>& quaternions,
    const std::vector<double>& translation) {
  Eigen::Quaternionf quaternion(quaternions[3], quaternions[0], quaternions[1],
                                quaternions[2]);
  Eigen::Vector3f base;
  Eigen::Matrix4f tf;
  tf.setIdentity();
  tf.block<3, 3>(0, 0) = quaternion.toRotationMatrix();
  tf.block<3, 1>(0, 3) << translation[0], translation[1], translation[2];
  return tf;
}

void PoseToTfNode::getAndPublishT_I_D() {
  std::vector<double> translation;
  std::vector<double> rotation;

  // T Imu tango to Depth
  node_handle_.getParam("/rovioli_marker_to_tf/T_I_D/quaternion", rotation);
  node_handle_.getParam("/rovioli_marker_to_tf/T_I_D/translation_m",
                        translation);
  Eigen::Matrix4f T_T_D = getMatrix(rotation, translation);

  // T imu tango to fisheye
  node_handle_.getParam("/rovioli_marker_to_tf/T_T_F/quaternion", rotation);
  node_handle_.getParam("/rovioli_marker_to_tf/T_T_F/translation_m",
                        translation);
  Eigen::Matrix4f T_T_F = getMatrix(rotation, translation);

  // T imu rovioli to fisheye
  std::vector<double> matrix_as_vector;
  node_handle_.getParam("/rovioli_marker_to_tf/T_R_F/matrix", matrix_as_vector);

  Eigen::Matrix4f T_R_F;
  for (size_t i = 0u; i < 4u; ++i) {
    for (size_t j = 0u; j < 4u; ++j) {
      T_R_F(i, j) = matrix_as_vector[4 * i + j];
    }
  }

  T_R_D = T_R_F * T_T_F.inverse() * T_T_D;

  publishStaticTransform(T_R_D, imu_estimated_frame_, depth_estimated_frame_);
}

void PoseToTfNode::publishStaticTransform(const Eigen::Matrix4f& matrix,
                                          const std::string& frame_id,
                                          const std::string& child_frame_id) {
  geometry_msgs::TransformStamped tf;
  Eigen::Quaternionf quat(matrix.block<3, 3>(0, 0));
  tf.transform.rotation.x = quat.x();
  tf.transform.rotation.y = quat.y();
  tf.transform.rotation.z = quat.z();
  tf.transform.rotation.w = quat.w();
  tf.transform.translation.x = matrix(0, 3);
  tf.transform.translation.y = matrix(1, 3);
  tf.transform.translation.z = matrix(2, 3);
  tf.header.frame_id = frame_id;
  tf.child_frame_id = child_frame_id;
  tf_static.sendTransform(tf);
}
}  // namespace loop_closure
}  // namespace modelify

int main(int argc, char* argv[]) {
  ros::init(argc, argv, "pose_to_tf");
  ros::NodeHandle node_handle;
  google::ParseCommandLineFlags(&argc, &argv, true);
  google::InitGoogleLogging(argv[0]);
  FLAGS_stderrthreshold = 0;
  LOG(INFO) << "Started converter marker to tf.";
  modelify::loop_closure::PoseToTfNode node(node_handle);

  ros::spin();
}