void KMEANS<T>::print()
{
	ofstream fout;
	fout.open("res.txt");
	if (!fout)
	{
		cout << "file res.txt open failed" << endl;
		exit(0);
	}

	typename vector< vector<T> >::iterator it = dataSet.begin();
	typename vector< tNode >::iterator itt = clusterAssignment.begin();
	for (int i = 0; i < rowLen; ++i)
	{
		typename vector<T>::iterator it2 = it->begin();
		while ( it2 != it->end() )
		{
			fout << *it2 << "\t";
			++it2;
		}
		fout << (*itt).minIndex << endl;
		++itt;
		++it;
	}
}

int Polynomial<CoefficientType>::getDegree() const {
  int max_degree = 0;
  for (typename vector<Monomial>::const_iterator iter=monomials.begin(); iter!=monomials.end(); iter++) {
    int monomial_degree = iter->getDegree();
    if (monomial_degree > max_degree) max_degree = monomial_degree; 
  }
    
  return max_degree;
}

Error_PN2S NetworkAnalyzer::importCompts(vector<models::Compartment > &cmpts)
{
	typename vector<models::Compartment >::iterator n;

	for(n = cmpts.begin(); n != cmpts.end(); ++n) {
		allCompartments.push_back(n.base());
//		importHHChannels(n->hhchannels);
	}
	return Error_PN2S::NO_ERROR;
}

Error_PN2S NetworkAnalyzer::importHHChannels(vector<models::HHChannel > &chs)
{
	if(chs.size() > 0)
	{
		typename vector<models::HHChannel >::iterator n;
		for(n = chs.begin(); n != chs.end(); ++n) {
			allHHChannels.push_back(n.base());
		}
	}
	return Error_PN2S::NO_ERROR;
}

        bool CellMap::removeEntity (Entity *entity) {
            typename vector<std::unique_ptr<Entity>>::iterator it;
            for (it = entityInside.begin(); it != entityInside.end();) {
                if (entity == it->get()) {
                    it->release();
                    it = entityInside.erase(it);
                    return true;
                } else
                    it++;

            }
            return false;
        }

 virtual int         thread_local_alloc          (void (*dtor)(intptr_t))
 {
     int index = (int)entries_.size();
     entries_.resize(index + 1);
     entry& ent = entries_[index];
     ent.alive_ = true;
     ent.dtor_ = dtor;
     for (size_t i = 0; i != thread_count; ++i)
     {
         ent.value_[i] = 0;
     }
     return index;
 }

void
State_t<EVENT>::operator-=(const State_t<EVENT>&s)
{
   for (auto & elem : s._arcs) {
      typename vector<arc_type>::reverse_iterator j;

      for (j = _arcs.rbegin(); j != _arcs.rend(); ++j) {
         if (j->match(elem.event())) {
            _arcs.erase(j.base() - 1);
            break;
         }
      }
   }
}

template<class T> void printArray2D(vector< vector<T> > &I)
{
    // This is how we iterate using an iterator for 2d vectors
    typename vector< vector<T> >::iterator row; // Iterator for row of 2d vector
    typename vector<T>::iterator col; // Iterator for columns of 2d vector

    cout << "Matrix size: " << "[" << I.size() << "x" << I[0].size() << "]" << endl; // print the row and columns

    for(row = I.begin(); row !=I.end(); row++)
    {
        for(col = row->begin(); col != row->end(); col++)
        {
            cout << *col << ","; // print the value contained in each row of our 2d vector.
        }

        cout << endl;
    }
}

void Node<D>:: bulkLoading(vector<Dados<D> > dados, int ordem){
	typename vector<Dados<D> >::iterator atual = dados.begin();
	Node<D> *novo = new Node<D>(ordem);
	novo->folha = true;
	Node<D> *first = novo;
	Node<D> *aux;

	for(typename vector<Dados<D> >::iterator it=dados.begin()+1; it!=dados.end(); ++it ){
		if(*atual==*it){
			atual->addRef( it->getOffsets().back() );
			it->clearOffsets();
		}else{
			unsigned int a = atual-dados.begin();
			novo->chaves.push_back(dados[a]);
			atual = it ;
			if(novo->chaves.size()>=(unsigned int)(ordem-1)/2){
				aux = novo;
				novo = new Node<D>(ordem);
				novo->folha = true;
				aux->prox = novo;
			}
		}
	}

	novo->prox = (Node<D>*)NULL;
	aux->prox =  (Node<D>*)NULL;
	novo = first;
	while(novo->prox != (Node<D>*)NULL ){
		for(unsigned int i = 0; i<novo->chaves.size() && novo->folha; i++){
       #ifdef DEBUG
       cout<<novo->chaves[i]<<endl;
       #endif
       Node<D>::inserePai( novo, novo->prox);
			 novo = novo->prox;
		}
	}

	// for( vector<Dados<D> >::iterator it=dados.begin(); it!=dados.end(); ++it )
	// 	if(!it->getOffsets().empty())
	// 		cout<<*it<<endl;
};

  void FeatureGroupingAlgorithmQT::group_(const vector<MapType>& maps,
                                          ConsensusMap& out)
  {
    // check that the number of maps is ok:
    if (maps.size() < 2)
    {
      throw Exception::IllegalArgument(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION,
                                       "At least two maps must be given!");
    }

    QTClusterFinder cluster_finder;
    cluster_finder.setParameters(param_.copy("", true));

    cluster_finder.run(maps, out);

    StringList ms_run_locations;

    // add protein IDs and unassigned peptide IDs to the result map here,
    // to keep the same order as the input maps (useful for output later):
    for (typename vector<MapType>::const_iterator map_it = maps.begin();
         map_it != maps.end(); ++map_it)
    {      
      // add protein identifications to result map:
      out.getProteinIdentifications().insert(
        out.getProteinIdentifications().end(),
        map_it->getProteinIdentifications().begin(),
        map_it->getProteinIdentifications().end());

      // add unassigned peptide identifications to result map:
      out.getUnassignedPeptideIdentifications().insert(
        out.getUnassignedPeptideIdentifications().end(),
        map_it->getUnassignedPeptideIdentifications().begin(),
        map_it->getUnassignedPeptideIdentifications().end());
    }

    // canonical ordering for checking the results:
    out.sortByQuality();
    out.sortByMaps();
    out.sortBySize();
    return;
  }

IGL_INLINE void igl::polygon_mesh_to_triangle_mesh(
  const std::vector<std::vector<Index> > & vF,
  Eigen::PlainObjectBase<DerivedF>& F)
{
  using namespace std;
  using namespace Eigen;
  int m = 0;
  // estimate of size
  for(typename vector<vector<Index > >::const_iterator fit = vF.begin();
    fit!=vF.end();
    fit++)
  {
    if(fit->size() >= 3)
    {
      m += fit->size() - 2;
    }
  }
  // Resize output
  F.resize(m,3);
  {
    int k = 0;
    for(typename vector<vector<Index > >::const_iterator fit = vF.begin();
      fit!=vF.end();
      fit++)
    {
      if(fit->size() >= 3)
      {
        typename vector<Index >::const_iterator cit = fit->begin();
        cit++;
        typename vector<Index >::const_iterator pit = cit++;
        for(;
          cit!=fit->end();
          cit++,pit++)
        {
          F(k,0) = *(fit->begin());
          F(k,1) = *pit;
          F(k,2) = *cit;
          k++;
        }
      }
    }
    assert(k==m);
  }

}

  void switchScores_(IDType& id, Size& counter)
  {
    for (typename vector<typename IDType::HitType>::iterator hit_it = id.getHits().begin();
         hit_it != id.getHits().end(); ++hit_it, ++counter)
    {
      if (!hit_it->metaValueExists(new_score_))
      {
        String msg = "Meta value '" + new_score_ + "' not found for " + 
          describeHit_(*hit_it);
        throw Exception::MissingInformation(__FILE__, __LINE__,
                                            __PRETTY_FUNCTION__, msg);
      }

      String old_score_meta = (old_score_.empty() ? id.getScoreType() : 
                               old_score_);
      DataValue dv = hit_it->getMetaValue(old_score_meta);
      if (!dv.isEmpty()) // meta value for old score already exists
      {
        if (fabs((double(dv) - hit_it->getScore()) * 2.0 /
                 (double(dv) + hit_it->getScore())) > tolerance_)
        {
          String msg = "Meta value '" + old_score_meta + "' already exists "
            "with a conflicting value for " + describeHit_(*hit_it);
          throw Exception::InvalidValue(__FILE__, __LINE__, __PRETTY_FUNCTION__,
                                        msg, dv.toString());
        } // else: values match, nothing to do
      }
      else
      {
        hit_it->setMetaValue(old_score_meta, hit_it->getScore());
      }
      hit_it->setScore(hit_it->getMetaValue(new_score_));
    }
    id.setScoreType(new_score_type_);
    id.setHigherScoreBetter(higher_better_);
  }

void test_createFilters(const Kernel& f)
{
    using namespace MatchedFilter;

    if (os_) *os_ << "test_createFilters() " << typeid(f).name() << endl;

    int sampleRadius = 2;
    int subsampleFactor = 4;
    double dx = 1;

    typedef typename KernelTraits<Kernel>::filter_type filter_type;
    typedef typename KernelTraits<Kernel>::ordinate_type ordinate_type;
    vector<filter_type> filters = details::createFilters(f, 
                                                         sampleRadius, 
                                                         subsampleFactor, 
                                                         dx);
    
    // verify filter count
    unit_assert((int)filters.size() == subsampleFactor);

    for (typename vector<filter_type>::const_iterator it=filters.begin(); it!=filters.end(); ++it)
    {
        if (os_)
        {
            copy(it->begin(), it->end(), ostream_iterator<ordinate_type>(*os_, " "));
            *os_ << endl;
        }

        // verify filter size
        unit_assert((int)it->size() == sampleRadius*2 + 1);
    
        // verify filter normalization
        double sum = 0;
        for (typename filter_type::const_iterator jt=it->begin(); jt!=it->end(); ++jt)
            sum += norm(complex<double>(*jt));
        unit_assert_equal(sum, 1, 1e-14);
    }

    if (os_) *os_ << endl;
}

void ListPorts(const vector<PortClass> &ports, bool input) {
  typename vector<PortClass>::const_iterator port_iter;
  for (port_iter = ports.begin(); port_iter != ports.end(); ++port_iter) {
    cout << "  port " << port_iter->Id() << ", ";

    if (input)
      cout << "IN";
    else
      cout << "OUT";

    if (!port_iter->Description().empty()) {
      cout << " " << port_iter->Description();
    }

    switch (port_iter->PriorityCapability()) {
      case ola::CAPABILITY_STATIC:
        cout << ", priority " << static_cast<int>(port_iter->Priority());
        break;
      case ola::CAPABILITY_FULL:
        cout << ", priority ";
        if (port_iter->PriorityMode() == ola::PRIORITY_MODE_INHERIT)
          cout << "inherited";
        else
          cout << "overide " << static_cast<int>(port_iter->Priority());
        break;
      default:
        break;
    }

    if (port_iter->IsActive())
      cout << ", patched to universe " << port_iter->Universe();

    if (port_iter->SupportsRDM())
      cout << ", RDM supported";
    cout << endl;
  }
}

void nl_shinji_kneip_ransac(NormalAOPoseAdapter<POSE_T, POINT_T>& adapter, 
	const POINT_T thre_3d_, const POINT_T thre_2d_, const POINT_T nl_thre, int& Iter, POINT_T confidence = 0.99){
	typedef Matrix<POINT_T, Dynamic, Dynamic> MX;
	typedef Matrix<POINT_T, 3, 1> P3;
	typedef SE3Group<POSE_T> RT;

	POSE_T cos_thr = cos(atan(thre_2d_ / adapter.getFocal()));
	POINT_T cos_nl_thre = cos(nl_thre);

	RandomElements<int> re((int)adapter.getNumberCorrespondences());
	const int K = 3;
	MX Xw(3, K + 1), Xc(3, K + 1), bv(3, K + 1);
	MX Nw(3, K + 1), Nc(3, K + 1);
	Matrix<short, Dynamic, Dynamic> inliers(adapter.getNumberCorrespondences(), 3);

	adapter.setMaxVotes(-1);
	for (int ii = 0; ii < Iter; ii++)	{
		//randomly select K candidates
		RT solution_kneip, solution_shinji, solution_nl;
		vector<RT> v_solutions;
		vector<int> selected_cols;
		re.run(K + 1, &selected_cols);

		if (assign_sample<POSE_T, POINT_T>(adapter, selected_cols, &Xw, &Nw, &Xc, &Nc, &bv)){
			solution_shinji = shinji<POSE_T, POINT_T>(Xw, Xc, K);
			v_solutions.push_back(solution_shinji);
		}

		if (kneip<POSE_T, POINT_T>(Xw, bv, &solution_kneip)){
			v_solutions.push_back(solution_kneip);
		}

		nl_2p<POSE_T, POINT_T>(Xc.col(0), Nc.col(0), Xc.col(1), Xw.col(0), Nw.col(0), Xw.col(1), &solution_nl);
		v_solutions.push_back(solution_nl);

		for (typename vector<RT>::iterator itr = v_solutions.begin(); itr != v_solutions.end(); ++itr) {
			//collect votes
			int votes = 0;
			inliers.setZero();
			P3 eivE; P3 pc; POINT_T cos_a;
			for (int c = 0; c < adapter.getNumberCorrespondences(); c++) {
				if (adapter.isValid(c)){
					//with normal data
					POINT_T cos_alpha = adapter.getNormalCurr(c).dot(itr->so3().template cast<POINT_T>() * adapter.getNormalGlob(c));
					if (cos_alpha > cos_nl_thre){
						inliers(c, 2) = 1;
						votes++;
					}
				
					//with 3d data
					eivE = adapter.getPointCurr(c) - (itr->so3().template cast<POINT_T>() * adapter.getPointGlob(c) + itr->translation().template cast<POINT_T>());
					if (eivE.norm() < thre_3d_){
						inliers(c, 1) = 1;
						votes++;
					}
				}
				//with 2d
				pc = itr->so3().template cast<POINT_T>() * adapter.getPointGlob(c) + itr->translation().template cast<POINT_T>();// transform pw into pc
				pc = pc / pc.norm(); //normalize pc

				//compute the score
				cos_a = pc.dot( adapter.getBearingVector(c) );
				if (cos_a > cos_thr){
					inliers(c, 0) = 1;
					votes++;
				}
			}
			//cout << endl;

			if (votes > adapter.getMaxVotes() ){
				assert(votes == inliers.sum());
				adapter.setMaxVotes(votes);
				adapter.setRcw(itr->so3());
				adapter.sett(itr->translation());
				adapter.setInlier(inliers);

				//cout << inliers.inverse() << endl << endl;
				//adapter.printInlier();
				//Iter = RANSACUpdateNumIters(confidence, (POINT_T)(adapter.getNumberCorrespondences() * 3 - votes) / adapter.getNumberCorrespondences() / 3, K, Iter);
			}
		}//for(vector<RT>::iterator itr = v_solutions.begin() ...
	}//for(int ii = 0; ii < Iter; ii++)
	PnPPoseAdapter<POSE_T, POINT_T>* pAdapterPnP = &adapter;
	pAdapterPnP->cvtInlier();
	AOPoseAdapter<POSE_T, POINT_T>* pAdapterAO = &adapter;
	pAdapterAO->cvtInlier();
	adapter.cvtInlier();
	return;
}

#ifndef BTL_AO_NORM_POSE_HEADER
#define BTL_AO_NORM_POSE_HEADER

#include "common/OtherUtil.hpp"
#include <limits>
#include <Eigen/Dense>
#include "NormalAOPoseAdapter.hpp"
#include "AbsoluteOrientation.hpp"

using namespace Eigen;
using namespace std;

template<typename POSE_T, typename POINT_T>
Matrix<POSE_T, 3, 1> find_opt_cc(NormalAOPoseAdapter<POSE_T, POINT_T>& adapter)
{
	//the R has been fixed, we need to find optimal cc, camera center, given n pairs of 2-3 correspondences
	//Slabaugh, G., Schafer, R., & Livingston, M. (2001). Optimal Ray Intersection For Computing 3D Points From N -View Correspondences.
	typedef Matrix<POSE_T, 3, 1> V3;
	typedef Matrix<POSE_T, 3, 3> M3;

	M3 Rwc = adapter.getRcw().inverse().matrix();
	M3 AA; AA.setZero();
	V3 bb; bb.setZero();
	for (int i = 0; i < adapter.getNumberCorrespondences(); i++)
	{
		if (adapter.isInlier23(i)){
			V3 vr_w = Rwc * adapter.getBearingVector(i).template cast<POSE_T>();
			M3 A;
			A(0,0) = 1 - vr_w(0)*vr_w(0);
			A(1,0) = A(0,1) = - vr_w(0)*vr_w(1);
			A(2,0) = A(0,2) = - vr_w(0)*vr_w(2);
			A(1,1) = 1 - vr_w(1)*vr_w(1);
			A(2,1) = A(1,2) = - vr_w(1)*vr_w(2);
			A(2,2) = 1 - vr_w(2)*vr_w(2);
			V3 b = A * adapter.getPointGlob(i).template cast<POSE_T>();
			AA += A;
			bb += b;
		}
	}
	V3 c_w;
	if (fabs(AA.determinant()) < POSE_T(0.0001))
		c_w = V3(numeric_limits<POSE_T>::quiet_NaN(), numeric_limits<POSE_T>::quiet_NaN(), numeric_limits<POSE_T>::quiet_NaN());
	else
		c_w = AA.jacobiSvd(ComputeFullU | ComputeFullV).solve(bb);
	return c_w;
}

template< typename POSE_T, typename POINT_T >
bool assign_sample(const NormalAOPoseAdapter<POSE_T, POINT_T>& adapter, 
	const vector<int>& selected_cols_, 
	Matrix<POINT_T, Dynamic, Dynamic>* p_X_w_, Matrix<POINT_T, Dynamic, Dynamic>* p_N_w_, 
	Matrix<POINT_T, Dynamic, Dynamic>* p_X_c_, Matrix<POINT_T, Dynamic, Dynamic>* p_N_c_, Matrix<POINT_T, Dynamic, Dynamic>* p_bv_){
	
	int K = (int)selected_cols_.size() - 1;
	bool use_shinji = false;
	int nValid = 0;
	for (int nSample = 0; nSample < K; nSample++) {
		p_X_w_->col(nSample) = adapter.getPointGlob(selected_cols_[nSample]);
		p_N_w_->col(nSample) = adapter.getNormalGlob(selected_cols_[nSample]);
		p_bv_->col(nSample) = adapter.getBearingVector(selected_cols_[nSample]);
		if (adapter.isValid(selected_cols_[nSample])){
			p_X_c_->col(nSample) = adapter.getPointCurr(selected_cols_[nSample]);
			p_N_c_->col(nSample) = adapter.getNormalCurr(selected_cols_[nSample]);
			nValid++;
		}
	}
	if (nValid == K)
		use_shinji = true;
	//assign the fourth elements for 
	p_X_w_->col(3) = adapter.getPointGlob(selected_cols_[3]);
	p_N_w_->col(3) = adapter.getNormalGlob(selected_cols_[3]);
	p_bv_->col(3) = adapter.getBearingVector(selected_cols_[3]);

	return use_shinji;
}

template<typename POSE_T, typename POINT_T>
void nl_2p( const Matrix<POINT_T,3,1>& pt1_c, const Matrix<POINT_T,3,1>& nl1_c, const Matrix<POINT_T,3,1>& pt2_c, 
			const Matrix<POINT_T,3,1>& pt1_w, const Matrix<POINT_T,3,1>& nl1_w, const Matrix<POINT_T,3,1>& pt2_w, 
			SE3Group<POSE_T>* p_solution){
	//Drost, B., Ulrich, M., Navab, N., & Ilic, S. (2010). Model globally, match locally: Efficient and robust 3D object recognition. In CVPR (pp. 998?005). Ieee. http://doi.org/10.1109/CVPR.2010.5540108
	typedef Matrix<POINT_T, Dynamic, Dynamic> MatrixX;
	typedef Matrix<POSE_T, 3, 1> V3;
	typedef SO3Group<POSE_T> ROTATION;
	typedef SE3Group<POSE_T> RT;

	V3 c_w = pt1_w.template cast<POSE_T>(); // c_w is the origin of coordinate g w.r.t. world

	POSE_T alpha = acos(nl1_w(0)); // rotation nl1_c to x axis (1,0,0)
	V3 axis( 0, nl1_w(2), -nl1_w(1)); //rotation axis between nl1_c to x axis (1,0,0) i.e. cross( nl1_w, x );
	axis.normalized();

	//verify quaternion and rotation matrix
	Quaternion<POSE_T> q_g_f_w(AngleAxis<POSE_T>(alpha, axis));
	//cout << q_g_f_w << endl;
	ROTATION R_g_f_w(q_g_f_w);
	//cout << R_g_f_w << endl;

	V3 nl_x = R_g_f_w * nl1_w.template cast<POSE_T>();
	axis.normalized();
//.........这里部分代码省略.........