text:
#ifndef bempp_type_combinations_hpp
#define bempp_type_combinations_hpp

#include "bempp/common/config_data_types.hpp"

#include <boost/mpl/list.hpp>
#include <complex>

template <typename BasisFunctionType_, typename KernelType_, typename ResultType_>
struct BasisKernelResultTypeTraits
{
    typedef BasisFunctionType_ BasisFunctionType;
    typedef KernelType_ KernelType;
    typedef ResultType_ ResultType;
};

#if defined(ENABLE_COMPLEX_BASIS_FUNCTIONS)
#  if defined(ENABLE_COMPLEX_KERNELS)

typedef boost::mpl::list<
#    if defined(ENABLE_SINGLE_PRECISION)
BasisKernelResultTypeTraits<float, float, float>,
BasisKernelResultTypeTraits<float, float, std::complex<float> >,
BasisKernelResultTypeTraits<float, std::complex<float>, std::complex<float> >,
BasisKernelResultTypeTraits<std::complex<float>, float, std::complex<float> >,
BasisKernelResultTypeTraits<std::complex<float>, std::complex<float>, std::complex<float> >
#    endif
#    if defined(ENABLE_SINGLE_PRECISION) && defined(ENABLE_DOUBLE_PRECISION)
,
#    endif
#    if defined(ENABLE_DOUBLE_PRECISION)
BasisKernelResultTypeTraits<double, double, double>,
BasisKernelResultTypeTraits<double, double, std::complex<double> >,
BasisKernelResultTypeTraits<double, std::complex<double>, std::complex<double> >,
BasisKernelResultTypeTraits<std::complex<double>, double, std::complex<double> >,
BasisKernelResultTypeTraits<std::complex<double>, std::complex<double>, std::complex<double> >
#    endif
>
basis_kernel_result_combinations;

#  else // !defined(ENABLE_COMPLEX_KERNELS)

typedef boost::mpl::list<
#    if defined(ENABLE_SINGLE_PRECISION)
BasisKernelResultTypeTraits<float, float, float>,
BasisKernelResultTypeTraits<float, float, std::complex<float> >,
BasisKernelResultTypeTraits<std::complex<float>, float, std::complex<float> >
#    endif
#    if defined(ENABLE_SINGLE_PRECISION) && defined(ENABLE_DOUBLE_PRECISION)
,
#    endif
#    if defined(ENABLE_DOUBLE_PRECISION)
BasisKernelResultTypeTraits<double, double, double>,
BasisKernelResultTypeTraits<double, double, std::complex<double> >,
BasisKernelResultTypeTraits<std::complex<double>, double, std::complex<double> >
#    endif
>
basis_kernel_result_combinations;

#  endif
#else // !defined(ENABLE_COMPLEX_BASIS_FUNCTIONS)
#  if defined(ENABLE_COMPLEX_KERNELS)

typedef boost::mpl::list<
#    if defined(ENABLE_SINGLE_PRECISION)
BasisKernelResultTypeTraits<float, float, float>,
BasisKernelResultTypeTraits<float, float, std::complex<float> >,
BasisKernelResultTypeTraits<float, std::complex<float>, std::complex<float> >
#    endif
#    if defined(ENABLE_SINGLE_PRECISION) && defined(ENABLE_DOUBLE_PRECISION)
,
#    endif
#    if defined(ENABLE_DOUBLE_PRECISION)
BasisKernelResultTypeTraits<double, double, double>,
BasisKernelResultTypeTraits<double, double, std::complex<double> >,
BasisKernelResultTypeTraits<double, std::complex<double>, std::complex<double> >
#    endif
>
basis_kernel_result_combinations;

#  else // !defined(ENABLE_COMPLEX_KERNELS)

typedef boost::mpl::list<
#    if defined(ENABLE_SINGLE_PRECISION)
BasisKernelResultTypeTraits<float, float, float>
#    endif
#    if defined(ENABLE_SINGLE_PRECISION) && defined(ENABLE_DOUBLE_PRECISION)
,
#    endif
#    if defined(ENABLE_DOUBLE_PRECISION)
BasisKernelResultTypeTraits<double, double, double>
#    endif
>
basis_kernel_result_combinations;

#  endif
#endif

template <typename BasisFunctionType_, typename ResultType_>
struct BasisResultTypeTraits
{
    typedef BasisFunctionType_ BasisFunctionType;
    typedef ResultType_ ResultType;
};

#if defined(ENABLE_COMPLEX_BASIS_FUNCTIONS)

typedef boost::mpl::list<
#  if defined(ENABLE_SINGLE_PRECISION)
BasisResultTypeTraits<float, float>,
BasisResultTypeTraits<float, std::complex<float> >,
BasisResultTypeTraits<std::complex<float>, std::complex<float> >
#  endif
#  if defined(ENABLE_SINGLE_PRECISION) && defined(ENABLE_DOUBLE_PRECISION)
,
#  endif
#  if defined(ENABLE_DOUBLE_PRECISION)
BasisResultTypeTraits<double, double>,
BasisResultTypeTraits<double, std::complex<double> >,
BasisResultTypeTraits<std::complex<double>, std::complex<double> >
#  endif
>
basis_result_combinations;

#else // !defined(ENABLE_COMPLEX_BASIS_FUNCTIONS)
#  if defined(ENABLE_COMPLEX_KERNELS)

typedef boost::mpl::list<
#    if defined(ENABLE_SINGLE_PRECISION)
BasisResultTypeTraits<float, float>,
BasisResultTypeTraits<float, std::complex<float> >
#    endif
#    if defined(ENABLE_SINGLE_PRECISION) && defined(ENABLE_DOUBLE_PRECISION)
,
#    endif
#    if defined(ENABLE_DOUBLE_PRECISION)
BasisResultTypeTraits<double, double>,
BasisResultTypeTraits<double, std::complex<double> >
#    endif
>
basis_result_combinations;

#  else // !defined(ENABLE_COMPLEX_KERNELS)

typedef boost::mpl::list<
#    if defined(ENABLE_SINGLE_PRECISION)
BasisResultTypeTraits<float, float>
#    endif
#    if defined(ENABLE_SINGLE_PRECISION) && defined(ENABLE_DOUBLE_PRECISION)
,
#    endif
#    if defined(ENABLE_DOUBLE_PRECISION)
BasisResultTypeTraits<double, double>
#    endif
>
basis_result_combinations;

#  endif
#endif

#endif

max_stars_repo_name: mdavezac/bempp

max_issues_repo_name: mdavezac/bempp

max_forks_repo_name: mdavezac/bempp


text:
#include <armadillo>

using namespace arma;

int main()
{
    vec A(100000000);
    wall_clock clock;

    clock.tic();

    for (unsigned int i = 0; i < A.size(); ++i) {
        A(i) = log(i) * log(i);
    }

    double tiempo1 = clock.toc();

    vec B(100000000);

    clock.tic();

#pragma omp parallel for
    for (unsigned int i = 0; i < A.size(); ++i) {
        B(i) = log(i) * log(i);
    }

    double tiempo2 = clock.toc();

    cout << "Tiempo bucle simple: " << tiempo1 << " segundos\n"
         << "Tiempo bucle paralelizado: " << tiempo2 << " segundos" << endl;

    return 0;
}

ext: cpp


text:
/*
 * ColorKeeper.cpp
 *
 *  Created on: 5 fevr. 2009
 *      Author: mfe
 */

#include "gui/GuiLoader.h"
#include "core/ColorKeeperModel.h"

#include <boost/shared_ptr.hpp>
#include <cstdlib>
#include <exception>
#include <stdlib.h>
#include <signal.h>
#include <unistd.h>
#include <iostream>
#include <sstream>
#include <locale>
#include <QFile>
#include <QMessageBox>
#ifdef __WIN32__

#ifndef HAVE_DECL_SYS_SIGLIST
const char *const sys_siglist[] = {
	NULL, /* 0 */
	"SIGHUP", /* 1 */
	"SIGINT", /* 2 */
	"SIGQUIT", /* 3 */
	"SIGILL", /* 4 */
	"SIGTRAP", /* 5 */
	"SIGABRT", /* 6 */
	"SIGEMT", /* 7 */
	"SIGFPE", /* 8 */
	"SIGKILL", /* 9 */
	"SIGUSR1", /* 10 */
	"SIGSEGV", /* 11 */
	"SIGUSR2", /* 12 */
	"SIGPIPE", /* 13 */
	"SIGALRM", /* 14 */
	"SIGTERM", /* 15 */
	"SIGSTKFLT", /* 16 */
	"SIGCHLD", /* 17 */
	"SIGCONT", /* 18 */
	"SIGSTOP", /* 19 */
	"SIGTSTP", /* 20 */
	"SIGTTIN", /* 21 */
	"SIGTTOU", /* 22 */
};
#else
extern const char *const sys_siglist[];
#endif

#ifndef HAVE_STRSIGNAL
const char *
strsignal(int s)
{
	static char buf[100];
	if (s > 0 && s < (int)sizeof(sys_siglist) / (int)sizeof(sys_siglist[0]))
	return sys_siglist[s];
	sprintf(buf, ("Unknown signal %d"), s);
	return buf;
}
#endif

#endif

using namespace std;

typedef boost::shared_ptr<GuiLoader> ptrGUILoader;

bool alreadyDone = false;

void getOut() {
	if (!alreadyDone) {
		ColorKeeperModel::deleteLockFile();
		alreadyDone = true;
	}
}
void handleAtExit() {
	ColorKeeperModel::logMessage("atexit");
	getOut();
}
void handleSetTerminate() {
	ColorKeeperModel::logMessage("set_terminate");
	getOut();
}
void handleSetUnexpected() {
	ColorKeeperModel::logMessage("set_unexpected");
	getOut();
}

void getOut(int signum) {
	ostringstream os;
	os << "Receive signal : " << strsignal(signum) << endl;
	ColorKeeperModel::logMessage(os.str());
	getOut();
}

void getOutAndQuit(int signum) {
	getOut(signum);
	exit(signum);
}

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

	atexit(handleAtExit);
	std::set_terminate(handleSetTerminate);
	std::set_unexpected(handleSetUnexpected);

#ifdef __linux__
	signal(SIGTSTP, getOut);
	signal(SIGHUP, getOutAndQuit); //deconnexion
	signal(SIGCHLD, getOut);
	signal(SIGQUIT, getOutAndQuit);
#endif
	signal(SIGTERM, getOut);
	signal(SIGABRT, getOut);
	signal(SIGINT, getOutAndQuit);//ctrl-c
	signal(SIGSEGV, getOutAndQuit); // seg fault
	//signal(SIGKILL, getOut);//can't be catched
	setlocale(LC_ALL, "en.US_UTF-8");
	ColorKeeperModel::logMessage(":: ColorKeeper ::");
	ptrGUILoader guiLoader(new GuiLoader());
	guiLoader->openGUI(argc, argv);

}

max_stars_repo_name: mikrosimage/OpenDisplayCalib

max_issues_repo_name: mikrosimage/OpenDisplayCalib

max_forks_repo_name: mikrosimage/OpenDisplayCalib


text:
/**
 *  \file   PriorityScheduler.cpp
 *  \date   Aug 23, 2013
 *  \author "James C. Sutherland"
 *
 *
 * The MIT License
 *
 * Copyright (c) 2013-2017 The University of Utah
 *
 * 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 <expression/PriorityScheduler.h>

//-- SpatialOps
#include <spatialops/structured/MemoryTypes.h>
#include <spatialops/structured/ExternalAllocators.h>

//-- Boost includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/transpose_graph.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/bind.hpp>

#include <sstream>
#include <iostream>
#include <string>
#include <stdexcept>

namespace Expr{

  /**
   * \brief Boost visitor structure for setting node priorities
   */
  struct ExecPriorityVisitor: public boost::default_bfs_visitor {

    ExecPriorityVisitor(){}

    inline void examine_edge( const Edge e, const Graph& g ){
      const Vertex src = boost::source(e,g);
      const Vertex dest = boost::target(e,g);
      const int srcPriority = g[src].priority;
      Graph& g2 = const_cast<Graph&>(g);
      int& priority = g2[dest].priority;
      priority = std::max(priority, srcPriority + 1);
    }
  };

  //=================================================================

  PriorityScheduler::PriorityScheduler( Graph& depGraph,
                                        Graph& execGraph,
                                        const bool logNodeTimings,
                                        const std::string name )
  : Scheduler(execGraph,depGraph,logNodeTimings,name)
  {}

  void PriorityScheduler::call( VertexProperty& target )
  {
    FieldManagerList* const fml = extract_field_manager_list( this->fmls_, target.fmlid );

    try{
      const std::string& exprName = target.expr->get_tags()[0].name();
      target.expr->base_bind_fields(*fml);
      target.execute_expression();
      if( doNodeTimings_ && !target.expr->is_placeholder() ){
        // Reset the timings held on the scheduler for this expression. We cannot
        // directly time execution here since there are recursive calls occurring.
        timer_->add_entry( exprName, target.cumExecTime );
      }
      this->run_pollers( target, fml );
#     ifdef ENABLE_THREADS
      if( pool_.active() == 1 ) block_pollers();
#     endif
    }
    catch( std::exception& err ){
      std::ostringstream msg;
      msg << std::endl << "Error trapped while executing expression: "
          << target.expr->get_tags()[0] << std::endl
          << "details follow..." << std::endl
          << err.what() << std::endl;
      throw std::runtime_error( msg.str() );
    }
  }

  //-------------------------------------------------------------------

  void PriorityScheduler::exec_callback_handler( void* expr_vertex )
  {
    const Vertex v = (Vertex) expr_vertex;
    VertexProperty& vpJustFinished = execGraph_[v];

    this->release_fields_as_allowed( vpJustFinished );

    // Notify the vertex that an ancestor has finished, it returns true if it is ready.
    // If it is, we either toss it to the thread pool or run it.
    std::vector<VertexProperty*>::iterator vpend = vpJustFinished.consumerList.end();
    std::vector<VertexProperty*>::iterator vpit  = vpJustFinished.consumerList.begin();
    for( ; vpit!=vpend; ++vpit ){
      VertexProperty& vp = **vpit;
      if( vp.ancestor_finished() ){
#       ifdef ENABLE_THREADS
        this->pool_.schedule( boost::threadpool::prio_task_func( vp.priority, boost::bind( &PriorityScheduler::call, this, vp ) ) );
#       else
        this->call(vp);
#       endif
      }
    }

    dec_remaining();
  }

  //-------------------------------------------------------------------

  void PriorityScheduler::dec_remaining()
  {
#   ifdef ENABLE_THREADS
    ExecMutex<200> lock;
#   endif
    --nremaining_;
#   ifdef ENABLE_THREADS
    if( nremaining_ == 0 ) this->schedBarrier_.post();
#   endif
  }

  //-------------------------------------------------------------------

  void PriorityScheduler::setup( const bool hasRegisteredFields )
  {
   /*
    * If we have been invalidated
    *  - reset all callback handles in the graph
    *  - recalculate all nparent_ counts for each vertex
    *
    * Notes on whats going on here
    *  - execGraph is the execution graph
    *  - depGraph is the consumer (dependency) graph
    *
    *  Both are used to build up information required for determining node priority,
    *  node consumers, and node execution requirements.
    *
    *  Step 1: reset and reconnect all variables to place the graph into state which is execute ready.
    *
    *  Step 2: inspect the dependency graph in order to determine each nodes execution priority and
    *    determine its consumer count. ( The number of nodes that consume an expression and its fields ).
    *
    *  Step 3: inspect the execution graph to determine the number of parent nodes for each expression;
    *    during execution this will allow us to know when an expression is ready to run.
    *
    *  Step 4: determine each node's memory constraints, currently this is limited to deciding if the
    *    expression can use dynamic memory.
    */

    if( !invalid_ ) return; // Quick return if we're already valid

    rootList_.clear();

    ID2VP execVertexMap;  // jcs this duplicates the exprVertexMapT_ container on the ExpressionTree

    // Update element counts
    nelements_ = boost::num_vertices(execGraph_);
    nremaining_ = nelements_;

    const std::pair<VertIter, VertIter> execGraphVertices = boost::vertices(execGraph_);
    const std::pair<VertIter, VertIter> depGraphVertices  = boost::vertices(depGraph_ );

    // ------------------------------

    copy_vertex_properties( depGraph_, execGraph_ );

    /* jcs
       need to see if we can make this more efficient.
        - Can we leave callback signals in place?
        - Do we need to reset execution counts?
     */

    /* Step 1
     * Reconnect all signals and reset execution counts
     */
    for( VertIter viter = execGraphVertices.first; viter != execGraphVertices.second; ++viter ){
      const Vertex& vert = *viter;
      execVertexMap.insert( std::make_pair(execGraph_[vert].id, vert) );

      VertexProperty& vp = execGraph_[vert];
      vp.self       = (void*) (vert);
      vp.nparents   = 0;
      vp.nconsumers = 0;
      vp.priority   = 0;
      vp.execSignalCallback.reset( new VertexProperty::Signal() );
      vp.execSignalCallback->connect( boost::bind(&PriorityScheduler::exec_callback_handler, this, vp.self) );
      vp.ancestorList.clear();
      vp.consumerList.clear();
      vp.set_is_edge(false);
      if( vp.poller ) vp.poller->set_vertex_property(&vp);
    }

    // set node information - must be done in a separate loop because the
    // ancestorList and consumerList must be cleared before we push back on them.
    for( VertIter viter = execGraphVertices.first; viter != execGraphVertices.second; ++viter ){
      VertexProperty& vp = execGraph_[*viter];
      execVertexMap.insert( std::make_pair(vp.id, *viter) );
      std::pair<OutEdgeIter, OutEdgeIter> edges = boost::out_edges(*viter, execGraph_);
      for( OutEdgeIter eit = edges.first; eit != edges.second; ++eit ){
        VertexProperty& tvp = execGraph_[ boost::target(*eit, execGraph_) ];

        vp.consumerList.push_back(&tvp);
        tvp.ancestorList.push_back(&vp);
        (vp.nconsumers)++;
        (tvp.nparents)++;
        if( vp.poller ) (tvp.nparents)++;
        if( tvp.poller ) (vp.nconsumers)++;
      }
    }

    //-- Steps 3 & 4
    for( VertIter viter = execGraphVertices.first; viter != execGraphVertices.second; ++viter ){
      VertexProperty& vp = execGraph_[*viter];

      //The execution graph nodes at the bottom of the tree are roots and have no parents.
      //Since edge nodes cannot be 'dynamic' we flag these nodes as persistent
      if( vp.nparents == 0 ){
        vp.set_is_edge(true);
        rootList_.push_back(*viter);
      }

      FieldManagerList& fml = *extract_field_manager_list( fmls_, vp.fmlid );

      if( vp.nconsumers == 0     ) vp.set_is_edge(true);
      if( vp.get_is_edge()       ) vp.set_is_persistent( true );
      if( vp.get_is_persistent() ) vp.expr->field_type().lock_fields( fml );

      vp.expr->field_type().set_field_memory_manager( fml, vp.mm, vp.execTarget );
      vp.nremaining  = vp.nparents;
      vp.ncremaining = vp.nconsumers;
    }

    copy_vertex_properties( execGraph_, depGraph_ );

    /* Step 2 - top down priority scheduling
     * bfs from each top down 'root'
     * Since this is the dependence graph, root nodes are at the 'top' and will have no consumers
     * Since no edge nodes can be 'scratch' we find root nodes in the consumer graph and use exprIDs
     * to flag them as persistent in the execution graph.
     */
    for( VertIter vit = depGraphVertices.first; vit != depGraphVertices.second; ++vit ){
      const VertexProperty& vp = depGraph_[*vit];
      if (vp.nconsumers == 0) {
        boost::breadth_first_search( depGraph_, *vit,
            boost::color_map(boost::get(&VertexProperty::color, depGraph_)).visitor( ExecPriorityVisitor()));
      }
    }

    copy_vertex_properties( depGraph_, execGraph_ );

    invalid_ = false;
  }

  //-------------------------------------------------------------------

  void PriorityScheduler::run()
  {
    /*
     *  NOTE: at this point, the vertex properties on the execution and
     *        dependency graphs will not be synchronized.  During execution,
     *        timings are recorded on the execution graph, and these are not
     *        reflected on the dependency graph.
     */
    if( doNodeTimings_ ) timer_->start( "FullGraph" );
    // Execute everything in the root list
    for( RootIter rit = rootList_.begin(); rit != rootList_.end(); ++rit ) {
      VertexProperty& vp = execGraph_[*rit];
#     ifdef ENABLE_THREADS
      this->pool_.schedule(
          boost::threadpool::prio_task_func( vp.priority,
                                             boost::bind(&PriorityScheduler::call,this,vp))
      );
#     else
      this->call(vp);
#     endif
    }

#   ifdef ENABLE_THREADS
    this->schedBarrier_.wait();
#   endif

    // ensure that all pollers have completed
    this->block_pollers();

    finish();

    // sync up the vertex properties
    copy_vertex_properties( execGraph_, depGraph_ );

    if( doNodeTimings_ ) timer_->stop( "FullGraph" );
  }

  //-------------------------------------------------------------------

  void PriorityScheduler::finish()
  {
    this->nelements_ = boost::num_vertices(execGraph_);
    this->nremaining_ = this->nelements_;

    const std::pair<VertIter, VertIter> execGraphVertices = boost::vertices( execGraph_ );

    for( VertIter iter = execGraphVertices.first; iter != execGraphVertices.second; ++iter ){
      VertexProperty& vp = execGraph_[*iter];
      vp.nremaining  = vp.nparents;
      vp.ncremaining = vp.nconsumers;
    }
  }

  //=================================================================

} // namespace Expr

max_stars_repo_name: MaxZZG/ExprLib

max_issues_repo_name: MaxZZG/ExprLib

max_forks_repo_name: MaxZZG/ExprLib


text:
#include <boost/interprocess/sync/named_mutex.hpp>
#include <boost/interprocess/sync/named_semaphore.hpp>
#include <iostream>

using namespace std;
using namespace boost::interprocess;

void first_process(named_mutex& mtx);
void second_process(named_mutex& mtx);

int main()
{
	try {
		named_mutex mtx(create_only, "test_named_mutex");
		first_process(mtx);
	} catch (interprocess_exception& e) {
        cout << e.what() << endl;
		named_mutex mtx(open_only, "test_named_mutex");
		second_process(mtx);
	}
    //named_mutex::remove("test_named_mutex");
}

void first_process(named_mutex& mtx) throw(interprocess_exception)
{
	named_semaphore sema(open_or_create, "test_named_semaphore", 0);
	mtx.lock();
	sema.post();
}

void second_process(named_mutex& mtx) throw(interprocess_exception)
{
	named_semaphore sema(open_or_create, "test_named_semaphore", 0);
	sema.wait();
	mtx.lock();
}


text:
/* Copyright 2003-2018 Joaquin M Lopez Munoz.
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org/libs/multi_index for library home page.
 */

#include <boost/multi_index/key.hpp>

#if !defined(BOOST_MULTI_INDEX_KEY_SUPPORTED)
#error BOOST_MULTI_INDEX_KEY_SUPPORTED macro not defined
#endif


text: #include <boost/spirit/include/karma_left_alignment.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
#include <bsplines/BSplinePose.hpp>
#include <sm/assert_macros.hpp>
// boost::tie
#include <boost/tuple/tuple.hpp>
#include <sm/kinematics/transformations.hpp>

using namespace sm::kinematics;
using namespace bsplines;

BSplinePose::BSplinePose(int splineOrder, const RotationalKinematics::Ptr& rotationalKinematics)
    : BSpline(splineOrder), rotation_(rotationalKinematics) {}

BSplinePose::~BSplinePose() {}

Eigen::Matrix4d BSplinePose::transformation(double tk) const { return curveValueToTransformation(eval(tk)); }

Eigen::Matrix4d BSplinePose::transformationAndJacobian(double tk, Eigen::MatrixXd* J,
                                                       Eigen::VectorXi* coefficientIndices) const {
    Eigen::MatrixXd JS;
    Eigen::VectorXd p;
    p = evalDAndJacobian(tk, 0, &JS, coefficientIndices);

    Eigen::MatrixXd JT;
    Eigen::Matrix4d T = curveValueToTransformationAndJacobian(p, &JT);

    if (J) {
        *J = JT * JS;
    }

    return T;
}

Eigen::Matrix3d BSplinePose::orientationAndJacobian(double tk, Eigen::MatrixXd* J,
                                                    Eigen::VectorXi* coefficientIndices) const {
    Eigen::Matrix3d C;
    Eigen::MatrixXd JS;
    Eigen::VectorXd p;
    p = evalDAndJacobian(tk, 0, &JS, coefficientIndices);

    Eigen::Matrix3d S;
    C = rotation_->parametersToRotationMatrix(p.tail<3>(), &S);

    Eigen::MatrixXd JO = Eigen::MatrixXd::Zero(3, 6);
    JO.block(0, 3, 3, 3) = S;
    if (J) {
        *J = JO * JS;
    }

    return C;
}

Eigen::Matrix3d BSplinePose::inverseOrientationAndJacobian(double tk, Eigen::MatrixXd* J,
                                                           Eigen::VectorXi* coefficientIndices) const {
    Eigen::Matrix3d C;
    Eigen::MatrixXd JS;
    Eigen::VectorXd p;
    p = evalDAndJacobian(tk, 0, &JS, coefficientIndices);

    Eigen::Matrix3d S;
    C = rotation_->parametersToRotationMatrix(p.tail<3>(), &S).transpose();

    Eigen::MatrixXd JO = Eigen::MatrixXd::Zero(3, 6);
    JO.block(0, 3, 3, 3) = S;
    if (J) {
        *J = -C * JO * JS;
    }

    return C;
}

Eigen::Matrix4d BSplinePose::inverseTransformationAndJacobian(double tk, Eigen::MatrixXd* J,
                                                              Eigen::VectorXi* coefficientIndices) const {
    // std::cout << __FUNCTION__ << "()\n";
    // ASRL_THROW(std::runtime_error,"Not Implemented");
    Eigen::MatrixXd JS;
    Eigen::VectorXd p;
    p = evalDAndJacobian(tk, 0, &JS, coefficientIndices);

    Eigen::MatrixXd JT;
    Eigen::Matrix4d T = curveValueToTransformationAndJacobian(p, &JT);
    // Invert the transformation.
    T.topLeftCorner<3, 3>().transposeInPlace();
    T.topRightCorner<3, 1>() = (-T.topLeftCorner<3, 3>() * T.topRightCorner<3, 1>()).eval();

    if (J) {
        // The "box times" is the linearized transformation way of inverting the jacobian.
        *J = -sm::kinematics::boxTimes(T) * JT * JS;
    }

    if (coefficientIndices) {
        *coefficientIndices = localCoefficientVectorIndices(tk);
    }

    return T;
}

Eigen::Matrix4d BSplinePose::inverseTransformation(double tk) const {
    Eigen::Matrix4d T = curveValueToTransformation(eval(tk));
    T.topLeftCorner<3, 3>().transposeInPlace();
    T.topRightCorner<3, 1>() = (-T.topLeftCorner<3, 3>() * T.topRightCorner<3, 1>()).eval();
    return T;
}

Eigen::Vector4d BSplinePose::transformVectorAndJacobian(double tk, const Eigen::Vector4d& v_tk, Eigen::MatrixXd* J,
                                                        Eigen::VectorXi* coefficientIndices) const {
    Eigen::MatrixXd JT;
    Eigen::Matrix4d T_n_vk = transformationAndJacobian(tk, &JT, coefficientIndices);
    Eigen::Vector4d v_n = T_n_vk * v_tk;

    if (J) {
        *J = sm::kinematics::boxMinus(v_n) * JT;
    }

    return v_n;
}

// Position at certain time. p = x[:3]
Eigen::Vector3d BSplinePose::position(double tk) const { return eval(tk).head<3>(); }

// Orientation at certain time, phi = x[3:], R = Exp(phi)
Eigen::Matrix3d BSplinePose::orientation(double tk) const {
    return rotation_->parametersToRotationMatrix(eval(tk).tail<3>());
}

Eigen::Matrix3d BSplinePose::inverseOrientation(double tk) const {
    return rotation_->parametersToRotationMatrix(eval(tk).tail<3>()).transpose();
}

// v_W = dp/dt
Eigen::Vector3d BSplinePose::linearVelocity(double tk) const { return evalD(tk, 1).head<3>(); }

// v_B = R_BW * v_W = R_WB^T * dp/dt
Eigen::Vector3d BSplinePose::linearVelocityBodyFrame(double tk) const {
    Eigen::VectorXd r = evalD(tk, 0);
    Eigen::Matrix3d C_wb = rotation_->parametersToRotationMatrix(r.tail<3>());
    return C_wb.transpose() * evalD(tk, 1).head<3>();
}

// a_W = dp^2/dt^2
Eigen::Vector3d BSplinePose::linearAcceleration(double tk) const { return evalD(tk, 2).head<3>(); }

// a_B = R_BW * a_W = R_WB^T * dp^2/dt^2
Eigen::Vector3d BSplinePose::linearAccelerationBodyFrame(double tk) const {
    Eigen::VectorXd r = evalD(tk, 0);
    Eigen::Matrix3d C_wb = rotation_->parametersToRotationMatrix(r.tail<3>());
    return C_wb.transpose() * evalD(tk, 2).head<3>();
}

Eigen::Vector3d BSplinePose::linearAccelerationAndJacobian(double tk, Eigen::MatrixXd* J,
                                                           Eigen::VectorXi* coefficientIndices) const {
    Eigen::Vector3d a = evalDAndJacobian(tk, 2, J, coefficientIndices).head<3>();
    if (J) {
        J->conservativeResize(3, J->cols());
    }
    return a;
}

// \omega_w_{b,w} (angular velocity of the body frame as seen from the world frame, expressed in the world frame)
Eigen::Vector3d BSplinePose::angularVelocity(double tk) const {
    Eigen::Vector3d omega;
    Eigen::VectorXd r = evalD(tk, 0);
    Eigen::VectorXd v = evalD(tk, 1);

    // \omega = S(\bar \theta) \dot \theta
    omega = -rotation_->parametersToSMatrix(r.tail<3>()) * v.tail<3>();
    return omega;
}

// \omega_b_{w,b} (angular velocity of the world frame as seen from the body frame, expressed in the body frame)
Eigen::Vector3d BSplinePose::angularVelocityBodyFrame(double tk) const {
    Eigen::Vector3d omega;
    Eigen::VectorXd r = evalD(tk, 0);
    Eigen::VectorXd v = evalD(tk, 1);
    Eigen::Matrix3d S;
    Eigen::Matrix3d C_w_b = rotation_->parametersToRotationMatrix(r.tail<3>(), &S);

    // \omega = S(\bar \theta) \dot \theta
    omega = -C_w_b.transpose() * S * v.tail<3>();
    return omega;
}

// \omega_b_{w,b} (angular velocity of the world frame as seen from the body frame, expressed in the body frame)
Eigen::Vector3d BSplinePose::angularVelocityBodyFrameAndJacobian(double tk, Eigen::MatrixXd* J,
                                                                 Eigen::VectorXi* coefficientIndices) const {
    Eigen::Vector3d omega;
    Eigen::Vector3d p;
    Eigen::Vector3d pdot;
    Eigen::MatrixXd Jp;
    Eigen::MatrixXd Jpdot;
    p = evalDAndJacobian(tk, 0, &Jp, NULL).tail<3>();
    pdot = evalDAndJacobian(tk, 1, &Jpdot, coefficientIndices).tail<3>();

    Eigen::MatrixXd Jr;
    Eigen::Matrix3d C_w_b = inverseOrientationAndJacobian(tk, &Jr, NULL);

    // Rearrange the spline jacobian matrices. Now Jpdot is the
    // jacobian of p wrt the spline coefficients stacked on top
    // of the jacobian of pdot wrt the spline coefficients.
    Jpdot.block(0, 0, 3, Jpdot.cols()) = Jp.block(3, 0, 3, Jp.cols());

    // std::cout << "Jpdot\n" << Jpdot << std::endl;

    Eigen::Matrix<double, 3, 6> Jo;
    omega = -C_w_b * rotation_->angularVelocityAndJacobian(p, pdot, &Jo);
    Jo = (-C_w_b * Jo).eval();
    // std::cout << "Jo:\n" << Jo << std::endl;
    if (J) {
        *J = Jo * Jpdot + sm::kinematics::crossMx(omega) * Jr;
    }

    return omega;
}

// \omega_w_{b,w} (angular velocity of the body frame as seen from the world frame, expressed in the world frame)
Eigen::Vector3d BSplinePose::angularVelocityAndJacobian(double tk, Eigen::MatrixXd* J,
                                                        Eigen::VectorXi* coefficientIndices) const {
    Eigen::Vector3d omega;
    Eigen::Vector3d p;
    Eigen::Vector3d pdot;
    Eigen::MatrixXd Jp;
    Eigen::MatrixXd Jpdot;
    p = evalDAndJacobian(tk, 0, &Jp, nullptr).tail<3>();
    pdot = evalDAndJacobian(tk, 1, &Jpdot, coefficientIndices).tail<3>();

    // Rearrange the spline jacobian matrices. Now Jpdot is the
    // jacobian of p wrt the spline coefficients stacked on top
    // of the jacobian of pdot wrt the spline coefficients.
    Jpdot.block(0, 0, 3, Jpdot.cols()) = Jp.block(3, 0, 3, Jp.cols());

    // std::cout << "Jpdot\n" << Jpdot << std::endl;

    Eigen::Matrix<double, 3, 6> Jo;
    // FixMe by CC: seems like lost the minus "-"?
    omega = rotation_->angularVelocityAndJacobian(p, pdot, &Jo);

    // std::cout << "Jo:\n" << Jo << std::endl;
    if (J) {
        *J = Jo * Jpdot;
    }

    return omega;
}

// \omega_dot_b_{w,b} (angular acceleration of the world frame as seen from the body frame, expressed in the body frame)
Eigen::Vector3d BSplinePose::angularAccelerationBodyFrame(double tk) const {
    Eigen::Vector3d omega;
    Eigen::VectorXd r = evalD(tk, 0);
    Eigen::VectorXd v = evalD(tk, 2);
    Eigen::Matrix3d S;
    Eigen::Matrix3d C_w_b = rotation_->parametersToRotationMatrix(r.tail<3>(), &S);

    // \omega = S(\bar \theta) \dot \theta
    omega = -C_w_b.transpose() * S * v.tail<3>();
    return omega;
}

// \omega_dot_b_{w,b} (angular acceleration of the world frame as seen from the body frame, expressed in the body frame)
Eigen::Vector3d BSplinePose::angularAccelerationBodyFrameAndJacobian(double tk, Eigen::MatrixXd* J,
                                                                     Eigen::VectorXi* coefficientIndices) const {
    Eigen::Vector3d omega;
    Eigen::Vector3d p;
    Eigen::Vector3d pdot;
    Eigen::MatrixXd Jp;
    Eigen::MatrixXd Jpdot;
    p = evalDAndJacobian(tk, 0, &Jp, NULL).tail<3>();
    pdot = evalDAndJacobian(tk, 2, &Jpdot, coefficientIndices).tail<3>();

    Eigen::MatrixXd Jr;
    Eigen::Matrix3d C_w_b = inverseOrientationAndJacobian(tk, &Jr, NULL);

    // Rearrange the spline jacobian matrices. Now Jpdot is the
    // jacobian of p wrt the spline coefficients stacked on top
    // of the jacobian of pdot wrt the spline coefficients.
    Jpdot.block(0, 0, 3, Jpdot.cols()) = Jp.block(3, 0, 3, Jp.cols());

    Eigen::Matrix<double, 3, 6> Jo;
    omega = -C_w_b * rotation_->angularVelocityAndJacobian(p, pdot, &Jo);
    Jo = (-C_w_b * Jo).eval();
    if (J) {
        *J = Jo * Jpdot + sm::kinematics::crossMx(omega) * Jr;
    }

    return omega;
}

// \omega_dot_w_{b,w} (angular acceleration of the body frame as seen from the world frame, expressed in the world
// frame)
Eigen::Vector3d BSplinePose::angularAccelerationAndJacobian(double tk, Eigen::MatrixXd* J,
                                                            Eigen::VectorXi* coefficientIndices) const {
    Eigen::Vector3d omega;
    Eigen::Vector3d p;
    Eigen::Vector3d pdot;
    Eigen::MatrixXd Jp;
    Eigen::MatrixXd Jpdot;
    p = evalDAndJacobian(tk, 0, &Jp, NULL).tail<3>();
    pdot = evalDAndJacobian(tk, 2, &Jpdot, coefficientIndices).tail<3>();

    // Rearrange the spline jacobian matrices. Now Jpdot is the
    // jacobian of p wrt the spline coefficients stacked on top
    // of the jacobian of pdot wrt the spline coefficients.
    Jpdot.block(0, 0, 3, Jpdot.cols()) = Jp.block(3, 0, 3, Jp.cols());

    Eigen::Matrix<double, 3, 6> Jo;
    // FixMe by CC: seems like lost the minus "-"?
    omega = rotation_->angularVelocityAndJacobian(p, pdot, &Jo);
    if (J) {
        *J = Jo * Jpdot;
    }

    return omega;
}

void BSplinePose::initPoseSpline(double t0, double t1, const Eigen::Matrix4d& T_n_t0, const Eigen::Matrix4d& T_n_t1) {
    Eigen::VectorXd v0 = transformationToCurveValue(T_n_t0);
    Eigen::VectorXd v1 = transformationToCurveValue(T_n_t1);

    initSpline(t0, t1, v0, v1);
}

void BSplinePose::initPoseSpline2(const Eigen::VectorXd& times, const Eigen::Matrix<double, 6, Eigen::Dynamic>& poses,
                                  int numSegments, double lambda) {
    initSpline2(times, poses, numSegments, lambda);
}

void BSplinePose::initPoseSpline3(const Eigen::VectorXd& times, const Eigen::Matrix<double, 6, Eigen::Dynamic>& poses,
                                  int numSegments, double lambda) {
    initSpline3(times, poses, numSegments, lambda);
}

void BSplinePose::initPoseSplineSparse(const Eigen::VectorXd& times,
                                       const Eigen::Matrix<double, 6, Eigen::Dynamic>& poses, int numSegments,
                                       double lambda) {
    initSplineSparse(times, poses, numSegments, lambda);
}

void BSplinePose::initPoseSplineSparseKnots(const Eigen::VectorXd& times, const Eigen::MatrixXd& interpolationPoints,
                                            const Eigen::VectorXd& knots, double lambda) {
    initSplineSparseKnots(times, interpolationPoints, knots, lambda);
}

void BSplinePose::addPoseSegment(double tk, const Eigen::Matrix4d& T_n_tk) {
    Eigen::VectorXd vk = transformationToCurveValue(T_n_tk);

    addCurveSegment(tk, vk);
}

void BSplinePose::addPoseSegment2(double tk, const Eigen::Matrix4d& T_n_tk, double lambda) {
    Eigen::VectorXd vk = transformationToCurveValue(T_n_tk);

    addCurveSegment2(tk, vk, lambda);
}

Eigen::Matrix4d BSplinePose::curveValueToTransformation(const Eigen::VectorXd& c) const {
    SM_ASSERT_EQ_DBG(Exception, c.size(), 6, "The curve value is an unexpected size!");
    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
    T.topLeftCorner<3, 3>() = rotation_->parametersToRotationMatrix(c.tail<3>());
    T.topRightCorner<3, 1>() = c.head<3>();

    return T;
}

Eigen::VectorXd BSplinePose::transformationToCurveValue(const Eigen::Matrix4d& T) const {
    Eigen::VectorXd c(6);
    c.head<3>() = T.topRightCorner<3, 1>();
    c.tail<3>() = rotation_->rotationMatrixToParameters(T.topLeftCorner<3, 3>());

    return c;
}

RotationalKinematics::Ptr BSplinePose::rotation() const { return rotation_; }

Eigen::Matrix4d BSplinePose::curveValueToTransformationAndJacobian(const Eigen::VectorXd& p, Eigen::MatrixXd* J) const {
    SM_ASSERT_EQ_DBG(Exception, p.size(), 6, "The curve value is an unexpected size!");
    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
    Eigen::Matrix3d S;
    T.topLeftCorner<3, 3>() = rotation_->parametersToRotationMatrix(p.tail<3>(), &S);
    T.topRightCorner<3, 1>() = p.head<3>();

    if (J) {
        *J = Eigen::MatrixXd::Identity(6, 6);
        J->topRightCorner<3, 3>() = -crossMx(p.head<3>()) * S;
        J->bottomRightCorner<3, 3>() = S;
    }

    return T;
}

max_stars_repo_name: chengfzy/kalibr

max_issues_repo_name: chengfzy/kalibr

max_forks_repo_name: chengfzy/kalibr


text:
/** @file
    @brief Main file for a unified video-based and inertial tracking system.

    @date 2015

    @author
    Sensics, Inc.
    <http://sensics.com/osvr>
*/

// Copyright 2015 Sensics, Inc.
//
// 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.

// Internal Includes
#include "AdditionalReports.h"
#include "ConfigurationParser.h"
#include "HDKData.h"
#include "MakeHDKTrackingSystem.h"
#include "TrackerThread.h"

// ImageSources mini-library
#include "ImageSources/ImageSource.h"
#include "ImageSources/ImageSourceFactories.h"

// uvbi-core mini-library
#include "CameraParameters.h"
#include "ThreadsafeBodyReporting.h"
#include "TrackingSystem.h"

#include <osvr/AnalysisPluginKit/AnalysisPluginKitC.h>
#include <osvr/ClientKit/InterfaceC.h>
#include <osvr/ClientKit/InterfaceCallbackC.h>
#include <osvr/PluginKit/AnalogInterfaceC.h>
#include <osvr/PluginKit/PluginKit.h>
#include <osvr/PluginKit/TrackerInterfaceC.h>

#include <osvr/Util/EigenInterop.h>

// Generated JSON header file
#include "org_osvr_unifiedvideoinertial_json.h"

// Library/third-party includes
#include <json/reader.h>
#include <json/value.h>
#include <json/writer.h>
#include <opencv2/core/core.hpp> // for basic OpenCV types
#include <opencv2/core/operations.hpp>
#include <opencv2/highgui/highgui.hpp> // for image capture
#include <opencv2/imgproc/imgproc.hpp> // for image scaling

#include <boost/assert.hpp>
#include <boost/noncopyable.hpp>
#include <boost/variant.hpp>

#include <util/Stride.h>

// Standard includes
#include <fstream>
#include <iomanip>
#include <iostream>
#include <memory>
#include <sstream>
#include <stdexcept>
#include <string>

// Anonymous namespace to avoid symbol collision
namespace {
static const auto DRIVER_NAME = "UnifiedTrackingSystem";

static const auto DEBUGGABLE_BEACONS = 34;
static const auto DATAPOINTS_PER_BEACON = 5;
using TrackingSystemPtr = std::unique_ptr<osvr::vbtracker::TrackingSystem>;
using osvr::vbtracker::TrackerThread;
using osvr::vbtracker::BodyReportingVector;
using osvr::vbtracker::TrackedBody;
using osvr::vbtracker::TrackedBodyIMU;
using osvr::vbtracker::BodyId;

class UnifiedVideoInertialTracker : boost::noncopyable {
  public:
    using size_type = std::size_t;

  private:
    osvr::pluginkit::DeviceToken m_dev;
    OSVR_ClientContext m_clientCtx;
    OSVR_ClientInterface m_clientInterface;
    OSVR_TrackerDeviceInterface m_tracker;
    OSVR_AnalogDeviceInterface m_analog;
    osvr::vbtracker::ImageSourcePtr m_source;
    cv::Mat m_frame;
    cv::Mat m_imageGray;
    TrackingSystemPtr m_trackingSystem;
    /// @todo kind-of assumes there's only one body.
    TrackedBody *m_mainBody = nullptr;
    /// @todo kind-of assumes there's only one body.
    TrackedBodyIMU *m_imu = nullptr;
    const double m_additionalPrediction;
    const std::int32_t m_camUsecOffset = 0;
    const std::int32_t m_oriUsecOffset = 0;
    const std::int32_t m_angvelUsecOffset = 0;
    const bool m_continuousReporting;
    const bool m_debugData;
    BodyReportingVector m_bodyReportingVector;
    std::unique_ptr<TrackerThread> m_trackerThreadManager;
    bool m_threadLoopStarted = false;
    std::thread m_trackerThread;

  public:
    UnifiedVideoInertialTracker(OSVR_PluginRegContext ctx,
                                osvr::vbtracker::ImageSourcePtr &&source,
                                osvr::vbtracker::ConfigParams params,
                                TrackingSystemPtr &&trackingSystem)
        : m_source(std::move(source)),
          m_trackingSystem(std::move(trackingSystem)),
          m_additionalPrediction(params.additionalPrediction),
          m_camUsecOffset(params.cameraMicrosecondsOffset),
          m_oriUsecOffset(params.imu.orientationMicrosecondsOffset),
          m_angvelUsecOffset(params.imu.angularVelocityMicrosecondsOffset),
          m_continuousReporting(params.continuousReporting),
          m_debugData(params.streamBeaconDebugInfo) {
        if (params.numThreads > 0) {
            // Set the number of threads for OpenCV to use.
            cv::setNumThreads(params.numThreads);
        }

        /// Create the initialization options
        OSVR_DeviceInitOptions opts = osvrDeviceCreateInitOptions(ctx);

        // Configure the tracker interface.
        osvrDeviceTrackerConfigure(opts, &m_tracker);
        if (m_debugData) {
            osvrDeviceAnalogConfigure(opts, &m_analog,
                                      osvr::vbtracker::DEBUG_ANALOGS_REQUIRED);
        }

        /// Create the analysis device token with the options
        OSVR_DeviceToken dev;
        if (OSVR_RETURN_FAILURE ==
            osvrAnalysisSyncInit(ctx, DRIVER_NAME, opts, &dev, &m_clientCtx)) {
            throw std::runtime_error("Could not initialize analysis plugin!");
        }
        m_dev = osvr::pluginkit::DeviceToken(dev);

        /// Create/update and send JSON descriptor
        m_dev.sendJsonDescriptor(createDeviceDescriptor());

        m_mainBody = &(m_trackingSystem->getBody(BodyId(0)));
        if (m_mainBody->hasIMU()) {
            m_imu = &(m_mainBody->getIMU());
            /// Create our client interface and register a callback.
            if (OSVR_RETURN_FAILURE ==
                osvrClientGetInterface(m_clientCtx, params.imu.path.c_str(),
                                       &m_clientInterface)) {
                throw std::runtime_error(
                    "Could not get client interface for analysis plugin!");
            }
            osvrRegisterOrientationCallback(
                m_clientInterface, &UnifiedVideoInertialTracker::oriCallback,
                this);
            osvrRegisterAngularVelocityCallback(
                m_clientInterface, &UnifiedVideoInertialTracker::angVelCallback,
                this);
        }

        /// Set up thread communication.
        setupBodyReporting();

        /// Set up the object that will run in the other thread, and spawn the
        /// thread.
        startTrackerThread();

        /// Register update callback
        m_dev.registerUpdateCallback(this);
    }

    /// Processes a tracker report.
    template <typename ReportType>
    void handleData(OSVR_TimeValue const &timestamp, ReportType const &report) {
        m_trackerThreadManager->submitIMUReport(*m_imu, timestamp, report);
    }

    static void oriCallback(void *userdata, const OSVR_TimeValue *timestamp,
                            const OSVR_OrientationReport *report) {
        auto &self = *static_cast<UnifiedVideoInertialTracker *>(userdata);
        OSVR_TimeValue tv = *timestamp;
        if (self.m_oriUsecOffset != 0) {
            // apply offset, if non-zero.
            const OSVR_TimeValue offset{0, self.m_oriUsecOffset};
            osvrTimeValueSum(&tv, &offset);
        }
        self.handleData(tv, *report);
    }

    static void angVelCallback(void *userdata, const OSVR_TimeValue *timestamp,
                               const OSVR_AngularVelocityReport *report) {
        auto &self = *static_cast<UnifiedVideoInertialTracker *>(userdata);
        OSVR_TimeValue tv = *timestamp;
        if (self.m_angvelUsecOffset != 0) {
            // apply offset, if non-zero.
            const OSVR_TimeValue offset{0, self.m_angvelUsecOffset};
            osvrTimeValueSum(&tv, &offset);
        }
        self.handleData(tv, *report);
    }

    ~UnifiedVideoInertialTracker() { stopTrackerThread(); }

    /// Create a "BodyReporting" interchange structure for each body we track.
    void setupBodyReporting() {
        m_bodyReportingVector.clear();
        auto n = totalNumBodies();
        for (decltype(n) i = 0; i < n; ++i) {
            m_bodyReportingVector.emplace_back(
                osvr::vbtracker::BodyReporting::make());
        }
    }

    size_type numTrackedBodies() const {
        return m_trackingSystem->getNumBodies();
    }

    int totalNumBodies() const {
        static const auto EXTRA_TRACKING_SENSORS =
            osvr::vbtracker::extra_outputs::numExtraOutputs;
        // Add the extra sensors as configured in AdditionalReports.h
        return static_cast<int>(numTrackedBodies()) + EXTRA_TRACKING_SENSORS;
    }

    static std::string getTrackerPath(int sensor) {
        std::ostringstream os;
        os << "tracker/" << sensor;
        return os.str();
    }

    /// Augment the static JSON device descriptor with individual
    /// "semi-semantic" paths for real bodies, as well as the semantic paths for
    /// the "extra" outputs (like the camera pose)
    std::string createDeviceDescriptor() const {
        auto n = numTrackedBodies();
        auto origJson = std::string{org_osvr_unifiedvideoinertial_json};
        Json::Value root;
        {
            Json::Reader reader;
            /// This call can't fail, but...
            if (!reader.parse(origJson, root)) {
                BOOST_ASSERT_MSG(false,
                                 "Not possible for parsing to fail: loading "
                                 "a file that was parsed before "
                                 "transformation into string literal!");
                return origJson;
            }
        }
        auto &semantic = root["semantic"];
        auto &body = semantic["body"];
        body = Json::Value(Json::objectValue);
        /// Set up a (minimally) semantic path (numbered) for each body under
        /// bodies
        for (decltype(n) i = 0; i < n; ++i) {
            body[std::to_string(i)] = getTrackerPath(i);
        }

        /// Set up semantic paths for extra outputs.
        namespace extra_outputs = osvr::vbtracker::extra_outputs;
        if (extra_outputs::outputCam) {
            semantic["camera"] =
                getTrackerPath(n + extra_outputs::outputCamIndex);
        }

        if (n > 0 && extra_outputs::haveHMDExtraOutputs) {
            /// Re-shuffle body 0 so it is both an alias and a group
            auto &body0 = body["0"];
            std::string oldTarget = body0.asString();
            body0 = Json::Value(Json::objectValue);
            body0["$target"] = oldTarget;

            if (extra_outputs::outputImu) {
                body0["imu"] =
                    getTrackerPath(n + extra_outputs::outputImuIndex);
            }
            if (extra_outputs::haveHMDCameraSpaceExtraOutputs) {
                auto &cameraSpace = body0["cameraSpace"];
                cameraSpace = Json::Value(Json::objectValue);

                if (extra_outputs::outputImuCam) {
                    cameraSpace["imu"] =
                        getTrackerPath(n + extra_outputs::outputImuCamIndex);
                }
                if (extra_outputs::outputHMDCam) {
                    cameraSpace["body"] =
                        getTrackerPath(n + extra_outputs::outputHMDCamIndex);
                }
            }
        }

        /// Now, serialize back to a string.
        Json::FastWriter writer;
        return writer.write(root);
    }

    OSVR_ReturnCode update();

    void startTrackerThread() {
        if (m_trackerThreadManager) {
            throw std::logic_error("Trying to start the tracker thread when "
                                   "it's already started!");
        }
        std::cout << "Starting the tracker thread..." << std::endl;
        m_trackerThreadManager.reset(new TrackerThread(
            *m_trackingSystem, *m_source, m_bodyReportingVector,
            osvr::vbtracker::getHDKCameraParameters(), m_camUsecOffset,
            !m_continuousReporting, m_debugData));

        /// This will start the thread, but it won't enter its full main loop
        /// until we call permitStart()
        m_trackerThread =
            std::thread([&] { m_trackerThreadManager->threadAction(); });
    }
    void stopTrackerThread() {
        if (m_trackerThreadManager) {
            std::cout << "Shutting down the tracker thread..." << std::endl;
            m_trackerThreadManager->triggerStop();
            if (m_trackerThread.joinable()) {
                m_trackerThread.join();
            }
            m_trackerThreadManager.reset();
            m_trackerThread = std::thread();
        }
    }
};

inline OSVR_ReturnCode UnifiedVideoInertialTracker::update() {
    if (!m_threadLoopStarted) {
        /// First call to update, we let the tracker go flying.
        m_threadLoopStarted = true;
        m_trackerThreadManager->permitStart();
        return OSVR_RETURN_SUCCESS;
    }
    namespace ei = osvr::util::eigen_interop;
    std::size_t numSensors = m_bodyReportingVector.size();
    /// On each update pass, we go through and attempt to report for every body,
    /// at the current time + additional prediction as requested.
    for (std::size_t i = 0; i < numSensors; ++i) {
        osvr::vbtracker::BodyReport report;
        auto gotReport =
            m_bodyReportingVector[i]->getReport(m_additionalPrediction, report);
        if (!gotReport) {
            /// couldn't get a report for this sensor for one reason or another.
            // std::cout << "Couldn't get report for " << i << std::endl;
            continue;
        }
        osvrDeviceTrackerSendPoseTimestamped(m_dev, m_tracker, &report.pose, i,
                                             &report.timestamp);
        if (OSVR_TRUE == report.vel.angularVelocityValid ||
            OSVR_TRUE == report.vel.linearVelocityValid) {
            osvrDeviceTrackerSendVelocityTimestamped(
                m_dev, m_tracker, &report.vel, i, &report.timestamp);
        }
    }
    if (m_debugData) {
        osvr::vbtracker::DebugArray arr;
        /// send each debug report that has been queued.
        while (m_trackerThreadManager->checkForDebugData(arr)) {
            osvrDeviceAnalogSetValues(m_dev, m_analog, arr.data(), arr.size());
        }
    }
    return OSVR_RETURN_SUCCESS;
}

class ConfiguredDeviceConstructor {
  public:
    /// @brief This is the required signature for a device instantiation
    /// callback.
    OSVR_ReturnCode operator()(OSVR_PluginRegContext ctx, const char *params) {

        // Read the JSON data from parameters.
        Json::Value root;
        if (params) {
            Json::Reader r;
            if (!r.parse(params, root)) {
                std::cerr << "Could not parse parameters!" << std::endl;
            }
        }

        // Read these parameters from a "params" field in the device Json
        // configuration file.

        // This is in a separate function/header for sharing and for clarity.
        auto config = osvr::vbtracker::parseConfigParams(root);

#ifdef _WIN32
        auto cam = osvr::vbtracker::openHDKCameraDirectShow(config.highGain);
#else // !_WIN32
        /// @todo This is rather crude, as we can't select the exact camera we
        /// want, nor set the "50Hz" high-gain mode (and only works with HDK
        /// camera firmware v7 and up). Presumably eventually use libuvc on
        /// other platforms instead, at least for the HDK IR camera.

        auto cam = osvr::vbtracker::openOpenCVCamera(0);
#endif

        if (!cam || !cam->ok()) {
            std::cerr << "Could not access the tracking camera, skipping "
                         "video-based tracking!"
                      << std::endl;
            return OSVR_RETURN_FAILURE;
        }

        auto trackingSystem = osvr::vbtracker::makeHDKTrackingSystem(config);
        // OK, now that we have our parameters, create the device.
        osvr::pluginkit::PluginContext context(ctx);
        auto newTracker = osvr::pluginkit::registerObjectForDeletion(
            ctx, new UnifiedVideoInertialTracker(ctx, std::move(cam), config,
                                                 std::move(trackingSystem)));

        return OSVR_RETURN_SUCCESS;
    }
};

} // namespace

OSVR_PLUGIN(org_osvr_unifiedvideoinertial) {
    osvr::pluginkit::PluginContext context(ctx);

    /// Tell the core we're available to create a device object.
    osvr::pluginkit::registerDriverInstantiationCallback(
        ctx, DRIVER_NAME, new ConfiguredDeviceConstructor);

    return OSVR_RETURN_SUCCESS;
}

max_stars_repo_name: ethanpeng/OSVR-Core

max_issues_repo_name: ethanpeng/OSVR-Core

max_forks_repo_name: ethanpeng/OSVR-Core


text:
//  Copyright (c) 2007-2012 Hartmut Kaiser
//
//  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)

#if !defined(HPX_7B5783D6_FCA3_4E3D_BBCE_FBB0A6CF644B)
#define HPX_7B5783D6_FCA3_4E3D_BBCE_FBB0A6CF644B

#include <hpx/hpx_fwd.hpp>
#include <hpx/util/interval_timer.hpp>
#include <hpx/lcos/local/mutex.hpp>
#include <hpx/include/performance_counters.hpp>

#include <string>
#include <vector>

#include <boost/cstdint.hpp>

namespace hpx { namespace util
{
    ///////////////////////////////////////////////////////////////////////////
    class HPX_EXPORT activate_counters
    {
    public:
        activate_counters(std::vector<std::string> const& names);

        ~activate_counters();

    protected:
        void find_counters();
        bool find_counter(performance_counters::counter_info const& info,
            error_code& ec);

        void start_counters(error_code& ec = throws);
        void stop_counters(error_code& ec = throws);

    public:
        void reset_counters(error_code& ec = throws);

        typedef std::vector<future<performance_counters::counter_value> >
            async_counter_values_type;

        typedef std::vector<performance_counters::counter_value>
            sync_counter_values_type;

        async_counter_values_type
        evaluate_counters_async(bool reset = false, error_code& ec = throws);

        sync_counter_values_type
        evaluate_counters_sync(bool reset = false, error_code& ec = throws);

        std::string name(std::size_t i) const
        {
            return names_[i];
        }

        naming::id_type id(std::size_t i) const
        {
            return ids_[i];
        }

        std::string unit_of_measure(std::size_t i) const
        {
            return uoms_[i];
        }

    private:
        std::vector<std::string> names_;      // counter instance names
        std::vector<naming::id_type> ids_;    // gids of counter instances
        std::vector<std::string> uoms_;       // units of measure
    };
}}

#endif // 7B5783D6_FCA3_4E3D_BBCE_FBB0A6CF644B

max_stars_repo_name: kempj/hpx

max_issues_repo_name: kempj/hpx

max_forks_repo_name: kempj/hpx