C++ Matrix::leftCols方法代码示例

Eigen::Matrix<double, 3, Eigen::Dynamic> PointMass::getBodyJacobian()
{
  assert(mParentSoftBodyNode != NULL);

  int dof = mParentSoftBodyNode->getNumDependentGenCoords();
  int totalDof = mParentSoftBodyNode->getNumDependentGenCoords() + 3;

  Eigen::Matrix<double, 3, Eigen::Dynamic> J
      = Eigen::MatrixXd::Zero(3, totalDof);

  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
  T.translation() = mX;

  J.leftCols(dof)
      = math::AdInvTJac(
          T, mParentSoftBodyNode->getBodyJacobian()).bottomRows<3>();
  J.rightCols<3>() = Eigen::Matrix3d::Identity();

  return J;
}

int main()
{
	float width = 640.0f, height=480.0f;
	char buf[255];
	int n, pose; 
	AsfMatrix data;

	Eigen::Matrix<float,40*6,-1> Shapes; //240x116

	float mean_size = 0;
	for(n=0;n<40;n++)
	{
		for(pose=0;pose<6;pose++)
		{
			sprintf(buf,"../../../CIL/data/imm_face_db/%.2d-%dm.asf",n+1,pose+1);
			if(readAsf2Eigen(buf, data) != 0)
			{
				sprintf(buf,"../../../CIL/data/imm_face_db/%.2d-%df.asf",n+1,pose+1);
				if (readAsf2Eigen(buf, data) != 0)
					continue;		
			}

			//Initialize The Shapes Container
			if(Shapes.cols() == 0)
				Shapes.resize(40*6,data.cols()*2);
			
			

			//Copy the found data
			Shapes.block(n*6+pose,0,1,data.cols()) = data.row(2) * width;
			Shapes.block(n*6+pose,data.cols(),1,data.cols()) = data.row(3) * height;

			

			//Compute MeanShape
			auto mean_x		= Shapes.block(n*6+pose,0,1,data.cols()).mean();
			auto mean_y		= Shapes.block(n*6+pose,data.cols(),1,data.cols()).mean();
			auto mshape_x	= (Shapes.block(n*6+pose,0,1,data.cols()).array()-mean_x).pow(2) ;
			auto mshape_y	= (Shapes.block(n*6+pose,data.cols(),1,data.cols()).array()-mean_y).pow(2) ;
			mean_size		+= sqrt((mshape_x+mshape_y).sum());

		
			//std::cout << Shapes.block(n*pose+pose,0,1,data.cols()) << std::endl;
		//		std::cout << Shapes.block(0,0,1,5) << std::endl ;
	//std::cout << Shapes.block(1,0,1,5) << std::endl ;
	//std::cout << Shapes.block(2,0,1,5) << std::endl ;
	//std::cout << Shapes.block(3,0,1,5) << std::endl << std::endl;
		}

	}
	mean_size /= 40*6;
	int number_of_landmarks = data.cols();
	int number_of_shapes	= Shapes.rows();

	//Complex notation and Substracting Mean.
	Eigen::MatrixXcf X(number_of_shapes, number_of_landmarks);
	X.real() = Shapes.leftCols(number_of_landmarks);
	X.imag() = Shapes.rightCols(number_of_landmarks);
	X.array().colwise() -= X.rowwise().mean().array();

	//Eigen::MatrixXcd XX(10,10);

	//double test[10] = {0};
	//Eigen::Map<Eigen::MatrixXd> mat(test, 10, 1);
	Eigen::MatrixXcf C;
	Eigen::MatrixXcf Mean;
	cil::alg::gpa(X,C,Mean);
	std::cout << X.rows() << " , " << X.cols() << std::endl<< std::endl;
	std::cout << C.rows() << " , " << C.cols() << std::endl<< std::endl;
	std::cout << Mean.rows() << " , " << Mean.cols() << std::endl<< std::endl;
	std::cout << C.row(1).transpose() << std::endl<< std::endl;
	return 0;

	X.array().colwise() -= X.rowwise().mean().array();

	//Eigen::MatrixXcf X = Shapes.block(0,0,Shapes.rows(),data.cols()) * std::complex<float>(0,1) +
	//	Shapes.block(0,data.cols(),Shapes.rows(),data.cols())*std::complex<float>(1,0);
	//Eigen::VectorXcf Mean = X.rowwise().mean();

	//std::complex<float> *m_ptr = Mean.data();
	//for(n=0;n<Mean.rows();++n)
	//	X.row(n) = X.row(n).array() - *m_ptr++;

	//Solve Eigen Problem
	Eigen::MatrixXcf A = X.transpose().conjugate() * X;
	Eigen::ComplexEigenSolver<Eigen::MatrixXcf> solver;
	solver.compute(A);
	
//	std::cout << "The Eigenvales of A are:" << std::endl << solver.eigenvalues() <<std::endl<<std::endl;
//	std::complex<float> lambda = solver.eigenvalues()[57];
//	std::cout << "Consider the first eigenvalue, lambda = " << lambda << std::endl;
//	std::cout << "EigenVec for largest EigenVals of A are:" << std::endl << solver.eigenvectors().col(57) <<std::endl<<std::endl;

	auto eigvec_mean = solver.eigenvectors().col(solver.eigenvectors().cols()-1);

	// Full Procrusters fits
	Eigen::MatrixXcf f = (X * eigvec_mean).array() / (X * X.transpose().conjugate()).diagonal().array();

	//Transform
	
//.........这里部分代码省略.........

/**
 * Initialize all structures.
 *
 * @param tld learning structures
 */
void tldInit(TldStruct& tld) {

	// initialize lucas kanade
	lkInit();

	// get initial bounding box
	Eigen::Vector4d bb;
	bb = tld.cfg->init;

	Eigen::Vector2i imsize;
	imsize(0) = tld.imgsize.m;
	imsize(1) = tld.imgsize.n;
	bb_scan(tld, bb, imsize, tld.model->min_win);

	// Features
	tldGenerateFeatures(tld, tld.model->num_trees, tld.model->num_features);

	// Initialize Detector
	fern0();

	ImgType im0;

	img_init(*(tld.cfg));

	im0.input = img_get();

	im0.blur = cvCloneImage(im0.input);
	im0.blur = img_blur(im0.blur);

	// allocate structures
	fern1(im0.input, tld.grid, tld.features, tld.scales);

	// Temporal structures
	Tmp temporal;
	temporal.conf = Eigen::VectorXd::Zero(tld.nGrid);
	temporal.patt = Eigen::Matrix<double, 10, Eigen::Dynamic>::Zero(tld.model->num_trees, tld.nGrid);
	tld.tmp = temporal;

	// RESULTS =================================================================

	// Initialize Trajectory

	tld.prevBB = Eigen::Vector4d::Constant(
			std::numeric_limits<double>::quiet_NaN());
	tld.currentImg = im0;
	tld.currentBB = bb;
	tld.conf = 1;
	tld.currentValid = 1;
	tld.size = 1;

	// TRAIN DETECTOR ==========================================================

	// Initialize structures
	tld.imgsize.m = DIMY;
	tld.imgsize.n = DIMX;

	Eigen::RowVectorXd overlap = bb_overlap(tld.currentBB, tld.nGrid, tld.grid.topRows(4));

	tld.target = img_patch(tld.currentImg.input, tld.currentBB);

	// Generate Positive Examples
	Eigen::Matrix<double, NTREES, Eigen::Dynamic> pX; // pX: 10 rows
	Eigen::Matrix<double, (PATCHSIZE * PATCHSIZE), Eigen::Dynamic> pEx;
	tld.currentBB = tldGeneratePositiveData(tld, overlap, tld.currentImg,
			tld.p_par_init, pX, pEx);

	Eigen::MatrixXd pY = Eigen::MatrixXd::Ones(1, pX.cols());
	// Generate Negative Examples
	Eigen::Matrix<double, NTREES, Eigen::Dynamic> nX; // nX: 10 rows
	Eigen::Matrix<double, (PATCHSIZE * PATCHSIZE), Eigen::Dynamic> nEx;
	tldGenerateNegativeData(tld, overlap, tld.currentImg, nX, nEx);

	// Split Negative Data to Training set and Validation set
	Eigen::Matrix<double, NTREES, Eigen::Dynamic> spnX;
	Eigen::Matrix<double, (PATCHSIZE * PATCHSIZE), Eigen::Dynamic> spnEx;
	tldSplitNegativeData(nX, nEx, spnX, spnEx);

	Eigen::MatrixXd nY1 = Eigen::MatrixXd::Zero(1, spnX.cols() / 2);

	Eigen::MatrixXd xCombined(pX.rows(), pX.cols() + spnX.cols() / 2);
	xCombined << pX, spnX.leftCols(spnX.cols() / 2);
	Eigen::MatrixXd yCombined(pY.rows(), pY.cols() + nY1.cols());
	yCombined << pY, nY1;
	Eigen::RowVectorXd idx(xCombined.cols());
	for (int i = 0; i < xCombined.cols(); i++)
		idx(i) = i;

	idx = permutate_cols(idx);

	Eigen::MatrixXd permX(xCombined.rows(), xCombined.cols());
	Eigen::VectorXd permY(yCombined.cols());
	for (int i = 0; i < idx.cols(); i++) {
		permX.col(i) = xCombined.col(idx(i));
		permY(i) = yCombined(0, idx(i));
	}
//.........这里部分代码省略.........