#include <iostream>
#include <algorithm>
#include <vector>
#include <boost/array.hpp>
#include <boost/algorithm/string.hpp>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
// OpenCV 特征检测模块
#include <opencv2/features2d/features2d.hpp>
// #include <opencv2/nonfree/nonfree.hpp> // use this if you want to use SIFT or SURF
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/ml/ml.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d.hpp>

using namespace std;
using namespace cv;
// using namespace YB_SLAM;
// struct Obj_Info_s
// {
//     int feature_num;
//     int move_num;
// };
struct Object_Label_s
{
    int label;
    char class_name[20];
};
const int MAX_FEATURES = 1000;
// const float GOOD_MATCH_PERCENT = 0.15f;
void priori_object_info(std::map<int, string> &static_object_m, std::map<int, string> &dynamic_object_m)
{
    static_object_m.insert({3, "floor"});
    static_object_m.insert({5, "ceiling"});
    static_object_m.insert({8, "window"});
    static_object_m.insert({19, "chair"});
    static_object_m.insert({33, "desk"});
    static_object_m.insert({74, "computer"});
    static_object_m.insert({75, "chair"});
    static_object_m.insert({100, "card"});
    static_object_m.insert({143, "monitor"});
    dynamic_object_m.insert({12, "person"});
}

// void NCCCheck()
// {
//     // Then if the matched pair is too close to the edge of the image or the pixel difference of the 3×3 image block at the center of the matched pair is too large, the matched pair will be discarded.
//     int limit_edge_corner = 5;
//     double limit_of_check = 2120;
//     for (int i = 0; i < status.size(); i++)
//     {
//         int dx[10] = {-1, 0, 1, -1, 0, 1, -1, 0, 1};
//         int dy[10] = {-1, -1, -1, 0, 0, 0, 1, 1, 1};
//         int x1 = prePts[i].x, y1 = prePts[i].y;
//         int x2 = nextPts[i].x, y2 = nextPts[i].y;
//         if ((x1 < limit_edge_corner || x1 >= nextImg.cols - limit_edge_corner || x2 < limit_edge_corner || x2 >= nextImg.cols - limit_edge_corner || y1 < limit_edge_corner || y1 >= nextImg.rows - limit_edge_corner || y2 < limit_edge_corner || y2 >= nextImg.rows - limit_edge_corner))
//         {
//             status[i] = 0;
//             continue;
//         }
//         double sum_check = 0;
//         for (int j = 0; j < 9; j++)
//             sum_check += abs(prevImg.at<uchar>(y1 + dy[j], x1 + dx[j]) - nextImg.at<uchar>(y2 + dy[j], x2 + dx[j]));
//         if (sum_check > limit_of_check)
//             status[i] = 0;
//         if (status[i])
//         {
//             prePtsInlier.push_back(prePts[i]);
//             nextPtsInlier.push_back(nextPts[i]);
//         }
//     }
// }

int main(int argc, char **argv)
{
    cv::Mat rgb1 = cv::imread("../data/walk/walk_rgb_1.png");
    cv::Mat label1 = cv::imread("../data/walk/walk_rgb_1_label.png", cv::IMREAD_GRAYSCALE);
    cv::Mat depth1 = cv::imread("../data/walk/walk_depth_1.png");

    cv::Mat rgb2 = cv::imread("../data/walk/walk_rgb_2.png");
    cv::Mat depth2 = cv::imread("../data/walk/walk_depth_2.png");
    cv::Mat label2 = cv::imread("../data/walk/walk_rgb_2_label.png", cv::IMREAD_GRAYSCALE);

    cv::resize(rgb1, rgb1, cv::Size(640, 480));
    cv::resize(label1, label1,  cv::Size(640, 480));
    cv::resize(depth1, depth1, cv::Size(640, 480));

    cv::resize(rgb2, rgb2, cv::Size(640, 480));
    cv::resize(label2, label2, cv::Size(640, 480));
    cv::resize(depth2, depth2, cv::Size(640, 480));

    cv::Mat prevImg;
    cv::Mat currentImg;
    cv::cvtColor(rgb1, prevImg, cv::COLOR_BGR2GRAY);
    cv::cvtColor(rgb2, currentImg, cv::COLOR_RGB2GRAY);

    if(prevImg.empty() || currentImg.empty() || prevImg.channels() != 1 || currentImg.channels() != 1)
    {
        cout<<"Input Image is nullptr or the image channels is not gray!" << endl;
        system("pause");
    }

    /* Feature extraction*/
    Ptr<cv::Feature2D> orb = ORB::create(MAX_FEATURES, 1.2, 8, 31, 0, 2, 0, 31, 20);
    vector<cv::KeyPoint> kpts1, kpts2;
    cv::Mat desc1, desc2;
    // 特征点检测算法...
    orb->detect(prevImg, kpts1);
    orb->detect(currentImg, kpts2);

    /*Object Cluster*/
    int obj_num_img_current =  0;
    std::vector<int> obj_label;
    for(int r=0; r < label2.rows; r++)
    {
        for(int c=0; c<label2.rows; c++)
        {
            int l = (int)label2.at<uchar>(r, c);
            std::vector<int>::iterator it = find(obj_label.begin(), obj_label.end(), l);
            if( l != 0 && it == obj_label.end())
            {
                obj_label.push_back(l);
            }
        }
    }

    std::cout<<"Object label: "<<std::endl;
    std::sort(obj_label.begin(), obj_label.end());
    for (size_t i=0; i<obj_label.size(); i++){
        std::cout<<obj_label.at(i)<<' ';
    }
    std::cout<<std::endl;

    obj_num_img_current = obj_label.size();
    std::cout<<"Object totlal number in previous image:"<<obj_num_img_current<<std::endl;

    /*Feature Cluster*/
    std::map<int, string> priori_static_m, priori_dynamic_m;
    priori_object_info(priori_static_m, priori_dynamic_m);

    std::vector<cv::KeyPoint> previous_feature[3]; //0->static, 1->random, 2->dynamic
    // std::vector<cv::KeyPoint> previous_static, previous_dynamic, previous_random;

    for(uint i=0; i<kpts1.size(); i++)
    {
        kpts1[i].class_id = (int)label1.at<uchar>(kpts1[i].pt);
        if( priori_static_m.find(kpts1[i].class_id) != priori_static_m.end())
        {
           previous_feature[0].push_back(kpts1[i]);
        }else if( priori_dynamic_m.find(kpts1[i].class_id) != priori_dynamic_m.end()) //dynamic, 12-> person
        {
            previous_feature[2].push_back(kpts1[i]);
        }else{ //random
            previous_feature[1].push_back(kpts1[i]);
        }
    }

    std::vector<cv::KeyPoint> current_feature[3];
    // std::vector<cv::KeyPoint> current_static, current_dynamic, current_random;
    for(uint i=0; i<kpts2.size(); i++)
    {
        kpts2[i].class_id = (int)label2.at<uchar>(kpts2[i].pt);
        // kpts2[i].class_id = (int)label2.at<uchar>((int)kpts2[i].pt.y, (int)kpts2[i].pt.x);
        if( priori_static_m.find(kpts2[i].class_id) != priori_static_m.end())
        {
            current_feature[0].push_back(kpts2[i]);
        }else if( priori_dynamic_m.find(kpts2[i].class_id) != priori_dynamic_m.end()) //dynamic, 12-> person
        {
            current_feature[2].push_back(kpts2[i]);
        }else{ //random
            current_feature[1].push_back(kpts2[i]);
        }
    }

    // cv::Mat show_current_feature[3];
    // for(size_t i=0; i<3; i++)
    // {
    //     rgb2.copyTo(show_current_feature[i]);
    //     for (size_t j = 0; j < current_feature[i].size(); j++)
    //     {
    //         cv::circle(show_current_feature[i], current_feature[i][j].pt, 1, cv::Scalar(0, 0, 255), 2, 8, 0);
    //     }
    //     char buf[50];
    //     sprintf(buf, "Feature Points in Current Image [%u]", i);
    //     cv::imshow(buf, show_current_feature[i]);
    //     cv::waitKey(0);
    // }

    //Geometry Moving Check
    // cv::Mat mask;
    std::vector<Point2f> points_pre, points_cur;
    size_t size = (previous_feature[0].size()>current_feature[0].size())?current_feature[0].size(): previous_feature[0].size();
    for(size_t i=0; i<size; i++)
    {
        points_pre.push_back(previous_feature[0][i].pt);
        points_cur.push_back(current_feature[0][i].pt);
    }

    // auto ip = std::unique(points_pre.begin(), points_pre.end());
    // previous_feature[0].resize(std::distance(points_pre.begin(), ip));

    // ip = std::unique(points_cur.begin(), points_cur.end());
    // current_feature[0].resize(std::distance(points_cur.begin(), ip));

    // size = (points_pre.size()>points_cur.size())?points_cur.size(): points_pre.size();

    cv::Mat mask(cv::Size(1, size), CV_8UC1);
    cout<<"Fundamental Matrix:"<<std::endl;
    cv::Mat F = cv::findFundamentalMat(points_pre, points_cur, mask, cv::FM_RANSAC, 0.1, 0.99);
    for(int i=0;i<F.rows;i++)
        for(int j=0; j<F.cols;j++)
            cout<<(int)F.at<uchar>(i,j)<<" ";
    std::cout<<endl;
    std::cout<<"Error residual:"<<endl;
    for(int i=0; i<mask.rows; i++)
    {
        if(mask.at<uchar>(i, 0) == 0) //outliers
        {
            for(auto it = current_feature[0].begin(); it != current_feature[0].end(); it++)
            {
                if(((int)it->pt.x == (int)points_cur[i].x ) && ((int)it->pt.y == (int)points_cur[0].y ))
                {
                    current_feature[0].erase(it);
                    current_feature[2].push_back(*it);
                    // std::cout<<it->pt<<" ";
                }
            }
        }else{  //inliner
            Eigen::Vector3f p1(points_pre[i].x, points_pre[i].y, 1);
            Eigen::Vector3f p2(points_cur[i].x, points_cur[i].y, 1);
            Eigen::Matrix3f FM;
            cv::cv2eigen(F, FM);
            int error = p2.transpose()*FM*p1;
            if(error>1)
            {
                std::cout<<error<<" ";
            }
        }
    }

cv::Mat previous_desp[3], current_desp[3];
    std::vector<cv::DMatch> match[3];
    Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
    for(size_t i=0; i<3; i++)
    {
        orb->compute(prevImg, previous_feature[i], previous_desp[i]);
        orb->compute(currentImg, current_feature[i], current_desp[i]);
        matcher->match(previous_desp[i], current_desp[i], match[i], cv::Mat());
    }

    //Calculate minimal distance
    double minDis = 9999;
    for(size_t i=0; i<3; i++)
    {
        for(size_t j=0; j<match[i].size(); j++)
        {
            if( match[i][j].distance > 5 && match[i][j].distance < minDis ) //2.15614e-25 >0, 5 is randomly set
            {
                minDis = match[i][j].distance;
                // cout<<minDis<<" "<<std::endl; //used for debug the minimal distance
            }
        }
    }
    cout<<"Minimal distance: "<<minDis<<std::endl;

    /*Good Match*/
    std::vector<cv::DMatch> goodmatches[3]; //0->static; 1->random; 2->dynamic;
    for(size_t i=0; i<3; i++)
    {
        for(size_t j=0; j<match[i].size();j++)
        {
            if(match[i][j].distance < 4 * minDis)
            {
                goodmatches[i].push_back(match[i][j]);
            }
        }
    }

    Mat good_match_img[3];
    for(size_t i=0;i<3;i++)
    {
        drawMatches(prevImg, previous_feature[i], currentImg, current_feature[i], goodmatches[i], good_match_img[i]);
    }

    for(size_t i=0;i<3;i++)
    {
        if(good_match_img[i].data)
        {
            char buf[50];
            sprintf(buf,"Good Matches [%d]", (int)i );
            cv::imshow(buf, good_match_img[i]);
        }
    }

    std::cout<<"END"<<std::endl;
    cv::waitKey(0);

    return 0;
}


#include <iostream>
#include <cmath>

#include <boost/numeric/linear_algebra/operators.hpp>
#include <boost/numeric/linear_algebra/concepts.hpp>

#include <libs/numeric/linear_algebra/test/algebraic_functions.hpp>
#include <libs/numeric/linear_algebra/test/positive_real.hpp>

// User defined data types and operators

using mtl::positive_real;

# ifdef __GXX_CONCEPTS__
  namespace math {
      concept_map Monoid< add<positive_real>, positive_real > {};
  }
# endif

int main(int, char* [])
{
    positive_real                a0(0.0), a2(2.0), a3(3.0), a4(4.0), a5(5.0);
    math::add<positive_real>     my_add;

    std::cout << "equal_results(a2,a5,  a3,a4, my_add) "
	      << mtl::equal_results(a2,a5,  a3,a4, my_add)  << '\n';
    std::cout << "equal_results(a2,a4,  a3,a4, my_add) "
	      << mtl::equal_results(a2,a4,  a3,a4, my_add)  << '\n';

    std::cout << "identity_pair(a2,a4, my_add) "
	      << mtl::identity_pair(a2,a4, my_add)  << '\n';
    std::cout << "identity_pair(a0,a0, my_add) "
	      << mtl::identity_pair(a0,a0, my_add)  << '\n';

    return 0;
}


#include "drake/solvers/fbstab/components/dense_data.h"

#include <cmath>
#include <stdexcept>

#include <Eigen/Dense>

namespace drake {
namespace solvers {
namespace fbstab {

using MatrixXd = Eigen::MatrixXd;
using VectorXd = Eigen::VectorXd;

DenseData::DenseData(const MatrixXd* H, const VectorXd* f, const MatrixXd* A,
                     const VectorXd* b)
    : H_{H}, f_{f}, A_{A}, b_{b} {
  if (H == nullptr || f == nullptr || A == nullptr || b == nullptr) {
    throw std::runtime_error("Inputs to DenseData::DenseData cannot be null.");
  }
  if (H->rows() != H->cols() || H->rows() != f->size()) {
    throw std::runtime_error(
        "In DenseData::DenseData: H must be square and the same size as f.");
  }
  if (A->cols() != H->rows() || A->rows() != b->size()) {
    throw std::runtime_error(
        "In DenseData::DenseData: Sizing of data defining Az <= b is "
        "inconsistent.");
  }

  nz_ = f->size();
  nv_ = b->size();
}

}  // namespace fbstab
}  // namespace solvers
}  // namespace drake


//!
//! A delegator that runs parallel tempering on several workers to perform an inversion.
//!
//! \file obsidian.cpp
//! \author Lachlan McCalman
//! \author Darren Shen
//! \author Nahid Akbar
//! \date February, 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

// Standard Library
#include <random>
#include <thread>
#include <chrono>
#include <numeric>
#include <iostream>
// Prerequisites
#include <glog/logging.h>
#include <boost/program_options.hpp>
// Project
#include "app/console.hpp"
#include "app/settings.hpp"
#include "input/input.hpp"
#include "comms/delegator.hpp"
#include "comms/requester.hpp"
#include "serial/serial.hpp"
#include "app/asyncdelegator.hpp"
#include "app/signal.hpp"
#include "infer/mcmc.hpp"
#include "infer/metropolis.hpp"
#include "infer/adaptive.hpp"
#include "fwdmodel/fwd.hpp"
#include "datatype/sensors.hpp"
#include "detail.hpp"
#include "distrib/multigaussian.hpp"
#include "prior/world.hpp"

using namespace obsidian;
using namespace stateline;

namespace ph = std::placeholders;

// Command line options specific to obsidian
po::options_description commandLineOptions()
{
  po::options_description cmdLine("Delegator Command Line Options");
  cmdLine.add_options() //
  ("port,p", po::value<uint>()->default_value(5555), "TCP port to accept connections") //
  ("configfile,c", po::value<std::string>()->default_value("obsidian_config"), "configuration file") //
  ("inputfile,i", po::value<std::string>()->default_value("input.obsidian"), "input file") //
  ("recover,r", po::bool_switch()->default_value(false), "force recovery")
  ("anneallength,a", po::value<uint>()->default_value(1000), "anneal chains with n samples before starting mcmc");
  return cmdLine;
}

int main(int ac, char* av[])
{
  init::initialiseSignalHandler();
  // Get the settings from the command line
  auto vm = init::initProgramOptions(ac, av, commandLineOptions());

  // Set up a random generator here
  std::random_device rd;
  std::mt19937 gen(rd());

  LOG(INFO)<< "Initialise the various settings and objects";

  readConfigFile(vm["configfile"].as<std::string>(), vm);
  readInputFile(vm["inputfile"].as<std::string>(), vm);

  std::set<ForwardModel> sensorsEnabled = parseSensorsEnabled(vm);
  DelegatorSettings delegatorSettings = parseDelegatorSettings(vm);
  MCMCSettings mcmcSettings = parseMCMCSettings(vm);
  DBSettings dbSettings = parseDBSettings(vm);
  dbSettings.recover = vm["recover"].as<bool>();
  WorldSpec worldSpec = parseSpec<WorldSpec>(vm, sensorsEnabled);
  GlobalPrior prior = parsePrior<GlobalPrior>(vm, sensorsEnabled);
  GlobalResults results = loadResults(worldSpec, vm, sensorsEnabled);

  LOG(INFO)<< "Create the specification data (serialised) for all the jobs";
  std::string worldSpecData = obsidian::comms::serialise(worldSpec);
  std::vector<uint> sensorId;
  std::vector<std::string> sensorSpecData;
  std::vector<std::string> sensorReadings;
  applyToSensorsEnabled<getData>(sensorsEnabled, std::ref(sensorId), std::ref(sensorSpecData), std::ref(sensorReadings),
                                 std::cref(worldSpec), std::ref(results), std::cref(vm), std::cref(sensorsEnabled));

  LOG(INFO)<< "Initialise comms";
  stateline::comms::Delegator delegator(worldSpecData, sensorId, sensorSpecData, sensorReadings, delegatorSettings);
  delegator.start();

  LOG(INFO)<< "Initialise parallel tempering mcmc";
  GeoAsyncPolicy policy(delegator, prior, sensorsEnabled);

  // Start the sampling
  LOG(INFO)<< "Starting inversion: Run for " << mcmcSettings.wallTime << " seconds";
  LOG(INFO)<< "Problem dimensionality: " << prior.size();
  mcmc::Sampler mcmc(mcmcSettings, dbSettings, prior.size(), global::interruptedBySignal);

  std::vector<Eigen::VectorXd> initialThetas;

  LOG(INFO)<< "Loading / generating initial thetas for the chains";

  for (uint s = 0; s < mcmcSettings.stacks; s++)
  {
    for (uint c = 0; c < mcmcSettings.chains; c++)
    {
      if (dbSettings.recover && !mcmc.chains().states(s * mcmcSettings.chains + c).empty())
      {
        initialThetas.push_back(mcmc.chains().states(s * mcmcSettings.chains + c)[0].sample);
      } else
      {
        uint nSamples = vm["anneallength"].as<uint>();
        std::vector<Eigen::VectorXd> thetas(nSamples);
        double lowestEnergy = std::numeric_limits<double>::max();
        uint bestIndex = 0;
        for ( uint i=0; i<nSamples; i++)
        {
          Eigen::VectorXd cand = prior.sample(gen);
          thetas[i] = cand;
          policy.submit(i,cand);
        }
        for (uint i=0; i<nSamples; i++)
        {
          auto result = policy.retrieve();
          uint id = result.first;
          double energy = result.second;
          if (energy < lowestEnergy)
          {
            bestIndex = id;
            lowestEnergy = energy;
            LOG(INFO) << "stack "<< s << " chain " << c << " best energy: " << lowestEnergy;
          }
        }
        initialThetas.push_back(thetas[bestIndex]);
      }
    }
  }
  LOG(INFO) << "initial samples generated";
  if (mcmcSettings.distribution.compare("Normal") == 0) {
	  LOG(INFO) << "normal proposal";
	  // RS 2018/08/23: Normal proposal is symmetric, so save some CPU
	  // double (*pFn)(const Eigen::VectorXd&, const double,
	  // const Eigen::VectorXd&, const Eigen::VectorXd&) = &mcmc::gaussianProposalPDF;
	  // auto proposalPDF = std::bind(
	  // 	pFn,
	  // 	ph::_1, ph::_2,
	  //  	prior.world.thetaMinBound(), prior.world.thetaMaxBound()
	  // );
	  auto proposalPDF = [=](const Eigen::VectorXd& theta, const double sigma){return 1.0;};
	  auto proposal = std::bind(
	  	&mcmc::adaptiveGaussianProposal,
		ph::_1, ph::_2, ph::_3,
		prior.world.thetaMinBound(), prior.world.thetaMaxBound()
	  );
	  mcmc.run(policy, initialThetas, proposal, proposalPDF, mcmcSettings.wallTime);
  } else if (mcmcSettings.distribution.compare("CrankNicolson") == 0) {
	  LOG(INFO) << "crank nicolson proposal";
	  auto proposalPDF = [=](const Eigen::VectorXd& theta, const double sigma){return prior.evaluate(theta);};
	  auto proposal = std::bind(
	  	&mcmc::crankNicolsonProposal,
		ph::_1, ph::_2, ph::_3, prior
	  );
	  mcmc.run(policy, initialThetas, proposal, proposalPDF, mcmcSettings.wallTime);
  } else if (mcmcSettings.distribution.compare("AdaptiveMulti") == 0) {
	  LOG(INFO) << "adaptive multigaussian proposal";
	  // RS 2018/08/23: AdaptiveMulti proposal is symmetric, so save some CPU
	  auto proposalPDF = [=](const Eigen::VectorXd& theta, const double sigma){return 1.0;};
	  auto proposal = std::bind(
	  	&mcmc::multiGaussianProposal,
		ph::_1, ph::_2, ph::_3
	  );
	  mcmc.run(policy, initialThetas, proposal, proposalPDF, mcmcSettings.wallTime);
  } else {
	LOG(INFO) << "no valid proposal string selected";
  }

  // This will gracefully stop all delegators internal threads
  delegator.stop();

  return 0;
}


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include <vector>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <string>

#include <itkImageFileWriter.h>
#include <itkMetaDataObject.h>
#include <itkVectorImage.h>
#include <mitkImageCast.h>

#include <mitkBaseData.h>
#include <mitkFiberBundle.h>
#include "mitkCommandLineParser.h"
#include <boost/lexical_cast.hpp>
#include <mitkCoreObjectFactory.h>
#include <mitkIOUtil.h>
#include <itkTractDensityImageFilter.h>
#include <itkTractsToFiberEndingsImageFilter.h>

mitk::FiberBundle::Pointer LoadFib(std::string filename)
{
    std::vector<mitk::BaseData::Pointer> fibInfile = mitk::IOUtil::Load(filename);
    if( fibInfile.empty() )
        std::cout << "File " << filename << " could not be read!";
    mitk::BaseData::Pointer baseData = fibInfile.at(0);
    return dynamic_cast<mitk::FiberBundle*>(baseData.GetPointer());
}

/*!
\brief Modify input tractogram: fiber resampling, compression, pruning and transformation.
*/
int main(int argc, char* argv[])
{
    mitkCommandLineParser parser;

    parser.setTitle("Tract Density");
    parser.setCategory("Fiber Tracking and Processing Methods");
    parser.setDescription("Generate tract density image, fiber envelope or fiber endpoints image.");
    parser.setContributor("MIC");

    parser.setArgumentPrefix("--", "-");
    parser.addArgument("input", "i", mitkCommandLineParser::String, "Input:", "input fiber bundle (.fib)", us::Any(), false);
    parser.addArgument("output", "o", mitkCommandLineParser::String, "Output:", "output image", us::Any(), false);
    parser.addArgument("binary", "", mitkCommandLineParser::Bool, "Binary output:", "calculate binary tract envelope", us::Any());
    parser.addArgument("endpoints", "", mitkCommandLineParser::Bool, "Output endpoints image:", "calculate image of fiber endpoints instead of mask", us::Any());
    parser.addArgument("reference_image", "", mitkCommandLineParser::String, "Reference image:", "output image will have geometry of this reference image", us::Any());

    std::map<std::string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
    if (parsedArgs.size()==0)
        return EXIT_FAILURE;

    bool binary = false;
    if (parsedArgs.count("binary"))
        binary = us::any_cast<bool>(parsedArgs["binary"]);

    bool endpoints = false;
    if (parsedArgs.count("endpoints"))
        endpoints = us::any_cast<bool>(parsedArgs["endpoints"]);

    std::string reference_image = "";
    if (parsedArgs.count("reference_image"))
        reference_image = us::any_cast<std::string>(parsedArgs["reference_image"]);

    std::string inFileName = us::any_cast<std::string>(parsedArgs["input"]);
    std::string outFileName = us::any_cast<std::string>(parsedArgs["output"]);

    try
    {
        mitk::FiberBundle::Pointer fib = LoadFib(inFileName);

        mitk::Image::Pointer ref_img;
        MITK_INFO << reference_image;
        if (!reference_image.empty())
            ref_img = dynamic_cast<mitk::Image*>(mitk::IOUtil::Load(reference_image)[0].GetPointer());

        if (endpoints)
        {
            typedef unsigned int OutPixType;
            typedef itk::Image<OutPixType, 3> OutImageType;

            typedef itk::TractsToFiberEndingsImageFilter< OutImageType > ImageGeneratorType;
            ImageGeneratorType::Pointer generator = ImageGeneratorType::New();
            generator->SetFiberBundle(fib);

            if (ref_img.IsNotNull())
            {
                OutImageType::Pointer itkImage = OutImageType::New();
                CastToItkImage(ref_img, itkImage);
                generator->SetInputImage(itkImage);
                generator->SetUseImageGeometry(true);

            }
            generator->Update();

            // get output image
            typedef itk::Image<OutPixType,3> OutType;
            OutType::Pointer outImg = generator->GetOutput();
            mitk::Image::Pointer img = mitk::Image::New();
            img->InitializeByItk(outImg.GetPointer());
            img->SetVolume(outImg->GetBufferPointer());

            mitk::IOUtil::Save(img, outFileName );
        }
        else if (binary)
        {
            typedef unsigned char OutPixType;
            typedef itk::Image<OutPixType, 3> OutImageType;

            itk::TractDensityImageFilter< OutImageType >::Pointer generator = itk::TractDensityImageFilter< OutImageType >::New();
            generator->SetFiberBundle(fib);
            generator->SetBinaryOutput(binary);
            generator->SetOutputAbsoluteValues(false);
            generator->SetWorkOnFiberCopy(false);

            if (ref_img.IsNotNull())
            {
                OutImageType::Pointer itkImage = OutImageType::New();
                CastToItkImage(ref_img, itkImage);
                generator->SetInputImage(itkImage);
                generator->SetUseImageGeometry(true);

            }
            generator->Update();

            // get output image
            typedef itk::Image<OutPixType,3> OutType;
            OutType::Pointer outImg = generator->GetOutput();
            mitk::Image::Pointer img = mitk::Image::New();
            img->InitializeByItk(outImg.GetPointer());
            img->SetVolume(outImg->GetBufferPointer());

            mitk::IOUtil::Save(img, outFileName );
        }
        else
        {
            typedef float OutPixType;
            typedef itk::Image<OutPixType, 3> OutImageType;

            itk::TractDensityImageFilter< OutImageType >::Pointer generator = itk::TractDensityImageFilter< OutImageType >::New();
            generator->SetFiberBundle(fib);
            generator->SetBinaryOutput(binary);
            generator->SetOutputAbsoluteValues(false);
            generator->SetWorkOnFiberCopy(false);

            if (ref_img.IsNotNull())
            {
                OutImageType::Pointer itkImage = OutImageType::New();
                CastToItkImage(ref_img, itkImage);
                generator->SetInputImage(itkImage);
                generator->SetUseImageGeometry(true);

            }
            generator->Update();

            // get output image
            typedef itk::Image<OutPixType,3> OutType;
            OutType::Pointer outImg = generator->GetOutput();
            mitk::Image::Pointer img = mitk::Image::New();
            img->InitializeByItk(outImg.GetPointer());
            img->SetVolume(outImg->GetBufferPointer());

            mitk::IOUtil::Save(img, outFileName );
        }

    }
    catch (itk::ExceptionObject e)
    {
        std::cout << e;
        return EXIT_FAILURE;
    }
    catch (std::exception e)
    {
        std::cout << e.what();
        return EXIT_FAILURE;
    }
    catch (...)
    {
        std::cout << "ERROR!?!";
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          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 __OPTIMISATION2_BUNDLE_MAKE_SPARSE_INDICE_HPP__
#define __OPTIMISATION2_BUNDLE_MAKE_SPARSE_INDICE_HPP__

#include <libv/lma/global.hpp>
#include <libv/lma/ttt/traits/typed_indice.hpp>
#include "indice_container.hpp"
#include <boost/fusion/include/as_map.hpp>
#include <boost/mpl/erase.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/find.hpp>

/*
 * Generative Algorithm : MakeSparseIndice
 * Input       : boost::mpl::vector<A,B,C>
 * Output      :
                map<
                    pair<
                            A,
                            map<
                                pair<B,Indice<B>>,
                                pair<C,Indice<C>>
                              >
                          >,
                    pair<
                            B,
                            map<
                                pair<A,Indice<A>>,
                                pair<C,Indice<C>>
                              >
                          >,
                    pair<
                            C,
                            map<
                                pair<A,Indice<A>>,
                                pair<B,Indice<B>>
                              >
                        >
                  >
!*/

namespace lma
{

  template<class Vector, class T> struct GenPairKeyIndice :
    br::as_map<
                typename mpl::transform<
                                          Vector,
//                                           typename mpl::erase<
//                                                               Vector,
//                                                               typename mpl::find<
//                                                                                Vector,
//                                                                                T
//                                                                                >::type
//                                                               >::type,
                                          bf::pair<
                                                    mpl::_1,
                                                    ttt::Indice<
                                                                bf::pair<
                                                                        T,
                                                                        mpl::_1
                                                                      >
                                                                >
                                                  >
                                        >::type
              > {};

  template<class Vector> struct MakeSparseIndice :
    br::as_map<
                typename mpl::transform<
                                        Vector,
                                        bf::pair<
                                                  mpl::_1,
                                                  GenPairKeyIndice<Vector,mpl::_1>
                                                >
                                        >::type
              > {};

  template<class Vector, class T> struct GenPairKeyContainer1 :
    boost::fusion::result_of::as_map<
                                      typename boost::mpl::transform<
                                                                      Vector,
//                                                                       typename boost::mpl::erase<
//                                                                                Vector,
//                                                                                typename boost::mpl::find<Vector,T>::type
//                                                                                >::type,
                                                                      boost::fusion::pair<
                                                                                boost::mpl::_1,
                                                                                SIC<boost::mpl::_1,T>
                                                                                >
                                                                    >::type
                                    > {};

  template<class Vector> struct MakeSparseIndiceContainer1 :
    boost::fusion::result_of::as_map<
                                      typename boost::mpl::transform<
                                                                      Vector,
                                                                      boost::fusion::pair<
                                                                                boost::mpl::_1,
                                                                                GenPairKeyContainer1<Vector,boost::mpl::_1>
                                                                                >
                                                                    >::type
                                    > {};

  //! seule différence : SIC -> SIC2
  template<class Vector, class T> struct GenPairKeyContainer2 :
    boost::fusion::result_of::as_map<typename boost::mpl::transform<typename
    boost::mpl::erase<Vector,typename boost::mpl::find<Vector,T>::type>::type,
    boost::fusion::pair<boost::mpl::_1,SIC2<boost::mpl::_1,T>>>::type> {};

  template<class Vector> struct MakeSparseIndiceContainer2 :
    boost::fusion::result_of::as_map<typename boost::mpl::transform<Vector,
    boost::fusion::pair<boost::mpl::_1,GenPairKeyContainer2<Vector,boost::mpl::_1>
    >>::type> {};
}

#endif


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "armnnOnnxParser/IOnnxParser.hpp"
#include  "ParserPrototxtFixture.hpp"

BOOST_AUTO_TEST_SUITE(OnnxParser)

struct SimpleConv2DFixture : public armnnUtils::ParserPrototxtFixture<armnnOnnxParser::IOnnxParser>
{
    SimpleConv2DFixture()
    {
        m_Prototext = R"(
                   ir_version: 3
                   producer_name:  "CNTK"
                   producer_version:  "2.5.1"
                   domain:  "ai.cntk"
                   model_version: 1
                   graph {
                     name:  "CNTKGraph"
                     input {
                        name: "Input"
                        type {
                          tensor_type {
                            elem_type: 1
                            shape {
                              dim {
                                dim_value: 1
                              }
                              dim {
                                dim_value: 1
                              }
                              dim {
                                dim_value: 3
                              }
                              dim {
                                dim_value: 3
                              }
                            }
                          }
                        }
                      }
                      input {
                        name: "Weight"
                        type {
                          tensor_type {
                            elem_type: 1
                            shape {
                              dim {
                                dim_value: 1
                              }
                              dim {
                                dim_value: 1
                              }
                              dim {
                                dim_value: 3
                              }
                              dim {
                                dim_value: 3
                              }
                            }
                          }
                        }
                      }
                      initializer {
                          dims: 1
                          dims: 1
                          dims: 3
                          dims: 3
                          data_type: 1
                          float_data: 2
                          float_data: 1
                          float_data: 0
                          float_data: 6
                          float_data: 2
                          float_data: 1
                          float_data: 4
                          float_data: 1
                          float_data: 2
                          name: "Weight"
                        }
                      node {
                         input: "Input"
                         input: "Weight"
                         output: "Output"
                         name: "Convolution"
                         op_type: "Conv"
                         attribute {
                           name: "kernel_shape"
                           ints: 3
                           ints: 3
                           type: INTS
                         }
                         attribute {
                           name: "strides"
                           ints: 1
                           ints: 1
                           type: INTS
                         }
                         attribute {
                           name: "auto_pad"
                           s: "VALID"
                           type: STRING
                         }
                         attribute {
                           name: "group"
                           i: 1
                           type: INT
                         }
                         attribute {
                           name: "dilations"
                           ints: 1
                           ints: 1
                           type: INTS
                         }
                         doc_string: ""
                         domain: ""
                       }
                      output {
                          name: "Output"
                          type {
                             tensor_type {
                               elem_type: 1
                               shape {
                                   dim {
                                       dim_value: 1
                                   }
                                   dim {
                                       dim_value: 1
                                   }
                                   dim {
                                       dim_value: 1
                                   }
                                   dim {
                                       dim_value: 1
                                   }
                               }
                            }
                        }
                        }
                    }
                   opset_import {
                      version: 7
                    })";
        Setup();
    }
};

struct Conv2DWithBiasesFixture :  public armnnUtils::ParserPrototxtFixture<armnnOnnxParser::IOnnxParser>
{
    Conv2DWithBiasesFixture() {
        m_Prototext = R"(
                   ir_version: 3
                   producer_name:  "CNTK"
                   producer_version:  "2.5.1"
                   domain:  "ai.cntk"
                   model_version: 1
                   graph {
                     name:  "CNTKGraph"
                     input {
                        name: "Input"
                        type {
                          tensor_type {
                            elem_type: 1
                            shape {
                              dim {
                                dim_value: 1
                              }
                              dim {
                                dim_value: 1
                              }
                              dim {
                                dim_value: 2
                              }
                              dim {
                                dim_value: 2
                              }
                            }
                          }
                        }
                      }
                      input {
                        name: "Weight"
                        type {
                          tensor_type {
                            elem_type: 1
                            shape {
                              dim {
                                dim_value: 1
                              }
                              dim {
                                dim_value: 1
                              }
                              dim {
                                dim_value: 2
                              }
                              dim {
                                dim_value: 2
                              }
                            }
                          }
                        }
                      }
                      initializer {
                          dims: 1
                          dims: 1
                          dims: 2
                          dims: 2
                          data_type: 1
                          float_data: 2
                          float_data: 1
                          float_data: 0
                          float_data: 6
                          name: "Weight"
                        }
                        input {
                          name: "Bias"
                          type {
                            tensor_type {
                              elem_type: 1
                              shape {
                                dim {
                                  dim_value: 4
                                }
                              }
                            }
                          }
                        }
                        initializer {
                            dims: 4
                            data_type: 1
                            float_data: 10
                            float_data: 0
                            float_data: 0
                            float_data: 0
                            name: "Bias"
                          }
                      node {
                         input: "Input"
                         input: "Weight"
                         input: "Bias"
                         output: "Output"
                         name: "Convolution"
                         op_type: "Conv"
                         attribute {
                           name: "kernel_shape"
                           ints: 2
                           ints: 2
                           type: INTS
                         }
                         attribute {
                           name: "strides"
                           ints: 1
                           ints: 1
                           type: INTS
                         }
                         attribute {
                           name: "auto_pad"
                           s: "SAME_UPPER"
                           type: STRING
                         }
                         attribute {
                           name: "group"
                           i: 1
                           type: INT
                         }
                         attribute {
                           name: "dilations"
                           ints: 1
                           ints: 1
                           type: INTS
                         }
                         doc_string: ""
                         domain: ""
                       }
                      output {
                          name: "Output"
                          type {
                             tensor_type {
                               elem_type: 1
                               shape {
                                   dim {
                                       dim_value: 1
                                   }
                                   dim {
                                       dim_value: 1
                                   }
                                   dim {
                                       dim_value: 2
                                   }
                                   dim {
                                       dim_value: 2
                                   }
                               }
                            }
                        }
                        }
                    }
                   opset_import {
                      version: 7
                    })";
        Setup();
    }
};

struct Conv2DDimReducingFixture :  public armnnUtils::ParserPrototxtFixture<armnnOnnxParser::IOnnxParser>
{
    Conv2DDimReducingFixture() {
        m_Prototext = R"(
                   ir_version: 3
                   producer_name:  "CNTK"
                   producer_version:  "2.5.1"
                   domain:  "ai.cntk"
                   model_version: 1
                   graph {
                     name:  "CNTKGraph"
                     input {
                        name: "Input"
                        type {
                          tensor_type {
                            elem_type: 1
                            shape {
                              dim {
                                dim_value: 1
                              }
                              dim {
                                dim_value: 3
                              }
                              dim {
                                dim_value: 2
                              }
                              dim {
                                dim_value: 2
                              }
                            }
                          }
                        }
                      }
                      input {
                        name: "Weight"
                        type {
                          tensor_type {
                            elem_type: 1
                            shape {
                              dim {
                                dim_value: 2
                              }
                              dim {
                                dim_value: 3
                              }
                              dim {
                                dim_value: 1
                              }
                              dim {
                                dim_value: 1
                              }
                            }
                          }
                        }
                      }
                      initializer {
                          dims: 2
                          dims: 3
                          dims: 1
                          dims: 1
                          data_type: 1
                          float_data: -1
                          float_data: 2
                          float_data: 0
                          float_data: 1
                          float_data: 0
                          float_data: 0
                          name: "Weight"
                        }
                      node {
                         input: "Input"
                         input: "Weight"
                         output: "Output"
                         name: "Convolution"
                         op_type: "Conv"
                         attribute {
                           name: "kernel_shape"
                           ints: 1
                           ints: 1
                           type: INTS
                         }
                         attribute {
                           name: "strides"
                           ints: 1
                           ints: 1
                           type: INTS
                         }
                         attribute {
                           name: "group"
                           i: 1
                           type: INT
                         }
                         attribute {
                           name: "dilations"
                           ints: 1
                           ints: 1
                           type: INTS
                         }
                         doc_string: ""
                         domain: ""
                       }
                      output {
                          name: "Output"
                          type {
                             tensor_type {
                               elem_type: 1
                               shape {
                                   dim {
                                       dim_value: 1
                                   }
                                   dim {
                                       dim_value: 2
                                   }
                                   dim {
                                       dim_value: 2
                                   }
                                   dim {
                                       dim_value: 2
                                   }
                               }
                            }
                        }
                        }
                    }
                   opset_import {
                      version: 7
                    })";
        Setup();
    }
};

struct Conv2DwithDilationFixture : public armnnUtils::ParserPrototxtFixture<armnnOnnxParser::IOnnxParser>
{
    Conv2DwithDilationFixture()
    {
        m_Prototext = R"(
                   ir_version: 3
                   producer_name:  "CNTK"
                   producer_version:  "2.5.1"
                   domain:  "ai.cntk"
                   model_version: 1
                   graph {
                     name:  "CNTKGraph"
                     input {
                        name: "Input"
                        type {
                          tensor_type {
                            elem_type: 1
                            shape {
                              dim {
                                dim_value: 1
                              }
                              dim {
                                dim_value: 1
                              }
                              dim {
                                dim_value: 6
                              }
                              dim {
                                dim_value: 6
                              }
                            }
                          }
                        }
                      }
                      input {
                        name: "Weight"
                        type {
                          tensor_type {
                            elem_type: 1
                            shape {
                              dim {
                                dim_value: 1
                              }
                              dim {
                                dim_value: 1
                              }
                              dim {
                                dim_value: 3
                              }
                              dim {
                                dim_value: 3
                              }
                            }
                          }
                        }
                      }
                      initializer {
                          dims: 1
                          dims: 1
                          dims: 3
                          dims: 3
                          data_type: 1
                          float_data: 2
                          float_data: 1
                          float_data: 0
                          float_data: 6
                          float_data: 2
                          float_data: 1
                          float_data: 4
                          float_data: 1
                          float_data: 2
                          name: "Weight"
                        }
                      node {
                         input: "Input"
                         input: "Weight"
                         output: "Output"
                         name: "Convolution"
                         op_type: "Conv"
                         attribute {
                           name: "kernel_shape"
                           ints: 3
                           ints: 3
                           type: INTS
                         }
                         attribute {
                           name: "strides"
                           ints: 1
                           ints: 1
                           type: INTS
                         }
                         attribute {
                           name: "auto_pad"
                           s: "VALID"
                           type: STRING
                         }
                         attribute {
                           name: "group"
                           i: 1
                           type: INT
                         }
                         attribute {
                           name: "dilations"
                           ints: 2
                           ints: 2
                           type: INTS
                         }
                         doc_string: ""
                         domain: ""
                       }
                      output {
                          name: "Output"
                          type {
                             tensor_type {
                               elem_type: 1
                               shape {
                                   dim {
                                       dim_value: 1
                                   }
                                   dim {
                                       dim_value: 1
                                   }
                                   dim {
                                       dim_value: 2
                                   }
                                   dim {
                                       dim_value: 2
                                   }
                               }
                            }
                        }
                        }
                    }
                   opset_import {
                      version: 7
                    })";
        Setup();
    }
};

BOOST_FIXTURE_TEST_CASE(ValidConvTest, SimpleConv2DFixture)
{
    RunTest<4>({{"Input", {1.0, 2.0, 3.0,
                           4.0, 5.0, 6.0,
                           7.0, 8.0, 9.0}}},
              {{"Output", {1.0 * 2 + 2.0 * 1 + 3.0 * 0 +
                           4.0 * 6 + 5.0 * 2 + 6.0 * 1 +
                           7.0 * 4 + 8.0 * 1 + 9.0 * 2}}});
}

BOOST_FIXTURE_TEST_CASE(ValidConvWithBiasTest, Conv2DWithBiasesFixture)
{
    RunTest<4>({{"Input", {1.0, 2.0,
                           3.0, 4.0}}},
              {{"Output", {1.0 * 2 + 2.0 * 1 + 3.0 * 0 + 4 * 6 + 10,
                           2.0 * 2 + 0 * 1 + 4.0 * 0 + 0 * 6 + 10,
                           3.0 * 2 + 4.0 * 1 + 0 * 0 + 0 * 6 + 10,
                           4.0 * 2 + 0 * 1 + 0 * 0 + 0 * 6 + 10}}});
}

BOOST_FIXTURE_TEST_CASE(ValidConvDimReducTest, Conv2DDimReducingFixture)
{
    RunTest<4>({{"Input", {1.0, 2.0, 3.0, 4.0, -1, -2, 3, 4, 1 , 1, 1, 1 }}},
              {{"Output", {-1 * 1 + 2 * -1, -1 * 2 + 2 * -2,
                           -1 * 3 + 2 * 3,  -1 * 4 + 2 * 4,
                           1, 2, 3, 4}}});
}

BOOST_FIXTURE_TEST_CASE(ValidConvWithDilationTest, Conv2DwithDilationFixture)
{
    RunTest<4>({{"Input", {1.0, 2.0, 3.0, 4.0, 5.0, 6.0,
                           7.0, 8.0, 9.0, 10.0, 11.0, 12.0,
                           1.0, 2.0, 3.0, 4.0, 5.0, 6.0,
                           7.0, 8.0, 9.0, 10.0, 11.0, 12.0,
                           1.0, 2.0, 3.0, 4.0, 5.0, 6.0,
                           7.0, 8.0, 9.0, 10.0, 11.0, 12.0}}},
               {{"Output", {39.0, 58.0, 153.0, 172.0 }}});
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "engine/polyline_compressor.hpp"
#include "util/coordinate_calculation.hpp"

#include <osrm/coordinate.hpp>

#include <cmath>
#include <vector>

BOOST_AUTO_TEST_SUITE(polyline)

using namespace osrm;
using namespace osrm::engine;

BOOST_AUTO_TEST_CASE(decode)
{
    // Polyline string for the 5 coordinates
    const std::string polyline = "_c`|@_c`|@o}@_pRo}@_pRo}@_pRo}@_pR";
    const auto coords = decodePolyline(polyline);

    // Test coordinates; these would be the coordinates we give the loc parameter,
    // e.g. loc=10.00,10.0&loc=10.01,10.1...
    util::Coordinate coord1(util::FloatLongitude{10.0}, util::FloatLatitude{10.00});
    util::Coordinate coord2(util::FloatLongitude{10.1}, util::FloatLatitude{10.01});
    util::Coordinate coord3(util::FloatLongitude{10.2}, util::FloatLatitude{10.02});
    util::Coordinate coord4(util::FloatLongitude{10.3}, util::FloatLatitude{10.03});
    util::Coordinate coord5(util::FloatLongitude{10.4}, util::FloatLatitude{10.04});

    // Put the test coordinates into the vector for comparison
    std::vector<util::Coordinate> cmp_coords = {coord1, coord2, coord3, coord4, coord5};

    BOOST_CHECK_EQUAL(cmp_coords.size(), coords.size());

    for (unsigned i = 0; i < cmp_coords.size(); ++i)
    {
        BOOST_CHECK_CLOSE(static_cast<double>(util::toFloating(coords[i].lat)),
                          static_cast<double>(util::toFloating(cmp_coords[i].lat)),
                          0.0001);
        BOOST_CHECK_CLOSE(static_cast<double>(util::toFloating(coords[i].lon)),
                          static_cast<double>(util::toFloating(cmp_coords[i].lon)),
                          0.0001);
    }
}

BOOST_AUTO_TEST_CASE(encode)
{
    // Coordinates; these would be the coordinates we give the loc parameter,
    // e.g. loc=10.00,10.0&loc=10.01,10.1...
    util::Coordinate coord1(util::FloatLongitude{10.0}, util::FloatLatitude{10.00});
    util::Coordinate coord2(util::FloatLongitude{10.1}, util::FloatLatitude{10.01});
    util::Coordinate coord3(util::FloatLongitude{10.2}, util::FloatLatitude{10.02});
    util::Coordinate coord4(util::FloatLongitude{10.3}, util::FloatLatitude{10.03});
    util::Coordinate coord5(util::FloatLongitude{10.4}, util::FloatLatitude{10.04});

    // Test polyline string for the 5 coordinates
    const std::string polyline = "_c`|@_c`|@o}@_pRo}@_pRo}@_pRo}@_pR";

    // Put the test coordinates into the vector for comparison
    std::vector<util::Coordinate> cmp_coords = {coord1, coord2, coord3, coord4, coord5};

    const auto encodedPolyline = encodePolyline<100000>(cmp_coords.begin(), cmp_coords.end());

    BOOST_CHECK_EQUAL(encodedPolyline, polyline);
}

BOOST_AUTO_TEST_CASE(encode6)
{
    // Coordinates; these would be the coordinates we give the loc parameter,
    // e.g. loc=10.00,10.0&loc=10.01,10.1...
    util::Coordinate coord1(util::FloatLongitude{10.0}, util::FloatLatitude{10.00});
    util::Coordinate coord2(util::FloatLongitude{10.1}, util::FloatLatitude{10.01});
    util::Coordinate coord3(util::FloatLongitude{10.2}, util::FloatLatitude{10.02});
    util::Coordinate coord4(util::FloatLongitude{10.3}, util::FloatLatitude{10.03});
    util::Coordinate coord5(util::FloatLongitude{10.4}, util::FloatLatitude{10.04});

    // Test polyline string for the 6 coordinates
    const std::string polyline = "_gjaR_gjaR_pR_ibE_pR_ibE_pR_ibE_pR_ibE";

    // Put the test coordinates into the vector for comparison
    std::vector<util::Coordinate> cmp_coords = {coord1, coord2, coord3, coord4, coord5};

    const auto encodedPolyline = encodePolyline<1000000>(cmp_coords.begin(), cmp_coords.end());

    BOOST_CHECK_EQUAL(encodedPolyline, polyline);
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#ifndef   CPPQEDCORE_UTILS_EVOLVED_TCC_INCLUDED
#define   CPPQEDCORE_UTILS_EVOLVED_TCC_INCLUDED

#include "Evolved.h"

#include "MathExtensions.h"

#include <boost/bind.hpp>

namespace evolved {

template<typename A>
EvolvedIO<A>::EvolvedIO(A& a, double dtInit, double epsRel, double epsAbs)
  : TimeStepBookkeeper(dtInit,epsRel,epsAbs), LoggingBase(), a_(a)
{}

template<typename A>
Evolved<A>::Evolved(A& a, Derivs derivs, double dtInit, double epsRel, double epsAbs)
  : EvolvedIO<A>(a,dtInit,epsRel,epsAbs), derivs_(derivs), countedDerivs_(boost::bind(&Evolved::countedDerivs,this,_1,_2,_3))
{}

template<typename A>
typename EvolvedIO<A>::Ptr makeIO(A& a, double time=0)
{
  typename EvolvedIO<A>::Ptr res = std::make_shared<EvolvedIO<A>, A &>(a,0,0,0);
  res->setTime(time);
  return res;
}

template<typename A>
void Evolved<A>::step(double deltaT)
{
  if (mathutils::sign(deltaT)!=mathutils::sign(EvolvedIO<A>::getDtTry())) {
    // Stepping backward
    this->setDtTry(-EvolvedIO<A>::getDtDid());
    step_v(deltaT);
  }
  else step_v(deltaT);
  this->registerStep();
  this->registerFailedSteps(nFailedStepsLast_v());
}

template<typename E>
void evolve(E& e, double deltaT)
{
  double endTime=e.getTime()+deltaT;
  while (double dt=endTime-e.getTime()) e.step(dt);
}
// evolves for exactly deltaT

} // evolved

#endif // CPPQEDCORE_UTILS_EVOLVED_TCC_INCLUDED


////////////////////////////////////////////////////////////////////
//
// Copyright Vicente J. Botet Escriba 2010
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.
//
////////////////////////////////////////////////////////////////////
#ifndef BOOST_MPL_ABS_HPP_INCLUDED
#define BOOST_MPL_ABS_HPP_INCLUDED

#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/aux_/na_spec.hpp>
#include <boost/mpl/aux_/lambda_support.hpp>
#include <boost/mpl/aux_/config/integral.hpp>
#include <boost/mpl/aux_/config/static_constant.hpp>

#if    !defined(BOOST_MPL_CFG_NO_NESTED_VALUE_ARITHMETIC_2) \
    && !defined(BOOST_MPL_PREPROCESSING_MODE) \
    && !defined(__CUDACC__) \
    && ( defined(BOOST_MSVC) \
        || BOOST_WORKAROUND(__EDG_VERSION__, <= 238) \
        )

#   define BOOST_MPL_CFG_NO_NESTED_VALUE_ARITHMETIC_2

#endif

namespace lslboost { namespace mpl {

template< typename Tag > struct abs_impl;

template< typename T > struct abs_tag
{
    typedef typename T::tag type;
};

template<
      typename BOOST_MPL_AUX_NA_PARAM(N)
    >
struct abs
    : abs_impl<
          typename abs_tag<N>::type
        >::template apply<N>::type
{
    BOOST_MPL_AUX_LAMBDA_SUPPORT(1, abs, (N))
};

BOOST_MPL_AUX_NA_SPEC(1, abs)

template<
      typename T
    , T n1
    >
struct abs_c
    : abs<integral_c<T,n1> >
{
};

#if defined(BOOST_MPL_CFG_NO_NESTED_VALUE_ARITHMETIC_2)
namespace aux {
template< typename T, T n > struct abs_wknd
{
    BOOST_STATIC_CONSTANT(T, value = (n < 0 ? -n : n));
    typedef integral_c<T,value> type;
};
}
#endif

template<>
struct abs_impl<integral_c_tag>
{
#if defined(BOOST_MPL_CFG_NO_NESTED_VALUE_ARITHMETIC_2)
    template< typename N > struct apply
        : aux::abs_wknd< typename N::value_type, N::value >
#else
    template< typename N > struct apply
        : integral_c< typename N::value_type, ((N::value < 0) ? (-N::value) : N::value ) >
#endif
    {
    };
};

}}

#endif // BOOST_MPL_ABS_HPP_INCLUDED


#include <iostream>
#include <fstream>

#include <opencv_workbench/utils/Stream.h>

#include <dlib/svm_threaded.h>
#include <dlib/gui_widgets.h>
#include <dlib/image_processing.h>
#include <dlib/image_io.h>
#include <dlib/data_io.h>

#include <dlib/opencv.h>
//#include <opencv2/highgui/highgui.hpp>
//#include <dlib/image_processing/frontal_face_detector.h>
//#include <dlib/image_processing/render_face_detections.h>
//#include <dlib/image_processing.h>
//#include <dlib/gui_widgets.h>

using std::cout;
using std::endl;

int main(int argc, char *argv[])
{
     if (argc < 2) {
          cout << "Usage: " << argv[0] << " <hog-model.svm>" << endl;
          return -1;
     }

     // Load HOG model
     typedef dlib::scan_fhog_pyramid<dlib::pyramid_down<6> > image_scanner_type;
     dlib::object_detector<image_scanner_type> detector;
     dlib::deserialize(argv[1]) >> detector;

     dlib::image_window hogwin(draw_fhog(detector), "Learned fHOG detector");

     std::string str;
     std::cin >> str;

     return 0;
}


#include <boost/foreach.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <cstdio>
#include <algorithm>

#include "constants.hpp"
#include "gcbias.hpp"
#include "logger.hpp"
#include "seqbias/twobitseq.hpp"

static size_t gc_count(char* seq, size_t n)
{
	size_t count = 0;
	for (size_t i = 0; i < n; ++i) {
		if (seq[i] == 'g' || seq[i] == 'G' || seq[i] == 'c' || seq[i] == 'C') ++count;
	}
	return count;
}

struct ReadPosSeqnameCmp
{
    bool operator () (const ReadPos& a, const ReadPos& b)
    {
        return a.seqname < b.seqname;
    }
};

GCBias::GCBias(const char* ref_filename, PosTable& foreground_position_table,
	           pos_t median_frag_len,
               sequencing_bias* seqbias[2],
               const char* task_name)
{
	faidx_t* ref_file = fai_load(ref_filename);
	if (!ref_file) {
        Logger::abort("Can't open fasta file '%s'.", ref_filename);
	}

	std::vector<ReadPos> foreground_positions;
    const size_t max_dump = 10000000;
	foreground_position_table.dump(foreground_positions, max_dump);
	std::sort(foreground_positions.begin(), foreground_positions.end(), ReadPosSeqnameCmp());

	Logger::push_task(task_name, foreground_positions.size());
	LoggerTask& task = Logger::get_task(task_name);

    typedef std::pair<float, float> WeightedGC;

	std::vector<WeightedGC> foreground_gc, background_gc;

    int  seqlen = 0;
    SeqName curr_seqname;
    char* seq = NULL;
    twobitseq tbseq;
    twobitseq tbseqrc;
    rng_t rng;

    pos_t L = seqbias[0] ? seqbias[0]->getL() : 0;

    std::vector<ReadPos>::iterator i;
    for (i = foreground_positions.begin(); i != foreground_positions.end(); ++i) {
        if (i->seqname != curr_seqname) {
            free(seq);
            seq = faidx_fetch_seq(ref_file, i->seqname.get().c_str(), 0, INT_MAX, &seqlen);
            Logger::debug("read sequence %s.", i->seqname.get().c_str());

            if (seq == NULL) {
                Logger::warn("warning: reference sequence not found, skipping.");
            }
            else {
                for (char* c = seq; *c; c++) *c = tolower(*c);
                tbseq = seq;
                tbseqrc = tbseq;
                tbseqrc.revcomp();

            }

            curr_seqname = i->seqname;
        }

        if (seq == NULL || (pos_t) tbseq.size() < median_frag_len) continue;

        // fragments with many copies tend to have too much weight when training
        // leading to somewhat less than stable results.
        if (i->count > 4) continue;

        // sample background position
        boost::random::uniform_int_distribution<pos_t> random_uniform(
                i->start + L, i->end - median_frag_len);
        pos_t pos = random_uniform(rng);
        float gc = (float) gc_count(seq + pos, median_frag_len) / median_frag_len;
        float sb = seqbias[0] ?
                   seqbias[0]->get_bias(tbseq, pos - L) *
                   seqbias[1]->get_bias(tbseqrc, seqlen - pos - 1 - L) : 1.0;
        background_gc.push_back(WeightedGC(gc, 1.0 / sb));

        // sample foreground position
        if (i->strand == 0) {
            if (i->pos >= i->start && i->pos + median_frag_len - 1 <= i->end) {
                float sb = seqbias[0] ?
                           seqbias[0]->get_bias(tbseq, i->pos - L) *
                           seqbias[1]->get_bias(tbseqrc, seqlen - i->pos - 1 - L) : 1.0;

                foreground_gc.push_back(
                    WeightedGC((float) gc_count(seq + i->pos, median_frag_len) / median_frag_len,
                               1.0 / sb));
            }
        } else {
            if (i->pos - median_frag_len >= i->start && i->pos <= i->end) {
                float sb = seqbias[0] ?
                           seqbias[0]->get_bias(tbseq, i->pos - median_frag_len - L) *
                           seqbias[1]->get_bias(tbseqrc, seqlen - i->pos - median_frag_len - 1 - L) : 1.0;
                foreground_gc.push_back(
                    WeightedGC((float) gc_count(seq + i->pos - median_frag_len, median_frag_len) / median_frag_len,
                               1.0 /sb));
            }
        }
        task.inc();
    }

    free(seq);
    fai_destroy(ref_file);

#if 0
    FILE* out = fopen("gcbias.tsv", "w");
    fprintf(out, "group\tgc\tweight\n");
    BOOST_FOREACH (WeightedGC& value, foreground_gc) {
        fprintf(out, "foreground\t%f\t%f\n", (double) value.first, (double) value.second);
    }
    BOOST_FOREACH (WeightedGC& value, background_gc) {
        fprintf(out, "background\t%f\t%f\n", (double) value.first, (double) value.second);
    }
    fclose(out);
#endif

    // bin into equally sized bins, measure bias

    std::sort(foreground_gc.begin(), foreground_gc.end());
    std::sort(background_gc.begin(), background_gc.end());

#if 0
    bins.resize(constants::gcbias_num_bins);
    for (size_t i = 0; i < constants::gcbias_num_bins; ++i) {
    	bins[i] = foreground_gc[(i+1) * (foreground_gc.size() / constants::gcbias_num_bins)].first;
    }
    bins.back() = 1.0;
#endif

    double fore_total_weight = 0.0, back_total_weight = 0.0;
    BOOST_FOREACH (const WeightedGC& gc, foreground_gc) {
        fore_total_weight += gc.second;
    }

    BOOST_FOREACH (const WeightedGC& gc, background_gc) {
        back_total_weight += gc.second;
    }

    bin_bias.resize(constants::gcbias_num_bins);
    size_t j_fore = 0, j_back = 0;
    for (size_t i = 0; i < constants::gcbias_num_bins; ++i) {
    	double fore_weight = 0.0;
        float gc_upper = constants::gcbias_bins[i];
        while (j_fore < foreground_gc.size() && foreground_gc[j_fore].first <= gc_upper) {
            fore_weight += foreground_gc[j_fore].second;
            ++j_fore;
        }
        fore_weight /= fore_total_weight;

        double back_weight = 0.0;
        while (j_back < background_gc.size() && background_gc[j_back].first <= gc_upper) {
            back_weight += background_gc[j_back].second;
            ++j_back;
        }
        back_weight /= back_total_weight;

    	bin_bias[i] = fore_weight / back_weight;

        if (!boost::math::isfinite(bin_bias[i])) bin_bias[i] = 1.0;
        if (bin_bias[i] > constants::gcbias_max_bias) {
            bin_bias[i] = constants::gcbias_max_bias;
        }
        if (bin_bias[i] < 1.0 / constants::gcbias_max_bias) {
            bin_bias[i] = 1.0 / constants::gcbias_max_bias;
        }
    }

    Logger::pop_task(task_name);
}

double GCBias::get_bias(double gc)
{
    const float* bin =
        std::lower_bound(constants::gcbias_bins,
                         constants::gcbias_bins + constants::gcbias_num_bins,
                         gc);

    size_t i = bin - constants::gcbias_bins;

    if (i >= constants::gcbias_num_bins) {
        return bin_bias.back();
    }

    return bin_bias[i];
}

GCBias::~GCBias()
{

}


#include "biggles/observation.hpp"
#include "biggles/detail/random.hpp"
#include "biggles/mh_moves/mh_moves.hpp"
#include "biggles/mh_moves/utility.hpp"
#include "biggles/simulate.hpp"
#include "biggles/partition.hpp"
#include "biggles/samplers.hpp"
#include "biggles/partition_sampler.hpp"
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <fstream>
#include <string>
#include <Eigen/Dense>

// include this last to stop pre-processor macros breaking things
extern "C" {
#include <ccan/tap/tap.h>
}

using namespace biggles;
using boost::format;

int main(int argc, char** argv)
{
    // seed the PRNG to ensure consistent results across runs.
    biggles::detail::seed_prng(0xfacedead);

    //bool verbose = (argc > 1) && (0 == strcmp(argv[1], "-v"));

    plan_tests(2);

    boost::shared_ptr<observation_collection> obs_col(new observation_collection());
    obs_col->insert(new_obs(0, 0, 0));
    obs_col->insert(new_obs(0, 1, 0));
    obs_col->insert(new_obs(1, 0, 0));
    obs_col->insert(new_obs(0, 0, 1));
    obs_col->insert(new_obs(0, 0.5, 1));
    obs_col->insert(new_obs(0.5, 0, 1));
    obs_col->insert(new_obs(0, 0, 2));
    obs_col->insert(new_obs(0, 1, 2));
    obs_col->insert(new_obs(1, 0, 2));
    obs_col->insert(new_obs(0, 0, 3));
    obs_col->insert(new_obs(0, 0.5, 3));
    obs_col->insert(new_obs(0.5, 0, 3));
    obs_col->insert(new_obs(0, 0, 4));
    obs_col->insert(new_obs(0, 1, 4));
    obs_col->insert(new_obs(1, 0, 4));
    obs_col->insert(new_obs(0, 0.3, 5));
    obs_col->insert(new_obs(0.3, 0, 5));

    clutter_ptr clutter(new clutter_t());

    boost::shared_ptr<track_collection> tracks(new track_collection());
    clutter->insert(obs_col->begin(), obs_col->end());

    Eigen::Matrix2f obs_var;
    obs_var << 0.01, 0.0, 0.0, 0.01;
    model::parameters input_paras;
    model::birth_rate(input_paras) = 1.0f;
    model::clutter_rate(input_paras) = 0.5f;
    model::survival_probability(input_paras) = 0.6f;
    model::observation_probability(input_paras) = 0.9f;
    model::observation_error_covariance(input_paras) = obs_var;
    model::constraint_radius(input_paras) = 5.0f;
    model::process_noise_covariance(input_paras) = detail::initQ();
    partition_ptr_t original_part_ptr(
        new partition(tracks, clutter, expansion_from_observations(*tracks, *clutter)));
    original_part_ptr->set_expansion(0.f, 2.f, 0.f, 2.f);
    partition_sampler test_sampler(original_part_ptr, input_paras);
    partition_ptr_t current_part_ptr;
    diag("original partition volume = %.1f", original_part_ptr->volume());

    std::map<int, std::string> move_name;
    std::map<int, int> move_count;
    std::map<int, int> error_move_count;
    std::map<int, int> accepted_count;
    typedef std::map<int, int>::const_iterator move_map_iter;
    move_name[0] = "none";
    move_name[1] = "birth";
    move_name[2] = "death";
    move_name[3] = "extend";
    move_name[4] = "reduce";
    move_name[5] = "split";
    move_name[6] = "merge";
    move_name[7] = "update";
    move_name[8] = "identity";
    move_name[9] = "what? 1";

    int new_inf(0);
    int old_inf(0);
    int qq_inf(0);
    int total(10000);
    float best = test_sampler.current_sample_log_density();

    diag("last log density = %.2f", test_sampler.current_sample_log_density());

    for (int i = 0; i < total; ++i) {
        float P_old = test_sampler.current_sample_log_density();
        if (best < P_old) best = P_old;
        current_part_ptr = test_sampler.draw().partition_sample_ptr;
        int proposed_move = test_sampler.proposed_sample().executed_move;
        int last_move = test_sampler.last_move();
        move_count[proposed_move]++;
        accepted_count[last_move]++;
        float P_new = test_sampler.last_log_density();
        float qq = test_sampler.last_pdr();
        /*
        std::cout
            << format("%7.3f < %10.3f - %10.3f %+10.3f = %10.3f (%6s) ? %2s #%2d")
            % alpha % P_new % P_old % qq % mh_ratio
            % move_name[proposed_move] % (alpha < mh_ratio ? "OK" : "-")
            % test_sampler.last_sample().template get<0>().tracks().size()
            << std::endl
        ;
        */
        if (isinff(qq)   ) qq_inf++;
        if (isinff(P_new)) new_inf++;
        if (isinff(P_old)) old_inf++;
        if (isinff(qq) or isinff(P_new) or isinff(P_old)) error_move_count[proposed_move]++;
    }
    model::parameters paras = test_sampler.parameters();
    diag("last volume = %.2f", test_sampler.last_partition()->volume());
    diag("best log-PDF = %.2f", best);
    diag("--------------------");
    diag("birth rate        (b) = %.4f", model::birth_rate(paras));
    diag("clutter rate      (c) = %.4f", model::clutter_rate(paras));
    diag("survival prob.    (s) = %.4f", model::survival_probability(paras));
    diag("observation prob. (o) = %.4f", model::observation_probability(paras));
    diag("--------------------");
    diag("proposed inf  = %d, (%6.2f%%)", new_inf, (float(new_inf)/float(total)*100.0));
    diag("current inf   = %d", old_inf);
    diag("QQ infinity   = %d", qq_inf);
    diag("total samples = %d", total);
    for (move_map_iter iter = error_move_count.begin(); iter != error_move_count.end(); ++iter) {
        diag("%s = %d", move_name.at(iter->first).c_str(), iter->second);
    }
    diag("--------------------");
    diag("proposed moves: ");
    for (int i = 0; i < 8; ++i) {
        diag("%s = %d", move_name.at(i).c_str(), move_count[i]);
    }
    diag("--------------------");
    diag("accepted moves: ");
    for (int i = 0; i < 8; ++i) {
        diag("%s = %d", move_name.at(i).c_str(), accepted_count[i]);
    }

    ok(mh_moves::observations_could_be_neighbours(new_obs(59.3354, 93.3354, 27), new_obs(59.3354, 90.3354, 26)),
        "Obs could be neighbours? ");

    ok1(true);

    return exit_status();
}


#ifndef VIENNACL_LINALG_TRED2_HPP_
#define VIENNACL_LINALG_TRED2_HPP_

/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** @file viennacl/linalg/tred2.hpp
*   @brief optimized CPU tridiagonalization
*
*   Contributed by Alex Christensen.
*/

#include "viennacl/linalg/host_based/sse_kernels.hpp"
#include <boost/numeric/ublas/matrix.hpp>

namespace viennacl
{
  namespace linalg
  {
    /** @brief Inplace reduction of a hermitian (or real symmetric) to tridiagonal form using householder similarity transforms (preserving eigenvalues)
     *
     * @param A            A dense matrix to be tridiagonalized
     * @param block_size   The block size to be used
     */
    template<typename ScalarType>
    void inplace_tred2(boost::numeric::ublas::matrix<ScalarType> const & A, vcl_size_t block_size = 1)
    {

#ifdef VIENNACL_WITH_OPENMP
      vcl_size_t num_threads=(vcl_size_t)omp_get_max_threads();
      omp_set_num_threads(omp_get_max_threads());
#else
      vcl_size_t num_threads=1;
#endif

      vcl_size_t n=A.size1();
      if (n!=A.size2())
        std::cerr << "ViennaCL: Warning in inplace_tred2(): Matrix is not hermitian (or real symmetric)" << std::endl;
      block_size=std::min(n,block_size);

      //get pointers to the value arrays
      ScalarType** rows=new ScalarType*[n];
      for (vcl_size_t i=0;i<n;i++)
        rows[i]=(ScalarType*)&A(i,0);

      //call the optimized CPU code
      inplace_tred2(rows,A.size1(),block_size,num_threads);

      delete [] rows;
    }

  } //namespace linalg
} //namespace viennacl
#endif


#pragma once
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Bool.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Vector3Stamped.h"

#include <boost/thread.hpp>
#include "opencv2/core/core.hpp"
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "Eigen/Dense"
#include "Eigen/Geometry"
#include "Eigen/Eigenvalues"
#include "EigenLM.hpp"

#include <rosbag/bag.h>

#include <cmath>

#include <rosbag/bag.h>
#include <rosbag/view.h>

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

using namespace Eigen;
using namespace std;

ros::Duration d(0.01);

typedef std::deque<geometry_msgs::PoseStamped> PoseVector;
typedef std::vector<Affine3f> Affine3fVector;
enum METHOD { OLD,NEW,WHOLE };

class Listener
{

public:
    // parameters need to be dynamicly adjusted

    // = time_aruco - time_opti(from the realy world to received in this thread)
    // used when pub optiRef
    float m_ArucoDelayCompensate;  // add to opti time
    METHOD m_methodUesd;

private:
    //
    Eigen::Vector3f m_P_aruco, m_P_opti, m_P_arFromOpti;

    geometry_msgs::PoseStamped m_Pose_aruco, m_Pose_opti, m_RefPose,m_Pose_err;
    PoseVector m_OptiPose_V,m_RefPose_V;
    const uint m_RefPosV_N = 120 * 0.2;  // the number stored in RefPose_V
    Affine3f m_T_r2c, m_T_w2b;
    Affine3f m_T_r2w, m_T_w2r, m_T_b2c;
    Affine3f m_T_r2c_Ref;
    Affine3f m_T_r2c_err;

    typedef  Matrix<float,4,8> Matrix48f;
    Matrix<float,4,8> X_r,X_w;

    ros::NodeHandle n;
    ros::Publisher opti_PoseRef_pub;
    ros::Publisher PoseErr_pub,ErrEuler_pub,optiEuler_pub,RefEuler_pub,arucoEuler_pub,RefVelocity_pub ;
    rosbag::Bag m_bag;
    const string m_InstallCalibResultPath = "Aruco_InstallCalibResult.bag";
    const string m_InstallCalibInputPath = "InstallCalibInput.bag";
    const string m_InCal_opti_Topic = "m_Pose_opti";
    const string m_InCal_aruco_Topic = "m_Pose_aruco";

    Vector3i m_aruco_eulerOrder,m_opti_eulerOrder;

    float m_aruco_ROSdelay,m_opti_ROSdelay;     // delay from ros pub to sub

  void CalDataDelay(float& optiDelay, float& arucoDelay);
  bool IsOptiNew(float& optiDelay);
  bool IsArucoNew(float& arucoDelay);

  float Cal_PoseErr();
  int FindClosestTime(PoseVector& PoseV, ros::Time,float&);
  void Cal_OptiRef();

  void Pose_to_Affine(geometry_msgs::PoseStamped Pose_ros_a2b, Affine3f& T_eigen_a2b);
  void Affine_to_Pose(const Affine3f& T_eigen_a2b,geometry_msgs::PoseStamped& Pose_ros_a2b);

  void Calculate_Pr2w(Eigen::Affine3f& Out_T_r2w);
  bool DoInstallCalib(bool IsLoadFromFile);
  void Cal_Install_Old(const int& N_points,const Affine3fVector& T_w2b_Array, const Affine3fVector& T_r2c_Array, Affine3f& Out_T_b2c, Affine3f& Out_T_r2w);
  void Cal_Install_New(const int& N_points,const Affine3fVector& T_w2b_Array, const Affine3fVector& T_r2c_Array, Affine3f& Out_T_b2c, Affine3f& Out_T_r2w);
  bool LoadBag_InstallCalibData( int& N_points,std::vector<Affine3f>& T_w2b_Array,std::vector<Affine3f>& T_r2c_Array);
  void LoadBag_CalErr(const string Path);
  bool GetInstallCalibData_Online( int N_points,std::vector<Affine3f>& T_w2b_Array,std::vector<Affine3f>& T_r2c_Array);
  void CheckCalibErr( const std::vector<Affine3f>& T_w2b_Array, const std::vector<Affine3f>& T_r2c_Array,Affine3f& In_T_b2c,Affine3f& In_T_r2w );

  bool ReadBag_Pose(const string Path, const string topic, std::vector<geometry_msgs::PoseStamped>& PoseVec);
  bool BagRead_InstallCalib(METHOD methodUsed);
  void InitInstallCalib();

  void UpdateInstallCalibBagFile(Affine3f* Out_T_b2c_p,Affine3f* Out_T_r2w_p,int N);
  void WriteBag_T(const Affine3f& T, const string name);
  void AppendBag_T(const Affine3f& T,const string name);

  void GetQM( Quaternionf Q,Matrix4f& M );
  void GetQMt( Quaternionf Q,Matrix4f& Mt );
  void NormalizeQ(Quaternionf &Q);

  void NormalizeAffine(Affine3f& T);

  void PoseMsg_to_EulerMsg(geometry_msgs::PoseStamped pose, geometry_msgs::Vector3Stamped& euler,Vector3i EulerOrder,string eulerFrameid);

  void modifyRosTime(ros::Time& Time,float addSec);

public:

  Listener();
  void chatter_aruco_pose(const geometry_msgs::PoseStamped::ConstPtr& msg);
  void chatter_opti_pose(const geometry_msgs::PoseStamped::ConstPtr& msg);

  void chatter_calibInstall(const std_msgs::Float32ConstPtr& msg)
  {
    cout << "received calib command" << endl;
    float d = msg->data;

    bool IsLoadFromFile = true;
    if(d<0)
    {
        IsLoadFromFile = false; // on line
        m_methodUesd = (METHOD) (-msg->data-1);
    }
    else
    {
        m_methodUesd = (METHOD) (msg->data-1);
    }
    cout << "(chatter_calibInstall) : m_methodUesd = " << m_methodUesd << endl;
    cout << "(chatter_calibInstall) : IsLoadFromFile = " << IsLoadFromFile << endl;

    this->DoInstallCalib(IsLoadFromFile);
  }
  void chatter_arucoDelayAdjust(const std_msgs::Float32ConstPtr& msg)
  {
      m_ArucoDelayCompensate = msg->data;
      cout<<"in chatter_arucoDelayAdjust " << endl << "m_ArucoDelayCompensate = " << m_ArucoDelayCompensate << endl;
  }

};


// plymerge
// merge multiple pointclouds together and subsample to remove duplicates
// only XYZ data is preser
// intended to be used to estimate background for automatic pointcloud cleanup
// by John De Witt

#include <Eigen/Dense>

#include <opencv2/opencv.hpp>
//#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/eigen.hpp>
//#include <opencv2/imgcodecs.hpp>

//#include <ImfRgbaFile.h>
//#include <ImfStringAttribute.h>
//#include <ImfMatrixAttribute.h>
//#include <ImfArray.h>

#include <map>
#include <math.h>
#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include <fstream>
#include <string>

#include <assert.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdio.h>
#include <string.h>

//#include <math.h>
//#include <string.h>
//#include <time.h>
//#include <stdint.h>

#include "util.h"
#include "sl_util.h"

#include <Eigen/Core>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>

using namespace cv;
using namespace std;
//using namespace Imf;
//using namespace Imath;

#define THRESHOLD_VALUE 5

#define SUBSAMPLE_GRID 0
#define SUBSAMPLE_POINT 1

// Types
typedef pcl::PointNormal PointNT;
typedef pcl::PointXYZ PointXYZT;
typedef pcl::PointCloud<PointXYZT> PointCloudT;

static void help()
{
   printf("This program accepts two or more ply point cloud files and outputs a merged and downsampled version. No registration is applied.\n"
          "ex: plymerge model_01.ply model_02.ply ..\n"
          "\n");
}

int main( int argc, char** argv ) {

   if(argc<2)
   {
      printf("not enough arguments, exiting..\n");
      return -1;
   }

   vector<string> fnames;
   vector<PointCloudT::Ptr> clouds;
   PointCloudT::Ptr merged_cloud (new PointCloudT);
   string fname_out = "merged_cloud.ply";

   // point lists
   vector< Vec3f > in_triPoints;
   vector< Vec3b > in_rgbPoints;

   vector< Vec3f > out_triPoints;
   vector< Vec3b > out_rgbPoints;

   float leaf_scl = 1.0;

   for(int i=1; i<argc; i++)
   {
        char* s=argv[i];
        if( strcmp(s,"-s") == 0 )
        {
            float v = 1;
            if( i+1 >= argc )
               return fprintf(stderr,"Leaf scale not provided\n"), -1;
            if( sscanf( argv[++i], "%f", &v) != 1 || v<=0 )
               return fprintf(stderr,"Invalid leaf scale value\n"), -1;
            leaf_scl = v;
            printf("leaf_scl set to %0.2f\n",leaf_scl);
        }
        else
        {
            string fn = s;
            fnames.push_back(fn);
            printf("argv[%d] : %s\n",i,fn.c_str());
        }
   }

   for (std::vector<string>::iterator it = fnames.begin() ; it != fnames.end(); ++it)
   {
       PointCloudT::Ptr tmp_cloud (new PointCloudT);
       if (pcl::io::loadPLYFile<PointXYZT>(*it, *tmp_cloud) == -1)
       {
           cout << "\tError loading [" << *it << "]" << endl;
           return -1;
       }
       else
       {
           printf("loaded [%s].. %8d points\n",it->c_str(),(int)tmp_cloud->points.size());
           for (size_t i = 0; i < tmp_cloud->points.size(); ++i)
           {
               PointXYZT tmp_pt(tmp_cloud->points[i]);
               merged_cloud->push_back(tmp_pt);
               if(i<0)
               std::cout << "    " << tmp_cloud->points[i].x
                         << " "    << tmp_cloud->points[i].y
                         << " "    << tmp_cloud->points[i].z << std::endl;
           }
       }
   }

   int pt_before = merged_cloud->points.size();

   printf("downsampling to %0.2fmm grid..\n",leaf_scl);
   pcl::VoxelGrid<PointXYZT> grid;
   const float leaf = leaf_scl;
   grid.setLeafSize(leaf, leaf, leaf);
   grid.setInputCloud(merged_cloud);
   grid.filter(*merged_cloud);

   int pt_after = merged_cloud->points.size();

   printf("in_size  = %d\n",pt_before);
   printf("out_size = %d\n",pt_after);

    for (size_t i = 0; i < merged_cloud->points.size(); ++i)
    {
       Vec3f tmp_pt(merged_cloud->points[i].x,merged_cloud->points[i].y,merged_cloud->points[i].z);
       out_triPoints.push_back(tmp_pt);
    }

   // PLY output
   if( true ){
      printf("saving ply file..\n");

      ofstream myfile;
      myfile.open (fname_out, ios::out | ios::binary);

      if(myfile.is_open()){
         printf("writing PLY file: [%s] [%d points]\n", fname_out.c_str(), (int)out_triPoints.size() );
         char buf[50];
         sprintf(buf, "%d", (int)out_triPoints.size());

         // write header
         myfile << "ply\n";
         myfile << "format binary_little_endian 1.0\n";
         myfile << "comment author: john dewitt\n";
         myfile << "element vertex " << buf << "\n";
         myfile << "property float x\n";
         myfile << "property float y\n";
         myfile << "property float z\n";
         //myfile << "property uchar red\n";
         //myfile << "property uchar green\n";
         //myfile << "property uchar blue\n";
         myfile << "end_header\n";

         // write binary data
         for(int i=0;i<out_triPoints.size();i++){
            myfile.write( (char*)(&out_triPoints[i]), sizeof(float)*3 );
            //myfile.write( (char*)(&out_rgbPoints[i]), sizeof(uchar)*3 );
         }
      }

   }

   return 0;
}


// Copyright 2020 The "Oko" project authors. All rights reserved.
// Use of this source code is governed by a MIT license that can be
// found in the LICENSE file.

#include "viewer/ui/go_to_timestamp_dialog.h"

#include <algorithm>
#include <boost/algorithm/string/trim.hpp>
#include <charconv>
#include <limits>

namespace oko {

namespace {
const int kFieldWidth = 15;
const std::string_view kSecondsLabel = "S:";
const std::string_view kNanoSecondsLabel = " NS:";
}  // namespace

GoToTimestampDialog::GoToTimestampDialog(AppModel* model) noexcept
    : DialogWindow(2),
      app_model_(model) {
  margin_ = std::max<int>(
      0,
      width_ - 2 * kFieldWidth - kSecondsLabel.size() -
          kNanoSecondsLabel.size()) / 2;
  int cur_x = margin_ + kSecondsLabel.size();
  // Seconds field
  fields_[0] = new_field(1, kFieldWidth, 1, cur_x, 0, 0);
  cur_x += kFieldWidth + kNanoSecondsLabel.size();
  set_field_back(fields_[0], A_REVERSE);
  set_field_type(fields_[0], TYPE_INTEGER, 0, 0, std::numeric_limits<int>::max);
  field_opts_off(fields_[0], O_AUTOSKIP);
  // Nanoseconds field
  fields_[1] = new_field(1, kFieldWidth, 1, cur_x, 0, 0);
  set_field_back(fields_[1], A_REVERSE);
  set_field_type(fields_[1], TYPE_INTEGER, 9, 0, 999999999);
  field_opts_off(fields_[1], O_AUTOSKIP);

  LogRecord::time_point selected_time_point =
      app_model_->GetSelectedRecordTimestamp();
  int sec_nsec[2];
  sec_nsec[0] = std::chrono::duration_cast<std::chrono::seconds>(
      selected_time_point.time_since_epoch()).count();
  sec_nsec[1] = std::chrono::duration_cast<std::chrono::nanoseconds>(
      selected_time_point.time_since_epoch() -
          std::chrono::seconds(sec_nsec[0])).count();
  for (int i = 0; i < 2; ++i) {
    char* buf = field_buffer(fields_[i], 0);
    std::to_chars(buf, buf + kFieldWidth, sec_nsec[i]);
  }

  InitForm();
}

void GoToTimestampDialog::DisplayImpl() noexcept {
  DialogWindow::DisplayImpl();
  int y = 0, x = 0;
  getyx(subwindow_.get(), y, x);
  mvwaddstr(subwindow_.get(), 1, margin_, kSecondsLabel.data());
  mvwaddstr(subwindow_.get(), 1, margin_ + kFieldWidth + kSecondsLabel.size(),
      kNanoSecondsLabel.data());
  wmove(subwindow_.get(), y, x);
}

bool GoToTimestampDialog::HandleEnter() noexcept {
  int sec_nsec[2];
  for (int i = 0; i < 2; ++i) {
    std::string buffer_data = field_buffer(fields_[i], 0);
    boost::algorithm::trim(buffer_data);
    auto res = std::from_chars(
        buffer_data.data(),
        buffer_data.data() + buffer_data.size(),
        sec_nsec[i]);
    if (res.ec != std::errc() ||
        res.ptr != buffer_data.data() + buffer_data.size()) {
      return false;
    }
  }
  LogRecord::time_point tp = LogRecord::time_point() +
      std::chrono::seconds(sec_nsec[0]) +
          std::chrono::nanoseconds(sec_nsec[1]);
  app_model_->SelectRecordByTimestamp(tp);
  return true;
}

std::string GoToTimestampDialog::GetTitle() const noexcept {
  return "Target timestamp";
}

}  // namespace oko


// Copyright (c) 2013, Thomas Goyne <plorkyeran@aegisub.org>
//
// Permission to use, copy, modify, and distribute this software for any
// purpose with or without fee is hereby granted, provided that the above
// copyright notice and this permission notice appear in all copies.
//
// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
// WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
// ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
// WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
// ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
// OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
//
// Aegisub Project http://www.aegisub.org/

/// @file ass_time.cpp
/// @brief Class for managing timestamps in subtitles
/// @ingroup subs_storage
///

#include "config.h"

#include "ass_time.h"

#include "utils.h"

#include <libaegisub/util.h>

#include <algorithm>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/format.hpp>
#include <boost/range/adaptor/filtered.hpp>

AssTime::AssTime(int time) : time(mid(0, time, 10 * 60 * 60 * 1000 - 1)) { }

AssTime::AssTime(std::string const& text) {
	int after_decimal = -1;
	int current = 0;
	for (char c : text | boost::adaptors::filtered(boost::is_any_of(",.0123456789:"))) {
		if (c == ':') {
			time = time * 60 + current;
			current = 0;
		}
		else if (c == '.' || c == ',') {
			time = (time * 60 + current) * 1000;
			current = 0;
			after_decimal = 100;
		}
		else if (after_decimal < 0) {
			current *= 10;
			current += c - '0';
		}
		else {
			time += (c - '0') * after_decimal;
			after_decimal /= 10;
		}
	}

	// Never saw a decimal, so convert now to ms
	if (after_decimal < 0)
		time = (time * 60 + current) * 1000;

	// Limit to the valid range
	time = mid(0, time, 10 * 60 * 60 * 1000 - 1);
}

std::string AssTime::GetAssFormated(bool msPrecision) const {
	std::string ret(10 + msPrecision, ':');
	ret[0] = '0' + GetTimeHours();
	ret[2] = '0' + (time % (60 * 60 * 1000)) / (60 * 1000 * 10);
	ret[3] = '0' + (time % (10 * 60 * 1000)) / (60 * 1000);
	ret[5] = '0' + (time % (60 * 1000)) / (1000 * 10);
	ret[6] = '0' + (time % (10 * 1000)) / 1000;
	ret[7] = '.';
	ret[8] = '0' + (time % 1000) / 100;
	ret[9] = '0' + (time % 100) / 10;
	if (msPrecision)
		ret[10] = '0' + time % 10;
	return ret;
}

int AssTime::GetTimeHours() const { return time / 3600000; }
int AssTime::GetTimeMinutes() const { return (time % 3600000) / 60000; }
int AssTime::GetTimeSeconds() const { return (time % 60000) / 1000; }
int AssTime::GetTimeMiliseconds() const { return (time % 1000); }
int AssTime::GetTimeCentiseconds() const { return (time % 1000) / 10; }

SmpteFormatter::SmpteFormatter(agi::vfr::Framerate fps, std::string sep)
: fps(std::move(fps))
, sep(std::move(sep))
{
}

std::string SmpteFormatter::ToSMPTE(AssTime time) const {
	int h=0, m=0, s=0, f=0;
	fps.SmpteAtTime(time, &h, &m, &s, &f);
	return str(boost::format("%02d%s%02d%s%02d%c%02d") % h % sep % m % sep % s % sep % f);
}

AssTime SmpteFormatter::FromSMPTE(std::string const& str) const {
	std::vector<std::string> toks;
	boost::split(toks, str, boost::is_any_of(sep));
	if (toks.size() != 4) return 0;

	int h, m, s, f;
	agi::util::try_parse(toks[0], &h);
	agi::util::try_parse(toks[1], &m);
	agi::util::try_parse(toks[2], &s);
	agi::util::try_parse(toks[3], &f);
	return fps.TimeAtSmpte(h, m, s, f);
}