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! }