C++ MatrixXf::row方法代码示例

void FiffSimulator::doContinousHPI(MatrixXf& matData)
{
    //This only works with babyMEG HPI channels 400 ... 407
    if(m_pFiffInfo && m_pHPIWidget && matData.rows() >= 407) {
        if(m_pHPIWidget->wasLastFitOk()) {
            // Load device to head transformation matrix from Fiff info
            QMatrix3x3 rot;

            for(int ir = 0; ir < 3; ir++) {
                for(int ic = 0; ic < 3; ic++) {
                    rot(ir,ic) = m_pFiffInfo->dev_head_t.trans(ir,ic);
                }
            }

            QQuaternion quatHPI = QQuaternion::fromRotationMatrix(rot);

            // Write rotation quaternion to HPI Ch #1~3
            matData.row(401) = MatrixXf::Constant(1,matData.cols(), quatHPI.x());
            matData.row(402) = MatrixXf::Constant(1,matData.cols(), quatHPI.y());
            matData.row(403) = MatrixXf::Constant(1,matData.cols(), quatHPI.z());

            // Write translation vector to HPI Ch #4~6
            matData.row(404) = MatrixXf::Constant(1,matData.cols(), m_pFiffInfo->dev_head_t.trans(0,3));
            matData.row(405) = MatrixXf::Constant(1,matData.cols(), m_pFiffInfo->dev_head_t.trans(1,3));
            matData.row(406) = MatrixXf::Constant(1,matData.cols(), m_pFiffInfo->dev_head_t.trans(2,3));

            // Write GOF to HPI Ch #7
            // Write goodness of fit (GOF)to HPI Ch #7
            float dpfitError = 0.0;
            float GOF = 1 - dpfitError;
            matData.row(407) = MatrixXf::Constant(1,matData.cols(), GOF);
        }
    }
}

void NeighbourJoining::calcNewD(MatrixXf& currentD, MatrixXi& rowsID, const Pair& p) {
	//calculates distances to new node
	int j = 0;
	for (int i = 0; i < numCurrentNodes - 1; ++i) {
		if (i == p.i)
			j++;
		currentD(numCurrentNodes, i) = (currentD(p.i, i + j) + currentD(p.j, i + j) - currentD(p.i, p.j)) / 2;
		currentD(i, numCurrentNodes) = currentD(numCurrentNodes, i);
	}
	//cout << "distances to new node: " << currentD.row(numCurrentNodes).head(numCurrentNodes-1) <<endl;

	//swaps rows and columns so that the closest pair nodes go right and at the bottom of the matrix
	currentD.row(p.i).head(numCurrentNodes - 1).swap(
			currentD.row(numCurrentNodes - 1).head(numCurrentNodes - 1));

	currentD.col(p.i).head(numCurrentNodes - 1).swap(
			currentD.col(numCurrentNodes - 1).head(numCurrentNodes - 1));

	currentD.row(p.j).head(numCurrentNodes - 1).swap(
			currentD.row(numCurrentNodes).head(numCurrentNodes - 1));

	currentD.col(p.j).head(numCurrentNodes - 1).swap(
			currentD.col(numCurrentNodes).head(numCurrentNodes - 1));

	currentD.diagonal().setZero();
	//cout << "new Matrix:" << endl;  	printMatrix(currentD);

	//adjusts node IDs to new matrix indices
	int newNode = 2 * numObservableNodes - numCurrentNodes;
	rowsID.row(p.i).swap(rowsID.row(numCurrentNodes - 1));
	rowsID.row(p.j).swap(rowsID.row(newNode));

	//cout << "rowsID:   " << rowsID.transpose(); cout << endl;
}

void call_ref()
{
  VectorXcf ca  = VectorXcf::Random(10);
  VectorXf a    = VectorXf::Random(10);
  RowVectorXf b = RowVectorXf::Random(10);
  MatrixXf A    = MatrixXf::Random(10,10);
  RowVector3f c = RowVector3f::Random();
  const VectorXf& ac(a);
  VectorBlock<VectorXf> ab(a,0,3);
  const VectorBlock<VectorXf> abc(a,0,3);
  

  VERIFY_EVALUATION_COUNT( call_ref_1(a,a), 0);
  VERIFY_EVALUATION_COUNT( call_ref_1(b,b.transpose()), 0);
//   call_ref_1(ac,a<c);           // does not compile because ac is const
  VERIFY_EVALUATION_COUNT( call_ref_1(ab,ab), 0);
  VERIFY_EVALUATION_COUNT( call_ref_1(a.head(4),a.head(4)), 0);
  VERIFY_EVALUATION_COUNT( call_ref_1(abc,abc), 0);
  VERIFY_EVALUATION_COUNT( call_ref_1(A.col(3),A.col(3)), 0);
//   call_ref_1(A.row(3),A.row(3));    // does not compile because innerstride!=1
  VERIFY_EVALUATION_COUNT( call_ref_3(A.row(3),A.row(3).transpose()), 0);
  VERIFY_EVALUATION_COUNT( call_ref_4(A.row(3),A.row(3).transpose()), 0);
//   call_ref_1(a+a, a+a);          // does not compile for obvious reason

  MatrixXf tmp = A*A.col(1);
  VERIFY_EVALUATION_COUNT( call_ref_2(A*A.col(1), tmp), 1);     // evaluated into a temp
  VERIFY_EVALUATION_COUNT( call_ref_2(ac.head(5),ac.head(5)), 0);
  VERIFY_EVALUATION_COUNT( call_ref_2(ac,ac), 0);
  VERIFY_EVALUATION_COUNT( call_ref_2(a,a), 0);
  VERIFY_EVALUATION_COUNT( call_ref_2(ab,ab), 0);
  VERIFY_EVALUATION_COUNT( call_ref_2(a.head(4),a.head(4)), 0);
  tmp = a+a;
  VERIFY_EVALUATION_COUNT( call_ref_2(a+a,tmp), 1);            // evaluated into a temp
  VERIFY_EVALUATION_COUNT( call_ref_2(ca.imag(),ca.imag()), 1);      // evaluated into a temp

  VERIFY_EVALUATION_COUNT( call_ref_4(ac.head(5),ac.head(5)), 0);
  tmp = a+a;
  VERIFY_EVALUATION_COUNT( call_ref_4(a+a,tmp), 1);           // evaluated into a temp
  VERIFY_EVALUATION_COUNT( call_ref_4(ca.imag(),ca.imag()), 0);

  VERIFY_EVALUATION_COUNT( call_ref_5(a,a), 0);
  VERIFY_EVALUATION_COUNT( call_ref_5(a.head(3),a.head(3)), 0);
  VERIFY_EVALUATION_COUNT( call_ref_5(A,A), 0);
//   call_ref_5(A.transpose(),A.transpose());   // does not compile because storage order does not match
  VERIFY_EVALUATION_COUNT( call_ref_5(A.block(1,1,2,2),A.block(1,1,2,2)), 0);
  VERIFY_EVALUATION_COUNT( call_ref_5(b,b), 0);             // storage order do not match, but this is a degenerate case that should work
  VERIFY_EVALUATION_COUNT( call_ref_5(a.row(3),a.row(3)), 0);

  VERIFY_EVALUATION_COUNT( call_ref_6(a,a), 0);
  VERIFY_EVALUATION_COUNT( call_ref_6(a.head(3),a.head(3)), 0);
  VERIFY_EVALUATION_COUNT( call_ref_6(A.row(3),A.row(3)), 1);           // evaluated into a temp thouth it could be avoided by viewing it as a 1xn matrix
  tmp = A+A;
  VERIFY_EVALUATION_COUNT( call_ref_6(A+A,tmp), 1);                // evaluated into a temp
  VERIFY_EVALUATION_COUNT( call_ref_6(A,A), 0);
  VERIFY_EVALUATION_COUNT( call_ref_6(A.transpose(),A.transpose()), 1);      // evaluated into a temp because the storage orders do not match
  VERIFY_EVALUATION_COUNT( call_ref_6(A.block(1,1,2,2),A.block(1,1,2,2)), 0);
  
  VERIFY_EVALUATION_COUNT( call_ref_7(c,c), 0);
}

bool singleModelRANSAC(const MatrixXf &data, int M, MatrixXf &inlier) {
  int maxdegen = 10;
  int dataSize = data.rows();
  int psize = 4;
  MatrixXf x1 = data.block(0, 0, data.rows(), 3);
  MatrixXf x2 = data.block(0, 3, data.rows(), 3);
  vector<int> sample;
  MatrixXf pts1(4, 3);
  MatrixXf pts2(4, 3);
  int maxInlier = -1;
  MatrixXf bestResidue;
  for (int m = 0; m < M; m++) {
    int degencount = 0;
    int isdegen = 1;
    while (isdegen==1 && degencount < maxdegen) {
      degencount ++;
      RandomSampling(psize, dataSize, sample);
      for (int i = 0; i < psize; i++) {
        pts1.row(i) = x1.row(sample[i]);
        pts2.row(i) = x2.row(sample[i]);
      }
      if (sampleValidTest(pts1, pts2))
          isdegen = 0;
    }
    if (isdegen) {
      cout << "Cannot find valid p-subset" << endl;
      return false;
    }
    Matrix3f local_H;
    MatrixXf local_A;
    fitHomography(pts1, pts2, local_H, local_A);

    MatrixXf residue;
    computeHomographyResidue(x1, x2, local_H, residue);
    int inlierCount = (residue.array() < THRESHOLD).count();
    if (inlierCount > maxInlier) {
      maxInlier = inlierCount;
      bestResidue = residue;
    }
  }
  inlier.resize(maxInlier, data.cols());
  int transferCounter = 0;
  for (int i = 0; i < dataSize; i++) {
    if (bestResidue(i) < THRESHOLD) {
      inlier.row(transferCounter) = data.row(i);
      transferCounter++;
    }
  }
  if (transferCounter != maxInlier) {
    cout << "RANSAC result size does not match!!!!" << endl;
    return false;
  }
  return true;
}

void fitHomography(MatrixXf pts1, MatrixXf pts2, Matrix3f &H, MatrixXf &A) {
  int psize = pts1.rows();
  A.resize(psize*2, 9);
  for (auto i = 0; i < psize; i++) {
    Vector3f p1 = pts1.row(i);
    Vector3f p2 = pts2.row(i);
    A.row(i*2) << 0, 0, 0, -p1[0], -p1[1], -p1[2], p2[1]*p1[0], p2[1]*p1[1], p2[1]*p1[2];
    A.row(i*2+1) << p1[0], p1[1], p1[2], 0, 0, 0, -p2[0]*p1[0], -p2[0]*p1[1], -p2[0]*p1[2];
  }
  
  JacobiSVD<MatrixXf, HouseholderQRPreconditioner> svd(A, ComputeFullV);
  MatrixXf V = svd.matrixV();
  VectorXf h = V.col(V.cols()-1);
  H = rollVector9f(h);
}

BoundingBox MeshRenderer::getBoundingBox() {
    std::vector<Vector3f> coordinates;
    for(uint i = 0; i < getNrOfInputData(); i++) {
        BoundingBox transformedBoundingBox = mMeshToRender[i]->getTransformedBoundingBox();
        MatrixXf corners = transformedBoundingBox.getCorners();
        for(uint j = 0; j < 8; j++) {
            coordinates.push_back((Vector3f)corners.row(j));
        }
    }
    return BoundingBox(coordinates);
}

void filterPointAtInfinity(MatrixXf &pts1, MatrixXf &pts2) {
  int finiteCount = 0;
  for (int i = 0; i < pts1.rows(); i++) {
    if (abs(pts1(i, 2)) > FLT_EPSILON && abs(pts2(i, 2) > FLT_EPSILON)) 
      finiteCount++;
  }
  MatrixXf temp_pts1, temp_pts2;
  temp_pts1.resize(finiteCount, pts1.cols());
  temp_pts2.resize(finiteCount, pts2.cols());
  int idx = 0;
  for (int i = 0; i < pts1.rows(); i++) {
    if (abs(pts1(i, 2)) > FLT_EPSILON && abs(pts2(i, 2) > FLT_EPSILON)) {
      temp_pts1.row(idx) = pts1.row(i); 
      temp_pts2.row(idx) = pts2.row(i); 
      idx++;
    }
  }
  pts1 = temp_pts1;
  pts2 = temp_pts2;
}

IGL_INLINE void igl::embree::ambient_occlusion(
  const igl::embree::EmbreeIntersector & ei,
  const Eigen::PlainObjectBase<DerivedP> & P,
  const Eigen::PlainObjectBase<DerivedN> & N,
  const int num_samples,
  Eigen::PlainObjectBase<DerivedS> & S)
{
  using namespace Eigen;
  using namespace igl;
  const int n = P.rows();
  // Resize output
  S.resize(n,1);
  // Embree seems to be parallel when constructing but not when tracing rays
#pragma omp parallel for
  // loop over mesh vertices
  for(int p = 0;p<n;p++)
  {
    const Vector3f origin = P.row(p).template cast<float>();
    const Vector3f normal = N.row(p).template cast<float>();
    int num_hits = 0;
    MatrixXf D = random_dir_stratified(num_samples).cast<float>();
    for(int s = 0;s<num_samples;s++)
    {
      //Vector3d d = random_dir();
      Vector3f d = D.row(s);
      if(d.dot(normal) < 0)
      {
        // reverse ray
        d *= -1;
      }
      igl::embree::Hit hit;
      const float tnear = 1e-4f;
      if(ei.intersectRay(origin,d,hit,tnear))
      {
        num_hits++;
      }
    }
    S(p) = (double)num_hits/(double)num_samples;
  }
}

void NeighbourJoining::updateD(MatrixXf& D, const MatrixXf& currentD, const Pair& p,
		int newNode) {
	//calculates distance form all nodes to the new node
	D.row(newNode).head(newNode) = (((D.row(p.iID) + D.row(p.jID))
			- MatrixXf::Constant(1, D.rows(), 1) * D(p.iID, p.jID)) / 2).head(
			newNode);
	D.col(newNode).head(newNode) = D.row(newNode).head(newNode);

	//calculates distances from child nodes to new node
	D(newNode, p.iID) = abs(currentD.row(p.i).head(numCurrentNodes).sum()
			- currentD.row(p.j).head(numCurrentNodes).sum());
	D(newNode, p.iID) /= (2 * (numCurrentNodes - 2));
	D(newNode, p.iID) = D(p.iID, p.jID)/2;
	D(p.iID, newNode) = D(newNode, p.iID);
	D(newNode, p.jID) = D(p.jID, newNode) = D(p.iID, p.jID) - D(p.iID, newNode);
	//cout << "updated D: " << endl << D << endl;
}

// Sample degeneration test, return false if at least three point are colinear
bool sampleValidTest(const MatrixXf &pts1, const MatrixXf &pts2) {
  return !(colinearity(pts1.row(1), pts1.row(2), pts1.row(3)) ||
           colinearity(pts1.row(0), pts1.row(1), pts1.row(2)) ||
           colinearity(pts1.row(0), pts1.row(2), pts1.row(3)) ||
           colinearity(pts1.row(0), pts1.row(1), pts1.row(3)) ||
           colinearity(pts2.row(1), pts2.row(2), pts2.row(3)) ||
           colinearity(pts2.row(0), pts2.row(1), pts2.row(2)) ||
           colinearity(pts2.row(0), pts2.row(2), pts2.row(3)) ||
           colinearity(pts2.row(0), pts2.row(1), pts2.row(3)));
}

void display()
{
  using namespace igl;
  using namespace std;
  using namespace Eigen;
  const float back[4] = {0.75, 0.75, 0.75,0};
  glClearColor(back[0],back[1],back[2],0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  static bool first = true;
  if(first)
  {
    sort();
    first = false;
  }

  if(is_animating)
  {
    double t = (get_seconds() - animation_start_time)/ANIMATION_DURATION;
    if(t > 1)
    {
      t = 1;
      is_animating = false;
    }
    Quaterniond q = animation_from_quat.slerp(t,animation_to_quat).normalized();
    camera.orbit(q.conjugate());
  }

  glEnable(GL_DEPTH_TEST);
  glDepthFunc(GL_LEQUAL);
  glEnable(GL_NORMALIZE);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  lights();
  push_scene();

  // Draw a nice floor
  glEnable(GL_DEPTH_TEST);
  glPushMatrix();
  const double floor_offset =
    -2./bbd*(V.col(1).maxCoeff()-Vmid(1));
  glTranslated(0,floor_offset,0);
  const float GREY[4] = {0.5,0.5,0.6,1.0};
  const float DARK_GREY[4] = {0.2,0.2,0.3,1.0};
  glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
  glEnable(GL_CULL_FACE);
  glCullFace(GL_BACK);
  draw_floor(GREY,DARK_GREY);
  glDisable(GL_CULL_FACE);
  glPopMatrix();

  push_object();

  const auto & draw_skeleton = []()
  {
    switch(skel_style)
    {
      default:
      case SKEL_STYLE_TYPE_3D:
      {
        MatrixXf colors = MAYA_VIOLET.transpose().replicate(s.BE.rows(),1);
        for(int si=0;si<s.sel.size();si++)
        {
          for(int b=0;b<s.BE.rows();b++)
          {
            if(s.BE(b,0) == s.sel(si) || s.BE(b,1) == s.sel(si))
            {
              colors.row(b) = MAYA_SEA_GREEN;
            }
          }
        }
        draw_skeleton_3d(s.C,s.BE,MatrixXd(),colors);
        break;
      }
      case SKEL_STYLE_TYPE_VECTOR_GRAPHICS:
        draw_skeleton_vector_graphics(s.C,s.BE);
        break;
    }
  };
  
  if(!skeleton_on_top)
  {
    draw_skeleton();
  }

  // Set material properties
  glDisable(GL_COLOR_MATERIAL);
  glMaterialfv(GL_FRONT, GL_AMBIENT,
    Vector4f(GOLD_AMBIENT[0],GOLD_AMBIENT[1],GOLD_AMBIENT[2],alpha).data());
  glMaterialfv(GL_FRONT, GL_DIFFUSE,
    Vector4f(GOLD_DIFFUSE[0],GOLD_DIFFUSE[1],GOLD_DIFFUSE[2],alpha).data());
  glMaterialfv(GL_FRONT, GL_SPECULAR,
    Vector4f(GOLD_SPECULAR[0],GOLD_SPECULAR[1],GOLD_SPECULAR[2],alpha).data());
  glMaterialf (GL_FRONT, GL_SHININESS, 128);
  glMaterialfv(GL_BACK, GL_AMBIENT,
    Vector4f(SILVER_AMBIENT[0],SILVER_AMBIENT[1],SILVER_AMBIENT[2],alpha).data());
  glMaterialfv(GL_BACK, GL_DIFFUSE,
    Vector4f(FAST_GREEN_DIFFUSE[0],FAST_GREEN_DIFFUSE[1],FAST_GREEN_DIFFUSE[2],alpha).data());
  glMaterialfv(GL_BACK, GL_SPECULAR,
    Vector4f(SILVER_SPECULAR[0],SILVER_SPECULAR[1],SILVER_SPECULAR[2],alpha).data());
//.........这里部分代码省略.........

IplImage* CloudProjection::computeProjection(const sensor_msgs::PointCloud& data,
					     const std::vector<int>& interest_region_indices)
{
  // -- Put cluster points into matrix form.
  MatrixXf points(interest_region_indices.size(), 3);
  for(size_t i=0; i<interest_region_indices.size(); ++i) {
    points(i, 0) = data.points[interest_region_indices[i]].x;
    points(i, 1) = data.points[interest_region_indices[i]].y;
    points(i, 2) = data.points[interest_region_indices[i]].z;
  }

  // -- Subtract off the mean and flatten to z=0 to prepare for PCA.
  MatrixXf X = points;
  X.col(2) = VectorXf::Zero(X.rows());
  VectorXf pt_mean = X.colwise().sum() / (float)X.rows();
  for(int i=0; i<X.rows(); ++i) {
    X.row(i) -= pt_mean.transpose();
  }
  MatrixXf Xt = X.transpose();
  
  // -- Find the long axis.
  // Start with a random vector.
  VectorXf pc = VectorXf::Zero(3);
  pc(0) = 1; //Chosen by fair dice roll.
  pc(1) = 1;
  pc.normalize();
  
  // Power method.
  VectorXf prev = pc;
  double thresh = 1e-4;
  int ctr = 0;
  while(true) { 
    prev = pc;
    pc =  Xt * (X * pc);
    pc.normalize();
    ctr++;
    if((pc - prev).norm() < thresh)
      break;
  }
  assert(abs(pc(2)) < 1e-4);
  
  // -- Find the short axis.
  VectorXf shrt = VectorXf::Zero(3);
  shrt(1) = -pc(0);
  shrt(0) = pc(1);
  assert(abs(shrt.norm() - 1) < 1e-4);
  assert(abs(shrt.dot(pc)) < 1e-4);
  
  // -- Build the basis of normalized coordinates.
  MatrixXf basis = MatrixXf::Zero(3,3);
  basis.col(0) = pc;
  basis.col(1) = shrt;
  basis(2,2) = -1.0;
  assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
  assert(abs(basis.col(0).norm() - 1) < 1e-4);
  assert(abs(basis.col(1).norm() - 1) < 1e-4);
  assert(abs(basis.col(2).norm() - 1) < 1e-4);


  // -- Put the cluster into normalized coordinates, and choose which axis to project on.
  MatrixXf projected_basis(3, 2);
  if(axis_ == 0) { 
    projected_basis.col(0) = basis.col(1);
    projected_basis.col(1) = basis.col(2);
  }
  else if(axis_ == 1) { 
    projected_basis.col(0) = basis.col(0);
    projected_basis.col(1) = basis.col(2);
  }
  else if(axis_ == 2) { 
    projected_basis.col(0) = basis.col(0);
    projected_basis.col(1) = basis.col(1);
  }
  MatrixXf projected = points * projected_basis;
    
  // -- Transform into pixel units.
  for(int i=0; i<projected.rows(); ++i) {
    projected(i, 0) *= pixels_per_meter_;
    projected(i, 1) *= pixels_per_meter_;
  }

  // -- Find min and max of u and v.  TODO: noise sensitivity?
  float min_v = FLT_MAX;
  float min_u = FLT_MAX;
  float max_v = -FLT_MAX;
  float max_u = -FLT_MAX;
  for(int i=0; i<projected.rows(); ++i) {
    float u = projected(i, 0);
    float v = projected(i, 1);
    if(u < min_u)
      min_u = u;
    if(u > max_u)
      max_u = u;
    if(v < min_v)
      min_v = v;
    if(v > max_v)
      max_v = v;
  }

  // -- Shift the origin based on {u,v}_offset_pct. 
//.........这里部分代码省略.........

int EMclustering::EM(int k, int *IDX, bool spatial, bool att)
{
	clusternum = k;
	MatrixXf x;
	/*if(spatial)
	{
		if(att)
		{
			x.resize(4,dataSize);
			for(int i=0;i<dataSize;i++)
			{
				x(0,i) = dataPos[i][0];
				x(1,i) = dataPos[i][1];
				x(2,i) = dataPos[i][2];
				x(3,i) = dataDen[i];
			}
		}
		else
		{
			x.resize(6,dataSize);
			for(int i=0;i<dataSize;i++)
			{
				x(0,i) = dataPos[i][0];
				x(1,i) = dataPos[i][1];
				x(2,i) = dataPos[i][2];
				x(3,i) = dataVel[i][0];
				x(4,i) = dataVel[i][1];
				x(5,i) = dataVel[i][2];
			}
		}
	}
	else
	{*/
		if(att)
		{
			x.resize(1,dataDen.size());
			for(int i=0;i<dataDen.size();i++)
			{
				x(0,i) = dataDen[i];
			}
			//cerr<<x;
			//cerr<<endl;
			if(k>dataDen.size())
				return -1;
		}
		else
		{
			x.resize(3,dataSize);
			for(int i=0;i<dataSize;i++)
			{
				x(0,i) = dataVel[i][0];//fabs(cos(-PI/4)*dataVel[i][0] - sin(-PI/4)*dataVel[i][1]);
				x(1,i) = dataVel[i][1];//fabs(sin(-PI/4)*dataVel[i][0] + cos(-PI/4)*dataVel[i][1]);
				x(2,i) = dataVel[i][2];
			}
			if(k>dataSize)
				return -1;
		}
	//}

	//cout<<"EM for Gaussian mixture: running ... "<<endl;
	//cerr<<x<<endl;
	MatrixXf r =initialization(x,k);// kmeans(x,k);//
	//cerr<<"Initialization is Done"<<endl;//cerr<<r<<endl;
	VectorXi label(r.rows());
	for(int i=0;i<r.rows();i++)
	{
		int index;
		float tmp1 = r.row(i).maxCoeff(&index);
		label(i) = index;
	}//cerr<<label<<endl;
	VectorXi tmpp(label.size()); 
	VectorXi tmp2 = unique(label,tmpp);
	int tmpd = tmp2.size();  //cerr<<tmpd<<endl;
	MatrixXf tmpr(r.rows(),tmpd);
	for(int i=0;i<tmpd;i++)
	{
		tmpr.col(i) = r.col(tmp2(i));
	}//cerr<<"done1"<<endl;
	r.resize(r.rows(),tmpd);
	r = tmpr;//cerr<<r.cols()<<endl;
	float tol = 1e-10;
	int max = 300;

	double llh = -9e+9;
	bool converged = false;
	int t = 1;
	//cerr<<"done1"<<endl;
	//gaussian_model model;

	int clusternum_error;

	MatrixXf tmpmodel;

	while(!converged&&t<max)
	{
		t = t + 1;
		gaussian_model model = maximization(x,r);//cerr<<t<<" "<<"max"<<endl;
		float tmpllh = llh;
		r = expectation(x,model,llh);//cerr<<t<<" "<<"exp"<<endl;
		for(int i=0;i<r.rows();i++)
//.........这里部分代码省略.........

void TMSI::run()
{
    while(m_bIsRunning)
    {
        //std::cout<<"TMSI::run(s)"<<std::endl;

        //pop matrix only if the producer thread is running
        if(m_pTMSIProducer->isRunning())
        {
            MatrixXf matValue = m_pRawMatrixBuffer_In->pop();

            // Set Beep trigger (if activated)
            if(m_bBeepTrigger && m_qTimerTrigger.elapsed() >= m_iTriggerInterval)
            {
                QFuture<void> future = QtConcurrent::run(Beep, 450, 700);
                //Set trigger in received data samples - just for one sample, so that this event is easy to detect
                matValue(136, m_iSamplesPerBlock-1) = 252;
                m_qTimerTrigger.restart();

                Q_UNUSED(future);
            }

            // Set keyboard trigger (if activated and !=0)
            if(m_bUseKeyboardTrigger && m_iTriggerType!=0)
                matValue(136, m_iSamplesPerBlock-1) = m_iTriggerType;

            //Write raw data to fif file
            if(m_bWriteToFile)
                m_pOutfid->write_raw_buffer(matValue.cast<double>(), m_cals);

            // TODO: Use preprocessing if wanted by the user
            if(m_bUseFiltering)
            {
                MatrixXf temp = matValue;

                matValue = matValue - m_matOldMatrix;
                m_matOldMatrix = temp;

                //    //Check filter class - will be removed in the future - testing purpose only!
                //    FilterTools* filterObject = new FilterTools();

                //    //kaiser window testing
                //    qint32 numberCoeff = 51;
                //    QVector<float> impulseResponse(numberCoeff);
                //    filterObject->createDynamicFilter(QString('LP'), numberCoeff, (float)0.3, impulseResponse);

                //    ofstream outputFileStream("mne_x_plugins/resources/tmsi/filterToolsTest.txt", ios::out);

                //    outputFileStream << "impulseResponse:\n";
                //    for(int i=0; i<impulseResponse.size(); i++)
                //        outputFileStream << impulseResponse[i] << " ";
                //    outputFileStream << endl;

                //    //convolution testing
                //    QVector<float> in (12, 2);
                //    QVector<float> kernel (4, 2);

                //    QVector<float> out = filterObject->convolve(in, kernel);

                //    outputFileStream << "convolution result:\n";
                //    for(int i=0; i<out.size(); i++)
                //        outputFileStream << out[i] << " ";
                //    outputFileStream << endl;
            }

            // TODO: Perform a fft if wanted by the user
            if(m_bUseFFT)
            {
                QElapsedTimer timer;
                timer.start();

                FFT<float> fft;
                Matrix<complex<float>, 138, 16> freq;

                for(qint32 i = 0; i < matValue.rows(); ++i)
                    fft.fwd(freq.row(i), matValue.row(i));

//                cout<<"FFT postprocessing done in "<<timer.nsecsElapsed()<<" nanosec"<<endl;
//                cout<<"matValue before FFT:"<<endl<<matValue<<endl;
//                cout<<"freq after FFT:"<<endl<<freq<<endl;
//                matValue = freq.cwiseAbs();
//                cout<<"matValue after FFT:"<<endl<<matValue<<endl;
            }

            //Change values of the trigger channel for better plotting - this change is not saved in the produced fif file
            if(m_iNumberOfChannels>137)
            {
                for(int i = 0; i<matValue.row(137).cols(); i++)
                {
                    // Left keyboard or capacitive
                    if(matValue.row(136)[i] == 254)
                        matValue.row(136)[i] = 4000;

                    // Right keyboard
                    if(matValue.row(136)[i] == 253)
                        matValue.row(136)[i] = 8000;

                    // Beep
                    if(matValue.row(136)[i] == 252)
                        matValue.row(136)[i] = 2000;
//.........这里部分代码省略.........

bool multiModelRANSAC(const MatrixXf &data, int M, MatrixXf &inlier) {
  int maxdegen = 10;
  int dataSize = data.rows();
  int psize = 4;
  int blockSize = 10;
  MatrixXf x1 = data.block(0, 0, data.rows(), 3);
  MatrixXf x2 = data.block(0, 3, data.rows(), 3);
  vector<int> sample;
  MatrixXf pts1(4, 3);
  MatrixXf pts2(4, 3);

  int h = 0;
  MatrixXf Hs(M, 9);
  MatrixXf inx(M, psize);
  MatrixXf res(dataSize, M);
  MatrixXi resIndex(dataSize, M);

  for (int m = 0; m < M; m++) {
    int degencount = 0;
    int isdegen = 1;
    
    while (isdegen==1 && degencount < maxdegen) {
      degencount++;
      if (m < blockSize)
        RandomSampling(psize, dataSize, sample);
      else 
        WeightedSampling(psize, dataSize, resIndex, sample, h);
      for (int i = 0; i < psize; i++) {
        pts1.row(i) = x1.row(sample[i]);
        pts2.row(i) = x2.row(sample[i]);
      }
      if (sampleValidTest(pts1, pts2))
        isdegen = 0;
    }
    if (isdegen) {
      cout << "Cannot find valid p-subset" << endl;
      return false;
    }
    for (int i = 0; i < psize; i++)
      inx(m, i) = sample[i]; 

    Matrix3f temp_H;
    MatrixXf temp_A, localResidue;
    fitHomography(pts1, pts2, temp_H, temp_A);
    computeHomographyResidue(x1, x2, temp_H, localResidue);
    Hs.row(m) = unrollMatrix3f(temp_H);
    res.col(m) = localResidue;
    if (m >= (blockSize-1) && (m+1)%blockSize == 0) {
      h = round(0.1f*m);
      sortResidueForIndex(res, (m/blockSize)*blockSize, ((m+1)/blockSize)*blockSize, resIndex);
    }
  }

  VectorXf bestModel(M);
  bestModel.setZero();
  int bestIndex = 0;
  int bestCount = -1;
  for (int i = 0; i < M; i++) {
    for (int j = 0; j < dataSize; j++) 
      if (res(j, i) < THRESHOLD)
        bestModel(i) += 1;
    if (bestModel(i) > bestCount) {
      bestIndex = i;
      bestCount = bestModel(i);
    }
  }

  VectorXf bestModelRes = res.col(bestIndex);
  int inlierCount = (bestModelRes.array() < THRESHOLD).count();
  inlier.resize(inlierCount, data.cols());
  int runningIdx = 0;
  for (int i = 0; i < dataSize; i++) 
    if (bestModelRes(i) < THRESHOLD) {
      inlier.row(runningIdx) = data.row(i);
      runningIdx ++;
    }

  return true;
}