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

bool
af::loadMatrix (const std::string filename, Eigen::MatrixXf &matrix)
{
  std::ifstream file (filename.c_str (), std::ios::binary | std::ios::in);
  if (!file.is_open ())
  {
    PCL_ERROR ("Error reading matrix from file %s.\n", filename.c_str ());
    return (false);
  }

  size_t num_rows, num_cols;
  file.read (reinterpret_cast<char*> (&num_rows), sizeof (num_rows));
  file.read (reinterpret_cast<char*> (&num_cols), sizeof (num_cols));

  printf ("Reading matrix of size %zu %zu from %s\n", num_rows, num_cols, filename.c_str ());

  matrix.resize (num_rows, num_cols);
  for (size_t i = 0; i < num_rows; ++i)
    for (size_t j = 0; j < num_cols; ++j)
    {
      float val;
      file.read (reinterpret_cast<char*> (&val), sizeof (val));
      matrix (i, j) = val;
    }
  file.close ();

  return (true);
}

int Conversion::convert(const Eigen::MatrixXf & point3D,
                        Eigen::MatrixXf & depthMat,
                                 const int &height,
                                 const int &width,
                                 double &fx,
                                 double &fy,
                                 double &cx,
                                 double &cy)
{
      if(point3D.rows()==0)
        return 0;
        
      depthMat.resize(height,width);
      for (int i=0; i<depthMat.rows();i++)
        for (int j=0 ; j<depthMat.cols();j++)
          depthMat(i,j)=-1;
      
      
      for (int index=0; index<point3D.rows();index++)
        {
          float val = point3D(index,2);//*1000;
          //if(val>0){
          int   i   = round(fx*point3D(index,0)/point3D(index,2)+cx);
          int   j   = round(fy*point3D(index,1)/point3D(index,2)+cy);
          depthMat(i,j)=val;
          //}
          index++;
        }
        return 1;
}  

void detectSiftMatchWithVLFeat(const char* img1_path, const char* img2_path, Eigen::MatrixXf &match) {

    int *m = 0;
    double *kp1 = 0, *kp2 = 0;
    vl_uint8 *desc1 = 0, *desc2 = 0;

    int nkp1 = detectSiftAndCalculateDescriptor(img1_path, kp1, desc1);
    int nkp2 = detectSiftAndCalculateDescriptor(img2_path, kp2, desc2);
    cout << "num kp1: " << nkp1 << endl;
    cout << "num kp2: " << nkp2 << endl;
    int nmatch = matchDescriptorWithRatioTest(desc1, desc2, nkp1, nkp2, m);
    cout << "num match: " << nmatch << endl;
    match.resize(nmatch, 6);
    for (int i = 0; i < nmatch; i++) {
        int index1 = m[i*2+0];
        int index2 = m[i*2+1];
        match.row(i) << kp1[index1*4+1], kp1[index1*4+0], 1, kp2[index2*4+1], kp2[index2*4+0], 1;
    }

    free(kp1);
    free(kp2);
    free(desc1);
    free(desc2);
    free(m);
}

	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 PixelMapper::mergeProjections(Eigen::MatrixXf& depthImage, Eigen::MatrixXi& indexImage,
				   Eigen::MatrixXf* depths, Eigen::MatrixXi* indices, int numImages){
  assert (numImages>0);
  int rows=depths[0].rows();
  int cols=depths[0].cols();
  depthImage.resize(indexImage.rows(), indexImage.cols());
  depthImage.fill(std::numeric_limits<float>::max());
  indexImage.resize(rows, cols);
  indexImage.fill(-1);

  #pragma omp parallel for
  for (int c=0; c<cols; c++){
    int* destIndexPtr = &indexImage.coeffRef(0,c);
    float* destDepthPtr = &depthImage.coeffRef(0,c);
    int* srcIndexPtr[numImages];
    float* srcDepthPtr[numImages];
    for (int i=0; i<numImages; i++){
      srcIndexPtr[i] = &indices[i].coeffRef(0,c);
      srcDepthPtr[i] = &depths[i].coeffRef(0,c);
    }
    for (int r=0; r<rows; r++){
      for (int i=0; i<numImages; i++){
	if (*destDepthPtr>*srcDepthPtr[i]){
	  *destDepthPtr = *srcDepthPtr[i];
	  *destIndexPtr = *srcIndexPtr[i];
	}
	srcDepthPtr[i]++;
	srcIndexPtr[i]++;
      }
      destDepthPtr++;
      destIndexPtr++;
    }
  }

}

template <typename PointInT, typename PointOutT> void
pcl::BilateralUpsampling<PointInT, PointOutT>::computeDistances (Eigen::MatrixXf &val_exp_depth, Eigen::VectorXf &val_exp_rgb)
{
  val_exp_depth.resize (2*window_size_+1,2*window_size_+1);
  val_exp_rgb.resize (3*255);

  int j = 0;
  for (int dx = -window_size_; dx < window_size_+1; ++dx) 
  {
    int i = 0;
    for (int dy = -window_size_; dy < window_size_+1; ++dy)
    {
      float val_exp = expf (- (dx*dx + dy*dy) / (2.0f * static_cast<float> (sigma_depth_ * sigma_depth_)));
      val_exp_depth(i,j) = val_exp;
      i++;
    }
    j++;
  }
    
  for (int d_color = 0; d_color < 3*255; d_color++) 
  {    
    float val_exp = expf (- d_color * d_color / (2.0f * sigma_color_ * sigma_color_));
    val_exp_rgb(d_color) = val_exp;
  }
}

/**
 * Create the augmented knots vector and fill in matrix Xt with the spline basis vectors
 */
void basis_splines_endreps_local_v2(float *knots, int n_knots, int order, int *boundaries, int n_boundaries,  Eigen::MatrixXf &Xt) {
  assert(n_boundaries == 2 && boundaries[0] < boundaries[1]);
  int n_rows = boundaries[1] - boundaries[0]; // this is frames so evaluate at each frame we'll need
  int n_cols = n_knots + order;  // number of basis vectors we'll have at end 
  Xt.resize(n_cols, n_rows); // swapped to transpose later. This order lets us use it as a scratch space
  Xt.fill(0);
  int n_std_knots = n_knots + 2 * order;
  float std_knots[n_std_knots];

  // Repeat the boundary knots on there to ensure linear out side of knots
  for (int i = 0; i < order; i++) {
    std_knots[i] = boundaries[0];
  }
  // Our original boundary knots here
  for (int i = 0; i < n_knots; i++) {
    std_knots[i+order] = knots[i];
  }
  // Repeat the boundary knots on there to ensure linear out side of knots
  for (int i = 0; i < order; i++) {
    std_knots[i+order+n_knots] = boundaries[1];
  }
  // Evaluate our basis splines at each frame.
  for (int i = boundaries[0]; i < boundaries[1]; i++) {
    int idx = -1;
    // find index such that i >= knots[idx] && i < knots[idx+1]
    float *val = std::upper_bound(std_knots, std_knots + n_std_knots - 1, 1.0f * i);
    idx = val - std_knots - 1;
    assert(idx >= 0);
    float *f = Xt.data() + i * n_cols + idx - (order - 1); //column offset
    basis_spline_xi_v2(std_knots, n_std_knots, order, idx, i, f);
  }
  // Put in our conventional format where each column is a basis vector
  Xt.transposeInPlace();
}

Eigen::MatrixXf
readDescrFromFile(const std::string &file, int padding, int rowSize)
{

    // check if file exists
    boost::filesystem::path path = file;
    if ( ! (boost::filesystem::exists ( path ) && boost::filesystem::is_regular_file(path)) )
        throw std::runtime_error ("Given file path to read Matrix does not exist!");

    std::ifstream in (file.c_str (), std::ifstream::in);

    int bufferSize = 819200;
    //int bufferSize = rowSize * 10;

    char linebuf[bufferSize];



    Eigen::MatrixXf matrix;
    matrix.resize(0,rowSize);
    int j=0;
    while(in.getline (linebuf, bufferSize)){
        int start_s=clock();
        std::string line (linebuf);
        std::vector < std::string > strs_2;
        boost::split (strs_2, line, boost::is_any_of (" "));
        matrix.conservativeResize(matrix.rows()+1,rowSize);
        for (int i = 0; i < strs_2.size()-1; i++)
            matrix (j, i) = static_cast<float> (atof (strs_2[i].c_str ()));
        j++;
        int stop_s=clock();
        std::cout << "time: " << (stop_s-start_s)/double(CLOCKS_PER_SEC)*1000 << std::endl;
    }
    return matrix;
}

//observations is a matrix of nx1 where n is the number of landmarks observed
//each value in the matrix represents the angle at which the landmark was observed
//params is a matrix of nx2 where n is the number of landmarks
//for each landmark, the x and y pose of where it is
//pose is a matrix of 2x1 containing the initial guess of the robot pose
void LMAlgr::computeLM(Eigen::MatrixXf observations, Eigen::MatrixXf params, Eigen::MatrixXf &pose){
	
	double lambda = 0.0001;
	//compute the squared error
	Eigen::MatrixXf error;
	error.resize(observations.rows(), 1);
	double squared_error = computeError(observations, params, pose, error);
	double old_error = std::numeric_limits<double>::max();
	while(old_error - squared_error > threshold){
		//std::cout << "squared error: " << squared_error << std::endl;
		Eigen::MatrixXf delta(2, 1);
		computeIncrement(params, pose, lambda, error, delta);
		/*std::cout << "delta: \n" << delta << std::endl;*/
		Eigen::MatrixXf new_pose = pose + delta;
		
		//std::cout << new_pose.transpose() << std::endl;
		Eigen::MatrixXf new_error;
		new_error.resize(observations.rows(), 1);
		double new_sq_error = computeError(observations, params, new_pose, new_error);
		if(new_sq_error < squared_error){
			/*std::cout << "case 1: squared error: " << new_sq_error << ", new pose:\n " << new_pose << std::endl;*/
			pose = new_pose;
			error = new_error;
			old_error = squared_error;
			squared_error = new_sq_error;
			lambda = lambda / 10;
		}
		else{
			/*std::cout << "case 2: squared error: " << squared_error << std::endl;*/
			lambda = lambda * 10;
		}
	}
}

void StructureSimilarityDialog::setMolecules(const std::vector<MongoChem::MoleculeRef> &molecules)
{
  MongoChem::MongoDatabase *db = MongoChem::MongoDatabase::instance();

  m_molecules = molecules;

  // calculate similarity matrix
  Eigen::MatrixXf similarityMatrix;
  similarityMatrix.resize(m_molecules.size(), m_molecules.size());

  for(size_t i = 0; i < m_molecules.size(); i++){
    const MongoChem::MoleculeRef &refI = m_molecules[i];
    boost::shared_ptr<Molecule> molecule = db->createMolecule(refI);
    StructureSimilarityDescriptor descriptor;
    descriptor.setMolecule(molecule);

    for(size_t j = i + 1; j < m_molecules.size(); j++){
      const MongoChem::MoleculeRef &refJ = m_molecules[j];
      boost::shared_ptr<Molecule> moleculeJ = db->createMolecule(refJ);
      float similarity = descriptor.value(moleculeJ.get()).toFloat();

      similarityMatrix(i, j) = similarity;
      similarityMatrix(j, i) = similarity;
    }
  }

  // recalculate graph
  m_graphWidget->setSimilarityMatrix(similarityMatrix);
}

void sumAndNormalize( Eigen::MatrixXf & out, const Eigen::MatrixXf & in, const Eigen::MatrixXf & Q ) {
	out.resize( in.rows(), in.cols() );
	for( int i=0; i<in.cols(); i++ ){
		Eigen::VectorXf b = in.col(i);
		Eigen::VectorXf q = Q.col(i);
		out.col(i) = b.array().sum()*q - b;
	}
}

///////////////////////
/////  Inference  /////
///////////////////////
void expAndNormalize ( Eigen::MatrixXf & out, const Eigen::MatrixXf & in ) {
	out.resize( in.rows(), in.cols() );
	for( int i=0; i<out.cols(); i++ ){
		Eigen::VectorXf b = in.col(i);
		b.array() -= b.maxCoeff();
		b = b.array().exp();
		out.col(i) = b / b.array().sum();
	}
}

void load( Archive & ar,
           Eigen::MatrixXf & t,
           const unsigned int file_version )
{
    int n0;
    ar >> BOOST_SERIALIZATION_NVP( n0 );
    int n1;
    ar >> BOOST_SERIALIZATION_NVP( n1 );
    t.resize( n0, n1 );
    ar >> make_array( t.data(), t.rows()*t.cols() );
}

	virtual Eigen::VectorXf parameters() const {
		if (ktype_ == CONST_KERNEL)
			return Eigen::VectorXf();
		else if (ktype_ == DIAG_KERNEL)
			return parameters_;
		else {
			Eigen::MatrixXf p = parameters_;
			p.resize( p.cols()*p.rows(), 1 );
			return p;
		}
	}

	virtual void setParameters( const Eigen::VectorXf & p ) {
		if (ktype_ == DIAG_KERNEL) {
			parameters_ = p;
			initLattice( p.asDiagonal() * f_ );
		}
		else if (ktype_ == FULL_KERNEL) {
			Eigen::MatrixXf tmp = p;
			tmp.resize( parameters_.rows(), parameters_.cols() );
			parameters_ = tmp;
			initLattice( tmp * f_ );
		}
	}