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

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

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

///////////////////////
/////  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();
	}
}

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

//update values. Order matters
void SoftConstraint::updateDefGrad() {
    this->D_s.col(0) << p0->position - p3->position;
    this->D_s.col(1) << p1->position - p3->position;
    this->D_s.col(2) << p2->position - p3->position;

    this->defGrad = D_s * D_m.inverse();
    
    float det = defGrad.determinant();
    //check if cell is degenerated, i.e. det(defgrad) <= 0)
    if(det<= 0) {
        
        this->degenrated = true;
        //compute singular value decomposition of defGrad
        JacobiSVD<_Matrix3> svd(this->defGrad, ComputeFullU | ComputeFullV);
        MatrixXf F_sing = svd.singularValues();
        MatrixXf U  = svd.matrixU();
        MatrixXf V  = svd.matrixV();
        
        this->U = U;
        this->V = V;
        
        //find smalles Element and negate corresponding colums
         int smallestEntry = 0;
         
         if(F_sing(1) < F_sing(0)) {
             smallestEntry = 1;
         }
         if(F_sing(2) < F_sing(smallestEntry)) {
             smallestEntry = 2;
         }
         
         F_sing(smallestEntry) = - F_sing(smallestEntry);
        U.col(smallestEntry) = -U.col(smallestEntry);
         //compose new defGrad
  
        _Matrix3 F;
         F << F_sing(0), 0.0f, 0.0f,
              0.0f, F_sing(1), 0.0f,                
              0.0f, 0.0f, F_sing(2);
         
    
        this->defGrad = F;
        //cout << "det <0"; 
    }
    
    else {
        this->degenrated = false;
    }
    
 
};

MatrixXf Arm::compute_Jacobian() {
	/*X, Y, Z are three different orthogonal axises on which a ball joint can rotate. */
	MatrixXf Jacobian = MatrixXf(3, 3 * arm_sequence.size() ); 
	for (int i = 0; i < arm_sequence.size(); i++) {
		Vector3f dPdT_joint_i_X = dPdT(i, X);
		Vector3f dPdT_joint_i_Y = dPdT(i, Y);
		Vector3f dPdT_joint_i_Z = dPdT(i, Z);
		Jacobian.col(3 * i) << dPdT_joint_i_X;
		Jacobian.col(3 * i + 1) << dPdT_joint_i_Y;
		Jacobian.col(3 * i + 2) << dPdT_joint_i_Z;
	}
	//cout << "Jacobian \n" << Jacobian << endl;
	return Jacobian;
}

    void operator()() {
#ifdef FAST_AND_FAT
        //! Performing feature transformation on a block results
        //! is about a factor of two speedup. This suggests moving
        //! feature storage from nodes to images. However, the
        //! change will make introduction of different node
        //! types (with different sized feature vectors) difficult.
        //! It also means that the metric functors would need to
        //! be passed a copy of the graph in addition to the nodes.
        for (set<unsigned>::const_iterator it = _imgIndxes.begin(); it != _imgIndxes.end(); ++it) {
            lock();
            map<unsigned, MatrixXf>::const_iterator ft = _imgFeatureData.find(*it);
            if (ft == _imgFeatureData.end()) {
                MatrixXf X(_Lt.cols(), _srcGraph[*it].numNodes());
                for (unsigned segId = 0; segId < _srcGraph[*it].numNodes(); segId++) {
                    X.col(segId) = _srcGraph[*it][segId].features;
                }
                ft = _imgFeatureData.insert(_imgFeatureData.end(), make_pair(*it, X));
            }
            unlock();

            const MatrixXf X = _Lt.triangularView<Eigen::Upper>() * ft->second;
            for (unsigned segId = 0; segId < _srcGraph[*it].numNodes(); segId++) {
                _negGraph[*it][segId].features = _posGraph[*it][segId].features = X.col(segId);
            }
        }
#else
        const TriangularView<MatrixXf, Eigen::Upper> Lt(_Lt);
        for (set<unsigned>::const_iterator it = _imgIndxes.begin(); it != _imgIndxes.end(); ++it) {
            for (unsigned segId = 0; segId < _srcGraph[*it].numNodes(); segId++) {
                _negGraph[*it][segId].features = _posGraph[*it][segId].features = Lt * _srcGraph[*it][segId].features;
            }
        }
#endif
    }

/**
 * Normalizes each eigenface in a matrix.
 * 
 * @param eigenfaces  A matrix of eigen faces to normalize
 */
void normalizeEigenFaces(MatrixXf &eigenfaces)
{
    for(int i = 0; i < eigenfaces.cols(); i++)
    {
        eigenfaces.col(i).normalize();  
    }
}

void RealtimeMF_openni::projectDirections(cv::Mat& I, const MatrixXf& dirs,
    double f_d, const Matrix<uint8_t,Dynamic,Dynamic>& colors)
{
  double scale = 0.1;
  VectorXf p0(3); p0 << 0.35,0.25,1;
  double u0 = p0(0)/p0(2)*f_d + 320.;
  double v0 = p0(1)/p0(2)*f_d + 240.;
  for(uint32_t k=0; k < dirs.cols(); ++k)
  {
    VectorXf p1 = p0 + dirs.col(k)*scale;
    double u1 = p1(0)/p1(2)*f_d + 320.;
    double v1 = p1(1)/p1(2)*f_d + 240.;
    cv::line(I, cv::Point(u0,v0), cv::Point(u1,v1),
        CV_RGB(colors(k,0),colors(k,1),colors(k,2)), 2, CV_AA);

    double arrowLen = 10.;
    double angle = atan2(v1-v0,u1-u0);

    double ru1 = u1 - arrowLen*cos(angle + M_PI*0.25);
    double rv1 = v1 - arrowLen*sin(angle + M_PI*0.25);
    cv::line(I, cv::Point(u1,v1), cv::Point(ru1,rv1),
        CV_RGB(colors(k,0),colors(k,1),colors(k,2)), 2, CV_AA);
    ru1 = u1 - arrowLen*cos(angle - M_PI*0.25);
    rv1 = v1 - arrowLen*sin(angle - M_PI*0.25);
    cv::line(I, cv::Point(u1,v1), cv::Point(ru1,rv1),
        CV_RGB(colors(k,0),colors(k,1),colors(k,2)), 2, CV_AA);
  }
  cv::circle(I, cv::Point(u0,v0), 2, CV_RGB(0,0,0), 2, CV_AA);
}

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

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

MatrixXf RealtimeMF::mfAxes()
{
  MatrixXf mfAx = MatrixXf::Zero(3,6*cRmfs_.size());
  for(uint32_t k=0; k<6*cRmfs_.size(); ++k){
    int j = (k%6)/2; // which of the rotation columns does this belong to
    float sign = (- float((k%6)%2) +0.5f)*2.0f; // sign of the axis
    mfAx.col(k) = sign*cRmfs_[k/6].col(j);
  }
  return mfAx;
};

Vector3f Lu::estimate_t( MatrixXf R,MatrixXf G,std::vector<Matrix3f > F,MatrixXf P,int n ){


Vector3f sum_ =Vector3f::Zero();

for (int i=0;i< n; i++){
   sum_=sum_+F[i]*R*P.col(i);
} 
   return G*sum_;
}

VectorXs DenseCRF::currentMap( const MatrixXf & Q ) const{
	VectorXs r(Q.cols());
	// Find the map
	for( int i=0; i<N_; i++ ){
		int m;
		Q.col(i).maxCoeff( &m );
		r[i] = m;
	}
	return r;
}