bool FirthRegression::FitFirthModel(Matrix & X, Vector & y, int nrrounds)
{
this-> Reset(X);
G_to_Eigen(X, &this->w->X);
G_to_Eigen(y, &this->w->y);
int rounds = 0;
// double lastDeviance, currentDeviance;
Eigen::MatrixXf xw; // W^(1/2) * X
// Newton-Raphson
while (rounds < nrrounds) {
// std::cout << "beta = " << this->w->beta << "\n";
this->w->eta = this->w->X * this->w->beta;
this->w->p = (1.0 + (-this->w->eta.array()).exp()).inverse();
this->w->V = this->w->p.array() * (1.0 - this->w->p.array());
xw = (this->w->V.array().sqrt().matrix().asDiagonal() * this->w->X).eval(); // W^(1/2) * X
this->w->D = xw.transpose() * xw; // X' V X
this->w->covB = this->w->D.eval().ldlt().solve(Eigen::MatrixXf::Identity(this->w->D.rows(), this->w->D.rows()));
// double rel = ((this->w->D * this->w->covB).array() - Eigen::MatrixXf::Identity(this->w->D.rows(), this->w->D.rows()).array()).matrix().norm() / this->w->D.rows() / this->w->D.rows();
// // printf("norm = %g\n", rel);
// if (rel > 1e-6) { // use relative accuracy to evalute convergence
if ((this->w->D * this->w->covB - Eigen::MatrixXf::Identity(this->w->D.rows(), this->w->D.rows())).norm() > 1e-3) {
// cannot inverse
// printToFile(this->w->D, "matD", "D");
// printToFile(this->w->covB, "matCovB", "B");
return false;
}
this->w->h = (xw * this->w->covB * xw.transpose()).diagonal();
this->w->r = this->w->X.transpose() * (this->w->y - this->w->p + (this->w->h.array() * (0.5 - this->w->p.array())).matrix()); // X' (y-mu)
this->w->delta_beta = this->w->covB * this->w->r;
this->w->beta += this->w->delta_beta;
// printf("norm = %g\n", this->w->delta_beta.norm());
// use relative accuracy to evalute convergence
if (rounds > 1 && (this->w->beta.norm() > 0 && this->w->delta_beta.norm() / this->w->beta.norm() < 1e-6)) {
rounds = 0;
break;
}
rounds ++;
}
if (rounds == nrrounds)
{
printf("Not enough iterations!");
return false;
}
// this->w->covB = this->w->D.eval().llt().solve(Eigen::MatrixXf::Identity(this->w->D.rows(), this->w->D.rows()));
Eigen_to_G(this->w->beta, &B);
Eigen_to_G(this->w->covB, &covB);
Eigen_to_G(this->w->p, &p);
Eigen_to_G(this->w->V, &V);
return true;
}
void getBetaSigma2(double delta) {
Eigen::MatrixXf x = (this->lambda.array() + delta).sqrt().matrix().asDiagonal() * this->ux;
Eigen::MatrixXf y = (this->lambda.array() + delta).sqrt().matrix().asDiagonal() * this->uy;
this->beta = (x.transpose() * x).eval().ldlt().solve(x.transpose() * y);
double sumResidual2 = getSumResidual2(delta);
// if ( model == GrammarGamma::MLE) {
this->sigma2_g = sumResidual2 / x.rows();
// } else {
// this->sigma2 = sumResidual2 / (x.rows() - x.cols());
// }
}
// NOTE: need to fit null model fit before calling this function
double GetAF(const EigenMatrix& kinshipU, const EigenMatrix& kinshipS) const{
const Eigen::MatrixXf& U = kinshipU.mat;
Eigen::MatrixXf u1 = Eigen::MatrixXf::Ones(U.rows(), 1);
u1 = U.transpose() * u1;
Eigen::MatrixXf x = (this->lambda.array() + delta).sqrt().matrix().asDiagonal() * u1;
Eigen::MatrixXf y = (this->lambda.array() + delta).sqrt().matrix().asDiagonal() * this->ug;
Eigen::MatrixXf beta = (x.transpose() * x).inverse() * x.transpose() * y;
// here x is represented as 0, 1, 2, so beta(0, 0) is the mean genotype
// multiply by 0.5 to get AF
double af = 0.5 * beta(0, 0);
return af;
}
// Perform a linear regression on the power ratio in each order
// Omit l=2 - tends to be abnormally small due to non-isotropic brain-wide fibre distribution
std::pair<float, float> get_regression (const std::vector<float>& ratios)
{
const size_t n = ratios.size() - 1;
Eigen::VectorXf Y (n), b (2);
Eigen::MatrixXf A (n, 2);
for (size_t i = 1; i != ratios.size(); ++i) {
Y[i-1] = ratios[i];
A(i-1,0) = 1.0f;
A(i-1,1) = (2*i)+2;
}
b = (A.transpose() * A).ldlt().solve (A.transpose() * Y);
return std::make_pair (b[0], b[1]);
}
bool FirthRegression::FitFirthModel(Matrix & X, Vector & succ, Vector& total, int nrrounds) {
this-> Reset(X);
G_to_Eigen(X, &this->w->X);
G_to_Eigen(succ, &this->w->succ);
G_to_Eigen(total, &this->w->total);
int rounds = 0;
// double lastDeviance, currentDeviance;
Eigen::MatrixXf xw; // W^(1/2) * X
// Newton-Raphson
while (rounds < nrrounds) {
// beta = beta + solve( t(X)%*%diag(p*(1-p)) %*%X) %*% t(X) %*% (Y-p);
this->w->eta = this->w->X * this->w->beta;
this->w->p = (-this->w->eta.array().exp() + 1.0).inverse();
this->w->V = this->w->p.array() * (1.0 - this->w->p.array()) * this->w->total.array();
xw = (this->w->V.array().sqrt().matrix().asDiagonal() * this->w->X).eval();
this->w->D = xw.transpose() * xw; // this->w->X.transpose() * this->w->V.asDiagonal() * this->w->X; // X' V X
this->w->covB = this->w->D.eval().llt().solve(Eigen::MatrixXf::Identity(this->w->D.rows(), this->w->D.rows()));
// double rel = ((this->w->D * this->w->covB).array() - Eigen::MatrixXf::Identity(this->w->D.rows(), this->w->D.rows()).array()).matrix().norm() / this->w->D.rows() / this->w->D.rows();
// if (rel > 1e-6) { // use relative accuracy to evalute convergence
if ((this->w->D * this->w->covB - Eigen::MatrixXf::Identity(this->w->D.rows(), this->w->D.rows())).norm() > 1e-3) {
// cannot inverse
return false;
}
this->w->h = (xw.transpose() * this->w->covB * xw).diagonal();
this->w->r = this->w->X.transpose() * (this->w->succ.array() - this->w->total.array() * this->w->p.array()
+ this->w->total.array() * this->w->h.array() * (0.5 - this->w->p.array())).matrix();
this->w->delta_beta = this->w->covB * this->w->r;
this->w->beta += this->w->delta_beta;
if (rounds > 1 && (this->w->beta.norm() > 0 && this->w->delta_beta.norm() / this->w->beta.norm() < 1e-6)) {
rounds = 0;
break;
}
rounds ++;
}
if (rounds == nrrounds)
{
printf("Not enough iterations!");
return false;
}
Eigen_to_G(this->w->beta, &B);
Eigen_to_G(this->w->covB, &covB);
Eigen_to_G(this->w->p, &p);
Eigen_to_G(this->w->V, &V);
return true;
}
//params is a matrix of nx2 where n is the number of landmarks
//for each landmark, the x and y pose of where it is
//pose is a matrix of 2x1 containing the initial guess of the robot pose
//delta is a matrix of 2x1 returns the increment in the x and y of the robot
void LMAlgr::computeIncrement(Eigen::MatrixXf params, Eigen::MatrixXf pose, double lambda, Eigen::MatrixXf error, Eigen::MatrixXf &delta){
Eigen::MatrixXf Jac;
Jac.resize(params.rows(), 2);
//initialize the jacobian matrix
for(int i = 0; i < params.rows(); i++){
Jac(i, 0) = (params(i, 1) - pose(1, 0)) / (pow(params(i, 0) - pose(0, 0), 2) + pow(params(i, 1) - pose(1, 0), 2));
Jac(i, 1) = -1 * (params(i, 0) - pose(0, 0)) / (pow(params(i, 0) - pose(0, 0), 2) + pow(params(i, 1) - pose(1, 0), 2));
}
Eigen::MatrixXf I;
I = Eigen::MatrixXf::Identity(2, 2);
Eigen::MatrixXf tmp = (Jac.transpose() * Jac) + (lambda * I);
Eigen::MatrixXf incr = tmp.inverse() * Jac.transpose() * error;
delta = incr;
}
float pseudo_inv(const Eigen::MatrixXf *mat_in,
Eigen::MatrixXf *mat_out) {
int dim = 0;
// Get matrices dimension :
if (mat_in->cols () != mat_in->rows ()) {
THROW_ERR("Cannot compute matrix pseudo_inverse");
} else {
dim = mat_in->cols ();
}
mat_out->resize (dim, dim);
Eigen::MatrixXf U (dim,dim);
Eigen::MatrixXf eig_val (dim, 1);
Eigen::MatrixXf eig_val_inv (dim, dim);
Eigen::MatrixXf V (dim, dim);
float det;
eig_val_inv = Eigen::MatrixXf::Identity(dim,dim);
// Compute the SVD decomposition
Eigen::JacobiSVD<Eigen::MatrixXf> svd(*mat_in, Eigen::ComputeFullU | Eigen::ComputeFullV);
eig_val = svd.singularValues();
U = svd.matrixU();
V = svd.matrixV();
// Compute pseudo-inverse
// - quick'n'dirty inversion of eigen matrix
for (int i = 0; i<dim; ++i) {
if (eig_val(i,0) != 0.f)
eig_val_inv(i,i) = 1.f / eig_val(i,0);
else
eig_val_inv(i,i) = 0.f;
}
*mat_out = V.transpose() * eig_val_inv * U.transpose();
// Compute determinant from eigenvalues..
det = 1.f;
for (int i=0; i<dim; ++i) {
det *= eig_val(i,0);
}
return det;
}
template <typename PointSource, typename PointTarget> void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::getTransformationFromCorrelation (
const Eigen::MatrixXf &cloud_src_demean,
const Eigen::Vector4f ¢roid_src,
const Eigen::MatrixXf &cloud_tgt_demean,
const Eigen::Vector4f ¢roid_tgt,
Eigen::Matrix4f &transformation_matrix)
{
transformation_matrix.setIdentity ();
// Assemble the correlation matrix H = source * target'
Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
// Compute the Singular Value Decomposition
Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f u = svd.matrixU ();
Eigen::Matrix3f v = svd.matrixV ();
// Compute R = V * U'
if (u.determinant () * v.determinant () < 0)
{
for (int x = 0; x < 3; ++x)
v (x, 2) *= -1;
}
Eigen::Matrix3f R = v * u.transpose ();
// Return the correct transformation
transformation_matrix.topLeftCorner<3, 3> () = R;
Eigen::Vector3f Rc = R * centroid_src.head<3> ();
transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.head<3> () - Rc;
}
void gaussNewtonFromSamplesWeighed(const Eigen::VectorXf &xb, const Eigen::VectorXf &rb, const Eigen::MatrixXf &X, const Eigen::VectorXf &weights, const Eigen::VectorXf &residuals, float regularization, Eigen::VectorXf &out_result)
{
//Summary:
//out_result=xb - G rb
//xb is the best sample, rb is the best sample residual vector
//G=AB'inv(BB'+kI)
//A.col(i)=weights[i]*(X.row(i)-best sample)'
//B.col(i)=weights[i]*(residuals - rb)'
//k=regularization
//Get xb, r(xb)
//cv::Mat xb=X.row(bestIndex);
//cv::Mat rb=residuals.row(bestIndex);
//Compute A and B
MatrixXf A=X.transpose();
MatrixXf B=residuals.transpose();
for (int i=0; i<A.cols(); i++)
{
A.col(i)=weights[i]*(X.row(i).transpose()-xb);
B.col(i)=weights[i]*(residuals.row(i).transpose()-rb);
}
MatrixXf I=MatrixXf::Identity(B.rows(),B.rows());
I=I*regularization;
MatrixXf G=(A*B.transpose())*(B*B.transpose()+I).inverse();
out_result=xb - G * rb;
}
int main()
{
// ArrayFire
std::cout << "#ArrayFire" << std::endl;
for(g_i = 1; g_i <= (1 << BITS); g_i <<= 1) {
for(g_j = 1; g_j <= (1 << BITS); g_j <<= 1) {
A = af::randu(g_i, g_j);
A_trans = af::transpose(A);
ev(calc);
}
}
// Eigen
std::cout << "#Eigen" << std::endl;
for(g_i = 1; g_i <= (1 << BITS); g_i <<= 1) {
for(g_j = 1; g_j <= (1 << BITS); g_j <<= 1) {
A_Eigen = Eigen::MatrixXf::Random(g_i, g_j);
A_Eigen_trans = A_Eigen.transpose();
ev(calcEigen);
}
}
/*
// Eigen Parallel
for(g_i = 1; g_i <= (1 << BITS); g_i <<= 1) {
A_Eigen = Eigen::MatrixXf::Random(g_i, g_i);
ev(calcEigenParallel);
}
*/
return 0;
}
Eigen::MatrixXf PhotoCamera::getPseudoInverse()
{
Eigen::MatrixXf P = intrinsicMatrix*extrinsicMatrix.matrix();
Eigen::MatrixXf Pt = P.transpose();
Eigen::MatrixXf pseudoInverse = Pt*(P*Pt).inverse();
return pseudoInverse;
}
Eigen::MatrixXf JacobiProvider::computePseudoInverseJacobianMatrix(const Eigen::MatrixXf &m) const
{
#ifdef CHECK_PERFORMANCE
clock_t startT = clock();
#endif
MatrixXf pseudo;
switch (inverseMethod)
{
case eTranspose:
{
if (jointWeights.rows() == m.cols())
{
Eigen::MatrixXf W = jointWeights.asDiagonal();
Eigen::MatrixXf W_1 = W.inverse();
pseudo = W_1 * m.transpose() * (m*W_1*m.transpose()).inverse();
}
else
{
pseudo = m.transpose() * (m*m.transpose()).inverse();
}
break;
}
case eSVD:
{
float pinvtoler = 0.00001f;
pseudo = MathTools::getPseudoInverse(m, pinvtoler);
break;
}
case eSVDDamped:
{
float pinvtoler = 0.00001f;
pseudo = MathTools::getPseudoInverseDamped(m,pinvtoler);
break;
}
default:
THROW_VR_EXCEPTION("Inverse Jacobi Method nyi...");
}
#ifdef CHECK_PERFORMANCE
clock_t endT = clock();
float diffClock = (float)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0f);
//if (diffClock>10.0f)
cout << "Inverse Jacobi time:" << diffClock << endl;
#endif
return pseudo;
}
tf::Transform SvdTransformOptimization::svdOwnImpl(
std::vector<tf::Vector3> pointcloudX,
std::vector<tf::Vector3> pointcloudP) {
// Calculate center of mass for both pointclouds.
int numOfPoints = pointcloudX.size();
tf::Vector3 centerOfMassX, centerOfMassP;
for (int i = 0; i < numOfPoints; i++) {
centerOfMassX += pointcloudX[i];
centerOfMassP += pointcloudP[i];
}
centerOfMassX /= numOfPoints;
centerOfMassP /= numOfPoints;
// Extract the center of mass from the corresponding points.
std::vector<tf::Vector3> pointcloudXPrime, pointcloudPPrime;
for (int i = 0; i < numOfPoints; i++) {
pointcloudXPrime.push_back(pointcloudX[i] - centerOfMassX);
pointcloudPPrime.push_back(pointcloudP[i] - centerOfMassP);
}
// Calculate matrix W
Eigen::MatrixXf W = Eigen::MatrixXf::Zero(3, 3);
for (int i = 0; i < numOfPoints; i++) {
Eigen::Vector3f currentPointXPrime(pointcloudXPrime[i].getX(),
pointcloudXPrime[i].getY(), pointcloudXPrime[i].getZ());
Eigen::Vector3f currentPointPPrime(pointcloudPPrime[i].getX(),
pointcloudPPrime[i].getY(), pointcloudPPrime[i].getZ());
W += currentPointXPrime * currentPointPPrime.transpose();
}
// Perform the SVD.
Eigen::JacobiSVD<Eigen::MatrixXf> svd(W);
svd.compute(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::MatrixXf U = svd.matrixU();
Eigen::MatrixXf V = svd.matrixV();
// Caclulate Rotation and translation and convert to tf.
Eigen::MatrixXf R = U * V.transpose();
Eigen::Vector3f centerOfMassXEigen(centerOfMassX.getX(),
centerOfMassX.getY(), centerOfMassX.getZ());
Eigen::Vector3f centerOfMassPEigen(centerOfMassP.getX(),
centerOfMassP.getY(), centerOfMassP.getZ());
Eigen::MatrixXf t = centerOfMassXEigen - R * (centerOfMassPEigen);
tf::Matrix3x3 Rtf(R(0, 0), R(0, 1), R(0, 2), R(1, 0), R(1, 1), R(1, 2),
R(2, 0), R(2, 1), R(2, 2));
tf::Vector3 ttf(t(0), t(1), t(2));
// Create and return the new transform.
tf::Transform newTransform(Rtf, ttf);
return newTransform;
}
virtual Eigen::VectorXf gradient( const Eigen::MatrixXf & a, const Eigen::MatrixXf & b ) const {
if (ktype_ == CONST_KERNEL)
return Eigen::VectorXf();
Eigen::MatrixXf fg = featureGradient( a, b );
if (ktype_ == DIAG_KERNEL)
return (f_.array()*fg.array()).rowwise().sum();
else {
Eigen::MatrixXf p = fg*f_.transpose();
p.resize( p.cols()*p.rows(), 1 );
return p;
}
}
void calcMeanAndCovarWeighedVectorized(const Eigen::MatrixXf &input, const Eigen::VectorXd &inputWeights, Eigen::MatrixXf &out_covMat, Eigen::VectorXf &out_mean,Eigen::MatrixXf &temp)
{
out_mean=input.col(0); //to resize
out_mean.setZero();
double wSumInv=1.0/inputWeights.sum();
for (int k=0;k<inputWeights.size();k++){
double w=inputWeights[k];
out_mean+=input.col(k)*(float)(w*wSumInv);
}
out_mean = input.rowwise().mean();
temp = (input.colwise() - out_mean);
for (int k=0;k<inputWeights.size();k++){
temp.col(k) *= (float)(sqrt(inputWeights(k)*wSumInv)); //using square roots, as we only want the normalized weights to be included once for each result element in the multiplication below
}
out_covMat = temp*temp.transpose();
}
/**
* estimateRigidTransformationSVD
*/
void RigidTransformationRANSAC::estimateRigidTransformationSVD(
const std::vector<Eigen::Vector3f > &srcPts,
const std::vector<int> &srcIndices,
const std::vector<Eigen::Vector3f > &tgtPts,
const std::vector<int> &tgtIndices,
Eigen::Matrix4f &transform)
{
Eigen::Vector3f srcCentroid, tgtCentroid;
// compute the centroids of source, target
ComputeCentroid (srcPts, srcIndices, srcCentroid);
ComputeCentroid (tgtPts, tgtIndices, tgtCentroid);
// Subtract the centroids from source, target
Eigen::MatrixXf srcPtsDemean;
DemeanPoints(srcPts, srcIndices, srcCentroid, srcPtsDemean);
Eigen::MatrixXf tgtPtsDemean;
DemeanPoints(tgtPts, tgtIndices, tgtCentroid, tgtPtsDemean);
// Assemble the correlation matrix H = source * target'
Eigen::Matrix3f H = (srcPtsDemean * tgtPtsDemean.transpose ()).topLeftCorner<3, 3>();
// Singular Value Decomposition
Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f u = svd.matrixU ();
Eigen::Matrix3f v = svd.matrixV ();
// Compute R = V * U'
if (u.determinant () * v.determinant () < 0)
{
for (int x = 0; x < 3; ++x)
v (x, 2) *= -1;
}
// Return the transformation
Eigen::Matrix3f R = v * u.transpose ();
Eigen::Vector3f t = tgtCentroid - R * srcCentroid;
// set rotation
transform.block(0,0,3,3) = R;
// set translation
transform.block(0,3,3,1) = t;
transform.block(3, 0, 1, 3).setZero();
transform(3,3) = 1.;
}
void run(Mat& A, const int rank){
if (A.cols() == 0 || A.rows() == 0) return;
int r = (rank < A.cols()) ? rank : A.cols();
r = (r < A.rows()) ? r : A.rows();
// Gaussian Random Matrix
Eigen::MatrixXf O(A.rows(), r);
Util::sampleGaussianMat(O);
// Compute Sample Matrix of A
Eigen::MatrixXf Y = A.transpose() * O;
// Orthonormalize Y
Util::processGramSchmidt(Y);
Eigen::MatrixXf B = Y.transpose() * A * Y;
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigenOfB(B);
eigenValues_ = eigenOfB.eigenvalues();
eigenVectors_ = Y * eigenOfB.eigenvectors();
}
void run(Mat& A, const int rank){
if (A.cols() == 0 || A.rows() == 0) return;
int r = (rank < A.cols()) ? rank : A.cols();
r = (r < A.rows()) ? r : A.rows();
// Gaussian Random Matrix for A^T
Eigen::MatrixXf O(A.rows(), r);
Util::sampleGaussianMat(O);
// Compute Sample Matrix of A^T
Eigen::MatrixXf Y = A.transpose() * O;
// Orthonormalize Y
Util::processGramSchmidt(Y);
// Range(B) = Range(A^T)
Eigen::MatrixXf B = A * Y;
// Gaussian Random Matrix
Eigen::MatrixXf P(B.cols(), r);
Util::sampleGaussianMat(P);
// Compute Sample Matrix of B
Eigen::MatrixXf Z = B * P;
// Orthonormalize Z
Util::processGramSchmidt(Z);
// Range(C) = Range(B)
Eigen::MatrixXf C = Z.transpose() * B;
Eigen::JacobiSVD<Eigen::MatrixXf> svdOfC(C, Eigen::ComputeThinU | Eigen::ComputeThinV);
// C = USV^T
// A = Z * U * S * V^T * Y^T()
matU_ = Z * svdOfC.matrixU();
matS_ = svdOfC.singularValues();
matV_ = Y * svdOfC.matrixV();
}
int TestCovariate(Matrix& Xnull, Matrix& Y, Matrix& Xcol,
const EigenMatrix& kinshipU, const EigenMatrix& kinshipS){
Eigen::MatrixXf g;
G_to_Eigen(Xcol, &g);
// store U'*G for computing AF later.
const Eigen::MatrixXf& U = kinshipU.mat;
this->ug = U.transpose() * g;
Eigen::RowVectorXf g_mean = g.colwise().mean();
g = g.rowwise() - g_mean;
double gTg = g.array().square().sum();
double t_new = (g.array() * this->transformedY.array()).sum();
t_new = t_new * t_new / gTg;
double t_score = t_new / this->gamma;
this->betaG = (g.transpose() * this->transformedY).sum() / gTg / this->gamma;
this->betaGVar = this->ySigmaY / gTg / this->gamma;
this->pvalue = gsl_cdf_chisq_Q(t_score, 1.0);
return 0;
}
void dart::ReportedJointsPrior::computeContribution(Eigen::SparseMatrix<float> & fullJTJ,
Eigen::VectorXf & fullJTe,
const int * modelOffsets,
const int priorParamOffset,
const std::vector<MirroredModel *> & models,
const std::vector<Pose> & poses,
const OptimizationOptions & opts)
{
// get mapping of reported joint names and values
std::map<std::string, float> rep_map;
for(unsigned int i=0; i<_reported.getReducedArticulatedDimensions(); i++) {
// apply lower and upper joint limits
rep_map[_reported.getReducedName(i)] =
std::min(std::max(_reported.getReducedArticulation()[i], _reported.getReducedMin(i)), _reported.getReducedMax(i));
}
#ifdef LCM_DEBUG_GRADIENT
std::vector<std::string> names;
#if FILTER_FIXED_JOINTS
const bool pub_grad = (_skipped==GRADIENT_SKIP);
#endif
#endif
// compute difference of reported to estimated joint value
Eigen::VectorXf diff = Eigen::VectorXf::Zero(_estimated.getReducedArticulatedDimensions());
for(unsigned int i=0; i<_estimated.getReducedArticulatedDimensions(); i++) {
const std::string jname = _estimated.getReducedName(i);
#ifdef LCM_DEBUG_GRADIENT
#if FILTER_FIXED_JOINTS
if(pub_grad)
if( !(_estimated.getReducedMin(i)==0 && _estimated.getReducedMin(i)==0) )
#endif
names.push_back(jname);
#endif
float rep = rep_map.at(jname);
float est = _estimated.getReducedArticulation()[i];
diff[i] = rep_map.at(jname) - _estimated.getReducedArticulation()[i];
}
// set nan values to 0, e.g. comparison of nan values always yields false
diff = (diff.array()!=diff.array()).select(0,diff);
// get Gauss-Newton parameter for specific objective function
Eigen::MatrixXf J = Eigen::MatrixXf::Zero(_estimated.getReducedArticulatedDimensions(), 1);
Eigen::VectorXf JTe = Eigen::VectorXf::Zero(_estimated.getReducedArticulatedDimensions());
std::tie(J,JTe) = computeGNParam(diff);
const Eigen::MatrixXf JTJ = J.transpose()*J;
#ifdef LCM_DEBUG_GRADIENT
#if FILTER_FIXED_JOINTS
if(pub_grad) {
#endif
// publish gradient (JTe)
bot_core::joint_angles_t grad;
grad.num_joints = names.size();
grad.joint_name = names;
for(unsigned int i = 0; i<JTe.size(); i++) {
#if FILTER_FIXED_JOINTS
if(!(_estimated.getReducedMin(i)==0 && _estimated.getReducedMin(i)==0))
#endif
grad.joint_position.push_back(JTe[i]);
}
LCM_CommonBase::publish("DART_GRADIENT", &grad);
#if FILTER_FIXED_JOINTS
_skipped=0;
}
else {
_skipped++;
}
#endif
#endif // LCM_DEBUG_GRADIENT
for(unsigned int r=0; r<JTJ.rows(); r++)
for(unsigned int c=0; c<JTJ.cols(); c++)
if(JTJ(r,c)!=0)
fullJTJ.coeffRef(modelOffsets[_modelID]+6+r, modelOffsets[_modelID]+6+c) += JTJ(r,c);
for(unsigned int r=0; r<JTe.rows(); r++)
if(JTe[r]!=0)
fullJTe[modelOffsets[_modelID]+6+r] += JTe[r];
}
//********************************
//* main
int main(int argc, char* argv[]) {
if( (argc != 12) && (argc != 14) ){
std::cerr << "usage: " << argv[0] << " [path] <rank_num> <exist_voxel_num_threshold> [model_pca_filename] <dim_model> <size1> <size2> <size3> <detect_th> <distance_th> /input:=/camera/rgb/points" << std::endl;
exit( EXIT_FAILURE );
}
char tmpname[ 1000 ];
ros::init (argc, argv, "detectObj", ros::init_options::AnonymousName);
// read the length of voxel side
sprintf( tmpname, "%s/param/parameters.txt", argv[1] );
voxel_size = Param::readVoxelSize( tmpname );
detect_th = atof( argv[9] );
distance_th = atof( argv[10] );
rank_num = atoi( argv[2] );
// read the number of voxels in each subdivision's side of scene
box_size = Param::readBoxSizeScene( tmpname );
// read the dimension of compressed feature vectors
dim = Param::readDim( tmpname );
// set the dimension of the target object's subspace
const int dim_model = atoi(argv[5]);
if( dim <= dim_model ){
std::cerr << "ERR: dim_model should be less than dim(in dim.txt)" << std::endl;
exit( EXIT_FAILURE );
}
// read the threshold for RGB binalize
sprintf( tmpname, "%s/param/color_threshold.txt", argv[1] );
Param::readColorThreshold( color_threshold_r, color_threshold_g, color_threshold_b, tmpname );
// determine the size of sliding box
region_size = box_size * voxel_size;
float tmp_val = atof(argv[6]) / region_size;
int size1 = (int)tmp_val;
if( ( ( tmp_val - size1 ) >= 0.5 ) || ( size1 == 0 ) ) size1++;
tmp_val = atof(argv[7]) / region_size;
int size2 = (int)tmp_val;
if( ( ( tmp_val - size2 ) >= 0.5 ) || ( size2 == 0 ) ) size2++;
tmp_val = atof(argv[8]) / region_size;
int size3 = (int)tmp_val;
if( ( ( tmp_val - size3 ) >= 0.5 ) || ( size3 == 0 ) ) size3++;
sliding_box_size_x = size1 * region_size;
sliding_box_size_y = size2 * region_size;
sliding_box_size_z = size3 * region_size;
// set variables
search_obj.setRange( size1, size2, size3 );
search_obj.setRank( rank_num );
search_obj.setThreshold( atoi(argv[3]) );
search_obj.readAxis( argv[4], dim, dim_model, ASCII_MODE_P, MULTIPLE_SIMILARITY );
// read projection axis of the target object's subspace
PCA pca;
sprintf( tmpname, "%s/models/compress_axis", argv[1] );
pca.read( tmpname, ASCII_MODE_P );
Eigen::MatrixXf tmpaxis = pca.getAxis();
Eigen::MatrixXf axis = tmpaxis.block( 0,0,tmpaxis.rows(),dim );
Eigen::MatrixXf axis_t = axis.transpose();
Eigen::VectorXf variance = pca.getVariance();
if( WHITENING )
search_obj.setSceneAxis( axis_t, variance, dim );
else
search_obj.setSceneAxis( axis_t );
// object detection
VoxelizeAndDetect vad;
vad.loop();
ros::spin();
return 0;
}
void BVHAnimator::solveLeftArm(int frame_no, float scale, float x, float y, float z)
{
_bvh->quaternionMoveTo(frame_no, scale);
// NOTE: you can use either matrix or quaternion to calculate the transformation
float *LArx, *LAry, *LArz, *LFAry;
float *mdata = _bvh->getMotionDataPtr(frame_no);
// 3 channels - Xrotation, Yrotation, Zrotation
// extract value address from motion data
CHANNEL *channel = larm->channels[0];
LArx = &mdata[channel->index];
channel = larm->channels[1];
LAry = &mdata[channel->index];
channel = larm->channels[2];
LArz = &mdata[channel->index];
channel = lforearm->channels[1];
LFAry = &mdata[channel->index];
cout << "Solving inverse kinematics..." << endl;
clock_t start_time = clock();
// -------------------------------------------------------
// TODO: [Part 3] - Inverse Kinematics
//
// Put your code below
// -------------------------------------------------------
// 1. Compute Jacobian
// 2. Take the inverse of the Jacobian
// 3. Compute Changes in DOFS. dtheta = J-1 * de
// 4. Apply the changes to DOFs and continue
float dtheta = 0.01;
int counter = 0;
float error;
//glm::vec3 startPos = lhand->transform.
do {
glm::vec3 endEffectorPos = lhand->transform.translation;
// Take a theoretical step
*LArx += glm::degrees(dtheta);
*LAry += glm::degrees(dtheta);
*LArz += glm::degrees(dtheta);
*LFAry += glm::degrees(dtheta);
_bvh->quaternionMoveTo(frame_no, scale);
// Get the difference between end effector and theoretical end effector (with small theta applied)
glm::vec3 endEffectorPosNew = lhand->transform.translation;
glm::vec3 de = endEffectorPosNew - endEffectorPos;
// Move back, we only want the theoretical difference
*LArx -= glm::degrees(dtheta);
*LAry -= glm::degrees(dtheta);
*LArz -= glm::degrees(dtheta);
*LFAry -= glm::degrees(dtheta);
_bvh->quaternionMoveTo(frame_no, scale);
// Compute Jacobian
Eigen::MatrixXf J = Eigen::MatrixXf::Zero(3, 4);
for (int row = 0; row < 3; row++) {
for (int col = 0; col < 4; col++) {
J(row, col) = de[row] / dtheta;
}
}
// Trouble calculating pseudo inverse here.. Fallback with transpose instead
//Eigen::MatrixXf J_inverse = J.transpose() * (J * J.transpose()).inverse();
//Eigen::MatrixXf J_inverse = (J.transpose() * J).inverse() * J.transpose();
Eigen::MatrixXf J_inverse = J.transpose();
//std::cout << " J_inverse " << J_inverse << std::endl;
// Compute changes in DOFS through jacobian pseudo inverse method
Eigen::VectorXf de_2 = Eigen::VectorXf::Zero(3, 1);
de_2(0) = x - endEffectorPos.x;
de_2(1) = y - endEffectorPos.y;
de_2(2) = z - endEffectorPos.z;
Eigen::VectorXf thetaChange = J_inverse * de_2;
// Apply the changes in angles
*LArx += glm::degrees(thetaChange(0));
*LAry += glm::degrees(thetaChange(1));
*LArz += glm::degrees(thetaChange(2));
*LFAry += glm::degrees(thetaChange(3));
_bvh->quaternionMoveTo(frame_no, scale);
counter++;
error = glm::abs(endEffectorPos.x - x) + glm::abs(endEffectorPos.y - y) + glm::abs(endEffectorPos.z - z);
} while (counter < 1000 && error > 0.003); // Keep looping while error is bigger than threshhold or limit reached..
// ----------------------------------------------------------
// Do not touch
// ----------------------------------------------------------
clock_t end_time = clock();
float elapsed = (end_time - start_time) / (float)CLOCKS_PER_SEC;
cout << "Solving done in " << elapsed * 1000 << " ms." << endl;
}
int Fit(Vector& res_G, // residual under NULL -- may change when permuting
Vector& v_G, // variance under NULL -- may change when permuting
Matrix& X_G, // covariance
Matrix& G_G, // genotype
Vector& w_G) // weight
{
this->nPeople = X_G.rows;
this->nMarker = G_G.cols;
this->nCovariate = X_G.cols;
// calculation w_sqrt
G_to_Eigen(w_G, &this->w_sqrt);
w_sqrt = w_sqrt.cwiseSqrt();
// calculate K = G * W * G'
Eigen::MatrixXf G;
G_to_Eigen(G_G, &G);
this->K_sqrt.noalias() = w_sqrt.asDiagonal() * G.transpose();
// calculate Q = ||res * K||
Eigen::VectorXf res;
G_to_Eigen(res_G, &res);
this->Q = (this->K_sqrt * res).squaredNorm();
// calculate P0 = V - V X (X' V X)^(-1) X' V
Eigen::VectorXf v;
G_to_Eigen(v_G, &v);
if (this->nCovariate == 1) {
P0 = -v * v.transpose() / v.sum();
// printf("dim(P0) = %d, %d\n", P0.rows(), P0.cols());
// printf("dim(v) = %d\n", v.size());
P0.diagonal() += v;
// printf("dim(v) = %d\n", v.size());
} else {
Eigen::MatrixXf X;
G_to_Eigen(X_G, &X);
Eigen::MatrixXf XtV; // X^t V
XtV.noalias() = X.transpose() * v.asDiagonal();
P0 = -XtV.transpose() * ((XtV * X).inverse()) * XtV;
P0.diagonal() += v;
}
// dump();
// Eigen::MatrixXf tmp = K_sqrt * P0 * K_sqrt.transpose();
// dumpToFile(tmp, "out.tmp");
// eigen decomposition
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es;
es.compute(K_sqrt * P0 * K_sqrt.transpose());
#ifdef DEBUG
std::ofstream k("K");
k << K_sqrt;
k.close();
#endif
// std::ofstream p("P0");
// p << P0;
// p.close();
this->mixChiSq.reset();
int r_ub = std::min(nPeople, nMarker);
int r = 0; // es.eigenvalues().size();
int eigen_len = es.eigenvalues().size();
for (int i = eigen_len - 1; i >= 0; i--) {
if (es.eigenvalues()[i] > ZBOUND && r < r_ub) {
this->mixChiSq.addLambda(es.eigenvalues()[i]);
r++;
} else
break;
}
// calculate p-value
this->pValue = this->mixChiSq.getPvalue(this->Q);
if (this->pValue == 0.0 || this->pValue == 1.0) {
this->pValue = this->mixChiSq.getLiuPvalue(this->Q);
}
return 0;
};
void linearTMatrixTest(StrainLin * ene)
{
ElementRegGrid * grid = new ElementRegGrid(1, 1, 1);
MaterialQuad * material = new MaterialQuad(ene);
grid->m.push_back(material);
grid->x[1][0] += 0.1f;
grid->x[3][1] += 0.2f;
MatrixXf K = grid->getStiffness();
//linear material stiffness
ElementHex * ele = (ElementHex*)grid->e[0];
const Quadrature & q = Quadrature::Gauss2;
Eigen::MatrixXf Ka = Eigen::MatrixXf::Zero(3 * ele->nV(), 3 * ele->nV());
Eigen::MatrixXf E = ene->EMatrix();
Eigen::VectorXf U = Eigen::VectorXf::Zero(3 * ele->nV());
for (int ii = 0; ii < ele->nV(); ii++){
for (int jj = 0; jj < 3; jj++){
U[3 * ii + jj] = grid->x[ii][jj] - grid->X[ii][jj];
}
}
for (unsigned int ii = 0; ii < q.x.size(); ii++){
Eigen::MatrixXf B = ele->BMatrix(q.x[ii], grid->X);
Eigen::MatrixXf ss = E*B*U;
//std::cout <<"sigma:\n"<< ss << "\n";
Matrix3f F = ele->defGrad(q.x[ii], grid->X, grid->x);
Matrix3f P = ene->getPK1(F);
//std::cout << "P:\n";
//P.print();
Ka += q.w[ii] * B.transpose() * E * B;
//std::cout << "B:\n" << B << "\n";
}
//std::cout << "E:\n" << E << "\n";
//std::cout << "alt K:\n";
//std::cout << Ka << "\n";
float maxErr = 0;
for (int ii = 0; ii<K.mm; ii++){
for (int jj = 0; jj<K.nn; jj++){
float err = (float)std::abs(Ka(ii, jj) - K(ii, jj));
if (err>maxErr){
maxErr = err;
}
}
}
std::cout << "max err " << maxErr << "\n";
//assemble boundary matrix HNEB
std::ofstream out("T.txt");
// 2 point quadrature is accurate enough
const Quadrature & q2d = Quadrature::Gauss2_2D;
Eigen::MatrixXf T = Eigen::MatrixXf::Zero(3 * ele->nV(), 3 * ele->nV());
for (int ii = 0; ii < ele->nF(); ii++){
Eigen::MatrixXf Tf = Eigen::MatrixXf::Zero(3 * ele->nV(), 3 * ele->nV());
Eigen::MatrixXf N = ele->NMatrix(ii);
N.block(0, 3, 3, 3) = Eigen::MatrixXf::Zero(3, 3);
//std::cout << "N:\n"<<N << "\n";
for (unsigned int jj = 0; jj < q2d.x.size(); jj++){
Vector3f quadp = ele->facePt(ii, q2d.x[jj]);
Eigen::MatrixXf B0 = ele->BMatrix(quadp, grid->X);
Eigen::MatrixXf B = Eigen::MatrixXf::Zero(6, 3 * ele->nV());
//only add contributions from the face
for (int kk = 0; kk < 4; kk++){
int vidx = faces[ii][kk];
B.block(0, 3 * vidx, 6, 3) = B0.block(0, 3 * vidx, 6, 3);
}
//B=B0;
Eigen::MatrixXf H = ele->HMatrix(quadp);
//std::cout << "H:\n" << H.transpose() << "\n";
//std::cout << "B:\n" << B.transpose() << "\n";
//std::cout << "traction:\n";
//Tf += q2d.w[jj] * H.transpose() * N * E * B;
Tf += q2d.w[jj] * H.transpose() * N * E * N.transpose() * H;
//Tf += q2d.w[jj] * B.transpose() * E * B;
Eigen::Vector3f surfF = (N * E * B * U);
//std::cout << surfF << "\n";
Matrix3f F = ele->defGrad(quadp, grid->X, grid->x);
Matrix3f P = ene->getPK1(F);
Vector3f surfF1 = P * Vector3f(facen[ii][0], facen[ii][1], facen[ii][2]);
std::cout << surfF1[0] << " " << surfF1[1] << " " << surfF1[2] << "\n";
}
//out << Tf << "\n\n";
T += Tf;
}
//out << T << "\n\n";
//out << Ka << "\n";
out.close();
}
/**
* @function calculateTrifocalTensor
*/
void trifocalTensor::calculateTrifocalTensor() {
Eigen::JacobiSVD<Eigen::MatrixXf> svd( mEq, Eigen::ComputeThinU | Eigen::ComputeThinV );
Eigen::MatrixXf V = svd.matrixV();
printf("* V has %d rows and %d cols \n", V.rows(), V.cols() );
Eigen::FullPivLU<Eigen::MatrixXf> lu(mEq);
printf("* Rank of mEq is: %d \n", lu.rank() );
//std::cout << "Columns are nullspace : " << std::endl;
//std::cout<< lu.kernel() << std::endl;
//Eigen::MatrixXf kernel = lu.kernel();
//mmEq(mPointer, ToIndex1(3,1,2)=;3 = kernel.col( kernel.cols() - 1 );
// Eigen::MatrixXf Vt = V.transpose(); mmEq(mPointer, ToIndex1(3,1,2)=;3 = Vt.col( Vt.cols() - 1 );
mT123 = V.col( V.cols() - 1 );
printf("mT123: Rows: %d cols: %d \n", mT123.rows(), mT123.cols() );
// Saving them properly
mT.resize(0);
Eigen::MatrixXf T1(3,3);
T1(0,0) = mT123(0,0); T1(0,1) = mT123(1,0); T1(0,2) = mT123(2,0);
T1(1,0) = mT123(3,0); T1(1,1) = mT123(4,0); T1(1,2) = mT123(5,0);
T1(2,0) = mT123(6,0); T1(2,1) = mT123(7,0); T1(2,2) = mT123(8,0);
mT.push_back(T1);
printf("Saved T1 \n");
Eigen::MatrixXf T2(3,3);
T2(0,0) = mT123(9,0); T2(0,1) = mT123(10,0); T2(0,2) = mT123(11,0);
T2(1,0) = mT123(12,0); T2(1,1) = mT123(13,0); T2(1,2) = mT123(14,0);
T2(2,0) = mT123(15,0); T2(2,1) = mT123(16,0); T2(2,2) = mT123(17,0);
mT.push_back(T2);
printf("Saved T2 \n");
Eigen::MatrixXf T3(3,3);
T3(0,0) = mT123(18,0); T3(0,1) = mT123(19,0); T3(0,2) = mT123(20,0);
T3(1,0) = mT123(21,0); T3(1,1) = mT123(22,0); T3(1,2) = mT123(23,0);
T3(2,0) = mT123(24,0); T3(2,1) = mT123(25,0); T3(2,2) = mT123(26,0);
mT.push_back(T3);
printf("Saved T3 \n");
// Checking
Eigen::MatrixXf res = mEq*mT123;
std::cout << "Checking mEq*T: \n"<< res.transpose() << std::endl;
// Making it with last guy = 1
// Normalizing
for( int i = 0; i < mT.size(); ++i ) {
float temp = mT[i](2,2);
for( int j = 0; j < 3; ++j ) {
for( int k = 0; k < 3; ++k ) {
float orig = mT[i](j,k);
mT[i](j,k) = orig / temp;
}
}
}
// Visualize
for( int i = 0; i < mT.size(); ++i ) {
std::cout << "T("<<i<<"): \n" << mT[i] << std::endl;
}
// Test lines
for( int i = 0; i < mLLL.size(); ++i ) {
Eigen::VectorXf A(3);
Eigen::VectorXf B(3);
Eigen::VectorXf C(3);
Eigen::VectorXf Ap(3);
A(0) = mLLL[i][0].x;
A(1) = mLLL[i][0].y;
A(2) = mLLL[i][0].z;
B(0) = mLLL[i][1].x;
B(1) = mLLL[i][1].y;
B(2) = mLLL[i][1].z;
C(0) = mLLL[i][2].x;
C(1) = mLLL[i][2].y;
C(2) = mLLL[i][2].z;
Eigen::MatrixXf r0, r1, r2;
Eigen::MatrixXf Tt;
Tt = mT[0];
r0 = ( B.transpose() )*Tt*C;
Ap(0) = r0(0,0);
Tt = mT[1];
r1 = ( B.transpose() )*Tt*C;
Ap(1) = r1(0,0);
Tt = mT[2];
r2 = ( B.transpose() )*Tt*C;
Ap(2) = r2(0,0);
// Normalize Ap
float temp = A(2) / Ap(2);
float num;
num = Ap(0)*temp; Ap(0) = num;
num = Ap(1)*temp; Ap(1) = num;
num = Ap(2)*temp; Ap(2) = num;
std::cout <<" ("<<i<<") " <<" A: " << A.transpose() << std::endl;
std::cout <<" ("<<i<<") " <<" Ap: " << Ap.transpose() << std::endl;
}
}
inline
void cal_campose(Eigen::MatrixXf XXc,Eigen::MatrixXf XXw,
int n,Eigen::MatrixXf &R2,Eigen::VectorXf &t2)
{
//A
Eigen::MatrixXf X = XXw;
//B
Eigen::MatrixXf Y = XXc;
Eigen::MatrixXf eyen(n,n);
eyen = Eigen::MatrixXf::Identity(n,n);
Eigen::MatrixXf ones(n,n);
ones.setOnes();
Eigen::MatrixXf K(n,n);
K = eyen - ones/n;
vfloat3 ux;
for(int i =0; i < n; i++)
{
ux = ux + X.col(i);
}
ux = ux/n;
vfloat3 uy;
for(int i =0; i < n; i++)
{
uy = uy + Y.col(i);
}
uy = uy/n;
Eigen::MatrixXf XK(3,n);
XK = X*K;
Eigen::MatrixXf XKarre(3,n);
for(int i = 0 ; i < n ; i++)
{
XKarre(0,i) = XK(0,i)*XK(0,i);
XKarre(1,i) = XK(1,i)*XK(1,i);
XKarre(2,i) = XK(2,i)*XK(2,i);
}
Eigen::VectorXf sumXKarre(n);
float sigmx2 = 0;
for(int i = 0 ; i < n ; i++)
{
sumXKarre[i] = XKarre(0,i) + XKarre(1,i) + XKarre(2,i);
sigmx2 += sumXKarre[i];
}
sigmx2 /=n;
Eigen::MatrixXf SXY(3,3);
SXY = Y*K*(X.transpose())/n;
JacobiSVD<MatrixXf> svd(SXY, ComputeThinU | ComputeThinV);
Eigen::MatrixXf S(3,3);
S = Eigen::MatrixXf::Identity(3,3);
if(SXY.determinant() < 0)
{
S(3,3) = -1;
}
R2 = svd.matrixU() * S * (svd.matrixV()).transpose();
Eigen::MatrixXf D(3,3);
D.setZero();
for(int i = 0 ; i < svd.singularValues().size() ; i++)
{
D(i,i) = (svd.singularValues())[i];
}
float c2 = (D*S).trace()/sigmx2;
t2 = uy - c2*R2*ux;
vfloat3 Xx = R2.col(0);
vfloat3 Yy = R2.col(1);
vfloat3 Zz = R2.col(2);
if((x_cross(Xx,Yy)-Zz).norm()>2e-2)
{
R2.col(2) = -Zz;
}
}
template<typename PointT> inline void
pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag)
{
if (!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");
Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
const size_t n = eigenvectors_.cols ();// number of eigen vectors
Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
Eigen::VectorXf h = y - input;
if (h.norm() > 0)
h.normalize ();
else
h.setZero ();
float gamma = h.dot(input - mean_.head<3>());
Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
D.block(0,0,n,n) = a * a.transpose();
D /= float(n)/float((n+1) * (n+1));
for(std::size_t i=0; i < a.size(); i++) {
D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
D(i,D.cols()-1) = D(D.rows()-1,i);
D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
}
Eigen::MatrixXf R(D.rows(), D.cols());
Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
Eigen::VectorXf alphap = D_evd.eigenvalues().real();
eigenvalues_.resize(eigenvalues_.size() +1);
for(std::size_t i=0; i<eigenvalues_.size(); i++) {
eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
R.col(i) = D.col(D.cols()-i-1);
}
Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
Up.rightCols<1>() = h;
eigenvectors_ = Up*R;
if (!basis_only_) {
Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
coefficients_(coefficients_.rows()-1,i) = 0;
coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
}
a.resize(a.size()+1);
a(a.size()-1) = 0;
coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
}
mean_.head<3>() = meanp;
switch (flag)
{
case increase:
if (eigenvectors_.rows() >= eigenvectors_.cols())
break;
case preserve:
if (!basis_only_)
coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
eigenvalues_.resize(eigenvalues_.size()-1);
break;
default:
PCL_ERROR("[pcl::PCA] unknown flag\n");
}
}
int FitNullModel(Matrix& mat_Xnull, Matrix& mat_y,
const EigenMatrix& kinshipU, const EigenMatrix& kinshipS){
// type conversion
Eigen::MatrixXf x;
Eigen::MatrixXf y;
G_to_Eigen(mat_Xnull, &x);
G_to_Eigen(mat_y, &y);
this->lambda = kinshipS.mat;
const Eigen::MatrixXf& U = kinshipU.mat;
// rotate
this->ux = U.transpose() * x;
this->uy = U.transpose() * y;
// get beta, sigma2_g and delta
// where delta = sigma2_e / sigma2_g
double loglik[101];
int maxIndex = -1;
double maxLogLik = 0;
for (int i = 0; i <= 100; ++i ){
double d = exp(-10 + i * 0.2);
getBetaSigma2(d);
loglik[i] = getLogLikelihood(d);
// fprintf(stderr, "%d\tdelta=%g\tll=%lf\n", i, delta, loglik[i]);
if (std::isnan(loglik[i])) {
continue;
}
if (maxIndex < 0 || loglik[i] > maxLogLik) {
maxIndex = i;
maxLogLik = loglik[i];
}
}
if (maxIndex < -1) {
fprintf(stderr, "Cannot optimize\n");
return -1;
}
if (maxIndex == 0 || maxIndex == 100) {
// on the boundary
// do not try maximize it.
} else {
gsl_function F;
F.function = goalFunction;
F.params = this;
Minimizer minimizer;
double lb = exp(-10 + (maxIndex-1) * 0.2);
double ub = exp(-10 + (maxIndex+1) * 0.2);
double start = exp(-10 + maxIndex * 0.2);
if (minimizer.minimize(F, start, lb, ub)) {
// fprintf(stderr, "Minimization failed, fall back to initial guess.\n");
this->delta = start;
} else {
this->delta = minimizer.getX();
// fprintf(stderr, "minimization succeed when delta = %g, sigma2_g = %g\n", this->delta, this->sigma2_g);
}
}
// store some intermediate results
// fprintf(stderr, "maxIndex = %d, delta = %g, Try brent\n", maxIndex, delta);
// fprintf(stderr, "beta[%d][%d] = %g\n", (int)beta.rows(), (int)beta.cols(), beta(0,0));
this->h2 = 1.0 /(1.0 + this->delta);
this->sigma2 = this->sigma2_g * this->h2;
// we derive different formular to replace original eqn (7)
this->gamma = (this->lambda.array() / (this->lambda.array() + this->delta)).sum() / this->sigma2_g / (this->ux.rows() - 1 ) ;
// fprintf(stderr, "gamma = %g\n", this->gamma);
// transformedY = \Sigma^{-1} * (y_tilda) and y_tilda = y - X * \beta
// since \Sigma = (\sigma^2_g * h^2 ) * (U * (\lambda + delta) * U')
// transformedY = 1 / (\sigma^2_g * h^2 ) * (U * (\lambda+delta)^{-1} * U' * (y_tilda))
// = 1 / (\sigma^2_g * h^2 ) * (U * \lambda^{-1} * (uResid))
// since h^2 = 1 / (1+delta)
// transformedY = (1 + delta/ (\sigma^2_g ) * (U * \lambda^{-1} * (uResid))
Eigen::MatrixXf resid = y - x * (x.transpose() * x).eval().ldlt().solve(x.transpose() * y); // this is y_tilda
this->transformedY.noalias() = U.transpose() * resid;
this->transformedY = (this->lambda.array() + this->delta).inverse().matrix().asDiagonal() * this->transformedY;
this->transformedY = U * this->transformedY;
this->transformedY /= this->sigma2_g;
// fprintf(stderr, "transformedY(0,0) = %g\n", transformedY(0,0));
this->ySigmaY= (resid.array() * transformedY.array()).sum();
return 0;
}
//********************************
//* main
int main(int argc, char* argv[]) {
if( (argc != 13) && (argc != 15) ){
std::cerr << "usage: " << argv[0] << " [path] <rank_num> <exist_voxel_num_threshold> [model_pca_filename] <dim_model> <size1> <size2> <size3> <detect_th> <distance_th> <model_num> /input:=/camera/rgb/points" << std::endl;
exit( EXIT_FAILURE );
}
char tmpname[ 1000 ];
ros::init (argc, argv, "detect_object_vosch_multi", ros::init_options::AnonymousName);
// read the length of voxel side
sprintf( tmpname, "%s/param/parameters.txt", argv[1] );
voxel_size = Param::readVoxelSize( tmpname );
detect_th = atof( argv[9] );
distance_th = atof( argv[10] );
model_num = atoi( argv[11] );
rank_num = atoi( argv[2] );
// set marker color
const int marker_model_num = 6;
if( model_num > marker_model_num ){
std::cerr << "Please set more marker colors for detection of more than " << marker_model_num << " objects." << std::endl;
exit( EXIT_FAILURE );
}
marker_color_r = new float[ marker_model_num ];
marker_color_g = new float[ marker_model_num ];
marker_color_b = new float[ marker_model_num ];
marker_color_r[ 0 ] = 1.0; marker_color_g[ 0 ] = 0.0; marker_color_b[ 0 ] = 0.0; // red
marker_color_r[ 1 ] = 0.0; marker_color_g[ 1 ] = 1.0; marker_color_b[ 1 ] = 0.0; // green
marker_color_r[ 2 ] = 0.0; marker_color_g[ 2 ] = 0.0; marker_color_b[ 2 ] = 1.0; // blue
marker_color_r[ 3 ] = 1.0; marker_color_g[ 3 ] = 1.0; marker_color_b[ 3 ] = 0.0; // yellow
marker_color_r[ 4 ] = 0.0; marker_color_g[ 4 ] = 1.0; marker_color_b[ 4 ] = 1.0; // cyan
marker_color_r[ 5 ] = 1.0; marker_color_g[ 5 ] = 0.0; marker_color_b[ 5 ] = 1.0; // magenta
// marker_color_r[ 0 ] = 0.0; marker_color_g[ 0 ] = 1.0; marker_color_b[ 0 ] = 0.0; // green
// marker_color_r[ 1 ] = 0.0; marker_color_g[ 1 ] = 0.0; marker_color_b[ 1 ] = 1.0; // blue
// marker_color_r[ 2 ] = 0.0; marker_color_g[ 2 ] = 1.0; marker_color_b[ 2 ] = 1.0; // cyan
// marker_color_r[ 3 ] = 1.0; marker_color_g[ 3 ] = 0.0; marker_color_b[ 3 ] = 0.0; // pink
// read the number of voxels in each subdivision's side of scene
box_size = Param::readBoxSizeScene( tmpname );
// read the dimension of compressed feature vectors
dim = Param::readDim( tmpname );
const int dim_model = atoi(argv[5]);
if( dim <= dim_model ){
std::cerr << "ERR: dim_model should be less than dim(in dim.txt)" << std::endl;
exit( EXIT_FAILURE );
}
// read the threshold for RGB binalize
sprintf( tmpname, "%s/param/color_threshold.txt", argv[1] );
Param::readColorThreshold( color_threshold_r, color_threshold_g, color_threshold_b, tmpname );
// determine the size of sliding box
region_size = box_size * voxel_size;
float tmp_val = atof(argv[6]) / region_size;
int size1 = (int)tmp_val;
if( ( ( tmp_val - size1 ) >= 0.5 ) || ( size1 == 0 ) ) size1++;
tmp_val = atof(argv[7]) / region_size;
int size2 = (int)tmp_val;
if( ( ( tmp_val - size2 ) >= 0.5 ) || ( size2 == 0 ) ) size2++;
tmp_val = atof(argv[8]) / region_size;
int size3 = (int)tmp_val;
if( ( ( tmp_val - size3 ) >= 0.5 ) || ( size3 == 0 ) ) size3++;
sliding_box_size_x = size1 * region_size;
sliding_box_size_y = size2 * region_size;
sliding_box_size_z = size3 * region_size;
// set variables
search_obj.setModelNum( model_num );
#ifdef CCHLAC_TEST
sprintf( tmpname, "%s/param/max_c.txt", argv[1] );
#else
sprintf( tmpname, "%s/param/max_r.txt", argv[1] );
#endif
search_obj.setNormalizeVal( tmpname );
search_obj.setRange( size1, size2, size3 );
search_obj.setRank( rank_num * model_num ); // for removeOverlap()
search_obj.setThreshold( atoi(argv[3]) );
// read projection axes of the target objects' subspace
FILE *fp = fopen( argv[4], "r" );
char **model_file_names = new char* [ model_num ];
char line[ 1000 ];
for( int i=0; i<model_num; i++ ){
model_file_names[ i ] = new char [ 1000 ];
if( fgets( line, sizeof(line), fp ) == NULL ) std::cerr<<"fgets err"<<std::endl;
line[ strlen( line ) - 1 ] = '\0';
//fscanf( fp, "%s\n", model_file_names + i );
//model_file_names[ i ] = line;
sprintf( model_file_names[ i ], "%s", line );
//std::cout << model_file_names[ i ] << std::endl;
}
fclose(fp);
search_obj.readAxis( model_file_names, dim, dim_model, ASCII_MODE_P, MULTIPLE_SIMILARITY );
// read projection axis for feature compression
PCA pca;
sprintf( tmpname, "%s/models/compress_axis", argv[1] );
pca.read( tmpname, ASCII_MODE_P );
Eigen::MatrixXf tmpaxis = pca.getAxis();
Eigen::MatrixXf axis = tmpaxis.block( 0,0,tmpaxis.rows(),dim );
Eigen::MatrixXf axis_t = axis.transpose();
Eigen::VectorXf variance = pca.getVariance();
if( WHITENING )
search_obj.setSceneAxis( axis_t, variance, dim );
else
search_obj.setSceneAxis( axis_t );
// object detection
VoxelizeAndDetect vad;
vad.loop();
ros::spin();
return 0;
}
Eigen::MatrixXf PseudoInverse(Eigen::MatrixXf matrix)
{
Eigen::Matrix3f squaredMatrix = matrix*(matrix.transpose());
Eigen::Matrix3f dampedIdentity = pow(dampingPinv,2)*Eigen::Matrix3f::Identity();
return matrix.transpose()*((squaredMatrix+dampedIdentity).inverse());// NOT VERFIED, MAY CAUSE BUGS!
}