text:
/**
 *
 * This class is designed to only run on the RaspberryPi and setup an unencrypted TCP/IP server
 * on port 9890.  It recieves screen buffer data and then copies it to the next available
 * buffer on the MatrixStrip instance after shifting the inbound 16bit color to the desired
 * 32bit color scheme.
 *
 * This thread is also only to run on CPU 3, with a 99 priority.
 */
#include <cstdlib>
#include <iostream>
#include <thread>
#include <utility>
#include <boost/asio.hpp>
#include <cstring>
#include <pthread.h>
#include <time.h>

#include "NetworkServer.h"

using boost::asio::ip::tcp;
using std::min;
using std::max;

NetworkServer::NetworkServer(struct NetworkServerConfig config) {
#ifdef __linux__
  pthread_mutex_destroy(&mMutex);
  pthread_mutex_init(&mMutex, nullptr);
#endif

  mNumberSamples = 0;
  mTotalDelta = 0;
  mAverage = 0;
  mPriorAverage = 0;

  mFrameCount = 0;
  mSegmentId = config.segmentId;
  mIncomingPort = config.incomingPort;

  mSinglePanelWidth = config.singlePanelWidth;
  mSinglePanelHeight = config.singlePanelHeight;

  mPixelsPerPanel = mSinglePanelWidth * mSinglePanelWidth;

  mSegmentWidth = config.singlePanelWidth * config.numPanelsWide;
  mSegmentHeight = config.singlePanelHeight * config.numPanelsTall;

  mPanelsWide = config.numPanelsWide;
  mPanelsTall = config.numPanelsTall;

  mTotalPixels = mPixelsPerPanel * mPanelsWide * mPanelsTall;
  mTotalBytes = mTotalPixels * sizeof(uint16_t);

  mMatrixStrip = config.matrixStripInstance;

#ifdef __MATRIX_SHOW_DEBUG_MESSAGES__
  Describe();
#endif
}

uint16_t nColor = 0;

void NetworkServer::ReceiveDataThread(tcp::socket sock) {
  int numBytesReceived = 0;
  uint16_t sbIndex = 0;

  volatile clock_t start = 0;
  volatile clock_t end = 0;

  static uint16_t data[32768];
//  memset(data,0,mTotalBytes);
//  auto *data = (uint16_t *)malloc(mTotalBytes);
  const char *returnData = "K";

  while (GetThreadRunning()) {
    nColor = random() & UINT16_MAX;

    try {
      nColor++;
      start = clock();

      boost::system::error_code error;
      size_t length = boost::asio::read(sock, boost::asio::buffer(&data, mTotalBytes), boost::asio::transfer_exactly(mTotalBytes), error);

      numBytesReceived += length;

      // Ended early! No bueno!
      if (error == boost::asio::error::eof){
//        printf("Eof\n"); fflush(stdout);
        break;
      }
      else if (error) {
        throw boost::system::system_error(error); // Some other error.
      }

      boost::asio::write(sock, boost::asio::buffer(returnData, 1));
//
//
//      if (numBytesReceived == mTotalBytes) {
//        end = clock();
//
//        mNumberSamples++;
//        double delta = (end - start);
//        mTotalDelta += delta;
//        mAverage = (mTotalDelta / mNumberSamples) ;
//        /// CLOCKS_PER_SEC
//
////        uint8_t r = (nColor & 0xF800) >> 8;       // rrrrr... ........ -> rrrrr000
////        uint8_t g = (nColor & 0x07E0) >> 3;       // .....ggg ggg..... -> gggggg00
////        uint8_t b = (nColor & 0x1F) << 3;         // ............bbbbb -> bbbbb000
////        mMatrixStrip->GetDisplayCanvas()->Fill(r,g,b);
//#ifdef __MATRIX_STRIP_BOTTOM_UP__
//        int ptrIndex = mTotalPixels - 1;
//#else
//        int ptrIndex = 0;
//#endif
//        uint16_t *outputBuff = data;
//
//        int col = 0;
//        int row = mMatrixStrip->mCanvasWidth;
//        for (; row > 0 ; row--) {
//
//          col = 0;
//          for (; col < mMatrixStrip->mCanvasHeight; col++) {
//
//#ifdef __MATRIX_STRIP_BOTTOM_UP__
//            uint16_t pixel = outputBuff[ptrIndex--];
////            pixel = nColor;
//#else
//            uint16_t pixel = outputBuff[ptrIndex++];
//#endif
//            // Color separation based off : https://stackoverflow.com/questions/38557734/how-to-convert-16-bit-hex-color-to-rgb888-values-in-c
//            uint8_t r = (pixel & 0xF800) >> 8;       // rrrrr... ........ -> rrrrr000
//            uint8_t g = (pixel & 0x07E0) >> 3;       // .....ggg ggg..... -> gggggg00
//            uint8_t b = (pixel & 0x1F) << 3;         // ............bbbbb -> bbbbb000
//
//            mMatrixStrip->GetRenderCanvas()->SetPixel(row, col, r, g, b);
//          }
//
//        }

      // this thing re-orients the data so that the pixels are mapped vertically
      if (numBytesReceived == mTotalBytes) {
        end = clock();

        mNumberSamples++;
        double delta = (end - start);
        mTotalDelta += delta;
        mAverage = (mTotalDelta / mNumberSamples) ;
        /// CLOCKS_PER_SEC

//        uint8_t r = (nColor & 0xF800) >> 8;       // rrrrr... ........ -> rrrrr000
//        uint8_t g = (nColor & 0x07E0) >> 3;       // .....ggg ggg..... -> gggggg00
//        uint8_t b = (nColor & 0x1F) << 3;         // ............bbbbb -> bbbbb000
//        printf("Here\n");
//        mMatrixStrip->GetRenderCanvas()->Fill(r,g,b);
//        mMatrixStrip->mFrameCount++;
//
//        break;

        int ptrIndex = mTotalPixels - 1;
        uint16_t *outputBuff = data;

        int col = 0;
        int row = mMatrixStrip->mCanvasWidth - 1;
        for (; row > -1 ; row--) {
//          printf("row %i\n", row);
          col = 0;
          for (; col < mMatrixStrip->mCanvasHeight; col++) {

            uint16_t pixel = outputBuff[ptrIndex--];
            // Color separation based off : https://stackoverflow.com/questions/38557734/how-to-convert-16-bit-hex-color-to-rgb888-values-in-c
            uint8_t r = (pixel & 0xF800) >> 8;       // rrrrr... ........ -> rrrrr000
            uint8_t g = (pixel & 0x07E0) >> 3;       // .....ggg ggg..... -> gggggg00
            uint8_t b = (pixel & 0x1F) << 3;         // ............bbbbb -> bbbbb000

            mMatrixStrip->GetRenderCanvas()->SetPixel(row, col, r, g, b);
          }

        }

//        if ((mFrameCount % 10) == 0) printf("%i\n",mFrameCount);
        mMatrixStrip->mFrameCount++;
        break;
      }

    }
    catch (std::exception& e) {
      std::cerr <<  __FUNCTION__ << " Exception: " << e.what() << "\n";
    }
  }

  mFrameCount++;
}

void NetworkServer::ServerStartingThread() {
  mThreadRunning = true;

  boost::asio::io_context io_context;
  unsigned short port = 9890;
  tcp::acceptor a(io_context, tcp::endpoint(tcp::v4(), port));

  sched_param sch_params;
  sch_params.sched_priority = 99;

  int affinity_mask = (1<<2);

  cpu_set_t cpu_mask;
  CPU_ZERO(&cpu_mask);

  for (int i = 0; i < 32; ++i) {
    if ((affinity_mask & (1<<i)) != 0) {
      CPU_SET(i, &cpu_mask);
    }
  }

  while (mThreadRunning) {
    mThread = std::thread(&NetworkServer::ReceiveDataThread, this, a.accept());
    pthread_setschedparam(mThread.native_handle(), SCHED_FIFO, &sch_params);

    int err;

    if ((err=pthread_setaffinity_np(mThread.native_handle(), sizeof(cpu_mask), &cpu_mask))) {
      fprintf(stderr, "FYI: Couldn't set affinity 0x%x: %s\n",
              affinity_mask, strerror(err));
    }

    mThread.detach();
  }
}

void NetworkServer::StartThread() {
  sched_param sch_params;
  sch_params.sched_priority = 99;

  int affinity_mask = (1<<1);
  cpu_set_t cpu_mask;
  CPU_ZERO(&cpu_mask);

  for (int i = 0; i < 32; ++i) {
    if ((affinity_mask & (1<<i)) != 0) {
      CPU_SET(i, &cpu_mask);
    }
  }

  int err;

  std::thread starterThread = std::thread(&NetworkServer::ServerStartingThread, this);

  pthread_setschedparam(starterThread.native_handle(), SCHED_FIFO, &sch_params);

  if ((err=pthread_setaffinity_np(starterThread.native_handle(), sizeof(cpu_mask), &cpu_mask))) {
    fprintf(stderr, "FYI: Couldn't set affinity 0x%x: %s\n",
            affinity_mask, strerror(err));
  }

  starterThread.detach();
//
//  mThreadRunning = true;
//
//  boost::asio::io_context io_context;
//  unsigned short port = 9890;
//  tcp::acceptor a(io_context, tcp::endpoint(tcp::v4(), port));
//
//  printf("%s\n", __FUNCTION__);
//  while (mThreadRunning) {
//    mThread = std::thread(&NetworkServer::ReceiveDataThread, this, a.accept());
//    mThread.detach();
//  }
}

void NetworkServer::StopThread() {

  mThreadRunning = false;
  if (mThread.joinable()) {
    mThread.join();
  }
  usleep(100);

}

void NetworkServer::LockMutex() {
//  printf("%i NetworkServer::%s %p\n", mSegmentId, __FUNCTION__, &mMutex);
  pthread_mutex_lock(&mMutex);
}

void NetworkServer::UnlockMutex() {
//  printf("NetworkServer::%s\n", __FUNCTION__);fflush(stdout);

  pthread_mutex_unlock(&mMutex);
}

void NetworkServer::Describe() {
  printf("NetworkServer %p\n", this);
  printf("\tmSegmentId = %i\n", mSegmentId);
  printf("\tmSinglePanelWidth = %i\n", mSinglePanelWidth);
  printf("\tmSinglePanelHeight = %i\n", mSinglePanelHeight);
  printf("\tmPixelsPerPanel = %i\n", mPixelsPerPanel);
  printf("\tmSegmentWidth = %i\n", mSegmentWidth);
  printf("\tmSegmentHeight = %i\n", mSegmentHeight);
  printf("\tmMatricesWide = %i\n", mPanelsWide);
  printf("\tmMatricesTall = %i\n", mPanelsTall);
  printf("\tmTotalPixels = %i\n", mTotalPixels);
  printf("\tmTotalBytes = %lu\n", mTotalBytes);
  printf("\tmIncomingPort = %i\n", mIncomingPort);
  printf("\n");
  fflush(stdout);
}

NetworkServer::~NetworkServer() {
  StopThread();
}

max_stars_repo_name: jaygarcia/network-rgb-matrix-display

max_issues_repo_name: jaygarcia/network-rgb-matrix-display

max_forks_repo_name: jaygarcia/network-rgb-matrix-display


text:
/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   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.

	 Author: 							Jaume Coll-Font
	 Last Modification:		September 6 2017
*/

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

// Tikhonov specific headers
#include <Core/Algorithms/Legacy/Inverse/TikhonovAlgoAbstractBase.h>
#include <Core/Algorithms/Legacy/Inverse/TikhonovImpl.h>
#include <Core/Algorithms/Legacy/Inverse/SolveInverseProblemWithStandardTikhonovImpl.h>
#include <Core/Algorithms/Legacy/Inverse/SolveInverseProblemWithTikhonovSVD_impl.h>
#include <Core/Algorithms/Legacy/Inverse/SolveInverseProblemWithTSVD_impl.h>

// Datatypes
#include <Core/Datatypes/Matrix.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Core/Math/MiscMath.h>
#include <unsupported/Eigen/Splines>

// SCIRun structural
#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Algorithms/Base/AlgorithmVariableNames.h>
#include <Core/Logging/LoggerInterface.h>
#include <Core/Logging/Log.h>
#include <Core/Utils/Exception.h>

using namespace SCIRun;
using namespace SCIRun::Core;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Logging;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Algorithms::Inverse;

// shared inputs
const AlgorithmInputName TikhonovAlgoAbstractBase::ForwardMatrix("ForwardMatrix");
const AlgorithmInputName TikhonovAlgoAbstractBase::MeasuredPotentials("MeasuredPotentials");
const AlgorithmInputName TikhonovAlgoAbstractBase::WeightingInSourceSpace("WeightingInSourceSpace");
const AlgorithmInputName TikhonovAlgoAbstractBase::WeightingInSensorSpace("WeightingInSensorSpace");

// Inputs specific from the Tikhonov SVD module
const AlgorithmInputName TikhonovAlgoAbstractBase::matrixU("matrixU");
const AlgorithmInputName TikhonovAlgoAbstractBase::singularValues("singularValues");
const AlgorithmInputName TikhonovAlgoAbstractBase::matrixV("matrixV");

// outputs
const AlgorithmOutputName TikhonovAlgoAbstractBase::InverseSolution("InverseSolution");
const AlgorithmOutputName TikhonovAlgoAbstractBase::RegularizationParameter("RegularizationParameter");
const AlgorithmOutputName TikhonovAlgoAbstractBase::LambdaArray("LambdaArray");
const AlgorithmOutputName TikhonovAlgoAbstractBase::Lambda_Index("Lambda_Index");
const AlgorithmOutputName TikhonovAlgoAbstractBase::RegInverse("RegInverse");

ALGORITHM_PARAMETER_DEF( Inverse, TikhonovImplementation);
ALGORITHM_PARAMETER_DEF( Inverse, RegularizationMethod);
ALGORITHM_PARAMETER_DEF( Inverse, regularizationChoice);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaFromDirectEntry);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaMin);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaMax);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaNum);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaResolution);
ALGORITHM_PARAMETER_DEF( Inverse, LambdaSliderValue);
//ALGORITHM_PARAMETER_DEF( Inverse, LambdaCorner);
//ALGORITHM_PARAMETER_DEF( Inverse, LCurveText);
ALGORITHM_PARAMETER_DEF( Inverse, regularizationSolutionSubcase);
ALGORITHM_PARAMETER_DEF( Inverse, regularizationResidualSubcase);

TikhonovAlgoAbstractBase::TikhonovAlgoAbstractBase()
{
	addParameter(Parameters::TikhonovImplementation, std::string("NoMethodSelected") );
	addOption(Parameters::RegularizationMethod, "lcurve", "single|slider|lcurve");
	addParameter(Parameters::regularizationChoice, 0);
	addParameter(Parameters::LambdaFromDirectEntry,1e-6);
	addParameter(Parameters::LambdaMin,1e-6);
	addParameter(Parameters::LambdaMax,1);
	addParameter(Parameters::LambdaNum,200);
	addParameter(Parameters::LambdaResolution,1e-6);
	addParameter(Parameters::LambdaSliderValue,0);
	addParameter(Parameters::regularizationSolutionSubcase,solution_constrained);
	addParameter(Parameters::regularizationResidualSubcase,residual_constrained);
}

////// CHECK IF INPUT MATRICES HAVE THE CORRECT SIZE
bool TikhonovAlgoAbstractBase::checkInputMatrixSizes( const AlgorithmInput & input) const
{
	auto forwardMatrix_ = input.get<Matrix>(ForwardMatrix);
	auto measuredData_ = input.get<Matrix>(MeasuredPotentials);
	auto sourceWeighting_ = input.get<Matrix>(WeightingInSourceSpace);
	auto sensorWeighting_ = input.get<Matrix>(WeightingInSensorSpace);

  const int M = forwardMatrix_->nrows();
  const int N = forwardMatrix_->ncols();

  // check that rows of fwd matrix equal number of measurements
  if ( M != measuredData_->nrows() )
  {
  	THROW_ALGORITHM_INPUT_ERROR("Input matrix dimensions must agree.");
  	return false;
  }

  // check source regularization matrix sizes
  if (sourceWeighting_)
  {
    if( get(Parameters::regularizationSolutionSubcase).toInt()==solution_constrained )
    {
      // check that the matrix is of appropriate size (equal number of rows as columns in fwd matrix)
      if ( N != sourceWeighting_->ncols() )
      {
    		THROW_ALGORITHM_INPUT_ERROR("Solution Regularization Matrix must have the same number of rows as columns in the Forward Matrix !");
    		return false;
      }
    }
    // otherwise, if the source regularization is provided as the squared version (RR^T)
    else if ( get(Parameters::regularizationSolutionSubcase).toInt()==solution_constrained_squared )
    {
      // check that the matrix is of appropriate size and squared (equal number of rows as columns in fwd matrix)
      if ( ( N != sourceWeighting_->nrows() ) || ( N != sourceWeighting_->ncols() ) )
      {
        THROW_ALGORITHM_INPUT_ERROR("The squared solution Regularization Matrix must have the same number of rows and columns and must be equal to the number of columns in the Forward Matrix !");
	      return false;
      }
    }
  }

  // check measurement regularization matrix sizes
  if (sensorWeighting_)
  {
    if (get(Parameters::regularizationResidualSubcase).toInt() == residual_constrained)
    {
      // check that the matrix is of appropriate size (equal number of rows as rows in fwd matrix)
      if(M != sensorWeighting_->ncols())
      {
        THROW_ALGORITHM_INPUT_ERROR("Data Residual Weighting Matrix must have the same number of rows as the Forward Matrix !");
        return false;
      }
    }
    // otherwise if the source covariance matrix is provided in squared form
    else if  ( get(Parameters::regularizationResidualSubcase).toInt() == residual_constrained_squared )
    {
      // check that the matrix is of appropriate size and squared (equal number of rows as rows in fwd matrix)
      if( (M != sensorWeighting_->nrows()) || (M != sensorWeighting_->ncols()) )
      {
        THROW_ALGORITHM_INPUT_ERROR("Squared data Residual Weighting Matrix must have the same number of rows and columns as number of rows in the Forward Matrix !");
        return false;
      }
    }
  }
  return true;
}

AlgorithmOutput TikhonovAlgoAbstractBase::run(const AlgorithmInput & input) const
{
	auto forwardMatrix = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::ForwardMatrix));
	auto measuredData = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::MeasuredPotentials));
	auto sourceWeighting = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::WeightingInSourceSpace));
	auto sensorWeighting = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::WeightingInSensorSpace));

	auto RegularizationMethod_gotten = getOption(Parameters::RegularizationMethod);
	auto implOption = get(Parameters::TikhonovImplementation).toString();

	// check input MATRICES
	checkInputMatrixSizes( input );

	// Determine specific Tikhonov Implementation
	std::shared_ptr<TikhonovImpl> algoImpl;
	if (implOption == "standardTikhonov")
  {
		int regularizationChoice = get(Parameters::regularizationChoice).toInt();
		int regularizationSolutionSubcase = get(Parameters::regularizationSolutionSubcase).toInt();
		int regularizationResidualSubcase = get(Parameters::regularizationResidualSubcase).toInt();

		algoImpl = std::make_shared<SolveInverseProblemWithStandardTikhonovImpl>( *forwardMatrix, *measuredData, *sourceWeighting, *sensorWeighting,
      regularizationChoice, regularizationSolutionSubcase, regularizationResidualSubcase);
	}
	else if (implOption == "TikhonovSVD")
  {
		// get TikhonovSVD special inputs
		auto matrixU = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::matrixU));
		auto singularValues = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::singularValues));
		auto matrixV = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::matrixV));

		// If there is a missing matrix from the precomputed SVD input
		if (!matrixU || !singularValues || !matrixV)
			algoImpl = std::make_shared<SolveInverseProblemWithTikhonovSVD_impl>(*forwardMatrix, *measuredData, *sourceWeighting, *sensorWeighting);
		else
			algoImpl = std::make_shared<SolveInverseProblemWithTikhonovSVD_impl>(*forwardMatrix, *measuredData, *sourceWeighting, *sensorWeighting, *matrixU, *singularValues, *matrixV);
	}
	else if (implOption == "TSVD")
  {
		// get TikhonovSVD special inputs
		auto matrixU = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::matrixU));
		auto singularValues = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::singularValues));
		auto matrixV = castMatrix::toDense(input.get<Matrix>(TikhonovAlgoAbstractBase::matrixV));

		// If there is a missing matrix from the precomputed SVD input
		if (!matrixU || !singularValues || !matrixV)
			algoImpl = std::make_shared<SolveInverseProblemWithTSVD_impl>(*forwardMatrix, *measuredData, *sourceWeighting, *sensorWeighting);
		else
			algoImpl = std::make_shared<SolveInverseProblemWithTSVD_impl>(*forwardMatrix, *measuredData, *sourceWeighting, *sensorWeighting, *matrixU, *singularValues, *matrixV);
	}
	else
  {
		THROW_ALGORITHM_PROCESSING_ERROR("Not a valid Tikhonov Implementation selection");
	}

  double lambda = 0;
  int lambda_index = 0;
  AlgorithmOutput output;
  DenseMatrixHandle lambdamatrix;
  //Get Regularization parameter(s) : Lambda
  if ((RegularizationMethod_gotten == "single") || (RegularizationMethod_gotten == "slider"))
  {
    if (RegularizationMethod_gotten == "single")
    {
      // Use single fixed lambda value, entered in UI
      lambda = get(Parameters::LambdaFromDirectEntry).toDouble();
    }
    else if (RegularizationMethod_gotten == "slider")
    {
      // Use single fixed lambda value, select via slider
      lambda = get(Parameters::LambdaSliderValue).toDouble();
    }
    lambdamatrix.reset(new DenseMatrix(1,1,lambda));
  }
  else if (RegularizationMethod_gotten == "lcurve")
  {
    lambda = computeLcurve( *algoImpl, input,  lambdamatrix, lambda_index);
  }
	else
	{
		THROW_ALGORITHM_PROCESSING_ERROR("Lambda selection was never set");
	}

  // compute final inverse solution
	auto solution = algoImpl->computeInverseSolution(lambda, true);

	// Set outputs

	output[InverseSolution] = boost::make_shared<DenseMatrix>(solution);
	output[RegularizationParameter] = boost::make_shared<DenseMatrix>(1, 1, lambda);
  output[LambdaArray] = lambdamatrix;
  output[Lambda_Index]= boost::make_shared<DenseMatrix>(1, 1, lambda_index);

	return output;
}

double TikhonovAlgoAbstractBase::computeLcurve( const SCIRun::Core::Algorithms::Inverse::TikhonovImpl& algoImpl, const AlgorithmInput & input , DenseMatrixHandle& lambdamatrix, int& lambda_index) const
{
	// get inputs
	auto forwardMatrix = input.get<Matrix>(TikhonovAlgoAbstractBase::ForwardMatrix);
	auto measuredData = input.get<Matrix>(TikhonovAlgoAbstractBase::MeasuredPotentials);
	auto sourceWeighting = input.get<Matrix>(TikhonovAlgoAbstractBase::WeightingInSourceSpace);
	auto sensorWeighting = input.get<Matrix>(TikhonovAlgoAbstractBase::WeightingInSensorSpace);
  // define the step size of the lambda vector to be computed  (distance between min and max divided by number of desired lambdas in log scale)
  const int nLambda = get(Parameters::LambdaNum).toInt();
	const double lambdaMin = get(Parameters::LambdaMin).toDouble();
	const double lambdaMax = get(Parameters::LambdaMax).toDouble();
	double lambda = 0;

	// prealocate vector of lambdas and eta and rho
  std::vector<double> rho(nLambda, 0.0);
  std::vector<double> eta(nLambda, 0.0);

  lambdamatrix.reset(new DenseMatrix(nLambda,3,0.0));

  auto lambdaArray = algoImpl.computeLambdaArray( lambdaMin, lambdaMax, nLambda );

  DenseMatrix CAx, Rx;
  DenseMatrix solution;

  lambdaArray[0] = lambdaMin;

  // for all lambdas
  for (int j = 0; j < nLambda; j++)
  {
    solution = algoImpl.computeInverseSolution( lambdaArray[j], false);
    lambdamatrix->put(j,0,lambdaArray[j]);

    // if using source regularization matrix, apply it to compute Rx (for the eta computations)
    if (sourceWeighting)
    {
      if (solution.nrows() == sourceWeighting->ncols()) // check that regularization matrix and solution match sizes
      {
        auto sw = castMatrix::toDense(sourceWeighting);
        Rx = (*sw) * solution;
      }
      else
      {
        BOOST_THROW_EXCEPTION(AlgorithmProcessingException() << ErrorMessage(" Solution weighting matrix unexpectedly does not fit to compute the weighted solution norm. "));
      }
    }
    else
      Rx = solution;

    auto forward = castMatrix::toDense(forwardMatrix);
    auto Ax = (*forward) * solution;
    auto measured = castMatrix::toDense(measuredData);
    auto residualSolution = Ax - (*measured);

    // if using source regularization matrix, apply it to compute Rx (for the eta computations)
    if (sensorWeighting)
    {
      auto sw = castMatrix::toDense(sensorWeighting);
      CAx = (*sw) * residualSolution;
    }
    else
      CAx = residualSolution;

    // compute rho and eta. Using Frobenious norm when using matrices
    rho[j] = CAx.norm();
    eta[j] = Rx.norm();
    lambdamatrix->put(j,1,rho[j]);
    lambdamatrix->put(j,2,eta[j]);
  }

  // Find corner in L-curve
  lambda = FindCorner( rho, eta, lambdaArray, nLambda,lambda_index);

	LOG_DEBUG("Lambda: {}", lambda);
  // TODO: update GUI

  return lambda;
}

///// Find Corner, find the maximal curvature which corresponds to the L-curve corner
double TikhonovAlgoAbstractBase::FindCorner( const std::vector<double>& rho, const std::vector<double>& eta, const std::vector<double>& lambdaArray, const int nLambda, int& lambda_index )
{

	DenseColumnMatrix lrho(nLambda);
	DenseColumnMatrix leta(nLambda);

	for (int i = 0; i < nLambda; i++)
	{
		lrho[i] = std::log10(rho[i]);
		leta[i] = std::log10(eta[i]);
	}

	// create L-curve
	DenseMatrix Gamma( 2, lrho.nrows());
	Gamma.row(0) = lrho;
	Gamma.row(1) = leta;//DenseColumnMatrix::LinSpaced(lrho.nrows(),0,1);

	// fit spline and compute curvature
	DenseColumnMatrix kappa = TikhonovAlgoAbstractBase::InterpolateCurvatureWithSplines( Gamma );

	// select maximum curvature
	kappa.maxCoeff(&lambda_index);

  	return lambdaArray[lambda_index];
}

SCIRun::Core::Datatypes::DenseColumnMatrix TikhonovAlgoAbstractBase::InterpolateCurvatureWithSplines( SCIRun::Core::Datatypes::DenseMatrix& samplePoints)
{

	// prealoate
	int numSamples = samplePoints.ncols();
	DenseColumnMatrix kappa = DenseMatrix::Zero( numSamples, 1 );

	// typedefs needed for the spline
	typedef Eigen::Spline<double,2> Spline2d;
	typedef Spline2d::KnotVectorType KnotVectorType;
	typedef Spline2d::ControlPointVectorType ControlPointVectorType;

	// fit cubic spline to data points
	ControlPointVectorType points = samplePoints;
	const Spline2d spline = Eigen::SplineFitting<Spline2d>::Interpolate(points,3);

	// determine position of samples along the spline curve
	KnotVectorType chord_lengths; // knot parameters
	Eigen::ChordLengths(points, chord_lengths);

	for (int i=0; i < numSamples; i++)
	{
		// compute derivatives up to 2nd order
		auto dpt = spline.derivatives( chord_lengths(i), 2);

		// compute curvature as:
		//			abs( ( ddrho*ddeta - ddrho*deta ) /  sqrt(  ( deta^2 + drho^2  )^2)  )
		kappa[i] = std::abs( (dpt(0,2) * dpt(1,2) - dpt(0,2) * dpt(1,1)) /  //compute curvature
	                       std::sqrt( std::pow(dpt(1,1)*dpt(1,1)+dpt(0,1)*dpt(0,1) , 3.0)) );

	}

	// return curvature for all points on the Lcurve
	return kappa;
}

ext: cc

max_stars_repo_name: Haydelj/SCIRun

max_issues_repo_name: Haydelj/SCIRun

max_forks_repo_name: Haydelj/SCIRun


text:
#include "interval.h"
#include "thread_pool.h"
#include <boost/property_tree/xml_parser.hpp>
#include <iostream>
#include <unordered_set>

#define NUM_ARGS 2

void parse_intervals(const char *filename, std::vector<Interval> *intervals);
void parse_primes(std::vector<Interval> *intervals, ThreadPool *tp, std::unordered_set<uint64_t> *primes, std::mutex *mutex);
void _parse_primes_in_interval(Interval interval, std::unordered_set<uint64_t> *primes, std::mutex *mutex);
bool _isprime(int64_t digit);

int main(int argc, char **argv)
{
    if(argc != NUM_ARGS)
    {
        std::cerr << "Usage: " << argv[0] << " <file>\n";
        exit(EXIT_FAILURE);
    }

    std::vector<Interval> intervals;
    parse_intervals(argv[1], &intervals);

    ThreadPool tp;
    std::unordered_set<uint64_t> primes;
    std::mutex mutex;
    parse_primes(&intervals, &tp, &primes, &mutex);
    tp.join_workers();

    std::cout << primes.size() << std::endl;

    return 0;
}

void
parse_intervals(const char *filename, std::vector<Interval> *intervals)
{
    boost::property_tree::ptree pt;
    try {
        boost::property_tree::read_xml(filename, pt);
    } catch(boost::property_tree::xml_parser_error &e) {
        std::cerr << e.what() << std::endl;
        exit(EXIT_FAILURE);
    }
    for(auto &kv : pt.get_child(INTERVALS_TAG))
        intervals->emplace_back(kv.second.get<int64_t>(INTERVALS_LEFT_TAG, 0),
                                kv.second.get<int64_t>(INTERVALS_RIGHT_TAG, 0));
}

void
parse_primes(std::vector<Interval> *intervals,
             ThreadPool *tp,
             std::unordered_set<uint64_t> *primes,
             std::mutex *mutex)
{
    for(auto &interval : *intervals)
        tp->add_task(std::bind(_parse_primes_in_interval, interval, primes, mutex));
}

void
_parse_primes_in_interval(Interval interval,
                          std::unordered_set<uint64_t> *primes,
                          std::mutex *mutex)
{
    for(int64_t digit = interval.left_bound;
        digit < interval.right_bound;
        ++digit)
    {
        if(_isprime(digit))
        {
            std::lock_guard<std::mutex> lock(*mutex);
            primes->insert(digit);
        }
    }
}

bool
_isprime(int64_t digit)
{
    if(digit <= 1)
        return false;
    if(digit <= 3)
        return true;
    for(int64_t j = 2; j*j <= digit; ++j)
        if(digit % j == 0)
            return false;

    return true;
}

ext: cpp

max_stars_repo_name: nsmkhn/hpc

max_issues_repo_name: nsmkhn/hpc

max_forks_repo_name: nsmkhn/hpc


text:
#include "Common/Common.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "Simulation/SimulationModel.h"
#include "Simulation/TimeStepController.h"
#include <iostream>
#include "Demos/Visualization/Visualization.h"
#include "Utils/Logger.h"
#include "Utils/Timing.h"
#include "Utils/FileSystem.h"
#include "Demos/Common/DemoBase.h"
#include "Demos/Common/TweakBarParameters.h"
#include "Simulation/Simulation.h"

// Enable memory leak detection
#if defined(_DEBUG) && !defined(EIGEN_ALIGN)
	#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;
using namespace Utilities;

void timeStep ();
void buildModel ();
void createMesh();
void render ();
void reset();
void TW_CALL setBendingMethod(const void *value, void *clientData);
void TW_CALL getBendingMethod(void *value, void *clientData);
void TW_CALL setSimulationMethod(const void *value, void *clientData);
void TW_CALL getSimulationMethod(void *value, void *clientData);
void TW_CALL setBendingStiffness(const void* value, void* clientData);
void TW_CALL getBendingStiffness(void* value, void* clientData);
void TW_CALL setDistanceStiffness(const void* value, void* clientData);
void TW_CALL getDistanceStiffness(void* value, void* clientData);
void TW_CALL setXXStiffness(const void* value, void* clientData);
void TW_CALL getXXStiffness(void* value, void* clientData);
void TW_CALL setYYStiffness(const void* value, void* clientData);
void TW_CALL getYYStiffness(void* value, void* clientData);
void TW_CALL setXYStiffness(const void* value, void* clientData);
void TW_CALL getXYStiffness(void* value, void* clientData);
void TW_CALL setXYPoissonRatio(const void* value, void* clientData);
void TW_CALL getXYPoissonRatio(void* value, void* clientData);
void TW_CALL setYXPoissonRatio(const void* value, void* clientData);
void TW_CALL getYXPoissonRatio(void* value, void* clientData);
void TW_CALL setNormalizeStretch(const void* value, void* clientData);
void TW_CALL getNormalizeStretch(void* value, void* clientData);
void TW_CALL setNormalizeShear(const void* value, void* clientData);
void TW_CALL getNormalizeShear(void* value, void* clientData);

const int nRows = 50;
const int nCols = 50;
const Real width = 10.0;
const Real height = 10.0;
short simulationMethod = 2;
short bendingMethod = 2;
Real distanceStiffness = 1.0;
Real xxStiffness = 1.0;
Real yyStiffness = 1.0;
Real xyStiffness = 1.0;
Real xyPoissonRatio = 0.3;
Real yxPoissonRatio = 0.3;
bool normalizeStretch = false;
bool normalizeShear = false;
Real bendingStiffness = 0.01;
DemoBase *base;

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

	base = new DemoBase();
	base->init(argc, argv, "Cloth demo");

	SimulationModel *model = new SimulationModel();
	model->init();
	Simulation::getCurrent()->setModel(model);

	buildModel();

	base->createParameterGUI();

	// OpenGL
	MiniGL::setClientIdleFunc (timeStep);
	MiniGL::addKeyFunc('r', reset);
	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport (40.0f, 0.1f, 500.0f, Vector3r (5.0, 10.0, 30.0), Vector3r (5.0, 0.0, 0.0));

	TwType enumType2 = TwDefineEnum("SimulationMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "SimulationMethod", enumType2, setSimulationMethod, getSimulationMethod, &simulationMethod,
		" label='Simulation method' enum='0 {None}, 1 {Distance constraints}, 2 {FEM based PBD}, 3 {Strain based dynamics}, 4 {XPBD distance constraints}' group=Simulation");
	TwType enumType3 = TwDefineEnum("BendingMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "BendingMethod", enumType3, setBendingMethod, getBendingMethod, &bendingMethod,
		" label='Bending method' enum='0 {None}, 1 {Dihedral angle}, 2 {Isometric bending}, 3 {XPBD isometric bending}' group=Bending");
	TwAddVarCB(MiniGL::getTweakBar(), "BendingStiffness", TW_TYPE_REAL, setBendingStiffness, getBendingStiffness, model, " label='Bending stiffness'  min=0.0 step=0.1 precision=4 group='Bending' ");
	TwAddVarCB(MiniGL::getTweakBar(), "DistanceStiffness", TW_TYPE_REAL, setDistanceStiffness, getDistanceStiffness, model, " label='Distance constraint stiffness'  min=0.0 step=0.1 precision=4 group='Cloth' ");
	TwAddVarCB(MiniGL::getTweakBar(), "xxStiffness", TW_TYPE_REAL, setXXStiffness, getXXStiffness, model, " label='xx stiffness'  min=0.0 step=0.1 precision=4 group='Cloth' ");
	TwAddVarCB(MiniGL::getTweakBar(), "yyStiffness", TW_TYPE_REAL, setYYStiffness, getYYStiffness, model, " label='yy stiffness'  min=0.0 step=0.1 precision=4 group='Cloth' ");
	TwAddVarCB(MiniGL::getTweakBar(), "xyStiffness", TW_TYPE_REAL, setXYStiffness, getXYStiffness, model, " label='xy stiffness'  min=0.0 step=0.1 precision=4 group='Cloth' ");
	TwAddVarCB(MiniGL::getTweakBar(), "xyPoissonRatio", TW_TYPE_REAL, setXYPoissonRatio, getXYPoissonRatio, model, " label='xy Poisson ratio'  min=0.0 step=0.1 precision=4 group='Cloth' ");
	TwAddVarCB(MiniGL::getTweakBar(), "yxPoissonRatio", TW_TYPE_REAL, setYXPoissonRatio, getYXPoissonRatio, model, " label='yx Poisson ratio'  min=0.0 step=0.1 precision=4 group='Cloth' ");
	TwAddVarCB(MiniGL::getTweakBar(), "normalizeStretch", TW_TYPE_BOOL32, setNormalizeStretch, getNormalizeStretch, model, " label='Normalize stretch' group='Cloth' ");
	TwAddVarCB(MiniGL::getTweakBar(), "normalizeShear", TW_TYPE_BOOL32, setNormalizeShear, getNormalizeShear, model, " label='Normalize shear' group='Cloth' ");

	MiniGL::mainLoop();

	base->cleanup();

	Utilities::Timing::printAverageTimes();
	Utilities::Timing::printTimeSums();

	delete Simulation::getCurrent();
	delete base;
	delete model;

	return 0;
}

void reset()
{
	Utilities::Timing::printAverageTimes();
	Utilities::Timing::reset();

	Simulation::getCurrent()->reset();
	base->getSelectedParticles().clear();

	Simulation::getCurrent()->getModel()->cleanup();

	buildModel();
}

void timeStep ()
{
	const Real pauseAt = base->getValue<Real>(DemoBase::PAUSE_AT);
	if ((pauseAt > 0.0) && (pauseAt < TimeManager::getCurrent()->getTime()))
		base->setValue(DemoBase::PAUSE, true);

	if (base->getValue<bool>(DemoBase::PAUSE))
		return;

	// Simulation code
	SimulationModel *model = Simulation::getCurrent()->getModel();
	const unsigned int numSteps = base->getValue<unsigned int>(DemoBase::NUM_STEPS_PER_RENDER);
	for (unsigned int i = 0; i < numSteps; i++)
	{
		START_TIMING("SimStep");
		Simulation::getCurrent()->getTimeStep()->step(*model);
		STOP_TIMING_AVG;
	}

	for (unsigned int i = 0; i < model->getTriangleModels().size(); i++)
		model->getTriangleModels()[i]->updateMeshNormals(model->getParticles());
}

void buildModel ()
{
	TimeManager::getCurrent ()->setTimeStepSize (static_cast<Real>(0.005));

	createMesh();
}

void render ()
{
	base->render();
}

/** Create a particle model mesh
*/
void createMesh()
{
	SimulationModel *model = Simulation::getCurrent()->getModel();
	model->addRegularTriangleModel(nCols, nRows,
		Vector3r(0,1,0), AngleAxisr(M_PI*0.5, Vector3r(1,0,0)).matrix(), Vector2r(width, height));

	// Set mass of points to zero => make it static
	ParticleData& pd = model->getParticles();
	pd.setMass(0, 0.0);
	pd.setMass(nRows-1, 0.0);

	// init constraints
	for (unsigned int cm = 0; cm < model->getTriangleModels().size(); cm++)
	{
		distanceStiffness = 1.0;
		if (simulationMethod == 4)
			distanceStiffness = 100000;
		model->addClothConstraints(model->getTriangleModels()[cm], simulationMethod, distanceStiffness, xxStiffness,
			yyStiffness, xyStiffness, xyPoissonRatio, yxPoissonRatio, normalizeStretch, normalizeShear);

		bendingStiffness = 0.01;
		if (bendingMethod == 3)
			bendingStiffness = 100.0;
		model->addBendingConstraints(model->getTriangleModels()[cm], bendingMethod, bendingStiffness);
	}

	LOG_INFO << "Number of triangles: " << model->getTriangleModels()[0]->getParticleMesh().numFaces();
	LOG_INFO << "Number of vertices: " << nRows*nCols;

}

void TW_CALL setBendingMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	*((short*)clientData) = val;
	reset();
}

void TW_CALL getBendingMethod(void *value, void *clientData)
{
	*(short *)(value) = *((short*)clientData);
}

void TW_CALL setSimulationMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	*((short*)clientData) = val;
	reset();
}

void TW_CALL getSimulationMethod(void *value, void *clientData)
{
	*(short *)(value) = *((short*)clientData);
}

void TW_CALL setBendingStiffness(const void* value, void* clientData)
{
	bendingStiffness = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<DihedralConstraint, Real, &DihedralConstraint::m_stiffness>(bendingStiffness);
	((SimulationModel*)clientData)->setConstraintValue<IsometricBendingConstraint, Real, &IsometricBendingConstraint::m_stiffness>(bendingStiffness);
	((SimulationModel*)clientData)->setConstraintValue<IsometricBendingConstraint_XPBD, Real, &IsometricBendingConstraint_XPBD::m_stiffness>(bendingStiffness);
}

void TW_CALL getBendingStiffness(void* value, void* clientData)
{
	*(Real*)(value) = bendingStiffness;
}

void TW_CALL setDistanceStiffness(const void* value, void* clientData)
{
	distanceStiffness = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<DistanceConstraint, Real, &DistanceConstraint::m_stiffness>(distanceStiffness);
	((SimulationModel*)clientData)->setConstraintValue<DistanceConstraint_XPBD, Real, &DistanceConstraint_XPBD::m_stiffness>(distanceStiffness);
}

void TW_CALL getDistanceStiffness(void* value, void* clientData)
{
	*(Real*)(value) = distanceStiffness;
}

void TW_CALL setXXStiffness(const void* value, void* clientData)
{
	xxStiffness = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<FEMTriangleConstraint, Real, &FEMTriangleConstraint::m_xxStiffness>(xxStiffness);
	((SimulationModel*)clientData)->setConstraintValue<StrainTriangleConstraint, Real, &StrainTriangleConstraint::m_xxStiffness>(xxStiffness);
}

void TW_CALL getXXStiffness(void* value, void* clientData)
{
	*(Real*)(value) = xxStiffness;
}

void TW_CALL getYYStiffness(void* value, void* clientData)
{
	*(Real*)(value) = yyStiffness;
}

void TW_CALL setYYStiffness(const void* value, void* clientData)
{
	yyStiffness = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<FEMTriangleConstraint, Real, &FEMTriangleConstraint::m_yyStiffness>(yyStiffness);
	((SimulationModel*)clientData)->setConstraintValue<StrainTriangleConstraint, Real, &StrainTriangleConstraint::m_yyStiffness>(yyStiffness);
}

void TW_CALL getXYStiffness(void* value, void* clientData)
{
	*(Real*)(value) = xyStiffness;
}

void TW_CALL setXYStiffness(const void* value, void* clientData)
{
	xyStiffness = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<FEMTriangleConstraint, Real, &FEMTriangleConstraint::m_xyStiffness>(xyStiffness);
	((SimulationModel*)clientData)->setConstraintValue<StrainTriangleConstraint, Real, &StrainTriangleConstraint::m_xyStiffness>(xyStiffness);
}

void TW_CALL getXYPoissonRatio(void* value, void* clientData)
{
	*(Real*)(value) = xyPoissonRatio;
}

void TW_CALL setXYPoissonRatio(const void* value, void* clientData)
{
	xyPoissonRatio = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<FEMTriangleConstraint, Real, &FEMTriangleConstraint::m_xyPoissonRatio>(xyPoissonRatio);
}

void TW_CALL getYXPoissonRatio(void* value, void* clientData)
{
	*(Real*)(value) = yxPoissonRatio;
}

void TW_CALL setYXPoissonRatio(const void* value, void* clientData)
{
	yxPoissonRatio = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<FEMTriangleConstraint, Real, &FEMTriangleConstraint::m_yxPoissonRatio>(yxPoissonRatio);
}

void TW_CALL getNormalizeStretch(void* value, void* clientData)
{
	*(bool*)(value) = normalizeStretch;
}

void TW_CALL setNormalizeStretch(const void* value, void* clientData)
{
	normalizeStretch = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<StrainTriangleConstraint, bool, &StrainTriangleConstraint::m_normalizeStretch>(normalizeStretch);
}

void TW_CALL getNormalizeShear(void* value, void* clientData)
{
	*(bool*)(value) = normalizeShear;
}

void TW_CALL setNormalizeShear(const void* value, void* clientData)
{
	normalizeShear= *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<StrainTriangleConstraint, bool, &StrainTriangleConstraint::m_normalizeShear>(normalizeShear);
}

max_stars_repo_name: mcx/PositionBasedDynamics

max_issues_repo_name: Taiyuan-Zhang/PositionBasedDynamics

max_forks_repo_name: Taiyuan-Zhang/PositionBasedDynamics


text:
// json parser
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>
namespace pt = boost::property_tree;

#include "GreensTensor.h"

GreensTensor::GreensTensor(double v, double beta) : v(v), beta(beta) {
  assert(v >= 0 && v < 1);
  assert(beta > 0);
}

GreensTensor::GreensTensor(const std::string& input_file) {
  // Create a root
  pt::ptree root;

  // Load the json file in this ptree
  pt::read_json(input_file, root);

  // read parameters
  this->v = root.get<double>("GreensTensor.v");
  assert(v >= 0 && v < 1);
  this->beta = root.get<double>("GreensTensor.beta");
  assert(beta > 0);
}

max_stars_repo_name: QuaCaTeam/quaca

max_issues_repo_name: myoelmy/quaca

max_forks_repo_name: myoelmy/quaca


text:
/*
 *Author:GeneralSandman
 *Code:https://github.com/GeneralSandman/TinyWeb
 *E-mail:generalsandman@163.com
 *Web:www.dissigil.cn
 */

/*---XXX---
 *
 ****************************************
 *
 */

#include <tiny_base/api.h>
#include <tiny_base/log.h>
#include <tiny_base/sync.h>
#include <tiny_core/eventloop.h>
#include <tiny_core/process.h>
#include <tiny_core/processpool.h>
#include <tiny_core/slave.h>
#include <tiny_core/timerid.h>

#include <boost/bind.hpp>
#include <iostream>
#include <string>
#include <memory>
#include <sys/types.h>
#include <unistd.h>

// void Process::childSignalHandler(int);
#include <tiny_base/buffer.h>
void test_child_MessageCallback(Connection* con, Buffer* buf, Time time)
{
    pid_t pid = getpid();
    LOG(Debug) << "[child] (" << pid << ") get message:"
               << buf->getAll() << std::endl;
}

void test_child_CloseCallback(Connection* con)
{
    pid_t pid = getpid();
    LOG(Debug) << "[child] (" << pid << ") connection with parent close"
               << std::endl;
}

void test_child_period_print(void)
{
    pid_t pid = getpid();
    LOG(Debug) << "[child] (" << pid << ") print every second\n";
}

void test_child_period_read_sharememory(Process* process)
{
    pid_t pid = getpid();
    std::string data = process->readFromSharedMemory();
    LOG(Debug) << "[child] (" << pid << ") read share-memory every second,data-size(" << data.size() <<
        "),data(" << data << ")\n";
}

Process* Process::m_pProcessInstance = nullptr;

Process::Process(const std::string& name,
    int number,
    int sockfd[2],
    std::shared_ptr<Sync> sync)
    : m_pEventLoop(new EventLoop())
    , m_pSlave(new Slave(m_pEventLoop, number, name))
    , m_nName(name)
    , m_nNumber(number)
    , m_nPid(getpid())
    , m_nPipe(SocketPair(m_pEventLoop, sockfd))
    , m_pSync(sync)
{
    m_pProcessInstance = this;
    LOG(Debug) << "class Process constructor\n";
}

void Process::setAsChild(int port)
{
    m_nPipe.setChildSocket(port);
    m_nPipe.setMessageCallback(boost::bind(&test_child_MessageCallback, _1, _2, _3));
    m_nPipe.setCloseCallback(boost::bind(&test_child_CloseCallback, _1));
}

void Process::createListenServer(const NetSocketPair& pair)
{
    m_pSlave->createListenServer(pair);
}

void Process::setSignalHandlers()
{
    std::vector<Signal> signals = {
        Signal(SIGINT, "SIGINT", "kill all", childSignalHandler),
        Signal(SIGTERM, "SIGTERM", "kill softly", childSignalHandler),
        Signal(SIGUSR1, "SIGUSR1", "restart", childSignalHandler),
        Signal(SIGUSR2, "SIGUSR2", "reload", childSignalHandler),
        Signal(SIGQUIT, "QIGQUIT", "quit softly", childSignalHandler),
        Signal(SIGPIPE, "SIGPIPE", "socket close", childSignalHandler),
        Signal(SIGHUP, "SIGHUP", "reconfigure", childSignalHandler),
    };

    for (auto t : signals)
        m_nSignalManager.addSignal(t);
}

void Process::start()
{
    m_pEventLoop->runEvery(1, boost::bind(&test_child_period_print));
    m_pEventLoop->runEvery(1, boost::bind(&test_child_period_read_sharememory, this));

    m_nStatus = true;

    while (m_nStatus) {
        m_pSlave->work();

        if (status_terminate || status_quit_softly || status_restart || status_reconfigure)
            m_nStatus = false;
    }
}

pid_t Process::getPid()
{
    return m_nPid;
}

bool Process::started()
{
    return m_nStatus;
}

int Process::join()
{
    // TODO:
    return 0;
}

    void Process::writeToShareMemory(const std::string& data)
    {
        void* address = nullptr;
        m_pSync->sem->lock();
        address = m_pSync->memory->getSpace();
        memcpy(address, (const void*)data.c_str(), data.size());
        m_pSync->sem->unLock();
    }

    std::string Process::readFromSharedMemory(void)
    {
        char* address = nullptr;
        unsigned int len = 0;
        std::string res;

        m_pSync->sem->lock();
        address = (char*)m_pSync->memory->getSpace();

        len = strlen(address);
        res.reserve(len);
        res.assign((const char*)address, len);

        m_pSync->sem->unLock();

        return res;
    }

Process::~Process()
{
    m_nPipe.clearSocket();
    delete m_pSlave;
    delete m_pEventLoop;
    LOG(Debug) << "class Process destructor\n";
}


text:
//头文件
#include <ros/ros.h>

#include <iostream>

//话题头文件

#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/State.h>

#include <Eigen/Eigen>
#include <math.h>
#include <math_utils.h>

using namespace std;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "test");
    ros::NodeHandle nh("~");

    ros::Rate rate(20.0);
    Eigen::Vector3d vector;

    vector       = Eigen::Vector3d(0.0,0.0,0.0);

    //float a;

    Eigen::Vector3d thr_sp;
    float yaw_sp;

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主  循  环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while(ros::ok())
    {

        cout << "Please input the thrust_x: "<<endl;
        cin >> thr_sp(0);
        cout << "Please input the thrust_y: "<<endl;
        cin >> thr_sp(1);
        cout << "Please input the thrust_z: "<<endl;
        cin >> thr_sp(2);
        cout << "Please input the yaw_sp: "<<endl;
        cin >> yaw_sp;

        //Eigen::Quaterniond q_sp = ThrottleToAttitude(thr_sp, yaw_sp);

        //周期休眠
        rate.sleep();
    }

    return 0;

}


text: #include <boost/spirit/home/x3/char/unicode.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
#include <ct/types/opencv.hpp>

#include <Aquila/types/DetectionDescription.hpp>
#include <Aquila/types/ObjectDetection.hpp>
#include <Aquila/types/SyncedMemory.hpp>

#include <MetaObject/core/metaobject_config.hpp>
#include <MetaObject/params/detail/MetaParamImpl.hpp>
#include <MetaObject/python/PythonAllocator.hpp>

#include <ct/reflect/cerealize.hpp>

#include <cereal/types/string.hpp>

#ifdef MO_HAVE_PYTHON
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

namespace mo
{
    namespace python
    {

        template <class T, size_t N>
        inline void convertFromPython(const boost::python::object& obj, ct::ArrayAdapter<T, N> result)
        {
            if (result.ptr)
            {
                for (size_t i = 0; i < N; ++i)
                {
                    boost::python::extract<T> extractor(obj[i]);
                    result.ptr[i] = extractor();
                }
            }
        }
    } // namespace python
} // namespace mo
#endif

#include <MetaObject/python/PythonAllocator.hpp>

namespace cereal
{
    template <class AR>
    void serialize(AR& ar, aq::Category const*& cat)
    {
    }
} // namespace cereal

#if MO_HAVE_PYTHON == 1

namespace ct
{
    template <>
    inline auto convertToPython(const aq::SyncedMemory& obj) -> boost::python::object
    {
        auto host = obj.host();
        return convertToPython(host);
    }

    template <>
    inline auto convertToPython(aq::Category* const& obj) -> boost::python::object
    {
        if (obj)
        {
            return boost::python::object(obj->name);
        }
        else
        {
            return boost::python::object();
        }
    }
} // namespace ct

namespace mo
{
    namespace python
    {

        template <class T, class Enable>
        struct ToPythonDataConverter;

        void registerSetupFunction(std::function<void(void)>&& func);

        template <>
        struct ToPythonDataConverter<aq::SyncedMemory, void>
        {
            ToPythonDataConverter(SystemTable* table, const char* name)
            {
                python::registerSetupFunction(std::bind(&ToPythonDataConverter<aq::SyncedMemory, void>::setup));
            }

            static void setup()
            {
                boost::python::class_<aq::SyncedMemory> bpobj("SyncedMemory");
                bpobj.def("asnumpy",
                          static_cast<boost::python::object (*)(const aq::SyncedMemory&)>(&ct::convertToPython));
            }
        };

        template <class T>
        struct ToPythonDataConverter<aq::TDetectedObjectSet<T>, void>
        {
            ToPythonDataConverter(SystemTable* /*table*/, const char* name)
            {
                python::registerSetupFunction(
                    std::bind(&ToPythonDataConverter<aq::TDetectedObjectSet<T>, void>::setup));
            }

            static void setup()
            {
                std::string name = "DetectedObjectSet<" + TypeTable::instance()->typeToName(TypeInfo(typeid(T))) + ">";
                boost::python::class_<aq::TDetectedObjectSet<T>> bpobj(name.c_str());
                bpobj.def(boost::python::vector_indexing_suite<aq::TDetectedObjectSet<T>>());
            }
        };

        template <>
        struct ToPythonDataConverter<aq::CategorySet, void>
        {
            ToPythonDataConverter(SystemTable* /*table*/, const char* name)
            {
                python::registerSetupFunction(std::bind(&ToPythonDataConverter<aq::CategorySet, void>::setup));
            }

            static void setup()
            {
                boost::python::class_<aq::CategorySet> bpobj("CategorySet");
                bpobj.def(boost::python::vector_indexing_suite<aq::CategorySet>());
            }
        };
    } // namespace python
} // namespace mo
#endif

namespace aq
{

    namespace serialization
    {
        template <class T>
        using vector = std::vector<T>;

        void initModule(SystemTable* table)
        {
            INSTANTIATE_META_PARAM(SyncedMemory, table);
            INSTANTIATE_META_PARAM(vector<SyncedMemory>, table);
            INSTANTIATE_META_PARAM(cv::Mat, table);
            // INSTANTIATE_META_PARAM(vector<cv::Mat>, table);
            INSTANTIATE_META_PARAM(Category, table);
            INSTANTIATE_META_PARAM(Classification, table);
            INSTANTIATE_META_PARAM(DetectedObject, table);
            INSTANTIATE_META_PARAM(DetectedObjectSet, table);
            // INSTANTIATE_META_PARAM(LandmarkDetection, table);
            INSTANTIATE_META_PARAM(CategorySet, table);
            // INSTANTIATE_META_PARAM(DetectionDescription, table);
            // INSTANTIATE_META_PARAM(DetectionDescriptionPatch, table);
            INSTANTIATE_META_PARAM(DetectionDescriptionSet, table);
            INSTANTIATE_META_PARAM(DetectionDescriptionPatchSet, table);
            INSTANTIATE_META_PARAM(LandmarkDetectionSet, table);
        }
    } // namespace serialization
} // namespace aq

max_stars_repo_name: dtmoodie/Acquilla

max_issues_repo_name: dtmoodie/Acquilla

max_forks_repo_name: dtmoodie/Acquilla


text:
/*
 * Copyright (c) 2009-2013 Charlie Wiseman, Jyoti Parwatikar, John DeHart
 * and Washington University in St. Louis
 *
 * 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 <string>
#include <list>
#include <boost/shared_ptr.hpp>

#include "onldb_resp.h"

using namespace onl;

// onldb_resp stuff starts here

onldb_resp::onldb_resp(int res, std::string msg) throw()
{
  r = res;
  m = msg;
}

onldb_resp::~onldb_resp() throw()
{
}

int onldb_resp::result() throw()
{
  return r;
}

std::string onldb_resp::msg() throw()
{
  return m;
}

node_info::node_info() throw()
{
  node_ = "";
  state_ = "";
  has_cp_ = false;
  do_keeboot_ = false;
  cp_ = "";
  cp_port_ = 0;
  type_ = "";
  is_dependent_ = false;
}

node_info::node_info(std::string node, std::string state, bool has_cp, bool do_keeboot, std::string cp, unsigned short cp_port, std::string type, bool is_dependent, std::string cluster) throw()
{
  node_ = node;
  state_ = state;
  has_cp_ = has_cp;
  do_keeboot_ = do_keeboot;
  cp_ = cp;
  cp_port_ = cp_port;
  type_ = type;
  is_dependent_ = is_dependent;
  if (cluster == "NULL")
    cluster_ = "";
  else
    cluster_ = cluster;
}

node_info::node_info(const node_info& ni) throw()
{
  node_ = ni.node_;
  state_ = ni.state_;
  has_cp_ = ni.has_cp_;
  do_keeboot_ = ni.do_keeboot_;
  cp_ = ni.cp_;
  cp_port_ = ni.cp_port_;
  type_ = ni.type_;
  is_dependent_ = ni.is_dependent_;
}

node_info::~node_info() throw()
{
}

node_info&
node_info::operator=(const node_info& ni)
{
  node_ = ni.node_;
  state_ = ni.state_;
  has_cp_ = ni.has_cp_;
  do_keeboot_ = ni.do_keeboot_;
  cp_ = ni.cp_;
  cp_port_ = ni.cp_port_;
  type_ = ni.type_;
  is_dependent_ = ni.is_dependent_;
  cluster_ = ni.cluster_;
  return *this;
}

std::string node_info::node() throw()
{
  return node_;
}

std::string node_info::state() throw()
{
  return state_;
}

std::string node_info::cluster() throw()
{
  return cluster_;
}

bool node_info::has_cp() throw()
{
  return has_cp_;
}

bool node_info::do_keeboot() throw()
{
  return do_keeboot_;
}

std::string node_info::cp() throw()
{
  return cp_;
}

unsigned short node_info::cp_port() throw()
{
  return cp_port_;
}

std::string node_info::type() throw()
{
  return type_;
}

bool
node_info::is_dependent() throw()
{
  return is_dependent_;
}

switch_port_info::switch_port_info() throw()
{
  switch_name = "";
  port = 0;
  interswitch_port = false;
  pass_tag = false;
}

switch_port_info::switch_port_info(std::string sw, unsigned int portnum, bool is_interswitch_port, bool pt) throw()
{
  switch_name = sw;
  port = portnum;
  interswitch_port = is_interswitch_port;
  pass_tag = pt;
}

switch_port_info::switch_port_info(const switch_port_info& spi) throw()
{
  switch_name = spi.switch_name;
  port = spi.port;
  interswitch_port = spi.interswitch_port;
  pass_tag = spi.pass_tag;
}

switch_port_info::~switch_port_info() throw()
{
}

switch_port_info&
switch_port_info::operator=(const switch_port_info& spi)
{
  switch_name = spi.switch_name;
  port = spi.port;
  interswitch_port = spi.interswitch_port;
  pass_tag = spi.pass_tag;
  return *this;
}

switch_info::switch_info() throw()
{
  switch_name = "";
  num_ports = 0;
  mgmt_port = 0;
}

switch_info::switch_info(std::string sw, unsigned int numports, unsigned int mgmtport) throw()
{
  switch_name = sw;
  num_ports = numports;
  mgmt_port = mgmtport;
}

switch_info::switch_info(const switch_info& si) throw()
{
  switch_name = si.switch_name;
  num_ports = si.num_ports;
  mgmt_port = si.mgmt_port;
}

switch_info::~switch_info() throw()
{
}

switch_info&
switch_info::operator=(const switch_info& si)
{
  switch_name = si.switch_name;
  num_ports = si.num_ports;
  mgmt_port = si.mgmt_port;
  return *this;
}

max_stars_repo_name: WU-ARL/ONLdaemons

max_issues_repo_name: WU-ARL/ONLdaemons

max_forks_repo_name: WU-ARL/ONLdaemons


text:
#include <pybind11_ext/boost/optional.hpp>

#include <gtest/gtest.h>
#include <gmock/gmock.h>

#include <boost/optional.hpp>
#include <pybind11/embed.h>

#include <string>

using namespace std;
using namespace testing;
namespace py = pybind11;

namespace boost_optional {

template <typename T, bool has_value_t, T value_t = 0>
struct test_fixture
{
    using type = boost::optional<T>;
    using optional_type = boost::optional<T>;

    static const bool is_none = !has_value_t;
    static const T value = value_t;

    static boost::optional<T> get_value()
    {
        if (is_none)
            return boost::none;

        return T(value);
    }

};

static const char empty[] = "";

template <bool has_value_t, const char* value_t = empty>
struct test_fixture__string
{
    using type = boost::optional<string>;
    using optional_type = boost::optional<string>;

    static const bool is_none = !has_value_t;

    static boost::optional<string> get_value()
    {
        if (is_none)
            return boost::none;

        return string(value_t);
    }

};

static const double zero = 0.0;

template <bool has_value_t, const double& value_t = zero>
struct test_fixture__double
{
    using type = boost::optional<double>;
    using optional_type = boost::optional<double>;

    static const bool is_none = !has_value_t;

    static boost::optional<double> get_value()
    {
        if (is_none)
            return boost::none;

        return value_t;
    }

};

}

template <typename T>
class boost_optional_test : public Test
{ };

TYPED_TEST_CASE_P(boost_optional_test);

TYPED_TEST_P(boost_optional_test, cast_load)
{
    auto original_value = TypeParam::get_value();
    auto is_none = TypeParam::is_none;

    // convert CXX -> Python
    py::object py_value = py::cast(original_value);
    ASSERT_EQ(py_value.is_none(), is_none);

    // convert Python -> CXX
    auto restored_value = py::cast<typename TypeParam::optional_type>(py_value);
    if (is_none)
    {
        ASSERT_FALSE(restored_value);
    }
    else
    {
        ASSERT_TRUE(restored_value);
        ASSERT_EQ(*restored_value, *original_value);
    }
}

REGISTER_TYPED_TEST_CASE_P(boost_optional_test, cast_load);

static constexpr char test_string[] = "abc";
static constexpr double test_double = 78.96;
using test_fixtures = Types<
        boost_optional::test_fixture<int, true, 5>,
        boost_optional::test_fixture<int, false>,
        boost_optional::test_fixture<unsigned int, true, 5>,
        boost_optional::test_fixture<unsigned int, false>,
        boost_optional::test_fixture<long long, true, 5>,
        boost_optional::test_fixture<long long, false>,
        boost_optional::test_fixture<unsigned long long, true, 5>,
        boost_optional::test_fixture<unsigned long long, false>,
        boost_optional::test_fixture__string<true, test_string>,
        boost_optional::test_fixture__string<false>,
        boost_optional::test_fixture__double<true, test_double>,
        boost_optional::test_fixture__double<false>>;
INSTANTIATE_TYPED_TEST_CASE_P(basic, boost_optional_test, test_fixtures);


text:
#ifndef BOOST_NETWORK_PROTOCOL_HTTP_TAGS_HPP_20101019
#define BOOST_NETWORK_PROTOCOL_HTTP_TAGS_HPP_20101019

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

#include <boost/network/tags.hpp>

namespace boost {
namespace network {
namespace http {
namespace tags {

struct http {
  typedef mpl::true_::type is_http;
};
struct keepalive {
  typedef mpl::true_::type is_keepalive;
};
struct simple {
  typedef mpl::true_::type is_simple;
};
struct server {
  typedef mpl::true_::type is_server;
};
struct client {
  typedef mpl::true_::type is_client;
};

using namespace boost::network::tags;

template <class Tag>
struct components;

typedef mpl::vector<http, client, simple, sync, tcp, default_string>
    http_default_8bit_tcp_resolve_tags;
typedef mpl::vector<http, client, simple, sync, udp, default_string>
    http_default_8bit_udp_resolve_tags;
typedef mpl::vector<http, client, keepalive, sync, tcp, default_string>
    http_keepalive_8bit_tcp_resolve_tags;
typedef mpl::vector<http, client, keepalive, sync, udp, default_string>
    http_keepalive_8bit_udp_resolve_tags;
typedef mpl::vector<http, client, simple, async, udp, default_string>
    http_async_8bit_udp_resolve_tags;
typedef mpl::vector<http, client, simple, async, tcp, default_string>
    http_async_8bit_tcp_resolve_tags;
typedef mpl::vector<http, simple, sync, pod, default_string, server>
    http_server_tags;
typedef mpl::vector<http, simple, async, pod, default_string, server>
    http_async_server_tags;

BOOST_NETWORK_DEFINE_TAG(http_default_8bit_tcp_resolve);
BOOST_NETWORK_DEFINE_TAG(http_default_8bit_udp_resolve);
BOOST_NETWORK_DEFINE_TAG(http_keepalive_8bit_tcp_resolve);
BOOST_NETWORK_DEFINE_TAG(http_keepalive_8bit_udp_resolve);
BOOST_NETWORK_DEFINE_TAG(http_async_8bit_udp_resolve);
BOOST_NETWORK_DEFINE_TAG(http_async_8bit_tcp_resolve);
BOOST_NETWORK_DEFINE_TAG(http_server);
BOOST_NETWORK_DEFINE_TAG(http_async_server);

} /* tags */

} /* http */

} /* network */

} /* boost */

#endif /* BOOST_NETWORK_PROTOCOL_HTTP_TAGS_HPP_20101019 */

max_stars_repo_name: ChadSki/Phasor


text:
#include "mtf/SM/PF.h"
#include "mtf/Utilities/miscUtils.h"
#include <boost/random/random_device.hpp>
#include <boost/random/seed_seq.hpp>
#include "opencv2/highgui/highgui.hpp"

//! OpenMP scheduler
#ifndef PF_OMP_SCHD
#define PF_OMP_SCHD auto
#endif

_MTF_BEGIN_NAMESPACE

template <class AM, class SSM>
PF<AM, SSM >::PF(const ParamType *pf_params,
	const AMParams *am_params, const SSMParams *ssm_params) :
	SearchMethod<AM, SSM>(am_params, ssm_params),
	params(pf_params), max_wt_id(0),
	enable_adaptive_resampling(false), min_eff_particles(0){
	printf("\n");
	printf("Using Particle Filter SM with:\n");
	printf("max_iters: %d\n", params.max_iters);
	printf("n_particles: %d\n", params.n_particles);
	printf("epsilon: %f\n", params.epsilon);
	printf("dynamic_model: %s\n", PFParams::toString(params.dynamic_model));
	printf("update_type: %s\n", PFParams::toString(params.update_type));
	printf("likelihood_func: %s\n", PFParams::toString(params.likelihood_func));
	printf("resampling_type: %s\n", PFParams::toString(params.resampling_type));
	printf("adaptive_resampling_thresh: %f\n", params.adaptive_resampling_thresh);
	printf("mean_type: %s\n", PFParams::toString(params.mean_type));
	printf("reset_to_mean: %d\n", params.reset_to_mean);
	ssm_state_size = ssm.getStateSize();
	if(params.pix_sigma.empty() || params.pix_sigma[0] <= 0){
		if(params.ssm_sigma.empty()){
			throw utils::InvalidArgument("Sigma must be provided for at least one sampler");
		}
		using_pix_sigma = false;
	} else{
		using_pix_sigma = true;
		printf("pix_sigma: %f\n", params.pix_sigma[0]);
	}
	printf("measurement_sigma: %f\n", params.measurement_sigma);
	printf("show_particles: %d\n", params.show_particles);
	printf("enable_learning: %d\n", params.enable_learning);
	printf("debug_mode: %d\n", params.debug_mode);
	printf("appearance model: %s\n", am.name.c_str());
	printf("state space model: %s\n", ssm.name.c_str());
	printf("ssm_state_size: %d\n", ssm_state_size);
	printf("\n");

	name = "pf";
	log_fname = "log/pf_debug.txt";
	time_fname = "log/pf_times.txt";
	frame_id = 0;

	const double pi = 3.14159265358979323846;
	measurement_factor = 1.0 / sqrt(2 * pi * params.measurement_sigma);

	for(int set_id = 0; set_id < 2; ++set_id){
		particle_states[set_id].resize(params.n_particles);
		particle_ar[set_id].resize(params.n_particles);
		for(int particle_id = 0; particle_id < params.n_particles; ++particle_id){
			particle_states[set_id][particle_id].resize(ssm_state_size);
			particle_ar[set_id][particle_id].resize(ssm_state_size);
		}
	}
	curr_set_id = 0;
	particle_wts.resize(params.n_particles);
	particle_cum_wts.setZero(params.n_particles);

	perturbed_state.resize(ssm_state_size);
	perturbed_ar.resize(ssm_state_size);
	mean_state.resize(ssm_state_size);
	state_sigma.resize(ssm_state_size);
	state_mean.resize(ssm_state_size);

	if(!using_pix_sigma){
		if(params.ssm_sigma[0].size() < ssm_state_size){
			throw utils::InvalidArgument(
				cv::format("SSM sigma has invalid size: %d",
				params.ssm_sigma[0].size()));
		}
		if(params.ssm_mean.empty()){
			state_mean = VectorXd::Zero(ssm_state_size);
		} else{
			if(params.ssm_mean[0].size() < ssm_state_size){
				throw utils::InvalidArgument(
					cv::format("SSM ssm_mean has invalid size: %d",
					params.ssm_mean[0].size()));
			}
			state_mean = Map<const VectorXd>(params.ssm_mean[0].data(), ssm_state_size);
		}
		state_sigma = Map<const VectorXd>(params.ssm_sigma[0].data(), ssm_state_size);
		utils::printMatrix(state_sigma.transpose(), "state_sigma");
	}
	// initialize random number generators for resampling and measurement function
	boost::random_device r;
	boost::random::seed_seq measurement_seed{ r(), r(), r(), r(), r(), r(), r(), r() };

	measurement_gen = RandGenT(measurement_seed);
	measurement_dist = MeasureDistT(0, params.measurement_sigma);

	boost::random::seed_seq resample_seed{ r(), r(), r(), r(), r(), r(), r(), r() };
	resample_gen = RandGenT(resample_seed);
	resample_dist = ResampleDistT(0, 1);

	if(params.adaptive_resampling_thresh > 0 && params.adaptive_resampling_thresh <= 1){
		printf("Using adaptive resampling\n");
		enable_adaptive_resampling = true;
		min_eff_particles = params.adaptive_resampling_thresh*params.n_particles;
	}

	if(params.debug_mode){
		fclose(fopen(log_fname, "w"));
		resample_ids.resize(params.n_particles);
		uniform_rand_nums.resize(params.n_particles);
	}

#ifdef ENABLE_PARALLEL
	for(int particle_id = 0; particle_id < params.n_particles; particle_id++){
		am_vec.push_back(AMPTr(new AM(am_params)));
		ssm_vec.push_back(SSMPTr(new SSM(ssm_params)));
	}
#endif
#if defined ENABLE_OMP
printf(" ******* Parallelization is enabled using OpenMP ******* \n");
#endif
}

template <class AM, class SSM>
void PF<AM, SSM >::initialize(const cv::Mat &corners){

	ssm.initialize(corners, am.getNChannels());

	if(using_pix_sigma){
		//! estimate SSM parameter sigma from pixel sigma
		ssm.estimateStateSigma(state_sigma, params.pix_sigma[0]);
		state_mean = VectorXd::Zero(ssm_state_size);
	}
	//! initialize SSM sampler with the first distribution
	ssm.initializeSampler(state_sigma, state_mean);

	state_sigma = ssm.getSamplerSigma();
	state_mean = ssm.getSamplerMean();

	am.initializePixVals(ssm.getPts());
	am.initializeSimilarity();
	max_similarity = am.getSimilarity();

	if(params.debug_mode){
		//! print the sigma and mean for the SSM distributions
		utils::printMatrix(state_sigma.transpose(), "state_sigma");
		utils::printMatrix(state_mean.transpose(), "state_mean");
		utils::printScalar(max_similarity, "max_similarity");
	}
	initializeParticles();

	prev_corners = ssm.getCorners();
	ssm.getCorners(cv_corners_mat);
#ifdef ENABLE_PARALLEL
	for(int particle_id = 0; particle_id < params.n_particles; particle_id++){
		ssm_vec[particle_id]->initialize(corners, am.getNChannels());
		ssm_vec[particle_id]->initializeSampler(state_sigma, VectorXd::Zero(ssm_state_size));
		am_vec[particle_id]->initializePixVals(ssm.getPts());
		am_vec[particle_id]->initializeSimilarity();
	}
#endif
}

template <class AM, class SSM>
void PF<AM, SSM >::initializeParticles(){
	double init_wt = 1.0 / params.n_particles;
	for(int particle_id = 0; particle_id < params.n_particles; particle_id++){
		particle_states[curr_set_id][particle_id] = ssm.getState();
		particle_wts[particle_id] = init_wt;
		if(particle_id > 0){
			particle_cum_wts[particle_id] = particle_wts[particle_id] + particle_cum_wts[particle_id - 1];
		} else{
			particle_cum_wts[particle_id] = particle_wts[particle_id];
		}
		particle_ar[curr_set_id][particle_id].setZero();
	}
}

template <class AM, class SSM>
void PF<AM, SSM >::update(){
	++frame_id;
	am.setFirstIter();
	int pause_after_show = 1;
	for(int i = 0; i < params.max_iters; i++){
		if(params.show_particles){
			am.getCurrImg().convertTo(curr_img_uchar, CV_8UC1);
		}
		double max_wt = std::numeric_limits<double>::lowest();
#ifdef ENABLE_OMP
#pragma omp parallel for schedule(PF_OMP_SCHD)
#endif
		for(int particle_id = 0; particle_id < params.n_particles; ++particle_id){
#ifdef ENABLE_PARALLEL
			SSM &ssm = *ssm_vec[particle_id];
			AM &am = *am_vec[particle_id];
#endif
			switch(params.dynamic_model){
			case DynamicModel::AutoRegression1:
				switch(params.update_type){
				case UpdateType::Additive:
					ssm.additiveAutoRegression1(perturbed_state, perturbed_ar,
						particle_states[curr_set_id][particle_id], particle_ar[curr_set_id][particle_id]);
					break;
				case UpdateType::Compositional:
					ssm.compositionalAutoRegression1(perturbed_state, perturbed_ar,
						particle_states[curr_set_id][particle_id], particle_ar[curr_set_id][particle_id]);
					break;
				}
				particle_ar[curr_set_id][particle_id] = perturbed_ar;
				break;
			case DynamicModel::RandomWalk:
				switch(params.update_type){
				case UpdateType::Additive:
					ssm.additiveRandomWalk(perturbed_state, particle_states[curr_set_id][particle_id]);
					break;
				case UpdateType::Compositional:
					ssm.compositionalRandomWalk(perturbed_state, particle_states[curr_set_id][particle_id]);
					break;
				}
				break;
			}
			particle_states[curr_set_id][particle_id] = perturbed_state;

			ssm.setState(particle_states[curr_set_id][particle_id]);
			am.updatePixVals(ssm.getPts());
			am.updateSimilarity(false);

			// a positive number that measures the dissimilarity between the
			// template and the patch corresponding to this particle
			double measuremnt_val = max_similarity - am.getSimilarity();

			// convert this dissimilarity to a likelihood proportional to the dissimilarity
			switch(params.likelihood_func){
			case LikelihoodFunc::AM:
				measurement_likelihood = am.getLikelihood();
				break;
			case LikelihoodFunc::Gaussian:
				measurement_likelihood = measurement_factor * exp(-0.5*measuremnt_val / params.measurement_sigma);
				break;
			case LikelihoodFunc::Reciprocal:
				measurement_likelihood = 1.0 / (1.0 + measuremnt_val);
				break;
			}
			if(params.show_particles){
				cv::Point2d corners[4];
				ssm.getCorners(corners);
				utils::drawCorners(curr_img_uchar, corners,
					cv::Scalar(0, 0, 255), cv::format("%d: %5.3e", particle_id + 1, measurement_likelihood));
				//printf("measurement_likelihood: %e\n", measurement_likelihood);
				if((particle_id + 1) % params.show_particles == 0){
					cv::imshow("Particles", curr_img_uchar);
					int key = cv::waitKey(1 - pause_after_show);
					if(key == 27){
						cv::destroyWindow("Particles");
						params.show_particles = 0;
					} else if(key == 32){
						pause_after_show = 1 - pause_after_show;
					}
					am.getCurrImg().convertTo(curr_img_uchar, CV_8UC1);
				}
			}
			particle_wts[particle_id] = measurement_likelihood;
			if(particle_id > 0){
				particle_cum_wts[particle_id] = particle_wts[particle_id] + particle_cum_wts[particle_id - 1];
			} else{
				particle_cum_wts[particle_id] = particle_wts[particle_id];
			}
			if(particle_wts[particle_id] >= max_wt){
				max_wt = particle_wts[particle_id];
				max_wt_id = particle_id;
			}
		}
		if(params.debug_mode){
			utils::printMatrixToFile(particle_wts.transpose(), "particle_wts", log_fname, "%e");
			utils::printMatrixToFile(particle_cum_wts.transpose(), "particle_cum_wts", log_fname, "%e");
		}
		bool perform_resampling = true;
		if(enable_adaptive_resampling){
			double n_eff_particles = (particle_wts / particle_wts.sum()).squaredNorm();
			n_eff_particles = n_eff_particles == 0 ? 0 :
				1.0 / n_eff_particles;
			if(n_eff_particles > min_eff_particles){
				perform_resampling = false;
			}
		}
		if(perform_resampling){
			switch(params.resampling_type){
			case ResamplingType::None:
				break;
			case ResamplingType::BinaryMultinomial:
				binaryMultinomialResampling();
				break;
			case ResamplingType::LinearMultinomial:
				linearMultinomialResampling();
				break;
			case ResamplingType::Residual:
				residualResampling();
				break;
			}
		}
		switch(params.mean_type){
		case MeanType::None:
			//! set the SSM state to that of the highest weighted particle
			ssm.setState(particle_states[curr_set_id][max_wt_id]);
			break;
		case MeanType::SSM:
			ssm.estimateMeanOfSamples(mean_state, particle_states[curr_set_id], params.n_particles);
			ssm.setState(mean_state);
			break;
		case MeanType::Corners:
			updateMeanCorners();
			ssm.setCorners(mean_corners);
		}

		double update_norm = (prev_corners - ssm.getCorners()).squaredNorm();
		prev_corners = ssm.getCorners();
		if(update_norm < params.epsilon){
			if(params.debug_mode){
				printf("n_iters: %d\n", i + 1);
			}
			break;
		}
		am.clearFirstIter();
	}
	if(params.reset_to_mean){
		initializeParticles();
	}
	if(params.enable_learning){
		am.updateModel(ssm.getPts());
	}
	ssm.getCorners(cv_corners_mat);
}

// uses binary search to find the particle with the smallest
// index whose cumulative weight is greater than the provided
// random number supposedly drawn from a uniform distribution
// between 0 and max cumulative weight
template <class AM, class SSM>
void PF<AM, SSM >::binaryMultinomialResampling(){
	// change the range of the uniform distribution used for resampling instead of normalizing the weights
	//resample_dist.param(ResampleDistParamT(0, particle_cum_wts[params.n_particles - 1]));

	// normalize the cumulative weights and leave the uniform distribution range to (0, 1]
	particle_cum_wts /= particle_cum_wts[params.n_particles - 1];
	if(params.debug_mode){
		utils::printMatrixToFile(particle_cum_wts.transpose(), "normalized particle_cum_wts", log_fname, "%e");
	}
	double max_wt = std::numeric_limits<double>::lowest();
	for(int particle_id = 0; particle_id < params.n_particles; ++particle_id){
		double uniform_rand_num = resample_dist(resample_gen);
		int lower_id = 0, upper_id = params.n_particles - 1;
		int resample_id = (lower_id + upper_id) / 2;
		int iter_id = 0;
		while(upper_id > lower_id){
			if(particle_cum_wts[resample_id] >= uniform_rand_num){
				upper_id = resample_id;
			} else{
				lower_id = resample_id + 1;
			}
			resample_id = (lower_id + upper_id) / 2;
			if(params.debug_mode){
				printf("iter_id: %d upper_id: %d lower_id: %d resample_id: %d\n", iter_id, upper_id,
					lower_id, resample_id);
			}
			++iter_id;
		}

		if(params.debug_mode){
			printf("particle_id: %d resample_id: %d\n", particle_id, resample_id);
			resample_ids[particle_id] = resample_id;
			uniform_rand_nums[particle_id] = uniform_rand_num;
		}

		// place the resampled particle states into the other set so as not to overwrite the current one
		particle_states[1 - curr_set_id][particle_id] = particle_states[curr_set_id][resample_id];
		particle_ar[1 - curr_set_id][particle_id] = particle_ar[curr_set_id][resample_id];
		if(particle_wts[resample_id] >= max_wt){
			max_wt = particle_wts[resample_id];
			max_wt_id = particle_id;
		}
	}
	if(params.debug_mode){
		utils::printMatrixToFile(resample_ids.transpose(), "resample_ids", log_fname, "%d");
		utils::printMatrixToFile(uniform_rand_nums.transpose(), "uniform_rand_nums", log_fname, "%e");
	}
	// make the other particle set the current one
	curr_set_id = 1 - curr_set_id;
}

template <class AM, class SSM>
void PF<AM, SSM >::linearMultinomialResampling(){
	// change the range of the uniform distribution used for resampling instead of normalizing the weights
	//resample_dist.param(ResampleDistParamT(0, particle_cum_wts[params.n_particles - 1]));

	// normalize the cumulative weights and leave the uniform distribution range to (0, 1]
	particle_cum_wts /= particle_cum_wts[params.n_particles - 1];
	if(params.debug_mode){
		utils::printMatrix(particle_cum_wts.transpose(), "normalized particle_cum_wts");
	}
	double max_wt = std::numeric_limits<double>::lowest();
	for(int particle_id = 0; particle_id < params.n_particles; ++particle_id){
		double uniform_rand_num = resample_dist(resample_gen);
		int resample_id = 0;
		while(particle_cum_wts[resample_id] < uniform_rand_num){ ++resample_id; }

		if(params.debug_mode){
			utils::printScalar(uniform_rand_num, "uniform_rand_num");
			utils::printScalar(resample_id, "resample_id", "%d");
		}

		// place the resampled particle states into the other set so as not to overwrite the current one
		particle_states[1 - curr_set_id][particle_id] = particle_states[curr_set_id][resample_id];
		particle_ar[1 - curr_set_id][particle_id] = particle_ar[curr_set_id][resample_id];
		if(particle_wts[resample_id] >= max_wt){
			max_wt = particle_wts[resample_id];
			max_wt_id = particle_id;
		}
	}
	// make the other particle set the current one
	curr_set_id = 1 - curr_set_id;
}

template <class AM, class SSM>
void PF<AM, SSM >::residualResampling() {
	// normalize the weights
	particle_wts /= particle_cum_wts[params.n_particles - 1];
	// vector of particle indies
	VectorXi particle_idx = VectorXi::LinSpaced(params.n_particles, 0, params.n_particles - 1);
	if(params.debug_mode){
		utils::printMatrix(particle_wts.transpose(), "normalized particle_wts");
		utils::printMatrix(particle_idx.transpose(), "particle_idx", "%d");
	}
	// sort, with highest weight first
	std::sort(particle_idx.data(), particle_idx.data() + params.n_particles - 1,
		[&](int a, int b){
		return particle_wts[a] > particle_wts[b];
	});
	if(params.debug_mode){
		utils::printMatrix(particle_idx.transpose(), "sorted particle_idx", "%d");
	}

	// now we append
	int particles_found = 0;
	for(int particle_id = 0; particle_id < params.n_particles; ++particle_id) {
		int resample_id = particle_idx[particle_id];
		int particle_copies = static_cast<int>(round(particle_wts[resample_id] * params.n_particles));
		for(int copy_id = 0; copy_id < particle_copies; ++copy_id) {
			particle_states[1 - curr_set_id][particles_found] = particle_states[curr_set_id][resample_id];
			particle_ar[1 - curr_set_id][particles_found] = particle_ar[curr_set_id][resample_id];
			if(++particles_found == params.n_particles) { break; }
		}
		if(particles_found == params.n_particles) { break; }
	}
	int resample_id = particle_idx[0];
	for(int particle_id = particles_found; particle_id < params.n_particles; ++particle_id) {
		// duplicate particle with highest weight to get exactly same number again
		particle_states[1 - curr_set_id][particle_id] = particle_states[curr_set_id][resample_id];
		particle_ar[1 - curr_set_id][particle_id] = particle_ar[curr_set_id][resample_id];
	}
	curr_set_id = 1 - curr_set_id;
	max_wt_id = particle_idx[0];
}

template <class AM, class SSM>
void PF<AM, SSM >::updateMeanCorners(){
	mean_corners.setZero();
	for(int particle_id = 0; particle_id < params.n_particles; ++particle_id) {
		// compute running average of corners corresponding to the resampled particle states
		ssm.setState(particle_states[curr_set_id][particle_id]);
		mean_corners += (ssm.getCorners() - mean_corners) / (particle_id + 1);
	}
}
template <class AM, class SSM>
void PF<AM, SSM >::setRegion(const cv::Mat& corners){
	ssm.setCorners(corners);
	ssm.getCorners(cv_corners_mat);
	initializeParticles();
}

_MTF_END_NAMESPACE

#ifndef HEADER_ONLY_MODE
#include "mtf/Macros/register.h"
_REGISTER_TRACKERS(PF);
#endif

max_issues_repo_name: siqiyan/MTF

max_forks_repo_name: siqiyan/MTF