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

bool pre_draw(igl::opengl::glfw::Viewer & viewer)
{
  using namespace Eigen;
  using namespace std;
  if(viewer.core.is_animating)
  {
    // Interpolate pose and identity
    RotationList anim_pose(pose.size());
    for(int e = 0;e<pose.size();e++)
    {
      anim_pose[e] = pose[e].slerp(anim_t,Quaterniond::Identity());
    }
    // Propagate relative rotations via FK to retrieve absolute transformations
    RotationList vQ;
    vector<Vector3d> vT;
    igl::forward_kinematics(C,BE,P,anim_pose,vQ,vT);
    const int dim = C.cols();
    MatrixXd T(BE.rows()*(dim+1),dim);
    for(int e = 0;e<BE.rows();e++)
    {
      Affine3d a = Affine3d::Identity();
      a.translate(vT[e]);
      a.rotate(vQ[e]);
      T.block(e*(dim+1),0,dim+1,dim) =
        a.matrix().transpose().block(0,0,dim+1,dim);
    }
    // Compute deformation via LBS as matrix multiplication
    U = M*T;

    // Also deform skeleton edges
    MatrixXd CT;
    MatrixXi BET;
    igl::deform_skeleton(C,BE,T,CT,BET);

    viewer.data().set_vertices(U);
    viewer.data().set_edges(CT,BET,sea_green);
    viewer.data().compute_normals();
    anim_t += anim_t_dir;
    anim_t_dir *= (anim_t>=1.0 || anim_t<=0.0?-1.0:1.0);
  }
  return false;
}

void CCorrelationFilters::fusion_matrix_inverse(Eigen::ArrayXXcd &X, Eigen::MatrixXi indices)
{
	/* We compute the inverse of the fusion matrix by using the schur complement
	 * on our data structure. We use an inplace implementation therefore it is quite
	 * memory efficient as well.
	 */
	
    int num_blocks=0;
    int total_blocks=0;
    int size_blocks = X.rows();
    
    if (indices.rows() != indices.cols()){
        std::cout << "Something Wrong" << std::endl;
        return;
    }
    else
    {
        num_blocks = indices.rows();
    }

    if (num_blocks == 1){
        X = X.inverse();
        return;
    }
	
	if (num_blocks == 2)
	{
        Eigen::ArrayXXcd temp1;
        Eigen::ArrayXXcd temp2;
        
		Eigen::ArrayXXcd DC(size_blocks,1);
		Eigen::ArrayXXcd BD(size_blocks,1);
		Eigen::ArrayXXcd BDC(size_blocks,1);
		Eigen::ArrayXXcd ABDC(size_blocks,1);
		
		DC = X.block(0,indices(1,0),size_blocks,1)/X.block(0,indices(1,1),size_blocks,1);
		BD = X.block(0,indices(0,1),size_blocks,1)/X.block(0,indices(1,1),size_blocks,1);
		BDC = X.block(0,indices(0,1),size_blocks,1)*DC;
		ABDC = (X.block(0,indices(0,0),size_blocks,1)-BDC).inverse();
		
		X.block(0,indices(0,0),size_blocks,1) = ABDC;
		X.block(0,indices(0,1),size_blocks,1) = -ABDC*BD;
		X.block(0,indices(1,0),size_blocks,1) = -DC*ABDC;
		X.block(0,indices(1,1),size_blocks,1) = 1/X.block(0,indices(1,1),size_blocks,1) - X.block(0,indices(1,0),size_blocks,1)*BD;
	}
	else
	{
        int num_B = num_blocks-1;
        int num_C = num_blocks-1;
        
        Eigen::MatrixXi ind_A;
        Eigen::MatrixXi ind_B;
        Eigen::MatrixXi ind_C;
        Eigen::MatrixXi ind_D;
        
        total_blocks = num_blocks*num_blocks;
        ind_D = indices.block(num_blocks-1,num_blocks-1,1,1);
        ind_B = indices.block(0,num_blocks-1,num_blocks-1,1);
        ind_C = indices.block(num_blocks-1,0,1,num_blocks-1);
        ind_A = indices.block(0,0,num_blocks-1,num_blocks-1).transpose();
        num_blocks = num_blocks-1;
		
		Eigen::ArrayXXcd D = Eigen::ArrayXXcd::Zero(size_blocks,1);
		Eigen::ArrayXXcd DC = Eigen::ArrayXXcd::Zero(size_blocks,num_blocks);
		Eigen::ArrayXXcd BD = Eigen::ArrayXXcd::Zero(size_blocks,num_blocks);
		Eigen::ArrayXXcd BDC = Eigen::ArrayXXcd::Zero(size_blocks,num_blocks*num_blocks);
		Eigen::ArrayXXcd ABDC = Eigen::ArrayXXcd::Zero(size_blocks,num_blocks*num_blocks);
		
		Eigen::ArrayXXcd tmp_B = Eigen::ArrayXXcd::Zero(size_blocks,num_B);
		Eigen::ArrayXXcd tmp_C = Eigen::ArrayXXcd::Zero(size_blocks,num_C);
		Eigen::ArrayXXcd tmp_D = Eigen::ArrayXXcd::Zero(size_blocks,1);
		
		for(int i=0;i<num_C;i++){
			tmp_C.block(0,i,size_blocks,1) = X.block(0,ind_C(0,i),size_blocks,1);
		}
		for(int i=0;i<num_B;i++){
			tmp_B.block(0,i,size_blocks,1) = X.block(0,ind_B(i,0),size_blocks,1);
		}
		
		D.block(0,0,size_blocks,1) = X.block(0,total_blocks-1,size_blocks,1).inverse();
		
		Eigen::Vector2i num_blocks_1, num_blocks_2;
		num_blocks_1 << 1,1;
		num_blocks_2 << 1,num_blocks;
		fusion_matrix_multiply(DC,D,tmp_C,num_blocks_1,num_blocks_2);
		
		num_blocks_1 << num_blocks,1;
		num_blocks_2 << 1,1;
		fusion_matrix_multiply(BD,tmp_B,D,num_blocks_1,num_blocks_2);
		
		num_blocks_1 << num_blocks,1;
		num_blocks_2 << 1,num_blocks;
		fusion_matrix_multiply(BDC,tmp_B,DC,num_blocks_1,num_blocks_2);
        
        for(int i=0;i<ind_A.rows();i++){
            for(int j=0;j<ind_A.cols();j++){
                X.block(0,ind_A(i,j),size_blocks,1) -= BDC.block(0,i,size_blocks,1);
            }
		}
		
//.........这里部分代码省略.........