text:
#ifndef RPVG_SRC_PATHABUNDANCEESTIMATOR_HPP
#define RPVG_SRC_PATHABUNDANCEESTIMATOR_HPP

#include <vector>
#include <random>

#include <Eigen/Dense>

#include "path_estimator.hpp"
#include "path_cluster_estimates.hpp"
#include "read_path_probabilities.hpp"
#include "utils.hpp"

using namespace std;

class PathAbundanceEstimator : public PathEstimator {

    public:

        PathAbundanceEstimator(const uint32_t max_em_its_in, const double max_rel_em_conv_in, const uint32_t num_gibbs_samples_in, const uint32_t gibbs_thin_its_in, const double prob_precision);
        virtual ~PathAbundanceEstimator() {};

        void estimate(PathClusterEstimates * path_cluster_estimates, const vector<ReadPathProbabilities> & cluster_probs, mt19937 * mt_rng);

    protected:

        const uint32_t max_em_its;
        const double max_rel_em_conv;

        const uint32_t num_gibbs_samples;
        const uint32_t gibbs_thin_its;

        void EMAbundanceEstimator(PathClusterEstimates * path_cluster_estimates, const Utils::ColMatrixXd & read_path_probs, const Utils::RowVectorXd & read_counts) const;
        void gibbsReadCountSampler(PathClusterEstimates * path_cluster_estimates, const Utils::ColMatrixXd & read_path_probs, const Utils::RowVectorXd & read_counts, const double gamma, mt19937 * mt_rng, const uint32_t num_samples) const;
};

class MinimumPathAbundanceEstimator : public PathAbundanceEstimator {

    public:

        MinimumPathAbundanceEstimator(const uint32_t max_em_its, const double max_rel_em_conv, const uint32_t num_gibbs_samples, const uint32_t gibbs_thin_its, const double prob_precision);
        ~MinimumPathAbundanceEstimator() {};

        void estimate(PathClusterEstimates * path_cluster_estimates, const vector<ReadPathProbabilities> & cluster_probs, mt19937 * mt_rng);

        vector<uint32_t> weightedMinimumPathCover(const Utils::ColMatrixXb & read_path_cover, const Utils::RowVectorXd & read_counts, const Utils::RowVectorXd & path_weights) const;
};

class NestedPathAbundanceEstimator : public PathAbundanceEstimator {

    public:

        NestedPathAbundanceEstimator(const uint32_t group_size_in, const double min_hap_prob_in, const bool infer_collapsed_in, const bool use_group_post_gibbs_in, const uint32_t max_em_its, const double max_rel_em_conv, const uint32_t num_gibbs_samples, const uint32_t gibbs_thin_its, const double prob_precision);
        ~NestedPathAbundanceEstimator() {};

        void estimate(PathClusterEstimates * path_cluster_estimates, const vector<ReadPathProbabilities> & cluster_probs, mt19937 * mt_rng);

    private:

        const uint32_t group_size;
        const double min_hap_prob;

        const bool infer_collapsed;
        const bool use_group_post_gibbs;

        void inferAbundancesIndependentGroups(PathClusterEstimates * path_cluster_estimates, const vector<ReadPathProbabilities> & cluster_probs, mt19937 * mt_rng) const;
        void inferAbundancesCollapsedGroups(PathClusterEstimates * path_cluster_estimates, const vector<ReadPathProbabilities> & cluster_probs, mt19937 * mt_rng);

        vector<vector<uint32_t> > findPathGroups(const vector<PathInfo> & paths) const;
        pair<vector<vector<uint32_t> >, vector<uint32_t> > findPathSourceGroups(const vector<PathInfo> & paths) const;

        void sampleGroupPathIndices(vector<vector<uint32_t> > * path_subset_samples, const PathClusterEstimates & group_path_cluster_estimates, const vector<uint32_t> & group, mt19937 * mt_rng) const;
        void selectPathSubsetIndices(spp::sparse_hash_map<vector<uint32_t>, double> * path_subset_samples, const PathClusterEstimates & group_path_cluster_estimates, const vector<vector<uint32_t> > & path_groups, mt19937 * mt_rng) const;

        void inferPathSubsetAbundance(PathClusterEstimates * path_cluster_estimates, const vector<ReadPathProbabilities> & cluster_probs, mt19937 * mt_rng, const spp::sparse_hash_map<vector<uint32_t>, double> & path_subset_samples) const;
};

#endif

max_stars_repo_name: jonassibbesen/vgprob

max_issues_repo_name: jonassibbesen/vgprob

max_forks_repo_name: jonassibbesen/vgprob


text:
//
// Copyright (c) 2021 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// 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)
//
// Official repository: https://github.com/CPPAlliance/http_proto
//

#ifndef BOOST_HTTP_PROTO_MIME_IMPL_MIME_TYPES_IPP
#define BOOST_HTTP_PROTO_MIME_IMPL_MIME_TYPES_IPP

#include <boost/http_proto/mime/mime_types.hpp>
#include <boost/http_proto/context.hpp>
#include <boost/http_proto/bnf/ctype.hpp>

namespace boost {
namespace http_proto {

mime_types::
~mime_types() noexcept = default;

namespace detail {

class mime_types_impl
    : public mime_types
    , public context::service
{
public:
    using key_type = mime_types;

    explicit
    mime_types_impl(
        context&) noexcept
    {
    }

    string_view
    content_type(
        string_view s) const noexcept override
    {
        return s;
    }
};

mime_types&
install_mime_types_service(
    context& ctx)
{
    return make_service<mime_types_impl>(ctx);
}

} // detail

} // http_proto
} // boost

#endif

ext: ipp


text:
#include "FWCore/ParameterSet/interface/ParameterSet.h"
#include "FWCore/ServiceRegistry/interface/Service.h"
#include "CondCore/DBOutputService/interface/PoolDBOutputService.h"
#include "CondFormats/Calibration/interface/Pedestals.h"
#include "CondFormats/Calibration/interface/mySiStripNoises.h"

#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include "writeMultipleRecords.h"

typedef boost::minstd_rand base_generator_type;
writeMultipleRecords::writeMultipleRecords(const edm::ParameterSet& iConfig)
{
   m_PedRecordName=iConfig.getParameter<std::string>("PedCallerName");
   m_StripRecordName=iConfig.getParameter<std::string>("StripCallerName");
}

writeMultipleRecords::~writeMultipleRecords()
{
  std::cout<<"writeMultipleRecords::writeMultipleRecords"<<std::endl;
}

void
writeMultipleRecords::analyze( const edm::Event& evt, const edm::EventSetup& evtSetup)
{
  std::cout<<"writeMultipleRecords::analyze "<<std::endl;
  base_generator_type rng(42u);
  boost::uniform_real<> uni_dist(0,1);
  boost::variate_generator<base_generator_type&, boost::uniform_real<> > uni(rng, uni_dist);
  edm::Service<cond::service::PoolDBOutputService> mydbservice;
  if( !mydbservice.isAvailable() ){
    std::cout<<"db service unavailable"<<std::endl;
    return;
  }
  try{
    mySiStripNoises* me = new mySiStripNoises;
    unsigned int detidseed=1234;
    unsigned int bsize=10;
    unsigned int nAPV=2;

    for (uint32_t detid=detidseed;detid<(detidseed+bsize);detid++){
      //Generate Noise for det detid
      std::vector<short> theSiStripVector;
      for(unsigned int strip=0; strip<128*nAPV; ++strip){
	std::cout<<strip<<std::endl;
	float noise = uni();;
	me->setData(noise,theSiStripVector);
      }
      me->put(detid,theSiStripVector);
    }

    mydbservice->writeOne(me,mydbservice->currentTime(),m_StripRecordName);

    Pedestals* myped=new Pedestals;
    for(int ichannel=1; ichannel<=5; ++ichannel){
      Pedestals::Item item;
      item.m_mean=1.11*ichannel;
      item.m_variance=1.12*ichannel;
      myped->m_pedestals.push_back(item);
    }
    mydbservice->writeOne(myped,mydbservice->currentTime(),m_PedRecordName);
  }catch(const cond::Exception& er){
    throw cms::Exception("DBOutputServiceUnitTestFailure","failed writeMultipleRecords",er);
    //std::cout<<er.what()<<std::endl;
  }catch(const cms::Exception& er){
    throw cms::Exception("DBOutputServiceUnitTestFailure","failed writeMultipleRecords",er);
  }/*catch(const std::exception& er){
    std::cout<<"caught std::exception "<<er.what()<<std::endl;
  }catch(...){
    std::cout<<"Funny error"<<std::endl;
    }*/
}
#include "FWCore/Framework/interface/MakerMacros.h"
DEFINE_FWK_MODULE(writeMultipleRecords);

ext: cc

max_stars_repo_name: nistefan/cmssw

max_issues_repo_name: nistefan/cmssw

max_forks_repo_name: nistefan/cmssw


text:
// This file is part of LatNet Builder.
//
// Copyright (C) 2012-2021  The LatNet Builder author's, supervised by Pierre L'Ecuyer, Universite de Montreal.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <fstream>
#include <chrono>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/join.hpp>
#include <iostream>
#include <limits>

#include "netbuilder/Types.h"
#include "netbuilder/Parser/CommandLine.h"
#include "netbuilder/Parser/EmbeddingTypeParser.h"
#include "netbuilder/Parser/NetConstructionParser.h"
#include "netbuilder/Parser/OutputStyleParser.h"
#include "netbuilder/Task/Task.h"

#include "latbuilder/Parser/Common.h"
#include "latbuilder/SizeParam.h"

// using namespace LatBuilder;
// using TextStream::operator<<;

namespace NetBuilder{
static unsigned int merit_digits_displayed = 0;

boost::program_options::options_description
makeOptionsDescription()
{
   namespace po = boost::program_options;
   po::options_description desc("allowed options");

   desc.add_options ()
   ("help,h", "produce help message")
   ("version", "show version")
    ("set-type,t", po::value<std::string>(),
    "(required) point set type; possible values:\n"
    "  lattice\n"
    "  net\n")
   ("construction,c", po::value<std::string>(),
   "digital net; possible constructions:\n"
   "  sobol\n"
   "  polynomial\n"
   "  lms\n"
   "  explicit\n")
    ("dimension,d", po::value<std::string>(),
    "(required) point set dimension\n")
   ("size-parameter,s", po::value<std::string>(),
    "(required) modulus of the net; possible values:\n"
   "  2^<max-level>\n"
   "  <modulus>: polynomial (list of coefficients: 1011 stands for 1 + X^2 + X^3) (only for polynomial construction)\n"
   )
   ("exploration-method,e", po::value<std::string>(),
    "(required) exploration method; possible values:\n"
    "  evaluation:<net_description>\n"
    "  exhaustive\n"
    "  random:<r>\n"
    "  full-CBC\n"
    "  random-CBC:<r>\n"
    "  mixed-CBC:<r>:<nb_full>\n"
    "where <net_description> is a net description (see documentation), <r> is the number of samples, and <nb_full> the number of coordinates for which full CBC exploration is used.")
   ("figure-of-merit,f", po::value<std::string>(),
    "(required) type of figure of merit; format: <merit>\n"
    "  and where <merit> is one of:\n"
    "    CU:P<alpha>  (requires l_2 norm)\n"
    "    CU:R (requires l_2 norm)\n"
    "    t-value (weights and norm-type are ignored)\n"
    "    projdep:t-value\n"
    "    projdep:t-value:starDisc\n"
    "    projdep:resolution-gap\n"
    "    CU:IA<alpha> (only for interlaced digital nets, requires l_1 norm)\n"
    "    CU:IB (only for interlaced digital nets, requires l_1 norm)\n"
    "where the \"CU:\" prefix stands for the coordinate-uniform evaluation algorithm and projdep for projection-dependent figure without coordinate-uniform evaluation algorithm\n")
    ("interlacing-factor,i", po::value<unsigned int>()->default_value(1),
    "(default: 1) interlacing factor. If larger than one, the constructed "
    "point set is an interlaced digital net. In this case, the figure of merit must be"
    "specific to interlaced digital nets.\n")
   ("norm-type,q", po::value<std::string>(),
    "norm type used to combine the value of the projection-dependent figure of merit for all projections; possible values:"
    "    <q>: a real number corresponding the l_<q> norm\n"
    "    inf: corresponding to the `max' norm\n")
    ("weights,w", po::value<std::vector<std::string>>()->multitoken(),
    "(required) whitespace-separated list of weights specifications (the actual weights are the sum of these); possible values:\n"
    "  product:<default>:<coord1-weight>[,...]\n"
    "  order-dependent:<default>:<order-1-weight>,...,<order-s-weight>\n"
    "  POD:<default>:<order-1-weight>,...,<order-s-weight>:<default>:<coord1-weight>[,...]\n"
    "  projection-dependent:<proj-1>:<weight-1>[:<proj-2>:<weight-2>[:...]]\n"
    "    where <proj-n> is a comma-separated list of coordinates\n"
    "  file:\"<file>\"\n"
    "    line format in <file>:\n"
    "      <i1>,<i2>,...: <weight>\n"
    "      order <x>: <weight>\n"
    "      default: <weight>\n"
    "    if <file> is `-' data is read from standard input\n")
   ("weights-power,p", po::value<Real>(),
    "(default: same value as for the --norm-type argument) real number specifying that the weights passed as input will be assumed to be already elevated at that power (a value of `inf' is mapped to 1)\n")
   ("multilevel,M", po::value<std::string>()->default_value("false"),
    "multilevel point set; possible values:\n"
   "  false (default)\n"
   "  true\n")
   ("combiner,C", po::value<std::string>(),
    "(required for multilevel) combiner for multilevel merit values; possible values:\n"
    "  sum\n"
    "  max\n"
    "  level:{<level>|max}\n")
   ("repeat,r", po::value<unsigned int>()->default_value(1),
    "(optional) number of times the construction must be executed\n"
   "(can be useful to obtain different results from random constructions)\n")
    ("verbose,v", po::value<std::string>()->default_value("0"),
   "specify the verbosity of the program;\n"
   "ranges between 0 (default) and 3\n")
    ("output-folder,o", po::value<std::string>(),
    "(optional) path to the folder for the outputs of LatNeBuilder. The contents of the folder may be overwritten. If the folder does not exist, it is created. If no path is provided, no output folder is created.")
    ("output-style,O", po::value<std::string>()->default_value(""),
    "(optional) TBD\n")
    ("merit-digits-displayed", po::value<unsigned int>()->default_value(0),
    "(optional) number of significant figures to use when displaying merit values\n");

   return desc;
}

boost::program_options::variables_map
parse(int argc, const char* argv[])
{
   namespace po = boost::program_options;

   auto desc = makeOptionsDescription();

   po::variables_map opt;
   po::store(po::parse_command_line(argc, argv, desc), opt);
   po::notify(opt);

   if (opt.count("help")) {
      std::cout << desc << std::endl;
      std::exit (0);
   }

   if (opt.count("weights") < 1)
      throw std::runtime_error("--weights must be specified (try --help)");
   for (const auto x : {"size-parameter", "exploration-method", "dimension", "figure-of-merit", "norm-type"}) {
      if (opt.count(x) != 1)
         throw std::runtime_error("--" + std::string(x) + " must be specified exactly once (try --help)");
   }

    if (opt["multilevel"].as<std::string>() == "true" && ! opt.count("combiner")){
      throw std::runtime_error("--combiner must be specified for multilevel set type (try --help)");
    }
   return opt;
}

#define BUILD_TASK(net_construction, point_set_type)\
NetBuilder::Parser::CommandLine<NetBuilder::NetConstruction::net_construction, NetBuilder::EmbeddingType::point_set_type> cmd;\
\
cmd.s_verbose = opt["verbose"].as<std::string>();\
cmd.s_explorationMethod = opt["exploration-method"].as<std::string>();\
cmd.s_size = opt["size-parameter"].as<std::string>();\
cmd.s_dimension = opt["dimension"].as<std::string>();\
cmd.s_figure = opt["figure-of-merit"].as<std::string>();\
cmd.s_weights       = opt["weights"].as<std::vector<std::string>>();\
cmd.m_normType = boost::lexical_cast<Real>(opt["norm-type"].as<std::string>());\
cmd.m_interlacingFactor = opt["interlacing-factor"].as<unsigned int>(); \
interlacingFactor = cmd.m_interlacingFactor;\
if (opt.count("combiner") < 1){\
  cmd.s_combiner = "";\
}\
else{\
  cmd.s_combiner = opt["combiner"].as<std::string>();\
}\
if (opt.count("weight-power") == 1 ){\
  cmd.m_weightPower = boost::lexical_cast<Real>(opt["weight-power"].as<std::string>());\
}\
else{\
  if (cmd.m_normType < std::numeric_limits<Real>::infinity())\
  {\
    cmd.m_weightPower = cmd.m_normType;\
  }\
  else\
  {\
    cmd.m_weightPower = 1;\
  }\
}\
task = cmd.parse();\
outputStyle = NetBuilder::Parser::OutputStyleParser<NetBuilder::NetConstruction::net_construction>::parse(s_outputStyle);

void TaskOutput(const Task::Task &task, std::string outputFolder, OutputStyle outputStyle, unsigned int interlacingFactor, std::vector<std::string> inputCL)
{
  unsigned int old_precision = (unsigned int)std::cout.precision();
  if (merit_digits_displayed){
    std::cout.precision(merit_digits_displayed);
  }
  std::cout << "====================\n       Result\n====================" << std::endl;
  std::cout << task.outputNet(OutputStyle::TERMINAL, interlacingFactor) << "Merit: " << task.outputMeritValue() << std::endl;

  if (outputFolder != ""){
    std::ofstream outFile;
    std::string fileName = outputFolder + "/output.txt";
    outFile.open(fileName);
    outFile << "# Input Command Line: " << boost::algorithm::join(inputCL, " ") << std::endl;
    outFile << "# Merit: " << task.outputMeritValue() << std::endl;
    outFile << task.outputNet(outputStyle, interlacingFactor);
    outFile.close();
  }

  if (merit_digits_displayed){
    std::cout.precision(old_precision);
  }
}

int main(int argc, const char *argv[])
{

   try {
     using namespace std::chrono;
     const std::string separator = "\n--------------------------------------------------------------------------------\n";

        auto opt = parse(argc, argv);

        auto repeat = opt["repeat"].as<unsigned int>();

        std::string outputFolder = "";
        if (opt.count("output-folder") >= 1){
          outputFolder = opt["output-folder"].as<std::string>();
          std::cout << "Writing in output folder: " << outputFolder << std::endl;
          if (! boost::filesystem::is_directory(outputFolder)){
            boost::filesystem::create_directories(outputFolder);
          }
        }

        // global variable
        merit_digits_displayed = opt["merit-digits-displayed"].as<unsigned int>();

        std::string s_multilevel = opt["multilevel"].as<std::string>();
        std::string s_construction = opt["construction"].as<std::string>();
        std::string s_outputStyle = opt["output-style"].as<std::string>();
        NetBuilder::EmbeddingType embeddingType = NetBuilder::Parser::EmbeddingTypeParser::parse(s_multilevel);

        NetBuilder::NetConstruction netConstruction;

        if (embeddingType==NetBuilder::EmbeddingType::UNILEVEL)
        {
          netConstruction =  NetBuilder::Parser::NetConstructionParser<NetBuilder::EmbeddingType::UNILEVEL>::parse(s_construction);
        }
        else
        {
          netConstruction =  NetBuilder::Parser::NetConstructionParser<NetBuilder::EmbeddingType::MULTILEVEL>::parse(s_construction);
        }

        std::chrono::time_point<std::chrono::high_resolution_clock> t0, t1;
        unsigned int interlacingFactor = 0;

        std::unique_ptr<NetBuilder::Task::Task> task;
        NetBuilder::OutputStyle outputStyle;

       if(netConstruction == NetBuilder::NetConstruction::SOBOL && embeddingType == NetBuilder::EmbeddingType::UNILEVEL){
          BUILD_TASK(SOBOL, UNILEVEL)
       }
       else if(netConstruction == NetBuilder::NetConstruction::SOBOL && embeddingType == NetBuilder::EmbeddingType::MULTILEVEL){
          BUILD_TASK(SOBOL, MULTILEVEL)
       }
       else if(netConstruction == NetBuilder::NetConstruction::POLYNOMIAL && embeddingType == NetBuilder::EmbeddingType::UNILEVEL){
          BUILD_TASK(POLYNOMIAL, UNILEVEL)
       }
       else if(netConstruction == NetBuilder::NetConstruction::EXPLICIT && embeddingType == NetBuilder::EmbeddingType::UNILEVEL){
          BUILD_TASK(EXPLICIT, UNILEVEL)
       }
       else if(netConstruction == NetBuilder::NetConstruction::EXPLICIT && embeddingType == NetBuilder::EmbeddingType::MULTILEVEL){
          BUILD_TASK(EXPLICIT, MULTILEVEL)
       }
       else if(netConstruction == NetBuilder::NetConstruction::LMS && embeddingType == NetBuilder::EmbeddingType::UNILEVEL){
          BUILD_TASK(LMS, UNILEVEL)
       }
       else if(netConstruction == NetBuilder::NetConstruction::LMS && embeddingType == NetBuilder::EmbeddingType::MULTILEVEL){
          BUILD_TASK(LMS, MULTILEVEL)
       }
       else {
         throw std::runtime_error("Unknown combination of NetConstruction and EmbeddingType");
       }

      std::vector<std::string> inputCL;
      if (argc > 1) {
        inputCL.assign(argv + 1, argv + argc);
      }

      for (unsigned i=0; i<repeat; i++){
        if (i == 0){
          std::cout << "====================\n       Input\n====================" << std::endl;
          std::cout << task->format();
          std::cout << std::endl;

          if (outputFolder != ""){
            std::ofstream outFile;
            std::string fileName = outputFolder + "/input.txt";
            outFile.open(fileName);
            outFile << "Input Command Line: " << boost::algorithm::join(inputCL, " ") << std::endl << std::endl;
            outFile << task->format();
            outFile.close();
          }
        }

          if (repeat > 1){
            std::cout << "====================\n       Run " << i+1 << "\n====================" << std::endl;
          }
          else{
            std::cout << "====================\nRunning the task... \n====================" << std::endl;
          }

          t0 = high_resolution_clock::now();
          task->execute();
          t1 = high_resolution_clock::now();
          auto dt = duration_cast<duration<double>>(t1 - t0);

          std::cout << std::endl;
          TaskOutput(*task, outputFolder, outputStyle, interlacingFactor, inputCL);
          std::cout << std::endl;
          std::cout << "ELAPSED CPU TIME: " << dt.count() << " seconds" << std::endl;
          task->reset();
      }
   }
   catch (LatBuilder::Parser::ParserError& e) {
      std::cerr << "COMMAND LINE ERROR: " << e.what() << std::endl;
      std::exit(1);
   }
   catch (std::exception& e) {
      std::cerr << "ERROR: " << e.what() << std::endl;
      std::exit(1);
   }

   return 0;
}

#undef BUILD_TASK
}

max_stars_repo_name: YochevedDarmon/latnetbuilder

max_issues_repo_name: YochevedDarmon/latnetbuilder

max_forks_repo_name: YochevedDarmon/latnetbuilder


text:
#include "base/reconstruction.h"
#include "util/logging.h"
#include "util/option_manager.h"
#include <pcl/common/eigen.h>
#include <pcl/io/ply_io.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <boost/algorithm/string.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <iostream>
#include <fstream>
#include <string>
#include <iterator>
#include <algorithm>
#include <Eigen/Dense>
#include <math.h>
#include <iomanip>
#include <sstream>
#include <boost/thread/thread.hpp>
#include <proj_api.h>
using namespace colmap;

#define IS_BIG_ENDIAN (*(uint16_t *)"\0\xff" < 0x100)

struct Vec3
{
    Vec3(double x = 0.0, double y = 0.0, double z = 0.0);
    Vec3(const Vec3 &o);

    double x_,y_,z_;    /**< The x, y and z values of the vector. **/

    /*!
      * \brief cross     The cross product between two vectors.
      **/
    Vec3 cross(Vec3 o) const;

    /*!
      * \brief dot     The scalar product between two vectors.
      **/
    double dot(Vec3 o) const;

    /*!
      * \brief length     The length of the vector.
      **/
    double length() const;

    /*!
      * \brief norm     Returns a normalized version of this vector.
      **/
    Vec3 norm() const;

    /*!
      * \brief Scales this vector.
      **/
    Vec3 operator*(double d) const;

    /*!
      * \brief Addition between two vectors.
      **/
    Vec3 operator+(Vec3 o) const;

    /*!
      * \brief Subtraction between two vectors.
      **/
    Vec3 operator-(Vec3 o) const;

    friend std::ostream & operator<<(std::ostream &os, Vec3 v)
    {
        return os << "[" << std::setprecision(8) << v.x_ << ", " << std::setprecision(4) << v.y_ << ", " << v.z_ << "]";
    }
};

class OnMat3
{
  public:
    OnMat3(Vec3 r1, Vec3 r2, Vec3 r3);
    OnMat3(const OnMat3 &o);

    Vec3 r1_;   /**< The first row of the matrix. **/
    Vec3 r2_;   /**< The second row of the matrix. **/
    Vec3 r3_;   /**< The third row of the matrix. **/
    Vec3 c1_;   /**< The first column of the matrix. **/
    Vec3 c2_;   /**< The second column of the matrix. **/
    Vec3 c3_;   /**< The third column of the matrix. **/

    /*!
      * \brief The determinant of the matrix.
      **/
    double det() const;

    /*!
      * \brief The transpose of the OnMat3 (equal to inverse).
      **/
    OnMat3 transpose() const;

    /*!
      * \brief Matrix multiplication between two ON matrices.
      **/
    OnMat3 operator*(OnMat3 o) const;

    /*!
      * \brief Right side multiplication with a 3d vector.
      **/
    Vec3 operator*(Vec3 o);

    friend std::ostream & operator<<(std::ostream &os, OnMat3 m)
    {
        return os << "[" << std::endl << m.r1_ << std::endl << m.r2_ << std::endl << m.r3_ << std::endl << "]" << std::endl;
    }
};

class Mat4
{
  public:
    Mat4();
    Mat4(OnMat3 rotation, Vec3 translation, double scaling);

    /*!
      * \brief Right side multiplication with a 3d vector.
      **/
    Vec3 operator*(Vec3 o);

    double r1c1_;   /**< Matrix element 0 0 **/
    double r1c2_;   /**< Matrix element 0 1 **/
    double r1c3_;   /**< Matrix element 0 2 **/
    double r1c4_;   /**< Matrix element 0 3 **/
    double r2c1_;   /**< Matrix element 1 0 **/
    double r2c2_;   /**< Matrix element 1 1 **/
    double r2c3_;   /**< Matrix element 1 2 **/
    double r2c4_;   /**< Matrix element 1 3 **/
    double r3c1_;   /**< Matrix element 2 0 **/
    double r3c2_;   /**< Matrix element 2 1 **/
    double r3c3_;   /**< Matrix element 2 2 **/
    double r3c4_;   /**< Matrix element 2 3 **/
    double r4c1_;   /**< Matrix element 3 0 **/
    double r4c2_;   /**< Matrix element 3 1 **/
    double r4c3_;   /**< Matrix element 3 2 **/
    double r4c4_;   /**< Matrix element 3 3 **/

    friend std::ostream & operator<<(std::ostream &os, Mat4 m)
    {
        std::stringstream ss;
        ss.precision(8);
        ss.setf(std::ios::fixed, std::ios::floatfield);

        ss << "[ " << m.r1c1_ << ",\t" << m.r1c2_ << ",\t" << m.r1c3_ << ",\t" << m.r1c4_ << " ]" << std::endl <<
              "[ " << m.r2c1_ << ",\t" << m.r2c2_ << ",\t" << m.r2c3_ << ",\t" << m.r2c4_ << " ]" << std::endl <<
              "[ " << m.r3c1_ << ",\t" << m.r3c2_ << ",\t" << m.r3c3_ << ",\t" << m.r3c4_ << " ]" << std::endl <<
              "[ " << m.r4c1_ << ",\t" << m.r4c2_ << ",\t" << m.r4c3_ << ",\t" << m.r4c4_ << " ]";

        return os << ss.str();
    }

};

class FindTransform
{
public:
    /*!
      * \brief findTransform    Generates an affine transform from the three 'from' vector to the three 'to' vectors.
      *                         The transform is such that transform * fromA = toA,
      *                                                    transform * fromB = toB,
      *                                                    transform * fromC = toC,
      **/
    void findTransform(Vec3 fromA, Vec3 fromB, Vec3 fromC, Vec3 toA, Vec3 toB, Vec3 toC);

    /*!
      * \brief error     Returns the distance beteween the 'from' and 'to' vectors, after the transform has been applied.
      **/
    double error(Vec3 fromA, Vec3 toA);

    Mat4 transform_;    /**< The affine transform. **/
};

Vec3::Vec3(double x, double y, double z) :x_(x), y_(y), z_(z)
{}
Vec3::Vec3(const Vec3 &o) : x_(o.x_), y_(o.y_), z_(o.z_)
{}

Vec3 Vec3::cross(Vec3 o) const
{
    Vec3 res;
    res.x_ = y_*o.z_ - z_*o.y_;
    res.y_ = z_*o.x_ - x_*o.z_;
    res.z_ = x_*o.y_ - y_*o.x_;
    return res;
}

double Vec3::dot(Vec3 o) const
{
    return x_*o.x_ + y_*o.y_ + z_*o.z_;
}

double Vec3::length() const
{
    return sqrt(x_*x_ + y_*y_ + z_*z_);
}

Vec3 Vec3::norm() const
{
    Vec3 res;
    double l = length();
    res.x_ = x_ / l;
    res.y_ = y_ / l;
    res.z_ = z_ / l;
    return res;
}

Vec3 Vec3::operator*(double d) const
{
    return Vec3(x_*d, y_*d, z_*d);
}

Vec3 Vec3::operator+(Vec3 o) const
{
    return Vec3(x_ + o.x_, y_ + o.y_,z_ + o.z_);
}

Vec3 Vec3::operator-(Vec3 o) const
{
    return Vec3(x_ - o.x_, y_ - o.y_,z_ - o.z_);
}

OnMat3::OnMat3(Vec3 r1, Vec3 r2, Vec3 r3) : r1_(r1), r2_(r2), r3_(r3)
{
    c1_.x_ = r1_.x_; c2_.x_ = r1_.y_; c3_.x_ = r1_.z_;
    c1_.y_ = r2_.x_; c2_.y_ = r2_.y_; c3_.y_ = r2_.z_;
    c1_.z_ = r3_.x_; c2_.z_ = r3_.y_; c3_.z_ = r3_.z_;
}
OnMat3::OnMat3(const OnMat3 &o) : r1_(o.r1_), r2_(o.r2_), r3_(o.r3_)
{
    c1_.x_ = r1_.x_; c2_.x_ = r1_.y_; c3_.x_ = r1_.z_;
    c1_.y_ = r2_.x_; c2_.y_ = r2_.y_; c3_.y_ = r2_.z_;
    c1_.z_ = r3_.x_; c2_.z_ = r3_.y_; c3_.z_ = r3_.z_;
}

double OnMat3::det() const
{
    return r1_.x_*r2_.y_*r3_.z_ + r1_.y_*r2_.z_*r3_.x_ + r1_.z_*r2_.x_*r3_.y_ - r1_.z_*r2_.y_*r3_.x_ - r1_.y_*r2_.x_*r3_.z_ - r1_.x_*r2_.z_*r3_.y_;
}

OnMat3 OnMat3::transpose() const
{
    return OnMat3(Vec3(r1_.x_, r2_.x_, r3_.x_), Vec3(r1_.y_, r2_.y_, r3_.y_), Vec3(r1_.z_, r2_.z_, r3_.z_));
}

OnMat3 OnMat3::operator*(OnMat3 o) const
{
    return OnMat3(  Vec3(r1_.dot(o.c1_), r1_.dot(o.c2_), r1_.dot(o.c3_)),
                    Vec3(r2_.dot(o.c1_), r2_.dot(o.c2_), r2_.dot(o.c3_)),
                    Vec3(r3_.dot(o.c1_), r3_.dot(o.c2_), r3_.dot(o.c3_)));
}

Vec3 OnMat3::operator*(Vec3 o)
{
    return Vec3(r1_.dot(o), r2_.dot(o), r3_.dot(o));
}

Mat4::Mat4()
{
    r1c1_ = 1.0;  r1c2_ = 0.0;  r1c3_ = 0.0;  r1c4_ = 0.0;
    r2c1_ = 0.0;  r2c2_ = 1.0;  r2c3_ = 0.0;  r2c4_ = 0.0;
    r3c1_ = 0.0;  r3c2_ = 0.0;  r3c3_ = 1.0;  r3c4_ = 0.0;
    r4c1_ = 0.0;  r4c2_ = 0.0;  r4c3_ = 0.0;  r4c4_ = 1.0;
}

Mat4::Mat4(OnMat3 rotation, Vec3 translation, double scaling)
{
    r1c1_ = scaling * rotation.r1_.x_;  r1c2_ = scaling * rotation.r1_.y_;  r1c3_ = scaling * rotation.r1_.z_;  r1c4_ = translation.x_;
    r2c1_ = scaling * rotation.r2_.x_;  r2c2_ = scaling * rotation.r2_.y_;  r2c3_ = scaling * rotation.r2_.z_;  r2c4_ = translation.y_;
    r3c1_ = scaling * rotation.r3_.x_;  r3c2_ = scaling * rotation.r3_.y_;  r3c3_ = scaling * rotation.r3_.z_;  r3c4_ = translation.z_;
    r4c1_ = 0.0;            r4c2_ = 0.0;            r4c3_ = 0.0;            r4c4_ = 1.0;
}

Vec3 Mat4::operator*(Vec3 o)
{
    return Vec3(
                r1c1_ * o.x_ + r1c2_* o.y_ + r1c3_* o.z_ + r1c4_,
                r2c1_ * o.x_ + r2c2_* o.y_ + r2c3_* o.z_ + r2c4_,
                r3c1_ * o.x_ + r3c2_* o.y_ + r3c3_* o.z_ + r3c4_
                );
}

void FindTransform::findTransform(Vec3 fromA, Vec3 fromB, Vec3 fromC, Vec3 toA, Vec3 toB, Vec3 toC)
{
    Vec3 a1 = toA;
    Vec3 b1 = toB;
    Vec3 c1 = toC;
    Vec3 a2 = fromA;
    Vec3 b2 = fromB;
    Vec3 c2 = fromC;

    Vec3 y1 = (a1 - c1).cross(b1 - c1).norm();
    Vec3 z1 = (a1 - c1).norm();
    Vec3 x1 = y1.cross(z1);

    Vec3 y2 = (a2 - c2).cross(b2 - c2).norm();
    Vec3 z2 = (a2 - c2).norm();
    Vec3 x2 = y2.cross(z2);
    OnMat3 mat1 = OnMat3(x1, y1, z1).transpose();
    OnMat3 mat2 = OnMat3(x2, y2, z2).transpose();

    OnMat3 rotation = mat1 * mat2.transpose();
    Vec3 translation = c1 - c2;

    double scale = (a1 - c1).length() / (a2 - c2).length();

    translation = rotation * c2 * (-scale) + c1;
    Mat4 transformation(rotation, translation, scale);
    transform_ = transformation;
}

double FindTransform::error(Vec3 fromA, Vec3 toA)
{
    return (transform_*fromA - toA).length();
}

struct GeorefBestTriplet
{
    size_t t_;          /**< First ordinate of the best triplet found. **/
    size_t s_;          /**< Second ordinate of the best triplet found. **/
    size_t p_;          /**< Third ordinate of the best triplet found. **/
    double err_;        /**< Error of this triplet. **/
};

class GeoRef
{
  public:
  std::vector<Vec3> local_ts;
  std::vector<Vec3> gps_t;
  GeoRef(std::vector<Vec3> local_ts,std::vector<Vec3> gps_t)
  {
    this->local_ts = local_ts;
    this->gps_t = gps_t;
  }

  void findBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2, size_t offset, size_t stride, double &minTotError);
  void chooseBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2);
};

void GeoRef::findBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2, size_t offset, size_t stride,double &minTotError)
{
    minTotError = std::numeric_limits<double>::infinity();

    for(size_t t = offset; t < local_ts.size(); t+=stride)
    {
        for(size_t s = t; s < local_ts.size(); ++s)
        {
            for(size_t p = s; p < local_ts.size(); ++p)
            {
                FindTransform trans;
                //easting_,northing_,altitude_
                trans.findTransform(local_ts[t], local_ts[s], local_ts[p],gps_t[t], gps_t[s], gps_t[p]);

                // The total error for the current camera triplet.
                double totError = 0.0;

                for(size_t r = 0; r < gps_t.size(); ++r)
                {
                    totError += trans.error(local_ts[r], gps_t[r]);
                }

                if(minTotError > totError)
                {
                    minTotError = totError;
                    cam0 = t;
                    cam1 = s;
                    cam2 = p;
                }
            }
        }
    }
}

void GeoRef::chooseBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2)
{
    size_t numThreads = boost::thread::hardware_concurrency();
    boost::thread_group threads;

    // To be changed
    std::vector<GeorefBestTriplet*> triplets;
    for(size_t t = 0; t < numThreads; ++t)
    {
        GeorefBestTriplet* triplet = new GeorefBestTriplet();
        triplets.push_back(triplet);
        threads.create_thread(boost::bind(&GeoRef::findBestCameraTriplet, this, boost::ref(triplet->t_), boost::ref(triplet->s_), boost::ref(triplet->p_), t, numThreads, boost::ref(triplet->err_)));
    }

    threads.join_all();

    double minTotError = std::numeric_limits<double>::infinity();
    for(size_t t = 0; t<numThreads; t++)
    {
        GeorefBestTriplet* triplet = triplets[t];
        if(minTotError > triplet->err_)
        {
            minTotError = triplet->err_;
            cam0 = triplet->t_;
            cam1 = triplet->s_;
            cam2 = triplet->p_;
        }
        delete triplet;
    }
}

template <typename Scalar>
void transformPointCloud(const char *inputFile, const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform, const char *outputFile){
    try{

        pcl::PointCloud<pcl::PointXYZRGBNormal> pointCloud;
        if(pcl::io::loadPLYFile<pcl::PointXYZRGBNormal> (inputFile, pointCloud) == -1) {
            std::cout << "Error when reading point cloud:\n" + std::string(inputFile) + "\n" <<std::endl;
            return;
        }
        else
        {
            std::cout << "Successfully loaded " << pointCloud.size() << " points with corresponding normals from file.\n"<<std::endl;
        }
        std::cout << "Writing transformed point cloud to " << outputFile << "...\n"<<std::endl;
        // We don't use PCL's built-in functions
        // because PCL does not support double coordinates
        // precision
        std::ofstream f (outputFile);
        f << "ply" << std::endl;
        if (IS_BIG_ENDIAN){
          f << "format binary_big_endian 1.0" << std::endl;
        }else{
          f << "format binary_little_endian 1.0" << std::endl;
        }
        const char *type = "double";
        if (sizeof(Scalar) == sizeof(float)){
            type = "float";
        }
        f   << "element vertex " << pointCloud.size() << std::endl
            << "property " << type << " x" << std::endl
            << "property " << type << " y" << std::endl
            << "property " << type << " z" << std::endl
            << "property " << type << " nx" << std::endl
            << "property " << type << " ny" << std::endl
            << "property " << type << " nz" << std::endl
            << "property uchar red" << std::endl
            << "property uchar green" << std::endl
            << "property uchar blue" << std::endl
            << "end_header" << std::endl;
        struct PlyPoint{
            Scalar x;
            Scalar y;
            Scalar z;
            Scalar nx;
            Scalar ny;
            Scalar nz;
            uint8_t r;
            uint8_t g;
            uint8_t b;
        } p;
        size_t psize = sizeof(Scalar) * 6 + sizeof(uint8_t) * 3;
        //For normal transformation
        Eigen::MatrixXd b(4,4);
        b << transform (0, 0),transform (0, 1),transform (0, 2), transform (0, 3),
        transform (1, 0),transform (1, 1),transform (1, 2), transform (1, 3),
        transform (2, 0),transform (2, 1),transform (2, 2), transform (2, 3),
        transform (3, 0),transform (3, 1),transform (3, 2), transform (3, 3);
        b = b.inverse();
        b = b.transpose();
        // Transform
        for (unsigned int i = 0; i < pointCloud.size(); i++){
            Scalar x = static_cast<Scalar>(pointCloud[i].x);
            Scalar y = static_cast<Scalar>(pointCloud[i].y);
            Scalar z = static_cast<Scalar>(pointCloud[i].z);
            Scalar tnx = static_cast<Scalar>(pointCloud[i].normal_x);
            Scalar tny = static_cast<Scalar>(pointCloud[i].normal_y);
            Scalar tnz = static_cast<Scalar>(pointCloud[i].normal_z);
            p.r = pointCloud[i].r;
            p.g = pointCloud[i].g;
            p.b = pointCloud[i].b;
            p.x = static_cast<Scalar> (transform (0, 0) * x + transform (0, 1) * y + transform (0, 2) * z + transform (0, 3));
            p.y = static_cast<Scalar> (transform (1, 0) * x + transform (1, 1) * y + transform (1, 2) * z + transform (1, 3));
            p.z = static_cast<Scalar> (transform (2, 0) * x + transform (2, 1) * y + transform (2, 2) * z + transform (2, 3));
            p.nx = static_cast<Scalar> (b (0, 0) * tnx + b (0, 1) * tny + b (0, 2) * tnz + b (0, 3));
            p.ny = static_cast<Scalar> (b (1, 0) * tnx + b (1, 1) * tny + b (1, 2) * tnz + b (1, 3));
            p.nz = static_cast<Scalar> (b (2, 0) * tnx + b (2, 1) * tny + b (2, 2) * tnz + b (2, 3));
            f.write(reinterpret_cast<char*>(&p), psize);
            // TODO: normals can be computed using the inverse transpose
            // https://paroj.github.io/gltut/Illumination/Tut09%20Normal%20Transformation.html
        }
        f.close();
        std::cout << "Point cloud file saved.\n" << std::endl;
    }
    catch (const std::exception & e)
    {
        std::cout << "Error while loading point cloud: " + std::string(e.what())<< std::endl;
        return;
    }
}

void convertGPS2UTM(const double &lon, const double &lat, const double &alt, double &x, double &y, double &z, int &utmZone, char &hemisphere)
{
  // Create WGS84 ecef coordinate system
  projPJ pjLatLon = pj_init_plus("+proj=latlong +datum=WGS84");
  if (!pjLatLon) {
    std::cout << ("Couldn't create WGS84 coordinate system with PROJ.4.") << std::endl;
    return;
  }

  // Calculate UTM zone if it's set to magic 99
  // NOTE: Special UTM cases in Norway/Svalbard not supported here
  if (utmZone == 99) {
    utmZone = ((static_cast<int>(floor((lon + 180.0)/6.0)) % 60) + 1);
    if (lat < 0)
      hemisphere = 'S';
    else
      hemisphere = 'N';
  }

  std::ostringstream ostr;
  ostr << utmZone;
  if (hemisphere == 'S')
    ostr << " +south";

  // Create UTM coordinate system
  projPJ pjUtm = pj_init_plus(("+proj=utm +datum=WGS84 +zone=" + ostr.str()).c_str());
  if (!pjUtm) {
    std::cout << ("Couldn't create UTM coordinate system with PROJ.4.") << std::endl;
    return;
  }

  // Convert to radians
  x = lon * 0.017453292;
  y = lat * 0.017453292;
  z = alt;

  // Transform
  int res = pj_transform(pjLatLon, pjUtm, 1, 1, &x, &y, &z);
  if (res != 0) {
    std::cout << "Failed to transform coordinates" << std::endl;
    return;
  }
}

// Simple example that reads and writes a reconstruction.
int main(int argc, char** argv) {
  InitializeGlog(argv);
  /********************************Load all the information**************************************/
  std::string input_path;
  std::string input_ply_path;
  std::string input_GNSS_path;
  std::string output_ply_path;
  bool use_utm = false;
  int zone = 31;
  char sphere = 'N';
  bool addOffset = false;
  OptionManager options;
  options.AddRequiredOption("input_path", &input_path);
  options.AddRequiredOption("input_ply_path", &input_ply_path);
  options.AddRequiredOption("input_GNSS_path", &input_GNSS_path);
  options.AddRequiredOption("output_ply_path", &output_ply_path);
  options.AddRequiredOption("use_utm",&use_utm);
  options.AddDefaultOption("add_offset",&addOffset);
  options.AddDefaultOption("utm_zone", &zone);
  options.AddDefaultOption("utm_hemisphere", &sphere);
  options.Parse(argc, argv);
  Reconstruction reconstruction;
  reconstruction.Read(input_path);
  //All vectors
  std::vector<std::string> image_names;
  std::vector<double> qws, qxs, qys, qzs;
  std::vector<Vec3> local_ts;
  //Get all images from model
  std::vector<image_t> reg_ids = reconstruction.RegImageIds();
  for (uint i = 0; i < reg_ids.size(); i++)
  {
    image_names.push_back(reconstruction.Image(reg_ids[i]).Name());
    qws.push_back(reconstruction.Image(reg_ids[i]).Qvec()[0]);
    qxs.push_back(reconstruction.Image(reg_ids[i]).Qvec()[1]);
    qys.push_back(reconstruction.Image(reg_ids[i]).Qvec()[2]);
    qzs.push_back(reconstruction.Image(reg_ids[i]).Qvec()[3]);
    double tx = reconstruction.Image(reg_ids[i]).Tvec()[0];
    double ty = reconstruction.Image(reg_ids[i]).Tvec()[1];
    double tz = reconstruction.Image(reg_ids[i]).Tvec()[2];
    Vec3 temp(tx,ty,tz);
    local_ts.push_back(temp);
  }
  //Load ECEF files
  std::vector<std::string> ecef_names;
  std::vector<Vec3> gps_t(reg_ids.size(),Vec3(0.0,0.0,0.0));
  std::ifstream myfile (input_GNSS_path);
  std::string stemp;
  uint counter = 0;
  if (myfile.is_open())
  {
    //Read line by line
    while (getline(myfile,stemp))
    {
      std::vector<std::string> strs;
      boost::split(strs,stemp,boost::is_any_of(" "));
      //Search Weather existed in reconstruction
      for(uint j = 0; j < image_names.size();j++)
      {
        if (strs[0] == image_names[j])
        {
          ecef_names.push_back(strs[0]);
          counter++;
          double gps_x = std::stod(strs[1]);
          double gps_y = std::stod(strs[2]);
          double gps_z = std::stod(strs[3]);
          Vec3 gps_temp(gps_x,gps_y,gps_z);
          gps_t[j]=gps_temp;
        }
      }
    }
    myfile.close();
  }
  else
  {
    std::cout << "GNSS File not found, Exiting......" <<std::endl;
    return 0;
  }
  if (counter == reg_ids.size())
  {
    std::cout << "ALL Images have corresponding GNSS! " << counter << " images in total!"<< std::endl;
  }
  else
  {
    std::cout << "Not All Images have corresponding GNSS!, Filtering out." << std::endl;
    //For each reconstruction image
    for (uint k=0;k<reg_ids.size();k++)
    {
      if ((gps_t[k].x_==0.0) && (gps_t[k].y_==0.0) && (gps_t[k].z_==0.0))
      {
        gps_t.erase(gps_t.begin()+k);
        local_ts.erase(local_ts.begin()+k);
      }
    }
    std::cout << "The vector size is :" << local_ts.size() << " vs " << gps_t.size() << std::endl;
  }

  /************************************PreProcessing-Centering****************************************/
  if (use_utm)
  {

    std::cout << "Using UTM, converting all GPS to UTM, using: zone: " << zone << " and hemisphere: " << sphere << std::endl;

    for (std::vector<Vec3>::iterator iter = gps_t.begin(); iter != gps_t.end(); ++iter)
    {
      const double lon = iter->x_;
      const double lat = iter->y_;
      const double alt = iter->z_;
      double x, y ,z;
      convertGPS2UTM(lon,lat,alt,x,y,z,zone,sphere);
      iter->x_ = x;
      iter->y_ = y;
      iter->z_ = z;
    }
  }
  double dx = 0.0, dy = 0.0,dz=0.0;
  double num = static_cast<double>(counter);
  for (std::vector<Vec3>::iterator iter = gps_t.begin(); iter != gps_t.end(); ++iter) {
    dx += iter->x_/num;
    dy += iter->y_/num;
    dz += iter->z_/num;
  }
  dx = floor(dx);
  dy = floor(dy);
  dz = floor(dz);

  std::ofstream geo_info_file;
  geo_info_file.open(output_ply_path+"_Geo_info.txt");
  std::string info_line;

  if (use_utm)
  {
    info_line = ("UTM: Zone: " + std::to_string(zone) + " Hemisphere: " + sphere + " Northing: " + std::to_string(dx) + " Easting: " + std::to_string(dy) + " Aviation: " + std::to_string(dz));
  }
  else
  {
    info_line = ("ECEF: Center_x: " + std::to_string(dx) + " Center_y: " + std::to_string(dy) + " Center_z: " + std::to_string(dz));
  }
  geo_info_file<<info_line;
  geo_info_file.close();

  std::cout << "Center point (Offset) at: " << dx << " " << dy<< " " << dz << std::endl;
  for (std::vector<Vec3>::iterator iter = gps_t.begin(); iter != gps_t.end(); ++iter) {
    iter->x_ -= dx;
    iter->y_ -= dy;
    iter->z_ -= dz;
  }

  /*************************************Choosing the best cameras************************************/
  size_t cam0,cam1,cam2;
  GeoRef georef(local_ts,gps_t);
  georef.chooseBestCameraTriplet(cam0,cam1,cam2);
  std::cout << "The best triplet is chosen as: " << cam0 << " " << cam1 << " " << cam2 << std::endl;
  FindTransform transFinal;
  transFinal.findTransform(local_ts[cam0], local_ts[cam1], local_ts[cam2],gps_t[cam0], gps_t[cam1], gps_t[cam2]);
  Mat4 transMat = transFinal.transform_;
  std::cout<< transFinal.transform_ << std::endl;
  Eigen::Transform<double, 3, Eigen::Affine> transform;
  transform(0, 0) = static_cast<double>(transMat.r1c1_);
  transform(1, 0) = static_cast<double>(transMat.r2c1_);
  transform(2, 0) = static_cast<double>(transMat.r3c1_);
  transform(3, 0) = static_cast<double>(transMat.r4c1_);
  transform(0, 1) = static_cast<double>(transMat.r1c2_);
  transform(1, 1) = static_cast<double>(transMat.r2c2_);
  transform(2, 1) = static_cast<double>(transMat.r3c2_);
  transform(3, 1) = static_cast<double>(transMat.r4c2_);
  transform(0, 2) = static_cast<double>(transMat.r1c3_);
  transform(1, 2) = static_cast<double>(transMat.r2c3_);
  transform(2, 2) = static_cast<double>(transMat.r3c3_);
  transform(3, 2) = static_cast<double>(transMat.r4c3_);
  transform(0, 3) = static_cast<double>(transMat.r1c4_);
  transform(1, 3) = static_cast<double>(transMat.r2c4_);
  transform(2, 3) = static_cast<double>(transMat.r3c4_);
  transform(3, 3) = static_cast<double>(transMat.r4c4_);

  if (addOffset){
    transform(0, 3) = transform(0, 3) + dx;
    transform(1, 3) = transform(1, 3) + dy;
    transform(2, 3) = transform(2, 3) + dz;
  }

  transformPointCloud(input_ply_path.c_str(),transform,output_ply_path.c_str());
}

max_stars_repo_name: alexmulford/colmap

max_issues_repo_name: alexmulford/colmap

max_forks_repo_name: alexmulford/colmap


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

#include <boost/thread/mutex.hpp>

#include <gazebo/gazebo.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/math/gzmath.hh>

#include <iostream>
#include <cstdlib>

# if GAZEBO_MAJOR_VERSION >= 6
#include <gazebo/gazebo_client.hh>
# endif

boost::mutex g_mutex;
boost::condition_variable g_condition;
gazebo::msgs::Request *g_requestMsg;
gazebo::msgs::Model g_modelMsg;

/////////////////////////////////////////////////
void OnResponse(ConstResponsePtr &_msg)
{
  boost::mutex::scoped_lock lock(g_mutex);
  if (!g_requestMsg || _msg->id() != g_requestMsg->id())
    return;

  if (_msg->has_type() && _msg->type() == g_modelMsg.GetTypeName())
  {
    g_modelMsg.ParseFromString(_msg->serialized_data());
    g_condition.notify_all();
  }
}

/////////////////////////////////////////////////
int main(int _argc, char **_argv)
{
  // Load gazebo
#if GAZEBO_MAJOR_VERSION > 2
# if GAZEBO_MAJOR_VERSION >= 6
  gazebo::client::setup(_argc, _argv);
# else
  gazebo::setupClient(_argc, _argv);
# endif
#else
  gazebo::load(_argc, _argv);
#endif

  std::string name = _argv[1];
  int id   = 0;
  double x = atof(_argv[2]);
  double y = atof(_argv[3]);
  double z = atof(_argv[4]);

  // Create our node for communication
  gazebo::transport::NodePtr node(new gazebo::transport::Node());
  node->Init();

  // Start transport
  gazebo::transport::run();

  // Create request for entity_info
  gazebo::transport::PublisherPtr requestPub =
    node->Advertise<gazebo::msgs::Request>("~/request");
  g_requestMsg = gazebo::msgs::CreateRequest("entity_info", name);

  {
    // Subscribe to response
    gazebo::transport::SubscriberPtr responseSub =
      node->Subscribe("~/response", &OnResponse);

    // Publish request and wait for response
    boost::mutex::scoped_lock lock(g_mutex);
    requestPub->Publish(*g_requestMsg);
    g_condition.wait(lock);
  }

  // Publish to a Gazebo topic
  gazebo::transport::PublisherPtr modelPub =
    node->Advertise<gazebo::msgs::Model>("~/model/modify");

  gazebo::msgs::Model modelMsg;
  modelMsg.set_name(name);
  modelMsg.set_id(g_modelMsg.id());
  gazebo::math::Pose pose(x, y, z, 0, 0, 0);
# if GAZEBO_MAJOR_VERSION >= 7
  gazebo::msgs::Set(modelMsg.mutable_pose(), pose.Ign());
# else
  gazebo::msgs::Set(modelMsg.mutable_pose(), pose);
# endif

  // Wait for a subscriber to connect
  modelPub->WaitForConnection();

  gazebo::common::Time::MSleep(100);
  modelPub->Publish(modelMsg);

  // Make sure to shut everything down.
  gazebo::transport::fini();
}

ext: cpp


text:
/*

CandyPoker
https://github.com/sweeterthancandy/CandyPoker

MIT License

Copyright (c) 2019 Gerry Candy

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

*/
#include <iostream>
#include <thread>
#include <atomic>
#include <numeric>
#include <bitset>
#include <cstdint>
#include <future>
#include <boost/format.hpp>
#include <boost/timer/timer.hpp>
#include "ps/base/cards.h"
#include "ps/detail/print.h"
#include "ps/base/board_combination_iterator.h"
#include "ps/eval/class_equity_evaluator.h"
#include "ps/eval/equity_breakdown_matrix.h"
#include "ps/eval/evaluator.h"
#include "ps/eval/evaluator_7_card_map.h"
#include "ps/eval/evaluator_5_card_map.h"
#include "ps/eval/equity_evaluator.h"
#include "ps/sim/holdem_class_strategy.h"
#include "ps/support/index_sequence.h"
#include "ps/support/config.h"

#include "ps/base/rank_hasher.h"
#include "ps/base/suit_hasher.h"
#include "ps/base/holdem_board_decl.h"
#include "ps/detail/dispatch.h"

#include <boost/range/algorithm.hpp>

using namespace ps;

namespace working{

struct evaluator_7_card_map : evaluator
{
        evaluator_7_card_map(){
                impl_ = &evaluator_factory::get("6_card_map");
                card_map_7_.resize(rhasher_.max());

                for(size_t i=0;i!=52;++i){
                        card_rank_device_[i] = card_decl::get(i).rank().id();
                }

                using iter_t = basic_index_iterator<
                        int, ordered_policy, rank_vector
                >;

                for(iter_t iter(7,13),end;iter!=end;++iter){
                        maybe_add_(*iter);
                }
        }
        ranking_t rank(long a, long b, long c, long d, long e)const override{
                return impl_->rank(a,b,c,d,e);
        }
        ranking_t rank(long a, long b, long c, long d, long e, long f)const override{
                return impl_->rank(a,b,c,d,e,f);
        }
        ranking_t rank(long a, long b, long c, long d, long e, long f, long g)const override{

                auto shash =  shasher_.create_from_cards(a,b,c,d,e,f,g);

                if( shasher_.has_flush(shash)){
                        //++miss;
                        return impl_->rank(a,b,c,d,e,f,g);
                }

                auto rhash = rhasher_.create_from_cards(a,b,c,d,e,f,g);
                auto ret = card_map_7_[rhash];

                return ret;
        }
        mutable std::atomic_int miss{0};
        mutable std::atomic_int hit{0};
        ranking_t rank(card_vector const& cv, size_t suit_hash, size_t rank_hash, long a, long b)const {

                if( shasher_.has_flush(suit_hash) ){
                        ++miss;
                        return impl_->rank(a,b,cv[0], cv[1], cv[2], cv[3], cv[4]);
                }
                ++hit;
                auto ret = card_map_7_[rank_hash];
                return ret;
        }
private:
        ranking_t rank_from_rank_impl_(long a, long b, long c, long d, long e, long f, long g)const{
                return impl_->rank( card_decl::make_id(0,a),
                                    card_decl::make_id(0,b),
                                    card_decl::make_id(0,c),
                                    card_decl::make_id(0,d),
                                    card_decl::make_id(1,e),
                                    card_decl::make_id(1,f),
                                    card_decl::make_id(1,g) );
        }
        ranking_t rank_from_rank_(long a, long b, long c, long d, long e, long f, long g)const{
                return this->rank( card_decl::make_id(0,a),
                                   card_decl::make_id(0,b),
                                   card_decl::make_id(0,c),
                                   card_decl::make_id(0,d),
                                   card_decl::make_id(1,e),
                                   card_decl::make_id(1,f),
                                   card_decl::make_id(1,g) );
        }
        void maybe_add_(rank_vector const& b){
                // first check we don't have more than 4 of each card
                std::array<int, 13> aux = {0};
                for(size_t i=0;i!=7;++i){
                        ++aux[b[i]];
                }
                for(size_t i=0;i!=aux.size();++i){
                        if( aux[i] > 4 )
                                return;
                }
                auto hash = rhasher_.create( b[0], b[1], b[2], b[3], b[4], b[5], b[6] );

                auto val  = rank_from_rank_impl_( b[0], b[1], b[2], b[3], b[4], b[5], b[6] );

                //std::cout << detail::to_string(aux) << " - " << detail::to_string(b) << " => " << std::bitset<30>(static_cast<unsigned long long>(hash)).to_string() << "\n";
                //

                card_map_7_[hash] = val;
        }
        rank_hasher rhasher_;
        suit_hasher shasher_;
        evaluator* impl_;
        std::array<size_t, 52> card_rank_device_;
        std::vector<ranking_t> card_map_7_;
};

template<class Impl_Type>
struct equity_evaulator_principal_tpl : public ps::equity_evaluator{
        std::shared_ptr<equity_breakdown> evaluate(std::vector<holdem_id> const& players)const override{
                // we first need to enumerate every run of the board,
                // for this we can create a mapping [0,51-n*2] -> [0,51],
                auto result = std::make_shared<equity_breakdown_matrix>(players.size());

                // vector of first and second card
                std::vector<card_id> x,y;
                std::vector<card_id> known;

                for( auto const& p : players){
                        x.push_back(holdem_hand_decl::get(p).first().id());
                        y.push_back(holdem_hand_decl::get(p).second().id());
                }
                auto n = players.size();

                boost::copy( x, std::back_inserter(known));
                boost::copy( y, std::back_inserter(known));

                size_t board_count = 0;
                for(board_combination_iterator iter(5, known),end;iter!=end;++iter){
                        ++board_count;

                        auto const& b(*iter);

                        std::vector<ranking_t> ranked;
                        for( size_t i=0;i!=n;++i){
                                ranked.push_back(impl_->rank(x[i], y[i],
                                                            b[0], b[1], b[2], b[3], b[4]) );
                        }
                        detail::dispatch_ranked_vector{}(*result, ranked);
                }
                PRINT(board_count);

                return result;
        }
protected:
        Impl_Type* impl_;
};

struct equity_evaulator_principal
        : equity_evaulator_principal_tpl<evaluator_7_card_map>
{
        equity_evaulator_principal()
        {
                impl_ = new evaluator_7_card_map;
        }
};
} // working

/*
        Idea here is that I'm not concerned about
        creating an object representing 5 cards,
        so that we create another object after adding
        2 more cards, ie,

 */
struct card_chain{};

int main(){
        #if 0
        for(board_combination_iterator iter(5),end;iter!=end;++iter){
                std::cout << *iter << "\n";
        }
        #endif
        working::equity_evaulator_principal ec;
        working::evaluator_7_card_map ev;
        holdem_class_vector cv;
        holdem_board_decl w;
        rank_hasher rh;
        suit_hasher sh;
        #if 0
        for(int i=0;i!=52;++i){
                auto const& card{ card_decl::get(i) };
                std::cout << card
                        << " - " << (int)card.suit() << " - " << (int)(i & 0x3 )
                        << " / " << (int)card.rank() << " - " << (int)(i >> 2 )
                        << "\n";
        }
        return 0;
        #endif
        #if 1
        cv.push_back("AA");
        cv.push_back("KK");
        #endif
        #if 0
        cv.push_back("AKs");
        cv.push_back("QJs");
        cv.push_back("T9s");
        cv.push_back("87s");
        #endif

        boost::timer::auto_cpu_timer at;
        auto result = std::make_shared<equity_breakdown_matrix_aggregator>(cv.size());
        for( auto hvt : cv.to_standard_form_hands()){
                auto const& perm = std::get<0>(hvt);
                auto const& hv   = std::get<1>(hvt);
                PRINT( hv );
                auto hv_mask = hv.mask();
                #if 1

                // put this here
                std::vector<ranking_t> ranked(hv.size());

                // cache stuff

                std::vector<card_id> hv_first(hv.size());
                std::vector<card_id> hv_second(hv.size());
                std::vector<rank_id> hv_first_rank(hv.size());
                std::vector<rank_id> hv_second_rank(hv.size());
                std::vector<suit_id> hv_first_suit(hv.size());
                std::vector<suit_id> hv_second_suit(hv.size());

                for(size_t i=0;i!=hv.size();++i){
                        auto const& hand{holdem_hand_decl::get(hv[i])};

                        hv_first[i]       = hand.first().id();
                        hv_first_rank[i]  = hand.first().rank().id();
                        hv_first_suit[i]  = hand.first().suit().id();
                        hv_second[i]      = hand.second().id();
                        hv_second_rank[i] = hand.second().rank().id();
                        hv_second_suit[i] = hand.second().suit().id();
                }

                auto sub = std::make_shared<equity_breakdown_matrix_aggregator>(cv.size());
                size_t board_count = 0;
                for(auto const& b : w ){

                        bool cond = (b.mask() & hv_mask ) == 0;
                        if(!cond){
                                continue;
                        }
                        ++board_count;
                        auto rank_proto = b.rank_hash();
                        auto suit_proto = b.suit_hash();

                        for(size_t i=0;i!=hv.size();++i){

                                auto rank_hash = rank_proto;
                                auto suit_hash = suit_proto;

                                rank_hash = rh.append(rank_hash, hv_first_rank[i]);
                                rank_hash = rh.append(rank_hash, hv_second_rank[i]);

                                suit_hash = sh.append(suit_hash, hv_first_suit[i] );
                                suit_hash = sh.append(suit_hash, hv_second_suit[i] );

                                ranked[i] = ev.rank(b.board(),
                                                    suit_hash, rank_hash,
                                                    hv_first[i],
                                                    hv_second[i]);
                        }
                        detail::dispatch_ranked_vector{}(*sub, ranked);

                }
                PRINT(board_count);
                result->append_matrix(*sub, perm );
                #else
                PRINT(detail::to_string(perm));
                PRINT(hv);
                auto ret = ec.evaluate(hv);
                result->append_matrix(*ret, perm);
                #endif
        }
        std::cout << *result << "\n";
        //auto r = static_cast<double>(ev.hit)/(ev.miss+ev.hit);
        //PRINT_SEQ((ev.hit)(ev.miss)(r));
}


text:
#ifndef PERIOR_TREE_QUERY_H
#define PERIOR_TREE_QUERY_H
#include <periortree/point_traits.hpp>
#include <periortree/boundary_conditions.hpp>
#include <periortree/rectangle.hpp>
#include <periortree/intersects.hpp>
#include <boost/static_assert.hpp>

namespace perior
{
namespace query
{

template<typename pointT>
struct query_intersects_box
{
    BOOST_STATIC_ASSERT(::perior::traits::is_point<pointT>::value);

    explicit query_intersects_box(const rectangle<pointT>& rct): rect(rct){}

    template<typename boundaryT>
    BOOST_FORCEINLINE
    bool match(const pointT& p, boundaryT const& b) const BOOST_NOEXCEPT_OR_NOTHROW
    {
        return ::perior::intersects(p, rect, b);
    }

    template<typename boundaryT>
    BOOST_FORCEINLINE
    bool match(const rectangle<pointT>& r, boundaryT const& b)
        const BOOST_NOEXCEPT_OR_NOTHROW
    {
        return ::perior::intersects(r, rect, b);
    }

    // for objects
    template<typename T>
    BOOST_FORCEINLINE
    bool match(T const& r) const BOOST_NOEXCEPT_OR_NOTHROW
    {
        return true;
    }

    BOOST_FORCEINLINE
    rectangle<pointT> const& box() const BOOST_NOEXCEPT_OR_NOTHROW
    {
        return rect;
    }

    rectangle<pointT> rect;
};

template<typename pointT>
struct query_within_box
{
    BOOST_STATIC_ASSERT(::perior::traits::is_point<pointT>::value);

    explicit query_within_box(const rectangle<pointT>& rct): rect(rct){}

    template<typename boundaryT>
    BOOST_FORCEINLINE
    bool match(const pointT& p, boundaryT const& b) const BOOST_NOEXCEPT_OR_NOTHROW
    {
        return ::perior::within(p, rect, b);
    }

    template<typename boundaryT>
    BOOST_FORCEINLINE
    bool match(const rectangle<pointT>& r, boundaryT const& b)
        const BOOST_NOEXCEPT_OR_NOTHROW
    {
        return ::perior::within(r, rect, b);
    }

    template<typename T>
    BOOST_FORCEINLINE
    bool match(T const& r) const BOOST_NOEXCEPT_OR_NOTHROW
    {
        return true;
    }

    BOOST_FORCEINLINE
    rectangle<pointT> const& box() const BOOST_NOEXCEPT_OR_NOTHROW
    {
        return rect;
    }

    rectangle<pointT> rect;
};

template<typename pointT>
inline typename boost::enable_if<
    ::perior::traits::is_point<pointT>, query_intersects_box<pointT> >::type
intersects_box(rectangle<pointT> const& rect)
{
    return query_intersects_box<pointT>(rect);
}

template<typename pointT>
inline typename boost::enable_if<
    ::perior::traits::is_point<pointT>, query_intersects_box<pointT> >::type
within_box(rectangle<pointT> const& rect)
{
    return query_within_box<pointT>(rect);
}

} // query
} // perior
#endif//PERIOR_TREE_QUERY_H

ext: hpp

max_stars_repo_name: ToruNiina/periortree

max_issues_repo_name: ToruNiina/periortree

max_forks_repo_name: ToruNiina/periortree


text:
/**
 * $Id$
 *
 * Copyright (C)
 * 2013 - $Date$
 *     Martin Wolf <boostnumpy@martin-wolf.org>
 *
 * \file    boost/numpy/detail/pygil.hpp
 * \version $Revision$
 * \date    $Date$
 * \author  Martin Wolf <boostnumpy@martin-wolf.org>
 *
 * \brief This file defines the PyGIL class for holding the python global
 *        interpreter lock. Thanks to Steve Jackson for the code inspiration.
 *
 *        This file is distributed under the Boost Software License,
 *        Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
 *        http://www.boost.org/LICENSE_1_0.txt).
 */
#ifndef BOOST_NUMPY_DETAIL_PYGIL_HPP_INCLUDED
#define BOOST_NUMPY_DETAIL_PYGIL_HPP_INCLUDED

#include <boost/python.hpp>

namespace boost {
namespace numpy {
namespace detail {

class PyGIL
{
  public:
    PyGIL(bool auto_acquire=true)
    {
        if(auto_acquire)
        {
            acquire();
        }
    }

    ~PyGIL()
    {
        if(is_acquired())
        {
            release();
        }
    }

    void
    acquire()
    {
        state_ = PyGILState_Ensure();
        acquired_ = true;
    }

    void
    release()
    {
        PyGILState_Release(state_);
        acquired_ = false;
    }

    bool
    is_acquired() const
    {
        return acquired_;
    }

  private:
    bool acquired_;
    PyGILState_STATE state_;
};

}// namespace detail
}// namespace numpy
}// namespace boost

#endif // !BOOST_NUMPY_DETAIL_PYGIL_HPP_INCLUDED

max_stars_repo_name: IceCube-SPNO/BoostNumpy

max_issues_repo_name: IceCube-SPNO/BoostNumpy

max_forks_repo_name: IceCube-SPNO/BoostNumpy


text:
#include <boost/test/included/unit_test.hpp>
#include <string>

#include "testInputBinder.hpp"
#include "testMySql.hpp"
#include "testOutputBinder.hpp"

// Boost lets you name your tests, but I just want my tests to have the same
// name as the function, so use this macro to fill them in
#define STR_EXPAND(tok) #tok
#define STR(tok) STR_EXPAND(tok)
#define FD(function) FunctionDescription(function, STR(function))

namespace test = boost::unit_test;
using std::string;

test::test_suite* init_unit_test_suite(int, char*[]) {
    typedef void(*testFunction)(void);
    class FunctionDescription {
        public:
            FunctionDescription(testFunction function, const char* name)
                : function_(function)
                , name_(name)
            {  // NOLINT[whitespaces/braces]
            }
            testFunction function_;
            const string name_;
    };

    FunctionDescription functions[] = {
        // Tests from testInputBinder.hpp
        FD(testBind),
        // Tests from testOutputBinder.hpp
        FD(testSetResult),
        FD(testSetParameter),
        // Tests from testMySql.hpp
        FD(testConnection),
        FD(testRunCommand),
        FD(testRunQuery),
        FD(testInvalidCommands),
        FD(testPreparedStatement)
    };

    for (const auto& functionDescription : functions) {
        test::framework::master_test_suite().add(
            test::make_test_case(
                test::callback0<>(functionDescription.function_),
                test::literal_string(
                    functionDescription.name_.c_str(),
                    functionDescription.name_.length())));
    }

    return 0;
}

max_stars_repo_name: Xu-Jianxin/mysql-cpp

max_issues_repo_name: Xu-Jianxin/mysql-cpp

max_forks_repo_name: Xu-Jianxin/mysql-cpp


text:
// #include <graphlab.hpp>
#include <graphlab/graph/graph_vertex_join.hpp>
#include <boost/functional/hash.hpp>
#include "pagerank.hpp"
#include "cgs_lda.hpp"

size_t NTOPICS = 20;
int JOIN_INTERVAL = 5;
bool JOIN_ON_ID = true;
int RUN_PAGERANK = 1;
int RUN_LDA = 2;

graphlab::distributed_control* dc_ptr;
graphlab::thread_group thgroup;
graphlab::omni_engine<pagerank::compute_transit_prob>* transit_prob_engine;
graphlab::omni_engine<pagerank::compute_pagerank>* pagerank_engine;
graphlab::omni_engine<lda::cgs_lda_vertex_program>* lda_engine;
pagerank::graph_type* pagerank_graph;
lda::graph_type* lda_graph;
graphlab::graph_vertex_join<pagerank::graph_type, lda::graph_type>* vjoinptr;

/*  Global thread func for running pagerank */
void fn_run_pagerank () {
  static graphlab::mutex m;
  if (m.try_lock()) {
    dc_ptr->cout() << "Running pagerank" << std::endl;
    pagerank_engine->signal_all();
    pagerank_engine->start();
    dc_ptr->cout() << "Pagerank finished" << std::endl;
    m.unlock();
  }
  // const float runtime = engine.elapsed_seconds();
  // dc.cout() << "Finished Running engine in " << runtime << " seconds." << std::endl;
}

/* Global thread func for running lda */
void fn_run_lda() {
  static graphlab::mutex m;
  if (m.try_lock()) {
    dc_ptr->cout() << "Running The Collapsed Gibbs Sampler" << std::endl;
    lda_engine->map_reduce_vertices<graphlab::empty>(lda::signal_only::docs);
    // Enable sampling
    lda::cgs_lda_vertex_program::DISABLE_SAMPLING = false;
    // Run the engine
    lda_engine->start();
    // Finalize the counts
    // lda::cgs_lda_vertex_program::DISABLE_SAMPLING = true;
    // lda_engine->signal_all();
    // lda_engine->start();
    m.unlock();
  }
}

void vertex_join_op (pagerank::graph_type::vertex_type& lvertex,
                     const lda::graph_type::vertex_data_type& rvertex_data) {
  // lvertex.data().topics.clear();
  float sum = 0;
  for (size_t i = 0; i < rvertex_data.factor.size(); i++) sum += (rvertex_data.factor[i] >= 0 ? rvertex_data.factor[i] : 0);
  for (size_t i = 0; i < rvertex_data.factor.size(); i++) {
    // lvertex.data().topics.push_back(rvertex_data.factor[i]);
    lvertex.data().topics[i] = float(rvertex_data.factor[i] >= 0 ? rvertex_data.factor[i] : 0) / sum;
  }
}
void fn_compute_transit_prob () {
  static graphlab::mutex m;
  if (m.try_lock()) {
    dc_ptr->cout() << "Compute transition prob" << std::endl;
    transit_prob_engine->signal_all();
    transit_prob_engine->start();
    m.unlock();
  }
}
void fn_join_vertex() {
  while(true) {
    dc_ptr->cout() << "Join vertex data" << std::endl;
    vjoinptr->left_injective_join(vertex_join_op);
    fn_compute_transit_prob();
    graphlab::timer::sleep(JOIN_INTERVAL);
  }
}

void launch_metric_server(int algorithm) {
  graphlab::launch_metric_server();

  // Start the pagerank webserver
  if (algorithm & RUN_PAGERANK)
    graphlab::add_metric_server_callback("pagerank", pagerank::pagerank_callback);

  graphlab::add_metric_server_callback("click", pagerank::weight_update_callback);
  // Start the lda webserver
  if (algorithm & RUN_LDA) {
    graphlab::add_metric_server_callback("ldaparam", lda::set_param_callback);
    graphlab::add_metric_server_callback("lockword", lda::lock_word_callback);
    graphlab::add_metric_server_callback("wordclouds", lda::word_cloud_callback);
    graphlab::add_metric_server_callback("addtopic", lda::add_topic_callback);
}
}

void load_pagerankgraph(const std::string& edge_dir, const std::string& vertex_dir) {
  pagerank::load_and_initialize_graph(*dc_ptr, *pagerank_graph, edge_dir, vertex_dir);
}

void load_ldagraph(const std::string& edge_dir,
                   const std::string& vertex_dir,
                   const std::string& dictionary_dir) {
  lda::load_and_initialize_graph(*dc_ptr, *lda_graph, edge_dir, vertex_dir);
  lda::load_dictionary(dictionary_dir);
}

void init_personal_weight(const std::string& file) {
    // initialized the personalized weights;
    if (!file.empty()) {
      std::ifstream in(file.c_str(), std::ios_base::in);
      float sum = 0.0;
      float w;
      while(in >> w) {
        pagerank::w_personal.push_back(w);
        sum += w;
      }
      in.close();
      ASSERT_EQ(pagerank::w_personal.size(), NTOPICS);
      for (size_t i = 0; i < NTOPICS; ++i)
        pagerank::w_personal[i] /= sum;
    } else {
      pagerank::w_personal.resize(NTOPICS, 1.0/(float)NTOPICS);
    }
}

int main(int argc, char** argv) {
  // Initialize control plain using mpi
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;
  dc_ptr = &dc;
  global_logger().set_log_level(LOG_INFO);

  // Parse command line options -----------------------------------------------
  graphlab::command_line_options clopts("Twitter Rank.");
  std::string pagerank_edges;
  std::string pagerank_vertices;
  std::string lda_edges;
  std::string lda_vertices;
  std::string lda_dictionary;
  std::string w_personal;
  bool USE_SYNC = false;
  std::string execution_type = "synchronous";
    // The dir of the link graph
  clopts.attach_option("pagerank_edges", pagerank_edges, "The pagerank graph (edges). Required ");
  clopts.attach_option("pagerank_vertices", pagerank_vertices, "The pagerank graph (vertices). Required ");
  clopts.attach_option("lda_edges", lda_edges, "The lda graph (edges). Required ");
  clopts.attach_option("lda_vertices", lda_vertices, "The lda graph (vertices). Required ");
  clopts.attach_option("lda_dictionary", lda_dictionary, "The lda word dictionary. Required ");
  clopts.attach_option("w_personal", w_personal, "The intial input of personalized weights.");
  clopts.attach_option("wordid_offset", lda::WORDID_OFFSET, "The starting id of the words in the lda input.");
  clopts.attach_option("execution", execution_type, "Execution type (synchronous or asynchronous)");
  clopts.attach_option("ntopics", NTOPICS, "Number of topics to use");
  clopts.attach_option("topkpr", pagerank::TOPK, "Top k pages(users) for display");
  clopts.attach_option("topklda", lda::TOPK, "Top k words in each topic for display");
  clopts.attach_option("default_ndocs", pagerank::DEFAULT_NDOCS, "Top k pages(users) to display");
  clopts.attach_option("default_topicval", pagerank::DEFAULT_TOPICVAL, "Top k pages(users) to display");
  clopts.attach_option("force_lock", lda::FORCE_LOCK, "force locked words");
  clopts.attach_option("use_sync", USE_SYNC, "Use Synchronous LDA");
  clopts.attach_option("join_on_id", JOIN_ON_ID, "If true, use the vertex id as join key. Otherwise, use vertex data as join key, so the vertex input for both graph must be provided.");
  clopts.attach_option("has_doc_count", pagerank::HAS_DOC_COUNT, "Whether or not the pagerank vertex data has a field for document count. This could be true for author-topic graph, depending on the input data.");

  bool pagerank_only = false;
  bool lda_only = false;
  clopts.attach_option("pagerank_only", pagerank_only, "Run pagerank only.");
  clopts.attach_option("lda_only", lda_only, "Run lda only.");

  if(!clopts.parse(argc, argv)) {
    dc.cout() << "Error in parsing command line arguments." << std::endl;
    return EXIT_FAILURE;
  }

  if (pagerank_only && lda_only) {
    dc.cout () << "pagerank_only and lda_only are mutually exclusive";
    return EXIT_FAILURE;
  }

  int algorithm = RUN_PAGERANK | RUN_LDA;
  if (pagerank_only) {
    dc.cout () << " Running pagerank only. " << std::endl;
    algorithm &= RUN_PAGERANK;
  } else if (lda_only) {
    dc.cout () << " Running LDA only. " << std::endl;
    algorithm &= RUN_LDA;
  } else {
    dc.cout () << " Running Joint Pagerank and LDA. " << std::endl;
  }

  if (!lda_only && pagerank_edges == "") {
    dc.cout() << "Pagerank Graph not specified. Cannot continue";
    return EXIT_FAILURE;
  }
  if (!pagerank_only && lda_edges == "") {
    dc.cout() << "LDA Graph not specified. Cannot continue";
    return EXIT_FAILURE;
  }

  if ((algorithm & RUN_PAGERANK) && (algorithm & RUN_LDA) && !JOIN_ON_ID && (pagerank_vertices == "" || lda_vertices == "")) {
    dc.cout() << "JOIN_ON_ID is false, vertex input for both graph must be provided. Please provide pagerank_vertices and lda_vertices.";
    return EXIT_FAILURE;
      }
  if ((algorithm & RUN_LDA) && lda_dictionary == "") {
    dc.cout() << "LDA Dictionary not specified. Cannot continue";
    return EXIT_FAILURE;
  }
  pagerank::NTOPICS = NTOPICS;
  lda::INITIAL_NTOPICS = NTOPICS;
  lda::NTOPICS = NTOPICS;
  pagerank::JOIN_ON_ID = JOIN_ON_ID;
  lda::JOIN_ON_ID = JOIN_ON_ID;

  // Build the pagerank (left) graph ----------------------------------------------------------
  // The global pagerank_graph points to lgraph
  pagerank::graph_type lgraph(dc);
  pagerank_graph = &lgraph;
  if (algorithm & RUN_PAGERANK) {
    load_pagerankgraph(pagerank_edges, pagerank_vertices);
  }

  // Build the lda (right) graph
  // The global lda_graph points to rgraph
  lda::graph_type rgraph(dc);
  lda_graph = &rgraph;
  if (algorithm & RUN_LDA) {
    load_ldagraph(lda_edges, lda_vertices, lda_dictionary);
  }

  // create the graph join object
  // prepare the join
  // assign the global pointer to the join object
  graphlab::graph_vertex_join<pagerank::graph_type, lda::graph_type> vjoin(dc, lgraph, rgraph);
  if ((algorithm & RUN_PAGERANK) && (algorithm & RUN_LDA)) {
    vjoin.prepare_injective_join(pagerank::left_emit_key, lda::right_emit_key);
    vjoinptr = &vjoin;
  }

  // Run pagerank ------------------------------------------------------------
  // Set up engine0 to compute transition probability
  graphlab::omni_engine<pagerank::compute_transit_prob> engine0(dc, lgraph, execution_type);
  if (algorithm  & RUN_PAGERANK)  {
    init_personal_weight(w_personal);
    dc.barrier();
    transit_prob_engine = &engine0;
    fn_compute_transit_prob();
  }

  graphlab::graphlab_options opts1;
  opts1.set_ncpus(3);
  // Set up engine1 to compute pagerank
  graphlab::omni_engine<pagerank::compute_pagerank> engine1(dc, lgraph, execution_type, opts1);
  if (algorithm & RUN_PAGERANK) {
    bool success = engine1.add_vertex_aggregator<pagerank::topk_aggregator>(
        "toppr", pagerank::topk_aggregator::map, pagerank::topk_aggregator::finalize) && engine1.aggregate_periodic("toppr", 1);
    ASSERT_TRUE(success);
    pagerank_engine = &engine1;
    thgroup.launch(fn_run_pagerank);
  }

  // Run lda -----------------------------------------------------------------
  graphlab::graphlab_options opts2;
  opts2.set_ncpus(8);
  std::string engine_type = "sync";
  if (!USE_SYNC) {
    opts2.get_engine_args().set_option("factorized",true);
    engine_type = "async";
  }
  //opts2.get_engine_args().set_option("handler_intercept",true);
  graphlab::omni_engine<lda::cgs_lda_vertex_program> engine2(dc, rgraph, engine_type, opts2);
  if (algorithm & RUN_LDA) {
    lda_engine = &engine2;
    lda::initialize_global();
    {
      const bool success =
        engine2.add_vertex_aggregator<lda::topk_aggregator>
        ("topk", lda::topk_aggregator::map, lda::topk_aggregator::finalize) &&
        engine2.aggregate_periodic("topk", 1);
      ASSERT_TRUE(success);
    }

    { // Add the Global counts aggregator
      const bool success =
        engine2.add_vertex_aggregator<lda::factor_type>
        ("global_counts",
         lda::global_counts_aggregator::map,
         lda::global_counts_aggregator::finalize) &&
        engine2.aggregate_periodic("global_counts", 1);
      ASSERT_TRUE(success);
    }

    /*{ // Add the likelihood aggregator
      const bool success =
        engine2.add_vertex_aggregator<lda::likelihood_aggregator>
        ("likelihood",
         lda::likelihood_aggregator::map,
         lda::likelihood_aggregator::finalize) &&
        engine2.aggregate_periodic("likelihood", 10);
      ASSERT_TRUE(success);
    }*/

    thgroup.launch(fn_run_lda);
  }

  // Run background vertex join
  if ((algorithm & RUN_PAGERANK) && (algorithm & RUN_LDA)) {
    thgroup.launch(fn_join_vertex);
  }

  thgroup.launch(boost::bind(launch_metric_server, algorithm));
  // --------------------------------------
  // ------------FINALIZE------------------
  // --------------------------------------
  graphlab::stop_metric_server_on_eof();
  thgroup.join();
  graphlab::mpi_tools::finalize();

  return EXIT_SUCCESS;
}

max_issues_repo_name: keerthanashanmugam/graphlabapi

max_forks_repo_name: keerthanashanmugam/graphlabapi


text:
<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#include <cmath>

#include <Eigen/LU>

#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"
#include "Tudat/Basics/utilities.h"

namespace tudat
{

namespace linear_algebra
{

//! Function to put a quaternion in 'vector format', e.g. a Vector4d with entries (w,x,y,z) of the quaternion
Eigen::Vector4d convertQuaternionToVectorFormat( const Eigen::Quaterniond& quaternion )
{
    Eigen::Vector4d vector;
    vector( 0 ) = quaternion.w( );
    vector( 1 ) = quaternion.x( );
    vector( 2 ) = quaternion.y( );
    vector( 3 ) = quaternion.z( );
    return vector;
}

//! Function to put a vector in 'quaternion format', i.e. a Quaterniond.
Eigen::Quaterniond convertVectorToQuaternionFormat( const Eigen::Vector4d& vector )
{
    Eigen::Quaterniond quaternion;
    quaternion.w( ) = vector( 0 );
    quaternion.x( ) = vector( 1 );
    quaternion.y( ) = vector( 2 );
    quaternion.z( ) = vector( 3 );

    return quaternion;
}

//! Function to take the product of two quaternions.
Eigen::Vector4d quaternionProduct( const Eigen::Vector4d& firstQuaternion, const Eigen::Vector4d& secondQuaternion )
{
    Eigen::Vector4d resultantQuaternion;
    resultantQuaternion[ 0 ] = firstQuaternion[ 0 ] * secondQuaternion[ 0 ] -
            firstQuaternion.segment( 1, 3 ).dot( secondQuaternion.segment( 1, 3 ) );
    resultantQuaternion.segment( 1, 3 ) = firstQuaternion[ 0 ] * secondQuaternion.segment( 1, 3 ) +
            secondQuaternion[ 0 ] * firstQuaternion.segment( 1, 3 ) +
            getCrossProductMatrix( firstQuaternion.segment( 1, 3 ) ) * secondQuaternion.segment( 1, 3 );
    return resultantQuaternion;
}

//! Function to invert a quaternion.
void invertQuaternion( Eigen::Vector4d& quaternionVector )
{
    quaternionVector.segment( 1, 3 ) *= -1.0;
}

//! Function that returns that 'cross-product matrix'
Eigen::Matrix3d getCrossProductMatrix( const Eigen::Vector3d& vector )
{
    Eigen::Matrix3d crossProductMatrix = Eigen::Matrix3d::Zero( );
    crossProductMatrix( 1, 0 ) = vector.z( );
    crossProductMatrix( 0, 1 ) = -vector.z( );
    crossProductMatrix( 2, 0 ) = -vector.y( );
    crossProductMatrix( 0, 2 ) = vector.y( );
    crossProductMatrix( 2, 1 ) = vector.x( );
    crossProductMatrix( 1, 2 ) = -vector.x( );
    return crossProductMatrix;
}

//! Compute cosine of the angle between two vectors.
double computeCosineOfAngleBetweenVectors( const Eigen::VectorXd& vector0,
                                           const Eigen::VectorXd& vector1 )
{
    if( !( vector0.size( ) == vector1.size( ) ) )
    {
        throw std::runtime_error( "Error when computing angle between vectors; size is incompatible" );
    }

    // Get the cosine of the angle by dotting the normalized vectors.
    double dotProductOfNormalizedVectors = vector0.normalized( ).dot( vector1.normalized( ) );

    // Explicitly define the extreme cases, which can give problems with the acos function.
    if ( dotProductOfNormalizedVectors >= 1.0 )
    {
        return 1.0;
    }

    else if ( dotProductOfNormalizedVectors <= -1.0 )
    {
        return -1.0;
    }
    // Determine the actual angle.
    else
    {
        return dotProductOfNormalizedVectors;
    }
}

//! Compute angle between two vectors.
double computeAngleBetweenVectors( const Eigen::VectorXd& vector0, const Eigen::VectorXd& vector1 )
{
    // Determine the cosine of the angle by using another routine.
    double dotProductOfNormalizedVectors = computeCosineOfAngleBetweenVectors( vector0, vector1 );

    // Return arccosine of the above, which is effectively the angle.
    return std::acos( dotProductOfNormalizedVectors );
}

//! Computes norm of the the difference between two 3d vectors.
double computeNormOfVectorDifference( const Eigen::Vector3d& vector0,
                                      const Eigen::Vector3d& vector1 )
{
    return ( vector0 - vector1 ).norm( );
}

//! Computes the norm of a 3d vector
double getVectorNorm( const Eigen::Vector3d& vector )
{
    return vector.norm( );
}

Eigen::Vector3d evaluateSecondBlockInStateVector(
        const std::function< Eigen::Vector6d( const double ) > stateFunction,
        const double time )
{
    return stateFunction( time ).segment( 3, 3 );
}

//! Computes the norm of a 3d vector from a vector-returning function.
double getVectorNormFromFunction( const std::function< Eigen::Vector3d( ) > vectorFunction )
{
    return getVectorNorm( vectorFunction( ) );
}

//! Function to calculate the jacobian of a normalized vector, from the partial of the unnormalized vector.
Eigen::Matrix3d calculatePartialOfNormalizedVector( const Eigen::Matrix3d& partialOfUnnormalizedVector,
                                                    const Eigen::Vector3d& unnormalizedVector )
{
    double normOfVector = unnormalizedVector.norm( );

    return ( Eigen::Matrix3d::Identity( ) / normOfVector - unnormalizedVector * unnormalizedVector.transpose( ) /
             ( normOfVector * normOfVector * normOfVector ) ) * partialOfUnnormalizedVector;
}

//! Function to compute the root mean square value of the entries in an Eigen vector
double getVectorEntryRootMeanSquare( const Eigen::VectorXd& inputVector )
{
    // Calculate RMS for vector
    double vectorRms = 0.0;
    for( int i = 0; i < inputVector.rows( ); i++ )
    {
        vectorRms += inputVector( i ) * inputVector( i );
    }
    vectorRms = std::sqrt( vectorRms / inputVector.rows( ) );

    return vectorRms;
}

//! Function to compute the partial derivative of a rotation matrix w.r.t. its associated quaterion elements
void computePartialDerivativeOfRotationMatrixWrtQuaternion(
        const Eigen::Vector4d quaternionVector,
        std::vector< Eigen::Matrix3d >& partialDerivatives )
{
    partialDerivatives[ 0 ]<< quaternionVector( 0 ), -quaternionVector( 3 ), quaternionVector( 2 ),
            quaternionVector( 3 ), quaternionVector( 0 ), -quaternionVector( 1 ),
            -quaternionVector( 2 ), quaternionVector( 1 ), quaternionVector( 0 );
     partialDerivatives[ 0 ] *= 2.0;

    partialDerivatives[ 1 ]<< quaternionVector( 1 ), quaternionVector( 2 ), quaternionVector( 3 ),
            quaternionVector( 2 ), -quaternionVector( 1 ), -quaternionVector( 0 ),
            quaternionVector( 3 ), quaternionVector( 0 ), -quaternionVector( 1 );
    partialDerivatives[ 1 ] *= 2.0;

    partialDerivatives[ 2 ]<< -quaternionVector( 2 ), quaternionVector( 1 ), quaternionVector( 0 ),
            quaternionVector( 1 ), quaternionVector( 2 ), quaternionVector( 3 ),
            -quaternionVector( 0 ), quaternionVector( 3 ), -quaternionVector( 2 );
    partialDerivatives[ 2 ] *= 2.0;

    partialDerivatives[ 3 ]<< -quaternionVector( 3 ), -quaternionVector( 0 ), quaternionVector( 1 ),
            quaternionVector( 0 ), -quaternionVector( 3 ), quaternionVector( 2 ),
            quaternionVector( 1 ), quaternionVector( 2 ), quaternionVector( 3 );
    partialDerivatives[ 3 ] *= 2.0;

}

} // namespace linear_algebra

} // namespace tudat

max_stars_repo_name: ViktorJordanov/tudat

max_issues_repo_name: ViktorJordanov/tudat

max_forks_repo_name: ViktorJordanov/tudat


text:
#include "libmotioncapture/nokov.h"

#include <string>
#include <thread>
#include <mutex>
#include <unordered_map>
#include <Eigen/Geometry>
//#include <ros/ros.h>
#include "SeekerSDKCAPI.h"
// NOKOV
#include "SeekerSDKClient.h"

namespace libmotioncapture {

    typedef struct
    {
        int ID;                                 // RigidBody identifier
        float x, y, z;                          // Position
        float qx, qy, qz, qw;                   // Orientation
    } sBodyData;

    typedef struct
    {
        int iFrame;                                     // host defined frame number
        int nOtherMarkers;                              // # of undefined markers
        MarkerData OtherMarkers[MAX_MARKERS];           // undefined marker data
        int nRigidBodies;                               // # of rigid bodies
        sBodyData RigidBodies[MAX_RIGIDBODIES];    // Rigid body data
        float fLatency;                                 // host defined time delta between capture and send
        unsigned int Timecode;                          // SMPTE timecode (if available)
        unsigned int TimecodeSubframe;                  // timecode sub-frame data
        long long iTimeStamp;                           // FrameGroup timestamp
        short params;                                   // host defined parameters

    } sFrameOfObjData;

    // Global Var
    sFrameOfObjData frameObjData = {0};
    std::mutex mtx;

    const sFrameOfObjData& GetCurrentFrame()
    {
        static sFrameOfObjData frame = {0};
        {
            std::lock_guard<std::mutex> lck (mtx);
            frame = frameObjData;
        }
        return frame;
    }

    void DataHandler(sFrameOfMocapData* pFrameOfData, void* pUserData)
    {
        if (nullptr == pFrameOfData)
            return;

        // Store the frame
        std::lock_guard<std::mutex> lck (mtx);

        int nmaker = (pFrameOfData->nOtherMarkers < MAX_MARKERS)?pFrameOfData->nOtherMarkers:MAX_MARKERS;

        frameObjData.iFrame = pFrameOfData->iFrame;
        frameObjData.nOtherMarkers = nmaker;
        frameObjData.nRigidBodies = pFrameOfData->nRigidBodies;
        frameObjData.fLatency = pFrameOfData->fLatency;
        frameObjData.Timecode = pFrameOfData->Timecode;
        frameObjData.TimecodeSubframe = pFrameOfData->TimecodeSubframe;
        frameObjData.iTimeStamp = pFrameOfData->iTimeStamp;
        frameObjData.params = pFrameOfData->params;

        for(int i = 0; i< nmaker; ++i)
        {
            frameObjData.OtherMarkers[i][0] = pFrameOfData->OtherMarkers[i][0] * 0.001;
            frameObjData.OtherMarkers[i][1] = pFrameOfData->OtherMarkers[i][1] * 0.001;
            frameObjData.OtherMarkers[i][2] = pFrameOfData->OtherMarkers[i][2] * 0.001;
        }

        for(int i = 0; i< pFrameOfData->nRigidBodies; ++i)
        {
            frameObjData.RigidBodies[i].ID =  pFrameOfData->RigidBodies[i].ID;
            frameObjData.RigidBodies[i].x =  pFrameOfData->RigidBodies[i].x * 0.001;
            frameObjData.RigidBodies[i].y =  pFrameOfData->RigidBodies[i].y * 0.001;
            frameObjData.RigidBodies[i].z =  pFrameOfData->RigidBodies[i].z * 0.001;
            frameObjData.RigidBodies[i].qx =  pFrameOfData->RigidBodies[i].qx;
            frameObjData.RigidBodies[i].qy =  pFrameOfData->RigidBodies[i].qy;
            frameObjData.RigidBodies[i].qz =  pFrameOfData->RigidBodies[i].qz;
            frameObjData.RigidBodies[i].qw =  pFrameOfData->RigidBodies[i].qw;
        }
    }

    class MotionCaptureNokovImpl
    {
    public:
        std::string version = "0.0.0.0";
        int updateFrequency = 100;
        bool enableFixedUpdate = false;
        sDataDescriptions* pBodyDefs = nullptr;
        SeekerSDKClient* pClient = nullptr;
        int lastFrame = 0;
        std::unordered_map<std::string, size_t> bodyMap;

        size_t GetBodyIdByName(const std::string& name) const {
            if (bodyMap.find(name) != bodyMap.end())
            {
                return bodyMap.at(name);
            }

            return -1;
        }

        const std::string GetBodyNameById(size_t id) const {
            for (auto pair : bodyMap)
            {
                if (pair.second == id)
                {
                    return pair.first;
                }
            }

            return std::string();
        }

        ~MotionCaptureNokovImpl()
        {
            if (nullptr != pClient)
            {
                pClient->Uninitialize();
                delete pClient;
                pClient = nullptr;
            }

            if (nullptr != pBodyDefs)
            {
                XingYing_FreeDescriptions(pBodyDefs);
                pBodyDefs= nullptr;
            }
        }
    };

    MotionCaptureNokov::MotionCaptureNokov(
        const std::string& hostname,
        bool enableFrequency,
        int updateFrequency)
    {
        pImpl = new MotionCaptureNokovImpl;

        SeekerSDKClient* theClient = new SeekerSDKClient();
        unsigned char version[4] = {0};
        theClient->SeekerSDKVersion(version);
        {
            std::stringstream sstr;
            sstr << (int)version[0] << "." << (int)version[1] << "." << (int)version[2] << "." << (int)version[3];
            pImpl->version = sstr.str();
        }

        theClient->SetDataCallback(DataHandler);

        // Check the ret value
        int retValue = theClient->Initialize((char*)hostname.c_str());
        if (ErrorCode_OK != retValue)
        {
            std::stringstream sstr;
            sstr << "Error connecting XINGYING on address: " << hostname << " Code:" << retValue;
            throw std::runtime_error(sstr.str());
        }

        if (ErrorCode_OK != theClient->GetDataDescriptions(&pImpl->pBodyDefs) || nullptr == pImpl->pBodyDefs)
        {
			std::stringstream sstr;
			sstr << "Error Request BodyDefs on address: " << hostname << " Code:" << retValue;
			throw std::runtime_error(sstr.str());
        }

        for (int iDataDef = 0; iDataDef < pImpl->pBodyDefs->nDataDescriptions; ++iDataDef)
        {
            if (pImpl->pBodyDefs->arrDataDescriptions[iDataDef].type == Descriptor_RigidBody)
            {
                auto bodeDef = pImpl->pBodyDefs->arrDataDescriptions[iDataDef].Data.RigidBodyDescription;
                pImpl->bodyMap[bodeDef->szName] = bodeDef->ID;
            }
        }

        pImpl->pClient = theClient;
        pImpl->enableFixedUpdate = enableFrequency;
        pImpl->updateFrequency = updateFrequency;
    }

    MotionCaptureNokov::~MotionCaptureNokov()
    {
        if (pImpl)
        {
            delete pImpl;
        }
    }

    const std::string& MotionCaptureNokov::version() const
    {
        return pImpl->version;
    }

    void MotionCaptureNokov::waitForNextFrame()
    {
        static auto lastTime = std::chrono::high_resolution_clock::now();
        auto now = std::chrono::high_resolution_clock::now();

        if (pImpl->enableFixedUpdate)
        {
            auto elapsed = now - lastTime;
            auto desiredPeriod = std::chrono::milliseconds(1000 / pImpl->updateFrequency);
            //std::cout <<"elapsed: " << std::chrono::duration<double>(elapsed).count() << "\tdesired:" << std::chrono::duration<double>(desiredPeriod).count() << std::endl;
            if (elapsed < desiredPeriod) {
                //std::cout << "Sleep Done" << std::endl;
                std::this_thread::sleep_for(desiredPeriod - elapsed);
            }
        }

        int frameNo;
        do
        {
            frameNo = GetCurrentFrame().iFrame;
        }
        while (frameNo == pImpl->lastFrame);
        pImpl->lastFrame = frameNo;
        lastTime = std::chrono::high_resolution_clock::now();
    }

	bool MotionCaptureNokov::supportsPointCloud() const
	{
		return true;
	}

	const std::map<std::string, libmotioncapture::RigidBody>& MotionCaptureNokov::rigidBodies() const
	{
        rigidBodies_.clear();

		auto frameData = GetCurrentFrame();
		for (int iBody = 0; iBody < frameData.nRigidBodies; ++iBody) {

			const auto& rb = frameData.RigidBodies[iBody];
			Eigen::Vector3f position(
				rb.x,
				rb.y,
				rb.z);

			// Convention
			Eigen::Quaternionf rotation(rb.qw, rb.qx, rb.qy, rb.qz);
            auto bodyName = pImpl->GetBodyNameById(rb.ID);

            rigidBodies_.emplace(bodyName, RigidBody(bodyName,position, rotation));
		}

        return rigidBodies_;
	}

	libmotioncapture::RigidBody MotionCaptureNokov::rigidBodyByName(const std::string& name) const
	{
        size_t bodyId = pImpl->GetBodyIdByName(name);
        if (bodyId < 0)
        {
            throw std::runtime_error("Unknown rigid body!");
        }

		auto frameData = GetCurrentFrame();
		for (size_t iBody = 0; iBody < frameData.nRigidBodies; ++iBody) {

			const auto& rb = frameData.RigidBodies[iBody];

			if (bodyId == rb.ID)
			{
				Eigen::Vector3f position(
					rb.x,
					rb.y,
					rb.z);

		        Eigen::Quaternionf rotation(rb.qw, rb.qz, rb.qy, rb.qx);

                return RigidBody(pImpl->GetBodyNameById(bodyId), position, rotation);
			}
		}

        throw std::runtime_error("Unknown rigid body!");
	}

	const libmotioncapture::PointCloud& MotionCaptureNokov::pointCloud() const
	{
		auto frameData = GetCurrentFrame();
        size_t count = frameData.nOtherMarkers;
		pointcloud_.resize(count, Eigen::NoChange);
		for (size_t iMarkerIdx = 0; iMarkerIdx < count; ++iMarkerIdx) {

			pointcloud_.row(iMarkerIdx) << frameData.OtherMarkers[iMarkerIdx][0],
                frameData.OtherMarkers[iMarkerIdx][1],
                frameData.OtherMarkers[iMarkerIdx][2];
		}
		return pointcloud_;
	}

	bool MotionCaptureNokov::supportsRigidBodyTracking() const
	{
        return true;
	}

}

max_stars_repo_name: knmcguire/libmotioncapture

max_issues_repo_name: NOKOV-MOCAP/libmotioncapture

max_forks_repo_name: NOKOV-MOCAP/libmotioncapture