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

		/** Randomly selects a point in the given tree node by considering probabilities 
		 * Runtime: O(S*S + log(S*S))
		 */
		inline Eigen::Vector2f ProbabilityCellPoint(const Eigen::MatrixXf& m0, int scale, int x, int y)
		{
			x *= scale;
			y *= scale;
			const auto& b = m0.block(x, y, scale, scale);
			// build cdf
			std::vector<float> cdf(scale*scale);
			for(int i=0; i<scale; ++i) {
				for(int j=0; j<scale; ++j) {
					float v = b(j,i);
					int q = scale*i + j;
					if(q > 0) {
						cdf[q] = cdf[q-1] + v;
					}
					else {
						cdf[q] = v;
					}
				}
			}
			// sample in cdf
			boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > rnd(
					Rnd(), boost::uniform_real<float>(0.0f, cdf.back()));
			float v = rnd();
			// find sample
			auto it = std::lower_bound(cdf.begin(), cdf.end(), v);
			int pos = std::distance(cdf.begin(), it);
			return Eigen::Vector2f(x + pos%scale, y + pos/scale);
		}

	bool ZMeshAlgorithms::runBilateralFiltering(float sigma_r, float sigma_s)
	{

		int nSize = mesh_->get_num_of_faces_list();
		int spatialDim = 3;
		int rangeDim = 3;

		Eigen::MatrixXf faceNormals = MeshTools::getAllFaceNormals(mesh_);
		//Eigen::MatrixXf faceNormals = MeshTools::getAllFaceNormalsSpherical(mesh_);
		Eigen::MatrixXf faceCenters = MeshTools::getAllFaceCenters(mesh_);
		Eigen::MatrixXf verticePos = MeshTools::getAllVerticePos(mesh_);
		Eigen::MatrixXf faceAttributes(nSize, spatialDim+rangeDim);
		faceAttributes.block(0, 0, nSize, spatialDim) = faceCenters;
		faceAttributes.block(0, spatialDim, nSize, rangeDim) = faceNormals;
		AnnWarper_Eigen annSearch;
		annSearch.init(faceCenters);
		ZMeshBilateralFilter filter(mesh_);
		filter.setAnnSearchHandle(&annSearch);
		float sigma_spatial = mesh_->m_minEdgeLength*sigma_s;
		float sigma_range = cos(sigma_r*Z_PI/180.f);
		//filter.setKernelFunc(NULL);
		filter.setPara(ZMeshFilterParaNames::SpatialSigma, sigma_spatial);
		filter.setPara(ZMeshFilterParaNames::RangeSigma, sigma_range);
		filter.apply(faceAttributes, std::vector<bool>(nSize, true));
		Eigen::MatrixXf output = filter.getResult();

		MeshTools::setAllFaceNormals2(mesh_, output.block(0, 3, nSize, 3));
		//MeshTools::setAllFaceNormal2Spherical(mesh_, output.block(0, spatialDim, nSize, rangeDim));

		return true;
	}

	void addEigenMatrixRow( Eigen::MatrixXf &m )
	{
		Eigen::MatrixXf temp=m;
		m.resize(m.rows()+1,m.cols());
		m.setZero();
		m.block(0,0,temp.rows(),temp.cols())=temp;
	}

	void ZMeshAlgorithms::updateRange()
	{
		pMeshFilterManifold_->updateRange();
		Eigen::MatrixXf output = pMeshFilterManifold_->getResult();
		meshNewNormals_ = output.block(0, pMeshFilterManifold_->getSpatialDim(), pMeshFilterManifold_->getPointSize(), pMeshFilterManifold_->getRangeDim());
		MeshTools::setAllFaceNormals2(mesh_, meshNewNormals_);
		//MeshTools::setAllFaceNormal2Spherical(mesh_, meshNewNormals_);
	}

	bool ZMeshAlgorithms::runManifoldSmooth(float sigma_r, float sigma_s)
	{
		SAFE_DELETE(pMeshFilterManifold_);
		int nSize = mesh_->get_num_of_faces_list();
		//int nSize = mesh_->get_num_of_vertex_list();
		int spatialDim = 3;
		int rangeDim = 3;
		Eigen::MatrixXf faceNormals = MeshTools::getAllFaceNormals(mesh_);
		//Eigen::MatrixXf faceNormals = MeshTools::getAllFaceNormalsSpherical(mesh_);
		Eigen::MatrixXf faceCenters = MeshTools::getAllFaceCenters(mesh_);
		Eigen::MatrixXf verticePos = MeshTools::getAllVerticePos(mesh_);
		Eigen::MatrixXf faceAttributes(nSize, spatialDim+rangeDim);
		faceAttributes.block(0, 0, nSize, spatialDim) = faceCenters;
		faceAttributes.block(0, 3, nSize, rangeDim) = faceNormals;
// 		faceAttributes.block(0, 0, nSize, spatialDim) = verticePos;
// 		faceAttributes.block(0, 3, nSize, rangeDim) = verticePos;
		//ZMeshFilterManifold filter(mesh_);
		pMeshFilterManifold_ = new ZMeshFilterManifold(mesh_);
		float sigma_spatial = mesh_->m_minEdgeLength*sigma_s;
		float sigma_range = sin(sigma_r*Z_PI/180.f);
		pMeshFilterManifold_->setPara(ZMeshFilterParaNames::SpatialSigma, sigma_spatial);
		pMeshFilterManifold_->setPara(ZMeshFilterParaNames::RangeSigma, sigma_range);
		pMeshFilterManifold_->setRangeDim(rangeDim);
		pMeshFilterManifold_->setSpatialDim(spatialDim);
		CHECK_FALSE_AND_RETURN(pMeshFilterManifold_->apply(faceAttributes, std::vector<bool>(nSize, true)));
		Eigen::MatrixXf output = pMeshFilterManifold_->getResult();
		meshNewNormals_ = output.block(0, spatialDim, nSize, rangeDim);
		MeshTools::setAllFaceNormals2(mesh_, meshNewNormals_);
		MeshTools::setAllFaceColorValue(mesh_, pMeshFilterManifold_->getPointClusterIds());
		//MeshTools::setAllVerticePos(mesh_, output.block(0, spatialDim, nSize, rangeDim));
		//MeshTools::setAllFaceNormal2Spherical(mesh_, output.block(0, spatialDim, nSize, rangeDim));

		ZFileHelper::saveEigenMatrixToFile("oldNormal.txt", faceNormals);
		ZFileHelper::saveEigenMatrixToFile("newNormal.txt", output.block(0,spatialDim, nSize, rangeDim));

		return true;
	}

	void ZMeshFilterManifold::init(const Eigen::MatrixXf& input)
	{
		destroy();
		computeTreeHeight();
		initMinPixelDist2Manifold();
		// init search tool
		pAnnSearch_ = new AnnWarper_Eigen;
		//Eigen::MatrixXf faceCenters = MeshTools::getAllFaceCenters(pMesh_);
		//Eigen::MatrixXf faceNormals = MeshTools::getAllFaceNormals(pMesh_);
		pAnnSearch_->init(input.block(0, 0, input.rows(), spatialDim_));
		// init gaussian filter
		pGaussianFilter_ = new ZMeshBilateralFilter(pMesh_);
		ZMeshBilateralFilter* pFilter = (ZMeshBilateralFilter*)pGaussianFilter_;
		pFilter->setAnnSearchHandle(pAnnSearch_);
	}

template <typename PointT> void
pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
                       const Eigen::Vector4f &centroid,
                       Eigen::MatrixXf &cloud_out)
{
  size_t npts = cloud_in.points.size ();

  cloud_out = Eigen::MatrixXf::Zero (4, npts);        // keep the data aligned

  for (size_t i = 0; i < npts; ++i)
    // One column at a time
    cloud_out.block<4, 1> (0, i) = cloud_in.points[i].getVector4fMap () - centroid;

  // Make sure we zero the 4th dimension out (1 row, N columns)
  cloud_out.block (3, 0, 1, npts).setZero ();
}

IGL_INLINE void igl::fit_rotations_AVX(
  const Eigen::MatrixXf & S,
  Eigen::MatrixXf & R)
{
  const int cStep = 8;

  assert(S.cols() == 3);
  const int dim = 3; //S.cols();
  const int nr = S.rows()/dim;  
  assert(nr * dim == S.rows());

  // resize output
  R.resize(dim,dim*nr); // hopefully no op (should be already allocated)

  Eigen::Matrix<float, 3*cStep, 3> siBig;
  // using SSE decompose cStep matrices at a time:
  int r = 0;
  for( ; r<nr; r+=cStep)
  {
    int numMats = cStep;
    if (r + cStep >= nr) numMats = nr - r;
    // build siBig:
    for (int k=0; k<numMats; k++)
    {
      for(int i = 0;i<dim;i++)
      {
        for(int j = 0;j<dim;j++)
        {
          siBig(i + 3*k, j) = S(i*nr + r + k, j);
        }
      }
    }
    Eigen::Matrix<float, 3*cStep, 3> ri;
    polar_svd3x3_avx(siBig, ri);    

    for (int k=0; k<cStep; k++)
      assert(ri.block(3*k, 0, 3, 3).determinant() >= 0);

    // Not sure why polar_dec computes transpose...
    for (int k=0; k<numMats; k++)
    {
      R.block(0, (r + k)*dim, dim, dim) = ri.block(3*k, 0, dim, dim).transpose();
    }    
  }
}

/**
 * DemeanPoints
 */
void RigidTransformationRANSAC::DemeanPoints (
      const std::vector<Eigen::Vector3f > &inPts, 
      const std::vector<int> &indices,
      const Eigen::Vector3f &centroid,
      Eigen::MatrixXf &outPts)
{
  if (inPts.size()==0 || indices.size()==0)
  {
    return;
  }

  outPts = Eigen::MatrixXf(4, indices.size());

  // Subtract the centroid from points
  for (unsigned i = 0; i < indices.size(); i++)
    outPts.block<3,1>(0,i) = inPts[indices[i]]-centroid;

  outPts.block(3, 0, 1, indices.size()).setZero();
}

		/** Selects the point in the tree node with highest probability */ 
		inline Eigen::Vector2f OptimalCellPoint(const Eigen::MatrixXf& m0, int scale, int x, int y)
		{
			x *= scale;
			y *= scale;
			const auto& b = m0.block(x, y, scale, scale);
			unsigned int best_j=scale/2, best_i=scale/2;
			float best_val = -1000000.0f;
			for(unsigned int i=0; i<b.cols(); ++i) {
				for(unsigned int j=0; j<b.rows(); ++j) {
					float val = b(j,i);
					if(val > best_val) {
						best_j = j;
						best_i = i;
						best_val = val;
					}
				}
			}
			return Eigen::Vector2f(x + best_j, y + best_i);
		}

void compressFeature( string filename, std::vector< std::vector<float> > &models, const int dim, bool ascii ){
  PCA pca;
  pca.read( filename.c_str(), ascii );
  Eigen::VectorXf variance = pca.getVariance();
  Eigen::MatrixXf tmpMat = pca.getAxis();
  Eigen::MatrixXf tmpMat2 = tmpMat.block(0,0,tmpMat.rows(),dim);
  const int num = (int)models.size();
  for( int i=0; i<num; i++ ){
    Eigen::Map<Eigen::VectorXf> vec( &(models[i][0]), models[i].size() );
    //vec = tmpMat2.transpose() * vec;
    Eigen::VectorXf tmpvec = tmpMat2.transpose() * vec;
    models[i].resize( dim );
    if( WHITENING ){
      for( int t=0; t<dim; t++ )
	models[i][t] = tmpvec[t] / sqrt( variance( t ) );
    }
    else{
      for( int t=0; t<dim; t++ )
	models[i][t] = tmpvec[t];
    }
  }
}

Eigen::MatrixXf LQRControler::Controler(Eigen::MatrixXf states,Eigen::MatrixXf ref,bool stop){

	if(stop){
		xs.setZero();
		xr.setZero();
		deltaxsi.setZero();
		xsi.setZero();
		xsiant.setZero();
		deltaxsi.setZero();
		deltaxsiant.setZero();
		deltaxs.setZero();
		xs_aumented.setZero();
		deltaxs.setZero();
		xsi.setZero();
		deltaU.setZero();
		xs_aumented.setZero();
		ur.setZero();
		auxu.setZero();
		ur.setZero();
		deltaU.setZero();
		xsiant.setZero();
		xsi.setZero();
		deltaxsiant.setZero();
		deltaxsi.setZero();
	}else{
		//Vectors of reference trajectory and control
		xs<<0,0,states(2),states.block(3,0,5,1),0,0,states(10),states.block(11,0,5,1);
		xr=trajectory->TrajetoryReference_LQR(ref);

		//Vector integration of error(Trapezoidal method)
		deltaxsi<<xs(2,0)-xr(2,0),xs(5,0)-xr(5,0);
		xsi=xsiant+ts*(deltaxsi+deltaxsiant)/2;

		// Error state vector
		deltaxs=xs-xr;
		// augmented error state vector
		xs_aumented<<deltaxs,xsi;
		//Control action variation
		deltaU=Ke*xs_aumented;
		//Control reference
		ur<<9857.54,9837.48,0,0;
		// Total control action
		auxu=ur+deltaU;
		//Variable update
		xsiant=xsi;
		deltaxsiant=deltaxsi;
	}

	if(auxu(0,0)>15000 ){
		auxu(0,0)=15000;
	}
	if(auxu(1,0)>15000 ){
		auxu(1,0)=15000;
	}
	/*The mass in the mathematical model was taken in grams,
	for this reason the controller calculate the forces in g .m/s^2 and the torque in g .m^2/s^2.
	But, the actuators are in the international units N and N. m for this reason the controls
	actions are transforming from g to Kg*/
	u(0,0)=auxu(0,0)/1000;
	u(1,0)=auxu(1,0)/1000;
	u(2,0)=auxu(2,0)/1000;
	u(3,0)=auxu(3,0)/1000;

	return u;
}

template<typename PointT> void
pcl::registration::LUM<PointT>::compute ()
{
  int n = static_cast<int> (getNumVertices ());
  if (n < 2)
  {
    PCL_ERROR("[pcl::registration::LUM::compute] The slam graph needs at least 2 vertices.\n");
    return;
  }
  for (int i = 0; i < max_iterations_; ++i)
  {
    // Linearized computation of C^-1 and C^-1*D and convergence checking for all edges in the graph (results stored in slam_graph_)
    typename SLAMGraph::edge_iterator e, e_end;
    for (boost::tuples::tie (e, e_end) = edges (*slam_graph_); e != e_end; ++e)
      computeEdge (*e);

    // Declare matrices G and B
    Eigen::MatrixXf G = Eigen::MatrixXf::Zero (6 * (n - 1), 6 * (n - 1));
    Eigen::VectorXf B = Eigen::VectorXf::Zero (6 * (n - 1));

    // Start at 1 because 0 is the reference pose
    for (int vi = 1; vi != n; ++vi)
    {
      for (int vj = 0; vj != n; ++vj)
      {
        // Attempt to use the forward edge, otherwise use backward edge, otherwise there was no edge
        Edge e;
        bool present1, present2;
        boost::tuples::tie (e, present1) = edge (vi, vj, *slam_graph_);
        if (!present1)
        {
          boost::tuples::tie (e, present2) = edge (vj, vi, *slam_graph_);
          if (!present2)
            continue;
        }

        // Fill in elements of G and B
        if (vj > 0)
          G.block (6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_;
        G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_;
        B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_;
      }
    }

    // Computation of the linear equation system: GX = B
    // TODO investigate accuracy vs. speed tradeoff and find the best solving method for our type of linear equation (sparse)
    Eigen::VectorXf X = G.colPivHouseholderQr ().solve (B);

    // Update the poses
    float sum = 0.0;
    for (int vi = 1; vi != n; ++vi)
    {
      Eigen::Vector6f difference_pose = static_cast<Eigen::Vector6f> (-incidenceCorrection (getPose (vi)).inverse () * X.segment (6 * (vi - 1), 6));
      sum += difference_pose.norm ();
      setPose (vi, getPose (vi) + difference_pose);
    }

    // Convergence check
    if (sum <= convergence_threshold_ * static_cast<float> (n - 1))
      return;
  }
}

//.........这里部分代码省略.........
  rank_num = atoi( argv[2] );

  // set marker color
  const int marker_model_num = 6;
  if( model_num > marker_model_num ){
    std::cerr << "Please set more marker colors for detection of more than " << marker_model_num << " objects." << std::endl;
    exit( EXIT_FAILURE );
  }
  marker_color_r = new float[ marker_model_num ];
  marker_color_g = new float[ marker_model_num ];
  marker_color_b = new float[ marker_model_num ];
  marker_color_r[ 0 ] = 1.0; marker_color_g[ 0 ] = 0.0; marker_color_b[ 0 ] = 0.0;  // red
  marker_color_r[ 1 ] = 0.0; marker_color_g[ 1 ] = 1.0; marker_color_b[ 1 ] = 0.0;  // green
  marker_color_r[ 2 ] = 0.0; marker_color_g[ 2 ] = 0.0; marker_color_b[ 2 ] = 1.0;  // blue
  marker_color_r[ 3 ] = 1.0; marker_color_g[ 3 ] = 1.0; marker_color_b[ 3 ] = 0.0;  // yellow
  marker_color_r[ 4 ] = 0.0; marker_color_g[ 4 ] = 1.0; marker_color_b[ 4 ] = 1.0;  // cyan
  marker_color_r[ 5 ] = 1.0; marker_color_g[ 5 ] = 0.0; marker_color_b[ 5 ] = 1.0;  // magenta
  // marker_color_r[ 0 ] = 0.0; marker_color_g[ 0 ] = 1.0; marker_color_b[ 0 ] = 0.0; // green
  // marker_color_r[ 1 ] = 0.0; marker_color_g[ 1 ] = 0.0; marker_color_b[ 1 ] = 1.0; // blue
  // marker_color_r[ 2 ] = 0.0; marker_color_g[ 2 ] = 1.0; marker_color_b[ 2 ] = 1.0; // cyan
  // marker_color_r[ 3 ] = 1.0; marker_color_g[ 3 ] = 0.0; marker_color_b[ 3 ] = 0.0; // pink

  // read the number of voxels in each subdivision's side of scene
  box_size = Param::readBoxSizeScene( tmpname );

  // read the dimension of compressed feature vectors
  dim = Param::readDim( tmpname );
  const int dim_model = atoi(argv[5]);
  if( dim <= dim_model ){
    std::cerr << "ERR: dim_model should be less than dim(in dim.txt)" << std::endl;
    exit( EXIT_FAILURE );
  }

  // read the threshold for RGB binalize
  sprintf( tmpname, "%s/param/color_threshold.txt", argv[1] );
  Param::readColorThreshold( color_threshold_r, color_threshold_g, color_threshold_b, tmpname );

  // determine the size of sliding box
  region_size = box_size * voxel_size;
  float tmp_val = atof(argv[6]) / region_size;
  int size1 = (int)tmp_val;
  if( ( ( tmp_val - size1 ) >= 0.5 ) || ( size1 == 0 ) ) size1++;
  tmp_val = atof(argv[7]) / region_size;
  int size2 = (int)tmp_val;
  if( ( ( tmp_val - size2 ) >= 0.5 ) || ( size2 == 0 ) ) size2++;
  tmp_val = atof(argv[8]) / region_size;
  int size3 = (int)tmp_val;
  if( ( ( tmp_val - size3 ) >= 0.5 ) || ( size3 == 0 ) ) size3++;
  sliding_box_size_x = size1 * region_size;
  sliding_box_size_y = size2 * region_size;
  sliding_box_size_z = size3 * region_size;

  // set variables
  search_obj.setModelNum( model_num );
#ifdef CCHLAC_TEST
  sprintf( tmpname, "%s/param/max_c.txt", argv[1] );
#else
  sprintf( tmpname, "%s/param/max_r.txt", argv[1] );
#endif
  search_obj.setNormalizeVal( tmpname );
  search_obj.setRange( size1, size2, size3 );
  search_obj.setRank( rank_num * model_num ); // for removeOverlap()
  search_obj.setThreshold( atoi(argv[3]) );

  // read projection axes of the target objects' subspace
  FILE *fp = fopen( argv[4], "r" );
  char **model_file_names = new char* [ model_num ];
  char line[ 1000 ];
  for( int i=0; i<model_num; i++ ){
    model_file_names[ i ] = new char [ 1000 ];
    if( fgets( line, sizeof(line), fp ) == NULL ) std::cerr<<"fgets err"<<std::endl;
    line[ strlen( line ) - 1 ] = '\0';
    //fscanf( fp, "%s\n", model_file_names + i );
    //model_file_names[ i ] = line;
    sprintf( model_file_names[ i ], "%s", line );
    //std::cout << model_file_names[ i ] << std::endl;
  }
  fclose(fp);
  search_obj.readAxis( model_file_names, dim, dim_model, ASCII_MODE_P, MULTIPLE_SIMILARITY );

  // read projection axis for feature compression
  PCA pca;
  sprintf( tmpname, "%s/models/compress_axis", argv[1] );
  pca.read( tmpname, ASCII_MODE_P );
  Eigen::MatrixXf tmpaxis = pca.getAxis();
  Eigen::MatrixXf axis = tmpaxis.block( 0,0,tmpaxis.rows(),dim );
  Eigen::MatrixXf axis_t = axis.transpose();
  Eigen::VectorXf variance = pca.getVariance();
  if( WHITENING )
    search_obj.setSceneAxis( axis_t, variance, dim );
  else
    search_obj.setSceneAxis( axis_t );

  // object detection
  VoxelizeAndDetect vad;
  vad.loop();
  ros::spin();

  return 0;
}

template<typename PointT> inline void
pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag)
{
    if (!compute_done_)
        initCompute ();
    if (!compute_done_)
        PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");

    Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
    const size_t n = eigenvectors_.cols ();// number of eigen vectors
    Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
    Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
    Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
    Eigen::VectorXf h = y - input;
    if (h.norm() > 0)
        h.normalize ();
    else
        h.setZero ();
    float gamma = h.dot(input - mean_.head<3>());
    Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
    D.block(0,0,n,n) = a * a.transpose();
    D /=  float(n)/float((n+1) * (n+1));
    for(std::size_t i=0; i < a.size(); i++) {
        D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
        D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
        D(i,D.cols()-1) = D(D.rows()-1,i);
        D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
    }

    Eigen::MatrixXf R(D.rows(), D.cols());
    Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
    Eigen::VectorXf alphap = D_evd.eigenvalues().real();
    eigenvalues_.resize(eigenvalues_.size() +1);
    for(std::size_t i=0; i<eigenvalues_.size(); i++) {
        eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
        R.col(i) = D.col(D.cols()-i-1);
    }
    Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
    Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
    Up.rightCols<1>() = h;
    eigenvectors_ = Up*R;
    if (!basis_only_) {
        Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
        coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
        for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
            coefficients_(coefficients_.rows()-1,i) = 0;
            coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
        }
        a.resize(a.size()+1);
        a(a.size()-1) = 0;
        coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
    }
    mean_.head<3>() = meanp;
    switch (flag)
    {
    case increase:
        if (eigenvectors_.rows() >= eigenvectors_.cols())
            break;
    case preserve:
        if (!basis_only_)
            coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
        eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
        eigenvalues_.resize(eigenvalues_.size()-1);
        break;
    default:
        PCL_ERROR("[pcl::PCA] unknown flag\n");
    }
}