bool Localizer::setIntrinsicsMatrix(const string &type, const Matrix &M)
{
if (type=="left")
{
if (PrjL!=NULL)
{
*PrjL=M;
*invPrjL=pinv(M.transposed()).transposed();
}
else
{
PrjL=new Matrix(M);
invPrjL=new Matrix(pinv(M.transposed()).transposed());
}
return true;
}
else if (type=="right")
{
if (PrjR!=NULL)
{
*PrjR=M;
*invPrjR=pinv(M.transposed()).transposed();
}
else
{
PrjR=new Matrix(M);
invPrjR=new Matrix(pinv(M.transposed()).transposed());
}
return true;
}
else
return false;
}
Vector Controller::computeNeckVelFromdxFP(const Vector &fp, const Vector &dfp)
{
// convert fp from root to the neck reference frame
Vector fpR=fp;
fpR.push_back(1.0);
Vector fpE=SE3inv(chainNeck->getH())*fpR;
// compute the Jacobian of the head joints alone
// (by adding the new fixation point beforehand)
Matrix HN=eye(4);
HN(0,3)=fpE[0];
HN(1,3)=fpE[1];
HN(2,3)=fpE[2];
mutexChain.lock();
chainNeck->setHN(HN);
Matrix JN=chainNeck->GeoJacobian();
chainNeck->setHN(eye(4,4));
mutexChain.unlock();
// take only the last three rows of the Jacobian
// belonging to the head joints
Matrix JNp=JN.submatrix(3,5,3,5);
return pinv(JNp)*dfp.subVector(3,5);
}
Vector EyePinvRefGen::getEyesCounterVelocity(const Matrix &eyesJ, const Vector &fp)
{
// ********** implement VOR
Matrix H=imu->getH(cat(fbTorso,fbHead.subVector(0,2)));
H(0,3)=fp[0]-H(0,3);
H(1,3)=fp[1]-H(1,3);
H(2,3)=fp[2]-H(2,3);
// gyro rate [rad/s]
Vector gyro=CTRL_DEG2RAD*commData->get_imu().subVector(6,8);
// filter out the noise on the gyro readouts
if (norm(gyro)<commData->gyro_noise_threshold)
gyro=0.0;
Vector vor_fprelv=(gyro[0]*cross(H,0,H,3)+
gyro[1]*cross(H,1,H,3)+
gyro[2]*cross(H,2,H,3));
// ********** implement OCR
H=chainNeck->getH();
Matrix HN=eye(4,4);
HN(0,3)=fp[0]-H(0,3);
HN(1,3)=fp[1]-H(1,3);
HN(2,3)=fp[2]-H(2,3);
chainNeck->setHN(HN);
Vector ocr_fprelv=chainNeck->GeoJacobian()*commData->get_v().subVector(0,2);
ocr_fprelv=ocr_fprelv.subVector(0,2);
chainNeck->setHN(eye(4,4));
// ********** blend the contributions
return -1.0*(pinv(eyesJ)*(counterRotGain[0]*vor_fprelv+counterRotGain[1]*ocr_fprelv));
}
// Algorithms -------------------------------------------------------------------------------------------------------------------------------------
Eigen::VectorXd controlEnv::mobile_manip_inverse_kinematics_simple(void){
return pinv(jacobian(mobile_manip_), 0.001)*scaled_pose_error_.v();
// scaled_pose_error_.plot();
// std::cout << "scaled err: " << scaled_pose_error_.v().transpose() << std::endl;
// std::cout << "jac: " << std::endl << jacobian(mobile_manip_) << std::endl;
// std::cout << "pinv jac: " << std::endl << pinv(jacobian(mobile_manip_), 0.001) << std::endl;
}
Vector Controller::computeEyesVelFromdxFP(const Vector &dfp)
{
Matrix eyesJ; Vector tmp;
if ((fbEyes[2]>CTRL_DEG2RAD*GAZECTRL_CRITICVER_STABILIZATION) &&
CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,tmp,eyesJ))
return pinv(eyesJ)*dfp.subVector(0,2);
else
return zeros(vEyes.length());
}
GURLS_EXPORT void pinv(const gMat2D<float>& A, gMat2D<float>& Ainv, float RCOND)
{
int r, c;
float* inv = pinv(A.getData(), A.rows(), A.cols(), r, c, &RCOND);
Ainv.resize(r, c);
gurls::copy(Ainv.getData(), inv, r*c);
delete[] inv;
}
/*********************************************************//**
Allocate and initialize skeleton decomposition
with a set of pivots and a given approximation
\param[in] f - function to approximate
\param[in] args - function arguments
\param[in] bounds - bounds on function
\param[in] cargs - cross2d args
\param[in] pivx - x pivots
\param[in] pivy - y pivots
\return skeleton decomposition
*************************************************************/
struct SkeletonDecomp *
skeleton_decomp_init2d_from_pivots(
double (*f)(double,double,void *),
void * args, const struct BoundingBox * bounds,
const struct Cross2dargs * cargs,
const double * pivx, const double * pivy)
{
size_t r = cargs->r;
struct SkeletonDecomp * skd = skeleton_decomp_alloc(r);
struct FiberCut ** fx;
struct FiberCut ** fy;
double * lb = bounding_box_get_lb(bounds);
double * ub = bounding_box_get_ub(bounds);
fx = fiber_cut_2darray(f,args,0,r, pivy);
quasimatrix_free(skd->xqm);
skd->xqm = quasimatrix_approx_from_fiber_cuts(
r, fiber_cut_eval2d, fx, cargs->fclass[0],
cargs->sub_type[0],
lb[0],ub[0], cargs->approx_args[0]);
fiber_cut_array_free(r, fx);
fy = fiber_cut_2darray(f,args,1,r, pivx);
quasimatrix_free(skd->yqm);
skd->yqm = quasimatrix_approx_from_fiber_cuts(
r, fiber_cut_eval2d, fy, cargs->fclass[1],
cargs->sub_type[1],
lb[1],ub[1], cargs->approx_args[1]);
fiber_cut_array_free(r, fy);
size_t ii,jj;
double * cmat = calloc_double(r*r);
for (ii = 0; ii <r; ii++){
for (jj = 0; jj <r; jj++){
cmat[ii * r + jj] = f(pivx[jj],pivy[ii], args);
}
}
/*
printf("cmat = ");
dprint2d_col(r,r,cmat);
*/
pinv(r,r,r,cmat,skd->skeleton,1e-15);
free(cmat);cmat=NULL;
return skd;
}
Eigen::VectorXd controlEnv::manipulator_inverse_kinematics_simple(void){
unsigned int n_manip_joints = mobile_manip_.n_manip_joints();
unsigned int n_joints = mobile_manip_.dim();
VectorXd manip_joint_inc(n_manip_joints);
manip_joint_inc = pinv(jacobian(mobile_manip_), 0.001)*scaled_pose_error_.v();
VectorXd joint_inc(n_joints);
for (unsigned int i=0; i<n_joints-n_manip_joints; i++) joint_inc(i) = 0.0;
for (unsigned int i=0; i<n_manip_joints; i++) joint_inc(i+n_joints-n_manip_joints) = manip_joint_inc(i);
return joint_inc;
}
/**
* The update function that actually updates the H matrix. The function takes
* in all the matrices and only changes the value of the H matrix.
*
* @param V Input matrix to be factorized.
* @param W Basis matrix.
* @param H Encoding matrix to be updated.
*/
inline static void Update(const arma::mat& V,
const arma::mat& W,
arma::mat& H)
{
H = pinv(W.t() * W) * W.t() * V;
// Set all negative numbers to 0.
for (size_t i = 0; i < H.n_elem; i++)
{
if (H(i) < 0.0)
{
H(i) = 0.0;
}
}
}
/* Test matrix invertion routine */
void testPinv(void)
{
double **A,val;
int nRows,nCols, i,j, n;
nRows=3;
nCols=4;
n = MAX(nRows,nCols);
printf("n=%d\n",n);
/* Allocate matrix */
A = (double **) calloc(n,sizeof(double *));
for (i=0;i<n;i++){
A[i] = (double *) calloc(n,sizeof(double));
}
/* Set up test matrix values */
val = 1.0;
for (j=0;j<nCols;j++){
if (j<nRows) A[j][j] = 2.0;
for (i=0;i<nRows;i++){
A[i][j]+=val;
val += 1.0;
}
}
/* Display matrix */
for (i=0;i<nRows;i++){
for (j=0;j<nCols;j++){
printf(" %16.8f",A[i][j]);
}
printf("\n");
}
printf("\n");
/* Invert matrix */
pinv(A,nRows,nCols);
/* Display matrix */
for (i=0;i<nCols;i++){
for (j=0;j<nRows;j++){
printf(" %16.8f",A[i][j]);
}
printf("\n");
}
printf("testPinv halting program\n");
_EXIT_;
}
/**
* The update function that actually updates the W matrix. The function takes
* in all the matrices and only changes the value of the W matrix.
*
* @param V Input matrix to be factorized.
* @param W Basis matrix to be updated.
* @param H Encoding matrix.
*/
inline static void Update(const arma::mat& V,
arma::mat& W,
const arma::mat& H)
{
// The call to inv() sometimes fails; so we are using the psuedoinverse.
// W = (inv(H * H.t()) * H * V.t()).t();
W = V * H.t() * pinv(H * H.t());
// Set all negative numbers to machine epsilon
for (size_t i = 0; i < W.n_elem; i++)
{
if (W(i) < 0.0)
{
W(i) = 0.0;
}
}
}
void fingerDetector::run()
{
//fprintf(stderr, "Entering the main thread\n");
//fprintf(stderr, "Reading: %s\n", analogs.toString().c_str());
int n = q1.size();
Vector q(n);
Vector qStar(n);
Vector dq(n);
Vector tStar(1);
Matrix Q1(n,1);
for (int i = 0; i < n ; i++)
{
Q1(i,0) = q1(i);
q(i) = fingerAnalog(i);
}
tStar = pinv(Q1)*(q-q0);
qStar = q0 + q1 * tStar(0);
dq = q - qStar;
double res = 0;
double q1N = 0;
for (int i = 0; i < n ; i++)
{
res += sqrt(dq(i)*dq(i));
q1N += sqrt(q1(i)*q1(i));
}
if (tStar(0) > maxT && res < max)
status = (tStar(0) - maxT) * q1N;
if (tStar(0) > maxT && res > max)
status = (tStar(0) - maxT) * q1N + (res - max);
if (tStar(0) < minT && res < max)
status = (minT - tStar(0)) * q1N;
if (tStar(0) < minT && res > max)
status = (minT - tStar(0)) * q1N + (res - max);
if ( tStar(0) > minT && tStar(0) < maxT && res < max)
status = 0.0;
if ( tStar(0) > minT && tStar(0) < maxT && res > max)
status = res - max;
}
// get the covariance of a contrast given a contrast vector
// if this matrix is X, this function computes c' pinv(X'X) c
double RtDesignMatrix::computeContrastCovariance(
vnl_vector<double> &contrastVector) {
if (contrastVector.size() != columns()) {
cerr << "ERROR: number of elements in contrast vector does not match the "
<< "number of columns in the design matrix" << endl;
return std::numeric_limits<double>::quiet_NaN();
}
// compute the contrast covariance based on the currently known regressors
// NOTE: this will not be the same as computing it at the end of the
// experiment when all regressors are known. it would be nice to compute
// final values using the known design.
vnl_matrix<double> convec(contrastVector.data_block(),
contrastVector.size(), 1);
vnl_svd<double> pinv(transpose() * (*this));
vnl_matrix<double> result = convec.transpose() * pinv.pinverse() * convec;
return result.get(0, 0);
}
double * solve(size_t nfield, double * perm, double h, double lb, double ub)
{
double * op = buildOP(nfield,perm,h);
double * rhs = buildRHS(nfield-1,lb+h,ub);
double * inv = calloc_double((nfield-1) * (nfield-1));
pinv(nfield-1,nfield-1,nfield-1,op,inv,0.0);
double * sol = calloc_double(nfield);
cblas_dgemv(CblasColMajor,CblasNoTrans,nfield-1,nfield-1,1.0,inv,nfield-1,
rhs,1,0.0,sol+1,1);
free(op); op = NULL;
free(rhs); rhs = NULL;
free(inv); inv = NULL;
return sol;
}
vec LinkedTreeRobot::computeDeltaThetas(const vec& desiredPositions, vec& axes) const {
assert(_numEffectors * 3 == desiredPositions.n_elem);
vec s(3 * _numEffectors);
getEffectorPositions(s);
vec e = clamp(desiredPositions - s, 0.5);
mat J;
computeJacobian(desiredPositions, J, axes);
if (_method == PINV) {
//cout << "PINV" << endl;
mat pInvJ = pinv(J); // magic. uses SVD
vec soln = pInvJ * e;
assert(soln.n_elem == _numJoints);
return soln;
} else if (_method == DLS) {
//cout << "DLS" << endl;
// delta theta = J^T(JJ^T + lambda^2I)^-1 * e
mat JT = trans(J);
mat JJT = J * JT;
mat I(J.n_rows, J.n_rows);
I.eye();
vec soln = (JT * inv(JJT + lambda_squared * I)) * e;
assert(soln.n_elem == _numJoints);
return soln;
} else
throw runtime_error("invalid method");
}
bool Localizer::triangulatePoint(const Vector &pxl, const Vector &pxr, Vector &x)
{
if ((pxl.length()<2) || (pxr.length()<2))
{
yError("Not enough values given for the pixels!");
return false;
}
if (PrjL && PrjR)
{
Vector torso=commData->get_torso();
Vector head=commData->get_q();
Vector qL(8);
qL[0]=torso[0];
qL[1]=torso[1];
qL[2]=torso[2];
qL[3]=head[0];
qL[4]=head[1];
qL[5]=head[2];
qL[6]=head[3];
qL[7]=head[4]+head[5]/2.0;
Vector qR=qL;
qR[7]-=head[5];
mutex.lock();
Matrix HL=SE3inv(eyeL->getH(qL));
Matrix HR=SE3inv(eyeR->getH(qR));
mutex.unlock();
Matrix tmp=zeros(3,4); tmp(2,2)=1.0;
tmp(0,2)=pxl[0]; tmp(1,2)=pxl[1];
Matrix AL=(*PrjL-tmp)*HL;
tmp(0,2)=pxr[0]; tmp(1,2)=pxr[1];
Matrix AR=(*PrjR-tmp)*HR;
Matrix A(4,3);
Vector b(4);
for (int i=0; i<2; i++)
{
b[i]=-AL(i,3);
b[i+2]=-AR(i,3);
for (int j=0; j<3; j++)
{
A(i,j)=AL(i,j);
A(i+2,j)=AR(i,j);
}
}
// solve the least-squares problem
x=pinv(A)*b;
return true;
}
else
{
yError("Unspecified projection matrix for at least one camera!");
return false;
}
}
/******************************************************************
* 函数功能：几何校正
*
* 待写：H_final的值也应该返回去
*/
arma::uvec geometricVerification(const arma::mat &frames1, const arma::mat &frames2,
const arma::mat &matches, const superluOpts &opts){
// 测试载入是否准确
/*std::cout<< "element测试: " << " x: " << frames1(0,1) << " y: " << frames1(1,1) << std::endl;
std::cout << " 行数： " << frames1.n_rows << " 列数：" << frames1.n_cols << std::endl;
std::cout << "==========================================================" << std::endl;*/
int numMatches = matches.n_cols;
// 测试匹配数目是否准确
/*std::cout << "没有RANSAC前匹配数目： " << numMatches << std::endl;
std::cout << "==========================================================" << std::endl;*/
arma::field<arma::uvec> inliers(1, numMatches);
arma::field<arma::mat> H(1, numMatches);
arma::uvec v = arma::linspace<arma::uvec>(0,1,2);
arma::mat onesVector = arma::ones(1, matches.n_cols);
arma::uvec matchedIndex_Query = arma::conv_to<arma::uvec>::from(matches.row(0)-1);
arma::uvec matchedIndex_Object = arma::conv_to<arma::uvec>::from(matches.row(1)-1);
arma::mat x1 = frames1(v, matchedIndex_Query) ;
arma::mat x2 = frames2(v, matchedIndex_Object);
/*std::cout << " x1查询图像匹配行数： " << x1.n_rows << " 查询图像匹配列数：" << x1.n_cols << std::endl;
std::cout << " x2目标图像匹配行数： " << x2.n_rows << " 目标图像匹配列数：" << x2.n_cols << std::endl;
std::cout<< "x1 element测试: " << " x: " << x1(0,1) << " y: " << x1(1,1) << std::endl;
std::cout<< "x2 element测试: " << " x: " << x2(0,1) << " y: " << x2(1,1) << std::endl;
std::cout << "==========================================================" << std::endl;*/
arma::mat x1hom = arma::join_cols(x1, arma::ones<arma::mat>(1, numMatches)); //在下面添加一行，注意和join_rows的区别
arma::mat x2hom = arma::join_cols(x2, arma::ones<arma::mat>(1, numMatches));
/*std::cout << " x1hom查询图像匹配行数： " << x1hom.n_rows << " 查询图像匹配列数：" << x1hom.n_cols << std::endl;
std::cout<< "x1hom element测试: " << " x: " << x1hom(0,1) << " y: " << x1hom(1,1) << " z: " << x1hom(2,1) << std::endl;
std::cout << "==========================================================" << std::endl;*/
arma::mat x1p, H21; //作用域
double tol;
for(int m = 0; m < numMatches; ++m){
//cout << "m: " << m << endl;
for(unsigned int t = 0; t < opts.numRefinementIterations; ++t){
//cout << "t: " << t << endl;
if (t == 0){
arma::mat tmp1 = frames1.col(matches(0, m)-1);
arma::mat A1 = toAffinity(tmp1);
//A1.print("A1 =");
arma::mat tmp2 = frames2.col(matches(1, m)-1);
arma::mat A2 = toAffinity(tmp2);
//A2.print("A2 =");
H21 = A2 * inv(A1);
//H21.print("H21 =");
x1p = H21.rows(0, 1) * x1hom ;
//x1p.print("x1p =");
tol = opts.tolerance1;
}else if(t !=0 && t <= 3){
arma::mat A1 = x1hom.cols(inliers(0, m));
arma::mat A2 = x2.cols(inliers(0, m));
//A1.print("A1 =");
//A2.print("A2 =");
H21 = A2*pinv(A1);
//H21.print("H21 =");
x1p = H21.rows(0, 1) * x1hom ;
//x1p.print("x1p =");
arma::mat v;
v << 0 << 0 << 1 << arma::endr;
H21 = join_vert(H21, v);
//H21.print("H21 =");
//x1p.print("x1p =");
tol = opts.tolerance2;
}else{
arma::mat x1in = x1hom.cols(inliers(0, m));
arma::mat x2in = x2hom.cols(inliers(0, m));
arma::mat S1 = centering(x1in);
arma::mat S2 = centering(x2in);
arma::mat x1c = S1 * x1in;
//x1c.print("x1c =");
arma::mat x2c = S2 * x2in;
//x2c.print("x2c =");
arma::mat A1 = arma::randu<arma::mat>(x1c.n_rows ,x1c.n_cols);
A1.zeros();
arma::mat A2 = arma::randu<arma::mat>(x1c.n_rows ,x1c.n_cols);
A2.zeros();
arma::mat A3 = arma::randu<arma::mat>(x1c.n_rows ,x1c.n_cols);
A3.zeros();
for(unsigned int i = 0; i < x1c.n_cols; ++i){
A2.col(i) = x1c.col(i)*(-x2c.row(0).col(i));
A3.col(i) = x1c.col(i)*(-x2c.row(1).col(i));
}
arma::mat T1 = join_cols(join_horiz(x1c, A1), join_horiz(A1, x1c));
arma::mat T2 = join_cols(T1, join_horiz(A2, A3));
//T2.print("T2 =");
arma::mat U;
arma::vec s;
arma::mat V;
svd_econ(U, s, V, T2);
//U.print("U =");
//V.print("V =");
arma::vec tmm = U.col(U.n_cols-1);
H21 = reshape(tmm, 3, 3).t();
H21 = inv(S2) * H21 * S1;
H21 = H21 / H21(H21.n_rows-1, H21.n_cols-1) ;
//H21.print("H21 =");
arma::mat x1phom = H21 * x1hom ;
arma::mat cc1 = x1phom.row(0) / x1phom.row(2);
arma::mat cc2 = x1phom.row(1) / x1phom.row(2);
arma::mat x1p = join_cols(cc1, cc2);
//x1p.print("x1p =");
tol = opts.tolerance3;
}
arma::mat tmp = arma::square(x2 - x1p); //精度跟matlab相比更高？
//tmp.print("tmp =");
arma::mat dist2 = tmp.row(0) + tmp.row(1);
//dist2.print("dist2 =");
inliers(0, m) = arma::find(dist2 < pow(tol, 2));
H(0, m) = H21;
//H(0, m).print("H(0, m) =");
//inliers(0, m).print("inliers(0, m) =");
//cout << inliers(0, m).size() << endl;
//cout << "==========================================================" << endl;
if (inliers(0, m).size() < opts.minInliers) break;
if (inliers(0, m).size() > 0.7 * numMatches) break;
}
}
arma::uvec scores(numMatches);
for(int i = 0; i < numMatches; ++i){
scores.at(i) = inliers(0, i).n_rows;
}
//scores.print("scores = ");
arma::uword index;
scores.max(index);
//cout << index << endl;
arma::mat H_final = inv(H(0, index));
//H_final.print("H_final = ");
arma::uvec inliers_final = inliers(0, index);
//inliers_final.print("inliers_final = ");
return inliers_final;
}
Datum
pseudoinverse(PG_FUNCTION_ARGS)
{
/*
* A note on types: PostgreSQL array dimensions are of type int. See, e.g.,
* the macro definition ARR_DIMS
*/
int rows, columns;
float8 *A, *Aplus;
ArrayType *A_PG, *Aplus_PG;
int lbs[2], dims[2];
/*
* Perform all the error checking needed to ensure that no one is
* trying to call this in some sort of crazy way.
*/
if (PG_NARGS() != 1)
{
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("pseudoinverse called with %d arguments",
PG_NARGS())));
}
if (PG_ARGISNULL(0))
PG_RETURN_NULL();
A_PG = PG_GETARG_ARRAYTYPE_P(0);
if (ARR_ELEMTYPE(A_PG) != FLOAT8OID)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("pseudoinverse only defined over float8[]")));
if (ARR_NDIM(A_PG) != 2)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("pseudoinverse only defined over 2 dimensional arrays"))
);
if (ARR_NULLBITMAP(A_PG))
ereport(ERROR,
(errcode(ERRCODE_NULL_VALUE_NOT_ALLOWED),
errmsg("null array element not allowed in this context")));
/* Extract rows, columns, and data */
rows = ARR_DIMS(A_PG)[0];
columns = ARR_DIMS(A_PG)[1];
A = (float8 *) ARR_DATA_PTR(A_PG);
/* Allocate a PG array for the result, "Aplus" is A+, the pseudo inverse of A */
lbs[0] = 1;
lbs[1] = 1;
dims[0] = columns;
dims[1] = rows;
Aplus_PG = construct_md_array(NULL, NULL, 2, dims, lbs, FLOAT8OID,
sizeof(float8), true, 'd');
Aplus = (float8 *) ARR_DATA_PTR(Aplus_PG);
pinv(rows,columns,A,Aplus);
PG_RETURN_ARRAYTYPE_P(Aplus_PG);
}
/*----------
* Do the computations requested from final functions.
*
* Compute regression coefficients, coefficient of determination (R^2),
* t-statistics, and p-values whenever the respective argument is non-NULL.
* Since these functions share a lot of computation, they have been distilled
* into this function.
*
* First, we compute the regression coefficients as:
*
* c = (X^T X)^+ * X^T * y = X^+ * y
*
* where:
*
* X^T = the transpose of X
* X^+ = the pseudo-inverse of X
*
* The identity X^+ = (X^T X)^+ X^T holds for all matrices X, a proof
* can be found here:
* http://en.wikipedia.org/wiki/Proofs_involving_the_Moore%2DPenrose_pseudoinverse
*
* Note that when the system X c = y is satisfiable (because (X|c) has rank at
* most inState->len), then setting c = X^+ y means that |c|_2 <= |d|_2 for all
* solutions d satisfying X d = y.
* (See http://en.wikipedia.org/wiki/Moore%2DPenrose_pseudoinverse)
*
* Caveat: Explicitly computing (X^T X)^+ can become a significant source of
* numerical rounding erros (see, e.g.,
* http://en.wikipedia.org/wiki/Moore%2DPenrose_pseudoinverse#Construction
* or http://www.mathworks.com/moler/leastsquares.pdf p.16).
*----------
*/
static void
float8_mregr_compute(MRegrState *inState,
ArrayType **outCoef,
float8 *outR2,
ArrayType **outTStats,
ArrayType **outPValues)
{
ArrayType *coef_array, *stdErr_array, *tStats_array = NULL, *pValues_array = NULL;
float8 ess = 0., /* explained sum of squares (regression sum of squares) */
tss = 0., /* total sum of squares */
rss, /* residual sum of squares */
r2,
variance;
float8 *XtX_inv, *coef, *stdErr, *tStats = NULL, *pValues = NULL;
uint32 i;
/*
* Precondition: inState->len * inState->len * sizeof(float8) < STATE_LEN(inState->len)
* and IS_FEASIBLE_STATE_LEN(STATE_LEN(inState->len))
*/
XtX_inv = palloc((uint64) inState->len * inState->len * sizeof(float8));
pinv(inState->len, inState->len, inState->XtX, XtX_inv);
/*
* FIXME: Decide on whether we want to display an info message or rather
* provide a second function that tells how well the data is conditioned.
*
* Check if we should expect multicollineratiy. [MPP-13582]
*
* See:
* Lichtblau, Daniel and Weisstein, Eric W. "Condition Number."
* From MathWorld--A Wolfram Web Resource.
* http://mathworld.wolfram.com/ConditionNumber.html
*
if (condNoOfXtX > 1000)
ereport(INFO,
(errmsg("matrix X^T X is ill-conditioned"),
errdetail("condition number = %f", condNoOfXtX),
errhint("This can indicate strong multicollinearity.")));
*/
/*
* We want to return a one-dimensional array (as opposed to a
* two-dimensional array).
*
* Note: Calling construct_array with NULL as first arguments is a Greenplum
* extension
*/
coef_array = construct_array(NULL, inState->len,
FLOAT8OID, sizeof(float8), true, 'd');
coef = (float8 *) ARR_DATA_PTR(coef_array);
symmetricMatrixTimesVector(inState->len, XtX_inv, inState->Xty, coef);
if (outCoef)
*outCoef = coef_array;
if (outR2 || outTStats || outPValues)
{
/*----------
* Computing the total sum of squares (tss) and the explained sum of squares (ess)
*
* ess = y'X * c - sum(y)^2/n
* tss = sum(y^2) - sum(y)^2/n
* R^2 = ess/tss
*----------
*/
ess = dotProduct(inState->len, inState->Xty, coef)
- inState->sumy*inState->sumy/inState->count;
tss = inState->sumy2 - inState->sumy * inState->sumy / inState->count;
/*
* With infinite precision, the following checks are pointless. But due to
* floating-point arithmetic, this need not hold at this point.
* Without a formal proof convincing us of the contrary, we should
* anticipate that numerical peculiarities might occur.
*/
if (tss < 0)
tss = 0;
if (ess < 0)
ess = 0;
/*
* Since we know tss with greater accuracy than ess, we do the following
* sanity adjustment to ess:
*/
if (ess > tss)
ess = tss;
}
if (outR2)
{
/*
* coefficient of determination
* If tss == 0, then the regression perfectly fits the data, so the
* coefficient of determination is 1.
*/
r2 = (tss == 0 ? 1 : ess / tss);
*outR2 = r2;
}
if (outTStats || outPValues)
{
stdErr_array = construct_array(NULL, inState->len,
FLOAT8OID, sizeof(float8), true, 'd');
stdErr = (float8 *) ARR_DATA_PTR(stdErr_array);
tStats_array = construct_array(NULL, inState->len,
FLOAT8OID, sizeof(float8), true, 'd');
tStats = (float8 *) ARR_DATA_PTR(tStats_array);
pValues_array = construct_array(NULL, inState->len,
FLOAT8OID, sizeof(float8), true, 'd');
pValues = (float8 *) ARR_DATA_PTR(pValues_array);
/*
* Computing t-statistics and p-values
*
* Total sum of squares (tss) = Residual Sum of sqaures (rss) +
* Explained Sum of Squares (ess) for linear regression.
* Proof: http://en.wikipedia.org/wiki/Sum_of_squares
*/
rss = tss - ess;
/* Variance is also called the Mean Square Error */
variance = rss / (inState->count - inState->len);
/*
* The t-statistic for each coef[i] is coef[i] / stdErr[i]
* where stdErr[i] is the standard error of coef[ii], i.e.,
* the square root of the i'th diagonoal element of
* variance * (X^T X)^{-1}.
*/
for (i = 0; i < inState->len; i++) {
/*
* In an abundance of caution, we see a tiny possibility that numerical
* instabilities in the pinv operation can lead to negative values on
* the main diagonal of even a SPD matrix
*/
if (XtX_inv[i * (inState->len + 1)] < 0) {
stdErr[i] = 0;
} else {
stdErr[i] = sqrt( variance * XtX_inv[i * (inState->len + 1)] );
}
if (coef[i] == 0 && stdErr[i] == 0) {
/*
* In this special case, 0/0 should be interpreted as 0:
* We know that 0 is the exact value for the coefficient, so
* the t-value should be 0 (corresponding to a p-value of 1)
*/
tStats[i] = 0;
} else {
/*
* If stdErr[i] == 0 then abs(tStats[i]) will be infinity, which is
* what we need.
*/
tStats[i] = coef[i] / stdErr[i];
}
}
}
if (outTStats)
*outTStats = tStats_array;
if (outPValues) {
for (i = 0; i < inState->len; i++)
if (inState->count <= inState->len) {
/*
* This test is purely for detecting long int overflows because
* studentT_cdf expects an unsigned long int as first argument.
*/
pValues[i] = NAN;
} else {
pValues[i] = 2. * (1. - studentT_cdf(
(uint64) (inState->count - inState->len), fabs( tStats[i] )
));
}
*outPValues = pValues_array;
}
}
Eigen::MatrixXd cinv(const Eigen::MatrixXd M, const Eigen::VectorXi TS, const Eigen::VectorXd H, const double tol){
// ******************************************************************************************************
// Function that implements the continuous inverse as in:
// Mansard, N., et al.; A Unified Approach to Integrate Unilateral Constraints in the Stack of Tasks
// IEEE, Transactions on Robotics, 2009
//
// INPUT
// - M: matrix from which we want to compute the continuous inverse
// - TS: task size vector with 'ts(i)' being the size of task 'i'. The sum of the elements of 'TS' == number of rows of 'M'.
// - H: task activation vector whose elements \in [0,1]
//
// OUTPUT
// Given B(m) the set of all permutations without repetitions of the first 'm' elements,
// that is, B(m==3) = {{1},{2},{3},{1,2},{1,3},{2,3},{1,2,3}} then:
// - cinv = sum_{P \in B(m)}( prod_{i \in P}(h_i) * prod_{i !\in P}(1.0 - h_i) * pinv(M_P) )
// with:
// - m = TS.size()
// - M_P = H0_P * M and H0_P as the diagonal matrix with HO_P(j,j) = 1 if j \in P, otherwise HO_P(j,j) = 0.
// std::cout << "M" << std::endl << M << std::endl;
// std::cout << "TS: " << TS.transpose() << std::endl;
// std::cout << "H: " << H.transpose() << std::endl;
unsigned int m_rows = M.rows();
unsigned int m_cols = M.cols();
// Gather task data
unsigned int n_tasks = TS.size();
std::vector< std::pair<unsigned int, unsigned int> > task_rows;
std::vector<Eigen::MatrixXd> task_M;
for (unsigned int i=0, curr_row=0; i<n_tasks; ++i){
task_rows.push_back( std::pair<unsigned int, unsigned int>(curr_row, curr_row+static_cast<unsigned int>(TS(i))) );
task_M.push_back(M.block(curr_row,0,TS(i),m_cols));
curr_row += static_cast<unsigned int>(TS(i));
}
// // Plot data by task
// std::cout << "curr rows" << std::endl;
// for (auto i=0; i<n_tasks; ++i){
// std::cout << task_rows[i].first << " " << task_rows[i].second << " - " << H[i] << std::endl;
// std::cout << "task_M_pinv[i]:" << std::endl << task_M[i] << std::endl;
// }
// Get number of active tasks
std::vector<unsigned int> active_tasks;
for (unsigned int i=0; i<n_tasks; ++i) if (H[i]) active_tasks.push_back(i);
unsigned int n_active_tasks = active_tasks.size();
// std::cout << "active_tasks: "; for (auto i=0; i<active_tasks.size(); ++i) std::cout << active_tasks[i] << " "; std::cout << std::endl;
Eigen::MatrixXd cinv(Eigen::MatrixXd::Zero(m_cols,m_rows));
for (unsigned int n_permutation_elem = 1; n_permutation_elem <= n_active_tasks; ++n_permutation_elem){
std::vector<bool> active_tasks_in_permutation(n_active_tasks);
std::fill(active_tasks_in_permutation.begin(), active_tasks_in_permutation.begin() + n_permutation_elem, true);
do {
Eigen::MatrixXd subset_task_m(Eigen::MatrixXd::Zero(m_rows,m_cols));
double h_subset_task_activation_prod = 1.0;
for (unsigned int j_task = 0; j_task < n_active_tasks; ++j_task) {
if (active_tasks_in_permutation[j_task]) {
subset_task_m.block(task_rows[active_tasks[j_task]].first,0,TS(active_tasks[j_task]),m_cols) = task_M[active_tasks[j_task]];
h_subset_task_activation_prod *= H(active_tasks[j_task]);
}
else
h_subset_task_activation_prod *= 1.0 - H(active_tasks[j_task]);
}
// std::cout << "permutation: "; for (auto k=0; k<active_tasks_in_permutation.size(); ++k) std::cout<< active_tasks_in_permutation[k] << " "; std::cout << std::endl;
// std::cout << "h_subset_task_activation_prod: " << h_subset_task_activation_prod << std::endl;
// std::cout << "subset_task_m" << std::endl << subset_task_m << std::endl;
cinv = cinv + h_subset_task_activation_prod * pinv(subset_task_m,tol);
} while (std::prev_permutation(active_tasks_in_permutation.begin(), active_tasks_in_permutation.end()));
}
return cinv;
}
int main(void){
std::cout << " ---- Programa de Tests de la pseudoinverse de Mansard ----" << std::endl;
unsigned int nq = 10;
unsigned int nx1 = 7;
Eigen::MatrixXd H1(Eigen::MatrixXd::Zero(nx1,nx1));
H1(0,0) = 0;
H1(1,1) = 0.2;
H1(2,2) = 0;
H1(3,3) = 0.5;
H1(4,4) = 0;
H1(5,5) = 0;
H1(6,6) = 0.7;
std::vector<unsigned int> active_joints;
for (unsigned int i=0; i<nx1; ++i) if (H1(i,i)) active_joints.push_back(i);
unsigned int n_active_joints = active_joints.size();
std::cout << "n_active_joints: " << n_active_joints << std::endl;
// Initialize pseudoinverse to zero
Eigen::MatrixXd pJ1(Eigen::MatrixXd::Zero(nq,nx1));
// // Per cada grup de 'i' elements
// for (unsigned int i=1; i<=n_active_joints; ++i){
// std::vector<bool> v(n_active_joints);
// std::fill(v.begin(), v.begin() + i, true);
// do {
// Eigen::MatrixXd J_sum(Eigen::MatrixXd::Zero(nx1,nq));
// double h_prod = 1.0;
// for (int j = 0; j < n_active_joints; ++j) {
// unsigned int qi = active_joints[j];
// if (v[j]) {
//// std::cout << j << " ";
// std::cout << qi << " ";
// J_sum(qi,qi+3) = 1.0;
// h_prod *= H1(qi,qi);
// }
// else h_prod *= 1.0 - H1(qi,qi);
// }
// std::cout << "\n";
//// std::cout << " - P h: " << h_prod << "\n";
// std::cout << " J_sum: " << "\n" << J_sum << "\n";
// Eigen::MatrixXd pJ_sum(pinv(J_sum,0.00001));
// std::cout << " h_prod * pJ_sum: " << "\n" << h_prod * pJ_sum << "\n";
// pJ1 = pJ1 + h_prod * pJ_sum;
// std::cout << " pJ1: " << "\n" << pJ1 << "\n";
// } while (std::prev_permutation(v.begin(), v.end()));
// std::cout << "-----\n";
// }
// std::cout << " pJ1: " << "\n" << pJ1 << "\n";
// Per cada grup de 'i' elements
pJ1 = Eigen::MatrixXd::Zero(nq,nx1);
for (unsigned int i=1; i<=n_active_joints; ++i){
std::vector<bool> v(n_active_joints);
std::fill(v.begin(), v.begin() + i, true);
do {
Eigen::MatrixXd J_sum(Eigen::MatrixXd::Zero(nx1,nq));
double h_prod = 1.0;
for (unsigned int j = 0; j < n_active_joints; ++j) {
unsigned int qi = active_joints[j];
if (v[j]) {
J_sum(qi,qi+3) = 1.0;
h_prod *= H1(qi,qi);
}
else
h_prod *= 1.0 - H1(qi,qi);
}
pJ1 = pJ1 + h_prod * pinv(J_sum,0.00001);
} while (std::prev_permutation(v.begin(), v.end()));
}
std::cout << " pJ1: " << "\n" << pJ1 << "\n";
// Eigen::MatrixXd m_id(Eigen::MatrixXd::Identity(6,6));
// m_id(1,1) = 0.2;
// m_id(4,4) = -3;
// Eigen::VectorXi TS(3);
// TS(0) = 2;
// TS(1) = 3;
// TS(2) = 1;
// Eigen::VectorXd H(3);
// H(0) = 0.1;
// H(1) = 0.6;
// H(2) = 0.4;
// std::cout << " cinv: " << "\n" << cinv(m_id,TS,H) << "\n";
double tol = 0.0000001;
Eigen::MatrixXd m_id(Eigen::MatrixXd::Identity(nx1,nx1));
Eigen::VectorXi TS(nx1);
for (unsigned int i=0; i<nx1; ++i) TS(i) = 1.0;
Eigen::VectorXd H(nx1);
H(0) = 0;
H(1) = 0.2;
H(2) = 0;
H(3) = 0.5;
H(4) = 0;
H(5) = 0;
H(6) = 0.7;
std::cout << "cinv: " << "\n" << cinv(m_id,TS,H) << "\n";
Eigen::MatrixXd lH(nx1,nx1);
lH(0,0) = 0;
lH(1,1) = 0.2;
lH(2,2) = 0;
lH(3,3) = 0.5;
lH(4,4) = 0;
lH(5,5) = 0;
lH(6,6) = 0.7;
std::cout << "cinv: " << "\n" << cinv(m_id,TS,lH) << "\n";
std::cout << "left cinv: " << "\n" << left_cinv(m_id,TS,lH) << "\n";
std::cout << "right cinv: " << "\n" << right_cinv(m_id,TS,lH) << "\n";
return 0;
}
Localizer::Localizer(exchangeData *_commData, const unsigned int _period) :
RateThread(_period), commData(_commData), period(_period)
{
iCubHeadCenter eyeC(commData->head_version>1.0?"right_v2":"right");
eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
// remove constraints on the links
// we use the chains for logging purpose
eyeL->setAllConstraints(false);
eyeR->setAllConstraints(false);
// release links
eyeL->releaseLink(0); eyeC.releaseLink(0); eyeR->releaseLink(0);
eyeL->releaseLink(1); eyeC.releaseLink(1); eyeR->releaseLink(1);
eyeL->releaseLink(2); eyeC.releaseLink(2); eyeR->releaseLink(2);
// add aligning matrices read from configuration file
getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);
// overwrite aligning matrices iff specified through tweak values
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);
}
// get the absolute reference frame of the head
Vector q(eyeC.getDOF(),0.0);
eyeCAbsFrame=eyeC.getH(q);
// ... and its inverse
invEyeCAbsFrame=SE3inv(eyeCAbsFrame);
// get the length of the half of the eyes baseline
eyesHalfBaseline=0.5*norm(eyeL->EndEffPose().subVector(0,2)-eyeR->EndEffPose().subVector(0,2));
bool ret;
// get camera projection matrix
ret=getCamPrj(commData->rf_cameras,"CAMERA_CALIBRATION_LEFT",&PrjL,true);
if (commData->tweakOverwrite)
{
Matrix *Prj;
if (getCamPrj(commData->rf_tweak,"CAMERA_CALIBRATION_LEFT",&Prj,true))
{
delete PrjL;
PrjL=Prj;
}
}
if (ret)
{
cxl=(*PrjL)(0,2);
cyl=(*PrjL)(1,2);
invPrjL=new Matrix(pinv(PrjL->transposed()).transposed());
}
else
PrjL=invPrjL=NULL;
// get camera projection matrix
ret=getCamPrj(commData->rf_cameras,"CAMERA_CALIBRATION_RIGHT",&PrjR,true);
if (commData->tweakOverwrite)
{
Matrix *Prj;
if (getCamPrj(commData->rf_tweak,"CAMERA_CALIBRATION_RIGHT",&Prj,true))
{
delete PrjR;
PrjR=Prj;
}
}
if (ret)
{
cxr=(*PrjR)(0,2);
cyr=(*PrjR)(1,2);
invPrjR=new Matrix(pinv(PrjR->transposed()).transposed());
}
else
PrjR=invPrjR=NULL;
Vector Kp(1,0.001), Ki(1,0.001), Kd(1,0.0);
Vector Wp(1,1.0), Wi(1,1.0), Wd(1,1.0);
Vector N(1,10.0), Tt(1,1.0);
Matrix satLim(1,2);
satLim(0,0)=0.05;
satLim(0,1)=10.0;
pid=new parallelPID(0.05,Kp,Ki,Kd,Wp,Wi,Wd,N,Tt,satLim);
Vector z0(1,0.5);
pid->reset(z0);
dominantEye="left";
port_xd=NULL;
}
Eigen::VectorXd controlEnv::mobile_manip_inverse_kinematics_siciliano(void){
// Siciliano; modelling, planning and control; pag 139
// --------------------------------------------------------------------------------
// std::cout << "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA: " << n_iteration_ << std::endl;
// Position
VectorXd p_err(3);
for (unsigned int i=0; i<3; i++) p_err(i) = pose_error_.p()(i);
MatrixXd Kp(3,3);
Kp = MatrixXd::Zero(3,3);
Kp(0,0) = 1.0;
Kp(1,1) = 1.0;
Kp(2,2) = 1.0;
VectorXd vd(3);
desired_pose_velocity_ = compute_pose_velocity(desired_pose_, past_desired_pose_, time_increment_);
vd = desired_pose_velocity_.p();
VectorXd v_p(3);
v_p = vd + Kp * p_err;
// Orientation
Quaternion<double> rot_current, rot_desired;
rot_current = mobile_manip_.tcp_pose().o();
rot_desired = desired_pose_.o();
MatrixXd L(3,3);
L = Lmat(rot_current, rot_desired);
MatrixXd Sne(3,3), Sse(3,3), Sae(3,3);
Sne = cross_product_matrix_S(rot_current.toRotationMatrix().col(0));
Sse = cross_product_matrix_S(rot_current.toRotationMatrix().col(1));
Sae = cross_product_matrix_S(rot_current.toRotationMatrix().col(2));
VectorXd nd(3), sd(3), ad(3);
nd = rot_desired.toRotationMatrix().col(0);
sd = rot_desired.toRotationMatrix().col(1);
ad = rot_desired.toRotationMatrix().col(2);
VectorXd o_err(3);
o_err = 0.5 * ( Sne*nd + Sse*sd + Sae*ad );
MatrixXd Ko(3,3);
Ko = MatrixXd::Zero(3,3);
Ko(0,0) = 1.0;
Ko(1,1) = 1.0;
Ko(2,2) = 1.0;
VectorXd wd(3);
wd = desired_pose_velocity_.o_angaxis_r3();
VectorXd v_o(3);
v_o = L.inverse() * (L.transpose() * wd + Ko * o_err );
// Put all together
VectorXd v(6);
for (unsigned int i=0; i<3; i++){
v(i) = v_p(i);
v(i+3) = v_o(i);
}
return pinv(jacobian(mobile_manip_), 0.001)*v;
}
//Class constructor
TimeSegmentation(Tobj &G, Col <T1> map_in, Col <T1> timeVec_in, uword a, uword b, uword c, uword interptype = 1,
uword shots = 1)
{
cout << "Entering Class constructor" << endl;
n1 = a; //Data size
n2 = b;//Image size
L = c; //number of time segments
type = interptype; // type of time segmentation performed
Nshots = shots; // number of shots
obj = &G;
fieldMap = map_in;
cout << "N1 = " << n1 << endl;
cout << "N2 = " << n2 << endl;
cout << "L = " << L << endl;
AA.set_size(n1, L); //time segments weights
timeVec = timeVec_in;
T_min =timeVec.min();
T1 rangt = timeVec.max()-T_min;
tau = (rangt + datum::eps) / (L - 1); // it was L-1 before
timeVec = timeVec-T_min;
uword NOneShot = n1/Nshots;
if (L==1) {
tau = 0;
AA.ones();
}
else {
Mat <CxT1> tempAA(NOneShot, L);
if (type==1) {// Hanning interpolator
cout << "Hanning interpolation" << endl;
for (unsigned int ii = 0; ii<L; ii++) {
for (unsigned int jj = 0; jj<NOneShot; jj++) {
if ((std::abs(timeVec(jj)-((ii)*tau)))<=tau) {
tempAA(jj, ii) = 0.5+0.5*std::cos((datum::pi)*(timeVec(jj)-((ii)*tau))/tau);
}
else {
tempAA(jj, ii) = 0.0;
}
}
}
AA = repmat(tempAA, Nshots, 1);
}
else if (type==2) { // Min-max interpolator: Exact LS interpolator
cout << "Min Max time segmentation" << endl;
Mat <CxT1> Ltp;
Ltp.ones(1, L);
Col <CxT1> ggtp;
ggtp.ones(n2, 1);
Mat <CxT1> gg;
gg = exp(i*fieldMap*tau)*Ltp;
Mat <CxT1> iGTGGT;
iGTGGT.set_size(L+1, n2);
Mat <CxT1> gl;
gl.zeros(n2, L);
for (unsigned int ii = 0; ii<L; ii++) {
for (unsigned int jj = 0; jj<n2; jj++) {
gl(jj, ii) = pow(gg(jj, ii), (T1) (ii+1));
}
}
Mat <CxT1> G;
G.set_size(n2, L);
for (unsigned int jj = 0; jj<L; jj++) {
if (jj==0) {
G.col(jj) = ggtp;
}
else {
G.col(jj) = gl.col(jj-1);
}
}
Col <CxT1> glsum;
Mat <CxT1> GTG;
GTG.zeros(L, L);
GTG.diag(0) += n2;
glsum = sum(gl.t(), 1);
Mat <CxT1> GTGtp(L, L);
for (unsigned int ii = 0; ii < (L - 1); ii++) {
GTGtp.zeros();
GTGtp.diag(-(T1) (ii+1)) += glsum(ii);
GTGtp.diag((T1) (ii+1)) += std::conj(glsum(ii));
GTG = GTG+GTGtp;
}
T1 rcn = 1/cond(GTG);
if (rcn>10*2e-16) { //condition number of GTG
iGTGGT = inv(GTG)*G.t();
}
else {
iGTGGT = pinv(GTG)*G.t(); // pseudo inverse
}
Mat <CxT1> iGTGGTtp;
Mat <CxT1> ftp;
Col <CxT1> res, temp;
for (unsigned int ii = 0; ii<NOneShot; ii++) {
ftp = exp(i*fieldMap*timeVec(ii));
res = iGTGGT*ftp;
tempAA.row(ii) = res.t();
}
AA = repmat(tempAA, Nshots, 1);
}
}
savemat("aamat.mat", "AA", vectorise(AA));
cout << "Exiting class constructor." << endl;
}
static void Diagonalizer_and_CrossCorrelationTable_qdiag (Diagonalizer me, CrossCorrelationTables thee, double *cweights, long maxNumberOfIterations, double delta) {
try {
CrossCorrelationTable c0 = (CrossCorrelationTable) thy item[1];
double **w = my data;
long dimension = c0 -> numberOfColumns;
autoEigen eigen = Thing_new (Eigen);
autoCrossCorrelationTables ccts = Data_copy (thee);
autoNUMmatrix<double> pinv (1, dimension, 1, dimension);
autoNUMmatrix<double> d (1, dimension, 1, dimension);
autoNUMmatrix<double> p (1, dimension, 1, dimension);
autoNUMmatrix<double> m1 (1, dimension, 1, dimension);
autoNUMmatrix<double> wc (1, dimension, 1, dimension);
autoNUMvector<double> wvec (1, dimension);
autoNUMvector<double> wnew (1, dimension);
autoNUMvector<double> mvec (1, dimension);
for (long i = 1; i <= dimension; i++) // Transpose W
for (long j = 1; j <= dimension; j++) {
wc[i][j] = w[j][i];
}
// d = diag(diag(W'*C0*W));
// W = W*d^(-1/2);
NUMdmatrix_normalizeColumnVectors (wc.peek(), dimension, dimension, c0 -> data);
// scale eigenvectors for sphering
// [vb,db] = eig(C0);
// P = db^(-1/2)*vb';
Eigen_initFromSymmetricMatrix (eigen.peek(), c0 -> data, dimension);
for (long i = 1; i <= dimension; i++) {
if (eigen -> eigenvalues[i] < 0) {
Melder_throw (U"Covariance matrix not positive definite, eigenvalue[", i, U"] is negative.");
}
double scalef = 1 / sqrt (eigen -> eigenvalues[i]);
for (long j = 1; j <= dimension; j++) {
p[dimension - i + 1][j] = scalef * eigen -> eigenvectors[i][j];
}
}
// P*C[i]*P'
for (long ic = 1; ic <= thy size; ic++) {
CrossCorrelationTable cov1 = (CrossCorrelationTable) thy item[ic];
CrossCorrelationTable cov2 = (CrossCorrelationTable) ccts -> item[ic];
NUMdmatrices_multiply_VCVp (cov2 -> data, p.peek(), dimension, dimension, cov1 -> data, 1);
}
// W = P'\W == inv(P') * W
NUMpseudoInverse (p.peek(), dimension, dimension, pinv.peek(), 0);
NUMdmatrices_multiply_VpC (w, pinv.peek(), dimension, dimension, wc.peek(), dimension);
// initialisation for order KN^3
for (long ic = 2; ic <= thy size; ic++) {
CrossCorrelationTable cov = (CrossCorrelationTable) ccts -> item[ic];
// C * W
NUMdmatrices_multiply_VC (m1.peek(), cov -> data, dimension, dimension, w, dimension);
// D += scalef * M1*M1'
NUMdmatrices_multiplyScaleAdd (d.peek(), m1.peek(), dimension, dimension, 2 * cweights[ic]);
}
long iter = 0;
double delta_w;
autoMelderProgress progress (U"Simultaneous diagonalization of many CrossCorrelationTables...");
try {
do {
// the standard diagonality measure is rather expensive to calculate so we compare the norms of
// differences of eigenvectors.
delta_w = 0;
for (long kol = 1; kol <= dimension; kol++) {
for (long i = 1; i <= dimension; i++) {
wvec[i] = w[i][kol];
}
update_one_column (ccts.peek(), d.peek(), cweights, wvec.peek(), -1, mvec.peek());
Eigen_initFromSymmetricMatrix (eigen.peek(), d.peek(), dimension);
// Eigenvalues already sorted; get eigenvector of smallest !
for (long i = 1; i <= dimension; i++) {
wnew[i] = eigen -> eigenvectors[dimension][i];
}
update_one_column (ccts.peek(), d.peek(), cweights, wnew.peek(), 1, mvec.peek());
for (long i = 1; i <= dimension; i++) {
w[i][kol] = wnew[i];
}
// compare norms of eigenvectors. We have to compare ||wvec +/- w_new|| because eigenvectors
// may change sign.
double normp = 0, normm = 0;
for (long j = 1; j <= dimension; j++) {
double dm = wvec[j] - wnew[j], dp = wvec[j] + wnew[j];
normp += dm * dm; normm += dp * dp;
}
normp = normp < normm ? normp : normm;
normp = sqrt (normp);
delta_w = normp > delta_w ? normp : delta_w;
}
iter++;
Melder_progress ((double) iter / (double) (maxNumberOfIterations + 1), U"Iteration: ", iter, U", norm: ", delta_w);
} while (delta_w > delta && iter < maxNumberOfIterations);
} catch (MelderError) {
Melder_clearError ();
}
// Revert the sphering W = P'*W;
// Take transpose to make W*C[i]W' diagonal instead of W'*C[i]*W => (P'*W)'=W'*P
NUMmatrix_copyElements (w, wc.peek(), 1, dimension, 1, dimension);
NUMdmatrices_multiply_VpC (w, wc.peek(), dimension, dimension, p.peek(), dimension); // W = W'*P: final result
// Calculate the "real" diagonality measure
// double dm = CrossCorrelationTables_and_Diagonalizer_getDiagonalityMeasure (thee, me, cweights, 1, thy size);
} catch (MelderError) {
Melder_throw (me, U" & ", thee, U": no joint diagonalization (qdiag).");
}
}
/*
* Sets up all data structures needed.
* Replace by codegen
*/
pwork* ECOS_setup(idxint n, idxint m, idxint p, idxint l, idxint ncones, idxint* q,
pfloat* Gpr, idxint* Gjc, idxint* Gir,
pfloat* Apr, idxint* Ajc, idxint* Air,
pfloat* c, pfloat* h, pfloat* b)
{
idxint i, j, k, cidx, conesize, lnz, amd_result, nK, *Ljc, *Lir, *P, *Pinv, *Sign;
pwork* mywork;
double Control [AMD_CONTROL], Info [AMD_INFO];
pfloat rx, ry, rz, *Lpr;
spmat *At, *Gt, *KU;
#if PROFILING > 0
timer tsetup;
#endif
#if PROFILING > 1
timer tcreatekkt;
timer tmattranspose;
timer tordering;
#endif
#if PROFILING > 0
tic(&tsetup);
#endif
#if PRINTLEVEL > 2
PRINTTEXT("\n");
PRINTTEXT(" *******************************************************************************\n");
PRINTTEXT(" * ECOS: Embedded Conic Solver - Sparse Interior Point method for SOCPs *\n");
PRINTTEXT(" * *\n");
PRINTTEXT(" * NOTE: The solver is based on L. Vandenberghe's 'The CVXOPT linear and quad- *\n");
PRINTTEXT(" * ratic cone program solvers', March 20, 2010. Available online: *\n");
PRINTTEXT(" * [http://abel.ee.ucla.edu/cvxopt/documentation/coneprog.pdf] *\n");
PRINTTEXT(" * *\n");
PRINTTEXT(" * This code uses T.A. Davis' sparse LDL package and AMD code. *\n");
PRINTTEXT(" * [http://www.cise.ufl.edu/research/sparse] *\n");
PRINTTEXT(" * *\n");
PRINTTEXT(" * Written during a summer visit at Stanford University with S. Boyd. *\n");
PRINTTEXT(" * *\n");
PRINTTEXT(" * (C) Alexander Domahidi, Automatic Control Laboratory, ETH Zurich, 2012-13. *\n");
PRINTTEXT(" * Email: [email protected] *\n");
PRINTTEXT(" *******************************************************************************\n");
PRINTTEXT("\n\n");
PRINTTEXT("PROBLEM SUMMARY:\n");
PRINTTEXT(" Primal variables (n): %d\n", (int)n);
PRINTTEXT("Equality constraints (p): %d\n", (int)p);
PRINTTEXT(" Conic variables (m): %d\n", (int)m);
PRINTTEXT("- - - - - - - - - - - - - - -\n");
PRINTTEXT(" Size of LP cone: %d\n", (int)l);
PRINTTEXT(" Number of SOCs: %d\n", (int)ncones);
for( i=0; i<ncones; i++ ){
PRINTTEXT(" Size of SOC #%02d: %d\n", (int)(i+1), (int)q[i]);
}
#endif
/* get work data structure */
mywork = (pwork *)MALLOC(sizeof(pwork));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for WORK struct\n");
#endif
/* dimensions */
mywork->n = n;
mywork->m = m;
mywork->p = p;
mywork->D = l + ncones;
#if PRINTLEVEL > 2
PRINTTEXT("Set dimensions\n");
#endif
/* variables */
mywork->x = (pfloat *)MALLOC(n*sizeof(pfloat));
mywork->y = (pfloat *)MALLOC(p*sizeof(pfloat));
mywork->z = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->s = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->lambda = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->dsaff_by_W = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->dsaff = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->dzaff = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->saff = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->zaff = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->W_times_dzaff = (pfloat *)MALLOC(m*sizeof(pfloat));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for variables\n");
#endif
/* best iterates so far */
mywork->best_x = (pfloat *)MALLOC(n*sizeof(pfloat));
mywork->best_y = (pfloat *)MALLOC(p*sizeof(pfloat));
mywork->best_z = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->best_s = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->best_info = (stats *)MALLOC(sizeof(stats));
/* cones */
mywork->C = (cone *)MALLOC(sizeof(cone));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for cone struct\n");
#endif
/* LP cone */
mywork->C->lpc = (lpcone *)MALLOC(sizeof(lpcone));
mywork->C->lpc->p = l;
if( l > 0 ){
mywork->C->lpc->w = (pfloat *)MALLOC(l*sizeof(pfloat));
mywork->C->lpc->v = (pfloat *)MALLOC(l*sizeof(pfloat));
mywork->C->lpc->kkt_idx = (idxint *)MALLOC(l*sizeof(idxint));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for LP cone\n");
#endif
} else {
mywork->C->lpc->w = NULL;
mywork->C->lpc->v = NULL;
mywork->C->lpc->kkt_idx = NULL;
#if PRINTLEVEL > 2
PRINTTEXT("No LP cone present, pointers filled with NULL\n");
#endif
}
/* Second-order cones */
mywork->C->soc = (socone *)MALLOC(ncones*sizeof(socone));
mywork->C->nsoc = ncones;
cidx = 0;
for( i=0; i<ncones; i++ ){
conesize = (idxint)q[i];
mywork->C->soc[i].p = conesize;
mywork->C->soc[i].a = 0;
mywork->C->soc[i].eta = 0;
mywork->C->soc[i].q = (pfloat *)MALLOC((conesize-1)*sizeof(pfloat));
mywork->C->soc[i].skbar = (pfloat *)MALLOC((conesize)*sizeof(pfloat));
mywork->C->soc[i].zkbar = (pfloat *)MALLOC((conesize)*sizeof(pfloat));
#if CONEMODE == 0
mywork->C->soc[i].Didx = (idxint *)MALLOC((conesize)*sizeof(idxint));
#endif
#if CONEMODE > 0
mywork->C->soc[i].colstart = (idxint *)MALLOC((conesize)*sizeof(idxint));
#endif
cidx += conesize;
}
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for second-order cones\n");
#endif
/* info struct */
mywork->info = (stats *)MALLOC(sizeof(stats));
#if PROFILING > 1
mywork->info->tfactor = 0;
mywork->info->tkktsolve = 0;
mywork->info->tfactor_t1 = 0;
mywork->info->tfactor_t2 = 0;
#endif
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for info struct\n");
#endif
#if defined EQUILIBRATE && EQUILIBRATE > 0
/* equilibration vector */
mywork->xequil = (pfloat *)MALLOC(n*sizeof(pfloat));
mywork->Aequil = (pfloat *)MALLOC(p*sizeof(pfloat));
mywork->Gequil = (pfloat *)MALLOC(m*sizeof(pfloat));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for equilibration vectors\n");
#endif
#endif
/* settings */
mywork->stgs = (settings *)MALLOC(sizeof(settings));
mywork->stgs->maxit = MAXIT;
mywork->stgs->gamma = GAMMA;
mywork->stgs->delta = DELTA;
mywork->stgs->eps = EPS;
mywork->stgs->nitref = NITREF;
mywork->stgs->abstol = ABSTOL;
mywork->stgs->feastol = FEASTOL;
mywork->stgs->reltol = RELTOL;
mywork->stgs->abstol_inacc = ATOL_INACC;
mywork->stgs->feastol_inacc = FTOL_INACC;
mywork->stgs->reltol_inacc = RTOL_INACC;
mywork->stgs->verbose = VERBOSE;
#if PRINTLEVEL > 2
PRINTTEXT("Written settings\n");
#endif
mywork->c = c;
mywork->h = h;
mywork->b = b;
#if PRINTLEVEL > 2
PRINTTEXT("Hung pointers for c, h and b into WORK struct\n");
#endif
/* Store problem data */
if(Apr && Ajc && Air) {
mywork->A = createSparseMatrix(p, n, Ajc[n], Ajc, Air, Apr);
} else {
mywork->A = NULL;
}
if (Gpr && Gjc && Gir) {
mywork->G = createSparseMatrix(m, n, Gjc[n], Gjc, Gir, Gpr);
} else {
/* create an empty sparse matrix */
mywork->G = createSparseMatrix(m, n, 0, Gjc, Gir, Gpr);
}
#if defined EQUILIBRATE && EQUILIBRATE > 0
set_equilibration(mywork);
#if PRINTLEVEL > 2
PRINTTEXT("Done equilibrating\n");
#endif
#endif
#if PROFILING > 1
mywork->info->ttranspose = 0;
tic(&tmattranspose);
#endif
if(mywork->A)
At = transposeSparseMatrix(mywork->A);
else
At = NULL;
#if PROFILING > 1
mywork->info->ttranspose += toc(&tmattranspose);
#endif
#if PRINTLEVEL > 2
PRINTTEXT("Transposed A\n");
#endif
#if PROFILING > 1
tic(&tmattranspose);
#endif
Gt = transposeSparseMatrix(mywork->G);
#if PROFILING > 1
mywork->info->ttranspose += toc(&tmattranspose);
#endif
#if PRINTLEVEL > 2
PRINTTEXT("Transposed G\n");
#endif
/* set up KKT system */
#if PROFILING > 1
tic(&tcreatekkt);
#endif
createKKT_U(Gt, At, mywork->C, &Sign, &KU);
#if PROFILING > 1
mywork->info->tkktcreate = toc(&tcreatekkt);
#endif
#if PRINTLEVEL > 2
PRINTTEXT("Created upper part of KKT matrix K\n");
#endif
/*
* Set up KKT system related data
* (L comes later after symbolic factorization)
*/
nK = KU->n;
#if DEBUG > 0
dumpSparseMatrix(KU, "KU0.txt");
#endif
#if PRINTLEVEL > 2
PRINTTEXT("Dimension of KKT matrix: %d\n", (int)nK);
PRINTTEXT("Non-zeros in KKT matrix: %d\n", (int)KU->nnz);
#endif
/* allocate memory in KKT system */
mywork->KKT = (kkt *)MALLOC(sizeof(kkt));
mywork->KKT->D = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->Parent = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->Pinv = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->work1 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->work2 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->work3 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->work4 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->work5 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->work6 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->Flag = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->Pattern = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->Lnz = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->RHS1 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->RHS2 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->dx1 = (pfloat *)MALLOC(mywork->n*sizeof(pfloat));
mywork->KKT->dx2 = (pfloat *)MALLOC(mywork->n*sizeof(pfloat));
mywork->KKT->dy1 = (pfloat *)MALLOC(mywork->p*sizeof(pfloat));
mywork->KKT->dy2 = (pfloat *)MALLOC(mywork->p*sizeof(pfloat));
mywork->KKT->dz1 = (pfloat *)MALLOC(mywork->m*sizeof(pfloat));
mywork->KKT->dz2 = (pfloat *)MALLOC(mywork->m*sizeof(pfloat));
mywork->KKT->Sign = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->PKPt = newSparseMatrix(nK, nK, KU->nnz);
mywork->KKT->PK = (idxint *)MALLOC(KU->nnz*sizeof(idxint));
#if PRINTLEVEL > 2
PRINTTEXT("Created memory for KKT-related data\n");
#endif
/* calculate ordering of KKT matrix using AMD */
P = (idxint *)MALLOC(nK*sizeof(idxint));
#if PROFILING > 1
tic(&tordering);
#endif
AMD_defaults(Control);
amd_result = AMD_order(nK, KU->jc, KU->ir, P, Control, Info);
#if PROFILING > 1
mywork->info->torder = toc(&tordering);
#endif
if( amd_result == AMD_OK ){
#if PRINTLEVEL > 2
PRINTTEXT("AMD ordering successfully computed.\n");
AMD_info(Info);
#endif
} else {
#if PRINTLEVEL > 2
PRINTTEXT("Problem in AMD ordering, exiting.\n");
AMD_info(Info);
#endif
return NULL;
}
/* calculate inverse permutation and permutation mapping of KKT matrix */
pinv(nK, P, mywork->KKT->Pinv);
Pinv = mywork->KKT->Pinv;
#if DEBUG > 0
dumpDenseMatrix_i(P, nK, 1, "P.txt");
dumpDenseMatrix_i(mywork->KKT->Pinv, nK, 1, "PINV.txt");
#endif
permuteSparseSymmetricMatrix(KU, mywork->KKT->Pinv, mywork->KKT->PKPt, mywork->KKT->PK);
/* permute sign vector */
for( i=0; i<nK; i++ ){ mywork->KKT->Sign[Pinv[i]] = Sign[i]; }
#if PRINTLEVEL > 3
PRINTTEXT("P = [");
for( i=0; i<nK; i++ ){ PRINTTEXT("%d ", (int)P[i]); }
PRINTTEXT("];\n");
PRINTTEXT("Pinv = [");
for( i=0; i<nK; i++ ){ PRINTTEXT("%d ", (int)Pinv[i]); }
PRINTTEXT("];\n");
PRINTTEXT("Sign = [");
for( i=0; i<nK; i++ ){ PRINTTEXT("%+d ", (int)Sign[i]); }
PRINTTEXT("];\n");
PRINTTEXT("SignP = [");
for( i=0; i<nK; i++ ){ PRINTTEXT("%+d ", (int)mywork->KKT->Sign[i]); }
PRINTTEXT("];\n");
#endif
/* symbolic factorization */
Ljc = (idxint *)MALLOC((nK+1)*sizeof(idxint));
#if PRINTLEVEL > 2
PRINTTEXT("Allocated memory for cholesky factor L\n");
#endif
LDL_symbolic2(
mywork->KKT->PKPt->n, /* A and L are n-by-n, where n >= 0 */
mywork->KKT->PKPt->jc, /* input of size n+1, not modified */
mywork->KKT->PKPt->ir, /* input of size nz=Ap[n], not modified */
Ljc, /* output of size n+1, not defined on input */
mywork->KKT->Parent, /* output of size n, not defined on input */
mywork->KKT->Lnz, /* output of size n, not defined on input */
mywork->KKT->Flag /* workspace of size n, not defn. on input or output */
);
/* assign memory for L */
lnz = Ljc[nK];
#if PRINTLEVEL > 2
PRINTTEXT("Nonzeros in L, excluding diagonal: %d\n", (int)lnz) ;
#endif
Lir = (idxint *)MALLOC(lnz*sizeof(idxint));
Lpr = (pfloat *)MALLOC(lnz*sizeof(pfloat));
mywork->KKT->L = createSparseMatrix(nK, nK, lnz, Ljc, Lir, Lpr);
#if PRINTLEVEL > 2
PRINTTEXT("Created Cholesky factor of K in KKT struct\n");
#endif
/* permute KKT matrix - we work on this one from now on */
permuteSparseSymmetricMatrix(KU, mywork->KKT->Pinv, mywork->KKT->PKPt, NULL);
#if DEBUG > 0
dumpSparseMatrix(mywork->KKT->PKPt, "PKPt.txt");
#endif
#if CONEMODE > 0
/* zero any off-diagonal elements in (permuted) scalings in KKT matrix */
for (i=0; i<mywork->C->nsoc; i++) {
for (j=1; j<mywork->C->soc[i].p; j++) {
for (k=0; k<j; k++) {
mywork->KKT->PKPt->pr[mywork->KKT->PK[mywork->C->soc[i].colstart[j]+k]] = 0;
}
}
}
#endif
#if DEBUG > 0
dumpSparseMatrix(mywork->KKT->PKPt, "PKPt0.txt");
#endif
/* set up RHSp for initialization */
k = 0; j = 0;
for( i=0; i<n; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = 0; }
for( i=0; i<p; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = b[i]; }
for( i=0; i<l; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = h[i]; j++; }
for( l=0; l<ncones; l++ ){
for( i=0; i < mywork->C->soc[l].p; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = h[j++]; }
#if CONEMODE == 0
mywork->KKT->RHS1[Pinv[k++]] = 0;
mywork->KKT->RHS1[Pinv[k++]] = 0;
#endif
}
#if PRINTLEVEL > 2
PRINTTEXT("Written %d entries of RHS1\n", (int)k);
#endif
/* set up RHSd for initialization */
for( i=0; i<n; i++ ){ mywork->KKT->RHS2[Pinv[i]] = -c[i]; }
for( i=n; i<nK; i++ ){ mywork->KKT->RHS2[Pinv[i]] = 0; }
/* get scalings of problem data */
rx = norm2(c, n); mywork->resx0 = MAX(1, rx);
ry = norm2(b, p); mywork->resy0 = MAX(1, ry);
rz = norm2(h, m); mywork->resz0 = MAX(1, rz);
/* get memory for residuals */
mywork->rx = (pfloat *)MALLOC(n*sizeof(pfloat));
mywork->ry = (pfloat *)MALLOC(p*sizeof(pfloat));
mywork->rz = (pfloat *)MALLOC(m*sizeof(pfloat));
/* clean up */
mywork->KKT->P = P;
FREE(Sign);
if(At) freeSparseMatrix(At);
freeSparseMatrix(Gt);
freeSparseMatrix(KU);
#if PROFILING > 0
mywork->info->tsetup = toc(&tsetup);
#endif
return mywork;
}
/*
* The equalities are eliminated.
*
*0=(Me_1 Me_2)(Re Ri)' + Qe
*Vi=(Mi_1 Mi_2)(Re Ri)' + Qi
*
*Re=-Me_1^{-1}(Me_2Ri+Qe)
*
*Vi=(Mi_2-Mi_1 Me_1^{-1} Me_2)Ri+Qi-Mi1 Me_1^{-1} Qe
*
*/
void GMPReducedSolve(GenericMechanicalProblem* pInProblem, double *reaction , double *velocity, int * info, SolverOptions* options, NumericsOptions* numerics_options)
{
SparseBlockStructuredMatrix* m = pInProblem->M->matrix1;
int nbRow = m->blocksize0[m->blocknumber0 - 1];
int nbCol = m->blocksize1[m->blocknumber1 - 1];
double *Me = (double *) malloc(nbRow * nbCol * sizeof(double));
double *Qe = (double *) malloc(nbRow * sizeof(double));
double *Mi = (double *) malloc(nbRow * nbCol * sizeof(double));
double *Qi = (double *) malloc(nbRow * sizeof(double));
int Me_size;
int Mi_size;
buildReducedGMP(pInProblem, Me, Mi, Qe, Qi, &Me_size, &Mi_size);
if ((Me_size == 0 || Mi_size == 0))
{
genericMechanicalProblem_GS(pInProblem, reaction, velocity, info, options, numerics_options);
free(Me);
free(Qe);
free(Mi);
free(Qi);
return;
}
double * pseduInvMe1 = (double *)malloc(Me_size * Me_size * sizeof(double));
memcpy(pseduInvMe1, Me, Me_size * Me_size * sizeof(double));
pinv(pseduInvMe1, Me_size, Me_size, 1e-16);
double *Mi2 = Mi + Mi_size * Me_size;
double *Mi1 = Mi;
double *Me2 = Me + Me_size * Me_size;
#ifdef GMP_DEBUG_GMPREDUCED_SOLVE
double *Me1 = Me;
FILE * titi = fopen("buildReducedGMP_output.txt", "w");
printf("GMPReducedsolve\n");
printDenseMatrice("Me1", titi, Me1, Me_size, Me_size);
printDenseMatrice("Me2", titi, Me2, Me_size, Mi_size);
printDenseMatrice("Mi1", titi, Mi1, Mi_size, Me_size);
printDenseMatrice("Mi2", titi, Mi2, Mi_size, Mi_size);
printDenseMatrice("Qe", titi, Qe, Me_size, 1);
printDenseMatrice("Qi", titi, Qi, Mi_size, 1);
printDenseMatrice("Me1inv", titi, pseduInvMe1, Me_size, Me_size);
#endif
double * reducedProb = (double *)malloc(Mi_size * Mi_size * sizeof(double));
memcpy(reducedProb, Mi2, Mi_size * Mi_size * sizeof(double));
double * Mi1pseduInvMe1 = (double *)malloc(Mi_size * Me_size * sizeof(double));
cblas_dgemm(CblasColMajor,CblasNoTrans, CblasNoTrans, Mi_size, Me_size, Me_size, -1.0, Mi1, Mi_size, pseduInvMe1, Me_size, 0.0, Mi1pseduInvMe1, Mi_size);
#ifdef GMP_DEBUG_GMPREDUCED_SOLVE
printDenseMatrice("minusMi1pseduInvMe1", titi, Mi1pseduInvMe1, Mi_size, Me_size);
fprintf(titi, "_minusMi1pseduInvMe1=-Mi1*Me1inv;\n");
#endif
cblas_dgemv(CblasColMajor,CblasNoTrans, Mi_size, Me_size, 1.0, Mi1pseduInvMe1, Mi_size, Qe, 1, 1.0, Qi, 1);
#ifdef GMP_DEBUG_GMPREDUCED_SOLVE
printDenseMatrice("newQi", titi, Qi, Mi_size, 1);
fprintf(titi, "_newQi=Qi+_minusMi1pseduInvMe1*Qe;\n");
#endif
cblas_dgemm(CblasColMajor,CblasNoTrans, CblasNoTrans, Mi_size, Mi_size, Me_size, 1.0, Mi1pseduInvMe1, Mi_size, Me2, Me_size, 1.0, reducedProb, Mi_size);
#ifdef GMP_DEBUG_GMPREDUCED_SOLVE
printDenseMatrice("W", titi, reducedProb, Mi_size, Mi_size);
fprintf(titi, "_W=Mi2+_minusMi1pseduInvMe1*Me2;\n");
#endif
listNumericsProblem * curProblem = 0;
GenericMechanicalProblem * _pnumerics_GMP = buildEmptyGenericMechanicalProblem();
curProblem = pInProblem->firstListElem;
while (curProblem)
{
switch (curProblem->type)
{
case SICONOS_NUMERICS_PROBLEM_EQUALITY:
{
break;
}
case SICONOS_NUMERICS_PROBLEM_LCP:
{
addProblem(_pnumerics_GMP, curProblem->type, curProblem->size);
break;
}
case SICONOS_NUMERICS_PROBLEM_FC3D:
{
FrictionContactProblem* pFC3D = (FrictionContactProblem*)addProblem(_pnumerics_GMP, curProblem->type, curProblem->size);
*(pFC3D->mu) = *(((FrictionContactProblem*)curProblem->problem)->mu);
break;
}
default:
printf("GMPReduced buildReducedGMP: problemType unknown: %d . \n", curProblem->type);
}
curProblem = curProblem->nextProblem;
}
NumericsMatrix numM;
numM.storageType = 0;
numM.matrix0 = reducedProb;
numM.matrix1 = 0;
numM.size0 = Mi_size;
numM.size1 = Mi_size;
_pnumerics_GMP->M = &numM;
_pnumerics_GMP->q = Qi;
double *Rreduced = (double *) malloc(Mi_size * sizeof(double));
double *Vreduced = (double *) malloc(Mi_size * sizeof(double));
genericMechanicalProblem_GS(_pnumerics_GMP, Rreduced, Vreduced, info, options, numerics_options);
#ifdef GMP_DEBUG_GMPREDUCED_SOLVE
if (*info)
{
printf("\nGMPREduced failed!\n");
}
else
{
printf("\nGMPREduced succed!\n");
printDenseMatrice("Ri", titi, Rreduced, Mi_size, 1);
printDenseMatrice("Vi", titi, Vreduced, Mi_size, 1);
}
#endif
if (!*info)
{
/*Re computation*/
double * Re = (double*)malloc(Me_size * sizeof(double));
double * Rbuf = (double*)malloc(Me_size * sizeof(double));
memcpy(Rbuf, Qe, Me_size * sizeof(double));
cblas_dgemv(CblasColMajor,CblasNoTrans, Me_size, Mi_size, 1.0, Me2, Me_size, Rreduced, 1, 1.0, Rbuf, 1);
cblas_dgemv(CblasColMajor,CblasNoTrans, Me_size, Me_size, -1.0, pseduInvMe1, Me_size, Rbuf, 1, 0.0, Re, 1);
#ifdef GMP_DEBUG_GMPREDUCED_SOLVE
fprintf(titi, "_Re=-Me1inv*(Me2*Ri+Qe);\n");
printDenseMatrice("Re", titi, Re, Me_size, 1);
#endif
GMPReducedSolToSol(pInProblem, reaction, velocity, Re, Rreduced, Vreduced);
double err;
int tolViolate = GenericMechanical_compute_error(pInProblem, reaction, velocity, options->dparam[0], options, &err);
if (tolViolate)
{
printf("GMPReduced, warnning, reduced problem solved, but error of intial probleme violated tol = %e, err= %e\n", options->dparam[0], err);
}
free(Re);
free(Rbuf);
}
#ifdef GMP_DEBUG_GMPREDUCED_SOLVE
fclose(titi);
#endif
free(Rreduced);
free(Vreduced);
freeGenericMechanicalProblem(_pnumerics_GMP, NUMERICS_GMP_FREE_GMP);
free(Me);
free(Mi);
free(Qe);
free(Qi);
free(pseduInvMe1);
free(reducedProb);
free(Mi1pseduInvMe1);
// GenericMechanicalProblem GMPOutProblem;
// SparseBlockStructuredMatrix mOut;
}
void EyePinvRefGen::run()
{
if (genOn)
{
LockGuard guard(mutex);
double timeStamp;
// read encoders
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData,&timeStamp);
updateTorsoBlockedJoints(chainNeck,fbTorso);
updateTorsoBlockedJoints(chainEyeL,fbTorso);
updateTorsoBlockedJoints(chainEyeR,fbTorso);
// get current target
Vector xd=commData->port_xd->get_xd();
// update neck chain
chainNeck->setAng(nJointsTorso+0,fbHead[0]);
chainNeck->setAng(nJointsTorso+1,fbHead[1]);
chainNeck->setAng(nJointsTorso+2,fbHead[2]);
// ask for saccades (if possible)
if (commData->saccadesOn && (saccadesRxTargets!=commData->port_xd->get_rx()) &&
!commData->saccadeUnderway && (Time::now()-saccadesClock>commData->saccadesInhibitionPeriod))
{
Vector fph=xd; fph.push_back(1.0);
fph=SE3inv(chainNeck->getH())*fph; fph[3]=0.0;
double rot=CTRL_RAD2DEG*acos(fph[2]/norm(fph)); fph[3]=1.0;
// estimate geometrically the target tilt and pan of the eyes
Vector ang(3,0.0);
ang[0]=-atan2(fph[1],fabs(fph[2]));
ang[1]=atan2(fph[0],fph[2]);
// enforce joints bounds
ang[0]=sat(ang[0],lim(0,0),lim(0,1));
ang[1]=sat(ang[1],lim(1,0),lim(1,1));
// favor the smooth-pursuit in case saccades are small
if (rot>commData->saccadesActivationAngle)
{
// init vergence
ang[2]=fbHead[5];
// get rid of eyes tilt
Vector axis(4);
axis[0]=1.0; axis[1]=0.0; axis[2]=0.0; axis[3]=-ang[0];
fph=axis2dcm(axis)*fph;
// go on iff the point is in front of us
if (fph[2]>0.0)
{
double L,R;
// estimate geometrically the target vergence Vg=L-R
if (fph[0]>=eyesHalfBaseline)
{
L=M_PI/2.0-atan2(fph[2],fph[0]+eyesHalfBaseline);
R=M_PI/2.0-atan2(fph[2],fph[0]-eyesHalfBaseline);
}
else if (fph[0]>-eyesHalfBaseline)
{
L=M_PI/2.0-atan2(fph[2],fph[0]+eyesHalfBaseline);
R=-(M_PI/2.0-atan2(fph[2],eyesHalfBaseline-fph[0]));
}
else
{
L=-(M_PI/2.0-atan2(fph[2],-fph[0]-eyesHalfBaseline));
R=-(M_PI/2.0-atan2(fph[2],eyesHalfBaseline-fph[0]));
}
ang[2]=L-R;
}
// enforce joints bounds
ang[2]=sat(ang[2],lim(2,0),lim(2,1));
commData->set_qd(3,ang[0]);
commData->set_qd(4,ang[1]);
commData->set_qd(5,ang[2]);
Vector vel(3,SACCADES_VEL);
ctrl->doSaccade(ang,vel);
saccadesClock=Time::now();
}
}
// update eyes chains for convergence purpose
updateNeckBlockedJoints(chainEyeL,fbHead); updateNeckBlockedJoints(chainEyeR,fbHead);
chainEyeL->setAng(nJointsTorso+3,qd[0]); chainEyeR->setAng(nJointsTorso+3,qd[0]);
chainEyeL->setAng(nJointsTorso+4,qd[1]+qd[2]/2.0); chainEyeR->setAng(nJointsTorso+4,qd[1]-qd[2]/2.0);
// converge on target
if (CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,fp,eyesJ))
{
Vector v=EYEPINVREFGEN_GAIN*(pinv(eyesJ)*(xd-fp));
// update eyes chains in actual configuration for velocity compensation
chainEyeL->setAng(nJointsTorso+3,fbHead[3]); chainEyeR->setAng(nJointsTorso+3,fbHead[3]);
chainEyeL->setAng(nJointsTorso+4,fbHead[4]+fbHead[5]/2.0); chainEyeR->setAng(nJointsTorso+4,fbHead[4]-fbHead[5]/2.0);
// compensate neck rotation at eyes level
if ((commData->eyesBoundVer>=0.0) || !CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,fp,eyesJ))
commData->set_counterv(zeros(qd.length()));
else
commData->set_counterv(getEyesCounterVelocity(eyesJ,fp));
// reset eyes controller and integral upon saccades transition on=>off
if (saccadeUnderWayOld && !commData->saccadeUnderway)
{
ctrl->resetCtrlEyes();
qd[0]=fbHead[3];
qd[1]=fbHead[4];
qd[2]=fbHead[5];
I->reset(qd);
}
// update reference
qd=I->integrate(v+commData->get_counterv());
}
else
commData->set_counterv(zeros(qd.length()));
// set a new target position
commData->set_xd(xd);
commData->set_x(fp,timeStamp);
commData->set_fpFrame(chainNeck->getH());
if (!commData->saccadeUnderway)
{
commData->set_qd(3,qd[0]);
commData->set_qd(4,qd[1]);
commData->set_qd(5,qd[2]);
}
// latch the saccades status
saccadeUnderWayOld=commData->saccadeUnderway;
saccadesRxTargets=commData->port_xd->get_rx();
}
}
///
/// \brief Vespucci::Math::DimensionReduction::VCA
/// Vertex Component Analysis
/// \param R The dataset
/// \param endmembers Number of endmembers to compute
/// \param indices Row indices of pure components.
/// \param endmember_spectra Spectra of pure components (note that these are in
/// columns, not rows as in spectra_)
/// \param projected_data Projected data
/// \param fractional_abundances Purity of a given spectrum relative to endmember
/// \return Convergeance (no actual test implemented...)
///
bool Vespucci::Math::DimensionReduction::VCA(const arma::mat &R, arma::uword p,
arma::uvec &indices, arma::mat &endmember_spectra,
arma::mat &projected_data, arma::mat &fractional_abundances)
{
//Initializations
arma::uword L = R.n_rows;
arma::uword N = R.n_cols;
if (L == 0 || N == 0){
std::cerr << "No data!" << std::endl;
return false;
}
if (p > L){
std::cerr << "wrong number of endmembers (" << p << ")!"<< std::endl;
std::cerr << "set to 5 or one less than number of spectra" << std::endl;
p = (L < 5? 5: L-1);
}
//mat of SNR
arma::mat r_m = mean(R, 1);
arma::mat R_m = arma::repmat(r_m, 1, N); //the mean of each spectral band
arma::mat R_o = R - R_m; //mean-center the data
arma::mat Ud;
arma::vec Sd;
arma::mat Vd;
//arma::svds(Ud, Sd, Vd, arma::sp_mat(R_o * R_o.t()/N), p);
Vespucci::Math::DimensionReduction::svds(R_o*R_o.t()/N, p, Ud, Sd, Vd);
arma::mat x_p;
try{
x_p = Ud.t() * R_o;
}catch(std::exception e){
std::cout << "Ud.t() * R_o" << std::endl;
}
double SNR = Vespucci::Math::DimensionReduction::estimate_snr(R, r_m, x_p);
double SNR_th = 15 + 10*log10(p);
//Choose projective projection or projection to p-1 subspace
arma::mat y;
if (SNR < SNR_th){
arma::uword d = p - 1;
Ud = Ud.cols(0, d-1);
projected_data = Ud * x_p.rows(0, d-1) + R_m; //in dimension L
arma::mat x = x_p.rows(0, d-1);//x_p = trans(Ud)*R_o, p-dimensional subspace
//following three lines are one in arma::matlab...
arma::mat sum_squares = sum(pow(x, 2));
double c = sum_squares.max();
c = std::sqrt(c);
y = arma::join_vert(x, c*arma::ones(1, N));
}
else{
arma::uword d = p;
Vespucci::Math::DimensionReduction::svds(R*R.t()/N, p, Ud, Sd, Vd);
arma::svds(Ud, Sd, Vd, arma::sp_mat(R*R.t()/N), d);//R_o is a mean centered version...
x_p = Ud.t() * R;
projected_data = Ud * x_p.rows(0, d-1);
arma::mat x = Ud.t() * R;
arma::mat u = arma::mean(x, 1);
y = x / arma::repmat(sum(x % arma::repmat(u, 1, N)), d, 1);
}
// The VCA algorithm
arma::vec w;
w.set_size(p);
arma::vec f;
arma::rowvec v;
indices.set_size(p);
//there are no fill functions for arma::uvecs
for (arma::uword i = 0; i < p; ++i)
indices(i) = 0;
arma::mat A = arma::zeros(p, p);
double v_max;
double sum_squares;
arma::uvec q1;
A(p-1, 0) = 1;
for (arma::uword i = 0; i < p; ++i){
w.randu();
f = w - A*arma::pinv(A)*w;
sum_squares = sqrt(sum(square(f)));
f /= sum_squares;
v = f.t() * y;
v_max = arma::max(abs(v));
q1 = arma::find(abs(v) == v_max, 1);
indices(i) = q1(0);
A.col(i) = y.col(indices(i)); //same as x.col(indices(i));
}
endmember_spectra = projected_data.cols(indices);
fractional_abundances = arma::trans(pinv(endmember_spectra) * projected_data);
return true;
}
std::vector<Pose2_cont>
generate_unicycle_motion(const Pose2_cont& start, const Pose2_cont& goal)
{
DEBUG_PRINT("--------------------------------------------------------------------------------\n");
DEBUG_PRINT("generating unicycle motion between %s and %s\n", to_string(start).c_str(), to_string(goal).c_str());
DEBUG_PRINT("--------------------------------------------------------------------------------\n");
auto almost_equals = [](double lhs, double rhs, double eps) { return fabs(lhs - rhs) < eps; };
const double eps = 1e-6;
// check for straight-line motion or turn-in-place
double dx = goal.x - start.x;
double dy = goal.y - start.y;
double dtheta = shortest_angle_diff(goal.yaw, start.yaw);
if ((dx == 0.0 && dy == 0.0) || (dtheta == 0.0)) {
if (almost_equals(atan2(dy, dx), start.yaw, eps) && almost_equals(atan2(dy, dx), goal.yaw, eps)) {
DEBUG_PRINT("Interpolated Motion\n");
return create_interpolated_motion(start, goal);
}
else {
DEBUG_PRINT("No unicycle motion for turns-in-place or skidding\n");
return { };
}
}
DEBUG_PRINT("Arc Motion\n");
Eigen::Matrix2d R;
R(0, 0) = cos(start.yaw);
R(0, 1) = sin(goal.yaw) - sin(start.yaw);
R(1, 0) = sin(start.yaw);
R(1, 1) = -(cos(goal.yaw) - cos(start.yaw));
Eigen::Matrix2d Rpinv;
if (!pinv(R, Rpinv)) {
DEBUG_PRINT("Failed to compute Moore-penrose pseudo-inverse");
return { };
}
Eigen::Vector2d S = Rpinv * Eigen::Vector2d(goal.x - start.x, goal.y - start.y);
double straight_length = S(0);
double radius = S(1);
if (fabs(radius) < 1e-6) {
DEBUG_PRINT("Unable to turn with radius %0.3f\n", radius);
return { };
}
double w = shortest_angle_diff(goal.yaw, start.yaw) + (straight_length / radius);
double v = radius * w;
double tl = straight_length / v;
if (straight_length < 0) {
DEBUG_PRINT("Not allowed to go backwards (length = %0.3f)\n", straight_length);
return { };
}
if (v < 0) {
DEBUG_PRINT("Not allowed to go backwards (velocity = %0.3f)\n", v);
return { };
}
if (tl < 0.0 || tl > 1.0) {
DEBUG_PRINT("Another dimension! Another dimension! (tl = %0.3f)\n", tl);
return { };
}
DEBUG_PRINT("R = [ %0.3f, %0.3f; %0.3f, %0.3f]\n", R(0, 0), R(0, 1), R(1, 0), R(1, 1));
DEBUG_PRINT("S = [%0.3f, %0.3f]\n", S(0), S(1));
DEBUG_PRINT("straight_length = %0.3f\n", straight_length);
DEBUG_PRINT("radius = %0.3f\n", radius);
DEBUG_PRINT("w = %0.3f\n", w);
DEBUG_PRINT("v = %0.3f\n", v);
DEBUG_PRINT("tl = %0.3f\n", tl);
const double res = 0.1;
double arc_length = w * (1.0 - tl);
double total_length = fabs(straight_length) + fabs(arc_length);
const int num_samples = (int)std::ceil(total_length / res);
std::vector<Pose2_cont> interm_poses;
interm_poses.resize(num_samples);
for (int i = 0; i < num_samples; ++i) {
double dt = (double)i / (double)(num_samples - 1);
if (dt < tl) {
double x = start.x + v * dt * cos(start.yaw);
double y = start.y + v * dt * sin(start.yaw);
double theta = start.yaw;
interm_poses[i] = { x, y, theta };
}
else {
double x = start.x + straight_length * cos(start.yaw) + radius * sin(w * (dt - tl) + start.yaw) - radius * sin(start.yaw);
double y = start.y + straight_length * sin(start.yaw) - radius * cos(w * (dt - tl) + start.yaw) + radius * cos(start.yaw);
double theta = start.yaw + w * (dt - tl);
interm_poses[i] = { x, y, theta };
}
}
return interm_poses;
}
