Example #1
0
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;
}
Example #2
0
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
	
	auto f_conj = f.conjugate().array();
	for(n=0;n<X.cols();++n)
		X.col(n) = X.col(n).array() * f_conj;
	auto mf = f.mean();
	std::cout << mf << std::endl<< std::endl;
	mf = mf / sqrt(mf.real()*mf.real()+mf.imag()*mf.imag());
	std::cout << mf << std::endl<< std::endl;
	auto m = eigvec_mean * mf;
	X = X*mf;



	std::cout << X.row(0).transpose() << std::endl<< std::endl;

	return 0;
}
Example #3
0
/**
 * 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));
	}

	// Train using training set ------------------------------------------------

	// Fern
	unsigned char bootstrap = 2;
	Eigen::VectorXd dummy(1);
	dummy(0) = -1;
	fern2(permX, permY, tld.model->thr_fern, bootstrap, dummy);

	// Nearest Neighbour
	tld.npex = 0;
	tld.nnex = 0;

	tldTrainNN(pEx, spnEx.leftCols(spnEx.cols() / 2), tld);
	tld.model->num_init = tld.npex;

	// Estimate thresholds on validation set  ----------------------------------

	// Fern
	unsigned int ferninsize = spnX.cols() / 2;
	Eigen::RowVectorXd conf_fern(ferninsize);
	Eigen::Matrix<double, 10, Eigen::Dynamic> fernin(10, ferninsize);
	fernin.leftCols(ferninsize) = spnX.rightCols(ferninsize);
	conf_fern = fern3(fernin, ferninsize);
	tld.model->thr_fern = std::max(conf_fern.maxCoeff() / tld.model->num_trees,
			tld.model->thr_fern);

	// Nearest neighbor
	Eigen::MatrixXd conf_nn(3, 3);
	conf_nn = tldNN(spnEx.rightCols(spnEx.cols() / 2), tld);

	tld.model->thr_nn = std::max(tld.model->thr_nn, conf_nn.block(0, 0, 1,
			conf_nn.cols() / 3).maxCoeff());
	tld.model->thr_nn_valid = std::max(tld.model->thr_nn_valid,
			tld.model->thr_nn);
}