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

void CDifodo::getPointsCoord(MatrixXf &x, MatrixXf &y, MatrixXf &z)
{
	x.resize(rows,cols);
	y.resize(rows,cols);
	z.resize(rows,cols);

	z = depth_inter[0];
	x = xx_inter[0];
	y = yy_inter[0];
}

void CDifodo::getDepthDerivatives(MatrixXf &cur_du, MatrixXf &cur_dv, MatrixXf &cur_dt)
{
	cur_du.resize(rows,cols);
	cur_dv.resize(rows,cols);
	cur_dt.resize(rows,cols);

	cur_du = du;
	cur_dv = dv;
	cur_dt = dt;
}

void computeRangeData( osg::Camera* camera , osg::ref_ptr<osg::Image> depthbufferimg, MatrixXf& Ximg, MatrixXf& Yimg, MatrixXf& Zimg) {
  // from https://svn.lcsr.jhu.edu/cisst/trunk/saw/components/sawOpenSceneGraph/code/osaOSGCamera.cpp
  // This is used by glutUnProject

  const osg::Viewport* viewport = camera->getViewport();
  size_t width = viewport->width();
  size_t height = viewport->height();

  GLint view[4];
  view[0] = (int)viewport->x();
  view[1] = (int)viewport->y();
  view[2] = width;
  view[3] = height;
  
  // Create a 3xN range data destination matrix.
  // [ x1 ... xN ]
  // [ y1 ... yN ]
  // [ z1 ... zN ]
  // VCT_COL_MAJOR is used because we will write in the following order
  // x1, y1, z1, x2, y2, z2, ..., xN, yN, zZ

  Ximg.resize(height,width);
  Yimg.resize(height,width);
  Zimg.resize(height,width);


  // get the intrinsic parameters of the camera
  double fovy, aspectRatio, Zn, Zf;
  camera->getProjectionMatrixAsPerspective( fovy, aspectRatio, Zn, Zf );
  
  osg::Matrixd modelm = camera->getViewMatrix();
  osg::Matrixd projm = camera->getProjectionMatrix();

  //for( int y=0; y<height; y++ ){
  for( int y=height-1; 0<=y; y-- ){
    for( size_t x=0; x<width; x++ ){
      GLdouble X, Y, Z;
      float* d = (float*)depthbufferimg->data( x, y );
      gluUnProject( x, y, *d, modelm.ptr(), projm.ptr(), view, &X, &Y, &Z );
      //gluUnProject( x, y, *d, &model[0][0], &proj[0][0], view, &X, &Y, &Z );
      // rangedata is 4xN column major
      Ximg(y,x) = X;
      Yimg(y,x) = Y;
      Zimg(y,x) = Z;
    }
  }

}

void detectSiftMatchWithOpenCV(const char* img1_path, const char* img2_path, MatrixXf &match) {
  Mat img1 = imread(img1_path);   
  Mat img2 = imread(img2_path);   

  SiftFeatureDetector detector;
  SiftDescriptorExtractor extractor;
  vector<KeyPoint> key1;
  vector<KeyPoint> key2;
  Mat desc1, desc2;
  detector.detect(img1, key1);
  detector.detect(img2, key2);
  extractor.compute(img1, key1, desc1);
  extractor.compute(img2, key2, desc2);

  FlannBasedMatcher matcher;
  vector<DMatch> matches;
  matcher.match(desc1, desc2, matches);

  match.resize(matches.size(), 6);
  cout << "match count: " << matches.size() << endl;
  for (int i = 0; i < matches.size(); i++) {
    match(i, 0) = key1[matches[i].queryIdx].pt.x;
    match(i, 1) = key1[matches[i].queryIdx].pt.y;
    match(i, 2) = 1;
    match(i, 3) = key2[matches[i].trainIdx].pt.x;
    match(i, 4) = key2[matches[i].trainIdx].pt.y;
    match(i, 5) = 1;
  }
  
}

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

void toHomogeneous(MatrixXf &mat) {
  MatrixXf temp;
  if (mat.cols() == 2) {
    temp.resize(mat.rows(), 3);
    temp.leftCols<2>() = mat.leftCols<2>();
    temp.col(2).setConstant(1);
    mat = temp;
  } else if (mat.cols() == 4) {
    temp.resize(mat.rows(), 6);
    temp.leftCols<2>() = mat.leftCols<2>();
    temp.col(2).setConstant(1);
    temp.block(0, 3, mat.rows(), 2) = temp.block(0, 2, mat.rows(), 2);
    temp.col(5).setConstant(1);
    mat = temp;
  } else 
    cout << "toHomogeneous with wrong dimension" << endl;
}

static inline MatrixXf toMatrix(const std::vector<Vector3f> &data) {
    MatrixXf result;
    result.resize(3, data.size());
    for (size_t i = 0; i < data.size(); ++i) {
        result.col(i) = data[i];
    }
    return std::move(result);
}

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

bool Surface::read(const QString &p_sFileName, Surface &p_Surface)
{
    p_Surface.clear();

    printf("Reading surface...\n");
    QFile t_File(p_sFileName);

    if (!t_File.open(QIODevice::ReadOnly))
    {
        printf("\tError: Couldn't open the file\n");
        return false;
    }

    QDataStream t_DataStream(&t_File);
    t_DataStream.setByteOrder(QDataStream::BigEndian);

    //
    //   Magic numbers to identify QUAD and TRIANGLE files
    //
    //   QUAD_FILE_MAGIC_NUMBER =  (-1 & 0x00ffffff) ;
    //   NEW_QUAD_FILE_MAGIC_NUMBER =  (-3 & 0x00ffffff) ;
    //
    qint32 NEW_QUAD_FILE_MAGIC_NUMBER =  16777213;
    qint32 TRIANGLE_FILE_MAGIC_NUMBER =  16777214;
    qint32 QUAD_FILE_MAGIC_NUMBER     =  16777215;

    qint32 magic = IOUtils::fread3(t_DataStream);

    qint32 nvert, nface;
    MatrixXf verts;
    MatrixXi faces;

    if(magic == QUAD_FILE_MAGIC_NUMBER || magic == NEW_QUAD_FILE_MAGIC_NUMBER)
    {
        qint32 nvert = IOUtils::fread3(t_DataStream);
        qint32 nquad = IOUtils::fread3(t_DataStream);
        if(magic == QUAD_FILE_MAGIC_NUMBER)
            printf("\t%s is a quad file (nvert = %d nquad = %d)\n", p_sFileName.toLatin1().constData(),nvert,nquad);
        else
            printf("\t%s is a new quad file (nvert = %d nquad = %d)\n", p_sFileName.toLatin1().constData(),nvert,nquad);

        //vertices
        verts.resize(nvert, 3);
        if(magic == QUAD_FILE_MAGIC_NUMBER)
        {
            qint16 iVal;
            for(qint32 i = 0; i < nvert; ++i)
            {
                for(qint32 j = 0; j < 3; ++j)
                {
                    t_DataStream >> iVal;
                    IOUtils::swap_short(iVal);
                    verts(i,j) = ((float)iVal) / 100;
                }
            }
        }

void noHomogeneous(MatrixXf &mat) {
  MatrixXf temp;
  if (mat.cols() == 3) {
    temp.resize(mat.rows(), 2);
    temp.col(0).array() = mat.col(0).array()/mat.col(2).array();
    temp.col(1).array() = mat.col(1).array()/mat.col(2).array();
    mat = temp;
  } else 
    cout << "toHomogeneous with wrong dimension" << endl;
}

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;
}

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

	MatrixXf SphereHelpers::toMatrix_3xN(std::vector<Vector3f> points)
	{
		MatrixXf m;
		m.resize(3, points.size());

		for (size_t i = 0; i<points.size(); i++)
		{
			m.block<3, 1>(0, i) = points.at(i);
		}

		return m;
	}

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

	virtual VectorXf gradient( const MatrixXf & a, const MatrixXf & b ) const {
		if (ktype_ == CONST_KERNEL)
			return VectorXf();
		MatrixXf fg = featureGradient( a, b );
		if (ktype_ == DIAG_KERNEL)
			return (f_.array()*fg.array()).rowwise().sum();
		else {
			MatrixXf p = fg*f_.transpose();
			p.resize( p.cols()*p.rows(), 1 );
			return p;
		}
	}