#include "tuw_marker_slam/ekf_slam.h"
#include "tuw_marker_slam/munkre.h"
#include <boost/math/distributions/chi_squared.hpp>

using namespace tuw;

EKFSLAM::EKFSLAM( const std::vector<double> beta ) :
    SLAMTechnique ( EKF ),
    beta_ ( beta ) {
}

void EKFSLAM::init() {
    reset_ = false;

    // initial mean
    y = cv::Mat_<double> ( 3, 1, 0.0 );

    // initial covariance
    C_Y = cv::Mat_<double> ( 3, 3, 0.0 );

    // initialize covariance submatrices pointing to the same data (no copy!)
    x = cv::Mat_<double> ( y, cv::Range ( 0, 3 ), cv::Range ( 0, 1 ) );
    C_X = cv::Mat_<double> ( C_Y, cv::Range ( 0, 3 ), cv::Range ( 0, 3 ) );

    // reset marker landmark correspondences
    f_kj.clear();
}

void EKFSLAM::cycle ( std::vector<Pose2D> &yt, cv::Mat_<double> &C_Yt, const Command &ut, const MeasurementConstPtr &zt ) {
    if ( reset_ ) init();

    // invariance check
    assert ( y.rows % 3 == 0 && y.cols == 1 );
    assert ( x.rows == 3 && x.cols == 1 );
    assert ( C_Y.rows == C_Y.cols && C_Y.rows == y.rows );
    assert ( C_X.rows == C_X.cols && C_X.rows == x.rows );

    prediction ( ut );
    if ( zt->getType() == tuw::Measurement::Type::MARKER && updateTimestamp ( zt->stamp() ) ) {
        MeasurementMarkerConstPtr z = std::static_pointer_cast<MeasurementMarker const> ( zt );

        data_association ( z );
        update();
        integration ( z );
    }

    // implicit return
    yt.resize ( y.rows/3 );
    for ( size_t i = 0; i < yt.size(); i++ ) {
        yt[i].x() = y[3*i + 0][0];
        yt[i].y() = y[3*i + 1][0];
        yt[i].theta() = y[3*i + 2][0];
    }
    C_Yt = C_Y;
}

void EKFSLAM::setConfig ( const void *config ) {
    config_ = * ( ( tuw_marker_slam::EKFSLAMConfig* ) config );
}

void EKFSLAM::prediction ( const Command &ut ) {
    if ( !config_.enable_prediction ) return;

    // control noise
    cv::Matx<double, 2, 2> M = cv::Matx<double, 2, 2> ( config_.alpha_1*ut.v() *ut.v() + config_.alpha_2*ut.w() *ut.w(), 0,
                               0, config_.alpha_3*ut.v() *ut.v() + config_.alpha_4*ut.w() *ut.w() );

    // pre-calculate needed data
    double dt = duration_last_update_.total_microseconds() /1000000.0;
    double r = ut.v() /ut.w();
    double s = sin ( x[2][0] );
    double c = cos ( x[2][0] );
    double s_dt = sin ( x[2][0] + ut.w() *dt );
    double c_dt = cos ( x[2][0] + ut.w() *dt );

    // calculate update and its derivatives (= jacobian matrices)
    cv::Vec<double, 3> dx_;
    cv::Matx<double, 3, 3> G_x_;
    cv::Matx<double, 3, 2> G_u;
    if ( std::abs ( ut.w() ) > __DBL_MIN__ ) {
        dx_ = cv::Vec<double, 3> ( -r*s + r*s_dt,
                                   r*c - r*c_dt,
                                   ut.w() *dt );
        G_x_ = cv::Matx<double, 3, 3> ( 1, 0, -r*c + r*c_dt,
                                        0, 1, -r*s + r*s_dt,
                                        0, 0, 1 );
        G_u = cv::Matx<double, 3, 2> ( ( -s + s_dt ) /ut.w(),  r * ( s - s_dt ) / ut.w() + r*c_dt*dt,
                                       ( c - c_dt ) /ut.w(), -r * ( c - c_dt ) / ut.w() + r*s_dt*dt,
                                       0, dt );
    } else {
        dx_ = cv::Vec<double, 3> ( ut.v() *dt*c,
                                   ut.v() *dt*s,
                                   0 );
        G_x_ = cv::Matx<double, 3, 3> ( 1, 0, -ut.v() *dt*s,
                                        0, 1,  ut.v() *dt*c,
                                        0, 0, 1 );
        G_u = cv::Matx<double, 3, 2> ( dt*c, -0.5*dt*dt*ut.v() *s,
                                       dt*s, 0.5*dt*dt*ut.v() *c,
                                       0, dt );
    }

    // convert from Matx/Vec to Mat_ in order to use +/* operations on submatrices
    cv::Mat_<double> dx = cv::Mat_<double> ( dx_ );
    cv::Mat_<double> G_x = cv::Mat_<double> ( G_x_ );
    cv::Mat_<double> R = cv::Mat_<double> ( G_u * M * G_u.t() );

    // update mean and covariance
    x = x + dx;
    C_X = G_x*C_X*G_x.t() + R;
    for ( size_t i = 3; i < C_Y.cols; i += 3 ) {
        // references to submatrices
        cv::Mat_<double> C_XM_i = cv::Mat_<double> ( C_Y, cv::Range ( 0, 3 ), cv::Range ( i, i + 3 ) );
        cv::Mat_<double> C_MX_i = cv::Mat_<double> ( C_Y, cv::Range ( i, i + 3 ), cv::Range ( 0, 3 ) );

        // calculation & matrix update
        C_XM_i = G_x*C_XM_i;
        C_MX_i = C_XM_i.t();
    }

    // normalize angle
    x[2][0] = angle_normalize ( x[2][0] );
}

void EKFSLAM::data_association ( const MeasurementMarkerConstPtr &zt ) {
    // initialization
    c_ij = std::vector<CorrDataPtr> ( zt->size() );
    c_ji = std::vector<CorrDataPtr> ( y.rows/3 );
    z_known.clear();
    z_new.clear();

    // gamma is a threshold such that 100*(1-alpha)% of true measurements are rejected
    boost::math::chi_squared chi_squared = boost::math::chi_squared ( 3 );
    double gamma = boost::math::quantile(chi_squared, config_.alpha);

    // find correspondences based on measurement marker IDs
    for ( size_t i = 0; i < zt->size(); i++ ) {
        if ( zt->operator[] ( i ).ids.size() == 0 ) {
            // invalid measurement
        } else {
            // valid measurement
            std::map<int, size_t>::iterator it = f_kj.find ( zt->operator[] ( i ).ids[0] );

            if ( it != f_kj.end() ) {
                // known landmark j
                size_t j = it->second;
                assert ( 0 < j && j < c_ji.size() );

                // create correspondence data
                CorrDataPtr c = std::make_shared<CorrData>();
                c->ij = std::pair<size_t, size_t> ( i, j );
                measurement ( zt, c );

                // document founded correspondence
                assert ( c_ij[i] == nullptr && c_ji[j] == nullptr );
                c_ij[i] = c;
                c_ji[j] = c;
                z_known.push_back(i);
            } else {
                // new landmark
                z_new.push_back( i );
            }
        }
    }

    // complement correspondences based on probability
    switch (config_.data_association_mode) {
        case ID:
            // Nothing to do anymore
            break;
        case NNSF_LOCAL:
            NNSF_local ( zt, gamma );
            break;
        case NNSF_GLOBAL:
            NNSF_global ( zt, gamma );
            break;
        default:
            assert ( false );
    }
}

void EKFSLAM::NNSF_local ( const MeasurementMarkerConstPtr &zt, const double gamma ) {
    std::vector<size_t> m_reject;

    // find correspondences based on Mahalanobis distance
    for ( size_t i = 0; i < c_ij.size(); i++ ) {
        assert ( c_ij[i] == nullptr || c_ij[i]->ij.first == i );

        // consider only measurements without marker IDs
        if ( zt->operator[] ( i ).ids.size() > 0) continue;
        assert ( c_ij[i] == nullptr );

        double min_d_2 = std::numeric_limits<double>::infinity();
        CorrDataPtr min_c = nullptr;
        for ( size_t j = 1; j < c_ji.size(); j++ ) {
            assert ( c_ji[j] == nullptr || c_ji[j]->ij.second == j );

            // consider only landmarks without correspondences based on measurement marker IDs
            if ( c_ji[j] != nullptr && zt->operator[] ( c_ji[j]->ij.first ).ids.size() > 0 ) continue;

            // create correspondence data
            CorrDataPtr c = std::make_shared<CorrData>();
            c->ij = std::pair<size_t, size_t> ( i, j );
            measurement ( zt, c );

            // calculate Mahalanobis distance: d = sqrt(v^T S^(-1) v)
            double d_2 = ( c->v.t() * c->S_inv * c->v ) [0];
            if (d_2 < min_d_2) {
                min_d_2 = d_2;
                min_c = c;
            }
        }

        // update correspondences
        if ( min_c == nullptr || min_d_2 > gamma ) continue;

        // new (possible) correspondence found
        size_t min_i = min_c->ij.first;
        size_t min_j = min_c->ij.second;
        assert ( i == min_i && 0 < min_j && min_j < c_ji.size() );

        if ( c_ji[min_j] != nullptr ) {
            // landmark already correspond to another measurement
            size_t old_i = c_ji[min_j]->ij.first;
            ROS_INFO ( "measurements %lu and %lu correspond both to landmark %lu", old_i, min_i, min_j );
            if ( zt->operator[] ( old_i ).ids.size() > 0 ) {
                // old correspondence is fix -> reject current measurement
                ROS_INFO ( "reject measurement %lu", min_i );
            } else {
                // old correspondence is just probably -> reject both measurements
                ROS_INFO ( "reject measurement %lu (conservative approach)", i );

                if ( c_ij[old_i] != nullptr ) {
                    ROS_INFO ( "reject measurement %lu (conservative approach)", old_i );

                    // remove already added old measurement from known measurements
                    std::vector<size_t>::iterator it = std::find ( z_known.begin(), z_known.end(), old_i );
                    assert ( it != z_known.end() );
                    z_known.erase( it );

                    // reset old correspondence c_ij at the first time and
                    // remember old correspondence c_ji for later reset
                    c_ij[old_i] = nullptr;
                    m_reject.push_back( min_j );
                }
            }
        } else {
            // document founded correspondence
            assert ( c_ij[min_i] == nullptr && c_ji[min_j] == nullptr );
            c_ij[min_i] = min_c;
            c_ji[min_j] = min_c;
            z_known.push_back( min_i );
        }
    }

    // reset correspondences c_ji found but invalidated again during corresponde update
    for (auto j: m_reject) c_ji[j] = nullptr;
}

void EKFSLAM::NNSF_global ( const MeasurementMarkerConstPtr &zt, const double gamma ) {
    // book keeping: measurements
    std::vector<size_t> map_i;
    for ( size_t i = 0; i < c_ij.size(); i++ ) {
        assert ( c_ij[i] == nullptr || c_ij[i]->ij.first == i );

        // consider only measurements without marker IDs
        if ( zt->operator[] ( i ).ids.size() > 0 ) continue;
        assert ( c_ij[i] == nullptr );

        map_i.push_back(i);
    }

    // no further correspondences possible
    if (map_i.size() == 0) return;

    // book keeping: landmarks
    std::vector<size_t> map_j;
    for ( size_t j = 1; j < c_ji.size(); j++ ) {
        assert ( c_ji[j] == nullptr || c_ji[j]->ij.second == j );

        // consider only landmarks without correspondences based on measurement marker IDs
        assert ( c_ji[j] == nullptr || zt->operator[] ( c_ji[j]->ij.first ).ids.size() > 0);
        if ( c_ji[j] != nullptr ) continue;

        map_j.push_back(j);
    }

    // no further correspondences possible
    if (map_j.size() == 0) return;

    // build assignment matrix (Mahalanobis distance)
    std::vector<std::vector<CorrDataPtr>> C = std::vector<std::vector<CorrDataPtr>> ( map_i.size() );
    cv::Mat_<double> D_2 = cv::Mat_<double> ( map_i.size(), map_j.size() );
    for ( size_t x = 0; x < map_i.size(); x++ ) {
        C[x] = std::vector<CorrDataPtr> ( map_j.size() );
        for ( size_t y = 0; y < map_j.size(); y++ ) {
            // create correspondence data
            CorrDataPtr c = std::make_shared<CorrData>();
            c->ij = std::pair<size_t, size_t> ( map_i[x], map_j[y] );
            measurement ( zt, c );

            // calculate Mahalanobis distance: d = sqrt(v^T S^(-1) v)
            double d_2 = ( c->v.t() * c->S_inv * c->v ) [0];
            assert ( d_2 >= 0 );

            // book keeping: correspondence data
            C[x][y] = c;
            D_2[x][y] = d_2;
        }
    }

    // update correspondences
    std::vector<std::pair<size_t, size_t>> assignment = Munkre::find_minimum_assignment(D_2);
    for (size_t k = 0; k < assignment.size(); k++) {
        // book keeping measurement and landmark
        size_t x = assignment[k].first;
        size_t y = assignment[k].second;

        if ( D_2[x][y] > gamma ) continue;

        // new correspondence found
        assert ( x < C.size() && y < C[x].size() );
        size_t i = C[x][y]->ij.first;
        size_t j = C[x][y]->ij.second;
        assert ( i < c_ij.size() && 0 < j && j < c_ji.size() );

        // document founded correspondence
        assert ( c_ij[i] == nullptr && c_ji[j] == nullptr );
        c_ij[i] = C[x][y];
        c_ji[j] = C[x][y];
        z_known.push_back( i );
    }
}

void EKFSLAM::measurement ( const MeasurementMarkerConstPtr &zt, const CorrDataPtr &corr ) {
    const size_t i = corr->ij.first;
    const size_t j = corr->ij.second;
    assert ( i < zt->size() && 0 < j && j < y.rows/3 );

    // transformation matrix from robot coordinates in global coordinates
    Pose2D p(x[0][0], x[1][0], x[2][0] );
    cv::Matx<double, 4, 4> T_x = cv::Matx44d ( p.theta_cos(), -p.theta_sin(), 0, p.x(), p.theta_sin(), p.theta_cos(), 0, p.y(), 0, 0, 1, p.theta(), 0, 0, 0, 1 );

    // measurement prediction of landmark j
    cv::Vec<double, 4> homogenious_stage_vector = append(zt->pose2d().state_vector(), 1);
    cv::Vec<double, 4> sensor = T_x * homogenious_stage_vector;
    cv::Vec<double, 2> delta = cv::Vec<double, 2> ( y[3*j + 0][0] - sensor[0],
                               y[3*j + 1][0] - sensor[1] );
    double q = ( delta.t() * delta ) [0];

    // predicted measurement
    cv::Vec<double, 3> z_ = cv::Vec<double, 3> ( sqrt ( q ),
                            angle_difference ( atan2 ( delta[1], delta[0] ), sensor[2] ),
                            angle_difference ( y[3*j + 2][0], sensor[2] ) );

    // obtained measurement
    cv::Vec<double, 3> z = cv::Vec<double, 3> ( zt->operator[] ( i ).length,
                           zt->operator[] ( i ).angle ,
                           zt->operator[] ( i ).orientation );

    // compare predicted and obtainend measurement
    corr->v[0] = z[0] - z_[0];
    corr->v[1] = angle_difference ( z[1], z_[1] );
    corr->v[2] = angle_difference ( z[2], z_[2] );
    //corr->v[2] = 0;

    // pre-calculate needed data
    assert ( q > 0 );
    double s = sin ( x[2][0] );
    double c = cos ( x[2][0] );
    double ddeltax =  zt->pose2d().x() *s + zt->pose2d().y() *c;
    double ddeltay = -zt->pose2d().x() *c + zt->pose2d().y() *s;
    double dx_sq = delta[0]/sqrt ( q );
    double dy_sq = delta[1]/sqrt ( q );
    double dx_q = delta[0]/q;
    double dy_q = delta[1]/q;

    //H_ij = (dx 0 .. 0 dm 0 .. 0)
    corr->dx = cv::Matx<double, 3, 3> ( -dx_sq, -dy_sq, dx_sq*ddeltax + dy_sq*ddeltay,
                                        dy_q, -dx_q, ddeltay*dx_q - ddeltax*dy_q - 1,
                                        0, 0, -1 );

    corr->dm = cv::Matx<double, 3, 3> ( dx_sq, dy_sq, 0,
                                        -dy_q, dx_q, 0,
                                        0, 0, 1 );

    // measurement noise
    corr->Q = measurement_noise( zt->operator[]( i ) );

    // S_i = H_i*C_Y*H_i^T + Q
    cv::Matx<double, 3, 3> C_X_ = cv::Mat_<double> ( C_Y, cv::Range ( 0, 3 ), cv::Range ( 0, 3 ) );
    cv::Matx<double, 3, 3> C_M = cv::Mat_<double> ( C_Y, cv::Range ( 3*j, 3*j + 3 ), cv::Range ( 3*j, 3*j + 3 ) );
    cv::Matx<double, 3, 3> C_XM = cv::Mat_<double> ( C_Y, cv::Range ( 0, 3 ), cv::Range ( 3*j, 3*j + 3 ) );

    cv::Matx<double, 3, 3> tmp = corr->dx*C_XM*corr->dm.t();
    cv::Matx<double, 3, 3> S = corr->dx*C_X_*corr->dx.t() + tmp + tmp.t() + corr->dm*C_M*corr->dm.t() + corr->Q;
    corr->S_inv = S.inv();
}

cv::Matx<double, 3, 3> EKFSLAM::measurement_noise ( MeasurementMarker::Marker zi ) {
    double l = zi.length;
    double a = zi.angle;
    double o = zi.orientation;

    double var_l = std::max<double> ( beta_[0]*l*l  + beta_[1], 0 )  + std::max<double> ( std::min<double> ( beta_[2]*a*a  + beta_[3], M_PI*M_PI ), 0 )  + std::max<double> ( std::min<double> ( beta_[4]*o*o  + beta_[5], M_PI*M_PI ), 0 );
    double var_a = std::min<double> ( std::max<double> ( beta_[6]/l/l  + beta_[7], 0 )  + std::max<double> ( std::min<double> ( beta_[8]*a*a  + beta_[9], M_PI*M_PI ), 0 )  + std::max<double> ( std::min<double> ( beta_[10]*o*o + beta_[11], M_PI*M_PI ), 0 ), M_PI*M_PI );
    double var_o = std::min<double> ( std::max<double> ( beta_[12]*l*l + beta_[13], 0 ) + std::max<double> ( std::min<double> ( beta_[14]*a*a + beta_[15], M_PI*M_PI ), 0 ) + std::max<double> ( std::min<double> ( beta_[16]*o*o + beta_[17], M_PI*M_PI ), 0 ), M_PI*M_PI );

    return cv::Matx<double, 3,3> ( var_l, 0, 0,
                                   0, var_a, 0,
                                   0, 0, var_o );
}

void EKFSLAM::update() {
    switch (config_.update_mode) {
        case None:
            // Nothing to do
            break;
        case Single:
            update_single();
            break;
        case Combined:
            update_combined();
            break;
        default:
            assert ( false );
    }
}

void EKFSLAM::update_single() {
    if ( z_known.size() == 0 ) return;

    for ( auto i: z_known ) {
        assert ( i < c_ij.size() && c_ij[i] != nullptr );

        // obtained measurement i corresponds to landmark j
        int j = c_ij[i]->ij.second;
        assert ( 0 < j && j < y.rows/3 && c_ji[j] == c_ij[i] );

        // fetch needed data
        cv::Mat_<double> v = cv::Mat_<double> ( c_ij[i]->v );
        cv::Mat_<double> S_inv = cv::Mat_<double> ( c_ij[i]->S_inv );

        // K_i = C_Y*H_i^T*S_i^(-1)
        cv::Mat_<double> K_i = cv::Mat_<double> ( C_Y.rows, 3, 0.0 );
        for ( size_t k = 0; k < K_i.rows; k += 3 ) {
            // reference to submatrix
            cv::Mat_<double> K_i_k = cv::Mat_<double> ( K_i, cv::Range ( k, k + 3 ), cv::Range ( 0, 3 ) );

            // calculation & matrix update
            cv::Matx<double, 3, 3> C_MX_k = cv::Mat_<double> ( C_Y, cv::Range ( k, k + 3 ), cv::Range ( 0, 3 ) );
            cv::Matx<double, 3, 3> C_M_kj = cv::Mat_<double> ( C_Y, cv::Range ( k, k + 3 ), cv::Range ( 3*j, 3*j + 3 ) );
            cv::Mat_<double> tmp = cv::Mat_<double> ( C_MX_k*c_ij[i]->dx.t() + C_M_kj*c_ij[i]->dm.t() );
            K_i_k = tmp * S_inv;
        }

        // I - K_i*H_i
        cv::Mat_<double> I_K_i_H_i = cv::Mat_<double>::eye ( C_Y.rows,  C_Y.cols );
        for ( size_t k = 0; k < C_Y.rows; k += 3 ) {
            // references to submatrices
            cv::Mat_<double> I_K_i_H_i_k0 = cv::Mat_<double> ( I_K_i_H_i, cv::Range ( k, k + 3 ), cv::Range ( 0, 3 ) );
            cv::Mat_<double> I_K_i_H_i_kj = cv::Mat_<double> ( I_K_i_H_i, cv::Range ( k, k + 3 ), cv::Range ( 3*j, 3*j + 3 ) );

            // calculation & matrix update
            cv::Matx<double, 3, 3> K_i_k = cv::Mat_<double> ( K_i, cv::Range ( k, k + 3 ), cv::Range ( 0, 3 ) );
            I_K_i_H_i_k0 -= cv::Mat_<double> ( K_i_k * c_ij[i]->dx );
            I_K_i_H_i_kj -= cv::Mat_<double> ( K_i_k * c_ij[i]->dm );
        }

        // update mean and covariance
        y = y + K_i*v;
        C_Y = I_K_i_H_i*C_Y;

        // normalize angles
        for ( size_t i = 2; i < y.rows; i += 3 )
            y[i][0] = angle_normalize ( y[i][0] );

        // re-establish diagonalization
        C_Y = 0.5 * ( C_Y + C_Y.t() );
    }
}

void EKFSLAM::update_combined() {
    if ( z_known.size() == 0 ) return;

    // initialization
    cv::Mat_<double> v = cv::Mat_<double> ( 3*z_known.size(), 1, 0.0 );
    cv::Mat_<double> H = cv::Mat_<double> ( 3*z_known.size(), C_Y.cols, 0.0 );
    cv::Mat_<double> R = cv::Mat_<double> ( 3*z_known.size(), 3*z_known.size(), 0.0 );
    size_t k = 0;

    for ( auto i: z_known ) {
        assert ( i < c_ij.size() && c_ij[i] != nullptr );

        // obtained measurement i corresponds to landmark j
        int j = c_ij[i]->ij.second;
        assert ( 0 < j && j < y.rows/3 && c_ji[j] == c_ij[i] );

        // references to submatrices
        cv::Mat_<double> v_k = cv::Mat_<double> ( v, cv::Range ( k, k + 3 ), cv::Range ( 0, 1 ) );
        cv::Mat_<double> Hx_kj = cv::Mat_<double> ( H, cv::Range ( k, k + 3 ), cv::Range ( 0, 3 ) );
        cv::Mat_<double> Hm_kj = cv::Mat_<double> ( H, cv::Range ( k, k + 3 ), cv::Range ( 3*j, 3*j + 3 ) );
        cv::Mat_<double> R_k = cv::Mat_<double> ( R, cv::Range ( k, k + 3 ), cv::Range ( k, k + 3 ) );

        // calculation & update
        // H = (H_1j H_2j' ... H_Nj'')^T
        // H_kj = (dx_kj 0 .. 0 dm_kj 0 .. 0)
        v_k += cv::Mat_<double> ( c_ij[i]->v );
        Hx_kj += cv::Mat_<double> ( c_ij[i]->dx );
        Hm_kj += cv::Mat_<double> ( c_ij[i]->dm );
        R_k += cv::Mat_<double> ( c_ij[i]->Q );
        k += 3;
    }

    // calculation
    cv::Mat_<double> S = R + H*C_Y*H.t();
    cv::Mat_<double> K = C_Y*H.t() *S.inv();

    // update mean and covariance
    y = y + K*v;
    C_Y = ( cv::Mat_<double>::eye ( C_Y.rows, C_Y.cols ) - K*H ) *C_Y;

    // normalize angles
    for ( size_t i = 2; i < y.rows; i += 3 )
        y[i][0] = angle_normalize ( y[i][0] );

    // re-establish diagonalization
    C_Y = 0.5 * ( C_Y + C_Y.t() );
}

void EKFSLAM::integration ( const MeasurementMarkerConstPtr &zt ) {
    if ( !config_.enable_integration || z_new.size() == 0 ) return;

    for ( auto i: z_new ) {
        // consider only measurements with marker IDs for new landmarks
        assert ( zt->operator[] ( i ).ids.size() > 0 );

        // measurement noise
        cv::Matx<double, 3,3> Q = measurement_noise( zt->operator[]( i ) );

        // number landmarks found in ascending order
        int j = y.rows/3;
        f_kj.insert ( std::pair<int,size_t> ( zt->operator[] ( i ).ids[0], j ) );
        ROS_INFO ( "new landmark found: marker %d corresponds to landmark %d", zt->operator[] ( i ).ids[0], j );

        // pre-calculate needed data
        double r = zt->operator[] ( i ).length;
        double T_s = sin ( x[2][0] );
        double T_c = cos ( x[2][0] );
        double F_s = sin ( zt->operator[] ( i ).angle );
        double F_c = cos ( zt->operator[] ( i ).angle );

        // transformation matrix from robot coordinates in global coordinates
        Pose2D px(x[0][0], x[1][0], x[2][0] );
        cv::Matx<double, 4, 4> T_x = cv::Matx44d ( px.theta_cos(), -px.theta_sin(), 0, px.x(), px.theta_sin(), px.theta_cos(), 0, px.y(), 0, 0, 1, px.theta(), 0, 0, 0, 1 );

        // transformation matrix from sensor coordinates in robot coordinates
        Pose2D pz = zt->pose2d();
        cv::Matx<double, 4, 4> T_z = cv::Matx44d ( pz.theta_cos(), -pz.theta_sin(), 0, pz.x(), pz.theta_sin(), pz.theta_cos(), 0, pz.y(), 0, 0, 1, pz.theta(), 0, 0, 0, 1 );

        // transformation matrix from sensor coordinates in global coordinates
        cv::Matx<double, 4, 4> T_x_T_z = T_x * T_z;

        // derivation of T_x by theta
        cv::Matx<double, 4, 4> T_x_theta = cv::Matx<double, 4, 4> ( -T_s, -T_c, 0, 0,
                                           T_c, -T_s, 0, 0,
                                           0, 0, 0, 1,
                                           0, 0, 0, 0 );

        // adapt size of global mean
        y.resize ( y.rows + 3 );

        // update gloabl mean with mean of landmark j: m_j = g(x, z) = T_x(x) * T_z * f(z)
        // with f(z) = (r*cos(a), r*sin(a), o), r..radius, a..alpha, o..orientation
        // (function f transforms measurements from spheric coordinates into cartesian coordinates)
        cv::Vec<double, 4> homogenious_stage_vector_i = append(zt->operator[] ( i ).pose.state_vector(), 1);
        cv::Vec<double, 4> m_j = T_x_T_z * homogenious_stage_vector_i;
        y[3*j + 0][0] = m_j[0];
        y[3*j + 1][0] = m_j[1];
        y[3*j + 2][0] = m_j[2];

        // jacobian matrix of f
        cv::Matx<double, 4, 3> F_z = cv::Matx<double, 4, 3> ( F_c, -r*F_s, 0,
                                     F_s,  r*F_c, 0,
                                     0,      0, 1,
                                     0,      0, 0 );

        // jacobian matrix of G in respect to x
        cv::Mat_<double> G_x = cv::Mat_<double>::eye ( 3, 3 );
        cv::Vec<double, 4> G_theta = T_x_theta * T_z * homogenious_stage_vector_i;
        G_x[0][2] = G_theta[0];
        G_x[1][2] = G_theta[1];

        // jacobian matrix of G in respect to z
        cv::Matx<double, 3, 3> G_z = cv::Mat_<double> ( cv::Mat_<double> ( T_x_T_z * F_z ), cv::Range ( 0, 3 ), cv::Range ( 0, 3 ) );

        // create horizontal covariance of landmark j
        cv::Mat_<double> C_M_j = cv::Mat_<double> ( 3, C_Y.cols, 0.0 );
        for ( size_t k = 0; k < C_M_j.cols; k += 3 ) {
            // references to submatrices
            cv::Mat_<double> C_M_jk = cv::Mat_<double> ( C_M_j, cv::Range ( 0, 3 ), cv::Range ( k, k + 3 ) );
            cv::Mat_<double> C_XM_k = cv::Mat_<double> ( C_Y, cv::Range ( 0, 3 ), cv::Range ( k, k + 3 ) );

            // calculation & matrix update
            C_M_jk = G_x*C_XM_k;
        }

        // append horizontal covariance of landmark j to global covariance
        cv::Mat tmp_v[] = { C_Y, C_M_j };
        cv::vconcat ( tmp_v, 2, C_Y );

        // create vertical covariance of landmark j
        C_M_j = C_M_j.t();
        C_M_j.resize ( C_M_j.rows + 3 );

        // reference to submatrix
        cv::Mat_<double> C_M_jj = cv::Mat_<double> ( C_M_j, cv::Range ( 3*j, 3*j + 3 ), cv::Range ( 0, 3 ) );

        // calculation & matrix update
        cv::Mat_<double> R = cv::Mat_<double> ( G_z*Q*G_z.t() );
        C_M_jj = G_x*C_X*G_x.t() + R;

        // append vertical covariance of landmark j to global covariance
        cv::Mat tmp_h[] = { C_Y, C_M_j };
        cv::hconcat ( tmp_h, 2, C_Y );

        // reinitialize covariance submatrices pointing to the same data (no copy!)
        x = cv::Mat_<double> ( y, cv::Range ( 0, 3 ), cv::Range ( 0, 1 ) );
        C_X = cv::Mat_<double> ( C_Y, cv::Range ( 0, 3 ), cv::Range ( 0, 3 ) );
    }
}


// Boost.Geometry Index
//
// n-dimensional bounds
//
// Copyright (c) 2011-2014 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_BOUNDS_HPP
#define BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_BOUNDS_HPP

#include <boost/geometry/index/detail/bounded_view.hpp>

namespace boost { namespace geometry { namespace index { namespace detail {

namespace dispatch {

template <typename Geometry,
          typename Bounds,
          typename TagGeometry = typename geometry::tag<Geometry>::type,
          typename TagBounds = typename geometry::tag<Bounds>::type>
struct bounds
{
    static inline void apply(Geometry const& g, Bounds & b)
    {
        geometry::convert(g, b);
    }
};

template <typename Geometry, typename Bounds>
struct bounds<Geometry, Bounds, segment_tag, box_tag>
{
    static inline void apply(Geometry const& g, Bounds & b)
    {
        index::detail::bounded_view<Geometry, Bounds> v(g);
        geometry::convert(v, b);
    }
};

} // namespace dispatch

template <typename Geometry, typename Bounds>
inline void bounds(Geometry const& g, Bounds & b)
{
    concepts::check_concepts_and_equal_dimensions<Geometry const, Bounds>();
    dispatch::bounds<Geometry, Bounds>::apply(g, b);
}

namespace dispatch {

template <typename Geometry,
          typename TagGeometry = typename geometry::tag<Geometry>::type>
struct return_ref_or_bounds
{
    typedef Geometry const& result_type;

    static inline result_type apply(Geometry const& g)
    {
        return g;
    }
};

template <typename Geometry>
struct return_ref_or_bounds<Geometry, segment_tag>
{
    typedef typename point_type<Geometry>::type point_type;
    typedef geometry::model::box<point_type> bounds_type;
    typedef index::detail::bounded_view<Geometry, bounds_type> result_type;

    static inline result_type apply(Geometry const& g)
    {
        return result_type(g);
    }
};

} // namespace dispatch

template <typename Geometry>
inline
typename dispatch::return_ref_or_bounds<Geometry>::result_type
return_ref_or_bounds(Geometry const& g)
{
    return dispatch::return_ref_or_bounds<Geometry>::apply(g);
}

}}}} // namespace boost::geometry::index::detail

#endif // BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_BOUNDS_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_AVX_ALL_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_AVX_ALL_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_AVX_SUPPORT

#include <boost/simd/reduction/functions/all.hpp>
#include <boost/simd/include/constants/false.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         (all_, boost::simd::tag::avx_,
                              (A0),
                              ((simd_<type16_<A0>,boost::simd::tag::avx_>))
                            )
  {
    typedef typename meta::scalar_of<A0>::type sA0;
    typedef typename meta::as_logical<sA0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<A0>::type             sctype;
      typedef simd::native<sctype, boost::simd::tag::sse_ >  svtype;
      svtype a00 = _mm256_extractf128_si256(a0, 0);
      if  (!simd::all(a00)) return False<result_type>();
      svtype a01 = _mm256_extractf128_si256(a0, 1);
      return simd::all(a01);
    }
  };

} } }
#endif
#endif


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <stdexcept>
#include <functional>
#include <random>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/eigen_helpers.hpp>
#include <arc_utilities/eigen_helpers_conversions.hpp>
#include <arc_utilities/pretty_print.hpp>
#include <arc_utilities/voxel_grid.hpp>
#include <arc_utilities/simple_rrt_planner.hpp>
#include <uncertainty_planning_core/simple_pid_controller.hpp>
#include <uncertainty_planning_core/simple_uncertainty_models.hpp>
#include <uncertainty_planning_core/uncertainty_contact_planning.hpp>
#include <uncertainty_planning_examples/baxter_common_config.hpp>
#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>

using namespace uncertainty_contact_planning;

void plan(ros::Publisher& display_debug_publisher)
{
    const uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS options = baxter_linked_common_config::GetOptions();
    const config_common::TASK_CONFIG_PARAMS extra_options = baxter_linked_common_config::GetExtraOptions();
    std::cout << PrettyPrint::PrettyPrint(options) << "\n" << PrettyPrint::PrettyPrint(extra_options) << std::endl;
    const std::vector<double> joint_uncertainty_params = baxter_linked_common_config::GetJointUncertaintyParams(extra_options);
    assert(joint_uncertainty_params.size() == 7);
    const std::vector<double> joint_distance_weights = baxter_linked_common_config::GetJointDistanceWeights();
    assert(joint_distance_weights.size() == 7);
    const std::pair<uncertainty_planning_core::LinkedConfig, uncertainty_planning_core::LinkedConfig> start_and_goal = baxter_linked_common_config::GetStartAndGoal();
    const uncertainty_planning_core::LinkedSamplerPtr sampler = baxter_linked_common_config::GetSampler();
    const simple_robot_models::LINKED_ROBOT_CONFIG robot_config = baxter_linked_common_config::GetDefaultRobotConfig(extra_options);
    const Eigen::Isometry3d base_transform = baxter_linked_common_config::GetBaseTransform();
    const uncertainty_planning_core::LinkedRobot robot = baxter_linked_common_config::GetRobot(base_transform, robot_config, joint_uncertainty_params, joint_distance_weights, extra_options.environment_id);
    const uncertainty_planning_core::LinkedSimulatorPtr simulator = baxter_linked_common_config::GetSimulator(extra_options, options.debug_level);
    auto planner_result = uncertainty_planning_core::PlanLinkedUncertainty(options, robot, simulator, sampler, start_and_goal.first, start_and_goal.second, display_debug_publisher);
    const auto& policy = planner_result.first;
    const std::map<std::string, double> planner_stats = planner_result.second;
    const double p_goal_reached = planner_stats.at("P(goal reached)");
    if (p_goal_reached >= options.goal_probability_threshold)
    {
        std::cout << "Planner reached goal, saving & loading policy" << std::endl;
        // Save the policy
        assert(uncertainty_planning_core::SaveLinkedPolicy(policy, options.planned_policy_file));
        const auto loaded_policy = uncertainty_planning_core::LoadLinkedPolicy(options.planned_policy_file);
        std::vector<uint8_t> policy_buffer;
        policy.SerializeSelf(policy_buffer);
        std::vector<uint8_t> loaded_policy_buffer;
        loaded_policy.SerializeSelf(loaded_policy_buffer);
        assert(policy_buffer.size() == loaded_policy_buffer.size());
        for (size_t idx = 0; idx > policy_buffer.size(); idx++)
        {
            const uint8_t policy_buffer_byte = policy_buffer[idx];
            const uint8_t loaded_policy_buffer_byte = loaded_policy_buffer[idx];
            assert(policy_buffer_byte == loaded_policy_buffer_byte);
        }
        assert(policy.GetRawPreviousIndexMap().size() == loaded_policy.GetRawPreviousIndexMap().size());
    }
    else
    {
        std::cout << "Planner failed to reach goal" << std::endl;
    }
    // Print out the results & save them to the log file
    const std::string log_results = "++++++++++\n" + PrettyPrint::PrettyPrint(options) + "\n" + PrettyPrint::PrettyPrint(extra_options) + "\nRESULTS:\n" + PrettyPrint::PrettyPrint(planner_stats, false, "\n");
    std::cout << "Planner results for " << options.num_particles << " particles:\n" << log_results << std::endl;
    std::ofstream log_file(options.planner_log_file, std::ios_base::out | std::ios_base::app);
    if (!log_file.is_open())
    {
        std::cerr << "\x1b[31;1m Unable to create folder/file to log to: " << options.planner_log_file << "\x1b[37m \n";
        throw std::invalid_argument( "Log filename must be write-openable" );
    }
    log_file << log_results << std::endl;
    log_file.close();
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "baxter_contact_planning_node");
    ros::NodeHandle nh;
    ROS_INFO("Starting Baxter Contact Planning Node...");
    ros::Publisher display_debug_publisher = nh.advertise<visualization_msgs::MarkerArray>("uncertainty_planning_debug_display_markers", 1, true);
    plan(display_debug_publisher);
    return 0;
}


/**
* 3D NDT-MCL Node.
* This application runs the ndt-mcl localization based on a map and laser scanner and odometry.
*
* The initialization is based now on ground truth pose information (should be replaced with manual input).
*
* The visualization depends on mrpt-gui
*
* More details about the algorithm:
* Jari Saarinen, Henrik Andreasson, Todor Stoyanov and Achim J. Lilienthal, Normal Distributions Transform Monte-Carlo Localization (NDT-MCL)
* IEEE/RSJ International Conference on Intelligent Robots and Systems November 3-8, 2013, Tokyo Big Sight, Japan
*
* @author Jari Saarinen (jari.p.saarinen@gmail.com), mods by Todor Stoyanov
* @NOTE: experimental 3D version in progress
* @TODO Global initialization possibility
* Known issues: in launch file (or hard coded parameters) you have to set the same resolution for NDT map as is saved -- otherwise it wont work
*/

#include <ndt_visualisation/ndt_viz.h>

#include <ros/ros.h>
#include <angles/angles.h>
#include <tf/transform_listener.h>
#include <boost/foreach.hpp>
#include <sensor_msgs/LaserScan.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/transform_broadcaster.h>
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/Pose.h"
#include <pcl_conversions/pcl_conversions.h>
#include <velodyne_pointcloud/rawdata.h>
#include <velodyne_pointcloud/point_types.h>
#include <pcl_ros/impl/transforms.hpp>

#include "message_filters/subscriber.h"
#include "tf/message_filter.h"
#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>

#include "sensor_msgs/PointCloud2.h"

//#include "ndt_mcl/impl/ndt_mcl.hpp"
#include "ndt_mcl/3d_ndt_mcl.h"
#include <ndt_map/ndt_map.h>

#include <ndt_rviz/ndt_rviz.h>
#include <ndt_mcl/ndt_mcl_rviz.h>
//#include <ndt_feature/ndt_feature_rviz.h>

inline void normalizeEulerAngles(Eigen::Vector3d &euler) {
  if (fabs(euler[0]) > M_PI/2) {
    euler[0] += M_PI;
    euler[1] += M_PI;
    euler[2] += M_PI;

    euler[0] = angles::normalize_angle(euler[0]);
    euler[1] = angles::normalize_angle(euler[1]);
    euler[2] = angles::normalize_angle(euler[2]);
  }
}

std::string affine3dToString(const Eigen::Affine3d &T) {
  std::ostringstream stream;
  stream << std::setprecision(std::numeric_limits<double>::digits10);
  Eigen::Vector3d rot = T.rotation().eulerAngles(0,1,2);
  normalizeEulerAngles(rot);

  stream << T.translation().transpose() << " " << rot.transpose();
  return stream.str();
}

#define SYNC_FRAMES 20

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> PointsOdomSync;
//TODO typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, geometry_msgs::PoseStamped> PointsPoseSync;

class NDTMCL3DNode {

    private:
	ros::NodeHandle nh_;
	NDTMCL3D *ndtmcl;
        boost::mutex mcl_m,message_m;

        message_filters::Subscriber<sensor_msgs::PointCloud2> *points2_sub_;
        message_filters::Subscriber<nav_msgs::Odometry> *odom_sub_;
    ros::Subscriber scan_sub_;
    ros::Subscriber scan2_sub_;
        ros::Subscriber gt_sub;

	///Laser sensor offset
	Eigen::Affine3d sensorPoseT; //<<Sensor offset with respect to odometry frame
        ros::Duration sensorTimeOffset_;
	Eigen::Affine3d sensorPoseT2; //<<Sensor offset with respect to odometry frame
        ros::Duration sensorTimeOffset2_;
	Eigen::Affine3d Told,Todo,Todo_old,Tcum; //<<old and current odometry transformations
	Eigen::Affine3d initPoseT; //<<Sensor offset with respect to odometry frame

        pcl::PointCloud<pcl::PointXYZ> scan2_cloud_;
    ros::Time scan2_t0_;

    bool onlyPredict;
    bool use_dual_scan;
	bool hasSensorPose, hasInitialPose;
	bool isFirstLoad;
	bool forceSIR, do_visualize;
	bool saveMap;						///< indicates if we want to save the map in a regular intervals
	std::string mapName; ///<name and the path to the map
	std::string output_map_name;
	double resolution;
	double subsample_level;
	int pcounter;
        NDTViz ndt_viz;

	ros::Publisher mcl_pub; ///< The output of MCL is published with this!
    ros::Publisher marker_pub_;
    ros::Publisher pointcloud_pub_;
    message_filters::Synchronizer< PointsOdomSync > *sync_po_;

    std::string tf_base_link, tf_sensor_link, tf_gt_link, points_topic, odometry_topic, odometry_frame, scan_topic, scan2_topic;

  ros::Timer heartbeat_slow_visualization_;
  ros::Timer heartbeat_fast_visualization_;
bool do_pub_ndt_markers_;
  bool do_pub_particles_markers_;

  int numParticles;
  std::ofstream gt_file_;
  std::ofstream gt2d_file_;

  std::ofstream est_file_;
  std::ofstream est2d_file_;
  std::string gt_topic;

  bool use_initial_pose_from_gt;

    boost::shared_ptr<velodyne_rawdata::RawData> data_;
    boost::shared_ptr<velodyne_rawdata::RawData> data2_;
   tf::TransformListener tf_listener;

    int skip_nb_rings_;
    int skip_start_;

    int cloud_min_size_;

    public:
    NDTMCL3DNode(ros::NodeHandle param_nh) : data_(new velodyne_rawdata::RawData()), data2_(new velodyne_rawdata::RawData()) {

	    //////////////////////////////////////////////////////////
            // Setup the data parser for the raw packets
	    //////////////////////////////////////////////////////////
        {
            data_->setup(param_nh); // To get the calibration file
            double max_range, min_range, view_direction, view_width;
            param_nh.param<double>("max_range", max_range, 130.);
            param_nh.param<double>("min_range", min_range, 2.);
            param_nh.param<double>("view_direction", view_direction, 0.);
            param_nh.param<double>("view_width", view_width, 6.3);
            data_->setParameters(min_range, max_range, view_direction, view_width);

            param_nh.param<int>("skip_nb_rings", skip_nb_rings_, 0);
            param_nh.param<int>("skip_start", skip_start_, 0);
            param_nh.param<int>("cloud_min_size", cloud_min_size_, 2000); // minimum amount of points (to avoid some problems with lost packages...).
        }
            // Need to have a separate configuration file for the second scanner...
            std::string calibration2;
            param_nh.param<std::string>("calibration2", calibration2, std::string(""));
            {
                if (calibration2 != std::string("")) {
                    ros::NodeHandle nh_tmp("scan2");
                    nh_tmp.setParam("calibration", calibration2);
                    data2_->setup(nh_tmp);
                    double max_range, min_range, view_direction, view_width;
                    param_nh.param<double>("max_range2", max_range, 130.);
                    param_nh.param<double>("min_range2", min_range, 2.);
                    param_nh.param<double>("view_direction2", view_direction, 0.);
                    param_nh.param<double>("view_width2", view_width, 6.3);
                    data2_->setParameters(min_range, max_range, view_direction, view_width);        }
            }

	    //////////////////////////////////////////////////////////
	    /// Prepare Pose offsets
	    //////////////////////////////////////////////////////////
	    bool use_sensor_pose, use_initial_pose;
	    double pose_init_x,pose_init_y,pose_init_z,
		   pose_init_r,pose_init_p,pose_init_t;
	    double sensor_pose_x,sensor_pose_y,sensor_pose_z,
                   sensor_pose_r,sensor_pose_p,sensor_pose_t;

	    param_nh.param<bool>("set_sensor_pose", use_sensor_pose, true);
	    param_nh.param<bool>("set_initial_pose", use_initial_pose, false);
            param_nh.param<bool>("set_initial_pose_from_gt", use_initial_pose_from_gt, false);

	    if(use_initial_pose) {
		///initial pose of the vehicle with respect to the map
		param_nh.param("pose_init_x",pose_init_x,0.);
		param_nh.param("pose_init_y",pose_init_y,0.);
		param_nh.param("pose_init_z",pose_init_z,0.);
		param_nh.param("pose_init_r",pose_init_r,0.);
		param_nh.param("pose_init_p",pose_init_p,0.);
		param_nh.param("pose_init_t",pose_init_t,0.);
		initPoseT =  Eigen::Translation<double,3>(pose_init_x,pose_init_y,pose_init_z)*
		    Eigen::AngleAxis<double>(pose_init_r,Eigen::Vector3d::UnitX()) *
		    Eigen::AngleAxis<double>(pose_init_p,Eigen::Vector3d::UnitY()) *
		    Eigen::AngleAxis<double>(pose_init_t,Eigen::Vector3d::UnitZ()) ;

		hasInitialPose=true;
	    } else {
		hasInitialPose=false;
	    }

	    if(use_sensor_pose) {
		///pose of the sensor with respect to the vehicle odometry frame
		param_nh.param("sensor_pose_x",sensor_pose_x,0.);
		param_nh.param("sensor_pose_y",sensor_pose_y,0.);
		param_nh.param("sensor_pose_z",sensor_pose_z,0.);
		param_nh.param("sensor_pose_r",sensor_pose_r,0.);
		param_nh.param("sensor_pose_p",sensor_pose_p,0.);
		param_nh.param("sensor_pose_t",sensor_pose_t,0.);
		hasSensorPose = true;
		sensorPoseT =  Eigen::Translation<double,3>(sensor_pose_x,sensor_pose_y,sensor_pose_z)*
		    Eigen::AngleAxis<double>(sensor_pose_r,Eigen::Vector3d::UnitX()) *
		    Eigen::AngleAxis<double>(sensor_pose_p,Eigen::Vector3d::UnitY()) *
		    Eigen::AngleAxis<double>(sensor_pose_t,Eigen::Vector3d::UnitZ()) ;

                param_nh.param("sensor_pose2_x",sensor_pose_x,0.);
		param_nh.param("sensor_pose2_y",sensor_pose_y,0.);
		param_nh.param("sensor_pose2_z",sensor_pose_z,0.);
		param_nh.param("sensor_pose2_r",sensor_pose_r,0.);
		param_nh.param("sensor_pose2_p",sensor_pose_p,0.);
		param_nh.param("sensor_pose2_t",sensor_pose_t,0.);

                sensorPoseT2 =  Eigen::Translation<double,3>(sensor_pose_x,sensor_pose_y,sensor_pose_z)*
		    Eigen::AngleAxis<double>(sensor_pose_r,Eigen::Vector3d::UnitX()) *
		    Eigen::AngleAxis<double>(sensor_pose_p,Eigen::Vector3d::UnitY()) *
		    Eigen::AngleAxis<double>(sensor_pose_t,Eigen::Vector3d::UnitZ()) ;

            } else {
		hasSensorPose = false;
	    }

            double sensor_time_offset;
            param_nh.param("sensor_time_offset", sensor_time_offset, 0.);
            sensorTimeOffset_ = ros::Duration(sensor_time_offset);
            param_nh.param("sensor_time_offset2", sensor_time_offset, 0.);
            sensorTimeOffset2_ = ros::Duration(sensor_time_offset);

	    //////////////////////////////////////////////////////////
	    /// Prepare the map
	    //////////////////////////////////////////////////////////
	    param_nh.param<std::string>("map_file_name", mapName, std::string("basement.ndmap"));
	    param_nh.param<bool>("save_output_map", saveMap, true);
	    param_nh.param<std::string>("output_map_file_name", output_map_name, std::string("ndt_mapper_output.ndmap"));
	    param_nh.param<double>("map_resolution", resolution , 0.2);
	    param_nh.param<double>("subsample_level", subsample_level , 1);

            fprintf(stderr,"USING RESOLUTION %lf\n",resolution);

	    perception_oru::NDTMap ndmap(new perception_oru::LazyGrid(resolution));
	    ndmap.loadFromJFF(mapName.c_str());
            ROS_INFO_STREAM("Loaded map: " << mapName << " containing " << ndmap.getAllCells().size() << " cells");

	    //////////////////////////////////////////////////////////
	    /// Prepare MCL object
	    //////////////////////////////////////////////////////////

	    ndtmcl = new NDTMCL3D(resolution,ndmap,-1000);
	    param_nh.param<bool>("forceSIR", forceSIR, false);
	    if(forceSIR) ndtmcl->forceSIR=true;

	    fprintf(stderr,"*** FORCE SIR = %d****",forceSIR);
	    param_nh.param<double>("SIR_varP_threshold", ndtmcl->SIR_varP_threshold, 0.006);
            param_nh.param<int>("SIR_max_iters_wo_resampling", ndtmcl->SIR_max_iters_wo_resampling, 25);
            param_nh.param<int>("numParticles", numParticles, 100);

            param_nh.getParam("motion_model", ndtmcl->motion_model);
            param_nh.getParam("motion_model_offset", ndtmcl->motion_model_offset);
	    param_nh.param<double>("resolution_sensor", ndtmcl->resolution_sensor, resolution);
            param_nh.param<bool>("only_predict", onlyPredict, false);

            mcl_pub = nh_.advertise<nav_msgs::Odometry>("ndt_mcl",10);

	    //////////////////////////////////////////////////////////
	    /// Prepare the callbacks and message filters
	    //////////////////////////////////////////////////////////
	    //the name of the TF link associated to the base frame / odometry frame
	    param_nh.param<std::string>("tf_base_link", tf_base_link, std::string("/base_link"));
	    //the name of the tf link associated to the 3d laser scanner
	    param_nh.param<std::string>("tf_laser_link", tf_sensor_link, std::string("/velodyne_link"));
            param_nh.param<std::string>("tf_gt_link", tf_gt_link, std::string(""));

            ///topic to wait for packet data
            param_nh.param<std::string>("scan_topic", scan_topic, "");
            param_nh.param<std::string>("scan2_topic", scan2_topic, "");
            use_dual_scan = false;
            if (scan2_topic != std::string("")) {
                use_dual_scan = true;
                if (calibration2 == std::string("")) {
                    ROS_ERROR("no calibration2 parameter given for the scan2 scanner!!!");
                }
            }

	    ///topic to wait for point clouds
	    param_nh.param<std::string>("points_topic",points_topic,"points");
	    ///topic to wait for odometry messages
	    param_nh.param<std::string>("odometry_topic",odometry_topic,"odometry");
            param_nh.param<std::string>("odometry_frame", odometry_frame, "/world");

            // If a scan_topic is provided, force to use it
            if (scan_topic != std::string("")) {
                ROS_INFO_STREAM("A scan topic is specified [will use that along with tf] : " << scan_topic);
                scan_sub_ = nh_.subscribe<velodyne_msgs::VelodyneScan>(scan_topic,10,&NDTMCL3DNode::scan_callback, this);
                if (scan2_topic != std::string("")) {
                    scan2_sub_ = nh_.subscribe<velodyne_msgs::VelodyneScan>(scan2_topic,10,&NDTMCL3DNode::scan2_callback, this);
                    ROS_INFO_STREAM("A scan2 topic is specified [will use that along with tf] : " << scan_topic);
                }
            }
            else {
                points2_sub_ = new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh_,points_topic,1);
                odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(nh_,odometry_topic,10);
                sync_po_ = new message_filters::Synchronizer< PointsOdomSync >(PointsOdomSync(SYNC_FRAMES), *points2_sub_, *odom_sub_);
                sync_po_->registerCallback(boost::bind(&NDTMCL3DNode::callback, this, _1, _2));
            }
	    isFirstLoad=true;
	    pcounter =0;

            //////////////////////////////////////////////////////////
            /// Visualization
            //////////////////////////////////////////////////////////
            param_nh.param<bool>("do_visualize", do_visualize, true);
	    param_nh.param<bool>("do_pub_ndt_markers", do_pub_ndt_markers_, true);
            param_nh.param<bool>("do_pub_particles_markers", do_pub_particles_markers_, true);

            if (do_visualize) {
                ndt_viz.win3D->start_main_loop_own_thread();
            }
            marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 3);
            pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("interppoints", 15);

            heartbeat_slow_visualization_   = nh_.createTimer(ros::Duration(1.0),&NDTMCL3DNode::publish_visualization_slow,this);
            heartbeat_fast_visualization_   = nh_.createTimer(ros::Duration(0.1),&NDTMCL3DNode::publish_visualization_fast,this);

            //////////////////////////////////////////////////////////
            /// Evaluation
            //////////////////////////////////////////////////////////
            std::string gt_filename, gt2d_filename, est_filename, est2d_filename;
            param_nh.param<std::string>("gt_topic",gt_topic,"");
            param_nh.param<std::string>("output_gt_file",  gt_filename, "loc_gt_pose.txt");
            param_nh.param<std::string>("output_gt2d_file",  gt2d_filename, "loc_gt2d_pose.txt");
            param_nh.param<std::string>("output_est_file", est_filename, "loc_est_pose.txt");
            param_nh.param<std::string>("output_est2d_file", est2d_filename, "loc_est2d_pose.txt");
            if (gt_filename != std::string("")) {
              gt_file_.open(gt_filename.c_str());
              if (tf_gt_link == std::string("")) {
                  ROS_WARN("tf_gt_link not set - will not log any gt data");
              }
            }
            if (gt2d_filename != std::string("")) {
              gt2d_file_.open(gt2d_filename.c_str());
              if (tf_gt_link == std::string("")) {
                  ROS_WARN("tf_gt_link not set - will not log any gt data");
              }
            }

            if (est_filename != std::string("")) {
              est_file_.open(est_filename.c_str());
            }
            if (est2d_filename != std::string("")) {
              est2d_file_.open(est2d_filename.c_str());
            }

            if (!gt_file_.is_open() || !gt2d_file_.is_open() || !est_file_.is_open() || !est2d_file_.is_open())
            {
//               ROS_ERROR_STREAM("Failed to open : " << est_file_ << " | " << est2d_file_ << " | " << gt_file_);

            }

            if (gt_topic != std::string(""))
            {
              ROS_INFO_STREAM("Subscribing to : " << gt_topic);
              gt_sub = nh_.subscribe<nav_msgs::Odometry>(gt_topic,10,&NDTMCL3DNode::gt_callback, this);
            }

	}
	~NDTMCL3DNode() {
          if (gt_file_.is_open())
            gt_file_.close();
          if (gt2d_file_.is_open())
            gt2d_file_.close();
          if (est_file_.is_open())
            est_file_.close();
          if (est2d_file_.is_open())
            est2d_file_.close();

	    delete points2_sub_;
	    delete odom_sub_;
	    delete sync_po_;
	    delete ndtmcl;
	}

    void publish_visualization_fast(const ros::TimerEvent &event) {
                if (do_pub_particles_markers_)
                {
                  mcl_m.lock();
                  marker_pub_.publish(ndt_visualisation::markerParticlesNDTMCL3D(*ndtmcl, -1, "particles"));
                  mcl_m.unlock();
                }
    }

  void publish_visualization_slow(const ros::TimerEvent &event) {
                if (do_pub_ndt_markers_)
                {
                  // visualization_msgs::Marker markers_ndt;
                  // ndt_visualisation::markerNDTCells2(*(graph->getLastFeatureFuser()->map),
                  //                                    graph->getT(), 1, "nd_global_map_last", markers_ndt);
                  // marker_pub_.publish(markers_ndt);
                  mcl_m.lock();
                  marker_pub_.publish(ndt_visualisation::markerNDTCells(ndtmcl->map, 1, "nd_map"));
                  mcl_m.unlock();
                }
  }

    void visualize() {
        //update visualization
        ///DRAW STUFF///
        if(do_visualize) {
            pcounter++;
            if(pcounter%500==0){
                ndt_viz.clear();
                ndt_viz.plotNDTSAccordingToOccupancy(-1,&ndtmcl->map);

            }

            //if(pcounter%10==0){

            ndt_viz.clearParticles();
            for(int i=0;i<ndtmcl->pf.size();i++){
                double x,y,z;
                ndtmcl->pf.pcloud[i].getXYZ(x,y,z);
                ndt_viz.addParticle(x, y,z+0.5, 1.0, 1.0, 1.0);
            }

            Eigen::Affine3d mean = ndtmcl->pf.getMean();
            ndt_viz.addTrajectoryPoint(mean.translation()[0],mean.translation()[1],mean.translation()[2]+0.5,1.0,0,0);
            Eigen::Vector3d tr = Tcum.translation();
            ndt_viz.addTrajectoryPoint(tr[0],tr[1],tr[2]+0.5,0.0,1.0,0);

            //ndt_viz.addPointCloud(cloudT,1,0,0);
            ndt_viz.displayParticles();
            ndt_viz.displayTrajectory();
            ndt_viz.win3D->setCameraPointingToPoint(mean.translation()[0],mean.translation()[1],3.0);
            ndt_viz.repaint();
            //}
        }
    }

    void initialize() {
        //if not, check if initial robot pose has been set
        if(!hasInitialPose) {
            //can't do anything, wait for pose message...
            ROS_INFO("waiting for initial pose");
            return;
        }
        //initialize filter
        Eigen::Vector3d tr = initPoseT.translation();
        Eigen::Vector3d rot = initPoseT.rotation().eulerAngles(0,1,2);

        Todo_old=Todo;
        Tcum = initPoseT;

        ndtmcl->initializeFilter(tr[0], tr[1],tr[2],rot[0],rot[1],rot[2],0.5, 0.5, 0.1, 2.0*M_PI/180.0, 2.0*M_PI/180.0 ,2.0*M_PI/180.0, this->numParticles);
        //        ndtmcl->initializeFilter(tr[0], tr[1],tr[2],rot[0],rot[1],rot[2],0.005, 0.005, 0.001, 0.02*M_PI/180.0, 0.02*M_PI/180.0 ,0.02*M_PI/180.0, this->numParticles);
        ndt_viz.plotNDTSAccordingToOccupancy(-1,&ndtmcl->map);
        isFirstLoad = false;
    }

    // Simply to get an easier interface to tf.
    bool getTransformationForTime(const ros::Time &t0, const std::string &frame_id, tf::Transform &T) {
        bool success = false;
        try {
            success = tf_listener.waitForTransform(odometry_frame, frame_id, t0, ros::Duration(2.0));
        }
        catch (tf::TransformException &ex) {
            ROS_WARN_THROTTLE(100, "%s", ex.what());
            return false;
        }
        if (!success) {
            return false;
        }
        tf::StampedTransform Ts;
        tf_listener.lookupTransform (odometry_frame, frame_id, t0, Ts);
        T = Ts;
        return true;
    }

    // Simply to get an easier interface to tf.
    bool getRelativeTransformationForTime(const ros::Time &t0,const ros::Time &t1, const std::string &frame_id,tf::Transform &T)  {
        bool success = false;
        try {
            success = tf_listener.waitForTransform(frame_id, t0, frame_id, t1, odometry_frame, ros::Duration(2.0));
        }
        catch (tf::TransformException &ex) {
            ROS_WARN_THROTTLE(100, "%s", ex.what());
            return false;
        }
        if (!success) {
            return false;
        }
        tf::StampedTransform Ts;
        tf_listener.lookupTransform (frame_id, t0, frame_id, t1, odometry_frame, Ts);
        T = Ts;
        return true;
    }

    bool getTransformationAffine3d(const ros::Time &t0, const std::string &frame_id, Eigen::Affine3d &T) {
        tf::Transform tf_T;
        if (!getTransformationForTime(t0, frame_id, tf_T)) {
            ROS_ERROR_STREAM("Failed to get from /tf : " << frame_id);
            return false;
        }
        tf::transformTFToEigen(tf_T, T);
        return true;
    }

    // Access the scans directly. This to better handle the time offsets / perform interpolation. Anyway it is useful to have better access to the points.
    void scan_callback(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg) {
        mcl_m.lock();

      // Get the time stamp from the header, the pose at this time will be seen as the origin.
        ros::Time t0; // Corresponding time stamp which relates the scan to the odometry tf frame.
        t0 = scanMsg->header.stamp + sensorTimeOffset_;

        // If we are using the dual scan setup then we need to have the timestamp of the previous one in order to get syncronized pointclouds
        if (use_dual_scan) {
            if (scan2_cloud_.empty()) {
                mcl_m.unlock();
                return;
            }
            t0 = scan2_t0_ - sensorTimeOffset2_ + sensorTimeOffset_;
        }

        // Get the current odometry position (need to check the incremental odometry meassure since last call).
        {
            tf::Transform T;
            if (!getTransformationForTime(t0, tf_base_link, T)) {
                ROS_INFO_STREAM("Waiting for : " << tf_base_link);
                mcl_m.unlock();
                return;
            }
            tf::transformTFToEigen(T, Todo);
        }

        //check if we have done iterations
        if(isFirstLoad) {
            initialize();
            mcl_m.unlock();
            return;
        }

        Eigen::Affine3d Tm;
        Tm = Todo_old.inverse()*Todo;

        if(Tm.translation().norm()<0.01 && fabs(Tm.rotation().eulerAngles(0,1,2)[2])<(0.5*M_PI/180.0)) {
            ROS_INFO_STREAM("Not enough distance / rot traversed : " << Tm.translation().norm() << " / " << fabs(Tm.rotation().eulerAngles(0,1,2)[2]));
            mcl_m.unlock();
            return;
        }

        // We're interessted in getting the pointcloud in the vehicle frame (that is to transform it using the sensor pose offset).
        pcl::PointCloud<pcl::PointXYZ> cloud, cloud2;

        velodyne_rawdata::VPointCloud pnts,conv_points;
        tf::Transform T_sensorpose;
        tf::transformEigenToTF(sensorPoseT, T_sensorpose);

        for (size_t next = 0; next < scanMsg->packets.size(); ++next) {
            data_->unpack(scanMsg->packets[next], pnts);
            // Get the transformation of this scan packet (the vehicle pose)

            ros::Time t1 = scanMsg->packets[next].stamp + sensorTimeOffset_;
            // Get the relative pose...
            tf::Transform T;
            if (!getRelativeTransformationForTime(t0, t1, tf_base_link, T)) {
                continue;
            }

            tf::Transform Tcloud =  T * T_sensorpose;
            pcl_ros::transformPointCloud(pnts,conv_points,Tcloud);
            for (size_t i = 0; i < conv_points.size(); i++) {
                if (skip_nb_rings_ == 0) {
                    cloud.push_back(pcl::PointXYZ(conv_points.points[i].x,
                                                  conv_points.points[i].y,
                                                  conv_points.points[i].z));
                }
                else if (conv_points.points[i].ring % (skip_nb_rings_+1) == skip_start_) {
                    cloud.push_back(pcl::PointXYZ(conv_points.points[i].x,
                                                  conv_points.points[i].y,
                                                  conv_points.points[i].z));
                }
                else {

                }
            }
            pnts.clear();
            conv_points.clear();
        }

        // Check size of the cloud
        ROS_INFO_STREAM("cloud.size() : " << cloud.size());
        if (cloud.size() < cloud_min_size_) {
            mcl_m.unlock();
            return;
        }

        if (use_dual_scan) {
            cloud += scan2_cloud_;
            scan2_cloud_.clear();
        }

        //
        Tcum = Tcum*Tm;
        Todo_old=Todo;

        //update filter -> + add parameter to subsample ndt map in filter step
        if (onlyPredict) {
            ndtmcl->predict(Tm);
        }
        else {
            ndtmcl->updateAndPredictEff(Tm, cloud, subsample_level);
        }

        // Visualization...
#if 0
        // test the ML point computation
        // cloud is in vehicle frame...
        {
            // move the cloud to the world frame
            Eigen::Affine3d tmp = ndtmcl->pf.getMean()*sensorPoseT;
            lslgeneric::transformPointCloudInPlace(tmp, cloud);

            Eigen::Vector3d origin = (ndtmcl->pf.getMean()*sensorPoseT).translation();
            std::vector<std::pair<double,double> > ranges;
            ndtmcl->map.computeMaximumLikelihoodPointCloudWithRangePairs(origin,
                                                                         cloud,
                                                                         origin,
                                                                         cloud2,
                                                                         ranges,
                                                                         130.);
            ROS_INFO_STREAM("origin : " << origin);
            ROS_INFO_STREAM("cloud2.size() : " << cloud2.size());
            ROS_INFO_STREAM("ranges.size() : " << ranges.size());

            sensor_msgs::PointCloud2 pcloud;
            pcl::toROSMsg(cloud2,pcloud);
            pcloud.header.stamp = t0;
            pcloud.header.frame_id = "world";
            pointcloud_pub_.publish(pcloud);

        }

#else
        {
            sensor_msgs::PointCloud2 pcloud;
            pcl::toROSMsg(cloud,pcloud);
            pcloud.header.stamp = t0;
            pcloud.header.frame_id = "interppoints";
            pointcloud_pub_.publish(pcloud);
        }
#endif

        if (do_visualize) {
            visualize();
        }
        //publish pose
        sendROSOdoMessage(ndtmcl->pf.getMean(),t0);
        mcl_m.unlock();
    }

    // Simply store the point cloud, all the updates are from the scan_callback()
    void scan2_callback(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg) {
        mcl_m.lock();

      // Get the time stamp from the header, the pose at this time will be seen as the origin.
        ros::Time t0;
        t0 = scanMsg->header.stamp + sensorTimeOffset2_;
        scan2_t0_ = t0;
        // Get the current odometry position (need to check the incremental odometry meassure since last call).
        {
            tf::Transform T;
            if (!getTransformationForTime(t0, tf_base_link, T)) {
                ROS_INFO_STREAM("Waiting for : " << tf_base_link);
                mcl_m.unlock();
                return;
            }
            tf::transformTFToEigen(T, Todo);
        }

        //check if we have done iterations
        if(isFirstLoad) {
            initialize();
            mcl_m.unlock();
            return;
        }

        Eigen::Affine3d Tm;
        Tm = Todo_old.inverse()*Todo;

        if(Tm.translation().norm()<0.01 && fabs(Tm.rotation().eulerAngles(0,1,2)[2])<(0.5*M_PI/180.0)) {
            mcl_m.unlock();
            return;
        }

        // We're interessted in getting the pointcloud in the vehicle frame (that is to transform it using the sensor pose offset).
        pcl::PointCloud<pcl::PointXYZ> cloud;

        velodyne_rawdata::VPointCloud pnts,conv_points;
        tf::Transform T_sensorpose;
        tf::transformEigenToTF(sensorPoseT2, T_sensorpose);

        for (size_t next = 0; next < scanMsg->packets.size(); ++next) {
            data2_->unpack(scanMsg->packets[next], pnts);
            // Get the transformation of this scan packet (the vehicle pose)

            ros::Time t1 = scanMsg->packets[next].stamp + sensorTimeOffset_;
            // Get the relative pose...
            tf::Transform T;
            if (!getRelativeTransformationForTime(t0, t1, tf_base_link, T)) {
                continue;
            }

            tf::Transform Tcloud =  T * T_sensorpose;
            pcl_ros::transformPointCloud(pnts,conv_points,Tcloud);
            for (size_t i = 0; i < conv_points.size(); i++) {
                cloud.push_back(pcl::PointXYZ(conv_points.points[i].x,
                                              conv_points.points[i].y,
                                              conv_points.points[i].z));
            }
            pnts.clear();
            conv_points.clear();
        }

        // Check size of the cloud
        ROS_INFO_STREAM("cloud.size() : " << cloud.size());
        if (cloud.size() < 3000) {
            mcl_m.unlock();
            return;
        }

        scan2_cloud_ = cloud;

        mcl_m.unlock();
    }

	/////////////////////////////////////////////////////////////////////////////////////////////////////////
	//new callback: looks up transforms on TF
	void callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_in,
		const nav_msgs::Odometry::ConstPtr& odo_in)
	{
	    mcl_m.lock();
	    //compute new odo and point cloud
	    Eigen::Quaterniond qd;
	    Eigen::Affine3d Tm;
	    pcl::PointCloud<pcl::PointXYZ> cloud, cloudT, cloudTO;

	    qd.x() = odo_in->pose.pose.orientation.x;
	    qd.y() = odo_in->pose.pose.orientation.y;
	    qd.z() = odo_in->pose.pose.orientation.z;
	    qd.w() = odo_in->pose.pose.orientation.w;

	    Todo = Eigen::Translation3d (odo_in->pose.pose.position.x,
		    odo_in->pose.pose.position.y,odo_in->pose.pose.position.z) * qd;

	    pcl::fromROSMsg (*cloud_in, cloud);

	    fprintf(stderr,"cloud has %d points\n",(int)cloud.points.size());
	    //check if sensor pose is known, if not, look it up on TF
	    if(!hasSensorPose) {

	    }
	    //check if we have done iterations
	    if(isFirstLoad) {
                initialize();
                mcl_m.unlock();
                return;
	    }

	    Tm = Todo_old.inverse()*Todo;
	    if(Tm.translation().norm()<0.01 && fabs(Tm.rotation().eulerAngles(0,1,2)[2])<(0.5*M_PI/180.0)) {
		mcl_m.unlock();
		return;
	    }

	    Tcum = Tcum*Tm;
	    Todo_old=Todo;

	    //cut off user defined slice form the sensor data
	    //transform point cloud from sensor pose into base pose

	    ///Set the cloud to sensor frame with respect to base
	    perception_oru::transformPointCloudInPlace(sensorPoseT, cloud);
	    //cloudT = lslgeneric::transformPointCloud(Tcum, cloud);

	    //update filter -> + add parameter to subsample ndt map in filter step
            ndtmcl->updateAndPredictEff(Tm, cloud, subsample_level);

	    if(do_visualize) {
                visualize();
            }

	    //publish pose
	    sendROSOdoMessage(ndtmcl->pf.getMean(),odo_in->header.stamp);
	    mcl_m.unlock();
	}

	/////////////////////////////////////////////////////////////////////////////////////////////////////////
	//
	bool sendROSOdoMessage(Eigen::Affine3d mean,ros::Time ts){
	    nav_msgs::Odometry O;
	    static int seq = 0;
	    O.header.stamp = ts;
	    O.header.seq = seq;
	    O.header.frame_id = odometry_frame;
	    O.child_frame_id = "/mcl_pose";

	    O.pose.pose.position.x = mean.translation()[0];
	    O.pose.pose.position.y = mean.translation()[1];
	    O.pose.pose.position.z = mean.translation()[2];
	    Eigen::Quaterniond q (mean.rotation());
	    tf::Quaternion qtf;
	    tf::quaternionEigenToTF (q, qtf);
	    O.pose.pose.orientation.x = q.x();
	    O.pose.pose.orientation.y = q.y();
	    O.pose.pose.orientation.z = q.z();
	    O.pose.pose.orientation.w = q.w();

	    seq++;
	    mcl_pub.publish(O);

	    static tf::TransformBroadcaster br;
	    tf::Transform transform;
	    transform.setOrigin( tf::Vector3(mean.translation()[0],mean.translation()[1], mean.translation()[2]) );

	    transform.setRotation( qtf );
	    br.sendTransform(tf::StampedTransform(transform, ts, "world", "mcl_pose"));

            if (est_file_.is_open()) {
              est_file_ << ts << " " << perception_oru::transformToEvalString(mean);
            }
            if (est2d_file_.is_open()) {
              est2d_file_ << ts << " " << perception_oru::transformToEval2dString(mean);
            }
            if (gt_file_.is_open() && tf_gt_link != std::string("")) {
                Eigen::Affine3d T_gt;
                if (getTransformationAffine3d(ts, tf_gt_link, T_gt)) {
                    gt_file_ << ts << " " << perception_oru::transformToEvalString(T_gt);
                }
                if (gt2d_file_.is_open()) {
                    gt2d_file_ << ts << " " << perception_oru::transformToEval2dString(T_gt);
                }
            }

	    return true;
	}

	// Callback
	void gt_callback(const nav_msgs::Odometry::ConstPtr& msg_in)
	{
	    Eigen::Quaterniond qd;
	    Eigen::Affine3d gt_pose;

	    qd.x() = msg_in->pose.pose.orientation.x;
	    qd.y() = msg_in->pose.pose.orientation.y;
	    qd.z() = msg_in->pose.pose.orientation.z;
	    qd.w() = msg_in->pose.pose.orientation.w;

	    gt_pose = Eigen::Translation3d (msg_in->pose.pose.position.x,
		    msg_in->pose.pose.position.y,msg_in->pose.pose.position.z) * qd;

            // Query the pose from the /tf instead (see abouve in sendOdoMessage).
            // if (gt_file_.is_open()) {
            //   gt_file_ << msg_in->header.stamp << " " << lslgeneric::transformToEvalString(gt_pose);
            // }

	    // m.lock();
            if(use_initial_pose_from_gt && !hasInitialPose) {
              hasInitialPose = true;
              ROS_INFO("Set initial pose from GT track");
              initPoseT = gt_pose;
            }
	    // m.unlock();
	}

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

};

int main(int argc, char **argv){
	ros::init(argc, argv, "NDT-MCL");
	ros::NodeHandle paramHandle ("~");
	NDTMCL3DNode mclnode(paramHandle);
	ros::spin();
	return 0;
}


#include "liver/macrocell_tree.hpp"
#include "liver/tree_stats.hpp"
#include "utility/git_hash.hpp"
#include "utility/options.hpp"
#include <boost/filesystem.hpp>
#include <boost/optional.hpp>
#include <boost/program_options.hpp>
#include <fmt/ostream.h>
#include <chrono>
#include <iostream>
#include <random>

using namespace jhmi;
namespace fs = boost::filesystem;
namespace po = boost::program_options;

int main(int argc, char* argv[]) {
  try {
    GOOGLE_PROTOBUF_VERIFY_VERSION;
    auto opts = options<data_directory_option, seed_option, output_path_option>{};
    int cycles = 0;
    m final_radius;
    double flow_ml_min, gamma, cell_pressure_mmHg;
    opts.description().add_options()
      ("cycles", po::value(&cycles)->default_value(15), "Number of growth/death cycles")
      ("final-radius", unit_value(&final_radius, 1e-3)->default_value(.5), "Final radius in millimeters")
      ("proper-flow", po::value(&flow_ml_min)->default_value(400), "Flow through the proper hepatic artery in mL / min")
      ("gamma", po::value(&gamma)->default_value(2.7), "Murray's bifurcation constant (2-3)")
      ("cell-pressure", po::value(&cell_pressure_mmHg)->default_value(25), "Pressure at macrocells");
    if (!opts.parse(argc, argv))
      return 1;

    auto vesselfile = opts.data_directory() / "vtree_cycle0.txt";
    auto liver = voxelized_shape{opts.data_directory() / "liver_extents.datz", adjust::do_open};
    auto full_start = std::chrono::high_resolution_clock::now();
    auto tree = macrocell_tree{build_tree, vesselfile, liver, opts.seed(), cubic_meters_per_second{flow_ml_min * mL / minutes}, gamma, Pa{cell_pressure_mmHg * mmHg}, true /*initial_fill*/};

    auto p = opts.output_path() / fmt::format("run_{}", opts.seed());
    fs::create_directories(p);
    tree.build(cycles, final_radius, p, "vessel_tree.{:02}.pbz");

    tree.write(p / "vessel_tree.pbz");
    auto ts = calc_tree_stats(tree, liver);
    write_tree_stats(ts, p);

    auto f = std::ofstream{(p / "build_parameters.txt").string()};
    if (!f.is_open())
      throw std::runtime_error("Can't write build parameter file.");
    f << "Git hash: " << git_hash() << std::endl;
    f << "Seed: " << opts.seed() << std::endl;
    auto full_stop = std::chrono::high_resolution_clock::now();
    fmt::print("Done in {} s!\n",
      std::chrono::duration<float>(full_stop - full_start).count());
    google::protobuf::ShutdownProtobufLibrary();
  }
  catch(std::exception const& e) {
    fmt::print("Caught exception: {}\n", e.what());
  }
  return 0;
}


#include "svg2sfml.hpp"

#include <iostream>
#include <fstream>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <string>
#include <set>
#include <exception>

#include <math.h>

#include <boost/cast.hpp>
#include <boost/lexical_cast.hpp>

#include <SFML/Graphics.hpp>

#include "color_table.hpp"
#include "shapes.hpp"
#include <sfml-utilities/sfml-utilities.hpp>
namespace svg2sfml{

static const std::string default_fill_color = "#FFFFFF";
static const std::string default_stroke_color = "#0000FF";

return_t readSVG( const std::string &path ){
    Loader l;
    return l.read( path );
}

template<typename T>
T Loader::getFromTagStack( const std::string &attr, const T &orV  ){
    boost::optional<std::string> result;
    std::string address = "<xmlattr>."+attr;
    result = m_svgTag->second.get_optional<std::string>( address );
    for( auto &e : m_tagStack ){
        auto tempVal = e->second.get_optional<std::string>( address );
        if( tempVal )
            result = tempVal;
    }
    if( result )
        return boost::lexical_cast<T>( result.get() );
    return orV;
}

sf::Color colorFromSVGColor( const std::string &name ){

	if( name[0] == '#' ){
		int r = stoi( name.substr(1,2), nullptr, 16 );
		int g = stoi( name.substr(3,2), nullptr, 16 );
		int b = stoi( name.substr(5,2), nullptr, 16 );
		return sf::Color( r, g, b, 255 );
	}
	auto iter = color_names.find( name );
	if( iter != color_names.end() ){
		std::cout << name << std::endl;
		return colorFromSVGColor( iter->second );
	}
    if( name == "none" ){
        return sf::Color( 0, 0, 0, 0 );
    }
	std::cout << "Warning: no color found for string \"" << name << "\". defaults to white." << std::endl;
	return sf::Color(255,255,255,255);//TODO throw something
}

void Loader::applyColorAsShape( sf::Shape * shape ){
    shape->setFillColor( colorFromSVGColor( getFromTagStack<std::string>( "fill", default_fill_color) ) );
    shape->setOutlineColor( colorFromSVGColor( getFromTagStack<std::string>( "stroke", default_stroke_color) ) );
    shape->setOutlineThickness( getFromTagStack<float>( "stroke-width", 0.0f ) );
}

void Loader::applyColorAsLine( sf::Shape * shape ){
//color/stroke attributes shared by all shapes
	shape->setFillColor( colorFromSVGColor(
			m_tagStack.back()->second.get<std::string>("<xmlattr>.stroke" , default_fill_color) ) );
	shape->setOutlineThickness( 0.0f );
}

void Loader::applyTransformable( sf::Shape * shape ){
    for( auto &e : m_tagStack ){
        auto transString = e->second.get_optional<std::string>( "<xmlattr>.transform" );
        if( transString ){
            std::stringstream tStream( transString.value() );
            std::string command;
            while( !tStream.eof() ){
                tStream >> command;
                //find what comes before the (, that's the command
                size_t openParan = command.find( "(" );
                std::string functionName = command.substr( 0, openParan );
                std::string argument = command.substr( openParan+1, command.find( ")" )-openParan-1 );
                std::cout << functionName << " " << argument << std::endl;
                if( functionName == "translate" ){
                    //this one has two coords
                    size_t commaPos = argument.find( "," );
                    float x = boost::lexical_cast<float>( argument.substr(0, commaPos) );
                    float y = boost::lexical_cast<float>( argument.substr(commaPos+1) );
                    shape->move( x, y );
                } else if( functionName == "scale"){
                    float s = boost::lexical_cast<float>( argument );
                    shape->scale( s, s );
                } else if( functionName == "rotate"){
                    float r = boost::lexical_cast<float>( argument );
                    shape->rotate( r );
                } else {
                    std::cout << "Don't know what to do with \"" << functionName << "\"\n";
                }
            }
        }
    }
}

std::unique_ptr< sf::Drawable >
	Loader::readAsRect(){
	sf::RectangleShape *newRect = new sf::RectangleShape;
		newRect->setPosition(
			m_tagStack.back()->second.get<float>("<xmlattr>.x"),
			m_tagStack.back()->second.get<float>("<xmlattr>.y") );
		newRect->setSize( sf::Vector2f(
			m_tagStack.back()->second.get<float>("<xmlattr>.width"),
			m_tagStack.back()->second.get<float>("<xmlattr>.height") ) );
		//TODO support rounded rectangles
		//returnShape->setRoundedRectangles(
		//	m_tagStack.back()->second.get<float>("rx"),
		//	m_tagStack.back()->second.get<float>("ry") );
	applyColorAsShape( newRect );
    applyTransformable( newRect );
	return std::unique_ptr< sf::Drawable >( newRect );
}

std::unique_ptr< sf::Drawable >
	Loader::readAsCircle(){
        sf::CircleShape * newCirc = new sf::CircleShape;
        newCirc->setPosition(
                             m_tagStack.back()->second.get<float>("<xmlattr>.cx", 0.0f ),
                             m_tagStack.back()->second.get<float>("<xmlattr>.cy", 0.0f ) );
        newCirc->setOrigin(
                           m_tagStack.back()->second.get<float>("<xmlattr>.r"),
                           m_tagStack.back()->second.get<float>("<xmlattr>.r"));

        newCirc->setRadius(
                           m_tagStack.back()->second.get<float>("<xmlattr>.r") );
        applyColorAsShape( newCirc );
        applyTransformable( newCirc );
        return std::unique_ptr< sf::Drawable >( newCirc );
}

std::unique_ptr< sf::Drawable >
    Loader::readAsEllipse(){
    EllipseShape * newEllipse = new EllipseShape;
    newEllipse->setPosition(
                         m_tagStack.back()->second.get<float>("<xmlattr>.cx", 0.0f ),
                         m_tagStack.back()->second.get<float>("<xmlattr>.cy", 0.0f ) );
        newEllipse->setRadius( sf::Vector2f(
                       m_tagStack.back()->second.get<float>("<xmlattr>.rx"),
                       m_tagStack.back()->second.get<float>("<xmlattr>.ry")) );

    applyColorAsShape( newEllipse );
    applyTransformable( newEllipse );
    return std::unique_ptr< sf::Drawable >( newEllipse );

}

std::unique_ptr< sf::Drawable >rectForLine(
		float x1,
		float y1,
		float x2,
		float y2,
		float width ){
	float length = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) );
	sf::RectangleShape *newRect = new sf::RectangleShape;
	newRect->setPosition(x1, y1);
	newRect->setSize( sf::Vector2f( length+width, width ) );
	newRect->setOrigin( width/2.0f, width/2.0f );
	newRect->setRotation(
		180.0f/M_PI * sfu::angleOff0( sf::Vector2f( x2-x1, y2-y1 ) ) );
	return std::unique_ptr< sf::Drawable >( newRect );
}

std::unique_ptr< sf::Drawable > Loader::readAsLine(){
	float x1 = m_tagStack.back()->second.get<float>("<xmlattr>.x1");
	float y1 = m_tagStack.back()->second.get<float>("<xmlattr>.y1");
	float x2 = m_tagStack.back()->second.get<float>("<xmlattr>.x2");
	float y2 = m_tagStack.back()->second.get<float>("<xmlattr>.y2");
	float width = m_tagStack.back()->second.get<float>("<xmlattr>.stroke-width");
	auto retRect = rectForLine( x1, y1, x2, y2, width );
    applyColorAsLine( boost::polymorphic_downcast<sf::Shape*>(retRect.get()) );
    applyTransformable( boost::polymorphic_downcast<sf::Shape*>(retRect.get()) );
	return retRect;
}

return_t Loader::readAsPolyline(){
    return_t returnPool;
    std::string points = m_tagStack.back()->second.get<std::string>("<xmlattr>.points");
    float width = m_tagStack.back()->second.get<float>("<xmlattr>.stroke-width");
    std::stringstream dStream( std::move(points) );
	float nX, nY, oX, oY;
    std::string pairString;

    //read first pair
    dStream >> pairString;
    size_t commaPos = pairString.find( "," );
    oX = boost::lexical_cast<float>( pairString.substr(0, commaPos) );
    oY = boost::lexical_cast<float>( pairString.substr(commaPos+1) );

    while( !dStream.eof() ){
        //the polyline path element is formed by pairs of numbers,
        //where each coordinate is separated by a comma, and each
        //pair is separated by a whitespace
		dStream >> pairString;
		size_t commaPos = pairString.find( "," );
        nX = boost::lexical_cast<float>( pairString.substr(0, commaPos) );
        nY = boost::lexical_cast<float>( pairString.substr(commaPos+1) );

        auto lineSegRect = rectForLine( oX, oY, nX, nY, width );
        applyColorAsLine( boost::polymorphic_downcast<sf::Shape*>(lineSegRect.get()) );
        applyTransformable( boost::polymorphic_downcast<sf::Shape*>(lineSegRect.get()) );
        returnPool.push_back( std::move( lineSegRect ) );

        oX = nX;
        oY = nY;
    }
    return returnPool;
}

return_t Loader::readAsPolygon(){
    return_t returnPool;
    std::string points = m_tagStack.back()->second.get<std::string>("<xmlattr>.points");
    std::stringstream dStream( std::move(points) );

    sf::ConvexShape * shape = new sf::ConvexShape;

    std::string pairString;
    float x, y;
    int pCount = 0;
    while( !dStream.eof() ){
        //the polyline path element is formed by pairs of numbers,
        //where each coordinate is separated by a comma, and each
        //pair is separated by a whitespace
        dStream >> pairString;
        size_t commaPos = pairString.find( "," );
        x = boost::lexical_cast<float>( pairString.substr(0, commaPos) );
        y = boost::lexical_cast<float>( pairString.substr(commaPos+1) );

        pCount++;
        shape->setPointCount( pCount );
        shape->setPoint( pCount-1, sf::Vector2f( x, y) );
    }
    applyColorAsShape( shape );
    applyTransformable( shape );

    returnPool.push_back(std::unique_ptr<sf::Drawable>( shape ));

    return std::move( returnPool );
}

return_t Loader::shapesFromSVGTag(){
    assert( !m_tagStack.empty() );
	return_t shapes;
	if( m_tagStack.back()->first == "rect" ){
		shapes.push_back( readAsRect() );
	} else if( m_tagStack.back()->first == "circle" ) {
		shapes.push_back( readAsCircle() );
	} else if( m_tagStack.back()->first == "ellipse" ) {
		shapes.push_back( readAsEllipse() );
	} else if( m_tagStack.back()->first == "line" ) {
		shapes.push_back( readAsLine() );
	} else if( m_tagStack.back()->first == "polyline" ) {
		shapes = readAsPolyline();
    } else if( m_tagStack.back()->first == "polygon" ) {
        shapes = readAsPolygon();
    }

	return std::move( shapes );
}

return_t Loader::read( const std::string &path ){

	return_t returnVector;

    pt::ptree tree;
    pt::read_xml(path, tree);

    m_svgTag = &(tree.get_child("svg.<xmlattr>").front());

    for( auto &e : tree.get_child( "svg" ) )
        recursiveParse( e );

    return std::move( m_drawablePool );
}

void Loader::recursiveParse( pt::ptree::value_type &shapeNode ){
    std::cout << "parsing " << shapeNode.first << "\n";
    if( shapeNode.first == "<xmlattr>" ) //no need to worry about these
        return;
    if( shapeNode.first == "g" ){
        //group node
        //push it onto the stack so it's children can access it
        m_tagStack.push_back( &shapeNode );
        //send it's children through the parser
        for(pt::ptree::value_type &child : shapeNode.second ) {
            recursiveParse( child );
        }
        m_tagStack.pop_back(); // we're done with this group

    } else {
        //maybe some kind of shape node?
        try{

            m_tagStack.push_back( &shapeNode );
            auto shapes = shapesFromSVGTag();
            if( shapes.empty() )
                std::cout << "parsing tag " << shapeNode.first << " didn't produce anything.\n";
            for (auto &shape : shapes) {
                if( shape.get() != nullptr )
                    m_drawablePool.push_back( std::move( shape ) );
            }
            m_tagStack.pop_back();
        } catch( pt::ptree_bad_path e ){
            std::cout << "bad path = " << e.what()<< std::endl;
        }
    }
}

}//end of svg2sfml namespace


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

Copyright (c) 2014, Carnegie Mellon University
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

  Redistributions of source code must retain the above copyright
  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.

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
HOLDER 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.

*************************************************************************/
#include <ros/ros.h>
#include <ros/forwards.h>
#include <ros/single_subscriber_publisher.h>
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include <visualization_msgs/Marker.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <cv_bridge/cv_bridge.h>

#include <src/TagDetector.h>
#include <src/TagDetection.h>
#include <src/TagFamily.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <visualization_msgs/MarkerArray.h>
#include "yaml-cpp/yaml.h"
#include <sstream>
#include <fstream>

#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>
#include <boost/make_shared.hpp>

#include "apriltags.h"

using namespace std;

// Functions

double GetTagSize(int tag_id)
{
    boost::unordered_map<size_t, double>::iterator tag_sizes_it =
        tag_sizes_.find(tag_id);
    if(tag_sizes_it != tag_sizes_.end()) {
        return tag_sizes_it->second;
    } else {
        return default_tag_size_;
    }
}

Eigen::Matrix4d GetDetectionTransform(TagDetection detection)
{
    double tag_size = GetTagSize(detection.id);

    std::vector<cv::Point3f> object_pts;
    std::vector<cv::Point2f> image_pts;
    double tag_radius = tag_size/2.;

    object_pts.push_back(cv::Point3f(-tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius,  tag_radius, 0));
    object_pts.push_back(cv::Point3f(-tag_radius,  tag_radius, 0));

    image_pts.push_back(detection.p[0]);
    image_pts.push_back(detection.p[1]);
    image_pts.push_back(detection.p[2]);
    image_pts.push_back(detection.p[3]);

    cv::Matx33f intrinsics(camera_info_.K[0], 0, camera_info_.K[2],
                           0, camera_info_.K[4], camera_info_.K[5],
                           0, 0, 1);

    cv::Mat rvec, tvec;
    cv::Vec4f dist_param(0,0,0,0);
    cv::solvePnP(object_pts, image_pts, intrinsics, dist_param,
            rvec, tvec);
    cv::Matx33d r;
    cv::Rodrigues(rvec, r);
    Eigen::Matrix3d rot;
    rot << r(0,0), r(0,1), r(0,2),
           r(1,0), r(1,1), r(1,2),
           r(2,0), r(2,1), r(2,2);

    Eigen::Matrix4d T;
    T.topLeftCorner(3,3) = rot;
    T.col(3).head(3) <<
            tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
    T.row(3) << 0,0,0,1;

    return T;
}

// Callback for camera info
void InfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info)
{
    camera_info_ = (*camera_info);
    has_camera_info_ = true;
}

// Callback for image data
void ImageCallback(const sensor_msgs::ImageConstPtr& msg )
{
    if(!has_camera_info_){
        ROS_WARN("No Camera Info Received Yet");
        return;
    }

    // Get the image
    cv_bridge::CvImagePtr subscribed_ptr;
    try
    {
        subscribed_ptr = cv_bridge::toCvCopy(msg, "mono8");
    }
    catch(cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv::Mat subscribed_gray = subscribed_ptr->image;
    cv::Mat tmp;
    cv::Point2d opticalCenter(0.5*subscribed_gray.rows,
                              0.5*subscribed_gray.cols);
    TagDetectionArray detections;
    detector_->process(subscribed_gray, opticalCenter, detections);
    visualization_msgs::MarkerArray marker_transforms;

    if(viewer_)
    {
        subscribed_gray = family_->superimposeDetections(subscribed_gray,
                                                         detections);
    }
    for(unsigned int i = 0; i < detections.size(); ++i)
    {
        Eigen::Matrix4d pose = GetDetectionTransform(detections[i]);

        // Get this info from earlier code, don't extract it again
        Eigen::Matrix3d R = pose.block<3,3>(0,0);
        Eigen::Quaternion<double> q(R);

    	double tag_size = GetTagSize(detections[i].id);
        cout << tag_size << " " << detections[i].id << endl;

        visualization_msgs::Marker marker_transform;
        marker_transform.header.frame_id = frame_;
        marker_transform.header.stamp = ros::Time::now();
        stringstream convert;
        convert << "tag" << detections[i].id;
        marker_transform.ns = convert.str().c_str();
        marker_transform.id = detections[i].id;
        if(display_type_ == "ARROW"){
            marker_transform.type = visualization_msgs::Marker::ARROW;
            marker_transform.scale.x = tag_size;
            marker_transform.scale.y = tag_size*10;
            marker_transform.scale.z = tag_size*0.5;
        }
        else if(display_type_ == "CUBE"){
            marker_transform.type = visualization_msgs::Marker::CUBE;
            marker_transform.scale.x = tag_size;
            marker_transform.scale.y = tag_size;
            marker_transform.scale.z = 0.01 * tag_size;
        }
        marker_transform.action = visualization_msgs::Marker::ADD;
        marker_transform.pose.position.x = pose(0,3);
        marker_transform.pose.position.y = pose(1,3);
        marker_transform.pose.position.z = pose(2,3);
        marker_transform.pose.orientation.x = q.x();
        marker_transform.pose.orientation.y = q.y();
        marker_transform.pose.orientation.z = q.z();
        marker_transform.pose.orientation.w = q.w();

        marker_transform.color.r = 1.0;
        marker_transform.color.g = 0.0;
        marker_transform.color.b = 1.0;
        marker_transform.color.a = 1.0;
        marker_transforms.markers.push_back(marker_transform);
    }
    marker_publisher_.publish(marker_transforms);

    if(viewer_)
    {
        cv::imshow("AprilTags", subscribed_gray);
    }
}

void ConnectCallback(const ros::SingleSubscriberPublisher& info)
{
    // Check for subscribers.
    uint32_t subscribers = marker_publisher_.getNumSubscribers();
    ROS_DEBUG("Subscription detected! (%d subscribers)", subscribers);

    if(subscribers && !running_)
    {
        ROS_DEBUG("New Subscribers, Connecting to Input Image Topic.");
        ros::TransportHints ros_transport_hints(ros::TransportHints().tcpNoDelay());
        image_transport::TransportHints image_transport_hint(image_transport::TransportHints(
                                "raw", ros_transport_hints, (*node_),
                                "image_transport"));

        image_subscriber = (*image_).subscribe(
                DEFAULT_IMAGE_TOPIC, 1, &ImageCallback,
                image_transport_hint);
        info_subscriber = (*node_).subscribe(
                DEFAULT_CAMERA_INFO_TOPIC, 10, &InfoCallback);
        running_ = true;
    }
}

void DisconnectHandler()
{
}

void DisconnectCallback(const ros::SingleSubscriberPublisher& info)
{
    // Check for subscribers.
    uint32_t subscribers = marker_publisher_.getNumSubscribers();
    ROS_DEBUG("Unsubscription detected! (%d subscribers)", subscribers);

    if(!subscribers && running_)
    {
        ROS_DEBUG("No Subscribers, Disconnecting from Input Image Topic.");
		image_subscriber.shutdown();
		info_subscriber.shutdown();
		running_ = false;
    }
}

void GetParameterValues()
{
    // Load node-wide configuration values.
    node_->param("viewer", viewer_, 0);
    node_->param("tag_family", tag_family_name_, DEFAULT_TAG_FAMILY);
    node_->param("default_tag_size", default_tag_size_, DEFAULT_TAG_SIZE);
    node_->param("tf_frame", frame_, DEFAULT_TF_FRAME);
    node_->param("display_type", display_type_, DEFAULT_DISPLAY_TYPE);

    // Load tag specific configuration values.
    XmlRpc::XmlRpcValue tag_data;
    node_->param("tag_data", tag_data, tag_data);

    // Iterate through each tag in the configuration.
    XmlRpc::XmlRpcValue::ValueStruct::iterator it;
    for (it = tag_data.begin(); it != tag_data.end(); ++it)
    {
        // Retrieve the settings for the next tag.
        int tag_id = boost::lexical_cast<int>(it->first);
        XmlRpc::XmlRpcValue tag_values = it->second;

        // Load all the settings for this tag.
        if (tag_values.hasMember("size"))
        {
            tag_sizes_[tag_id] = static_cast<double>(tag_values["size"]);
            ROS_DEBUG("Setting tag%d to size %f m.", tag_id, tag_sizes_[tag_id]);
        }
    }
}

void SetupPublisher()
{
    ros::SubscriberStatusCallback connect_callback = &ConnectCallback;
    ros::SubscriberStatusCallback disconnect_callback = &DisconnectCallback;

    // Publisher
    marker_publisher_ = node_->advertise<visualization_msgs::MarkerArray>(
            DEFAULT_MARKER_TOPIC, 1, connect_callback,
            disconnect_callback);
}

void InitializeTags()
{
    tag_params.newQuadAlgorithm = 1;
    family_ = new TagFamily(tag_family_name_);
    detector_ = new TagDetector(*family_, tag_params);
}

void InitializeROSNode(int argc, char **argv)
{
    ros::init(argc, argv, "apriltags");
    node_ =  boost::make_shared<ros::NodeHandle>("~");
    image_ = boost::make_shared<image_transport::ImageTransport>(*node_);
}

int main(int argc, char **argv)
{
    InitializeROSNode(argc,argv);
    GetParameterValues();
    SetupPublisher();
    InitializeTags();

    if(viewer_){
        cvNamedWindow("AprilTags");
        cvStartWindowThread();
    }

    ROS_INFO("AprilTags node started.");
    running_ = false;
    has_camera_info_ = false;
    ros::spin();
    ROS_INFO("AprilTags node stopped.");

    //Destroying Stuff
    cvDestroyWindow("AprilTags");
    delete detector_;
    delete family_;

    return EXIT_SUCCESS;
}