text:
#include <iostream>
#include <Eigen/Dense>
#include <string>
#include <cmath>
#include <fstream>
#include <numeric>

// Input: m Vector, k Vector, l Vector (even though it is not needed as we later realized), dimension N
Eigen::ArrayXcd springchain(Eigen::VectorXd& m, Eigen::VectorXd& k, Eigen::VectorXd& l, int N){
    // Initialisation of relative coordinates x (in protocol eta)
    Eigen::VectorXd x(N);
    x(0) = 0.;
    std::partial_sum(l.begin(), l.end(), &x(1), std::plus<double>());

    // Definition of M^-1 and K matrix
    Eigen::MatrixXd M_inv = m.asDiagonal().inverse();
    Eigen::MatrixXd K(N, N);
    for (int i= 0; i < N; i++){
        for(int j= 0; j <= i; j++){
            K(i,j) = 0;
            if(i==j){
                if(i == 0){
                    K(i,i) = k(i);
                    K(i, i+1) = -k(i);
                }
                else if(i== N-1){
                    K(i,i) = k(i-1);
                    K(i, i-1) = -k(i-1);
                }
                else{
                    K(i,i) = k(i-1)+k(i);
                    K(i, i-1) = -k(i-1);
                    K(i, i+1) = -k(i);
                }
            }
        }
    }
    Eigen::MatrixXd A = M_inv*K;
    // Calculate Eigenvalues
    return A.eigenvalues();
}

// Lanczos algorithm, sadly not finished. Returns only the tridiag matrix
Eigen::MatrixXd lanczos(Eigen::MatrixXd& A){

    // Implementation as defined in the lecture

    // q matrix with the new basis q
    Eigen::MatrixXd q(A.rows(), A.cols()+2);
    q(0,0) = 0;
    q(0,1) = 1;
    // gamma vector with all the normalisations
    Eigen::VectorXd gamma(A.cols()+2);
    gamma(1) = 1;
    // delta vector with diagonal elements of the new tridiag matrix
    Eigen::VectorXd delta(A.cols()+1);

    // the new basis build with a for loop
    for (int i=1; i<=A.rows(); i++){
        gamma(i+1) = std::sqrt(q.col(i).transpose()* q.col(i));
        delta(i) = q.col(i).transpose()*A*q.col(i);
        q.col(i+1) = 1/gamma(i+1) * (A- delta(1)*Eigen::MatrixXd::Identity(A.rows(), A.cols()))*q.col(i) - gamma(i)*q.col(i-1);
    }

    // make new tridiag matrix based on gamma-, and delta-vector, with some error
    Eigen::MatrixXd T(A.rows(), A.cols());
    for (int i= 0; i < A.rows()-1; i++){
        T(i,i) = delta(i+1);
        T(i, i+1) = gamma(i+3);
        T(i+1, i) = gamma(i+3);
    }
    T(A.rows()-1, A.cols()-1) = delta(A.cols());

    return T;

    // Begin of Jacobi-Rotation Implementation, sadly not finished
    Eigen::MatrixXd Z = Eigen::MatrixXd::Identity(T.rows(), T.cols());

    for (int i=0; i < T.rows()-1; i++){
        Eigen::JacobiRotation<double> J;
        Eigen::MatrixXd J_1 = Eigen::MatrixXd::Identity(T.rows(), T.cols());
        J.makeJacobi(T(Eigen::seq(i,i+1), Eigen::seq(i,i+1)), 0, 1);
        J_1(Eigen::seq(i,i+1), Eigen::seq(i,i+1)) << J.c(), J.s(), -J.s(), J.c();
        T(Eigen::seq(i,i+1), Eigen::seq(i,i+1)).applyOnTheLeft(0, 1, J.adjoint());
        T(Eigen::seq(i,i+1), Eigen::seq(i,i+1)).applyOnTheRight(0, 1, J);
        Z = Z * J_1;
    }

}

int main()
{
	// Ex. 1
    // N is the dimension, here 10 for part b)
    int N = 10;
    Eigen::VectorXd m(N), k(N-1), l(N-1);
    // Initialisation of m, k and l
    for (int i=0; i<N; i++){
        m(i) = i+1;
    }
    for (int j=0; j<N-1; j++){
        k(j) = N-j-1;
        l(j) = std::abs(5.-j-1) + 1;
    }
    // Calculate omega_squared with springchain function
    Eigen::ArrayXcd omega_squared = springchain(m, k, l, N);
    // Calculate omega with taking the square root of all omega_squared values
    Eigen::ArrayXd omega = omega_squared.real().abs().sqrt();

    // Output in file ./output/omega.txt
    std::ofstream file;
    file.open("output/omega.txt", std::ofstream::trunc);
    file << "\nDas Ergebnis:\n" << omega << std::endl;
    file.close();

    // Ex. 2
    int dim = 5;
    Eigen::MatrixXd A= Eigen::MatrixXd::Random(dim,dim);
    Eigen::MatrixXd T = lanczos(A);

    std::ofstream file2;
    file2.open("output/tridiag.txt", std::ofstream::trunc);
    file2 << "Die Zufallsmatrix A:\n" << A << std::endl;
    file2 << "\nwird zur Tridiagonalmatrix:\n" << T << std::endl;
    file2.close();

    return 0;
}


text:
#pragma once
#ifndef OPENGM_PARTITIONMOVE_HXX
#define OPENGM_PARTITIONMOVE_HXX

#include <algorithm>
#include <vector>
#include <queue>
#include <utility>
#include <string>
#include <iostream>
#include <fstream>
#include <typeinfo>
#include <limits>
#ifdef WITH_BOOST
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>
#else
#include <ext/hash_map>
#include <ext/hash_set>
#endif

#include "opengm/opengm.hxx"
#include "opengm/inference/inference.hxx"
#include "opengm/inference/new_visitors/new_visitors.hxx"

namespace opengm {

/// \brief Partition Move\n\n
/// Currently Partition Move only implements the Kernighan-Lin-Algorithm
///
/// - Cite: B.W. Kernighan and S. Lin, "An efficent heuristic procedure for partition graphs", 1970
/// - Maximum factor order : second order Potts functions only!
/// - Maximum number of labels : same as the number of variables !
/// - Restrictions : see above
/// - Convergent :   Converge to some local fix point
///
template<class GM, class ACC>
class PartitionMove : public Inference<GM, ACC>
{
public:
   typedef ACC AccumulationType;
   typedef GM GraphicalModelType;
   OPENGM_GM_TYPE_TYPEDEFS;
   typedef size_t LPIndexType;
   typedef visitors::VerboseVisitor<PartitionMove<GM, ACC> > VerboseVisitorType;
   typedef visitors::EmptyVisitor<PartitionMove<GM, ACC> >   EmptyVisitorType;
   typedef visitors::TimingVisitor<PartitionMove<GM, ACC> >  TimingVisitorType;
#ifdef WITH_BOOST
   typedef boost::unordered_map<IndexType, LPIndexType> EdgeMapType;
   typedef boost::unordered_set<IndexType>             VariableSetType;
#else
   typedef __gnu_cxx::hash_map<IndexType, LPIndexType> EdgeMapType;
   typedef __gnu_cxx::hash_set<IndexType>              VariableSetType;
#endif

   struct Parameter{
     Parameter ( ) {};
   };

   ~PartitionMove();
   PartitionMove(const GraphicalModelType&, Parameter para=Parameter());
   virtual std::string name() const {return "PartitionMove";}
   const GraphicalModelType& graphicalModel() const {return gm_;}
   virtual InferenceTermination infer();
   template<class VisitorType> InferenceTermination infer(VisitorType&);
   virtual InferenceTermination arg(std::vector<LabelType>&, const size_t = 1) const;

private:
   enum ProblemType {INVALID, MC, MWC};

   const GraphicalModelType& gm_;
   ProblemType problemType_;
   Parameter parameter_;

   LabelType   numberOfTerminals_;
   LPIndexType numberOfInternalEdges_;

   /// For each variable it contains a map indexed by neighbord nodes giving the index to the LP-variable
   /// e.g. neighbours[a][b] = i means a has the neighbour b and the edge has the index i in the linear objective
   std::vector<EdgeMapType >                       neighbours_;
   std::vector<double>                             edgeWeight_;
   double                                          constant_;
   std::vector<LabelType>                          states_;

   template<class VisitorType> InferenceTermination inferKL(VisitorType&);
   double solveBinaryKL(VariableSetType&, VariableSetType&);

};

template<class GM, class ACC>
PartitionMove<GM, ACC>::~PartitionMove() {
   ;
}

template<class GM, class ACC>
PartitionMove<GM, ACC>::PartitionMove
(
   const GraphicalModelType& gm,
   Parameter para
   ) : gm_(gm), parameter_(para)
{
   if(typeid(ACC) != typeid(opengm::Minimizer) || typeid(OperatorType) != typeid(opengm::Adder)) {
      throw RuntimeError("This implementation does only supports Min-Plus-Semiring.");
   }

   //Set Problem Type
   problemType_           = MC;
   numberOfInternalEdges_ = 0;
   numberOfTerminals_     = gm_.numberOfLabels(0);
   for(size_t i=0; i<gm_.numberOfVariables(); ++i){
      if(gm_.numberOfLabels(i)<gm_.numberOfVariables()) {
         problemType_ = MWC;
         numberOfTerminals_ = std::max(numberOfTerminals_ ,gm_.numberOfLabels(i));
      }
   }
   for(size_t f=0; f<gm_.numberOfFactors();++f) {
      if(gm_[f].numberOfVariables()==0) {
         continue;
      }
      else if(gm_[f].numberOfVariables()==1) {
         problemType_ = MWC;
      }
      else if(gm_[f].numberOfVariables()==2) {
         ++numberOfInternalEdges_;
         if(!gm_[f].isPotts()) {
            problemType_ = INVALID;
            break;
         }
      }
      else{
         problemType_ = INVALID;
         break;
      }
   }

   if(problemType_ == INVALID)
      throw RuntimeError("Invalid Model for Multicut-Solver! Solver requires a potts model!");
   if(problemType_ == MWC)
      throw RuntimeError("Invalid Model for Multicut-Solver! Solver currently do not support first order terms!");

   //Calculate Neighbourhood
   neighbours_.resize(gm_.numberOfVariables());
   edgeWeight_.resize(numberOfInternalEdges_,0);
   constant_=0;
   LPIndexType numberOfInternalEdges=0;
   // Add edges that have to be included
   for(size_t f=0; f<gm_.numberOfFactors(); ++f) {
      if(gm_[f].numberOfVariables()==0) {
         const LabelType l=0;
         constant_+=gm_[f](&l);
      }
      else if(gm_[f].numberOfVariables()==2) {
         LabelType cc0[] = {0,0};
         LabelType cc1[] = {0,1};
         edgeWeight_[numberOfInternalEdges] += gm_[f](cc1) - gm_[f](cc0);
         constant_ += gm_[f](cc0);
         IndexType u = gm_[f].variableIndex(0);
         IndexType v = gm_[f].variableIndex(1);
         neighbours_[u][v] = numberOfInternalEdges;
         neighbours_[v][u] = numberOfInternalEdges;
         ++numberOfInternalEdges;
      }
      else{
         throw RuntimeError("Only supports second order Potts functions!");
      }
   }
   OPENGM_ASSERT(numberOfInternalEdges==numberOfInternalEdges_);

   states_.resize(gm_.numberOfVariables(),0);
   size_t init = 2;

   if(init==1){
      for(size_t i=0; i<states_.size();++i){
         states_[i]=rand()%10;
      }
   }

   if(init==2){
      LabelType p=0;
      std::vector<bool> assigned(states_.size(),false);
      for(IndexType node=0; node<states_.size(); ++node) {
         if(assigned[node])
            continue;
         else{
            std::list<IndexType> nodeList;
            states_[node]  = p;
            assigned[node] = true;
            nodeList.push_back(node);
            while(!nodeList.empty()) {
               size_t n=nodeList.front(); nodeList.pop_front();
               for(typename EdgeMapType::const_iterator it=neighbours_[n].begin() ; it != neighbours_[n].end(); ++it) {
                  const IndexType node2 = (*it).first;
                  if(!assigned[node2] && edgeWeight_[(*it).second]>0) {
                     states_[node2] = p;
                     assigned[node2] = true;
                     nodeList.push_back(node2);
                  }
               }
            }
            ++p;
         }
      }
   }

   if(init==3){
      for(size_t i=0; i<states_.size();++i){
         states_[i]=i;
      }
   }

}

template <class GM, class ACC>
InferenceTermination
PartitionMove<GM,ACC>::infer()
{
   EmptyVisitorType visitor;
   return infer(visitor);
}

template <class GM, class ACC>
template<class VisitorType>
InferenceTermination
PartitionMove<GM,ACC>::infer(VisitorType& visitor)
{
   visitor.begin(*this);
   inferKL(visitor);
   visitor.end(*this);
   return NORMAL;
}

template <class GM, class ACC>
template<class VisitorType>
InferenceTermination
PartitionMove<GM,ACC>::inferKL(VisitorType& visitor)
{
   // Current Partition-Sets
   std::vector<VariableSetType> partitionSets;

   // Set-Up Partition-Sets from current/initial partitioning
   LabelType numberOfPartitions =0;
   for(size_t i=0; i<states_.size(); ++i)
      if(states_[i]+1>numberOfPartitions) numberOfPartitions=states_[i]+1;
   partitionSets.resize(numberOfPartitions);
   for(IndexType i=0; i<states_.size(); ++i){
      partitionSets[states_[i]].insert(i);
   }

   bool change = true;
   while(change){
      // std::cout << numberOfPartitions << " conncted subsets."<<std::endl;
      change = false;
      std::vector<size_t> pruneSets;
      // Check all pairs of partitions
      for(size_t part0=0; part0<numberOfPartitions; ++part0){
         //std::cout <<"*"<<std::flush;
         // Find neighbord sets
         std::set<size_t> neighbordSets;
         for(typename VariableSetType::const_iterator it=partitionSets[part0].begin(); it!=partitionSets[part0].end(); ++it){
            const IndexType node = (*it);
            for(typename EdgeMapType::const_iterator nit=neighbours_[node].begin() ; nit != neighbours_[node].end(); ++nit) {
                 const IndexType node2 = (*nit).first;
                 if(states_[node2]>part0){
                    neighbordSets.insert(states_[node2]);
                 }
            }
         }
         for(std::set<size_t>::const_iterator it=neighbordSets.begin(); it!=neighbordSets.end();++it){
            size_t part1 = *it;
            //for(size_t part1=part0+1; part1<numberOfPartitions; ++part1){
            if(partitionSets[part0].size()==0 || partitionSets[part1].size()==0)
               continue;
            double improvement = solveBinaryKL(partitionSets[part0],partitionSets[part1]);
            //std::cout <<part0<<" vs "<<part1<<" : " <<improvement<<std::endl;
            OPENGM_ASSERT(improvement<1e-8);
            if(-1e-8>improvement){
               change = true; // Partition has been improved
            }
         }
      }
      // Check for each Partition ...
      for(size_t part0=0; part0<numberOfPartitions; ++part0){
         // ... if it is empty and can be pruned
         if(partitionSets[part0].size()==0){
            //std::cout <<"Remove "<<part0<<std::endl;
            pruneSets.push_back(part0);
         }
         // ... or if it can be splited into two sets
         else if(partitionSets[part0].size()>1){
            // std::cout <<part0<<" vs "<<"NULL"<<std::endl;

            VariableSetType emptySet(partitionSets[part0].size());
            double improvement = solveBinaryKL(partitionSets[part0], emptySet);
            if(emptySet.size()>0){
               OPENGM_ASSERT(improvement<0);
               partitionSets.push_back(emptySet);
               change = true;
            }
         }
      }
      // Remove sets marked as to prune
      //std::cout << "Remove " <<pruneSets.size() << " subsets."<<std::endl;
      for(size_t i=0; i<pruneSets.size(); ++i){
         size_t part = pruneSets[pruneSets.size()-1-i];
         partitionSets.erase( partitionSets.begin()+part);
      }
      // Update Labeling
      numberOfPartitions = partitionSets.size();
      for(size_t part=0; part<numberOfPartitions; ++part){
         for(typename VariableSetType::const_iterator it=partitionSets[part].begin(); it!=partitionSets[part].end(); ++it){
            states_[*it] = part;
         }
      }
      if( visitor(*this) != visitors::VisitorReturnFlag::ContinueInf ){
         change = false;
      }
   }
   return NORMAL;
}

template <class GM, class ACC>
double PartitionMove<GM,ACC>::solveBinaryKL
(
   VariableSetType& set0,
   VariableSetType& set1
)
{
   double improvement = 0.0;
   //std::cout << "Set0: "<< set0.size() <<" Set1: "<< set1.size() << std::endl;

   for(size_t outerIt=0; outerIt<100;++outerIt){
      // Compute D[n] = E_n - I_n
      std::vector<double> D(gm_.numberOfVariables(),0);
      for(typename VariableSetType::const_iterator it=set0.begin(); it!=set0.end(); ++it){
         double E_a = 0.0;
         double I_a = 0.0;
         const IndexType node = *it;
         for (typename EdgeMapType::const_iterator eit=neighbours_[node].begin(); eit!=neighbours_[node].end(); ++eit){
            const IndexType node2 = (*eit).first;
            const double weight = edgeWeight_[(*eit).second];

            if (set0.find(node2) != set0.end()) {
                I_a += weight;
            }
            else if(set1.find(node2) != set1.end()){
               E_a += weight;
            }
         }
         D[node] = -(E_a - I_a);
      }
      for(typename VariableSetType::const_iterator it=set1.begin(); it!=set1.end(); ++it){
         double E_a = 0.0;
         double I_a = 0.0;
         const IndexType node = *it;
         for(typename EdgeMapType::const_iterator eit=neighbours_[node].begin(); eit!=neighbours_[node].end(); ++eit){
            const IndexType node2 = (*eit).first;
            const double weight = edgeWeight_[(*eit).second];

            if (set1.find(node2) != set1.end()) {
                I_a += weight;
            }
            else if(set0.find(node2) != set0.end()){
               E_a += weight;
            }
         }
         D[node] = -(E_a - I_a);
      }

      double d=0;
      for(size_t i=0; i<D.size(); ++i){
         if(D[i]<d)
            d=D[i];
      }

      // Search a gready move sequence
      std::vector<bool>      isMovedNode(gm_.numberOfVariables(),false);
      std::vector<IndexType> nodeSequence;
      std::vector<double>    improveSequence;
      std::vector<double>    improveSumSequence(1,0.0);
      size_t                 bestMove=0;

      // Build sequence of greedy best moves
      for(size_t innerIt=0; innerIt<1000; ++innerIt){
         double    improve = std::numeric_limits<double>::infinity();
         IndexType node;
         bool      moved = false;
         // Search over moves from set0
         for(typename VariableSetType::const_iterator it=set0.begin(); it!=set0.end(); ++it){
            if(isMovedNode[*it]){
               continue;
            }
            else{
               if(D[*it]<improve){
                  improve = D[*it];
                  node    = *it;
                  moved   = true;
               }
            }
         }
         // Search over moves from set1
         for(typename VariableSetType::const_iterator it=set1.begin(); it!=set1.end(); ++it){
            if(isMovedNode[*it]){
               continue;
            }
            else{
               if(D[*it]<improve){
                  improve = D[*it];
                  node    = *it;
                  moved   = true;
               }
            }
         }

         // No more moves?
         if(moved == false){
            break;
         }

         // Move node and recalculate D
         //std::cout << " " <<improveSumSequence.back()+improve;
         isMovedNode[node]=true;
         nodeSequence.push_back(node);
         improveSumSequence.push_back(improveSumSequence.back()+improve);
         improveSequence.push_back(improve);
         if (improveSumSequence[bestMove]>improveSumSequence.back()) {
            bestMove = improveSumSequence.size()-1;
         }

         VariableSetType& mySet = set0.find(node) != set0.end() ? set0 : set1;
         for(typename EdgeMapType::const_iterator eit=neighbours_[node].begin(); eit!=neighbours_[node].end(); ++eit){
            IndexType node2  = (*eit).first;
            double    weight = edgeWeight_[(*eit).second];
            if(mySet.find(node2) != mySet.end()){
               D[node2] -= 2.0 * weight;
            }
            else{
               D[node2] += 2.0 * weight;
            }

         }
      }

       // Perform Move
      if(improveSumSequence[bestMove]>-1e-10)
         break;
      else{
         improvement += improveSumSequence[bestMove];
         for (size_t i = 0; i < bestMove; ++i) {
            int node = nodeSequence[i];
            if (set0.find(node) != set0.end()) {
               set0.erase(node);
               set1.insert(node);
            }
            else{
               set1.erase(node);
               set0.insert(node);
            }
         }
      }
      // Search for the next move if this move has give improvement
   }
   return improvement;
}

template <class GM, class ACC>
InferenceTermination
PartitionMove<GM,ACC>::arg
(
   std::vector<typename PartitionMove<GM,ACC>::LabelType>& x,
   const size_t N
   ) const
{
   if(N!=1) {
      return UNKNOWN;
   }
   else{
      x.resize(gm_.numberOfVariables());
      for(size_t i=0; i<gm_.numberOfVariables(); ++i)
         x[i] = states_[i];
      return NORMAL;
   }
}

} // end namespace opengm

#endif

ext: hxx

max_stars_repo_name: yanlend/opengm

max_issues_repo_name: yanlend/opengm

max_forks_repo_name: yanlend/opengm


text:
// Copyright 2013, Ji Zhang, Carnegie Mellon University
// Further contributions copyright (c) 2016, Southwest Research Institute
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
// 3. Neither the name of the copyright holder nor the names of its
//    contributors may be used to endorse or promote products derived from this
//    software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// This is an implementation of the algorithm described in the following paper:
//   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
//     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.

#include "loam_velodyne/BasicLaserMapping.h"
#include "loam_velodyne/nanoflann_pcl.h"
#include "math_utils.h"

#include <Eigen/Eigenvalues>
#include <Eigen/QR>

namespace loam
{

using std::sqrt;
using std::fabs;
using std::asin;
using std::atan2;
using std::pow;

BasicLaserMapping::BasicLaserMapping(const float& scanPeriod, const size_t& maxIterations) :
   _scanPeriod(scanPeriod),
   _stackFrameNum(1),
   _mapFrameNum(5),
   _frameCount(0),
   _mapFrameCount(0),
   _maxIterations(maxIterations),
   _deltaTAbort(0.05),
   _deltaRAbort(0.05),
   _laserCloudCenWidth(10),
   _laserCloudCenHeight(5),
   _laserCloudCenDepth(10),
   _laserCloudWidth(21),
   _laserCloudHeight(11),
   _laserCloudDepth(21),
   _laserCloudNum(_laserCloudWidth * _laserCloudHeight * _laserCloudDepth),
   _laserCloudCornerLast(new pcl::PointCloud<pcl::PointXYZI>()),
   _laserCloudSurfLast(new pcl::PointCloud<pcl::PointXYZI>()),
   _laserCloudFullRes(new pcl::PointCloud<pcl::PointXYZI>()),
   _laserCloudCornerStack(new pcl::PointCloud<pcl::PointXYZI>()),
   _laserCloudSurfStack(new pcl::PointCloud<pcl::PointXYZI>()),
   _laserCloudCornerStackDS(new pcl::PointCloud<pcl::PointXYZI>()),
   _laserCloudSurfStackDS(new pcl::PointCloud<pcl::PointXYZI>()),
   _laserCloudSurround(new pcl::PointCloud<pcl::PointXYZI>()),
   _laserCloudSurroundDS(new pcl::PointCloud<pcl::PointXYZI>()),
   _laserCloudCornerFromMap(new pcl::PointCloud<pcl::PointXYZI>()),
   _laserCloudSurfFromMap(new pcl::PointCloud<pcl::PointXYZI>())
{
   // initialize frame counter
   _frameCount = _stackFrameNum - 1;
   _mapFrameCount = _mapFrameNum - 1;

   // setup cloud vectors
   _laserCloudCornerArray.resize(_laserCloudNum);
   _laserCloudSurfArray.resize(_laserCloudNum);
   _laserCloudCornerDSArray.resize(_laserCloudNum);
   _laserCloudSurfDSArray.resize(_laserCloudNum);

   for (size_t i = 0; i < _laserCloudNum; i++)
   {
      _laserCloudCornerArray[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
      _laserCloudSurfArray[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
      _laserCloudCornerDSArray[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
      _laserCloudSurfDSArray[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
   }

   // setup down size filters
   _downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);
   _downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4);
}

void BasicLaserMapping::transformAssociateToMap()
{
   _transformIncre.pos = _transformBefMapped.pos - _transformSum.pos;
   rotateYXZ(_transformIncre.pos, -(_transformSum.rot_y), -(_transformSum.rot_x), -(_transformSum.rot_z));

   float sbcx = _transformSum.rot_x.sin();
   float cbcx = _transformSum.rot_x.cos();
   float sbcy = _transformSum.rot_y.sin();
   float cbcy = _transformSum.rot_y.cos();
   float sbcz = _transformSum.rot_z.sin();
   float cbcz = _transformSum.rot_z.cos();

   float sblx = _transformBefMapped.rot_x.sin();
   float cblx = _transformBefMapped.rot_x.cos();
   float sbly = _transformBefMapped.rot_y.sin();
   float cbly = _transformBefMapped.rot_y.cos();
   float sblz = _transformBefMapped.rot_z.sin();
   float cblz = _transformBefMapped.rot_z.cos();

   float salx = _transformAftMapped.rot_x.sin();
   float calx = _transformAftMapped.rot_x.cos();
   float saly = _transformAftMapped.rot_y.sin();
   float caly = _transformAftMapped.rot_y.cos();
   float salz = _transformAftMapped.rot_z.sin();
   float calz = _transformAftMapped.rot_z.cos();

   float srx = -sbcx * (salx*sblx + calx * cblx*salz*sblz + calx * calz*cblx*cblz)
      - cbcx * sbcy*(calx*calz*(cbly*sblz - cblz * sblx*sbly)
                     - calx * salz*(cbly*cblz + sblx * sbly*sblz) + cblx * salx*sbly)
      - cbcx * cbcy*(calx*salz*(cblz*sbly - cbly * sblx*sblz)
                     - calx * calz*(sbly*sblz + cbly * cblz*sblx) + cblx * cbly*salx);
   _transformTobeMapped.rot_x = -asin(srx);

   float srycrx = sbcx * (cblx*cblz*(caly*salz - calz * salx*saly)
                          - cblx * sblz*(caly*calz + salx * saly*salz) + calx * saly*sblx)
      - cbcx * cbcy*((caly*calz + salx * saly*salz)*(cblz*sbly - cbly * sblx*sblz)
                     + (caly*salz - calz * salx*saly)*(sbly*sblz + cbly * cblz*sblx) - calx * cblx*cbly*saly)
      + cbcx * sbcy*((caly*calz + salx * saly*salz)*(cbly*cblz + sblx * sbly*sblz)
                     + (caly*salz - calz * salx*saly)*(cbly*sblz - cblz * sblx*sbly) + calx * cblx*saly*sbly);
   float crycrx = sbcx * (cblx*sblz*(calz*saly - caly * salx*salz)
                          - cblx * cblz*(saly*salz + caly * calz*salx) + calx * caly*sblx)
      + cbcx * cbcy*((saly*salz + caly * calz*salx)*(sbly*sblz + cbly * cblz*sblx)
                     + (calz*saly - caly * salx*salz)*(cblz*sbly - cbly * sblx*sblz) + calx * caly*cblx*cbly)
      - cbcx * sbcy*((saly*salz + caly * calz*salx)*(cbly*sblz - cblz * sblx*sbly)
                     + (calz*saly - caly * salx*salz)*(cbly*cblz + sblx * sbly*sblz) - calx * caly*cblx*sbly);
   _transformTobeMapped.rot_y = atan2(srycrx / _transformTobeMapped.rot_x.cos(),
                                      crycrx / _transformTobeMapped.rot_x.cos());

   float srzcrx = (cbcz*sbcy - cbcy * sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly * sblx*sblz)
                                                  - calx * calz*(sbly*sblz + cbly * cblz*sblx) + cblx * cbly*salx)
      - (cbcy*cbcz + sbcx * sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz * sblx*sbly)
                                        - calx * salz*(cbly*cblz + sblx * sbly*sblz) + cblx * salx*sbly)
      + cbcx * sbcz*(salx*sblx + calx * cblx*salz*sblz + calx * calz*cblx*cblz);
   float crzcrx = (cbcy*sbcz - cbcz * sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz * sblx*sbly)
                                                  - calx * salz*(cbly*cblz + sblx * sbly*sblz) + cblx * salx*sbly)
      - (sbcy*sbcz + cbcy * cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly * sblx*sblz)
                                        - calx * calz*(sbly*sblz + cbly * cblz*sblx) + cblx * cbly*salx)
      + cbcx * cbcz*(salx*sblx + calx * cblx*salz*sblz + calx * calz*cblx*cblz);
   _transformTobeMapped.rot_z = atan2(srzcrx / _transformTobeMapped.rot_x.cos(),
                                      crzcrx / _transformTobeMapped.rot_x.cos());

   Vector3 v = _transformIncre.pos;
   rotateZXY(v, _transformTobeMapped.rot_z, _transformTobeMapped.rot_x, _transformTobeMapped.rot_y);
   _transformTobeMapped.pos = _transformAftMapped.pos - v;
}

void BasicLaserMapping::transformUpdate()
{
   if (0 < _imuHistory.size())
   {
      size_t imuIdx = 0;

      while (imuIdx < _imuHistory.size() - 1 && toSec(_laserOdometryTime - _imuHistory[imuIdx].stamp) + _scanPeriod > 0)
      {
         imuIdx++;
      }

      IMUState2 imuCur;

      if (imuIdx == 0 || toSec(_laserOdometryTime - _imuHistory[imuIdx].stamp) + _scanPeriod > 0)
      {
         // scan time newer then newest or older than oldest IMU message
         imuCur = _imuHistory[imuIdx];
      }
      else
      {
         float ratio = (toSec(_imuHistory[imuIdx].stamp - _laserOdometryTime) - _scanPeriod)
            / toSec(_imuHistory[imuIdx].stamp - _imuHistory[imuIdx - 1].stamp);

         IMUState2::interpolate(_imuHistory[imuIdx], _imuHistory[imuIdx - 1], ratio, imuCur);
      }

      _transformTobeMapped.rot_x = 0.998 * _transformTobeMapped.rot_x.rad() + 0.002 * imuCur.pitch.rad();
      _transformTobeMapped.rot_z = 0.998 * _transformTobeMapped.rot_z.rad() + 0.002 * imuCur.roll.rad();
   }

   _transformBefMapped = _transformSum;
   _transformAftMapped = _transformTobeMapped;
}

void BasicLaserMapping::pointAssociateToMap(const pcl::PointXYZI& pi, pcl::PointXYZI& po)
{
   po.x = pi.x;
   po.y = pi.y;
   po.z = pi.z;
   po.intensity = pi.intensity;

   rotateZXY(po, _transformTobeMapped.rot_z, _transformTobeMapped.rot_x, _transformTobeMapped.rot_y);

   po.x += _transformTobeMapped.pos.x();
   po.y += _transformTobeMapped.pos.y();
   po.z += _transformTobeMapped.pos.z();
}

void BasicLaserMapping::pointAssociateTobeMapped(const pcl::PointXYZI& pi, pcl::PointXYZI& po)
{
   po.x = pi.x - _transformTobeMapped.pos.x();
   po.y = pi.y - _transformTobeMapped.pos.y();
   po.z = pi.z - _transformTobeMapped.pos.z();
   po.intensity = pi.intensity;

   rotateYXZ(po, -_transformTobeMapped.rot_y, -_transformTobeMapped.rot_x, -_transformTobeMapped.rot_z);
}

void BasicLaserMapping::transformFullResToMap()
{
   // transform full resolution input cloud to map
   for (auto& pt : *_laserCloudFullRes)
      pointAssociateToMap(pt, pt);
}

bool BasicLaserMapping::createDownsizedMap()
{
   // create new map cloud according to the input output ratio
   _mapFrameCount++;
   if (_mapFrameCount < _mapFrameNum)
      return false;

   _mapFrameCount = 0;

   // accumulate map cloud
   _laserCloudSurround->clear();
   for (auto ind : _laserCloudSurroundInd)
   {
      *_laserCloudSurround += *_laserCloudCornerArray[ind];
      *_laserCloudSurround += *_laserCloudSurfArray[ind];
   }

   // down size map cloud
   _laserCloudSurroundDS->clear();
   _downSizeFilterCorner.setInputCloud(_laserCloudSurround);
   _downSizeFilterCorner.filter(*_laserCloudSurroundDS);
   return true;
}

bool BasicLaserMapping::process(Time const& laserOdometryTime)
{
   // skip some frames?!?
   _frameCount++;
   if (_frameCount < _stackFrameNum)
   {
      return false;
   }
   _frameCount = 0;
   _laserOdometryTime = laserOdometryTime;

   pcl::PointXYZI pointSel;

   // relate incoming data to map
   transformAssociateToMap();

   for (auto const& pt : _laserCloudCornerLast->points)
   {
      pointAssociateToMap(pt, pointSel);
      _laserCloudCornerStack->push_back(pointSel);
   }

   for (auto const& pt : _laserCloudSurfLast->points)
   {
      pointAssociateToMap(pt, pointSel);
      _laserCloudSurfStack->push_back(pointSel);
   }

   pcl::PointXYZI pointOnYAxis;
   pointOnYAxis.x = 0.0;
   pointOnYAxis.y = 10.0;
   pointOnYAxis.z = 0.0;
   pointAssociateToMap(pointOnYAxis, pointOnYAxis);

   auto const CUBE_SIZE = 50.0;
   auto const CUBE_HALF = CUBE_SIZE / 2;

   int centerCubeI = int((_transformTobeMapped.pos.x() + CUBE_HALF) / CUBE_SIZE) + _laserCloudCenWidth;
   int centerCubeJ = int((_transformTobeMapped.pos.y() + CUBE_HALF) / CUBE_SIZE) + _laserCloudCenHeight;
   int centerCubeK = int((_transformTobeMapped.pos.z() + CUBE_HALF) / CUBE_SIZE) + _laserCloudCenDepth;

   if (_transformTobeMapped.pos.x() + CUBE_HALF < 0) centerCubeI--;
   if (_transformTobeMapped.pos.y() + CUBE_HALF < 0) centerCubeJ--;
   if (_transformTobeMapped.pos.z() + CUBE_HALF < 0) centerCubeK--;

   while (centerCubeI < 3)
   {
      for (int j = 0; j < _laserCloudHeight; j++)
      {
         for (int k = 0; k < _laserCloudDepth; k++)
         {
            for (int i = _laserCloudWidth - 1; i >= 1; i--)
            {
               const size_t indexA = toIndex(i, j, k);
               const size_t indexB = toIndex(i - 1, j, k);
               std::swap(_laserCloudCornerArray[indexA], _laserCloudCornerArray[indexB]);
               std::swap(_laserCloudSurfArray[indexA], _laserCloudSurfArray[indexB]);
            }
         }
      }
      centerCubeI++;
      _laserCloudCenWidth++;
   }

   while (centerCubeI >= _laserCloudWidth - 3)
   {
      for (int j = 0; j < _laserCloudHeight; j++)
      {
         for (int k = 0; k < _laserCloudDepth; k++)
         {
            for (int i = 0; i < _laserCloudWidth - 1; i++)
            {
               const size_t indexA = toIndex(i, j, k);
               const size_t indexB = toIndex(i + 1, j, k);
               std::swap(_laserCloudCornerArray[indexA], _laserCloudCornerArray[indexB]);
               std::swap(_laserCloudSurfArray[indexA], _laserCloudSurfArray[indexB]);
            }
         }
      }
      centerCubeI--;
      _laserCloudCenWidth--;
   }

   while (centerCubeJ < 3)
   {
      for (int i = 0; i < _laserCloudWidth; i++)
      {
         for (int k = 0; k < _laserCloudDepth; k++)
         {
            for (int j = _laserCloudHeight - 1; j >= 1; j--)
            {
               const size_t indexA = toIndex(i, j, k);
               const size_t indexB = toIndex(i, j - 1, k);
               std::swap(_laserCloudCornerArray[indexA], _laserCloudCornerArray[indexB]);
               std::swap(_laserCloudSurfArray[indexA], _laserCloudSurfArray[indexB]);
            }
         }
      }
      centerCubeJ++;
      _laserCloudCenHeight++;
   }

   while (centerCubeJ >= _laserCloudHeight - 3)
   {
      for (int i = 0; i < _laserCloudWidth; i++)
      {
         for (int k = 0; k < _laserCloudDepth; k++)
         {
            for (int j = 0; j < _laserCloudHeight - 1; j++)
            {
               const size_t indexA = toIndex(i, j, k);
               const size_t indexB = toIndex(i, j + 1, k);
               std::swap(_laserCloudCornerArray[indexA], _laserCloudCornerArray[indexB]);
               std::swap(_laserCloudSurfArray[indexA], _laserCloudSurfArray[indexB]);
            }
         }
      }
      centerCubeJ--;
      _laserCloudCenHeight--;
   }

   while (centerCubeK < 3)
   {
      for (int i = 0; i < _laserCloudWidth; i++)
      {
         for (int j = 0; j < _laserCloudHeight; j++)
         {
            for (int k = _laserCloudDepth - 1; k >= 1; k--)
            {
               const size_t indexA = toIndex(i, j, k);
               const size_t indexB = toIndex(i, j, k - 1);
               std::swap(_laserCloudCornerArray[indexA], _laserCloudCornerArray[indexB]);
               std::swap(_laserCloudSurfArray[indexA], _laserCloudSurfArray[indexB]);
            }
         }
      }
      centerCubeK++;
      _laserCloudCenDepth++;
   }

   while (centerCubeK >= _laserCloudDepth - 3)
   {
      for (int i = 0; i < _laserCloudWidth; i++)
      {
         for (int j = 0; j < _laserCloudHeight; j++)
         {
            for (int k = 0; k < _laserCloudDepth - 1; k++)
            {
               const size_t indexA = toIndex(i, j, k);
               const size_t indexB = toIndex(i, j, k + 1);
               std::swap(_laserCloudCornerArray[indexA], _laserCloudCornerArray[indexB]);
               std::swap(_laserCloudSurfArray[indexA], _laserCloudSurfArray[indexB]);
            }
         }
      }
      centerCubeK--;
      _laserCloudCenDepth--;
   }

   _laserCloudValidInd.clear();
   _laserCloudSurroundInd.clear();
   for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)
   {
      for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)
      {
         for (int k = centerCubeK - 2; k <= centerCubeK + 2; k++)
         {
            if (i >= 0 && i < _laserCloudWidth &&
                j >= 0 && j < _laserCloudHeight &&
                k >= 0 && k < _laserCloudDepth)
            {

               float centerX = 50.0f * (i - _laserCloudCenWidth);
               float centerY = 50.0f * (j - _laserCloudCenHeight);
               float centerZ = 50.0f * (k - _laserCloudCenDepth);

               pcl::PointXYZI transform_pos = (pcl::PointXYZI) _transformTobeMapped.pos;

               bool isInLaserFOV = false;
               for (int ii = -1; ii <= 1; ii += 2)
               {
                  for (int jj = -1; jj <= 1; jj += 2)
                  {
                     for (int kk = -1; kk <= 1; kk += 2)
                     {
                        pcl::PointXYZI corner;
                        corner.x = centerX + 25.0f * ii;
                        corner.y = centerY + 25.0f * jj;
                        corner.z = centerZ + 25.0f * kk;

                        float squaredSide1 = calcSquaredDiff(transform_pos, corner);
                        float squaredSide2 = calcSquaredDiff(pointOnYAxis, corner);

                        float check1 = 100.0f + squaredSide1 - squaredSide2
                           - 10.0f * sqrt(3.0f) * sqrt(squaredSide1);

                        float check2 = 100.0f + squaredSide1 - squaredSide2
                           + 10.0f * sqrt(3.0f) * sqrt(squaredSide1);

                        if (check1 < 0 && check2 > 0)
                        {
                           isInLaserFOV = true;
                        }
                     }
                  }
               }

               size_t cubeIdx = i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k;
               if (isInLaserFOV)
               {
                  _laserCloudValidInd.push_back(cubeIdx);
               }
               _laserCloudSurroundInd.push_back(cubeIdx);
            }
         }
      }
   }

   // prepare valid map corner and surface cloud for pose optimization
   _laserCloudCornerFromMap->clear();
   _laserCloudSurfFromMap->clear();
   for (auto const& ind : _laserCloudValidInd)
   {
      *_laserCloudCornerFromMap += *_laserCloudCornerArray[ind];
      *_laserCloudSurfFromMap += *_laserCloudSurfArray[ind];
   }

   // prepare feature stack clouds for pose optimization
   for (auto& pt : *_laserCloudCornerStack)
      pointAssociateTobeMapped(pt, pt);

   for (auto& pt : *_laserCloudSurfStack)
      pointAssociateTobeMapped(pt, pt);

   // down sample feature stack clouds
   _laserCloudCornerStackDS->clear();
   _downSizeFilterCorner.setInputCloud(_laserCloudCornerStack);
   _downSizeFilterCorner.filter(*_laserCloudCornerStackDS);
   size_t laserCloudCornerStackNum = _laserCloudCornerStackDS->size();

   _laserCloudSurfStackDS->clear();
   _downSizeFilterSurf.setInputCloud(_laserCloudSurfStack);
   _downSizeFilterSurf.filter(*_laserCloudSurfStackDS);
   size_t laserCloudSurfStackNum = _laserCloudSurfStackDS->size();

   _laserCloudCornerStack->clear();
   _laserCloudSurfStack->clear();

   // run pose optimization
   optimizeTransformTobeMapped();

   // store down sized corner stack points in corresponding cube clouds
   for (int i = 0; i < laserCloudCornerStackNum; i++)
   {
      pointAssociateToMap(_laserCloudCornerStackDS->points[i], pointSel);

      int cubeI = int((pointSel.x + CUBE_HALF) / CUBE_SIZE) + _laserCloudCenWidth;
      int cubeJ = int((pointSel.y + CUBE_HALF) / CUBE_SIZE) + _laserCloudCenHeight;
      int cubeK = int((pointSel.z + CUBE_HALF) / CUBE_SIZE) + _laserCloudCenDepth;

      if (pointSel.x + CUBE_HALF < 0) cubeI--;
      if (pointSel.y + CUBE_HALF < 0) cubeJ--;
      if (pointSel.z + CUBE_HALF < 0) cubeK--;

      if (cubeI >= 0 && cubeI < _laserCloudWidth &&
          cubeJ >= 0 && cubeJ < _laserCloudHeight &&
          cubeK >= 0 && cubeK < _laserCloudDepth)
      {
         size_t cubeInd = cubeI + _laserCloudWidth * cubeJ + _laserCloudWidth * _laserCloudHeight * cubeK;
         _laserCloudCornerArray[cubeInd]->push_back(pointSel);
      }
   }

   // store down sized surface stack points in corresponding cube clouds
   for (int i = 0; i < laserCloudSurfStackNum; i++)
   {
      pointAssociateToMap(_laserCloudSurfStackDS->points[i], pointSel);

      int cubeI = int((pointSel.x + CUBE_HALF) / CUBE_SIZE) + _laserCloudCenWidth;
      int cubeJ = int((pointSel.y + CUBE_HALF) / CUBE_SIZE) + _laserCloudCenHeight;
      int cubeK = int((pointSel.z + CUBE_HALF) / CUBE_SIZE) + _laserCloudCenDepth;

      if (pointSel.x + CUBE_HALF < 0) cubeI--;
      if (pointSel.y + CUBE_HALF < 0) cubeJ--;
      if (pointSel.z + CUBE_HALF < 0) cubeK--;

      if (cubeI >= 0 && cubeI < _laserCloudWidth &&
          cubeJ >= 0 && cubeJ < _laserCloudHeight &&
          cubeK >= 0 && cubeK < _laserCloudDepth)
      {
         size_t cubeInd = cubeI + _laserCloudWidth * cubeJ + _laserCloudWidth * _laserCloudHeight * cubeK;
         _laserCloudSurfArray[cubeInd]->push_back(pointSel);
      }
   }

   // down size all valid (within field of view) feature cube clouds
   for (auto const& ind : _laserCloudValidInd)
   {
      _laserCloudCornerDSArray[ind]->clear();
      _downSizeFilterCorner.setInputCloud(_laserCloudCornerArray[ind]);
      _downSizeFilterCorner.filter(*_laserCloudCornerDSArray[ind]);

      _laserCloudSurfDSArray[ind]->clear();
      _downSizeFilterSurf.setInputCloud(_laserCloudSurfArray[ind]);
      _downSizeFilterSurf.filter(*_laserCloudSurfDSArray[ind]);

      // swap cube clouds for next processing
      _laserCloudCornerArray[ind].swap(_laserCloudCornerDSArray[ind]);
      _laserCloudSurfArray[ind].swap(_laserCloudSurfDSArray[ind]);
   }

   transformFullResToMap();
   _downsizedMapCreated = createDownsizedMap();

   return true;
}

void BasicLaserMapping::updateIMU(IMUState2 const& newState)
{
   _imuHistory.push(newState);
}

void BasicLaserMapping::updateOdometry(double pitch, double yaw, double roll, double x, double y, double z)
{
   _transformSum.rot_x = pitch;
   _transformSum.rot_y = yaw;
   _transformSum.rot_z = roll;

   _transformSum.pos.x() = float(x);
   _transformSum.pos.y() = float(y);
   _transformSum.pos.z() = float(z);
}

void BasicLaserMapping::updateOdometry(Twist const& twist)
{
   _transformSum = twist;
}

nanoflann::KdTreeFLANN<pcl::PointXYZI> kdtreeCornerFromMap;
nanoflann::KdTreeFLANN<pcl::PointXYZI> kdtreeSurfFromMap;

void BasicLaserMapping::optimizeTransformTobeMapped()
{
   if (_laserCloudCornerFromMap->size() <= 10 || _laserCloudSurfFromMap->size() <= 100)
      return;

   pcl::PointXYZI pointSel, pointOri, /*pointProj, */coeff;

   std::vector<int> pointSearchInd(5, 0);
   std::vector<float> pointSearchSqDis(5, 0);

   kdtreeCornerFromMap.setInputCloud(_laserCloudCornerFromMap);
   kdtreeSurfFromMap.setInputCloud(_laserCloudSurfFromMap);

   Eigen::Matrix<float, 5, 3> matA0;
   Eigen::Matrix<float, 5, 1> matB0;
   Eigen::Vector3f matX0;
   Eigen::Matrix3f matA1;
   Eigen::Matrix<float, 1, 3> matD1;
   Eigen::Matrix3f matV1;

   matA0.setZero();
   matB0.setConstant(-1);
   matX0.setZero();

   matA1.setZero();
   matD1.setZero();
   matV1.setZero();

   bool isDegenerate = false;
   Eigen::Matrix<float, 6, 6> matP;

   size_t laserCloudCornerStackNum = _laserCloudCornerStackDS->size();
   size_t laserCloudSurfStackNum = _laserCloudSurfStackDS->size();

   for (size_t iterCount = 0; iterCount < _maxIterations; iterCount++)
   {
      _laserCloudOri.clear();
      _coeffSel.clear();

      for (int i = 0; i < laserCloudCornerStackNum; i++)
      {
         pointOri = _laserCloudCornerStackDS->points[i];
         pointAssociateToMap(pointOri, pointSel);
         kdtreeCornerFromMap.nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

         if (pointSearchSqDis[4] < 1.0)
         {
            Vector3 vc(0, 0, 0);

            for (int j = 0; j < 5; j++)
               vc += Vector3(_laserCloudCornerFromMap->points[pointSearchInd[j]]);
            vc /= 5.0;

            Eigen::Matrix3f mat_a;
            mat_a.setZero();

            for (int j = 0; j < 5; j++)
            {
               Vector3 a = Vector3(_laserCloudCornerFromMap->points[pointSearchInd[j]]) - vc;

               mat_a(0, 0) += a.x() * a.x();
               mat_a(0, 1) += a.x() * a.y();
               mat_a(0, 2) += a.x() * a.z();
               mat_a(1, 1) += a.y() * a.y();
               mat_a(1, 2) += a.y() * a.z();
               mat_a(2, 2) += a.z() * a.z();
            }
            matA1 = mat_a / 5.0;

            Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> esolver(matA1);
            matD1 = esolver.eigenvalues().real();
            matV1 = esolver.eigenvectors().real();

            if (matD1(0, 2) > 3 * matD1(0, 1))
            {

               float x0 = pointSel.x;
               float y0 = pointSel.y;
               float z0 = pointSel.z;
               float x1 = vc.x() + 0.1 * matV1(0, 2);
               float y1 = vc.y() + 0.1 * matV1(1, 2);
               float z1 = vc.z() + 0.1 * matV1(2, 2);
               float x2 = vc.x() - 0.1 * matV1(0, 2);
               float y2 = vc.y() - 0.1 * matV1(1, 2);
               float z2 = vc.z() - 0.1 * matV1(2, 2);

               float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                                 * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                                 + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
                                 * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
                                 + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))
                                 * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));

               float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

               float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                           + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;

               float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                            - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

               float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
                            + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

               float ld2 = a012 / l12;

//                // TODO: Why writing to a variable that's never read? Maybe it should be used afterwards?
//                pointProj = pointSel;
//                pointProj.x -= la * ld2;
//                pointProj.y -= lb * ld2;
//                pointProj.z -= lc * ld2;

               float s = 1 - 0.9f * fabs(ld2);

               coeff.x = s * la;
               coeff.y = s * lb;
               coeff.z = s * lc;
               coeff.intensity = s * ld2;

               if (s > 0.1)
               {
                  _laserCloudOri.push_back(pointOri);
                  _coeffSel.push_back(coeff);
               }
            }
         }
      }

      for (int i = 0; i < laserCloudSurfStackNum; i++)
      {
         pointOri = _laserCloudSurfStackDS->points[i];
         pointAssociateToMap(pointOri, pointSel);
         kdtreeSurfFromMap.nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

         if (pointSearchSqDis[4] < 1.0)
         {
            for (int j = 0; j < 5; j++)
            {
               matA0(j, 0) = _laserCloudSurfFromMap->points[pointSearchInd[j]].x;
               matA0(j, 1) = _laserCloudSurfFromMap->points[pointSearchInd[j]].y;
               matA0(j, 2) = _laserCloudSurfFromMap->points[pointSearchInd[j]].z;
            }
            matX0 = matA0.colPivHouseholderQr().solve(matB0);

            float pa = matX0(0, 0);
            float pb = matX0(1, 0);
            float pc = matX0(2, 0);
            float pd = 1;

            float ps = sqrt(pa * pa + pb * pb + pc * pc);
            pa /= ps;
            pb /= ps;
            pc /= ps;
            pd /= ps;

            bool planeValid = true;
            for (int j = 0; j < 5; j++)
            {
               if (fabs(pa * _laserCloudSurfFromMap->points[pointSearchInd[j]].x +
                        pb * _laserCloudSurfFromMap->points[pointSearchInd[j]].y +
                        pc * _laserCloudSurfFromMap->points[pointSearchInd[j]].z + pd) > 0.2)
               {
                  planeValid = false;
                  break;
               }
            }

            if (planeValid)
            {
               float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

               //                // TODO: Why writing to a variable that's never read? Maybe it should be used afterwards?
               //                pointProj = pointSel;
               //                pointProj.x -= pa * pd2;
               //                pointProj.y -= pb * pd2;
               //                pointProj.z -= pc * pd2;

               float s = 1 - 0.9f * fabs(pd2) / sqrt(calcPointDistance(pointSel));

               coeff.x = s * pa;
               coeff.y = s * pb;
               coeff.z = s * pc;
               coeff.intensity = s * pd2;

               if (s > 0.1)
               {
                  _laserCloudOri.push_back(pointOri);
                  _coeffSel.push_back(coeff);
               }
            }
         }
      }

      float srx = _transformTobeMapped.rot_x.sin();
      float crx = _transformTobeMapped.rot_x.cos();
      float sry = _transformTobeMapped.rot_y.sin();
      float cry = _transformTobeMapped.rot_y.cos();
      float srz = _transformTobeMapped.rot_z.sin();
      float crz = _transformTobeMapped.rot_z.cos();

      size_t laserCloudSelNum = _laserCloudOri.size();
      if (laserCloudSelNum < 50)
         continue;

      Eigen::Matrix<float, Eigen::Dynamic, 6> matA(laserCloudSelNum, 6);
      Eigen::Matrix<float, 6, Eigen::Dynamic> matAt(6, laserCloudSelNum);
      Eigen::Matrix<float, 6, 6> matAtA;
      Eigen::VectorXf matB(laserCloudSelNum);
      Eigen::VectorXf matAtB;
      Eigen::VectorXf matX;

      for (int i = 0; i < laserCloudSelNum; i++)
      {
         pointOri = _laserCloudOri.points[i];
         coeff = _coeffSel.points[i];

         float arx = (crx*sry*srz*pointOri.x + crx * crz*sry*pointOri.y - srx * sry*pointOri.z) * coeff.x
            + (-srx * srz*pointOri.x - crz * srx*pointOri.y - crx * pointOri.z) * coeff.y
            + (crx*cry*srz*pointOri.x + crx * cry*crz*pointOri.y - cry * srx*pointOri.z) * coeff.z;

         float ary = ((cry*srx*srz - crz * sry)*pointOri.x
                      + (sry*srz + cry * crz*srx)*pointOri.y + crx * cry*pointOri.z) * coeff.x
            + ((-cry * crz - srx * sry*srz)*pointOri.x
               + (cry*srz - crz * srx*sry)*pointOri.y - crx * sry*pointOri.z) * coeff.z;

         float arz = ((crz*srx*sry - cry * srz)*pointOri.x + (-cry * crz - srx * sry*srz)*pointOri.y)*coeff.x
            + (crx*crz*pointOri.x - crx * srz*pointOri.y) * coeff.y
            + ((sry*srz + cry * crz*srx)*pointOri.x + (crz*sry - cry * srx*srz)*pointOri.y)*coeff.z;

         matA(i, 0) = arx;
         matA(i, 1) = ary;
         matA(i, 2) = arz;
         matA(i, 3) = coeff.x;
         matA(i, 4) = coeff.y;
         matA(i, 5) = coeff.z;
         matB(i, 0) = -coeff.intensity;
      }

      matAt = matA.transpose();
      matAtA = matAt * matA;
      matAtB = matAt * matB;
      matX = matAtA.colPivHouseholderQr().solve(matAtB);

      if (iterCount == 0)
      {
         Eigen::Matrix<float, 1, 6> matE;
         Eigen::Matrix<float, 6, 6> matV;
         Eigen::Matrix<float, 6, 6> matV2;

         Eigen::SelfAdjointEigenSolver< Eigen::Matrix<float, 6, 6> > esolver(matAtA);
         matE = esolver.eigenvalues().real();
         matV = esolver.eigenvectors().real();

         matV2 = matV;

         isDegenerate = false;
         float eignThre[6] = { 100, 100, 100, 100, 100, 100 };
         for (int i = 0; i < 6; i++)
         {
            if (matE(0, i) < eignThre[i])
            {
               for (int j = 0; j < 6; j++)
               {
                  matV2(i, j) = 0;
               }
               isDegenerate = true;
            }
            else
            {
               break;
            }
         }
         matP = matV.inverse() * matV2;
      }

      if (isDegenerate)
      {
         Eigen::Matrix<float, 6, 1> matX2(matX);
         matX = matP * matX2;
      }

      _transformTobeMapped.rot_x += matX(0, 0);
      _transformTobeMapped.rot_y += matX(1, 0);
      _transformTobeMapped.rot_z += matX(2, 0);
      _transformTobeMapped.pos.x() += matX(3, 0);
      _transformTobeMapped.pos.y() += matX(4, 0);
      _transformTobeMapped.pos.z() += matX(5, 0);

      float deltaR = sqrt(pow(rad2deg(matX(0, 0)), 2) +
                          pow(rad2deg(matX(1, 0)), 2) +
                          pow(rad2deg(matX(2, 0)), 2));
      float deltaT = sqrt(pow(matX(3, 0) * 100, 2) +
                          pow(matX(4, 0) * 100, 2) +
                          pow(matX(5, 0) * 100, 2));

      if (deltaR < _deltaRAbort && deltaT < _deltaTAbort)
         break;
   }

   transformUpdate();
}

} // end namespace loam

ext: cpp


text:
// This file is part of comma, a generic and flexible library
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

/// @author vsevolod vlaskine

#ifdef WIN32
#include <stdio.h>
#include <fcntl.h>
#include <io.h>
#else
#include <sys/time.h>
#include <sys/resource.h>
#endif

#include <unordered_map>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include "../../../base/exception.h"
#include "../../../csv/stream.h"
#include "../../../csv/traits.h"
#include "../../../io/file_descriptor.h"
#include "../../../name_value/parser.h"
#include "../../../visiting/traits.h"
#include "split.h"

namespace comma { namespace csv { namespace applications {

template < typename T > struct filename_record
{
    T id;
    std::string filename;
    filename_record( const T& id = 0, const std::string& filename = "" ): id( id ), filename( filename ) {}
};

} } } // namespace comma { namespace csv { namespace applications {

namespace comma { namespace visiting {

template < typename T > struct traits< comma::csv::applications::filename_record< T > >
{
    template< typename K, typename V > static void visit( const K& k, comma::csv::applications::filename_record< T >& t, V& v )
    {
        v.apply( "id", t.id );
        v.apply( "filename", t.filename );
    }

    template< typename K, typename V > static void visit( const K& k, const comma::csv::applications::filename_record< T >& t, V& v )
    {
        v.apply( "id", t.id );
        v.apply( "filename", t.filename );
    }
};

} } // namespace comma { namespace visiting {

namespace comma { namespace csv { namespace applications {

std::pair< std::unordered_map< comma::uint32, std::string >, bool > static filenames( const std::string& filename )
{
    std::pair< std::unordered_map< comma::uint32, std::string >, bool > r;
    r.second = false;
    if( filename.empty() ) { return r; }
    auto csv = comma::name_value::parser( "filename" ).get< comma::csv::options >( filename );
    if( csv.fields.empty() ) { csv.fields = "filename"; }
    std::ifstream ifs( csv.filename );
    if( !ifs.is_open() ) { COMMA_THROW( comma::exception, "could not open --files='" << csv.filename << "'" ); }
    comma::csv::input_stream< filename_record< comma::uint32 > > is( ifs, csv ); // quick and dirty; todo: support templated map
    comma::uint32 id = 0;
    r.second = csv.has_field( "id" );
    while( is.ready() || ifs.good() )
    {
        auto p = is.read();
        if( p == nullptr ) { break; }
        r.first[ r.second ? p->id : id++ ] = p->filename; // quick and dirty
    }
    if( r.first.empty() ) { COMMA_THROW( comma::exception, "got no filenames from '" << csv.filename << "'" ); }
    return r;
}

template < typename T >
split< T >::split( const boost::optional< boost::posix_time::time_duration >& period
                 , const std::string& suffix
                 , const comma::csv::options& csv
                 , bool pass
                 , const std::string& filenames
                 , const std::string& default_filename )
    : ofstream_( std::bind( &split< T >::ofstream_by_time_, this ) )
    , period_( period )
    , suffix_( suffix )
    , pass_ ( pass )
    , flush_( csv.flush )
    , is_shutdown_( false )
{
    if( ( csv.has_field( "t" ) || csv.fields.empty() ) && !period ) { COMMA_THROW( comma::exception, "please specify --period" ); }
    if( csv.fields.empty() ) { return; }
    if( csv.binary() ) { binary_.reset( new comma::csv::binary< input >( csv ) ); }
    else { ascii_.reset( new comma::csv::ascii< input >( csv ) ); }
    boost::tie( filenames_, filenames_have_id_ ) = applications::filenames( filenames );
    if( csv.has_field( "block" ) )
    {
        ofstream_ = std::bind( &split< T >::ofstream_by_block_, this );
    }
    else
    {
        if( csv.has_field( "id" ) )
        {
            ofstream_ = std::bind( &split< T >::ofstream_by_id_, this );
        }
        else
        {
            if( !filenames_.empty() ) { COMMA_THROW( comma::exception, "--files given, but no block field specified in --fields" ); }
        }
    }
}

template < typename T >
split< T >::split( const boost::optional< boost::posix_time::time_duration >& period
                 , const std::string& suffix
                 , const comma::csv::options& csv
                 , const std::vector< std::string >& streams //to-do
                 , bool pass
                 , const std::string& filenames
                 , const std::string& default_filename )
    : split( period, suffix, csv, pass, filenames, default_filename )
{
    if( streams.empty() ) { return; }
    auto const io_mode = csv.binary() ? comma::io::mode::binary : comma::io::mode::ascii;
    for( auto const& si : streams )
    {
        auto const stream_values = comma::split( si, ';' );
        if( 2 > stream_values.size() || stream_values[ 0 ].empty() || stream_values[ 1 ].empty() ) { COMMA_THROW( comma::exception, "please specify <id> and output <stream> in format <id>;<stream>, got: " << si ); }
        transaction t( publishers_ );
        std::unique_ptr< comma::io::publisher > publisher( new comma::io::publisher( stream_values[1], io_mode, false, csv.flush ) );
        if( "..." == stream_values[0] )
        {
            if( default_publisher_ ) { COMMA_THROW( comma::exception, "multiple output streams have the id: ..." ); }
            default_publisher_ = std::move( publisher );
        }
        else
        {
            auto publisher_pos = t->insert( std::move( publisher ) );
            auto const keys = comma::split( stream_values[0], ',' );
            for( auto const& ki : keys )
            {
                auto const kii = boost::lexical_cast< T >( ki );
                if( seen_ids_.end() !=  seen_ids_.find( kii ) ) { COMMA_THROW( comma::exception, "multiple output streams have the id: " << ki ); }
                seen_ids_.insert( kii );
                mapped_publishers_.insert( std::make_pair( kii, publisher_pos.first->get() ) );
            }
        }
    }
    acceptor_thread_ = std::thread( std::bind( &split< T >::accept_, std::ref( *this )));
}

template < typename T >
split< T >::~split()
{
    is_shutdown_ = true;
    if( acceptor_thread_.joinable() )
    {
        acceptor_thread_.join();
        transaction t( publishers_ );
        for( auto& ii : *t ) { ii->close(); }
    }
}

template < typename T >
void split< T >::accept_()
{
    comma::io::select select;
    {
        transaction t( publishers_ );
        for( auto& ii : *t ) { if( ii->acceptor_file_descriptor() != comma::io::invalid_file_descriptor ) { select.read().add( ii->acceptor_file_descriptor() ); } }
        if( default_publisher_ ) { if( default_publisher_->acceptor_file_descriptor() != comma::io::invalid_file_descriptor ) { select.read().add( default_publisher_->acceptor_file_descriptor() ); } }
    }
    while( !is_shutdown_ )
    {
        select.wait( boost::posix_time::millisec( 100 ) ); // arbitrary timeout
        transaction t( publishers_ );
        for( auto& ii : *t ) { if( select.read().ready( ii->acceptor_file_descriptor() ) ) { ii->accept(); } }
        if( default_publisher_ ) { if( select.read().ready( default_publisher_->acceptor_file_descriptor() ) ) { default_publisher_->accept(); } }
    }
}

template < typename T >
bool split< T >::published_on_stream( const char* data, unsigned int size )
{
    transaction t( publishers_ );
    if( t->empty() && !default_publisher_ ) { return false; }

    auto iter = mapped_publishers_.find( current_.id );
    if( mapped_publishers_.end() != iter ) { iter->second->write( data, size, false ); return true; }
    if( default_publisher_ ) { default_publisher_->write( data, size, false ); return true; }
    return true;
}

template < typename T >
void split< T >::write( const char* data, unsigned int size )
{
    mode_ = std::ofstream::out | std::ofstream::binary;
    if( binary_ ) { binary_->get( current_, data ); }
    else { current_.timestamp = boost::get_system_time(); }
    if( !published_on_stream( data, size ) ) // todo? or bind write function on initialisation and call it here?
    {
        auto ofs = ofstream_();
        if( ofs )
        {
            ofs->write( data, size );
            if( flush_ ) { ofs->flush(); }
        }
    }
    if ( pass_ ) { std::cout.write( data, size ); std::cout.flush(); }
}

template < typename T >
void split< T >::write ( std::string line )
{
    mode_ = std::ofstream::out; // quick and dirty
    if( ascii_ ) { ascii_->get( current_, line ); }
    else { current_.timestamp = boost::get_system_time(); }
    line += '\n';
    if( !published_on_stream( &line[0], line.size()) ) // todo? or bind write function on initialisation and call it here?
    {
        auto ofs = ofstream_();
        if( ofs )
        {
            ofs->write( &line[0], line.size() );
            //ofs.put( '\n' );
            if( flush_ ) { ofs->flush(); }
        }
    }
    if ( pass_ ) { std::cout.write( &line[0], line.size() ); /*std::cout.put('\n');*/ std::cout.flush(); }
}

template < typename T >
std::ofstream* split< T >::ofstream_by_time_()
{
    if( !last_ || current_.timestamp > ( last_->timestamp + *period_ ) )
    {
        file_.close();
        std::string time = boost::posix_time::to_iso_string( current_.timestamp );
        if( time.find_first_of( '.' ) == std::string::npos ) { time += ".000000"; }
        file_.open( ( time + suffix_ ).c_str(), mode_ );
        last_ = current_;
    }
    return &file_;
}

template < typename T >
std::ofstream* split< T >::ofstream_by_block_()
{
    static comma::uint32 id = 0;
    if( !last_ || last_->block != current_.block )
    {
        file_.close();
        std::string filename;
        if( !filenames_.empty() )
        {
            auto it = filenames_.find( filenames_have_id_ ? current_.block : id );
            if( it == filenames_.end() ) { return nullptr; }
            filename = it->second;
            const auto& dirname = boost::filesystem::path( filename ).parent_path();
            if( !( dirname.empty() || boost::filesystem::is_directory( dirname ) || boost::filesystem::create_directories( dirname ) ) )
            {
                COMMA_THROW( comma::exception, "failed to create directory '" << dirname << "' for file: '" << filename << "'" );
            }
        }
        if( filename.empty() ) { filename = boost::lexical_cast< std::string >( current_.block ) + suffix_; }
        file_.open( &filename[0], mode_ );
        if( !file_.is_open() ) { COMMA_THROW( comma::exception, "failed to open '" << filename << "'" ); }
        last_ = current_;
        ++id;
    }
    return &file_;
}

template < typename T > static std::string to_string( const T& v ) { return boost::lexical_cast< std::string >( v ); }

template <> std::string to_string< boost::posix_time::ptime >( const boost::posix_time::ptime& v ) { return boost::posix_time::to_iso_string( v ); }

template < typename T, typename M > static std::string find_( const M& m, const T& id ) { COMMA_THROW( comma::exception, "id-to-filename map not implemented for this type" ); }

template <> std::string find_< comma::uint32, std::unordered_map< comma::uint32, std::string > >( const std::unordered_map< comma::uint32, std::string >& m, const comma::uint32& id )
{
    auto it = m.find( id );
    return it == m.end() ? std::string() : it->second;
}

template < typename T >
std::string split< T >::filename_from_id_( const T& id ) { return filenames_.empty() ? to_string( id ) + suffix_ : find_( filenames_, id ); }

template < typename T >
std::ofstream* split< T >::ofstream_by_id_()
{
    typename Files::iterator it = files_.find( current_.id );
    if( it == files_.end() )
    {
        #ifdef WIN32
        static unsigned int max_number_of_open_files = 128;
        #else
        static struct rlimit r;
        static int q = getrlimit( RLIMIT_NOFILE, &r );
        if( q != 0 ) { COMMA_THROW( comma::exception, "getrlimit() failed" ); }
        static unsigned int max_number_of_open_files = static_cast< unsigned int >( r.rlim_cur );
        #endif
        if( files_.size() + 10 > max_number_of_open_files ) { files_.clear(); } // quick and dirty, may be too drastic...
        std::ios_base::openmode mode = mode_;
        if( seen_ids_.find( current_.id ) == seen_ids_.end() ) { seen_ids_.insert( current_.id ); }
        else { mode |= std::ofstream::app; }
        std::string name = filename_from_id_( current_.id );
        if( name.empty() ) { return nullptr; }
        const auto& dirname = boost::filesystem::path( name ).parent_path();
        if( !( dirname.empty() || boost::filesystem::is_directory( dirname ) || boost::filesystem::create_directories( dirname ) ) )
        {
            COMMA_THROW( comma::exception, "failed to create directory '" << dirname << "' for file: '" << name << "'" );
        }
        std::shared_ptr< std::ofstream > stmp( new std::ofstream( &name[0], mode ) );
        it = files_.insert( std::make_pair( current_.id, stmp ) ).first;
    }
    return it->second.get();
}

template class split< comma::uint32 >;
template class split< std::string >;
template class split< boost::posix_time::ptime >;

} } } // namespace comma { namespace csv { namespace applications {

max_stars_repo_name: mission-systems-pty-ltd/comma

max_issues_repo_name: mission-systems-pty-ltd/comma

max_forks_repo_name: mission-systems-pty-ltd/comma


text:
#include <cmath>
#include <iostream>
#include <ale_interface.hpp>
#include <glog/logging.h>
#include <boost/program_options.hpp>
//#include "prettyprint.hpp"
#include "../include/agent.hpp"
#include "../include/tools.hpp"

/*
  DeepQLearner(
    const ActionVect& legal_actions,
    double epsilon_start,
    const double epsilon_min,
    const double epsilon_decay,
    const int epsilon_explore_idx,
    const int replay_batch_size,
    const int replay_memory_capacity,
    const int replay_start_size,
    const std::string sampleStrategy,
    const int update_frequency,
    const double discount_factor,
    const std::string solver_param
    ):
*/

using namespace deepRL;
namespace po = boost::program_options;
double EpisodeLearning( ALEInterface& ale, deepRL::DeepQLearner& dqlearner, const bool update);
po::variables_map argmap;

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

  po::options_description desc("Valid options");
  desc.add_options()
  ("help,h", "Help message")
  ("gpu,u", po::value<bool>()->default_value(false), "Use GPU to brew Caffe")
	("gui,i", po::value<bool>()->default_value(false), "Open a GUI window")
  ("rom,o", po::value<std::string>()->default_value("./roms/breakout.bin"), "Rom file to play")
	("epsilon_start,s", po::value<double>()->default_value(1.0), "Epsilon start value for action selection")
	("epsilon_min,m", po::value<double>()->default_value(0.1), "The final epsilon")
	("epsilon_decay,d", po::value<double>()->default_value(0.98), "Epsilon decay ratio")
	("epsilon_explore_idx,x", po::value<int>()->default_value(100000), "Number of iterations needed for epsilon to reach epsilon_min")
	("replay_memory_capacity,c", po::value<int>()->default_value(50000), "Capacity of replay memory")
	("replay_start_size,r", po::value<int>()->default_value(500), "UEnough amount of transitions to start learning")
	("sampleStrategy,y", po::value<std::string>()->default_value("uniform")->required(), "Sampling strategy for transition")
	("update_frequency,f", po::value<int>()->default_value(1000), "Every #update_frequency set target_net_ = net_")
	("discount_factor,n", po::value<double>()->default_value(0.98), "Discount factor of future rewards (0,1]")
	("solver,v", po::value<std::string>()->default_value("./prototxt/aleSolver.prototxt")->required(), "Solver parameter file (*.prototxt)")
	("skip_frame,p", po::value<int>()->default_value(3), "Number of frames skipped")
	("model,l", po::value<std::string>()->default_value("")->required(), "Model file to load")
	("evaluate,u", po::value<bool>()->default_value(false), "Evaluation mode: only playing a game, no updates")
	("eval_epsilon,k", po::value<double>()->default_value(0.05), "Epsilon used in evaluate mode")
	("target_q_freq,g", po::value<int>()->default_value(1000), "Taregt_q_net_ update frequency")
  ("doubleQ,q", po::value<bool>()->default_value(false), "Enable double-Q learning");

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

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

  google::InitGoogleLogging(argv[0]);
  google::InstallFailureSignalHandler();
  google::LogToStderr();

  if (argmap["gpu"].as<bool>()) {
    caffe::Caffe::set_mode(caffe::Caffe::GPU);
  } else {
    caffe::Caffe::set_mode(caffe::Caffe::CPU);
  }

  ALEInterface ale(argmap["gui"].as<bool>());
  std::cout << "Init ale environment ok!" << std::endl;

  // Load the ROM file
  ale.loadROM(argmap["rom"].as<std::string>());

  std::cout << "Loading rom: " << argmap["rom"].as<std::string>() << " ok!" << std::endl;

  // Get the vector of legal actions
  const auto legal_actions = ale.getMinimalActionSet();

  std::cout << "Action space: " << legal_actions.size() << std::endl;

  // deepqlearner
  deepRL::DeepQLearner dqlearner(legal_actions,
				argmap["epsilon_start"].as<double>(),
				argmap["epsilon_min"].as<double>(),
				argmap["epsilon_decay"].as<double>(),
				argmap["epsilon_explore_idx"].as<int>(),
				deepRL::kMinibatchSize,
				argmap["replay_memory_capacity"].as<int>(),
				argmap["replay_start_size"].as<int>(),
				argmap["sampleStrategy"].as<std::string>(),
				argmap["update_frequency"].as<int>(),
				argmap["discount_factor"].as<double>(),
				argmap["solver"].as<std::string>(),
				argmap["evaluate"].as<bool>(),
        argmap["eval_epsilon"].as<double>(),
        argmap["target_q_freq"].as<int>(),
        argmap["doubleQ"].as<bool>());

  dqlearner.Initialize();

  if (argmap["model"].as<std::string>() !="null") {
    // Just evaluate the given trained model
    dqlearner.LoadPretrainedModel(argmap["model"].as<std::string>());
    std::cout << "Loading " << argmap["model"].as<std::string>() << std::endl;
  }
  //evaluate mode
  if (argmap["evaluate"].as<bool>()) {
    auto total_score = 0.0;
      const auto score =
          EpisodeLearning(ale, dqlearner, false);
      std::cout << "score: " << score << std::endl;
    return 0;
  }

  // learning mode
  for (auto episode = 0;; episode++) {
    dqlearner.Reset();
    const auto score = EpisodeLearning(ale, dqlearner, true);
    std::cout << "Episode: " << episode << " ,Score: " << score << std::endl;
    if (episode % 10 == 0) {
      // After every 10 episodes, evaluate the current strength
      const auto eval_score = EpisodeLearning(ale, dqlearner,false);
      std::cout << "evaluation score: " << eval_score << std::endl;
    }
  }
}

/**
 * one episode learning and return the total score
 */
double EpisodeLearning( ALEInterface& ale, deepRL::DeepQLearner& dqlearner, const bool update) {
  assert(!ale.game_over());
  std::deque<deepRL::FrameDataSp> past_frames;
  //dqlearner.replay_memory_.resetPool();

  auto total_score = 0.0;
  for (auto frame = 0; !ale.game_over(); ++frame) {
    //std::cout << "frame: " << frame << std::endl;
    const auto current_frame = deepRL::PreprocessScreen(ale.getScreen());

    past_frames.push_back(current_frame);
    if (past_frames.size() < deepRL::kInputFrameCount) {
      // If there are not past frames enough for DQN input, just select NOOP
      for (auto i = 0; i < argmap["skip_frame"].as<int>() + 1 && !ale.game_over(); ++i) {
        total_score += ale.act(PLAYER_A_NOOP);
      }
    } else {
      if (past_frames.size() > deepRL::kInputFrameCount) {
        past_frames.pop_front();
      }
      deepRL::InputFrames input_frames;
      std::copy(past_frames.begin(), past_frames.end(), input_frames.begin());
      const auto action = dqlearner.SelectAction(input_frames);
      auto immediate_score = 0.0;

      for (auto i = 0; i < argmap["skip_frame"].as<int>() + 1 && !ale.game_over(); ++i) {
        // Last action is repeated on skipped frames
        immediate_score += ale.act(action);
      }

      total_score += immediate_score;

      //clip reward for robust gradient update
      // Rewards for DQN are normalized as follows:
      // 1 for any positive score, -1 for any negative score, otherwise 0
      const auto reward =
          immediate_score == 0 ?
              0 :
              immediate_score /= std::abs(immediate_score);

      if (update) {
        // Add the current transition to replay memory
        const auto transition = ale.game_over() ?
            deepRL::Transition(input_frames, action, reward, boost::none) :
            deepRL::Transition(
                input_frames,
                action,
                reward,
                deepRL::PreprocessScreen(ale.getScreen()));
        dqlearner.replay_memory_.addTransition(transition);
	//std::cout << "Memorypool Size: " << dqlearner.replay_memory_.memory_size() << std::endl;
        // If the size of replay memory is enough, update DQN
        if (dqlearner.replay_memory_.memory_size() >= argmap["replay_start_size"].as<int>()
	           and dqlearner.numSteps()%argmap["update_frequency"].as<int>()==0 ) {
             dqlearner.MiniBatchUpdate();
        }
      }
    }
  }
  ale.reset_game();
  return total_score;
}


text:
// ------------------------------------------------------------------------
// Pion is a development platform for building Reactors that process Events
// ------------------------------------------------------------------------
// Copyright (C) 2007-2011 Atomic Labs, Inc.  (http://www.atomiclabs.com)
//
// Pion is free software: you can redistribute it and/or modify it under the
// terms of the GNU Affero General Public License as published by the Free
// Software Foundation, either version 3 of the License, or (at your option)
// any later version.
//
// Pion is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE.  See the GNU Affero General Public License for
// more details.
//
// You should have received a copy of the GNU Affero General Public License
// along with Pion.  If not, see <http://www.gnu.org/licenses/>.
//

#ifndef PION_STATIC_LINKING

#include <pion/PionConfig.hpp>
#include <pion/PluginManager.hpp>
#include <pion/PionUnitTestDefs.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

using namespace pion;

#if defined(PION_WIN32)
	static const std::string directoryOfPluginsForTests = "PluginsUsedByUnitTests/.libs";
	static const std::string sharedLibExt = ".dll";
#else
	#if defined(PION_XCODE)
		static const std::string directoryOfPluginsForTests = "PluginsUsedByUnitTests";
	#else
		static const std::string directoryOfPluginsForTests = "PluginsUsedByUnitTests/.libs";
	#endif
	static const std::string sharedLibExt = ".so";
#endif

/// fixture for unit tests on a newly created PluginManager
template <class T>
class NewPluginManager_F : public PluginManager<T> {
public:
	NewPluginManager_F() {
		BOOST_REQUIRE(GET_DIRECTORY(m_old_cwd, DIRECTORY_MAX_SIZE) != NULL);
		BOOST_REQUIRE(CHANGE_DIRECTORY(directoryOfPluginsForTests.c_str()) == 0);
	}
	~NewPluginManager_F() {
		BOOST_CHECK(CHANGE_DIRECTORY(m_old_cwd) == 0);
	}

private:
	char m_old_cwd[DIRECTORY_MAX_SIZE+1];
};

struct InterfaceStub {
};

BOOST_AUTO_TEST_SUITE_FIXTURE_TEMPLATE(NewPluginManager_S,
									   boost::mpl::list<NewPluginManager_F<InterfaceStub> >)

BOOST_AUTO_TEST_CASE_FIXTURE_TEMPLATE(checkEmptyIsTrue) {
	BOOST_CHECK(F::empty());
}

BOOST_AUTO_TEST_CASE_FIXTURE_TEMPLATE(checkLoad) {
	BOOST_CHECK(F::load("urn:id_1", "hasCreateAndDestroy") != NULL);
}

// TODO: tests for add(), find()

BOOST_AUTO_TEST_CASE_FIXTURE_TEMPLATE(checkGet) {
	BOOST_CHECK(F::get("urn:id_2") == NULL);
}

BOOST_AUTO_TEST_CASE_FIXTURE_TEMPLATE(checkRemove) {
	BOOST_CHECK_THROW(F::remove("urn:id_1"), PluginManager<InterfaceStub>::PluginNotFoundException);
}

BOOST_AUTO_TEST_CASE_FIXTURE_TEMPLATE(checkRun) {
	typename F::PluginRunFunction f;
	BOOST_CHECK_THROW(F::run("urn:id_3", f), PluginManager<InterfaceStub>::PluginNotFoundException);
}

BOOST_AUTO_TEST_CASE_FIXTURE_TEMPLATE(checkClear) {
	BOOST_CHECK_NO_THROW(F::clear());
}

BOOST_AUTO_TEST_SUITE_END()

/// fixture for unit tests on a PluginManager with a plugin loaded
class PluginManagerWithPluginLoaded_F : public NewPluginManager_F<InterfaceStub> {
public:
	PluginManagerWithPluginLoaded_F() {
		BOOST_REQUIRE(load("urn:id_1", "hasCreateAndDestroy") != NULL);
	}
	~PluginManagerWithPluginLoaded_F() {
	}
};

BOOST_AUTO_TEST_SUITE_FIXTURE_TEMPLATE(PluginManagerWithPluginLoaded_S,
									   boost::mpl::list<PluginManagerWithPluginLoaded_F>)

BOOST_AUTO_TEST_CASE_FIXTURE_TEMPLATE(checkEmptyIsFalse) {
	BOOST_CHECK(!F::empty());
}

BOOST_AUTO_TEST_CASE_FIXTURE_TEMPLATE(checkLoadSecondPlugin) {
	BOOST_CHECK(F::load("urn:id_2", "hasCreateAndDestroy") != NULL);
}

BOOST_AUTO_TEST_CASE_FIXTURE_TEMPLATE(checkLoadSecondPluginWithSameId) {
	BOOST_CHECK_THROW(F::load("urn:id_1", "hasCreateAndDestroy"), PluginManager<InterfaceStub>::DuplicatePluginException);
}

BOOST_AUTO_TEST_CASE_FIXTURE_TEMPLATE(checkGet) {
	BOOST_CHECK(F::get("urn:id_1") != NULL);
	BOOST_CHECK(F::get("urn:id_2") == NULL);
}

BOOST_AUTO_TEST_CASE_FIXTURE_TEMPLATE(checkRemove) {
	BOOST_CHECK_NO_THROW(F::remove("urn:id_1"));
	BOOST_CHECK(F::empty());
}

BOOST_AUTO_TEST_CASE_FIXTURE_TEMPLATE(checkClear) {
	BOOST_CHECK_NO_THROW(F::clear());
	BOOST_CHECK(F::empty());
}

// TODO: tests for add(), find(), run()

BOOST_AUTO_TEST_SUITE_END()

#endif // PION_STATIC_LINKING

max_stars_repo_name: marshallmcmullen/pion

max_issues_repo_name: marshallmcmullen/pion

max_forks_repo_name: marshallmcmullen/pion


text:
#include <nano/lib/optional_ptr.hpp>
#include <nano/lib/rate_limiting.hpp>
#include <nano/lib/threading.hpp>
#include <nano/lib/timer.hpp>
#include <nano/lib/utility.hpp>
#include <nano/secure/utility.hpp>

#include <gtest/gtest.h>

#include <boost/filesystem.hpp>

#include <future>

using namespace std::chrono_literals;

TEST (rate, basic)
{
	nano::rate::token_bucket bucket (10, 10);

	// Initial burst
	ASSERT_TRUE (bucket.try_consume (10));
	ASSERT_FALSE (bucket.try_consume (10));

	// With a fill rate of 10 tokens/sec, await 1/3 sec and get 3 tokens
	std::this_thread::sleep_for (300ms);
	ASSERT_TRUE (bucket.try_consume (3));
	ASSERT_FALSE (bucket.try_consume (10));

	// Allow time for the bucket to completely refill and do a full burst
	std::this_thread::sleep_for (1s);
	ASSERT_TRUE (bucket.try_consume (10));
	ASSERT_EQ (bucket.largest_burst (), 10);
}

TEST (rate, network)
{
	// For the purpose of the test, one token represents 1MB instead of one byte.
	// Allow for 10 mb/s bursts (max bucket size), 5 mb/s long term rate
	nano::rate::token_bucket bucket (10, 5);

	// Initial burst of 10 mb/s over two calls
	ASSERT_TRUE (bucket.try_consume (5));
	ASSERT_EQ (bucket.largest_burst (), 5);
	ASSERT_TRUE (bucket.try_consume (5));
	ASSERT_EQ (bucket.largest_burst (), 10);
	ASSERT_FALSE (bucket.try_consume (5));

	// After 200 ms, the 5 mb/s fillrate means we have 1 mb available
	std::this_thread::sleep_for (200ms);
	ASSERT_TRUE (bucket.try_consume (1));
	ASSERT_FALSE (bucket.try_consume (1));
}

TEST (rate, unlimited)
{
	nano::rate::token_bucket bucket (0, 0);
	ASSERT_TRUE (bucket.try_consume (5));
	ASSERT_EQ (bucket.largest_burst (), 5);
	ASSERT_TRUE (bucket.try_consume (static_cast<size_t> (1e9)));
	ASSERT_EQ (bucket.largest_burst (), static_cast<size_t> (1e9));

	// With unlimited tokens, consuming always succeed
	ASSERT_TRUE (bucket.try_consume (static_cast<size_t> (1e9)));
	ASSERT_EQ (bucket.largest_burst (), static_cast<size_t> (1e9));
}

TEST (optional_ptr, basic)
{
	struct valtype
	{
		int64_t x{ 1 };
		int64_t y{ 2 };
		int64_t z{ 3 };
	};

	nano::optional_ptr<valtype> opt;
	ASSERT_FALSE (opt);
	ASSERT_FALSE (opt.is_initialized ());

	{
		auto val = valtype{};
		opt = val;
		ASSERT_LT (sizeof (opt), sizeof (val));
		std::unique_ptr<valtype> uptr;
		ASSERT_EQ (sizeof (opt), sizeof (uptr));
	}
	ASSERT_TRUE (opt);
	ASSERT_TRUE (opt.is_initialized ());
	ASSERT_EQ (opt->x, 1);
	ASSERT_EQ (opt->y, 2);
	ASSERT_EQ (opt->z, 3);
}

TEST (thread, thread_pool)
{
	std::atomic<bool> passed_sleep{ false };

	auto func = [&passed_sleep] () {
		std::this_thread::sleep_for (std::chrono::seconds (1));
		passed_sleep = true;
	};

	nano::thread_pool workers (1u, nano::thread_role::name::unknown);
	workers.push_task (func);
	ASSERT_FALSE (passed_sleep);

	nano::timer<std::chrono::milliseconds> timer_l;
	timer_l.start ();
	while (!passed_sleep)
	{
		if (timer_l.since_start () > std::chrono::seconds (10))
		{
			break;
		}
	}
	ASSERT_TRUE (passed_sleep);
}

TEST (thread_pool_alarm, one)
{
	nano::thread_pool workers (1u, nano::thread_role::name::unknown);
	std::atomic<bool> done (false);
	nano::mutex mutex;
	nano::condition_variable condition;
	workers.add_timed_task (std::chrono::steady_clock::now (), [&] () {
		{
			nano::lock_guard<nano::mutex> lock (mutex);
			done = true;
		}
		condition.notify_one ();
	});
	nano::unique_lock<nano::mutex> unique (mutex);
	condition.wait (unique, [&] () { return !!done; });
}

TEST (thread_pool_alarm, many)
{
	nano::thread_pool workers (50u, nano::thread_role::name::unknown);
	std::atomic<int> count (0);
	nano::mutex mutex;
	nano::condition_variable condition;
	for (auto i (0); i < 50; ++i)
	{
		workers.add_timed_task (std::chrono::steady_clock::now (), [&] () {
			{
				nano::lock_guard<nano::mutex> lock (mutex);
				count += 1;
			}
			condition.notify_one ();
		});
	}
	nano::unique_lock<nano::mutex> unique (mutex);
	condition.wait (unique, [&] () { return count == 50; });
}

TEST (thread_pool_alarm, top_execution)
{
	nano::thread_pool workers (1u, nano::thread_role::name::unknown);
	int value1 (0);
	int value2 (0);
	nano::mutex mutex;
	std::promise<bool> promise;
	workers.add_timed_task (std::chrono::steady_clock::now (), [&] () {
		nano::lock_guard<nano::mutex> lock (mutex);
		value1 = 1;
		value2 = 1;
	});
	workers.add_timed_task (std::chrono::steady_clock::now () + std::chrono::milliseconds (1), [&] () {
		nano::lock_guard<nano::mutex> lock (mutex);
		value2 = 2;
		promise.set_value (false);
	});
	promise.get_future ().get ();
	nano::lock_guard<nano::mutex> lock (mutex);
	ASSERT_EQ (1, value1);
	ASSERT_EQ (2, value2);
}

TEST (filesystem, remove_all_files)
{
	auto path = nano::unique_path ();
	auto dummy_directory = path / "tmp";
	boost::filesystem::create_directories (dummy_directory);

	auto dummy_file1 = path / "my_file1.txt";
	auto dummy_file2 = path / "my_file2.txt";
	std::ofstream (dummy_file1.string ());
	std::ofstream (dummy_file2.string ());

	// Check all exist
	ASSERT_TRUE (boost::filesystem::exists (dummy_directory));
	ASSERT_TRUE (boost::filesystem::exists (dummy_file1));
	ASSERT_TRUE (boost::filesystem::exists (dummy_file2));

	// Should remove only the files
	nano::remove_all_files_in_dir (path);

	ASSERT_TRUE (boost::filesystem::exists (dummy_directory));
	ASSERT_FALSE (boost::filesystem::exists (dummy_file1));
	ASSERT_FALSE (boost::filesystem::exists (dummy_file2));
}

TEST (filesystem, move_all_files)
{
	auto path = nano::unique_path ();
	auto dummy_directory = path / "tmp";
	boost::filesystem::create_directories (dummy_directory);

	auto dummy_file1 = dummy_directory / "my_file1.txt";
	auto dummy_file2 = dummy_directory / "my_file2.txt";
	std::ofstream (dummy_file1.string ());
	std::ofstream (dummy_file2.string ());

	// Check all exist
	ASSERT_TRUE (boost::filesystem::exists (dummy_directory));
	ASSERT_TRUE (boost::filesystem::exists (dummy_file1));
	ASSERT_TRUE (boost::filesystem::exists (dummy_file2));

	// Should move only the files
	nano::move_all_files_to_dir (dummy_directory, path);

	ASSERT_TRUE (boost::filesystem::exists (dummy_directory));
	ASSERT_TRUE (boost::filesystem::exists (path / "my_file1.txt"));
	ASSERT_TRUE (boost::filesystem::exists (path / "my_file2.txt"));
	ASSERT_FALSE (boost::filesystem::exists (dummy_file1));
	ASSERT_FALSE (boost::filesystem::exists (dummy_file2));
}

TEST (relaxed_atomic_integral, basic)
{
	nano::relaxed_atomic_integral<uint32_t> atomic{ 0 };
	ASSERT_EQ (0, atomic++);
	ASSERT_EQ (1, atomic);
	ASSERT_EQ (2, ++atomic);
	ASSERT_EQ (2, atomic);
	ASSERT_EQ (2, atomic.load ());
	ASSERT_EQ (2, atomic--);
	ASSERT_EQ (1, atomic);
	ASSERT_EQ (0, --atomic);
	ASSERT_EQ (0, atomic);
	ASSERT_EQ (0, atomic.fetch_add (2));
	ASSERT_EQ (2, atomic);
	ASSERT_EQ (2, atomic.fetch_sub (1));
	ASSERT_EQ (1, atomic);
	atomic.store (3);
	ASSERT_EQ (3, atomic);

	uint32_t expected{ 2 };
	ASSERT_FALSE (atomic.compare_exchange_strong (expected, 1));
	ASSERT_EQ (3, expected);
	ASSERT_EQ (3, atomic);
	ASSERT_TRUE (atomic.compare_exchange_strong (expected, 1));
	ASSERT_EQ (1, atomic);
	ASSERT_EQ (3, expected);

	// Weak can fail spuriously, try a few times
	bool res{ false };
	for (int i = 0; i < 1000; ++i)
	{
		res |= atomic.compare_exchange_weak (expected, 2);
		expected = 1;
	}
	ASSERT_TRUE (res);
	ASSERT_EQ (2, atomic);
}

TEST (relaxed_atomic_integral, many_threads)
{
	std::vector<std::thread> threads;
	auto num = 4;
	nano::relaxed_atomic_integral<uint32_t> atomic{ 0 };
	for (int i = 0; i < num; ++i)
	{
		threads.emplace_back ([&atomic] {
			for (int i = 0; i < 10000; ++i)
			{
				++atomic;
				atomic--;
				atomic++;
				--atomic;
				atomic.fetch_add (2);
				atomic.fetch_sub (2);
			}
		});
	}

	for (auto & thread : threads)
	{
		thread.join ();
	}

	// Check values
	ASSERT_EQ (0, atomic);
}

max_stars_repo_name: regisd/nano-node

max_issues_repo_name: thsfs/nano-node

max_forks_repo_name: thsfs/nano-node


text:
/*
 *   Repast for High Performance Computing (Repast HPC)
 *
 *   Copyright (c) 2010 Argonne National Laboratory
 *   All rights reserved.
 *
 *   Redistribution and use in source and binary forms, with
 *   or without modification, are permitted provided that the following
 *   conditions are met:
 *
 *     Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *
 *     Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *
 *     Neither the name of the Argonne National Laboratory nor the names of its
 *     contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *   ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 *   PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE TRUSTEES OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *   EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 *   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 *   PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 *   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *   NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 *   EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *
 *  DataSet.cpp
 *
 *  Created on: Aug 23, 2010
 *      Author: nick
 */

#include <exception>

#include <boost/filesystem.hpp>

#include "SVDataSet.h"
#include "RepastProcess.h"
#include "io.h"
#include "RepastErrors.h"

namespace fs = boost::filesystem;

namespace repast {

SVDataSet::SVDataSet(const std::string& file, const std::string& separator, const Schedule* schedule) :
	_separator(separator), _schedule(schedule), out(), open(true) {
	rank = RepastProcess::instance()->rank();
	if (rank == 0) {
	  fs::path filepath(file);
    if (!fs::exists(filepath.parent_path()))  fs::create_directories(filepath.parent_path());
    int i = 1;
    std::string stem = filepath.stem().string();
    while(fs::exists(filepath)){    // This will increment i until it hits a unique name
      i++;
      std::stringstream ss;
      ss << stem << "_" << i << filepath.extension().string();
      fs::path newName(filepath.parent_path() / ss.str());
      filepath = newName;
    }
		out.open(filepath.string().c_str());
	}
}

SVDataSet::~SVDataSet() {
	close();
}

void SVDataSet::close() {
	if (open) {
		if (rank == 0) {
			out.close();
		}
		for (size_t i = 0, n = dataSources.size(); i < n; ++i) {
			delete dataSources[i];
		}
		dataSources.clear();

		for (size_t i = 0, n = vars.size(); i < n; ++i) {
			delete vars[i];
		}
		open = false;
	}
}

void SVDataSet::init() {
	if (rank == 0) {
		out << "\"tick\"";
		for (size_t i = 0; i < dataSources.size(); i++) {
			SVDataSource * ds = dataSources[i];
			SVDataSource::DataType type = ds->type();
			Variable* var;
			if (type == SVDataSource::INT)
				var = new IntVariable();
			else
				var = new DoubleVariable();
			vars.push_back(var);
			out << _separator << "\"" << ds->name() << "\"";
		}
		out << std::endl;
		out.flush();
	}
}

void SVDataSet::record() {
	if (!open) throw Repast_Error_28(); // Data set not open
	if (rank == 0) {
		ticks.push_back(_schedule->getCurrentTick());
	}
	for (size_t i = 0; i < dataSources.size(); i++) {
		dataSources[i]->record();
	}
}

void SVDataSet::write() {
	if (!open) throw Repast_Error_29();
	for (size_t i = 0; i < dataSources.size(); i++) {
		SVDataSource * ds = dataSources[i];
		Variable* var = 0;
		if (rank == 0) {
			var = vars[i];
		}
		ds->write(var);
	}

	if (rank == 0) {
		for (size_t ti = 0, k = ticks.size(); ti < k; ++ti) {
			out << ticks[ti];
			for (size_t i = 0, n = vars.size(); i < n; ++i) {
				Variable* var = vars[i];
				out << _separator;
				var->write(ti, out);
			}
			out << std::endl;
		}

		for (size_t i = 0, n = vars.size(); i < n; ++i) {
			vars[i]->clear();
		}
		out.flush();
	}

	ticks.clear();
}

}

max_stars_repo_name: etri/dtsim

max_issues_repo_name: etri/dtsim

max_forks_repo_name: etri/dtsim


text:
#ifndef FORMULA_INTERFACE_HPP_INCLUDED
#define FORMULA_INTERFACE_HPP_INCLUDED

#include <map>
#include <vector>

#include <boost/intrusive_ptr.hpp>
#include <boost/scoped_ptr.hpp>

#include "formula_callable.hpp"
#include "formula_callable_definition.hpp"
#include "variant.hpp"
#include "variant_type.hpp"

namespace game_logic
{

struct formula_interface_impl;

class formula_interface_instance_factory : public reference_counted_object
{
public:
	virtual ~formula_interface_instance_factory();

	virtual bool all_static_lookups() const = 0;
	virtual variant create(const variant& v) const = 0;
	virtual int get_id() const = 0;
};

class formula_interface : public formula_callable
{
public:
	struct interface_mismatch_error {
		explicit interface_mismatch_error(const std::string& msg) : msg(msg) {}
		std::string msg;
	};

	explicit formula_interface(const std::map<std::string, variant_type_ptr>& types);
	const std::map<std::string, variant_type_ptr>& get_types() const { return types_; }

	formula_interface_instance_factory* create_factory(variant_type_ptr type) const; //throw interface_mismatch_error
	formula_interface_instance_factory* get_dynamic_factory() const;

	const_formula_callable_definition_ptr get_definition() const;

	bool match(const variant& v) const;

	std::string to_string() const;

private:
	variant get_value(const std::string& key) const;

	std::map<std::string, variant_type_ptr> types_;
	boost::scoped_ptr<formula_interface_impl> impl_;
};

}

typedef boost::intrusive_ptr<game_logic::formula_interface> formula_interface_ptr;
typedef boost::intrusive_ptr<const game_logic::formula_interface> const_formula_interface_ptr;

#endif

ext: hpp

max_stars_repo_name: sweetkristas/anura

max_issues_repo_name: sweetkristas/anura

max_forks_repo_name: sweetkristas/anura