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