//************************************************************************************************************************* //************************************************* OLD STUFF ************************************************************* //************************************************************************************************************************* void wholeBodyReach::pinvTrunc(const MatrixRXd &A, double tol, MatrixRXd &Apinv, MatrixRXd &Spinv, VectorXd &sv) { // allocate memory int m = A.rows(), n = A.cols(), k = m<n?m:n; Spinv.setZero(k,k); // compute decomposition JacobiSVD<MatrixRXd> svd(A, ComputeThinU | ComputeThinV); // default Eigen SVD sv = svd.singularValues(); // compute pseudoinverse of singular value matrix for (int c=0;c<k; c++) if ( sv(c)> tol) Spinv(c,c) = 1/sv(c); // compute pseudoinverse Apinv = svd.matrixV() * Spinv * svd.matrixU().transpose(); }
//************************************************************************************************************************* void wholeBodyReach::pinvDampTrunc(const MatrixRXd &A, double tol, double damp, MatrixRXd &Apinv, MatrixRXd &ApinvDamp) { // allocate memory int m = A.rows(), n = A.cols(), k = m<n?m:n; MatrixRXd Spinv = MatrixRXd::Zero(k,k); MatrixRXd SpinvD = MatrixRXd::Zero(k,k); // compute decomposition JacobiSVD<MatrixRXd> svd(A, ComputeThinU | ComputeThinV); // default Eigen SVD VectorXd sv = svd.singularValues(); // compute pseudoinverses of singular value matrix double damp2 = damp*damp; for (int c=0;c<k; c++) { SpinvD(c,c) = sv(c) / (sv(c)*sv(c) + damp2); if ( sv(c)> tol) Spinv(c,c) = 1/sv(c); } // compute pseudoinverses Apinv = svd.matrixV() * Spinv * svd.matrixU().transpose(); // truncated pseudoinverse ApinvDamp = svd.matrixV() * SpinvD * svd.matrixU().transpose(); // damped pseudoinverse }
RcppExport SEXP crossprodrowmajor(SEXP X, SEXP yes) { using namespace Rcpp; using namespace RcppEigen; try { using Eigen::Map; using Eigen::MatrixXd; using Eigen::Lower; typedef float Scalar; typedef double Double; typedef Eigen::Matrix<Double, Eigen::Dynamic, Eigen::Dynamic> Matrix; typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRXd; typedef Eigen::Matrix<Double, Eigen::Dynamic, 1> Vector; typedef Eigen::Map<const Matrix> MapMat; typedef Eigen::Map<const Vector> MapVec; const bool rmyes(as<bool>(yes)); const Eigen::Map<MatrixXd> A(as<Map<MatrixXd> >(X)); //MapMat A(as<MapMat>(X)); MatrixRXd AA = A; const int n(A.cols()); MatrixXd AtA; if (rmyes) { AtA = MatrixXd(n, n).setZero().selfadjointView<Lower>().rankUpdate(AA.adjoint()); } else { AtA = MatrixXd(n, n).setZero().selfadjointView<Lower>().rankUpdate(A.adjoint()); } return wrap(AtA); } catch (std::exception &ex) { forward_exception_to_r(ex); } catch (...) { ::Rf_error("C++ exception (unknown reason)"); } return R_NilValue; //-Wall }
//************************************************************************************************************************* void wholeBodyReach::assertEqual(const MatrixRXd &A, const MatrixRXd &B, string testName, double tol) { if(A.cols() != B.cols() || A.rows()!=B.rows()) { cout<< testName<< ": dim(A) != dim(B): " << A.rows()<< "x"<<A.cols()<<" != "<< B.rows()<< "x"<<B.cols()<<endl; return testFailed(testName); } for(int r=0; r<A.rows(); r++) for(int c=0; c<A.cols(); c++) if(abs(A(r,c)-B(r,c))>tol) { printf("%s: element %d,%d is different, absolute difference is %f\n", testName.c_str(), r, c, abs(A(r,c)-B(r,c))); return testFailed(testName); } }
//port faster cross product RcppExport SEXP crossprodxval(SEXP X, SEXP idxvec_) { using namespace Rcpp; using namespace RcppEigen; try { using Eigen::Map; using Eigen::MatrixXd; using Eigen::VectorXi; using Eigen::Lower; using Rcpp::List; typedef float Scalar; typedef double Double; typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Matrix; typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRXd; typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector; typedef Eigen::Map<Matrix> MapMat; typedef Eigen::Map<MatrixRXd> MapRMat; typedef Eigen::Map<const Vector> MapVec; typedef Map<VectorXd> MapVecd; typedef Map<VectorXi> MapVeci; typedef Eigen::SparseVector<double> SparseVector; typedef Eigen::SparseVector<int> SparseVectori; const MapMat AA(as<MapMat>(X)); const MapVeci idxvec(as<MapVeci>(idxvec_)); const int n(AA.cols()); MatrixXd AtA(MatrixXd(n, n)); MatrixRXd A = AA; int nfolds = idxvec.maxCoeff(); List xtxlist(nfolds); for (int k = 1; k < nfolds + 1; ++k) { VectorXi idxbool = (idxvec.array() == k).cast<int>(); int nrow = idxbool.size(); int numelem = idxbool.sum(); VectorXi idx(numelem); int c = 0; for (int i = 0; i < nrow; ++i) { if (idxbool(i) == 1) { idx(c) = i; ++c; } } int nvars = A.cols() - 1; MatrixXd sub(numelem, n); for (int r = 0; r < numelem; ++r) { sub.row(r) = A.row(idx(r)); } MatrixXd AtAtmp(MatrixXd(n, n).setZero(). selfadjointView<Lower>().rankUpdate(sub.adjoint() )); xtxlist[k-1] = AtAtmp; } return wrap(xtxlist); } catch (std::exception &ex) { forward_exception_to_r(ex); } catch (...) { ::Rf_error("C++ exception (unknown reason)"); } return R_NilValue; //-Wall }